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 |