GCC Code Coverage Report

Directory: ./
File: src/eiquadprog-fast.cpp
Date: 2024-12-04 10:05:36
Exec Total Coverage
Lines: 151 222 68.0%
Branches: 151 362 41.7%
Line Branch Exec Source
1 #include "eiquadprog/eiquadprog-fast.hpp"
2
3 #include <iostream>
4
5 namespace eiquadprog {
6 namespace solvers {
7
8
14/28
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 12 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 12 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 12 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 12 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 12 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 12 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 12 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 12 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 12 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 12 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 12 times.
✗ Branch 42 not taken.
 12 EiquadprogFast::EiquadprogFast() {
9 12 m_maxIter = DEFAULT_MAX_ITER;
10 12 q = 0; // size of the active set A (containing the indices
11 // of the active constraints)
12 12 is_inverse_provided_ = false;
13 12 m_nVars = 0;
14 12 m_nEqCon = 0;
15 12 m_nIneqCon = 0;
16 12 }
17
18 24 EiquadprogFast::~EiquadprogFast() {}
19
20 12 void EiquadprogFast::reset(size_t nVars, size_t nEqCon, size_t nIneqCon) {
21 12 m_nVars = nVars;
22 12 m_nEqCon = nEqCon;
23 12 m_nIneqCon = nIneqCon;
24 12 m_J.setZero(nVars, nVars);
25 12 chol_.compute(m_J);
26 12 R.resize(nVars, nVars);
27 12 s.resize(nIneqCon);
28 12 r.resize(nIneqCon + nEqCon);
29 12 u.resize(nIneqCon + nEqCon);
30 12 z.resize(nVars);
31 12 d.resize(nVars);
32 12 np.resize(nVars);
33 12 A.resize(nIneqCon + nEqCon);
34 12 iai.resize(nIneqCon);
35 12 iaexcl.resize(nIneqCon);
36 12 x_old.resize(nVars);
37 12 u_old.resize(nIneqCon + nEqCon);
38 12 A_old.resize(nIneqCon + nEqCon);
39
40 #ifdef OPTIMIZE_ADD_CONSTRAINT
41 T1.resize(nVars);
42 #endif
43 12 }
44
45 10 bool EiquadprogFast::add_constraint(MatrixXd &R, MatrixXd &J, VectorXd &d,
46 size_t &iq, double &R_norm) {
47 10 size_t nVars = J.rows();
48 #ifdef EIQGUADPROG_TRACE_SOLVER
49 std::cerr << "Add constraint " << iq << '/';
50 #endif
51 size_t j, k;
52 double cc, ss, h, t1, t2, xny;
53
54 #ifdef OPTIMIZE_ADD_CONSTRAINT
55 Eigen::Vector2d cc_ss;
56 #endif
57
58 /* we have to find the Givens rotation which will reduce the element
59 d(j) to zero.
60 if it is already zero we don't have to do anything, except of
61 decreasing j */
62
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 10 times.
 16 for (j = d.size() - 1; j >= iq + 1; j--) {
63 /* The Givens rotation is done with the matrix (cc cs, cs -cc).
64 If cc is one, then element (j) of d is zero compared with
65 element (j - 1). Hence we don't have to do anything. If cc is zero, then
66 we just have to switch column (j) and column (j - 1) of J. Since we only
67 switch columns in J, we have to be careful how we update d depending on
68 the sign of gs. Otherwise we have to apply the Givens rotation to these
69 columns. The i - 1 element of d has to be updated to h. */
70 6 cc = d(j - 1);
71 6 ss = d(j);
72 6 h = utils::distance(cc, ss);
73
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
 6 if (h == 0.0) continue;
74 6 d(j) = 0.0;
75 6 ss = ss / h;
76 6 cc = cc / h;
77
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
 6 if (cc < 0.0) {
78 ✗ cc = -cc;
79 ✗ ss = -ss;
80 ✗ d(j - 1) = -h;
81 } else
82 6 d(j - 1) = h;
83 6 xny = ss / (1.0 + cc);
84
85 // #define OPTIMIZE_ADD_CONSTRAINT
86 #ifdef OPTIMIZE_ADD_CONSTRAINT // the optimized code is actually slower than
87 // the original
88 T1 = J.col(j - 1);
89 cc_ss(0) = cc;
90 cc_ss(1) = ss;
91 J.col(j - 1).noalias() = J.middleCols<2>(j - 1) * cc_ss;
92 J.col(j) = xny * (T1 + J.col(j - 1)) - J.col(j);
93 #else
94 // J.col(j-1) = J[:,j-1:j] * [cc; ss]
95
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 6 times.
 18 for (k = 0; k < nVars; k++) {
96 12 t1 = J(k, j - 1);
97 12 t2 = J(k, j);
98 12 J(k, j - 1) = t1 * cc + t2 * ss;
99 12 J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
100 }
101 #endif
102 }
103 /* update the number of constraints added*/
104 10 iq++;
105 /* To update R we have to put the iq components of the d vector
106 into column iq - 1 of R
107 */
108
3/6
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
 10 R.col(iq - 1).head(iq) = d.head(iq);
109 #ifdef EIQGUADPROG_TRACE_SOLVER
110 std::cerr << iq << std::endl;
111 #endif
112
113
2/2
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 9 times.
 10 if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
114 // problem degenerate
115 1 return false;
116 9 R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
117 9 return true;
118 }
119
120 ✗ void EiquadprogFast::delete_constraint(MatrixXd &R, MatrixXd &J, VectorXi &A,
121 VectorXd &u, size_t nEqCon, size_t &iq,
122 size_t l) {
123 ✗ size_t nVars = R.rows();
124 #ifdef EIQGUADPROG_TRACE_SOLVER
125 std::cerr << "Delete constraint " << l << ' ' << iq;
126 #endif
127 size_t i, j, k;
128 ✗ size_t qq = 0;
129 double cc, ss, h, xny, t1, t2;
130
131 /* Find the index qq for active constraint l to be removed */
132 ✗ for (i = nEqCon; i < iq; i++)
133 ✗ if (A(i) == static_cast<VectorXi::Scalar>(l)) {
134 ✗ qq = i;
135 ✗ break;
136 }
137
138 /* remove the constraint from the active set and the duals */
139 ✗ for (i = qq; i < iq - 1; i++) {
140 ✗ A(i) = A(i + 1);
141 ✗ u(i) = u(i + 1);
142 ✗ R.col(i) = R.col(i + 1);
143 }
144
145 ✗ A(iq - 1) = A(iq);
146 ✗ u(iq - 1) = u(iq);
147 ✗ A(iq) = 0;
148 ✗ u(iq) = 0.0;
149 ✗ for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0;
150 /* constraint has been fully removed */
151 ✗ iq--;
152 #ifdef EIQGUADPROG_TRACE_SOLVER
153 std::cerr << '/' << iq << std::endl;
154 #endif
155
156 ✗ if (iq == 0) return;
157
158 ✗ for (j = qq; j < iq; j++) {
159 ✗ cc = R(j, j);
160 ✗ ss = R(j + 1, j);
161 ✗ h = utils::distance(cc, ss);
162 ✗ if (h == 0.0) continue;
163 ✗ cc = cc / h;
164 ✗ ss = ss / h;
165 ✗ R(j + 1, j) = 0.0;
166 ✗ if (cc < 0.0) {
167 ✗ R(j, j) = -h;
168 ✗ cc = -cc;
169 ✗ ss = -ss;
170 } else
171 ✗ R(j, j) = h;
172
173 ✗ xny = ss / (1.0 + cc);
174 ✗ for (k = j + 1; k < iq; k++) {
175 ✗ t1 = R(j, k);
176 ✗ t2 = R(j + 1, k);
177 ✗ R(j, k) = t1 * cc + t2 * ss;
178 ✗ R(j + 1, k) = xny * (t1 + R(j, k)) - t2;
179 }
180 ✗ for (k = 0; k < nVars; k++) {
181 ✗ t1 = J(k, j);
182 ✗ t2 = J(k, j + 1);
183 ✗ J(k, j) = t1 * cc + t2 * ss;
184 ✗ J(k, j + 1) = xny * (J(k, j) + t1) - t2;
185 }
186 }
187 }
188
189 12 EiquadprogFast_status EiquadprogFast::solve_quadprog(
190 const MatrixXd &Hess, const VectorXd &g0, const MatrixXd &CE,
191 const VectorXd &ce0, const MatrixXd &CI, const VectorXd &ci0, VectorXd &x) {
192 12 const size_t nVars = g0.size();
193 12 const size_t nEqCon = ce0.size();
194 12 const size_t nIneqCon = ci0.size();
195
196
3/6
✓ Branch 0 taken 12 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 12 times.
 12 if (nVars != m_nVars || nEqCon != m_nEqCon || nIneqCon != m_nIneqCon)
197 ✗ reset(nVars, nEqCon, nIneqCon);
198
199
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 assert(static_cast<size_t>(Hess.rows()) == m_nVars &&
200 static_cast<size_t>(Hess.cols()) == m_nVars);
201
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
 12 assert(static_cast<size_t>(g0.size()) == m_nVars);
202
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 assert(static_cast<size_t>(CE.rows()) == m_nEqCon &&
203 static_cast<size_t>(CE.cols()) == m_nVars);
204
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
 12 assert(static_cast<size_t>(ce0.size()) == m_nEqCon);
205
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 assert(static_cast<size_t>(CI.rows()) == m_nIneqCon &&
206 static_cast<size_t>(CI.cols()) == m_nVars);
207
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
 12 assert(static_cast<size_t>(ci0.size()) == m_nIneqCon);
208
209 size_t i, k, l; // indices
210 size_t ip; // index of the chosen violated constraint
211 size_t iq; // current number of active constraints
212 double psi; // current sum of constraint violations
213 double c1; // Hessian trace
214 double c2; // Hessian Cholesky factor trace
215 double ss; // largest constraint violation (negative for violation)
216 double R_norm; // norm of matrix R
217 12 const double inf = std::numeric_limits<double>::infinity();
218 double t, t1, t2;
219 /* t is the step length, which is the minimum of the partial step length t1
220 * and the full step length t2 */
221
222 12 iter = 0; // active-set iteration number
223
224 /*
225 * Preprocessing phase
226 */
227 /* compute the trace of the original matrix Hess */
228
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 c1 = Hess.trace();
229
230 /* decompose the matrix Hess in the form LL^T */
231
1/2
✓ Branch 0 taken 12 times.
✗ Branch 1 not taken.
 12 if (!is_inverse_provided_) {
232 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION);
233
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 chol_.compute(Hess);
234 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION);
235 }
236
237 /* initialize the matrix R */
238
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 d.setZero(nVars);
239
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 R.setZero(nVars, nVars);
240 12 R_norm = 1.0;
241
242 /* compute the inverse of the factorized matrix Hess^-1,
243 this is the initial value for H */
244 // m_J = L^-T
245
1/2
✓ Branch 0 taken 12 times.
✗ Branch 1 not taken.
 12 if (!is_inverse_provided_) {
246 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_INVERSE);
247
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 m_J.setIdentity(nVars, nVars);
248 #ifdef OPTIMIZE_HESSIAN_INVERSE
249
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 chol_.matrixU().solveInPlace(m_J);
250 #else
251 m_J = chol_.matrixU().solve(m_J);
252 #endif
253 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_INVERSE);
254 }
255
256
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 c2 = m_J.trace();
257 #ifdef EIQGUADPROG_TRACE_SOLVER
258 utils::print_matrix("m_J", m_J);
259 #endif
260
261 /* c1 * c2 is an estimate for cond(Hess) */
262
263 /*
264 * Find the unconstrained minimizer of the quadratic
265 * form 0.5 * x Hess x + g0 x
266 * this is a feasible point in the dual space
267 * x = Hess^-1 * g0
268 */
269 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
270
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
 12 if (is_inverse_provided_) {
271 ✗ x = m_J * (m_J.transpose() * g0);
272 } else {
273 #ifdef OPTIMIZE_UNCONSTR_MINIM
274
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 x = -g0;
275
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 chol_.solveInPlace(x);
276 }
277 #else
278 x = chol_.solve(g0);
279 }
280 x = -x;
281 #endif
282 /* and compute the current solution value */
283
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 f_value = 0.5 * g0.dot(x);
284 #ifdef EIQGUADPROG_TRACE_SOLVER
285 std::cerr << "Unconstrained solution: " << f_value << std::endl;
286 utils::print_vector("x", x);
287 #endif
288 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
289
290 /* Add equality constraints to the working set A */
291
292 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
293 12 iq = 0;
294
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 11 times.
 16 for (i = 0; i < nEqCon; i++) {
295 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
296
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 np = CE.row(i);
297
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 compute_d(d, m_J, np);
298
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 update_z(z, m_J, d, iq);
299
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 update_r(R, r, d, iq);
300
301 #ifdef EIQGUADPROG_TRACE_SOLVER
302 utils::print_matrix("R", R);
303 utils::print_vector("z", z);
304 utils::print_vector("r", r);
305 utils::print_vector("d", d);
306 #endif
307
308 /* compute full step length t2: i.e.,
309 the minimum step in primal space s.t. the contraint
310 becomes feasible */
311 5 t2 = 0.0;
312
3/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✓ Branch 6 taken 1 times.
 5 if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())
