eiquadprog.cpp 13.8 KB
Newer Older
1
#include <eiquadprog/eiquadprog.hpp>
2
3
4
namespace eiquadprog {
namespace solvers {

5
6
7
using namespace Eigen;

/* solve_quadprog is used for on-demand QP solving */
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

double solve_quadprog(MatrixXd &G, VectorXd &g0, const MatrixXd &CE,
                      const VectorXd &ce0, const MatrixXd &CI,
                      const VectorXd &ci0, VectorXd &x, VectorXi &activeSet,
                      size_t &activeSetSize) {
  Eigen::DenseIndex p = CE.cols();
  Eigen::DenseIndex m = CI.cols();

  VectorXd y(p + m);

  return solve_quadprog(G, g0, CE, ce0, CI, ci0, x, y, activeSet,
                        activeSetSize);
}

double solve_quadprog(MatrixXd &G, VectorXd &g0, const MatrixXd &CE,
                      const VectorXd &ce0, const MatrixXd &CI,
                      const VectorXd &ci0, VectorXd &x, VectorXd &y,
                      VectorXi &activeSet, size_t &activeSetSize) {
26
27
28
29
30
31
32
33
  LLT<MatrixXd, Lower> chol(G.cols());
  double c1;
  /* compute the trace of the original matrix G */
  c1 = G.trace();

  /* decompose the matrix G in the form LL^T */
  chol.compute(G);

34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
  return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, y, activeSet,
                        activeSetSize);
}

double solve_quadprog(LLT<MatrixXd, Lower> &chol, double c1, VectorXd &g0,
                      const MatrixXd &CE, const VectorXd &ce0,
                      const MatrixXd &CI, const VectorXd &ci0, VectorXd &x,
                      VectorXi &activeSet, size_t &activeSetSize) {
  Eigen::DenseIndex p = CE.cols();
  Eigen::DenseIndex m = CI.cols();

  VectorXd y(p + m);

  return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, y, activeSet,
                        activeSetSize);
49
50
}

51
52
/* solve_quadprog2 is used for when the Cholesky decomposition of G is
 * pre-computed
53
54
55
 * @param A Output vector containing the indexes of the active constraints.
 * @param q Output value representing the size of the active set.
 */
