1 #include "quadprog.h"
2 #include <vector>
3 /*
4 FILE eiquadprog.hh
5
6 NOTE: this is a modified of uQuadProg++ package, working with Eigen data structures.
7 uQuadProg++ is itself a port made by Angelo Furfaro of QuadProg++ originally developed by
8 Luca Di Gaspero, working with ublas data structures.
9
10 The quadprog_solve() function implements the algorithm of Goldfarb and Idnani
11 for the solution of a (convex) Quadratic Programming problem
12 by means of a dual method.
13
14 The problem is in the form:
15
16 min 0.5 * x G x + g0 x
17 s.t.
18 CE^T x + ce0 = 0
19 CI^T x + ci0 >= 0
20
21 The matrix and vectors dimensions are as follows:
22 G: n * n
23 g0: n
24
25 CE: n * p
26 ce0: p
27
28 CI: n * m
29 ci0: m
30
31 x: n
32
33 The function will return the cost of the solution written in the x vector or
34 std::numeric_limits::infinity() if the problem is infeasible. In the latter case
35 the value of the x vector is not correct.
36
37 References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving
38 strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33.
39
40 Notes:
41 1. pay attention in setting up the vectors ce0 and ci0.
42 If the constraints of your problem are specified in the form
43 A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
44 2. The matrix G is modified within the function since it is used to compute
45 the G = L^T L cholesky factorization for further computations inside the function.
46 If you need the original matrix G you should make a copy of it and pass the copy
47 to the function.
48
49
50 The author will be grateful if the researchers using this software will
51 acknowledge the contribution of this modified function and of Di Gaspero's
52 original version in their research papers.
53
54
55 LICENSE
56
57 Copyright (2010) Gael Guennebaud
58 Copyright (2008) Angelo Furfaro
59 Copyright (2006) Luca Di Gaspero
60
61
62 This file is a porting of QuadProg++ routine, originally developed
63 by Luca Di Gaspero, exploiting uBlas data structures for vectors and
64 matrices instead of native C++ array.
65
66 uquadprog is free software; you can redistribute it and/or modify
67 it under the terms of the GNU General Public License as published by
68 the Free Software Foundation; either version 2 of the License, or
69 (at your option) any later version.
70
71 uquadprog is distributed in the hope that it will be useful,
72 but WITHOUT ANY WARRANTY; without even the implied warranty of
73 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
74 GNU General Public License for more details.
75
76 You should have received a copy of the GNU General Public License
77 along with uquadprog; if not, write to the Free Software
78 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
79
80 */
81
82 #include <Eigen/Dense>
83
quadprog(const Eigen::MatrixXd & G,const Eigen::VectorXd & g0,const Eigen::MatrixXd & CE,const Eigen::VectorXd & ce0,const Eigen::MatrixXd & CI,const Eigen::VectorXd & ci0,Eigen::VectorXd & x)84 IGL_INLINE bool igl::copyleft::quadprog(
85 const Eigen::MatrixXd & G,
86 const Eigen::VectorXd & g0,
87 const Eigen::MatrixXd & CE,
88 const Eigen::VectorXd & ce0,
89 const Eigen::MatrixXd & CI,
90 const Eigen::VectorXd & ci0,
91 Eigen::VectorXd& x)
92 {
93 using namespace Eigen;
94 typedef double Scalar;
95 const auto distance = [](Scalar a, Scalar b)->Scalar
96 {
97 Scalar a1, b1, t;
98 a1 = std::abs(a);
99 b1 = std::abs(b);
100 if (a1 > b1)
101 {
102 t = (b1 / a1);
103 return a1 * std::sqrt(1.0 + t * t);
104 }
105 else
106 if (b1 > a1)
107 {
108 t = (a1 / b1);
109 return b1 * std::sqrt(1.0 + t * t);
110 }
111 return a1 * std::sqrt(2.0);
112 };
113 const auto compute_d = [](VectorXd &d, const MatrixXd& J, const VectorXd& np)
114 {
115 d = J.adjoint() * np;
116 };
117
118 const auto update_z =
119 [](VectorXd& z, const MatrixXd& J, const VectorXd& d, int iq)
120 {
121 z = J.rightCols(z.size()-iq) * d.tail(d.size()-iq);
122 };
123
124 const auto update_r =
125 [](const MatrixXd& R, VectorXd& r, const VectorXd& d, int iq)
126 {
127 r.head(iq) =
128 R.topLeftCorner(iq,iq).triangularView<Upper>().solve(d.head(iq));
129 };
130
131 const auto add_constraint = [&distance](
132 MatrixXd& R,
133 MatrixXd& J,
134 VectorXd& d,
135 int& iq,
136 double& R_norm)->bool
137 {
138 int n=J.rows();
139 #ifdef TRACE_SOLVER
140 std::cerr << "Add constraint " << iq << '/';
141 #endif
142 int i, j, k;
143 double cc, ss, h, t1, t2, xny;
144
145 /* we have to find the Givens rotation which will reduce the element
146 d(j) to zero.
147 if it is already zero we don't have to do anything, except of
148 decreasing j */
149 for (j = n - 1; j >= iq + 1; j--)
150 {
151 /* The Givens rotation is done with the matrix (cc cs, cs -cc).
152 If cc is one, then element (j) of d is zero compared with element
153 (j - 1). Hence we don't have to do anything.
154 If cc is zero, then we just have to switch column (j) and column (j - 1)
155 of J. Since we only switch columns in J, we have to be careful how we
156 update d depending on the sign of gs.
157 Otherwise we have to apply the Givens rotation to these columns.
158 The i - 1 element of d has to be updated to h. */
159 cc = d(j - 1);
160 ss = d(j);
161 h = distance(cc, ss);
162 if (h == 0.0)
163 continue;
164 d(j) = 0.0;
165 ss = ss / h;
166 cc = cc / h;
167 if (cc < 0.0)
168 {
169 cc = -cc;
170 ss = -ss;
171 d(j - 1) = -h;
172 }
173 else
174 d(j - 1) = h;
175 xny = ss / (1.0 + cc);
176 for (k = 0; k < n; k++)
177 {
178 t1 = J(k,j - 1);
179 t2 = J(k,j);
180 J(k,j - 1) = t1 * cc + t2 * ss;
181 J(k,j) = xny * (t1 + J(k,j - 1)) - t2;
182 }
183 }
184 /* update the number of constraints added*/
185 iq++;
186 /* To update R we have to put the iq components of the d vector
187 into column iq - 1 of R
188 */
189 R.col(iq-1).head(iq) = d.head(iq);
190 #ifdef TRACE_SOLVER
191 std::cerr << iq << std::endl;
192 #endif
193
194 if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
195 {
196 // problem degenerate
197 return false;
198 }
199 R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
200 return true;
201 };
202
203 const auto delete_constraint = [&distance](
204 MatrixXd& R,
205 MatrixXd& J,
206 VectorXi& A,
207 VectorXd& u,
208 int p,
209 int& iq,
210 int l)
211 {
212 int n = R.rows();
213 #ifdef TRACE_SOLVER
214 std::cerr << "Delete constraint " << l << ' ' << iq;
215 #endif
216 int i, j, k, qq;
217 double cc, ss, h, xny, t1, t2;
218
219 /* Find the index qq for active constraint l to be removed */
220 for (i = p; i < iq; i++)
221 if (A(i) == l)
222 {
223 qq = i;
224 break;
225 }
226
227 /* remove the constraint from the active set and the duals */
228 for (i = qq; i < iq - 1; i++)
229 {
230 A(i) = A(i + 1);
231 u(i) = u(i + 1);
232 R.col(i) = R.col(i+1);
233 }
234
235 A(iq - 1) = A(iq);
236 u(iq - 1) = u(iq);
237 A(iq) = 0;
238 u(iq) = 0.0;
239 for (j = 0; j < iq; j++)
240 R(j,iq - 1) = 0.0;
241 /* constraint has been fully removed */
242 iq--;
243 #ifdef TRACE_SOLVER
244 std::cerr << '/' << iq << std::endl;
245 #endif
246
247 if (iq == 0)
248 return;
249
250 for (j = qq; j < iq; j++)
251 {
252 cc = R(j,j);
253 ss = R(j + 1,j);
254 h = distance(cc, ss);
255 if (h == 0.0)
256 continue;
257 cc = cc / h;
258 ss = ss / h;
259 R(j + 1,j) = 0.0;
260 if (cc < 0.0)
261 {
262 R(j,j) = -h;
263 cc = -cc;
264 ss = -ss;
265 }
266 else
267 R(j,j) = h;
268
269 xny = ss / (1.0 + cc);
270 for (k = j + 1; k < iq; k++)
271 {
272 t1 = R(j,k);
273 t2 = R(j + 1,k);
274 R(j,k) = t1 * cc + t2 * ss;
275 R(j + 1,k) = xny * (t1 + R(j,k)) - t2;
276 }
277 for (k = 0; k < n; k++)
278 {
279 t1 = J(k,j);
280 t2 = J(k,j + 1);
281 J(k,j) = t1 * cc + t2 * ss;
282 J(k,j + 1) = xny * (J(k,j) + t1) - t2;
283 }
284 }
285 };
286
287 int i, j, k, l; /* indices */
288 int ip, me, mi;
289 int n=g0.size(); int p=ce0.size(); int m=ci0.size();
290 MatrixXd R(G.rows(),G.cols()), J(G.rows(),G.cols());
291
292 LLT<MatrixXd,Lower> chol(G.cols());
293
294 VectorXd s(m+p), z(n), r(m + p), d(n), np(n), u(m + p);
295 VectorXd x_old(n), u_old(m + p);
296 double f_value, psi, c1, c2, sum, ss, R_norm;
297 const double inf = std::numeric_limits<double>::infinity();
298 double t, t1, t2; /* t is the step length, which is the minimum of the partial step length t1
299 * and the full step length t2 */
300 VectorXi A(m + p), A_old(m + p), iai(m + p);
301 int q;
302 int iq, iter = 0;
303 std::vector<bool> iaexcl(m + p);
304
305 me = p; /* number of equality constraints */
306 mi = m; /* number of inequality constraints */
307 q = 0; /* size of the active set A (containing the indices of the active constraints) */
308
309 /*
310 * Preprocessing phase
311 */
312
313 /* compute the trace of the original matrix G */
314 c1 = G.trace();
315
316 /* decompose the matrix G in the form LL^T */
317 chol.compute(G);
318
319 /* initialize the matrix R */
320 d.setZero();
321 R.setZero();
322 R_norm = 1.0; /* this variable will hold the norm of the matrix R */
323
324 /* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
325 // J = L^-T
326 J.setIdentity();
327 J = chol.matrixU().solve(J);
328 c2 = J.trace();
329 #ifdef TRACE_SOLVER
330 print_matrix("J", J, n);
331 #endif
332
333 /* c1 * c2 is an estimate for cond(G) */
334
335 /*
336 * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
337 * this is a feasible point in the dual space
338 * x = G^-1 * g0
339 */
340 x = chol.solve(g0);
341 x = -x;
342 /* and compute the current solution value */
343 f_value = 0.5 * g0.dot(x);
344 #ifdef TRACE_SOLVER
345 std::cerr << "Unconstrained solution: " << f_value << std::endl;
346 print_vector("x", x, n);
347 #endif
348
349 /* Add equality constraints to the working set A */
350 iq = 0;
351 for (i = 0; i < me; i++)
352 {
353 np = CE.col(i);
354 compute_d(d, J, np);
355 update_z(z, J, d, iq);
356 update_r(R, r, d, iq);
357 #ifdef TRACE_SOLVER
358 print_matrix("R", R, iq);
359 print_vector("z", z, n);
360 print_vector("r", r, iq);
361 print_vector("d", d, n);
362 #endif
363
364 /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
365 becomes feasible */
366 t2 = 0.0;
367 if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
368 t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
369
370 x += t2 * z;
371
372 /* set u = u+ */
373 u(iq) = t2;
374 u.head(iq) -= t2 * r.head(iq);
375
376 /* compute the new solution value */
377 f_value += 0.5 * (t2 * t2) * z.dot(np);
378 A(i) = -i - 1;
379
380 if (!add_constraint(R, J, d, iq, R_norm))
381 {
382 // FIXME: it should raise an error
383 // Equality constraints are linearly dependent
384 return false;
385 }
386 }
387
388 /* set iai = K \ A */
389 for (i = 0; i < mi; i++)
390 iai(i) = i;
391
392 l1: iter++;
393 #ifdef TRACE_SOLVER
394 print_vector("x", x, n);
395 #endif
396 /* step 1: choose a violated constraint */
397 for (i = me; i < iq; i++)
398 {
399 ip = A(i);
400 iai(ip) = -1;
401 }
402
403 /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
404 ss = 0.0;
405 psi = 0.0; /* this value will contain the sum of all infeasibilities */
406 ip = 0; /* ip will be the index of the chosen violated constraint */
407 for (i = 0; i < mi; i++)
408 {
409 iaexcl[i] = true;
410 sum = CI.col(i).dot(x) + ci0(i);
411 s(i) = sum;
412 psi += std::min(0.0, sum);
413 }
414 #ifdef TRACE_SOLVER
415 print_vector("s", s, mi);
416 #endif
417
418
419 if (std::abs(psi) <= mi * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
420 {
421 /* numerically there are not infeasibilities anymore */
422 q = iq;
423 return true;
424 }
425
426 /* save old values for u, x and A */
427 u_old.head(iq) = u.head(iq);
428 A_old.head(iq) = A.head(iq);
429 x_old = x;
430
431 l2: /* Step 2: check for feasibility and determine a new S-pair */
432 for (i = 0; i < mi; i++)
433 {
434 if (s(i) < ss && iai(i) != -1 && iaexcl[i])
435 {
436 ss = s(i);
437 ip = i;
438 }
439 }
440 if (ss >= 0.0)
441 {
442 q = iq;
443 return true;
444 }
445
446 /* set np = n(ip) */
447 np = CI.col(ip);
448 /* set u = (u 0)^T */
449 u(iq) = 0.0;
450 /* add ip to the active set A */
451 A(iq) = ip;
452
453 #ifdef TRACE_SOLVER
454 std::cerr << "Trying with constraint " << ip << std::endl;
455 print_vector("np", np, n);
456 #endif
457
458 l2a:/* Step 2a: determine step direction */
459 /* compute z = H np: the step direction in the primal space (through J, see the paper) */
460 compute_d(d, J, np);
461 update_z(z, J, d, iq);
462 /* compute N* np (if q > 0): the negative of the step direction in the dual space */
463 update_r(R, r, d, iq);
464 #ifdef TRACE_SOLVER
465 std::cerr << "Step direction z" << std::endl;
466 print_vector("z", z, n);
467 print_vector("r", r, iq + 1);
468 print_vector("u", u, iq + 1);
469 print_vector("d", d, n);
470 print_ivector("A", A, iq + 1);
471 #endif
472
473 /* Step 2b: compute step length */
474 l = 0;
475 /* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
476 t1 = inf; /* +inf */
477 /* find the index l s.t. it reaches the minimum of u+(x) / r */
478 for (k = me; k < iq; k++)
479 {
480 double tmp;
481 if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1) )
482 {
483 t1 = tmp;
484 l = A(k);
485 }
486 }
487 /* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
488 if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
489 t2 = -s(ip) / z.dot(np);
490 else
491 t2 = inf; /* +inf */
492
493 /* the step is chosen as the minimum of t1 and t2 */
494 t = std::min(t1, t2);
495 #ifdef TRACE_SOLVER
496 std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
497 #endif
498
499 /* Step 2c: determine new S-pair and take step: */
500
501 /* case (i): no step in primal or dual space */
502 if (t >= inf)
503 {
504 /* QPP is infeasible */
505 // FIXME: unbounded to raise
506 q = iq;
507 return false;
508 }
509 /* case (ii): step in dual space */
510 if (t2 >= inf)
511 {
512 /* set u = u + t * [-r 1) and drop constraint l from the active set A */
513 u.head(iq) -= t * r.head(iq);
514 u(iq) += t;
515 iai(l) = l;
516 delete_constraint(R, J, A, u, p, iq, l);
517 #ifdef TRACE_SOLVER
518 std::cerr << " in dual space: "
519 << f_value << std::endl;
520 print_vector("x", x, n);
521 print_vector("z", z, n);
522 print_ivector("A", A, iq + 1);
523 #endif
524 goto l2a;
525 }
526
527 /* case (iii): step in primal and dual space */
528
529 x += t * z;
530 /* update the solution value */
531 f_value += t * z.dot(np) * (0.5 * t + u(iq));
532
533 u.head(iq) -= t * r.head(iq);
534 u(iq) += t;
535 #ifdef TRACE_SOLVER
536 std::cerr << " in both spaces: "
537 << f_value << std::endl;
538 print_vector("x", x, n);
539 print_vector("u", u, iq + 1);
540 print_vector("r", r, iq + 1);
541 print_ivector("A", A, iq + 1);
542 #endif
543
544 if (t == t2)
545 {
546 #ifdef TRACE_SOLVER
547 std::cerr << "Full step has taken " << t << std::endl;
548 print_vector("x", x, n);
549 #endif
550 /* full step has taken */
551 /* add constraint ip to the active set*/
552 if (!add_constraint(R, J, d, iq, R_norm))
553 {
554 iaexcl[ip] = false;
555 delete_constraint(R, J, A, u, p, iq, ip);
556 #ifdef TRACE_SOLVER
557 print_matrix("R", R, n);
558 print_ivector("A", A, iq);
559 #endif
560 for (i = 0; i < m; i++)
561 iai(i) = i;
562 for (i = 0; i < iq; i++)
563 {
564 A(i) = A_old(i);
565 iai(A(i)) = -1;
566 u(i) = u_old(i);
567 }
568 x = x_old;
569 goto l2; /* go to step 2 */
570 }
571 else
572 iai(ip) = -1;
573 #ifdef TRACE_SOLVER
574 print_matrix("R", R, n);
575 print_ivector("A", A, iq);
576 #endif
577 goto l1;
578 }
579
580 /* a patial step has taken */
581 #ifdef TRACE_SOLVER
582 std::cerr << "Partial step has taken " << t << std::endl;
583 print_vector("x", x, n);
584 #endif
585 /* drop constraint l */
586 iai(l) = l;
587 delete_constraint(R, J, A, u, p, iq, l);
588 #ifdef TRACE_SOLVER
589 print_matrix("R", R, n);
590 print_ivector("A", A, iq);
591 #endif
592
593 s(ip) = CI.col(ip).dot(x) + ci0(ip);
594
595 #ifdef TRACE_SOLVER
596 print_vector("s", s, mi);
597 #endif
598 goto l2a;
599 }
600