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