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