19 #ifndef __eiquadprog_rt_hxx__ 20 #define __eiquadprog_rt_hxx__ 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;
33 is_inverse_provided_ =
false;
36 template <
int nVars,
int nEqCon,
int nIneqCon>
37 RtEiquadprog<nVars, nEqCon, nIneqCon>::~RtEiquadprog() {}
39 template <
int nVars,
int nEqCon,
int nIneqCon>
41 typename RtMatrixX<nVars, nVars>::d& J,
42 typename RtVectorX<nVars>::d& d,
int& iq,
double& R_norm) {
45 std::cerr <<
"Add constraint " << iq <<
'/';
48 double cc, ss, h, t1, t2, xny;
50 #ifdef OPTIMIZE_ADD_CONSTRAINT 51 Eigen::Vector2d cc_ss;
58 for (j = nVars - 1; j >= iq + 1; j--) {
70 if (h == 0.0)
continue;
80 xny = ss / (1.0 + cc);
83 #ifdef OPTIMIZE_ADD_CONSTRAINT // the optimized code is actually slower than the original 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);
91 for (k = 0; k < nVars; k++) {
94 J(k, j - 1) = t1 * cc + t2 * ss;
95 J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
104 R.col(iq - 1).head(iq) = d.head(iq);
106 std::cerr << iq << std::endl;
109 if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
112 R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
116 template <
int nVars,
int nEqCon,
int nIneqCon>
118 typename RtMatrixX<nVars, nVars>::d& J,
119 typename RtVectorX<nIneqCon + nEqCon>::i& A,
120 typename RtVectorX<nIneqCon + nEqCon>::d& u,
int& iq,
124 std::cerr <<
"Delete constraint " << l <<
' ' << iq;
128 double cc, ss, h, xny, t1, t2;
131 for (i = nEqCon; i < iq; i++)
138 for (i = qq; i < iq - 1; i++) {
141 R.col(i) = R.col(i + 1);
148 for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0;
152 std::cerr <<
'/' << iq << std::endl;
157 for (j = qq; j < iq; j++) {
161 if (h == 0.0)
continue;
172 xny = ss / (1.0 + cc);
173 for (k = j + 1; k < iq; k++) {
176 R(j, k) = t1 * cc + t2 * ss;
177 R(j + 1, k) = xny * (t1 + R(j, k)) - t2;
179 for (k = 0; k < nVars; k++) {
182 J(k, j) = t1 * cc + t2 * ss;
183 J(k, j + 1) = xny * (J(k, j) + t1) - t2;
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) {
202 const double inf = std::numeric_limits<double>::infinity();
216 if (!is_inverse_provided_) {
217 START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_CHOWLESKY_DECOMPOSITION);
219 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_CHOWLESKY_DECOMPOSITION);
224 R.setZero(nVars, nVars);
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);
235 m_J = chol_.matrixU().solve(m_J);
237 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_CHOWLESKY_INVERSE);
252 START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM);
253 if (is_inverse_provided_) {
254 x = m_J * (m_J.transpose() * g0);
256 #ifdef OPTIMIZE_UNCONSTR_MINIM 258 chol_.solveInPlace(x);
266 f_value = 0.5 * g0.dot(x);
268 std::cerr <<
"Unconstrained solution: " << f_value << std::endl;
271 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM);
275 START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_ADD_EQ_CONSTR);
277 for (i = 0; i < nEqCon; i++) {
278 START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1);
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);
295 if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())
296 t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
302 u.head(iq) -= t2 * r.head(iq);
305 f_value += 0.5 * (t2 * t2) * z.dot(np);
307 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1);
309 START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_ADD_EQ_CONSTR_2);
312 return RT_EIQUADPROG_REDUNDANT_EQUALITIES;
314 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_ADD_EQ_CONSTR_2);
316 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_ADD_EQ_CONSTR);
319 for (i = 0; i < nIneqCon; i++) iai(i) = i;
321 #ifdef USE_WARM_START 323 for (i = nEqCon; i < q; i++) {
324 iai(i - nEqCon) = -1;
327 compute_d(d, m_J, np);
328 update_z(z, m_J, d, iq);
329 update_r(R, r, d, iq);
334 if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())
335 t2 = (-np.dot(x) - ci0(ip)) / z.dot(np);
337 DEBUG_STREAM(
"[WARM START] z=0\n")
343 u.head(iq) -= t2 * r.head(iq);
346 f_value += 0.5 * (t2 * t2) * z.dot(np);
350 std::cerr <<
"[WARM START] Constraints are linearly dependent\n";
351 return RT_EIQUADPROG_REDUNDANT_EQUALITIES;
359 START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1);
361 if (iter >= m_maxIter) {
363 return RT_EIQUADPROG_MAX_ITER_REACHED;
370 for (i = nEqCon; i < iq; i++) {
376 START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1_2);
380 #ifdef OPTIMIZE_STEP_1_2 382 s.noalias() += CI * x;
384 psi = (s.cwiseMin(RtVectorX<nIneqCon>::d::Zero())).sum();
387 for (i = 0; i < nIneqCon; i++) {
389 s(i) = CI.row(i).dot(x) + ci0(i);
390 psi += std::min(0.0, s(i));
393 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1_2);
398 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1);
400 if (std::abs(psi) <= nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
404 return RT_EIQUADPROG_OPTIMAL;
408 u_old.head(iq) = u.head(iq);
409 A_old.head(iq) = A.head(iq);
413 START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2);
415 for (i = 0; i < nIneqCon; i++) {
416 if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
424 return RT_EIQUADPROG_OPTIMAL;
429 for (
int tmp = 0; tmp < nVars; tmp++) np[tmp] = CI(ip, tmp);
438 std::cerr <<
"Trying with constraint " << ip << std::endl;
441 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2);
444 START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2A);
446 compute_d(d, m_J, np);
452 update_z(z, m_J, d, iq);
455 update_r(R, r, d, iq);
457 std::cerr <<
"Step direction z" << std::endl;
464 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2A);
467 START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2B);
473 for (k = nEqCon; k < iq; k++) {
475 if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1)) {
481 if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())
482 t2 = -s(ip) / z.dot(np);
487 t = std::min(t1, t2);
489 std::cerr <<
"Step sizes: " << t <<
" (t1 = " << t1 <<
", t2 = " << t2 <<
") ";
491 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2B);
494 START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2C);
499 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2C);
500 return RT_EIQUADPROG_UNBOUNDED;
505 u.head(iq) -= t * r.head(iq);
510 std::cerr <<
" in dual space: " << f_value << std::endl;
515 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2C);
522 f_value += t * z.dot(np) * (0.5 * t + u(iq));
524 u.head(iq) -= t * r.head(iq);
528 std::cerr <<
" in both spaces: " << f_value << std::endl;
537 std::cerr <<
"Full step has taken " << t << std::endl;
549 for (i = 0; i < nIneqCon; i++) iai(i) = i;
550 for (i = 0; i < iq; i++) {
556 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2C);
564 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2C);
573 for (
int tmp = 0; tmp < nVars; tmp++) s(ip) += CI(ip, tmp) * x[tmp];
576 std::cerr <<
"Partial step has taken " << t << std::endl;
582 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2C);
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