GCC Code Coverage Report

Directory: ./
File: src/eiquadprog-fast.cpp
Date: 2025-05-18 04:15:09
Exec Total Coverage
Lines: 218 363 60.1%
Functions: 7 9 77.8%
Branches: 198 666 29.7%
Line Branch Exec Source
1 //
2 // Copyright (c) 2017 CNRS
3 //
4 // This file is part of tsid
5 // tsid is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 // tsid is distributed in the hope that it will be
10 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 // General Lesser Public License for more details. You should have
13 // received a copy of the GNU Lesser General Public License along with
14 // tsid If not, see
15 // <http://www.gnu.org/licenses/>.
16 //
17
18 #include "hpp/bezier-com-traj/solver/eiquadprog-fast.hpp"
19
20 #include <iostream>
21 namespace tsid {
22 namespace solvers {
23
24 /// Compute sqrt(a^2 + b^2)
25 template <typename Scalar>
26 1341211 inline Scalar distance(Scalar a, Scalar b) {
27 Scalar a1, b1, t;
28 1341211 a1 = std::abs(a);
29 1341211 b1 = std::abs(b);
30
2/2
✓ Branch 0 taken 11964 times.
✓ Branch 1 taken 1329247 times.
 1341211 if (a1 > b1) {
31 11964 t = (b1 / a1);
32 11964 return a1 * std::sqrt(1.0 + t * t);
33
2/2
✓ Branch 0 taken 661554 times.
✓ Branch 1 taken 667693 times.
 1329247 } else if (b1 > a1) {
34 661554 t = (a1 / b1);
35 661554 return b1 * std::sqrt(1.0 + t * t);
36 }
37 667693 return a1 * std::sqrt(2.0);
38 }
39
40
14/28
✓ Branch 2 taken 2571 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2571 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2571 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2571 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2571 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2571 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2571 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 2571 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 2571 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 2571 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 2571 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 2571 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 2571 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 2571 times.
✗ Branch 42 not taken.
 2571 EiquadprogFast::EiquadprogFast() {
41 2571 m_maxIter = DEFAULT_MAX_ITER;
42 2571 q = 0; // size of the active set A (containing the indices of the active
43 // constraints)
44 2571 is_inverse_provided_ = false;
45 2571 m_nVars = 0;
46 2571 m_nEqCon = 0;
47 2571 m_nIneqCon = 0;
48 2571 }
49
50 5142 EiquadprogFast::~EiquadprogFast() {}
51
52 2571 void EiquadprogFast::reset(int nVars, int nEqCon, int nIneqCon) {
53 2571 m_nVars = nVars;
54 2571 m_nEqCon = nEqCon;
55 2571 m_nIneqCon = nIneqCon;
56 2571 m_J.setZero(nVars, nVars);
57 2571 chol_.compute(m_J);
58 2571 R.resize(nVars, nVars);
59 2571 s.resize(nIneqCon);
60 2571 r.resize(nIneqCon + nEqCon);
61 2571 u.resize(nIneqCon + nEqCon);
62 2571 z.resize(nVars);
63 2571 d.resize(nVars);
64 2571 np.resize(nVars);
65 2571 A.resize(nIneqCon + nEqCon);
66 2571 iai.resize(nIneqCon);
67 2571 iaexcl.resize(nIneqCon);
68 2571 x_old.resize(nVars);
69 2571 u_old.resize(nIneqCon + nEqCon);
70 2571 A_old.resize(nIneqCon + nEqCon);
71
72 #ifdef OPTIMIZE_ADD_CONSTRAINT
73 T1.resize(nVars);
74 #endif
75 2571 }
76
77 7812 bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d,
78 int& iq, double& R_norm) {
79 7812 long int nVars = J.rows();
80 #ifdef TRACE_SOLVER
81 std::cout << "Add constraint " << iq << '/';
82 #endif
83 long int j, k;
84 double cc, ss, h, t1, t2, xny;
85
86 #ifdef OPTIMIZE_ADD_CONSTRAINT
87 Eigen::Vector2d cc_ss;
88 #endif
89
90 /* we have to find the Givens rotation which will reduce the element
91 d(j) to zero.
92 if it is already zero we don't have to do anything, except of
93 decreasing j */
94
2/2
✓ Branch 1 taken 1338412 times.
✓ Branch 2 taken 7812 times.
 1346224 for (j = d.size() - 1; j >= iq + 1; j--) {
95 /* The Givens rotation is done with the matrix (cc cs, cs -cc).
96 If cc is one, then element (j) of d is zero compared with
97 element (j - 1). Hence we don't have to do anything. If cc is zero, then
98 we just have to switch column (j) and column (j - 1) of J. Since we only
99 switch columns in J, we have to be careful how we update d depending on
100 the sign of gs. Otherwise we have to apply the Givens rotation to these
101 columns. The i - 1 element of d has to be updated to h. */
102 1338412 cc = d(j - 1);
103 1338412 ss = d(j);
104 1338412 h = distance(cc, ss);
105
2/2
✓ Branch 0 taken 667375 times.
✓ Branch 1 taken 671037 times.
 1338412 if (h == 0.0) continue;
106 671037 d(j) = 0.0;
107 671037 ss = ss / h;
108 671037 cc = cc / h;
109
2/2
✓ Branch 0 taken 40137 times.
✓ Branch 1 taken 630900 times.
 671037 if (cc < 0.0) {
110 40137 cc = -cc;
111 40137 ss = -ss;
112 40137 d(j - 1) = -h;
113 } else
114 630900 d(j - 1) = h;
115 671037 xny = ss / (1.0 + cc);
116
117 // #define OPTIMIZE_ADD_CONSTRAINT
118 #ifdef OPTIMIZE_ADD_CONSTRAINT // the optimized code is actually slower than
119 // the original
120 T1 = J.col(j - 1);
121 cc_ss(0) = cc;
122 cc_ss(1) = ss;
123 J.col(j - 1).noalias() = J.middleCols<2>(j - 1) * cc_ss;
124 J.col(j) = xny * (T1 + J.col(j - 1)) - J.col(j);
125 #else
126 // J.col(j-1) = J[:,j-1:j] * [cc; ss]
127
2/2
✓ Branch 0 taken 381398199 times.
✓ Branch 1 taken 671037 times.
 382069236 for (k = 0; k < nVars; k++) {
128 381398199 t1 = J(k, j - 1);
129 381398199 t2 = J(k, j);
130 381398199 J(k, j - 1) = t1 * cc + t2 * ss;
131 381398199 J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
132 }
133 #endif
134 }
135 /* update the number of constraints added*/
136 7812 iq++;
137 /* To update R we have to put the iq components of the d vector
138 into column iq - 1 of R
139 */
140
4/8
✓ Branch 1 taken 7812 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7812 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7812 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7812 times.
✗ Branch 11 not taken.
 7812 R.col(iq - 1).head(iq) = d.head(iq);
141 #ifdef TRACE_SOLVER
142 std::cout << iq << std::endl;
143 #endif
144
145
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 7812 times.
 7812 if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
146 // problem degenerate
147 ✗ return false;
148
1/2
✓ Branch 1 taken 7812 times.
✗ Branch 2 not taken.
 7812 R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
149 7812 return true;
150 }
151
152 1356 void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A,
153 VectorXd& u, int nEqCon, int& iq,
154 int l) {
155 1356 const long int nVars = R.rows();
156 #ifdef TRACE_SOLVER
157 std::cout << "Delete constraint " << l << ' ' << iq;
158 #endif
159 int i, j, k;
160 1356 int qq = 0;
161 double cc, ss, h, xny, t1, t2;
162
163 /* Find the index qq for active constraint l to be removed */
164
1/2
✓ Branch 0 taken 5760 times.
✗ Branch 1 not taken.
 5760 for (i = nEqCon; i < iq; i++)
165
2/2
✓ Branch 1 taken 1356 times.
✓ Branch 2 taken 4404 times.
 5760 if (A(i) == l) {
166 1356 qq = i;
167 1356 break;
168 }
169
170 /* remove the constraint from the active set and the duals */
171
2/2
✓ Branch 0 taken 2799 times.
✓ Branch 1 taken 1356 times.
 4155 for (i = qq; i < iq - 1; i++) {
172 2799 A(i) = A(i + 1);
173 2799 u(i) = u(i + 1);
174
3/6
✓ Branch 1 taken 2799 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2799 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2799 times.
✗ Branch 8 not taken.
 2799 R.col(i) = R.col(i + 1);
175 }
176
177 1356 A(iq - 1) = A(iq);
178 1356 u(iq - 1) = u(iq);
179 1356 A(iq) = 0;
180 1356 u(iq) = 0.0;
181
2/2
✓ Branch 1 taken 38355 times.
✓ Branch 2 taken 1356 times.
 39711 for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0;
182 /* constraint has been fully removed */
183 1356 iq--;
184 #ifdef TRACE_SOLVER
185 std::cout << '/' << iq << std::endl;
186 #endif
187
188
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1356 times.
 1356 if (iq == 0) return;
189
190
2/2
✓ Branch 0 taken 2799 times.
✓ Branch 1 taken 1356 times.
 4155 for (j = qq; j < iq; j++) {
191 2799 cc = R(j, j);
192 2799 ss = R(j + 1, j);
193 2799 h = distance(cc, ss);
194
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2799 times.
 2799 if (h == 0.0) continue;
195 2799 cc = cc / h;
196 2799 ss = ss / h;
197 2799 R(j + 1, j) = 0.0;
198
2/2
✓ Branch 0 taken 1375 times.
✓ Branch 1 taken 1424 times.
 2799 if (cc < 0.0) {
199 1375 R(j, j) = -h;
200 1375 cc = -cc;
201 1375 ss = -ss;
202 } else
203 1424 R(j, j) = h;
204
205 2799 xny = ss / (1.0 + cc);
206
2/2
✓ Branch 0 taken 55174 times.
✓ Branch 1 taken 2799 times.
 57973 for (k = j + 1; k < iq; k++) {
207 55174 t1 = R(j, k);
208 55174 t2 = R(j + 1, k);
209 55174 R(j, k) = t1 * cc + t2 * ss;
210 55174 R(j + 1, k) = xny * (t1 + R(j, k)) - t2;
211 }
212
2/2
✓ Branch 0 taken 1372909 times.
✓ Branch 1 taken 2799 times.
 1375708 for (k = 0; k < nVars; k++) {
213 1372909 t1 = J(k, j);
214 1372909 t2 = J(k, j + 1);
215 1372909 J(k, j) = t1 * cc + t2 * ss;
216 1372909 J(k, j + 1) = xny * (J(k, j) + t1) - t2;
217 }
218 }
219 }
220
221 template <class Derived>
222 void print_vector(const char* name, Eigen::MatrixBase<Derived>& x, int n) {
223 if (x.size() < 10) std::cout << name << x.transpose() << std::endl;
224 }
225 template <class Derived>
226 void print_matrix(const char* name, Eigen::MatrixBase<Derived>& x, int n) {
227 // std::cout << name << std::endl << x << std::endl;
228 }
229
230 2571 EiquadprogFast_status EiquadprogFast::solve_quadprog(
231 const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
232 const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x) {
233 2571 const int nVars = (int)g0.size();
234 2571 const int nEqCon = (int)ce0.size();
235 2571 const int nIneqCon = (int)ci0.size();
236
237
1/6
✗ Branch 0 not taken.
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 2571 if (nVars != m_nVars || nEqCon != m_nEqCon || nIneqCon != m_nIneqCon)
238
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 reset(nVars, nEqCon, nIneqCon);
239
240
2/4
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2571 times.
✗ Branch 5 not taken.
 2571 assert(Hess.rows() == m_nVars && Hess.cols() == m_nVars);
241
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2571 times.
 2571 assert(g0.size() == m_nVars);
242
2/4
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2571 times.
✗ Branch 5 not taken.
 2571 assert(CE.rows() == m_nEqCon && CE.cols() == m_nVars);
243
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2571 times.
 2571 assert(ce0.size() == m_nEqCon);
244
2/4
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2571 times.
✗ Branch 5 not taken.
 2571 assert(CI.rows() == m_nIneqCon && CI.cols() == m_nVars);
245
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2571 times.
 2571 assert(ci0.size() == m_nIneqCon);
246
247 int i, k, l; // indices
248 int ip; // index of the chosen violated constraint
249 int iq; // current number of active constraints
250 double psi; // current sum of constraint violations
251 double c1; // Hessian trace
252 double c2; // Hessian Chowlesky factor trace
253 double ss; // largest constraint violation (negative for violation)
254 double R_norm; // norm of matrix R
255 2571 const double inf = std::numeric_limits<double>::infinity();
256 double t, t1, t2;
257 /* t is the step length, which is the minimum of the partial step length t1
258 * and the full step length t2 */
259
260 2571 iter = 0; // active-set iteration number
261
262 /*
263 * Preprocessing phase
264 */
265 /* compute the trace of the original matrix Hess */
266
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 c1 = Hess.trace();
267
268 /* decompose the matrix Hess in the form LL^T */
269
1/2
✓ Branch 0 taken 2571 times.
✗ Branch 1 not taken.
 2571 if (!is_inverse_provided_) {
270 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION);
271
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 chol_.compute(Hess);
272 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION);
273 }
274
275 /* initialize the matrix R */
276
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 d.setZero(nVars);
277
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 R.setZero(nVars, nVars);
278 2571 R_norm = 1.0;
279
280 /* compute the inverse of the factorized matrix Hess^-1, this is the initial
281 * value for H */
282 // m_J = L^-T
283
1/2
✓ Branch 0 taken 2571 times.
✗ Branch 1 not taken.
 2571 if (!is_inverse_provided_) {
284 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_INVERSE);
285
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 m_J.setIdentity(nVars, nVars);
286 #ifdef OPTIMIZE_HESSIAN_INVERSE
287
2/4
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2571 times.
✗ Branch 5 not taken.
 2571 chol_.matrixU().solveInPlace(m_J);