56
57
58
59
double solve_quadprog(LLT<MatrixXd, Lower> &chol, double c1, VectorXd &g0,
                      const MatrixXd &CE, const VectorXd &ce0,
                      const MatrixXd &CI, const VectorXd &ci0, VectorXd &x,
                      VectorXd &u, VectorXi &A, size_t &q) {
60
61
62
63
64
65
66
  size_t i, k, l; /* indices */
  size_t ip, me, mi;
  size_t n = g0.size();
  size_t p = CE.cols();
  size_t m = CI.cols();
  MatrixXd R(g0.size(), g0.size()), J(g0.size(), g0.size());

67
  VectorXd s(m + p), z(n), r(m + p), d(n), np(n);
68
69
70
  VectorXd x_old(n), u_old(m + p);
  double f_value, psi, c2, sum, ss, R_norm;
  const double inf = std::numeric_limits<double>::infinity();
71
72
73
74
  double t, t1, t2; /* t is the step length, which is the minimum of the partial
                     * step length t1 and the full step length t2 */
  //        VectorXi A(m + p); // Del Prete: active set is now an output
  //        parameter
75
  if (static_cast<size_t>(A.size()) != m + p) A.resize(m + p);
76
77
78
79
80
81
  VectorXi A_old(m + p), iai(m + p), iaexcl(m + p);
  //        int q;
  size_t iq, iter = 0;

  me = p; /* number of equality constraints */
  mi = m; /* number of inequality constraints */
82
83
  q = 0;  /* size of the active set A (containing the indices of the active
             constraints) */
84
85
86
87
88
89
90
91
92
93

  /*
   * Preprocessing phase
   */

  /* initialize the matrix R */
  d.setZero();
  R.setZero();
  R_norm = 1.0; /* this variable will hold the norm of the matrix R */

94
95
  /* compute the inverse of the factorized matrix G^-1, this is the initial
   * value for H */
96
97
  // J = L^-T
  J.setIdentity();
98
  chol.matrixU().solveInPlace(J);
99
  c2 = J.trace();
100
#ifdef EIQGUADPROG_TRACE_SOLVER
101
  utils::print_matrix("J", J);
102
103
104
105
106
107
108
109
110
#endif

  /* c1 * c2 is an estimate for cond(G) */

  /*
   * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
   * this is a feasible point in the dual space
   * x = G^-1 * g0
   */
111
112
  x = -g0;
  chol.solveInPlace(x);
113
114
  /* and compute the current solution value */
  f_value = 0.5 * g0.dot(x);
115
#ifdef EIQGUADPROG_TRACE_SOLVER
116
  std::cerr << "Unconstrained solution: " << f_value << std::endl;
117
  utils::print_vector("x", x);
118
119
120
121
122
123
124
125
126
#endif

  /* Add equality constraints to the working set A */
  iq = 0;
  for (i = 0; i < me; i++) {
    np = CE.col(i);
    compute_d(d, J, np);
    update_z(z, J, d, iq);
    update_r(R, r, d, iq);
127
#ifdef EIQGUADPROG_TRACE_SOLVER
128
129
130
131
    utils::print_matrix("R", R);
    utils::print_vector("z", z);
    utils::print_vector("r", r);
    utils::print_vector("d", d);
132
133
#endif

134
135
    /* compute full step length t2: i.e., the minimum step in primal space s.t.
       the contraint becomes feasible */
136
    t2 = 0.0;
137
    if (std::abs(z.dot(z)) >
138
        std::numeric_limits<double>::epsilon())  // i.e. z != 0
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
      t2 = (-np.dot(x) - ce0(i)) / z.dot(np);

    x += t2 * z;

    /* set u = u+ */
    u(iq) = t2;
    u.head(iq) -= t2 * r.head(iq);

    /* compute the new solution value */
    f_value += 0.5 * (t2 * t2) * z.dot(np);
    A(i) = static_cast<VectorXi::Scalar>(-i - 1);

    if (!add_constraint(R, J, d, iq, R_norm)) {
      // FIXME: it should raise an error
      // Equality constraints are linearly dependent
      return f_value;
    }
  }

  /* set iai = K \ A */
159
  for (i = 0; i < mi; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
160
161
162

l1:
  iter++;
163
#ifdef EIQGUADPROG_TRACE_SOLVER
164
  utils::print_vector("x", x);
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
#endif
  /* step 1: choose a violated constraint */
  for (i = me; i < iq; i++) {
    ip = A(i);
    iai(ip) = -1;
  }

  /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
  ss = 0.0;
  psi = 0.0; /* this value will contain the sum of all infeasibilities */
  ip = 0;    /* ip will be the index of the chosen violated constraint */
  for (i = 0; i < mi; i++) {
    iaexcl(i) = 1;
    sum = CI.col(i).dot(x) + ci0(i);
    s(i) = sum;
    psi += std::min(0.0, sum);
  }
182
#ifdef EIQGUADPROG_TRACE_SOLVER
183
  utils::print_vector("s", s);
184
185
#endif

186
187
188
  if (std::abs(psi) <= static_cast<double>(mi) *
                           std::numeric_limits<double>::epsilon() * c1 * c2 *
                           100.0) {
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
    /* numerically there are not infeasibilities anymore */
    q = iq;
    return f_value;
  }

  /* save old values for u, x and A */
  u_old.head(iq) = u.head(iq);
  A_old.head(iq) = A.head(iq);
  x_old = x;

l2: /* Step 2: check for feasibility and determine a new S-pair */
  for (i = 0; i < mi; i++) {
    if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
      ss = s(i);
      ip = i;
    }
  }
  if (ss >= 0.0) {
    q = iq;
    return f_value;
  }

  /* set np = n(ip) */
  np = CI.col(ip);
  /* set u = (u 0)^T */
  u(iq) = 0.0;
  /* add ip to the active set A */
  A(iq) = static_cast<VectorXi::Scalar>(ip);

218
#ifdef EIQGUADPROG_TRACE_SOLVER
219
  std::cerr << "Trying with constraint " << ip << std::endl;
220
  utils::print_vector("np", np);
221
222
223
#endif

l2a: /* Step 2a: determine step direction */
224
225
  /* compute z = H np: the step direction in the primal space (through J, see
   * the paper) */
226
227
  compute_d(d, J, np);
  update_z(z, J, d, iq);
228
229
  /* compute N* np (if q > 0): the negative of the step direction in the dual
   * space */
230
  update_r(R, r, d, iq);
231
#ifdef EIQGUADPROG_TRACE_SOLVER
232
  std::cerr << "Step direction z" << std::endl;
233
234
235
236
237
  utils::print_vector("z", z);
  utils::print_vector("r", r);
  utils::print_vector("u", u);
  utils::print_vector("d", d);
  utils::print_vector("A", A);
238
239
240
241
#endif

  /* Step 2b: compute step length */
  l = 0;
242
243
  /* Compute t1: partial step length (maximum step in dual space without
   * violating dual feasibility */
244
245
246
247
248
249
250
251
252
  t1 = inf; /* +inf */
  /* find the index l s.t. it reaches the minimum of u+(x) / r */
  for (k = me; k < iq; k++) {
    double tmp;
    if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1)) {
      t1 = tmp;
      l = A(k);
    }
  }
