GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/eiquadprog.cpp Lines: 122 189 64.6 %
Date: 2021-03-10 23:02:29 Branches: 146 344 42.4 %

Line Branch Exec Source
1
#include <eiquadprog/eiquadprog.hpp>
2
namespace eiquadprog {
3
namespace solvers {
4
5
using namespace Eigen;
6
7
/* solve_quadprog is used for on-demand QP solving */
8
10
double solve_quadprog(MatrixXd& G, VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, const MatrixXd& CI,
9
                      const VectorXd& ci0, VectorXd& x, VectorXi& activeSet, size_t& activeSetSize) {
10

20
  LLT<MatrixXd, Lower> chol(G.cols());
11
  double c1;
12
  /* compute the trace of the original matrix G */
13
10
  c1 = G.trace();
14
15
  /* decompose the matrix G in the form LL^T */
16
10
  chol.compute(G);
17
18
20
  return solve_quadprog2(chol, c1, g0, CE, ce0, CI, ci0, x, activeSet, activeSetSize);
19
}
20
21
/* solve_quadprog2 is used for when the Cholesky decomposition of G is pre-computed
22
 * @param A Output vector containing the indexes of the active constraints.
23
 * @param q Output value representing the size of the active set.
24
 */
25
10
double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0,
26
                       const MatrixXd& CI, const VectorXd& ci0, VectorXd& x, VectorXi& A, size_t& q) {
27
  size_t i, k, l; /* indices */
28
  size_t ip, me, mi;
29
10
  size_t n = g0.size();
30
10
  size_t p = CE.cols();
31
10
  size_t m = CI.cols();
32



20
  MatrixXd R(g0.size(), g0.size()), J(g0.size(), g0.size());
33
34



20
  VectorXd s(m + p), z(n), r(m + p), d(n), np(n), u(m + p);
35

20
  VectorXd x_old(n), u_old(m + p);
36
  double f_value, psi, c2, sum, ss, R_norm;
37
10
  const double inf = std::numeric_limits<double>::infinity();
38
  double t, t1, t2; /* t is the step length, which is the minimum of the partial step length t1
39
                     * and the full step length t2 */
40
  //        VectorXi A(m + p); // Del Prete: active set is now an output parameter
41

10
  if (static_cast<size_t>(A.size()) != m + p) A.resize(m + p);
42

20
  VectorXi A_old(m + p), iai(m + p), iaexcl(m + p);
43
  //        int q;
44
10
  size_t iq, iter = 0;
45
46
10
  me = p; /* number of equality constraints */
47
10
  mi = m; /* number of inequality constraints */
48
10
  q = 0;  /* size of the active set A (containing the indices of the active constraints) */
49
50
  /*
51
   * Preprocessing phase
52
   */
53
54
  /* initialize the matrix R */
55
10
  d.setZero();
56
10
  R.setZero();
57
10
  R_norm = 1.0; /* this variable will hold the norm of the matrix R */
58
59
  /* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
60
  // J = L^-T
61
10
  J.setIdentity();
62

10
  J = chol.matrixU().solve(J);
63
10
  c2 = J.trace();
64
#ifdef TRACE_SOLVER
65
  print_matrix("J", J, n);
66
#endif
67
68
  /* c1 * c2 is an estimate for cond(G) */
69
70
  /*
71
   * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
72
   * this is a feasible point in the dual space
73
   * x = G^-1 * g0
74
   */
75

10
  x = chol.solve(g0);
76

10
  x = -x;
77
  /* and compute the current solution value */
78
10
  f_value = 0.5 * g0.dot(x);
79
#ifdef TRACE_SOLVER
80
  std::cerr << "Unconstrained solution: " << f_value << std::endl;
81
  print_vector("x", x, n);
82
#endif
83
84
  /* Add equality constraints to the working set A */
85
10
  iq = 0;
86
14
  for (i = 0; i < me; i++) {
87

5
    np = CE.col(i);
88
5
    compute_d(d, J, np);
89
5
    update_z(z, J, d, iq);
90
5
    update_r(R, r, d, iq);
91
#ifdef TRACE_SOLVER
92
    print_matrix("R", R, iq);
93
    print_vector("z", z, n);
94
    print_vector("r", r, iq);
95
    print_vector("d", d, n);
96
#endif
97
98
    /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
99
       becomes feasible */
100
5
    t2 = 0.0;
101

5
    if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())  // i.e. z != 0
102

4
      t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
103
104

5
    x += t2 * z;
105
106
    /* set u = u+ */
107
5
    u(iq) = t2;
108


5
    u.head(iq) -= t2 * r.head(iq);
109
110
    /* compute the new solution value */
111
5
    f_value += 0.5 * (t2 * t2) * z.dot(np);
112
5
    A(i) = static_cast<VectorXi::Scalar>(-i - 1);
113
114

5
    if (!add_constraint(R, J, d, iq, R_norm)) {
115
      // FIXME: it should raise an error
116
      // Equality constraints are linearly dependent
117
1
      return f_value;
118
    }
119
  }
120
121
  /* set iai = K \ A */
122

9
  for (i = 0; i < mi; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
123
124
9
l1:
125
14
  iter++;
126
#ifdef TRACE_SOLVER
127
  print_vector("x", x, n);
128
#endif
129
  /* step 1: choose a violated constraint */
130
20
  for (i = me; i < iq; i++) {
131
6
    ip = A(i);
132
6
    iai(ip) = -1;
133
  }
134
135
  /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
136
14
  ss = 0.0;
137
14
  psi = 0.0; /* this value will contain the sum of all infeasibilities */
138
14
  ip = 0;    /* ip will be the index of the chosen violated constraint */
139
34
  for (i = 0; i < mi; i++) {
140
20
    iaexcl(i) = 1;
141

20
    sum = CI.col(i).dot(x) + ci0(i);
142
20
    s(i) = sum;
143
20
    psi += std::min(0.0, sum);
144
  }
145
#ifdef TRACE_SOLVER
146
  print_vector("s", s, mi);
147
#endif
148
149
14
  if (std::abs(psi) <= static_cast<double>(mi) * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
150
    /* numerically there are not infeasibilities anymore */
151
7
    q = iq;
152
7
    return f_value;
153
  }
154
155
  /* save old values for u, x and A */
156

7
  u_old.head(iq) = u.head(iq);
157

7
  A_old.head(iq) = A.head(iq);
158
7
  x_old = x;
159
160
7
l2: /* Step 2: check for feasibility and determine a new S-pair */
161
20
  for (i = 0; i < mi; i++) {
162



13
    if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
163
7
      ss = s(i);
164
7
      ip = i;
165
    }
166
  }
167
7
  if (ss >= 0.0) {
168
    q = iq;
169
    return f_value;
170
  }
171
172
  /* set np = n(ip) */
173

7
  np = CI.col(ip);
174
  /* set u = (u 0)^T */
175
7
  u(iq) = 0.0;
176
  /* add ip to the active set A */
177
7
  A(iq) = static_cast<VectorXi::Scalar>(ip);
178
179
#ifdef TRACE_SOLVER
180
  std::cerr << "Trying with constraint " << ip << std::endl;
181
  print_vector("np", np, n);
182
#endif
183
184
7
l2a: /* Step 2a: determine step direction */
185
  /* compute z = H np: the step direction in the primal space (through J, see the paper) */
186
7
  compute_d(d, J, np);
187
7
  update_z(z, J, d, iq);
188
  /* compute N* np (if q > 0): the negative of the step direction in the dual space */
189
7
  update_r(R, r, d, iq);
190
#ifdef TRACE_SOLVER
191
  std::cerr << "Step direction z" << std::endl;
192
  print_vector("z", z, n);
193
  print_vector("r", r, iq + 1);
194
  print_vector("u", u, iq + 1);
195
  print_vector("d", d, n);
196
  print_vector("A", A, iq + 1);
197
#endif
198
199
  /* Step 2b: compute step length */
200
7
  l = 0;
201
  /* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
202
7
  t1 = inf; /* +inf */
203
  /* find the index l s.t. it reaches the minimum of u+(x) / r */
204
10
  for (k = me; k < iq; k++) {
205
    double tmp;
206



3
    if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1)) {
207
      t1 = tmp;
208
      l = A(k);
209
    }