288 #else
289 m_J = chol_.matrixU().solve(m_J);
290 #endif
291 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_INVERSE);
292 }
293
294
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 c2 = m_J.trace();
295 #ifdef _SOLVER
296 print_matrix("m_J", m_J, nVars);
297 #endif
298
299 /* c1 * c2 is an estimate for cond(Hess) */
300
301 /*
302 * Find the unconstrained minimizer of the quadratic form 0.5 * x Hess x + g0
303 * x this is a feasible point in the dual space x = Hess^-1 * g0
304 */
305 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
306
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2571 times.
 2571 if (is_inverse_provided_) {
307 ✗ x = m_J * (m_J.transpose() * g0);
308 } else {
309 #ifdef OPTIMIZE_UNCONSTR_MINIM
310
2/4
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2571 times.
✗ Branch 5 not taken.
 2571 x = -g0;
311
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 chol_.solveInPlace(x);
312 }
313 #else
314 x = chol_.solve(g0);
315 }
316 x = -x;
317 #endif
318 /* and compute the current solution value */
319
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 f_value = 0.5 * g0.dot(x);
320 #ifdef TRACE_SOLVER
321 std::cout << "Unconstrained solution: " << f_value << std::endl;
322 print_vector("x", x, nVars);
323 #endif
324 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
325
326 /* Add equality constraints to the working set A */
327
328 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
329 2571 iq = 0;
330
2/2
✓ Branch 0 taken 1926 times.
✓ Branch 1 taken 2571 times.
 4497 for (i = 0; i < nEqCon; i++) {
331 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
332
2/4
✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1926 times.
✗ Branch 5 not taken.
 1926 np = CE.row(i);
333
1/2
✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
 1926 compute_d(d, m_J, np);
334
1/2
✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
 1926 update_z(z, m_J, d, iq);
335
1/2
✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
 1926 update_r(R, r, d, iq);
336
337 #ifdef TRACE_SOLVER
338 print_matrix("R", R, iq);
339 print_vector("z", z, nVars);
340 print_vector("r", r, iq);
341 print_vector("d", d, nVars);
342 #endif
343
344 /* compute full step length t2: i.e., the minimum step in primal space s.t.
345 the contraint becomes feasible */
346 1926 t2 = 0.0;
347
2/4
✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1926 times.
✗ Branch 5 not taken.
 3852 if (std::abs(z.dot(z)) >
348 1926 std::numeric_limits<double>::epsilon()) // i.e. z != 0
349
3/6
✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1926 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1926 times.
✗ Branch 8 not taken.
 1926 t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
350
351
2/4
✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1926 times.
✗ Branch 5 not taken.
 1926 x += t2 * z;
352
353 /* set u = u+ */
354
1/2
✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
 1926 u(iq) = t2;
355
4/8
✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1926 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1926 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1926 times.
✗ Branch 11 not taken.
 1926 u.head(iq) -= t2 * r.head(iq);
356
357 /* compute the new solution value */
358
1/2
✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
 1926 f_value += 0.5 * (t2 * t2) * z.dot(np);
359
1/2
✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
 1926 A(i) = -i - 1;
360 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
361
362 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
363
2/4
✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1926 times.
 1926 if (!add_constraint(R, m_J, d, iq, R_norm)) {
364 // Equality constraints are linearly dependent
365 ✗ return EIQUADPROG_FAST_REDUNDANT_EQUALITIES;
366 }
367 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
368 }
369 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
370
371 /* set iai = K \ A */
372
3/4
✓ Branch 1 taken 99898 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 99898 times.
✓ Branch 4 taken 2571 times.
 102469 for (i = 0; i < nIneqCon; i++) iai(i) = i;