253
254
255
  /* Compute t2: full step length (minimum step in primal space such that the
   * constraint ip becomes feasible */
  if (std::abs(z.dot(z)) >
256
      std::numeric_limits<double>::epsilon())  // i.e. z != 0
257
258
259
260
261
262
    t2 = -s(ip) / z.dot(np);
  else
    t2 = inf; /* +inf */

  /* the step is chosen as the minimum of t1 and t2 */
  t = std::min(t1, t2);
263
264
265
#ifdef EIQGUADPROG_TRACE_SOLVER
  std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2
            << ") ";
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
#endif

  /* Step 2c: determine new S-pair and take step: */

  /* case (i): no step in primal or dual space */
  if (t >= inf) {
    /* QPP is infeasible */
    // FIXME: unbounded to raise
    q = iq;
    return inf;
  }
  /* case (ii): step in dual space */
  if (t2 >= inf) {
    /* set u = u +  t * [-r 1) and drop constraint l from the active set A */
    u.head(iq) -= t * r.head(iq);
    u(iq) += t;
    iai(l) = static_cast<VectorXi::Scalar>(l);
    delete_constraint(R, J, A, u, p, iq, l);
284
#ifdef EIQGUADPROG_TRACE_SOLVER
285
    std::cerr << " in dual space: " << f_value << std::endl;
286
287
288
    utils::print_vector("x", x);
    utils::print_vector("z", z);
    utils::print_vector("A", A);
289
290
291
292
293
294
295
296
297
298
299
300
#endif
    goto l2a;
  }

  /* case (iii): step in primal and dual space */

  x += t * z;
  /* update the solution value */
  f_value += t * z.dot(np) * (0.5 * t + u(iq));

  u.head(iq) -= t * r.head(iq);
  u(iq) += t;
301
#ifdef EIQGUADPROG_TRACE_SOLVER
302
  std::cerr << " in both spaces: " << f_value << std::endl;
303
304
305
306
  utils::print_vector("x", x);
  utils::print_vector("u", u);
  utils::print_vector("r", r);
  utils::print_vector("A", A);
307
308
309
#endif

  if (t == t2) {
310
#ifdef EIQGUADPROG_TRACE_SOLVER
311
    std::cerr << "Full step has taken " << t << std::endl;
312
    utils::print_vector("x", x);
313
314
315
316
317
318
#endif
    /* full step has taken */
    /* add constraint ip to the active set*/
    if (!add_constraint(R, J, d, iq, R_norm)) {
      iaexcl(ip) = 0;
      delete_constraint(R, J, A, u, p, iq, ip);
319
#ifdef EIQGUADPROG_TRACE_SOLVER
320
321
      utils::print_matrix("R", R);
      utils::print_vector("A", A);
322
#endif
323
      for (i = 0; i < m; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
324
325
326
327
328
329
330
331
332
      for (i = 0; i < iq; i++) {
        A(i) = A_old(i);
        iai(A(i)) = -1;
        u(i) = u_old(i);
      }
      x = x_old;
      goto l2; /* go to step 2 */
    } else
      iai(ip) = -1;
333
#ifdef EIQGUADPROG_TRACE_SOLVER
334
335
    utils::print_matrix("R", R);
    utils::print_vector("A", A);
336
337
338
339
340
#endif
    goto l1;
  }

  /* a patial step has taken */
341
#ifdef EIQGUADPROG_TRACE_SOLVER
342
  std::cerr << "Partial step has taken " << t << std::endl;
343
  utils::print_vector("x", x);
344
345
346
347
#endif
  /* drop constraint l */
  iai(l) = static_cast<VectorXi::Scalar>(l);
  delete_constraint(R, J, A, u, p, iq, l);
348
#ifdef EIQGUADPROG_TRACE_SOLVER
349
350
  utils::print_matrix("R", R);
  utils::print_vector("A", A);
351
352
353
354
#endif

  s(ip) = CI.col(ip).dot(x) + ci0(ip);

355
#ifdef EIQGUADPROG_TRACE_SOLVER
356
  utils::print_vector("s", s);
357
358
359
360
#endif
  goto l2a;
}