313 // i.e. z != 0
314
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
315
316
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 x += t2 * z;
317
318 /* set u = u+ */
319
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 u(iq) = t2;
320
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 5 u.head(iq) -= t2 * r.head(iq);
321
322 /* compute the new solution value */
323
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 f_value += 0.5 * (t2 * t2) * z.dot(np);
324
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 A(i) = static_cast<VectorXi::Scalar>(-i - 1);
325 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
326
327 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
328
3/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 4 times.
 5 if (!add_constraint(R, m_J, d, iq, R_norm)) {
329 // Equality constraints are linearly dependent
330 1 return EIQUADPROG_FAST_REDUNDANT_EQUALITIES;
331 }
332 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
333 }
334 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
335
336 /* set iai = K \ A */
337
3/4
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 11 times.
✓ Branch 4 taken 11 times.
 22 for (i = 0; i < nIneqCon; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
338
339 #ifdef USE_WARM_START
340 // DEBUG_STREAM("Gonna warm start using previous active
341 // set:\n"<<A.transpose()<<"\n")
342 for (i = nEqCon; i < q; i++) {
343 iai(i - nEqCon) = -1;
344 ip = A(i);
345 np = CI.row(ip);
346 compute_d(d, m_J, np);
347 update_z(z, m_J, d, iq);
348 update_r(R, r, d, iq);
349
350 /* compute full step length t2: i.e.,
351 the minimum step in primal space s.t. the contraint
352 becomes feasible */
353 t2 = 0.0;
354 if (std::abs(z.dot(z)) >
355 std::numeric_limits<double>::epsilon()) // i.e. z != 0
356 t2 = (-np.dot(x) - ci0(ip)) / z.dot(np);
357 else
358 DEBUG_STREAM("[WARM START] z=0\n")
359
360 x += t2 * z;
361
362 /* set u = u+ */
363 u(iq) = t2;
364 u.head(iq) -= t2 * r.head(iq);
365
366 /* compute the new solution value */
367 f_value += 0.5 * (t2 * t2) * z.dot(np);
368
369 if (!add_constraint(R, m_J, d, iq, R_norm)) {
370 // constraints are linearly dependent
371 std::cerr << "[WARM START] Constraints are linearly dependent\n";
372 return RT_EIQUADPROG_REDUNDANT_EQUALITIES;
373 }
374 }
375 #else
376
377 #endif
378
379 11 l1:
380 16 iter++;
381
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 16 if (iter >= m_maxIter) {
382 ✗ q = iq;
383 ✗ return EIQUADPROG_FAST_MAX_ITER_REACHED;
384 }
385
386 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
387
388 #ifdef EIQGUADPROG_TRACE_SOLVER
389 utils::print_vector("x", x);
390 #endif
391 /* step 1: choose a violated constraint */
392
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 16 times.
 22 for (i = nEqCon; i < iq; i++) {
393
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 ip = A(i);
394
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 iai(ip) = -1;
395 }
396
397 /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
398 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
399 16 ss = 0.0;
400 16 ip = 0; /* ip will be the index of the chosen violated constraint */
401
402 #ifdef OPTIMIZE_STEP_1_2
403
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 s = ci0;
404
3/6
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
 16 s.noalias() += CI * x;
405
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 iaexcl.setOnes();
406
3/6
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
 16 psi = (s.cwiseMin(VectorXd::Zero(nIneqCon))).sum();
407 #else
408 psi = 0.0; /* this value will contain the sum of all infeasibilities */
409 for (i = 0; i < nIneqCon; i++) {
410 iaexcl(i) = 1;
411 s(i) = CI.row(i).dot(x) + ci0(i);
412 psi += std::min(0.0, s(i));
413 }
414 #endif
415 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
416 #ifdef EIQGUADPROG_TRACE_SOLVER
417 utils::print_vector("s", s);
418 #endif
419
420 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
421
422 16 if (std::abs(psi) <= static_cast<double>(nIneqCon) *
423
2/2
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 7 times.
 16 std::numeric_limits<double>::epsilon() * c1 * c2 *
424 100.0) {
425 /* numerically there are not infeasibilities anymore */
426 9 q = iq;
427 // DEBUG_STREAM("Optimal active
428 // set:\n"<<A.head(iq).transpose()<<"\n\n")
429 9 return EIQUADPROG_FAST_OPTIMAL;
430 }
431
432 /* save old values for u, x and A */
433
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
 7 u_old.head(iq) = u.head(iq);
434
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
 7 A_old.head(iq) = A.head(iq);
435
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 x_old = x;
436
437 7 l2: /* Step 2: check for feasibility and determine a new S-pair */
438 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
439 // find constraint with highest violation
440 // (what about normalizing constraints?)
441
2/2
✓ Branch 0 taken 13 times.
✓ Branch 1 taken 7 times.
 20 for (i = 0; i < nIneqCon; i++) {
442
9/14
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✓ Branch 4 taken 6 times.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 7 times.
✓ Branch 16 taken 6 times.
 13 if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
443
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 ss = s(i);
444 7 ip = i;
445 }
446 }
447
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
 7 if (ss >= 0.0) {
448 ✗ q = iq;
449 // DEBUG_STREAM("Optimal active set:\n"<<A.transpose()<<"\n\n")
450 ✗ return EIQUADPROG_FAST_OPTIMAL;
451 }
452
453 /* set np = n(ip) */
454
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 np = CI.row(ip);
455 /* set u = (u 0)^T */
456
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 u(iq) = 0.0;
457 /* add ip to the active set A */
458
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 A(iq) = static_cast<VectorXi::Scalar>(ip);
459
460 // DEBUG_STREAM("Add constraint "<<ip<<" to active set\n")
461
462 #ifdef EIQGUADPROG_TRACE_SOLVER
463 std::cerr << "Trying with constraint " << ip << std::endl;
464 utils::print_vector("np", np);
465 #endif
466 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
467
468 7 l2a: /* Step 2a: determine step direction */
469 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
470 /* compute z = H np: the step direction in the primal space
471 (through m_J, see the paper) */
472
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 compute_d(d, m_J, np);
473 // update_z(z, m_J, d, iq);
474
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 6 times.
 7 if (iq >= nVars) {
475 // throw std::runtime_error("iq >= m_J.cols()");
476
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 z.setZero();
477 } else {
478
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 update_z(z, m_J, d, iq);
479 }
480 /* compute N* np (if q > 0): the negative of the
481 step direction in the dual space */
482
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 update_r(R, r, d, iq);
483 #ifdef EIQGUADPROG_TRACE_SOLVER
484 std::cerr << "Step direction z" << std::endl;
485 utils::print_vector("z", z);
486 utils::print_vector("r", r);
487 utils::print_vector("u", u);
488 utils::print_vector("d", d);
489 utils::print_vector("A", A);
490 #endif
491 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
492
493 /* Step 2b: compute step length */
494 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
495 7 l = 0;
496 /* Compute t1: partial step length (maximum step in dual
497 space without violating dual feasibility */
498 7 t1 = inf; /* +inf */
499 /* find the index l s.t. it reaches the minimum of u+(x) / r */
500 // l: index of constraint to drop (maybe)
501
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 7 times.
 10 for (k = nEqCon; k < iq; k++) {
502 double tmp;
503
3/12
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 3 times.
 3 if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1)) {
504 ✗ t1 = tmp;
505 ✗ l = A(k);
506 }
507 }
508 /* Compute t2: full step length (minimum step in primal
509 space such that the constraint ip becomes feasible */
510
3/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✓ Branch 6 taken 2 times.
 7 if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())