210
  }
211
  /* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
212

7
  if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())  // i.e. z != 0
213

5
    t2 = -s(ip) / z.dot(np);
214
  else
215
2
    t2 = inf; /* +inf */
216
217
  /* the step is chosen as the minimum of t1 and t2 */
218
7
  t = std::min(t1, t2);
219
#ifdef TRACE_SOLVER
220
  std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
221
#endif
222
223
  /* Step 2c: determine new S-pair and take step: */
224
225
  /* case (i): no step in primal or dual space */
226
7
  if (t >= inf) {
227
    /* QPP is infeasible */
228
    // FIXME: unbounded to raise
229
2
    q = iq;
230
2
    return inf;
231
  }
232
  /* case (ii): step in dual space */
233
5
  if (t2 >= inf) {
234
    /* set u = u +  t * [-r 1) and drop constraint l from the active set A */
235
    u.head(iq) -= t * r.head(iq);
236
    u(iq) += t;
237
    iai(l) = static_cast<VectorXi::Scalar>(l);
238
    delete_constraint(R, J, A, u, p, iq, l);
239
#ifdef TRACE_SOLVER
240
    std::cerr << " in dual space: " << f_value << std::endl;
241
    print_vector("x", x, n);
242
    print_vector("z", z, n);
243
    print_vector("A", A, iq + 1);
244
#endif
245
    goto l2a;
246
  }
