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
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>
119 void RtEiquadprog<nVars, nEqCon, nIneqCon>::delete_constraint(
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();
221 if (!is_inverse_provided_) {
229 R.setZero(nVars, nVars);
235 if (!is_inverse_provided_) {
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
258 if (is_inverse_provided_) {
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);
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);
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;
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);
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;
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);
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;
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
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;
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