GCC Code Coverage Report


Directory: ./
File: src/eiquadprog-fast.cpp
Date: 2025-03-18 04:20:50
Exec Total Coverage
Lines: 218 362 60.2%
Branches: 195 660 29.5%

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 1340500 inline Scalar distance(Scalar a, Scalar b) {
27 Scalar a1, b1, t;
28 1340500 a1 = std::abs(a);
29 1340500 b1 = std::abs(b);
30
2/2
✓ Branch 0 taken 11672 times.
✓ Branch 1 taken 1328828 times.
1340500 if (a1 > b1) {
31 11672 t = (b1 / a1);
32 11672 return a1 * std::sqrt(1.0 + t * t);
33
2/2
✓ Branch 0 taken 661135 times.
✓ Branch 1 taken 667693 times.
1328828 } else if (b1 > a1) {
34 661135 t = (a1 / b1);
35 661135 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 7101 bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d,
78 int& iq, double& R_norm) {
79 7101 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 1337701 times.
✓ Branch 2 taken 7101 times.
1344802 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 1337701 cc = d(j - 1);
103 1337701 ss = d(j);
104 1337701 h = distance(cc, ss);
105
2/2
✓ Branch 0 taken 667375 times.
✓ Branch 1 taken 670326 times.
1337701 if (h == 0.0) continue;
106 670326 d(j) = 0.0;
107 670326 ss = ss / h;
108 670326 cc = cc / h;
109
2/2
✓ Branch 0 taken 40196 times.
✓ Branch 1 taken 630130 times.
670326 if (cc < 0.0) {
110 40196 cc = -cc;
111 40196 ss = -ss;
112 40196 d(j - 1) = -h;
113 } else
114 630130 d(j - 1) = h;
115 670326 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 381396066 times.
✓ Branch 1 taken 670326 times.
382066392 for (k = 0; k < nVars; k++) {
128 381396066 t1 = J(k, j - 1);
129 381396066 t2 = J(k, j);
130 381396066 J(k, j - 1) = t1 * cc + t2 * ss;
131 381396066 J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
132 }
133 #endif
134 }
135 /* update the number of constraints added*/
136 7101 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
3/6
✓ Branch 2 taken 7101 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7101 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7101 times.
✗ Branch 9 not taken.
7101 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 7101 times.
7101 if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
146 // problem degenerate
147 return false;
148 7101 R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
149 7101 return true;
150 }
151
152 765 void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A,
153 VectorXd& u, int nEqCon, int& iq,
154 int l) {
155 765 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 765 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 4578 times.
✗ Branch 1 not taken.
4578 for (i = nEqCon; i < iq; i++)
165
2/2
✓ Branch 1 taken 765 times.
✓ Branch 2 taken 3813 times.
4578 if (A(i) == l) {
166 765 qq = i;
167 765 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 765 times.
3564 for (i = qq; i < iq - 1; i++) {
172 2799 A(i) = A(i + 1);
173 2799 u(i) = u(i + 1);
174
2/4
✓ Branch 2 taken 2799 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2799 times.
✗ Branch 6 not taken.
2799 R.col(i) = R.col(i + 1);
175 }
176
177 765 A(iq - 1) = A(iq);
178 765 u(iq - 1) = u(iq);
179 765 A(iq) = 0;
180 765 u(iq) = 0.0;
181
2/2
✓ Branch 1 taken 37173 times.
✓ Branch 2 taken 765 times.
37938 for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0;
182 /* constraint has been fully removed */
183 765 iq--;
184 #ifdef TRACE_SOLVER
185 std::cout << '/' << iq << std::endl;
186 #endif
187
188
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 765 times.
765 if (iq == 0) return;
189
190
2/2
✓ Branch 0 taken 2799 times.
✓ Branch 1 taken 765 times.
3564 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 7746 iter++;
415
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 7746 times.
7746 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 64503 times.
✓ Branch 1 taken 7746 times.
72249 for (i = nEqCon; i < iq; i++) {
427
1/2
✓ Branch 1 taken 64503 times.
✗ Branch 2 not taken.
64503 ip = A(i);
428
1/2
✓ Branch 1 taken 64503 times.
✗ Branch 2 not taken.
64503 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 7746 ss = 0.0;
434 7746 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 7746 times.
✗ Branch 2 not taken.
7746 s = ci0;
438
3/6
✓ Branch 1 taken 7746 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7746 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7746 times.
✗ Branch 8 not taken.
7746 s.noalias() += CI * x;
439
1/2
✓ Branch 1 taken 7746 times.
✗ Branch 2 not taken.
7746 iaexcl.setOnes();
440
3/6
✓ Branch 1 taken 7746 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7746 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7746 times.
✗ Branch 8 not taken.
7746 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 7746 if (std::abs(psi) <=
457
2/2
✓ Branch 1 taken 557 times.
✓ Branch 2 taken 7189 times.
7746 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 7189 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7189 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7189 times.
✗ Branch 8 not taken.
7189 u_old.head(iq) = u.head(iq);
467
3/6
✓ Branch 1 taken 7189 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7189 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7189 times.
✗ Branch 8 not taken.
7189 A_old.head(iq) = A.head(iq);
468
1/2
✓ Branch 1 taken 7189 times.
✗ Branch 2 not taken.
7189 x_old = x;
469
470 7189 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 1082767 times.
✓ Branch 1 taken 7189 times.
1089956 for (i = 0; i < nIneqCon; i++) {
475
10/14
✓ Branch 1 taken 1082767 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 21271 times.
✓ Branch 4 taken 1061496 times.
✓ Branch 6 taken 21271 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 19028 times.
✓ Branch 9 taken 2243 times.
✓ Branch 11 taken 19028 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 19028 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 19028 times.
✓ Branch 16 taken 1063739 times.
1082767 if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
476
1/2
✓ Branch 1 taken 19028 times.
✗ Branch 2 not taken.
19028 ss = s(i);
477 19028 ip = i;
478 }
479 }
480
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 7189 times.
7189 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 7189 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7189 times.
✗ Branch 5 not taken.
7189 np = CI.row(ip);
488 /* set u = (u 0)^T */
489
1/2
✓ Branch 1 taken 7189 times.
✗ Branch 2 not taken.
7189 u(iq) = 0.0;
490 /* add ip to the active set A */
491
1/2
✓ Branch 1 taken 7189 times.
✗ Branch 2 not taken.
7189 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 7954 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 7954 times.
✗ Branch 2 not taken.
7954 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 7953 times.
7954 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 7953 times.
✗ Branch 2 not taken.
7953 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 7954 times.
✗ Branch 2 not taken.
7954 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 7954 l = 0;
529 /* Compute t1: partial step length (maximum step in dual space without
530 * violating dual feasibility */
531 7954 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 69829 times.
✓ Branch 1 taken 7954 times.
77783 for (k = nEqCon; k < iq; k++) {
535 double tmp;
536
9/12
✓ Branch 1 taken 69829 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 23562 times.
✓ Branch 4 taken 46267 times.
✓ Branch 6 taken 23562 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 23562 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 7300 times.
✓ Branch 12 taken 16262 times.
✓ Branch 13 taken 7300 times.
✓ Branch 14 taken 62529 times.
69829 if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1)) {
537 7300 t1 = tmp;
538
1/2
✓ Branch 1 taken 7300 times.
✗ Branch 2 not taken.
7300 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 7954 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5394 times.
✓ Branch 5 taken 2560 times.
15908 if (std::abs(z.dot(z)) >
544 7954 std::numeric_limits<double>::epsilon()) // i.e. z != 0
545
2/4
✓ Branch 1 taken 5394 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5394 times.
✗ Branch 5 not taken.
5394 t2 = -s(ip) / z.dot(np);
546 else
547 2560 t2 = inf; /* +inf */
548
549 /* the step is chosen as the minimum of t1 and t2 */
550 7954 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 5940 times.
7954 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 546 times.
✓ Branch 1 taken 5394 times.
5940 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 546 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 546 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 546 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 546 times.
✗ Branch 11 not taken.
546 u.head(iq) -= t * r.head(iq);
570
1/2
✓ Branch 1 taken 546 times.
✗ Branch 2 not taken.
546 u(iq) += t;
571
1/2
✓ Branch 1 taken 546 times.
✗ Branch 2 not taken.
546 iai(l) = l;
572
1/2
✓ Branch 1 taken 546 times.
✗ Branch 2 not taken.
546 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 546 goto l2a;
581 }
582
583 /* case (iii): step in primal and dual space */
584
2/4
✓ Branch 1 taken 5394 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5394 times.
✗ Branch 5 not taken.
5394 x += t * z;
585 /* update the solution value */
586
2/4
✓ Branch 1 taken 5394 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5394 times.
✗ Branch 5 not taken.
5394 f_value += t * z.dot(np) * (0.5 * t + u(iq));
587
588
4/8
✓ Branch 1 taken 5394 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5394 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5394 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5394 times.
✗ Branch 11 not taken.
5394 u.head(iq) -= t * r.head(iq);
589
1/2
✓ Branch 1 taken 5394 times.
✗ Branch 2 not taken.
5394 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 5175 times.
✓ Branch 1 taken 219 times.
5394 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 5175 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5175 times.
5175 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 5175 times.
✗ Branch 2 not taken.
5175 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 5175 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