511 // i.e. z != 0
512
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 t2 = -s(ip) / z.dot(np);
513 else
514 2 t2 = inf; /* +inf */
515
516 /* the step is chosen as the minimum of t1 and t2 */
517 7 t = std::min(t1, t2);
518 #ifdef EIQGUADPROG_TRACE_SOLVER
519 std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2
520 << ") ";
521 #endif
522 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
523
524 /* Step 2c: determine new S-pair and take step: */
525 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
526 /* case (i): no step in primal or dual space */
527
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 5 times.
 7 if (t >= inf) {
528 /* QPP is infeasible */
529 2 q = iq;
530 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
531 2 return EIQUADPROG_FAST_UNBOUNDED;
532 }
533 /* case (ii): step in dual space */
534
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
 5 if (t2 >= inf) {
535 /* set u = u + t * [-r 1) and drop constraint l from the active set A */
536 ✗ u.head(iq) -= t * r.head(iq);
537 ✗ u(iq) += t;
538 ✗ iai(l) = static_cast<VectorXi::Scalar>(l);
539 ✗ delete_constraint(R, m_J, A, u, nEqCon, iq, l);
540 #ifdef EIQGUADPROG_TRACE_SOLVER
541 std::cerr << " in dual space: " << f_value << std::endl;
542 utils::print_vector("x", x);
543 utils::print_vector("z", z);
544 utils::print_vector("A", A);
545 #endif
546 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
547 ✗ goto l2a;
548 }
549
550 /* case (iii): step in primal and dual space */
551
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 x += t * z;
552 /* update the solution value */
553
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 f_value += t * z.dot(np) * (0.5 * t + u(iq));
554
555
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 5 u.head(iq) -= t * r.head(iq);
556
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 u(iq) += t;
557
558 #ifdef EIQGUADPROG_TRACE_SOLVER
559 std::cerr << " in both spaces: " << f_value << std::endl;
560 utils::print_vector("x", x);
561 utils::print_vector("u", u);
562 utils::print_vector("r", r);
563 utils::print_vector("A", A);
564 #endif
565
566
1/2
✓ Branch 0 taken 5 times.
✗ Branch 1 not taken.
 5 if (t == t2) {
567 #ifdef EIQGUADPROG_TRACE_SOLVER
568 std::cerr << "Full step has taken " << t << std::endl;
569 utils::print_vector("x", x);
570 #endif
571 /* full step has taken */
572 /* add constraint ip to the active set*/
573
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
 5 if (!add_constraint(R, m_J, d, iq, R_norm)) {
574 ✗ iaexcl(ip) = 0;
575 ✗ delete_constraint(R, m_J, A, u, nEqCon, iq, ip);
576 #ifdef EIQGUADPROG_TRACE_SOLVER
577 utils::print_matrix("R", R);
578 utils::print_vector("A", A);
579 #endif
580 ✗ for (i = 0; i < nIneqCon; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
581 ✗ for (i = 0; i < iq; i++) {
582 ✗ A(i) = A_old(i);
583 ✗ iai(A(i)) = -1;
584 ✗ u(i) = u_old(i);
585 }
586 ✗ x = x_old;
587 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
588 ✗ goto l2; /* go to step 2 */
589 } else
590
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 iai(ip) = -1;
591 #ifdef EIQGUADPROG_TRACE_SOLVER
592 utils::print_matrix("R", R);
593 utils::print_vector("A", A);
594 #endif
595 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
596 5 goto l1;
597 }
598
599 /* a partial step has been taken => drop constraint l */
600 ✗ iai(l) = static_cast<VectorXi::Scalar>(l);
601 ✗ delete_constraint(R, m_J, A, u, nEqCon, iq, l);
602 ✗ s(ip) = CI.row(ip).dot(x) + ci0(ip);
603
604 #ifdef EIQGUADPROG_TRACE_SOLVER
605 std::cerr << "Partial step has taken " << t << std::endl;
606 utils::print_vector("x", x);
607 utils::print_matrix("R", R);
608 utils::print_vector("A", A);
609 utils::print_vector("s", s);
610 #endif
611 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
612
613 ✗ goto l2a;
614 }
615
616 } /* namespace solvers */
617 } /* namespace eiquadprog */
618