361
362
bool add_constraint(MatrixXd &R, MatrixXd &J, VectorXd &d, size_t &iq,
                    double &R_norm) {
363
  size_t n = J.rows();
364
#ifdef EIQGUADPROG_TRACE_SOLVER
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
  std::cerr << "Add constraint " << iq << '/';
#endif
  size_t j, k;
  double cc, ss, h, t1, t2, xny;

  /* we have to find the Givens rotation which will reduce the element
     d(j) to zero.
     if it is already zero we don't have to do anything, except of
     decreasing j */
  for (j = n - 1; j >= iq + 1; j--) {
    /* The Givens rotation is done with the matrix (cc cs, cs -cc).
       If cc is one, then element (j) of d is zero compared with element
       (j - 1). Hence we don't have to do anything.
       If cc is zero, then we just have to switch column (j) and column (j - 1)
       of J. Since we only switch columns in J, we have to be careful how we
       update d depending on the sign of gs.
       Otherwise we have to apply the Givens rotation to these columns.
       The i - 1 element of d has to be updated to h. */
    cc = d(j - 1);
    ss = d(j);
Justin Carpentier's avatar
Justin Carpentier committed
385
    h = utils::distance(cc, ss);
386
    if (h == 0.0) continue;
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
    d(j) = 0.0;
    ss = ss / h;
    cc = cc / h;
    if (cc < 0.0) {
      cc = -cc;
      ss = -ss;
      d(j - 1) = -h;
    } else
      d(j - 1) = h;
    xny = ss / (1.0 + cc);
    for (k = 0; k < n; k++) {
      t1 = J(k, j - 1);
      t2 = J(k, j);
      J(k, j - 1) = t1 * cc + t2 * ss;
      J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
    }
  }
  /* update the number of constraints added*/
  iq++;
  /* To update R we have to put the iq components of the d vector
     into column iq - 1 of R
  */
  R.col(iq - 1).head(iq) = d.head(iq);
410
#ifdef EIQGUADPROG_TRACE_SOLVER
411
412
413
414
415
416
417
418
419
420
  std::cerr << iq << std::endl;
#endif

  if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
    // problem degenerate
    return false;
  R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
  return true;
}

421
422
void delete_constraint(MatrixXd &R, MatrixXd &J, VectorXi &A, VectorXd &u,
                       size_t p, size_t &iq, size_t l) {
423
  size_t n = R.rows();
424
#ifdef EIQGUADPROG_TRACE_SOLVER
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
  std::cerr << "Delete constraint " << l << ' ' << iq;
#endif
  size_t i, j, k, qq = 0;
  double cc, ss, h, xny, t1, t2;

  /* Find the index qq for active constraint l to be removed */
  for (i = p; i < iq; i++)
    if (static_cast<size_t>(A(i)) == l) {
      qq = i;
      break;
    }

  /* remove the constraint from the active set and the duals */
  for (i = qq; i < iq - 1; i++) {
    A(i) = A(i + 1);
    u(i) = u(i + 1);
    R.col(i) = R.col(i + 1);
  }

  A(iq - 1) = A(iq);
  u(iq - 1) = u(iq);
  A(iq) = 0;
  u(iq) = 0.0;
448
  for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0;
449
450
  /* constraint has been fully removed */
  iq--;
451
#ifdef EIQGUADPROG_TRACE_SOLVER
452
453
454
  std::cerr << '/' << iq << std::endl;
#endif

455
  if (iq == 0) return;
456
457
458
459

  for (j = qq; j < iq; j++) {
    cc = R(j, j);
    ss = R(j + 1, j);
Justin Carpentier's avatar
Justin Carpentier committed
460
    h = utils::distance(cc, ss);
461
    if (h == 0.0) continue;
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
    cc = cc / h;
    ss = ss / h;
    R(j + 1, j) = 0.0;
    if (cc < 0.0) {
      R(j, j) = -h;
      cc = -cc;
      ss = -ss;
    } else
      R(j, j) = h;

    xny = ss / (1.0 + cc);
    for (k = j + 1; k < iq; k++) {
      t1 = R(j, k);
      t2 = R(j + 1, k);
      R(j, k) = t1 * cc + t2 * ss;
      R(j + 1, k) = xny * (t1 + R(j, k)) - t2;
    }
    for (k = 0; k < n; k++) {
      t1 = J(k, j);
      t2 = J(k, j + 1);
      J(k, j) = t1 * cc + t2 * ss;
      J(k, j + 1) = xny * (J(k, j) + t1) - t2;
    }
  }
}
487

488
489
}  // namespace solvers
}  // namespace eiquadprog