373
374 #ifdef USE_WARM_START
375 // DEBUG_STREAM("Gonna warm start using previous active
376 // set:\n"<<A.transpose()<<"\n")
377 for (i = nEqCon; i < q; i++) {
378 iai(i - nEqCon) = -1;
379 ip = A(i);
380 np = CI.row(ip);
381 compute_d(d, m_J, np);
382 update_z(z, m_J, d, iq);
383 update_r(R, r, d, iq);
384
385 /* compute full step length t2: i.e., the minimum step in primal space s.t.
386 the contraint becomes feasible */
387 t2 = 0.0;
388 if (std::abs(z.dot(z)) >
389 std::numeric_limits<double>::epsilon()) // i.e. z != 0
390 t2 = (-np.dot(x) - ci0(ip)) / z.dot(np);
391 else
392 DEBUG_STREAM("[WARM START] z=0\n")
393
394 x += t2 * z;
395
396 /* set u = u+ */
397 u(iq) = t2;
398 u.head(iq) -= t2 * r.head(iq);
399
400 /* compute the new solution value */
401 f_value += 0.5 * (t2 * t2) * z.dot(np);
402
403 if (!add_constraint(R, m_J, d, iq, R_norm)) {
404 // constraints are linearly dependent
405 std::cout << "[WARM START] Constraints are linearly dependent\n";
406 return RT_EIQUADPROG_REDUNDANT_EQUALITIES;
407 }
408 }
409 #else
410
411 #endif
412
413 2571 l1:
414 8457 iter++;
415
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 8457 times.
 8457 if (iter >= m_maxIter) {
416 ✗ q = iq;
417 ✗ return EIQUADPROG_FAST_MAX_ITER_REACHED;
418 }
419
420 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
421
422 #ifdef TRACE_SOLVER
423 print_vector("x", x, nVars);
424 #endif
425 /* step 1: choose a violated constraint */
426
2/2
✓ Branch 0 taken 65925 times.
✓ Branch 1 taken 8457 times.
 74382 for (i = nEqCon; i < iq; i++) {
427
1/2
✓ Branch 1 taken 65925 times.
✗ Branch 2 not taken.
 65925 ip = A(i);
428
1/2
✓ Branch 1 taken 65925 times.
✗ Branch 2 not taken.
 65925 iai(ip) = -1;
429 }
430
431 /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
432 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
433 8457 ss = 0.0;
434 8457 ip = 0; /* ip will be the index of the chosen violated constraint */
435
436 #ifdef OPTIMIZE_STEP_1_2
437
1/2
✓ Branch 1 taken 8457 times.
✗ Branch 2 not taken.
 8457 s = ci0;