247
248
  /* case (iii): step in primal and dual space */
249
250

5
  x += t * z;
251
  /* update the solution value */
252

5
  f_value += t * z.dot(np) * (0.5 * t + u(iq));
253
254


5
  u.head(iq) -= t * r.head(iq);
255
5
  u(iq) += t;
256
#ifdef TRACE_SOLVER
257
  std::cerr << " in both spaces: " << f_value << std::endl;
258
  print_vector("x", x, n);
259
  print_vector("u", u, iq + 1);
260
  print_vector("r", r, iq + 1);
261
  print_vector("A", A, iq + 1);
262
#endif
263
264
5
  if (t == t2) {
265
#ifdef TRACE_SOLVER
266
    std::cerr << "Full step has taken " << t << std::endl;
267
    print_vector("x", x, n);
268
#endif
269
    /* full step has taken */
270
    /* add constraint ip to the active set*/
271

5
    if (!add_constraint(R, J, d, iq, R_norm)) {
272
      iaexcl(ip) = 0;
273
      delete_constraint(R, J, A, u, p, iq, ip);
274
#ifdef TRACE_SOLVER
275
      print_matrix("R", R, n);
276
      print_vector("A", A, iq);
277
#endif
278
      for (i = 0; i < m; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
279
      for (i = 0; i < iq; i++) {
280
        A(i) = A_old(i);
281
        iai(A(i)) = -1;
282
        u(i) = u_old(i);
283
      }
284
      x = x_old;
285
      goto l2; /* go to step 2 */
286
    } else
287
5
      iai(ip) = -1;
288
#ifdef TRACE_SOLVER
289
    print_matrix("R", R, n);
290
    print_vector("A", A, iq);
291
#endif
292
5
    goto l1;
293
  }
294
295
  /* a patial step has taken */
296
#ifdef TRACE_SOLVER
297
  std::cerr << "Partial step has taken " << t << std::endl;
298
  print_vector("x", x, n);
299
#endif
300
  /* drop constraint l */
301
  iai(l) = static_cast<VectorXi::Scalar>(l);
302
  delete_constraint(R, J, A, u, p, iq, l);
303
#ifdef TRACE_SOLVER
304
  print_matrix("R", R, n);
305
  print_vector("A", A, iq);
306
#endif
307
308
  s(ip) = CI.col(ip).dot(x) + ci0(ip);
309
310
#ifdef TRACE_SOLVER
311
  print_vector("s", s, mi);
312
#endif
313
  goto l2a;
314
}
315
316
10
bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R_norm) {
317
10
  size_t n = J.rows();
318
#ifdef TRACE_SOLVER
319
  std::cerr << "Add constraint " << iq << '/';
320
#endif
321
  size_t j, k;
322
  double cc, ss, h, t1, t2, xny;
323
324
  /* we have to find the Givens rotation which will reduce the element
325
     d(j) to zero.
326
     if it is already zero we don't have to do anything, except of
327
     decreasing j */
328
16
  for (j = n - 1; j >= iq + 1; j--) {
329
    /* The Givens rotation is done with the matrix (cc cs, cs -cc).
330
       If cc is one, then element (j) of d is zero compared with element
331
       (j - 1). Hence we don't have to do anything.
332
       If cc is zero, then we just have to switch column (j) and column (j - 1)
333
       of J. Since we only switch columns in J, we have to be careful how we
334
       update d depending on the sign of gs.
335
       Otherwise we have to apply the Givens rotation to these columns.
336
       The i - 1 element of d has to be updated to h. */
337
6
    cc = d(j - 1);
338
6
    ss = d(j);
339
6
    h = distance(cc, ss);
340
6
    if (h == 0.0) continue;
341
6
    d(j) = 0.0;
342
6
    ss = ss / h;
343
6
    cc = cc / h;
344
6
    if (cc < 0.0) {
345
      cc = -cc;
346
      ss = -ss;
347
      d(j - 1) = -h;
348
    } else
349
6
      d(j - 1) = h;
350
6
    xny = ss / (1.0 + cc);
351
18
    for (k = 0; k < n; k++) {
352
12
      t1 = J(k, j - 1);
353
12
      t2 = J(k, j);
354
12
      J(k, j - 1) = t1 * cc + t2 * ss;
355
12
      J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
356
    }
357
  }
358
  /* update the number of constraints added*/
359
10
  iq++;
360
  /* To update R we have to put the iq components of the d vector
361
     into column iq - 1 of R
362
  */
363

10
  R.col(iq - 1).head(iq) = d.head(iq);
364
#ifdef TRACE_SOLVER
365
  std::cerr << iq << std::endl;
366
#endif
367
368
10
  if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
369
    // problem degenerate
370
1
    return false;
371
9
  R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
372
9
  return true;
373
}
374
375
void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_t p, size_t& iq, size_t l) {
376
  size_t n = R.rows();
377
#ifdef TRACE_SOLVER
378
  std::cerr << "Delete constraint " << l << ' ' << iq;
379
#endif
380
  size_t i, j, k, qq = 0;
381
  double cc, ss, h, xny, t1, t2;
382
383
  /* Find the index qq for active constraint l to be removed */
384
  for (i = p; i < iq; i++)
385
    if (static_cast<size_t>(A(i)) == l) {
386
      qq = i;
387
      break;
388
    }
389
390
  /* remove the constraint from the active set and the duals */
391
  for (i = qq; i < iq - 1; i++) {
392
    A(i) = A(i + 1);
393
    u(i) = u(i + 1);
394
    R.col(i) = R.col(i + 1);
395
  }
396
397
  A(iq - 1) = A(iq);
398
  u(iq - 1) = u(iq);
399
  A(iq) = 0;
400
  u(iq) = 0.0;
401
  for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0;
402
  /* constraint has been fully removed */
403
  iq--;
404
#ifdef TRACE_SOLVER
405
  std::cerr << '/' << iq << std::endl;
406
#endif
407
408
  if (iq == 0) return;
409
410
  for (j = qq; j < iq; j++) {
411
    cc = R(j, j);
412
    ss = R(j + 1, j);
413
    h = distance(cc, ss);
414
    if (h == 0.0) continue;
415
    cc = cc / h;
416
    ss = ss / h;
417
    R(j + 1, j) = 0.0;
418
    if (cc < 0.0) {
419
      R(j, j) = -h;
420
      cc = -cc;
421
      ss = -ss;
422
    } else
423
      R(j, j) = h;
424
425
    xny = ss / (1.0 + cc);
426
    for (k = j + 1; k < iq; k++) {
427
      t1 = R(j, k);
428
      t2 = R(j + 1, k);
429
      R(j, k) = t1 * cc + t2 * ss;
430
      R(j + 1, k) = xny * (t1 + R(j, k)) - t2;
431
    }
432
    for (k = 0; k < n; k++) {
433
      t1 = J(k, j);
434
      t2 = J(k, j + 1);
435
      J(k, j) = t1 * cc + t2 * ss;
436
      J(k, j + 1) = xny * (J(k, j) + t1) - t2;
437
    }
438
  }
439
}
440
441
}  // namespace solvers
442

12
}  // namespace eiquadprog