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>
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 
37 template <int nVars, int nEqCon, int nIneqCon>
39 
40 template <int nVars, int nEqCon, int nIneqCon>
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
104 into 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 
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,
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>
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_) {
223  chol_.compute(Hess);
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_) {
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
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  */
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
277 
278  /* Add equality constraints to the working set A */
279 
281  iq = 0;
282  for (i = 0; i < nEqCon; i++) {
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;
314 
316  if (!add_constraint(R, m_J, d, iq, R_norm)) {
317  // Equality constraints are linearly dependent
319  }
321  }
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";
360  }
361  }
362 #else
363 
364 #endif
365 
366 l1:
368  iter++;
369  if (iter >= m_maxIter) {
370  q = iq;
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 */
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
402 #ifdef EIQGUADPROG_TRACE_SOLVER
403  utils::print_vector("s", s);
404 #endif
405 
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 
422 l2: /* Step 2: check for feasibility and determine a new S-pair */
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
453 
454 l2a: /* Step 2a: determine step direction */
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
478 
479  /* Step 2b: compute step length */
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
509 
510  /* Step 2c: determine new S-pair and take step: */
512  /* case (i): no step in primal or dual space */
513  if (t >= inf) {
514  /* QPP is infeasible */
515  q = iq;
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
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;
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
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
600 
601  goto l2a;
602 }
603 } /* namespace solvers */
604 } /* namespace eiquadprog */
605 #endif /* __eiquadprog_rt_hxx__ */
Definition: eiquadprog-rt.hpp:89
EIGEN_MAKE_ALIGNED_OPERATOR_NEW RtEiquadprog()
Definition: eiquadprog-rt.hxx:29
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
virtual ~RtEiquadprog()
Definition: eiquadprog-rt.hxx:38
bool is_inverse_provided_
Definition: eiquadprog-rt.hpp:156
#define DEFAULT_MAX_ITER
Definition: eiquadprog-fast.hpp:59
#define DEBUG_STREAM(msg)
Definition: eiquadprog-fast.hpp:34
#define START_PROFILER_EIQUADPROG_RT(x)
Definition: eiquadprog-rt.hpp:41
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_2
Definition: eiquadprog-rt.hpp:49
#define PROFILE_EIQUADPROG_STEP_2A
Definition: eiquadprog-rt.hpp:56
#define PROFILE_EIQUADPROG_STEP_2C
Definition: eiquadprog-rt.hpp:58
#define PROFILE_EIQUADPROG_CHOWLESKY_INVERSE
Definition: eiquadprog-rt.hpp:46
#define PROFILE_EIQUADPROG_STEP_2
Definition: eiquadprog-rt.hpp:55
#define PROFILE_EIQUADPROG_CHOWLESKY_DECOMPOSITION
Definition: eiquadprog-rt.hpp:45
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR
Definition: eiquadprog-rt.hpp:47
#define STOP_PROFILER_EIQUADPROG_RT(x)
Definition: eiquadprog-rt.hpp:42
#define PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM
Definition: eiquadprog-rt.hpp:53
#define PROFILE_EIQUADPROG_STEP_1_2
Definition: eiquadprog-rt.hpp:52
#define PROFILE_EIQUADPROG_STEP_2B
Definition: eiquadprog-rt.hpp:57
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1
Definition: eiquadprog-rt.hpp:48
#define PROFILE_EIQUADPROG_STEP_1
Definition: eiquadprog-rt.hpp:50
void update_z(Eigen::VectorXd &z, const Eigen::MatrixXd &J, const Eigen::VectorXd &d, size_t iq)
Definition: eiquadprog.hpp:92
RtEiquadprog_status
Definition: eiquadprog-rt.hpp:80
@ RT_EIQUADPROG_REDUNDANT_EQUALITIES
Definition: eiquadprog-rt.hpp:85
@ RT_EIQUADPROG_MAX_ITER_REACHED
Definition: eiquadprog-rt.hpp:84
@ RT_EIQUADPROG_UNBOUNDED
Definition: eiquadprog-rt.hpp:83
@ RT_EIQUADPROG_OPTIMAL
Definition: eiquadprog-rt.hpp:81
bool add_constraint(Eigen::MatrixXd &R, Eigen::MatrixXd &J, Eigen::VectorXd &d, size_t &iq, double &R_norm)
void compute_d(Eigen::VectorXd &d, const Eigen::MatrixXd &J, const Eigen::VectorXd &np)
Definition: eiquadprog.hpp:87
void delete_constraint(Eigen::MatrixXd &R, Eigen::MatrixXd &J, Eigen::VectorXi &A, Eigen::VectorXd &u, size_t p, size_t &iq, size_t l)
void update_r(const Eigen::MatrixXd &R, Eigen::VectorXd &r, const Eigen::VectorXd &d, size_t iq)
Definition: eiquadprog.hpp:97
Scalar distance(Scalar a, Scalar b)
Compute sqrt(a^2 + b^2)
Definition: eiquadprog-utils.hxx:12
void print_vector(const char *name, Eigen::MatrixBase< Derived > &x)
Definition: eiquadprog-utils.hxx:27
void print_matrix(const char *name, Eigen::MatrixBase< Derived > &x)
Definition: eiquadprog-utils.hxx:31
Definition: eiquadprog-fast.hpp:63
Definition: eiquadprog-rt.hpp:63
Eigen::Matrix< double, Rows, Cols > d
Definition: eiquadprog-rt.hpp:64
Definition: eiquadprog-rt.hpp:68
Eigen::Matrix< double, Rows, 1 > d
Definition: eiquadprog-rt.hpp:69