438
3/6
✓ Branch 1 taken 8457 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8457 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8457 times.
✗ Branch 8 not taken.
 8457 s.noalias() += CI * x;
439
1/2
✓ Branch 1 taken 8457 times.
✗ Branch 2 not taken.
 8457 iaexcl.setOnes();
440
3/6
✓ Branch 1 taken 8457 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8457 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8457 times.
✗ Branch 8 not taken.
 8457 psi = (s.cwiseMin(VectorXd::Zero(nIneqCon))).sum();
441 #else
442 psi = 0.0; /* this value will contain the sum of all infeasibilities */
443 for (i = 0; i < nIneqCon; i++) {
444 iaexcl(i) = 1;
445 s(i) = CI.row(i).dot(x) + ci0(i);
446 psi += std::min(0.0, s(i));
447 }
448 #endif
449 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
450 #ifdef TRACE_SOLVER
451 print_vector("s", s, nIneqCon);
452 #endif
453
454 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
455
456 8457 if (std::abs(psi) <=
457
2/2
✓ Branch 1 taken 557 times.
✓ Branch 2 taken 7900 times.
 8457 nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
458 /* numerically there are not infeasibilities anymore */
459 557 q = iq;
460 // DEBUG_STREAM("Optimal active
461 // set:\n"<<A.head(iq).transpose()<<"\n\n")
462 557 return EIQUADPROG_FAST_OPTIMAL;
463 }
464
465 /* save old values for u, x and A */
466
3/6
✓ Branch 1 taken 7900 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7900 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7900 times.
✗ Branch 8 not taken.
 7900 u_old.head(iq) = u.head(iq);
467
3/6
✓ Branch 1 taken 7900 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7900 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7900 times.
✗ Branch 8 not taken.
 7900 A_old.head(iq) = A.head(iq);
468
1/2
✓ Branch 1 taken 7900 times.
✗ Branch 2 not taken.
 7900 x_old = x;
469
470 7900 l2: /* Step 2: check for feasibility and determine a new S-pair */
471 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
472 // find constraint with highest violation (what about normalizing
473 // constraints?)
474
2/2
✓ Branch 0 taken 1098409 times.
✓ Branch 1 taken 7900 times.
 1106309 for (i = 0; i < nIneqCon; i++) {
475
10/14
✓ Branch 1 taken 1098409 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 23916 times.
✓ Branch 4 taken 1074493 times.
✓ Branch 6 taken 23916 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 21592 times.
✓ Branch 9 taken 2324 times.
✓ Branch 11 taken 21592 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 21592 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 21592 times.
✓ Branch 16 taken 1076817 times.
 1098409 if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
476
1/2
✓ Branch 1 taken 21592 times.
✗ Branch 2 not taken.
 21592 ss = s(i);
477 21592 ip = i;
478 }
479 }
480
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 7900 times.
 7900 if (ss >= 0.0) {
481 ✗ q = iq;
482 // DEBUG_STREAM("Optimal active set:\n"<<A.transpose()<<"\n\n")
483 ✗ return EIQUADPROG_FAST_OPTIMAL;
484 }
485
486 /* set np = n(ip) */
487
2/4
✓ Branch 1 taken 7900 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7900 times.
✗ Branch 5 not taken.
 7900 np = CI.row(ip);
488 /* set u = (u 0)^T */
489
1/2
✓ Branch 1 taken 7900 times.
✗ Branch 2 not taken.
 7900 u(iq) = 0.0;
490 /* add ip to the active set A */
491
1/2
✓ Branch 1 taken 7900 times.
✗ Branch 2 not taken.
 7900 A(iq) = ip;
