eiquadprog: include/eiquadprog/eiquadprog-rt.hxx Source File

eiquadprog 1.3.2 C++ reimplementation of eiquadprog

Loading...
Searching...
No Matches
eiquadprog-rt.hxx
Go to the documentation of this file.
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
24namespace eiquadprog {
25
26namespace solvers {
27
28template <int nVars, int nEqCon, int nIneqCon>
29RtEiquadprog<nVars, nEqCon, nIneqCon>::RtEiquadprog()
30 : solver_return_(std::numeric_limits<double>::infinity()) {
31 m_maxIter = DEFAULT_MAX_ITER;
32 q = 0; // size of the active set A
33 // (containing the indices of the active constraints)
34 is_inverse_provided_ = false;
35}
36
37template <int nVars, int nEqCon, int nIneqCon>
38RtEiquadprog<nVars, nEqCon, nIneqCon>::~RtEiquadprog() {}
39
40template <int nVars, int nEqCon, int nIneqCon>
41bool 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 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 cc = d(j - 1);
69 ss = d(j);
70 h = utils::distance(cc, ss);
71 if (h == 0.0) continue;
72 d(j) = 0.0;
73 ss = ss / h;
74 cc = cc / h;
75 if (cc < 0.0) {
76 cc = -cc;
77 ss = -ss;
78 d(j - 1) = -h;
79 } else
80 d(j - 1) = h;
81 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 for (k = 0; k < nVars; k++) {
94 t1 = J(k, j - 1);
95 t2 = J(k, j);
96 J(k, j - 1) = t1 * cc + t2 * ss;
97 J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
98 }
99#endif
100 }
101 /* update the number of constraints added*/
102 iq++;
103 /* To update R we have to put the iq components of the d vector
104into column iq - 1 of R
105*/
106 R.col(iq - 1).head(iq) = d.head(iq);
107#ifdef EIQGUADPROG_TRACE_SOLVER
108 std::cerr << iq << std::endl;
109#endif
110
111 if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
112 // problem degenerate
113 return false;
114 R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
115 return true;
116}
117
118template <int nVars, int nEqCon, int nIneqCon>
119void 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
190template <int nVars, int nEqCon, int nIneqCon>
191 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 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 iter = 0; // active-set iteration number
213
214 /*
215 * Preprocessing phase
216 */
217 /* compute the trace of the original matrix Hess */
218 c1 = Hess.trace();
219
220 /* decompose the matrix Hess in the form LL^T */
221 if (!is_inverse_provided_) {
222 START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_CHOWLESKY_DECOMPOSITION);
223 chol_.compute(Hess);
224 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_CHOWLESKY_DECOMPOSITION);
225 }
226
227 /* initialize the matrix R */
228 d.setZero(nVars);
229 R.setZero(nVars, nVars);
230 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 if (!is_inverse_provided_) {
236 START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_CHOWLESKY_INVERSE);
237 m_J.setIdentity(nVars, nVars);
238#ifdef OPTIMIZE_HESSIAN_INVERSE
239 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 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 if (is_inverse_provided_) {
259 x = m_J * (m_J.transpose() * g0);
260 } else {
261#ifdef OPTIMIZE_UNCONSTR_MINIM
262 x = -g0;
263 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 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 iq = 0;
282 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 for (int tmp = 0; tmp < nVars; tmp++) np[tmp] = CE(i, tmp);
286 compute_d(d, m_J, np);
287 update_z(z, m_J, d, iq);
288 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 t2 = 0.0;
300 if (std::abs(z.dot(z)) >
301 std::numeric_limits<double>::epsilon()) // i.e. z != 0
302 t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
303
304 x += t2 * z;
305
306 /* set u = u+ */
307 u(iq) = t2;
308 u.head(iq) -= t2 * r.head(iq);
309
310 /* compute the new solution value */
311 f_value += 0.5 * (t2 * t2) * z.dot(np);
312 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 if (!add_constraint(R, m_J, d, iq, R_norm)) {
317 // Equality constraints are linearly dependent
318 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 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
366l1:
367 START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1);
368 iter++;
369 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 for (i = nEqCon; i < iq; i++) {
379 ip = A(i);
380 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 ss = 0.0;
386 ip = 0; /* ip will be the index of the chosen violated constraint */
387
388#ifdef OPTIMIZE_STEP_1_2
389 s = ci0;
390 s.noalias() += CI * x;
391 iaexcl.setOnes();
392 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 if (std::abs(psi) <=
409 nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
410 /* numerically there are not infeasibilities anymore */
411 q = iq;
412 // DEBUG_STREAM("Optimal active
413 // set:\n"<<A.head(iq).transpose()<<"\n\n")
414 return RT_EIQUADPROG_OPTIMAL;
415 }
416
417 /* save old values for u, x and A */
418 u_old.head(iq) = u.head(iq);
419 A_old.head(iq) = A.head(iq);
420 x_old = x;
421
422l2: /* 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 for (i = 0; i < nIneqCon; i++) {
427 if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
428 ss = s(i);
429 ip = i;
430 }
431 }
432 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 for (int tmp = 0; tmp < nVars; tmp++) np[tmp] = CI(ip, tmp);
441 /* set u = (u 0)^T */
442 u(iq) = 0.0;
443 /* add ip to the active set A */
444 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
454l2a: /* 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 compute_d(d, m_J, np);
459 // update_z(z, m_J, d, iq);
460 if (iq >= nVars) {
461 // throw std::runtime_error("iq >= m_J.cols()");
462 z.setZero();
463 } else {
464 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 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 l = 0;
482 /* Compute t1: partial step length (maximum step in dual space without
483 * violating dual feasibility */
484 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 for (k = nEqCon; k < iq; k++) {
488 double tmp;
489 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 if (std::abs(z.dot(z)) >
497 std::numeric_limits<double>::epsilon()) // i.e. z != 0
498 t2 = -s(ip) / z.dot(np);
499 else
500 t2 = inf; /* +inf */
501
502 /* the step is chosen as the minimum of t1 and t2 */
503 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 if (t >= inf) {
514 /* QPP is infeasible */
515 q = iq;
516 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2C);
517 return RT_EIQUADPROG_UNBOUNDED;
518 }
519 /* case (ii): step in dual space */
520 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 x += t * z;
538 /* update the solution value */
539 f_value += t * z.dot(np) * (0.5 * t + u(iq));
540
541 u.head(iq) -= t * r.head(iq);
542 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 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 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 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 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__ */
eiquadprog::solvers::RtEiquadprog
Definition eiquadprog-rt.hpp:89
eiquadprog::solvers::RtEiquadprog::RtEiquadprog
EIGEN_MAKE_ALIGNED_OPERATOR_NEW RtEiquadprog()
Definition eiquadprog-rt.hxx:29
eiquadprog::solvers::RtEiquadprog::solve_quadprog
RtEiquadprog_status solve_quadprog(const typename RtMatrixX< nVars, nVars >::d &Hess, const typename RtVectorX< nVars >::d &g0, const typename RtMatrixX< nEqCon, nVars >::d &CE, const typename RtVectorX< nEqCon >::d &ce0, const typename RtMatrixX< nIneqCon, nVars >::d &CI, const typename RtVectorX< nIneqCon >::d &ci0, typename RtVectorX< nVars >::d &x)
Definition eiquadprog-rt.hxx:191
eiquadprog::solvers::RtEiquadprog::~RtEiquadprog
virtual ~RtEiquadprog()
Definition eiquadprog-rt.hxx:38
eiquadprog::solvers::RtEiquadprog::is_inverse_provided_
bool is_inverse_provided_
Definition eiquadprog-rt.hpp:156
DEFAULT_MAX_ITER
#define DEFAULT_MAX_ITER
Definition eiquadprog-fast.hpp:59
DEBUG_STREAM
#define DEBUG_STREAM(msg)
Definition eiquadprog-fast.hpp:34
START_PROFILER_EIQUADPROG_RT
#define START_PROFILER_EIQUADPROG_RT(x)
Definition eiquadprog-rt.hpp:41
PROFILE_EIQUADPROG_ADD_EQ_CONSTR_2
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_2
Definition eiquadprog-rt.hpp:49
PROFILE_EIQUADPROG_STEP_2A
#define PROFILE_EIQUADPROG_STEP_2A
Definition eiquadprog-rt.hpp:56
PROFILE_EIQUADPROG_STEP_2C
#define PROFILE_EIQUADPROG_STEP_2C
Definition eiquadprog-rt.hpp:58
PROFILE_EIQUADPROG_CHOWLESKY_INVERSE
#define PROFILE_EIQUADPROG_CHOWLESKY_INVERSE
Definition eiquadprog-rt.hpp:46
PROFILE_EIQUADPROG_STEP_2
#define PROFILE_EIQUADPROG_STEP_2
Definition eiquadprog-rt.hpp:55
PROFILE_EIQUADPROG_CHOWLESKY_DECOMPOSITION
#define PROFILE_EIQUADPROG_CHOWLESKY_DECOMPOSITION
Definition eiquadprog-rt.hpp:45
PROFILE_EIQUADPROG_ADD_EQ_CONSTR
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR
Definition eiquadprog-rt.hpp:47
STOP_PROFILER_EIQUADPROG_RT
#define STOP_PROFILER_EIQUADPROG_RT(x)
Definition eiquadprog-rt.hpp:42
PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM
#define PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM
Definition eiquadprog-rt.hpp:53
PROFILE_EIQUADPROG_STEP_1_2
#define PROFILE_EIQUADPROG_STEP_1_2
Definition eiquadprog-rt.hpp:52
PROFILE_EIQUADPROG_STEP_2B
#define PROFILE_EIQUADPROG_STEP_2B
Definition eiquadprog-rt.hpp:57
PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1
Definition eiquadprog-rt.hpp:48
PROFILE_EIQUADPROG_STEP_1
#define PROFILE_EIQUADPROG_STEP_1
Definition eiquadprog-rt.hpp:50
eiquadprog-utils.hxx
eiquadprog::solvers::update_z
void update_z(Eigen::VectorXd &z, const Eigen::MatrixXd &J, const Eigen::VectorXd &d, size_t iq)
Definition eiquadprog.hpp:93
eiquadprog::solvers::RtEiquadprog_status
RtEiquadprog_status
Definition eiquadprog-rt.hpp:80
eiquadprog::solvers::RT_EIQUADPROG_REDUNDANT_EQUALITIES
@ RT_EIQUADPROG_REDUNDANT_EQUALITIES
Definition eiquadprog-rt.hpp:85
eiquadprog::solvers::RT_EIQUADPROG_MAX_ITER_REACHED
@ RT_EIQUADPROG_MAX_ITER_REACHED
Definition eiquadprog-rt.hpp:84
eiquadprog::solvers::RT_EIQUADPROG_UNBOUNDED
@ RT_EIQUADPROG_UNBOUNDED
Definition eiquadprog-rt.hpp:83
eiquadprog::solvers::RT_EIQUADPROG_OPTIMAL
@ RT_EIQUADPROG_OPTIMAL
Definition eiquadprog-rt.hpp:81
eiquadprog::solvers::add_constraint
bool add_constraint(Eigen::MatrixXd &R, Eigen::MatrixXd &J, Eigen::VectorXd &d, size_t &iq, double &R_norm)
eiquadprog::solvers::compute_d
void compute_d(Eigen::VectorXd &d, const Eigen::MatrixXd &J, const Eigen::VectorXd &np)
Definition eiquadprog.hpp:88
eiquadprog::solvers::delete_constraint
void delete_constraint(Eigen::MatrixXd &R, Eigen::MatrixXd &J, Eigen::VectorXi &A, Eigen::VectorXd &u, size_t p, size_t &iq, size_t l)
eiquadprog::solvers::update_r
void update_r(const Eigen::MatrixXd &R, Eigen::VectorXd &r, const Eigen::VectorXd &d, size_t iq)
Definition eiquadprog.hpp:98
eiquadprog::utils::distance
Scalar distance(Scalar a, Scalar b)
Compute sqrt(a^2 + b^2)
Definition eiquadprog-utils.hxx:12
eiquadprog::utils::print_vector
void print_vector(const char *name, Eigen::MatrixBase< Derived > &x)
Definition eiquadprog-utils.hxx:27
eiquadprog::utils::print_matrix
void print_matrix(const char *name, Eigen::MatrixBase< Derived > &x)
Definition eiquadprog-utils.hxx:31
eiquadprog
Definition eiquadprog-fast.hpp:64
RtMatrixX::d
Eigen::Matrix< double, Rows, Cols > d
Definition eiquadprog-rt.hpp:64
RtVectorX
Definition eiquadprog-rt.hpp:68
RtVectorX::i
Eigen::Matrix< int, Rows, 1 > i
Definition eiquadprog-rt.hpp:70
RtVectorX::d
Eigen::Matrix< double, Rows, 1 > d
Definition eiquadprog-rt.hpp:69