19 #ifndef __eiquadprog_rt_hxx__ 20 #define __eiquadprog_rt_hxx__ 28 template <
int nVars,
int nEqCon,
int nIneqCon>
30 : solver_return_(std::numeric_limits<double>::infinity()) {
37 template <
int nVars,
int nEqCon,
int nIneqCon>
40 template <
int nVars,
int nEqCon,
int nIneqCon>
44 int &iq,
double &R_norm) {
46 #ifdef EIQGUADPROG_TRACE_SOLVER 47 std::cerr <<
"Add constraint " << iq <<
'/';
50 double cc, ss, h, t1, t2, xny;
52 #ifdef OPTIMIZE_ADD_CONSTRAINT 53 Eigen::Vector2d cc_ss;
60 for (j = nVars - 1; j >= iq + 1; j--) {
71 if (h == 0.0)
continue;
81 xny = ss / (1.0 + cc);
84 #ifdef OPTIMIZE_ADD_CONSTRAINT // the optimized code is actually slower than 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);
93 for (k = 0; k < nVars; k++) {
96 J(k, j - 1) = t1 * cc + t2 * ss;
97 J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
106 R.col(iq - 1).head(iq) = d.head(iq);
107 #ifdef EIQGUADPROG_TRACE_SOLVER 108 std::cerr << iq << std::endl;
111 if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
114 R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
118 template <
int nVars,
int nEqCon,
int nIneqCon>
125 #ifdef EIQGUADPROG_TRACE_SOLVER 126 std::cerr <<
"Delete constraint " << l <<
' ' << iq;
130 double cc, ss, h, xny, t1, t2;
133 for (i = nEqCon; i < iq; i++)
140 for (i = qq; i < iq - 1; i++) {
143 R.col(i) = R.col(i + 1);
150 for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0;
153 #ifdef EIQGUADPROG_TRACE_SOLVER 154 std::cerr <<
'/' << iq << std::endl;
159 for (j = qq; j < iq; j++) {
163 if (h == 0.0)
continue;
174 xny = ss / (1.0 + cc);
175 for (k = j + 1; k < iq; k++) {
178 R(j, k) = t1 * cc + t2 * ss;
179 R(j + 1, k) = xny * (t1 + R(j, k)) - t2;
181 for (k = 0; k < nVars; k++) {
184 J(k, j) = t1 * cc + t2 * ss;
185 J(k, j + 1) = xny * (J(k, j) + t1) - t2;
190 template <
int nVars,
int nEqCon,
int nIneqCon>
207 const double inf = std::numeric_limits<double>::infinity();
229 R.setZero(nVars, nVars);
237 m_J.setIdentity(nVars, nVars);
238 #ifdef OPTIMIZE_HESSIAN_INVERSE 239 chol_.matrixU().solveInPlace(
m_J);
241 m_J = chol_.matrixU().solve(
m_J);
247 #ifdef EIQGUADPROG_TRACE_SOLVER 259 x =
m_J * (
m_J.transpose() * g0);
261 #ifdef OPTIMIZE_UNCONSTR_MINIM 263 chol_.solveInPlace(x);
271 f_value = 0.5 * g0.dot(x);
272 #ifdef EIQGUADPROG_TRACE_SOLVER 273 std::cerr <<
"Unconstrained solution: " << f_value << std::endl;
282 for (i = 0; i < nEqCon; i++) {
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);
290 #ifdef EIQGUADPROG_TRACE_SOLVER 300 if (std::abs(z.dot(z)) >
301 std::numeric_limits<double>::epsilon())
302 t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
308 u.head(iq) -= t2 * r.head(iq);
311 f_value += 0.5 * (t2 * t2) * z.dot(np);
316 if (!add_constraint(R,
m_J, d, iq, R_norm)) {
325 for (i = 0; i < nIneqCon; i++) iai(i) = i;
327 #ifdef USE_WARM_START 330 for (i = nEqCon; i < q; i++) {
331 iai(i - nEqCon) = -1;
334 compute_d(d,
m_J, np);
335 update_z(z,
m_J, d, iq);
336 update_r(R, r, d, iq);
341 if (std::abs(z.dot(z)) >
342 std::numeric_limits<double>::epsilon())
343 t2 = (-np.dot(x) - ci0(ip)) / z.dot(np);
351 u.head(iq) -= t2 * r.head(iq);
354 f_value += 0.5 * (t2 * t2) * z.dot(np);
356 if (!add_constraint(R,
m_J, d, iq, R_norm)) {
358 std::cerr <<
"[WARM START] Constraints are linearly dependent\n";
369 if (iter >= m_maxIter) {
374 #ifdef EIQGUADPROG_TRACE_SOLVER 378 for (i = nEqCon; i < iq; i++) {
388 #ifdef OPTIMIZE_STEP_1_2 390 s.noalias() += CI * x;
395 for (i = 0; i < nIneqCon; i++) {
397 s(i) = CI.row(i).dot(x) + ci0(i);
398 psi += std::min(0.0, s(i));
402 #ifdef EIQGUADPROG_TRACE_SOLVER 409 nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
418 u_old.head(iq) = u.head(iq);
419 A_old.head(iq) = A.head(iq);
426 for (i = 0; i < nIneqCon; i++) {
427 if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
440 for (
int tmp = 0; tmp < nVars; tmp++) np[tmp] = CI(ip, tmp);
448 #ifdef EIQGUADPROG_TRACE_SOLVER 449 std::cerr <<
"Trying with constraint " << ip << std::endl;
458 compute_d(d,
m_J, np);
464 update_z(z,
m_J, d, iq);
468 update_r(R, r, d, iq);
469 #ifdef EIQGUADPROG_TRACE_SOLVER 470 std::cerr <<
"Step direction z" << std::endl;
487 for (k = nEqCon; k < iq; k++) {
489 if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1)) {
496 if (std::abs(z.dot(z)) >
497 std::numeric_limits<double>::epsilon())
498 t2 = -s(ip) / z.dot(np);
503 t = std::min(t1, t2);
504 #ifdef EIQGUADPROG_TRACE_SOLVER 505 std::cerr <<
"Step sizes: " << t <<
" (t1 = " << t1 <<
", t2 = " << t2
522 u.head(iq) -= t * r.head(iq);
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;
539 f_value += t * z.dot(np) * (0.5 * t + u(iq));
541 u.head(iq) -= t * r.head(iq);
544 #ifdef EIQGUADPROG_TRACE_SOLVER 545 std::cerr <<
" in both spaces: " << f_value << std::endl;
553 #ifdef EIQGUADPROG_TRACE_SOLVER 554 std::cerr <<
"Full step has taken " << t << std::endl;
559 if (!add_constraint(R,
m_J, d, iq, R_norm)) {
561 delete_constraint(R,
m_J, A, u, iq, ip);
562 #ifdef EIQGUADPROG_TRACE_SOLVER 566 for (i = 0; i < nIneqCon; i++) iai(i) = i;
567 for (i = 0; i < iq; i++) {
577 #ifdef EIQGUADPROG_TRACE_SOLVER 587 delete_constraint(R,
m_J, A, u, iq, l);
590 for (
int tmp = 0; tmp < nVars; tmp++) s(ip) += CI(ip, tmp) * x[tmp];
592 #ifdef EIQGUADPROG_TRACE_SOLVER 593 std::cerr <<
"Partial step has taken " << t << std::endl;
RtMatrixX< nVars, nVars >::d m_J
Definition: eiquadprog-rt.hpp:155
#define PROFILE_EIQUADPROG_STEP_2A
Definition: eiquadprog-rt.hpp:56
Eigen::Matrix< double, Rows, 1 > d
Definition: eiquadprog-rt.hpp:69
Definition: eiquadprog-rt.hpp:68
#define PROFILE_EIQUADPROG_STEP_2C
Definition: eiquadprog-rt.hpp:58
Definition: eiquadprog-rt.hpp:89
#define PROFILE_EIQUADPROG_STEP_1
Definition: eiquadprog-rt.hpp:50
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
Definition: eiquadprog-fast.hpp:63
Definition: eiquadprog-rt.hpp:85
#define PROFILE_EIQUADPROG_CHOWLESKY_INVERSE
Definition: eiquadprog-rt.hpp:46
#define PROFILE_EIQUADPROG_STEP_1_2
Definition: eiquadprog-rt.hpp:52
#define PROFILE_EIQUADPROG_STEP_2B
Definition: eiquadprog-rt.hpp:57
void print_matrix(const char *name, Eigen::MatrixBase< Derived > &x)
Definition: eiquadprog-utils.hxx:31
virtual ~RtEiquadprog()
Definition: eiquadprog-rt.hxx:38
#define DEFAULT_MAX_ITER
Definition: eiquadprog-fast.hpp:59
Scalar distance(Scalar a, Scalar b)
Compute sqrt(a^2 + b^2)
Definition: eiquadprog-utils.hxx:12
#define PROFILE_EIQUADPROG_STEP_2
Definition: eiquadprog-rt.hpp:55
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR
Definition: eiquadprog-rt.hpp:47
#define DEBUG_STREAM(msg)
Definition: eiquadprog-fast.hpp:34
bool is_inverse_provided_
Definition: eiquadprog-rt.hpp:156
Definition: eiquadprog-rt.hpp:83
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1
Definition: eiquadprog-rt.hpp:48
EIGEN_MAKE_ALIGNED_OPERATOR_NEW RtEiquadprog()
Definition: eiquadprog-rt.hxx:29
Definition: eiquadprog-rt.hpp:63
Definition: eiquadprog-rt.hpp:81
Definition: eiquadprog-rt.hpp:84
RtEiquadprog_status
Definition: eiquadprog-rt.hpp:80
#define START_PROFILER_EIQUADPROG_RT(x)
Definition: eiquadprog-rt.hpp:41
#define STOP_PROFILER_EIQUADPROG_RT(x)
Definition: eiquadprog-rt.hpp:42
#define PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM
Definition: eiquadprog-rt.hpp:53
Eigen::Matrix< double, Rows, Cols > d
Definition: eiquadprog-rt.hpp:64
#define PROFILE_EIQUADPROG_CHOWLESKY_DECOMPOSITION
Definition: eiquadprog-rt.hpp:45
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_2
Definition: eiquadprog-rt.hpp:49
void print_vector(const char *name, Eigen::MatrixBase< Derived > &x)
Definition: eiquadprog-utils.hxx:27