492
493 // DEBUG_STREAM("Add constraint "<<ip<<" to active set\n")
494
495 #ifdef TRACE_SOLVER
496 std::cout << "Trying with constraint " << ip << std::endl;
497 print_vector("np", np, nVars);
498 #endif
499 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
500
501 1356 l2a: /* Step 2a: determine step direction */
502 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
503 /* compute z = H np: the step direction in the primal space (through m_J, see
504 * the paper) */
505
1/2
✓ Branch 1 taken 9256 times.
✗ Branch 2 not taken.
 9256 compute_d(d, m_J, np);
506 // update_z(z, m_J, d, iq);
507
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 9255 times.
 9256 if (iq >= nVars) {
508 // throw std::runtime_error("iq >= m_J.cols()");
509
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 z.setZero();
510 } else {
511
1/2
✓ Branch 1 taken 9255 times.
✗ Branch 2 not taken.
 9255 update_z(z, m_J, d, iq);
512 }
513 /* compute N* np (if q > 0): the negative of the step direction in the dual
514 * space */
515
1/2
✓ Branch 1 taken 9256 times.
✗ Branch 2 not taken.
 9256 update_r(R, r, d, iq);
516 #ifdef TRACE_SOLVER
517 std::cout << "Step direction z" << std::endl;
518 print_vector("z", z, nVars);
519 print_vector("r", r, iq + 1);
520 print_vector("u", u, iq + 1);
521 print_vector("d", d, nVars);
522 print_vector("A", A, iq + 1);
523 #endif
524 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
525
526 /* Step 2b: compute step length */
527 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
528 9256 l = 0;
529 /* Compute t1: partial step length (maximum step in dual space without
530 * violating dual feasibility */
531 9256 t1 = inf; /* +inf */
532 /* find the index l s.t. it reaches the minimum of u+(x) / r */
533 // l: index of constraint to drop (maybe)
534
2/2
✓ Branch 0 taken 71761 times.
✓ Branch 1 taken 9256 times.
 81017 for (k = nEqCon; k < iq; k++) {
535 double tmp;
536
9/12
✓ Branch 1 taken 71761 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 24744 times.
✓ Branch 4 taken 47017 times.
✓ Branch 6 taken 24744 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 24744 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 8482 times.
✓ Branch 12 taken 16262 times.
✓ Branch 13 taken 8482 times.
✓ Branch 14 taken 63279 times.
 71761 if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1)) {
537 8482 t1 = tmp;
538
1/2
✓ Branch 1 taken 8482 times.
✗ Branch 2 not taken.
 8482 l = A(k);
539 }
540 }
541 /* Compute t2: full step length (minimum step in primal space such that the
542 * constraint ip becomes feasible */
543
3/4
✓ Branch 1 taken 9256 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6105 times.
✓ Branch 5 taken 3151 times.
 18512 if (std::abs(z.dot(z)) >
544 9256 std::numeric_limits<double>::epsilon()) // i.e. z != 0
545
2/4
✓ Branch 1 taken 6105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6105 times.
✗ Branch 5 not taken.
 6105 t2 = -s(ip) / z.dot(np);
546 else
547 3151 t2 = inf; /* +inf */
548
549 /* the step is chosen as the minimum of t1 and t2 */
550 9256 t = std::min(t1, t2);
551 #ifdef TRACE_SOLVER
552 std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2
553 << ") ";
554 #endif
555 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
556
557 /* Step 2c: determine new S-pair and take step: */
558 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
559 /* case (i): no step in primal or dual space */
560
2/2
✓ Branch 0 taken 2014 times.
✓ Branch 1 taken 7242 times.
 9256 if (t >= inf) {
561 /* QPP is infeasible */
562 2014 q = iq;
563 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
564 2014 return EIQUADPROG_FAST_UNBOUNDED;
565 }
566 /* case (ii): step in dual space */
567
2/2
✓ Branch 0 taken 1137 times.
✓ Branch 1 taken 6105 times.
 7242 if (t2 >= inf) {
568 /* set u = u + t * [-r 1) and drop constraint l from the active set A */
569
4/8
✓ Branch 1 taken 1137 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1137 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1137 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1137 times.
✗ Branch 11 not taken.
 1137 u.head(iq) -= t * r.head(iq);
570
1/2
✓ Branch 1 taken 1137 times.
✗ Branch 2 not taken.
 1137 u(iq) += t;
571
1/2
✓ Branch 1 taken 1137 times.
✗ Branch 2 not taken.
 1137 iai(l) = l;
572
1/2
✓ Branch 1 taken 1137 times.
✗ Branch 2 not taken.
 1137 delete_constraint(R, m_J, A, u, nEqCon, iq, l);
573 #ifdef TRACE_SOLVER
574 std::cout << " in dual space: " << f_value << std::endl;
575 print_vector("x", x, nVars);
576 print_vector("z", z, nVars);
577 print_vector("A", A, iq + 1);
578 #endif
579 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
580 1137 goto l2a;
581 }
582
583 /* case (iii): step in primal and dual space */
584
2/4
✓ Branch 1 taken 6105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6105 times.
✗ Branch 5 not taken.
 6105 x += t * z;
585 /* update the solution value */
586
2/4
✓ Branch 1 taken 6105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6105 times.
✗ Branch 5 not taken.
 6105 f_value += t * z.dot(np) * (0.5 * t + u(iq));
587
588
4/8
✓ Branch 1 taken 6105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6105 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6105 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6105 times.
✗ Branch 11 not taken.
 6105 u.head(iq) -= t * r.head(iq);
589
1/2
✓ Branch 1 taken 6105 times.
✗ Branch 2 not taken.
 6105 u(iq) += t;
590
591 #ifdef TRACE_SOLVER
592 std::cout << " in both spaces: " << f_value << std::endl;
593 print_vector("x", x, nVars);
594 print_vector("u", u, iq + 1);
595 print_vector("r", r, iq + 1);
596 print_vector("A", A, iq + 1);
597 #endif
598
599
2/2
✓ Branch 0 taken 5886 times.
✓ Branch 1 taken 219 times.
 6105 if (t == t2) {
600 #ifdef TRACE_SOLVER
601 std::cout << "Full step has taken " << t << std::endl;
602 print_vector("x", x, nVars);
603 #endif
604 /* full step has taken */
605 /* add constraint ip to the active set*/
606
2/4
✓ Branch 1 taken 5886 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5886 times.
 5886 if (!add_constraint(R, m_J, d, iq, R_norm)) {
607 ✗ iaexcl(ip) = 0;
608 ✗ delete_constraint(R, m_J, A, u, nEqCon, iq, ip);
609 #ifdef TRACE_SOLVER
610 print_matrix("R", R, nVars);
611 print_vector("A", A, iq);
612 #endif
613 ✗ for (i = 0; i < nIneqCon; i++) iai(i) = i;
614 ✗ for (i = 0; i < iq; i++) {
615 ✗ A(i) = A_old(i);
616 ✗ iai(A(i)) = -1;
617 ✗ u(i) = u_old(i);
618 }
619 ✗ x = x_old;
620 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
621 ✗ goto l2; /* go to step 2 */
622 } else
623
1/2
✓ Branch 1 taken 5886 times.
✗ Branch 2 not taken.
 5886 iai(ip) = -1;
624 #ifdef TRACE_SOLVER
625 print_matrix("R", R, nVars);
626 print_vector("A", A, iq);
627 #endif
628 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
629 5886 goto l1;
630 }
631
632 /* a partial step has been taken => drop constraint l */
633
1/2
✓ Branch 1 taken 219 times.
✗ Branch 2 not taken.
 219 iai(l) = l;
634
1/2
✓ Branch 1 taken 219 times.
✗ Branch 2 not taken.
 219 delete_constraint(R, m_J, A, u, nEqCon, iq, l);
635
4/8
✓ Branch 1 taken 219 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 219 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 219 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 219 times.
✗ Branch 11 not taken.
 219 s(ip) = CI.row(ip).dot(x) + ci0(ip);
636
637 #ifdef TRACE_SOLVER
638 std::cout << "Partial step has taken " << t << std::endl;
639 print_vector("x", x, nVars);
640 print_matrix("R", R, nVars);
641 print_vector("A", A, iq);
642 print_vector("s", s, nIneqCon);
643 #endif
644 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
645
646 219 goto l2a;
647 }
648
649 ✗ double trace_sparse(const EiquadprogFast::SpMat& Hess) {
650 ✗ double sum = 0;
651 ✗ for (int k = 0; k < Hess.outerSize(); ++k) sum += Hess.coeff(k, k);
652 ✗ return sum;
653 }
654
655 ✗ EiquadprogFast_status EiquadprogFast::solve_quadprog_sparse(
656 const EiquadprogFast::SpMat& Hess, const VectorXd& g0, const MatrixXd& CE,
657 const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x) {
658 ✗ const int nVars = (int)g0.size();
659 ✗ const int nEqCon = (int)ce0.size();
660 ✗ const int nIneqCon = (int)ci0.size();
661
662 ✗ if (nVars != m_nVars || nEqCon != m_nEqCon || nIneqCon != m_nIneqCon)
663 ✗ reset(nVars, nEqCon, nIneqCon);
664
665 ✗ assert(Hess.rows() == m_nVars && Hess.cols() == m_nVars);
666 ✗ assert(g0.size() == m_nVars);
667 ✗ assert(CE.rows() == m_nEqCon && CE.cols() == m_nVars);
668 ✗ assert(ce0.size() == m_nEqCon);
669 ✗ assert(CI.rows() == m_nIneqCon && CI.cols() == m_nVars);
670 ✗ assert(ci0.size() == m_nIneqCon);
671
672 int i, k, l; // indices
673 int ip; // index of the chosen violated constraint
674 int iq; // current number of active constraints
675 double psi; // current sum of constraint violations
676 double c1; // Hessian trace
677 double c2; // Hessian Chowlesky factor trace
678 double ss; // largest constraint violation (negative for violation)
679 double R_norm; // norm of matrix R
680 ✗ const double inf = std::numeric_limits<double>::infinity();
681 double t, t1, t2;
682 /* t is the step length, which is the minimum of the partial step length t1
683 * and the full step length t2 */
684
685 ✗ iter = 0; // active-set iteration number
686
687 /*
688 * Preprocessing phase
689 */
690 /* compute the trace of the original matrix Hess */
691 ✗ c1 = trace_sparse(Hess);
692
693 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION);
694 ✗ Eigen::SimplicialLLT<SpMat, Eigen::Lower> cholsp_(Hess);
695 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION);
696
697 /* initialize the matrix R */
698 ✗ d.setZero(nVars);
699 ✗ R.setZero(nVars, nVars);
700 ✗ R_norm = 1.0;
701
702 /* compute the inverse of the factorized matrix Hess^-1, this is the initial
703 * value for H */
704 // m_J = L^-T
705 ✗ if (!is_inverse_provided_) {
706 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_INVERSE);
707 ✗ m_J.setIdentity(nVars, nVars);
708 #ifdef OPTIMIZE_HESSIAN_INVERSE
709 ✗ cholsp_.matrixU().solve(m_J);
710 #else
711 m_J = cholsp_.matrixU().solve(m_J);
712 #endif
713 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_INVERSE);
714 }
715
716 ✗ c2 = m_J.trace();
717 #ifdef _SOLVER
718 print_matrix("m_J", m_J, nVars);
719 #endif
720
721 /* c1 * c2 is an estimate for cond(Hess) */
722
723 /*
724 * Find the unconstrained minimizer of the quadratic form 0.5 * x Hess x + g0
725 * x this is a feasible point in the dual space x = Hess^-1 * g0
726 */
727 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
728 ✗ if (is_inverse_provided_) {
729 ✗ x = m_J * (m_J.transpose() * g0);
730 } else {
731 #ifdef OPTIMIZE_UNCONSTR_MINIM
732 // x = -g0;
733 ✗ x = cholsp_.solve(-g0);
734 }
735 #else
736 x = cholsp_.solve(g0);
737 }
738 x = -x;
739 #endif
740 /* and compute the current solution value */
741 ✗ f_value = 0.5 * g0.dot(x);
742 #ifdef TRACE_SOLVER
743 std::cout << "Unconstrained solution: " << f_value << std::endl;
744 print_vector("x", x, nVars);
745 #endif
746 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
747
748 /* Add equality constraints to the working set A */
749
750 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
751 ✗ iq = 0;
752 ✗ for (i = 0; i < nEqCon; i++) {
753 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
754 ✗ np = CE.row(i);
755 ✗ compute_d(d, m_J, np);
756 ✗ update_z(z, m_J, d, iq);
757 ✗ update_r(R, r, d, iq);
758
759 #ifdef TRACE_SOLVER
760 print_matrix("R", R, iq);
761 print_vector("z", z, nVars);
762 print_vector("r", r, iq);
763 print_vector("d", d, nVars);
764 #endif
765
766 /* compute full step length t2: i.e., the minimum step in primal space s.t.
767 the contraint becomes feasible */
768 ✗ t2 = 0.0;
769 ✗ if (std::abs(z.dot(z)) >
770 ✗ std::numeric_limits<double>::epsilon()) // i.e. z != 0
771 ✗ t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
772
773 ✗ x += t2 * z;
774
775 /* set u = u+ */
776 ✗ u(iq) = t2;
777 ✗ u.head(iq) -= t2 * r.head(iq);
778
779 /* compute the new solution value */
780 ✗ f_value += 0.5 * (t2 * t2) * z.dot(np);
781 ✗ A(i) = -i - 1;
782 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
783
784 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
785 ✗ if (!add_constraint(R, m_J, d, iq, R_norm)) {
786 // Equality constraints are linearly dependent
787 ✗ return EIQUADPROG_FAST_REDUNDANT_EQUALITIES;
788 }
789 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
790 }
791 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
792
793 /* set iai = K \ A */
794 ✗ for (i = 0; i < nIneqCon; i++) iai(i) = i;
795
796 #ifdef USE_WARM_START
797 // DEBUG_STREAM("Gonna warm start using previous active
798 // set:\n"<<A.transpose()<<"\n")
799 for (i = nEqCon; i < q; i++) {
800 iai(i - nEqCon) = -1;
801 ip = A(i);
802 np = CI.row(ip);
803 compute_d(d, m_J, np);
804 update_z(z, m_J, d, iq);
805 update_r(R, r, d, iq);
806
807 /* compute full step length t2: i.e., the minimum step in primal space s.t.
808 the contraint becomes feasible */
809 t2 = 0.0;
810 if (std::abs(z.dot(z)) >
811 std::numeric_limits<double>::epsilon()) // i.e. z != 0
812 t2 = (-np.dot(x) - ci0(ip)) / z.dot(np);
813 else
814 DEBUG_STREAM("[WARM START] z=0\n")
815
816 x += t2 * z;
817
818 /* set u = u+ */
819 u(iq) = t2;
820 u.head(iq) -= t2 * r.head(iq);
821
822 /* compute the new solution value */
823 f_value += 0.5 * (t2 * t2) * z.dot(np);
824
825 if (!add_constraint(R, m_J, d, iq, R_norm)) {
826 // constraints are linearly dependent
827 std::cout << "[WARM START] Constraints are linearly dependent\n";
828 return RT_EIQUADPROG_REDUNDANT_EQUALITIES;
829 }
830 }
831 #else
832
833 #endif
834
835 ✗ l1:
836 ✗ iter++;
837 ✗ if (iter >= m_maxIter) {
838 ✗ q = iq;
839 ✗ return EIQUADPROG_FAST_MAX_ITER_REACHED;
840 }
841
842 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
843
844 #ifdef TRACE_SOLVER
845 print_vector("x", x, nVars);
846 #endif
847 /* step 1: choose a violated constraint */
848 ✗ for (i = nEqCon; i < iq; i++) {
849 ✗ ip = A(i);
850 ✗ iai(ip) = -1;
851 }
852
853 /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
854 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
855 ✗ ss = 0.0;
856 ✗ ip = 0; /* ip will be the index of the chosen violated constraint */
857
858 #ifdef OPTIMIZE_STEP_1_2
859 ✗ s = ci0;
860 ✗ s.noalias() += CI * x;
861 ✗ iaexcl.setOnes();
862 ✗ psi = (s.cwiseMin(VectorXd::Zero(nIneqCon))).sum();
863 #else
864 psi = 0.0; /* this value will contain the sum of all infeasibilities */
865 for (i = 0; i < nIneqCon; i++) {
866 iaexcl(i) = 1;
867 s(i) = CI.row(i).dot(x) + ci0(i);
868 psi += std::min(0.0, s(i));
869 }
870 #endif
871 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
872 #ifdef TRACE_SOLVER
873 print_vector("s", s, nIneqCon);
874 #endif
875
876 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
877
878 ✗ if (std::abs(psi) <=
879 ✗ nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
880 /* numerically there are not infeasibilities anymore */
881 ✗ q = iq;
882 // DEBUG_STREAM("Optimal active
883 // set:\n"<<A.head(iq).transpose()<<"\n\n")
884 ✗ return EIQUADPROG_FAST_OPTIMAL;
885 }
886
887 /* save old values for u, x and A */
888 ✗ u_old.head(iq) = u.head(iq);
889 ✗ A_old.head(iq) = A.head(iq);
890 ✗ x_old = x;
891
892 ✗ l2: /* Step 2: check for feasibility and determine a new S-pair */
893 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
894 // find constraint with highest violation (what about normalizing
895 // constraints?)
896 ✗ for (i = 0; i < nIneqCon; i++) {
897 ✗ if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
898 ✗ ss = s(i);
899 ✗ ip = i;
900 }
901 }
902 ✗ if (ss >= 0.0) {
903 ✗ q = iq;
904 // DEBUG_STREAM("Optimal active set:\n"<<A.transpose()<<"\n\n")
905 ✗ return EIQUADPROG_FAST_OPTIMAL;
906 }
907
908 /* set np = n(ip) */
909 ✗ np = CI.row(ip);
910 /* set u = (u 0)^T */
911 ✗ u(iq) = 0.0;
912 /* add ip to the active set A */
913 ✗ A(iq) = ip;
914
915 // DEBUG_STREAM("Add constraint "<<ip<<" to active set\n")
916
917 #ifdef TRACE_SOLVER
918 std::cout << "Trying with constraint " << ip << std::endl;
919 print_vector("np", np, nVars);
920 #endif
921 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
922
923 ✗ l2a: /* Step 2a: determine step direction */
924 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
925 /* compute z = H np: the step direction in the primal space (through m_J, see
926 * the paper) */
927 ✗ compute_d(d, m_J, np);
928 // update_z(z, m_J, d, iq);
929 ✗ if (iq >= nVars) {
930 // throw std::runtime_error("iq >= m_J.cols()");
931 ✗ z.setZero();
932 } else {
933 ✗ update_z(z, m_J, d, iq);
934 }
935 /* compute N* np (if q > 0): the negative of the step direction in the dual
936 * space */
937 ✗ update_r(R, r, d, iq);
938 #ifdef TRACE_SOLVER
939 std::cout << "Step direction z" << std::endl;
940 print_vector("z", z, nVars);
941 print_vector("r", r, iq + 1);
942 print_vector("u", u, iq + 1);
943 print_vector("d", d, nVars);
944 print_vector("A", A, iq + 1);
945 #endif
946 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
947
948 /* Step 2b: compute step length */
949 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
950 ✗ l = 0;
951 /* Compute t1: partial step length (maximum step in dual space without
952 * violating dual feasibility */
953 ✗ t1 = inf; /* +inf */
954 /* find the index l s.t. it reaches the minimum of u+(x) / r */
955 // l: index of constraint to drop (maybe)
956 ✗ for (k = nEqCon; k < iq; k++) {
957 double tmp;
958 ✗ if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1)) {
959 ✗ t1 = tmp;
960 ✗ l = A(k);
961 }
962 }
963 /* Compute t2: full step length (minimum step in primal space such that the
964 * constraint ip becomes feasible */
965 ✗ if (std::abs(z.dot(z)) >
966 ✗ std::numeric_limits<double>::epsilon()) // i.e. z != 0
967 ✗ t2 = -s(ip) / z.dot(np);
968 else
969 ✗ t2 = inf; /* +inf */
970
971 /* the step is chosen as the minimum of t1 and t2 */
972 ✗ t = std::min(t1, t2);
973 #ifdef TRACE_SOLVER
974 std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2
975 << ") ";
976 #endif
977 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
978
979 /* Step 2c: determine new S-pair and take step: */
980 START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
981 /* case (i): no step in primal or dual space */
982 ✗ if (t >= inf) {
983 /* QPP is infeasible */
984 ✗ q = iq;
985 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
986 ✗ return EIQUADPROG_FAST_UNBOUNDED;
987 }
988 /* case (ii): step in dual space */
989 ✗ if (t2 >= inf) {
990 /* set u = u + t * [-r 1) and drop constraint l from the active set A */
991 ✗ u.head(iq) -= t * r.head(iq);
992 ✗ u(iq) += t;
993 ✗ iai(l) = l;
994 ✗ delete_constraint(R, m_J, A, u, nEqCon, iq, l);
995 #ifdef TRACE_SOLVER
996 std::cout << " in dual space: " << f_value << std::endl;
997 print_vector("x", x, nVars);
998 print_vector("z", z, nVars);
999 print_vector("A", A, iq + 1);
1000 #endif
1001 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
1002 ✗ goto l2a;
1003 }
1004
1005 /* case (iii): step in primal and dual space */
1006 ✗ x += t * z;
1007 /* update the solution value */
1008 ✗ f_value += t * z.dot(np) * (0.5 * t + u(iq));
1009
1010 ✗ u.head(iq) -= t * r.head(iq);
1011 ✗ u(iq) += t;
1012
1013 #ifdef TRACE_SOLVER
1014 std::cout << " in both spaces: " << f_value << std::endl;
1015 print_vector("x", x, nVars);
1016 print_vector("u", u, iq + 1);
1017 print_vector("r", r, iq + 1);
1018 print_vector("A", A, iq + 1);
1019 #endif
1020
1021 ✗ if (t == t2) {
1022 #ifdef TRACE_SOLVER
1023 std::cout << "Full step has taken " << t << std::endl;
1024 print_vector("x", x, nVars);
1025 #endif
1026 /* full step has taken */
1027 /* add constraint ip to the active set*/
1028 ✗ if (!add_constraint(R, m_J, d, iq, R_norm)) {
1029 ✗ iaexcl(ip) = 0;
1030 ✗ delete_constraint(R, m_J, A, u, nEqCon, iq, ip);
1031 #ifdef TRACE_SOLVER
1032 print_matrix("R", R, nVars);
1033 print_vector("A", A, iq);
1034 #endif
1035 ✗ for (i = 0; i < nIneqCon; i++) iai(i) = i;
1036 ✗ for (i = 0; i < iq; i++) {
1037 ✗ A(i) = A_old(i);
1038 ✗ iai(A(i)) = -1;
1039 ✗ u(i) = u_old(i);
1040 }
1041 ✗ x = x_old;
1042 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
1043 ✗ goto l2; /* go to step 2 */
1044 } else
1045 ✗ iai(ip) = -1;
1046 #ifdef TRACE_SOLVER
1047 print_matrix("R", R, nVars);
1048 print_vector("A", A, iq);
1049 #endif
1050 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
1051 ✗ goto l1;
1052 }
1053
1054 /* a partial step has been taken => drop constraint l */
1055 ✗ iai(l) = l;
1056 ✗ delete_constraint(R, m_J, A, u, nEqCon, iq, l);
1057 ✗ s(ip) = CI.row(ip).dot(x) + ci0(ip);
1058
1059 #ifdef TRACE_SOLVER
1060 std::cout << "Partial step has taken " << t << std::endl;
1061 print_vector("x", x, nVars);
1062 print_matrix("R", R, nVars);
1063 print_vector("A", A, iq);
1064 print_vector("s", s, nIneqCon);
1065 #endif
1066 STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
1067
1068 ✗ goto l2a;
1069 ✗ }
1070
1071 } /* namespace solvers */
1072 } /* namespace tsid */
1073