Verified Commit 5ab45e01 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

core: apply new signature and remove some memory allocation

parent 1cd8e930
Pipeline #17935 failed with stage
in 13 seconds
...@@ -5,8 +5,25 @@ namespace solvers { ...@@ -5,8 +5,25 @@ namespace solvers {
using namespace Eigen; using namespace Eigen;
/* solve_quadprog is used for on-demand QP solving */ /* solve_quadprog is used for on-demand QP solving */
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) { 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) {
Eigen::DenseIndex p = CE.cols();
Eigen::DenseIndex m = CI.cols();
VectorXd y(p + m);
return solve_quadprog(G, g0, CE, ce0, CI, ci0, x, y, activeSet,
activeSetSize);
}
double solve_quadprog(MatrixXd &G, VectorXd &g0, const MatrixXd &CE,
const VectorXd &ce0, const MatrixXd &CI,
const VectorXd &ci0, VectorXd &x, VectorXd &y,
VectorXi &activeSet, size_t &activeSetSize) {
LLT<MatrixXd, Lower> chol(G.cols()); LLT<MatrixXd, Lower> chol(G.cols());
double c1; double c1;
/* compute the trace of the original matrix G */ /* compute the trace of the original matrix G */
...@@ -15,15 +32,33 @@ double solve_quadprog(MatrixXd& G, VectorXd& g0, const MatrixXd& CE, const Vecto ...@@ -15,15 +32,33 @@ double solve_quadprog(MatrixXd& G, VectorXd& g0, const MatrixXd& CE, const Vecto
/* decompose the matrix G in the form LL^T */ /* decompose the matrix G in the form LL^T */
chol.compute(G); chol.compute(G);
return solve_quadprog2(chol, c1, g0, CE, ce0, CI, ci0, x, activeSet, activeSetSize); return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, y, activeSet,
activeSetSize);
}
double solve_quadprog(LLT<MatrixXd, Lower> &chol, double c1, VectorXd &g0,
const MatrixXd &CE, const VectorXd &ce0,
const MatrixXd &CI, const VectorXd &ci0, VectorXd &x,
VectorXi &activeSet, size_t &activeSetSize) {
Eigen::DenseIndex p = CE.cols();
Eigen::DenseIndex m = CI.cols();
VectorXd y(p + m);
return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, y, activeSet,
activeSetSize);
} }
/* solve_quadprog2 is used for when the Cholesky decomposition of G is pre-computed /* solve_quadprog2 is used for when the Cholesky decomposition of G is
* pre-computed
* @param A Output vector containing the indexes of the active constraints. * @param A Output vector containing the indexes of the active constraints.
* @param q Output value representing the size of the active set. * @param q Output value representing the size of the active set.
*/ */
double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, double solve_quadprog(LLT<MatrixXd, Lower> &chol, double c1, VectorXd &g0,
const MatrixXd& CI, const VectorXd& ci0, VectorXd& x, VectorXi& A, size_t& q) { const MatrixXd &CE, const VectorXd &ce0,
const MatrixXd &CI, const VectorXd &ci0, VectorXd &x,
VectorXd &u, VectorXi &A, size_t &q) {
size_t i, k, l; /* indices */ size_t i, k, l; /* indices */
size_t ip, me, mi; size_t ip, me, mi;
size_t n = g0.size(); size_t n = g0.size();
...@@ -31,21 +66,24 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons ...@@ -31,21 +66,24 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
size_t m = CI.cols(); size_t m = CI.cols();
MatrixXd R(g0.size(), g0.size()), J(g0.size(), g0.size()); MatrixXd R(g0.size(), g0.size()), J(g0.size(), g0.size());
VectorXd s(m + p), z(n), r(m + p), d(n), np(n), u(m + p); VectorXd s(m + p), z(n), r(m + p), d(n), np(n);
VectorXd x_old(n), u_old(m + p); VectorXd x_old(n), u_old(m + p);
double f_value, psi, c2, sum, ss, R_norm; double f_value, psi, c2, sum, ss, R_norm;
const double inf = std::numeric_limits<double>::infinity(); const double inf = std::numeric_limits<double>::infinity();
double t, t1, t2; /* t is the step length, which is the minimum of the partial step length t1 double t, t1, t2; /* t is the step length, which is the minimum of the partial
* and the full step length t2 */ * step length t1 and the full step length t2 */
// VectorXi A(m + p); // Del Prete: active set is now an output parameter // VectorXi A(m + p); // Del Prete: active set is now an output
if (static_cast<size_t>(A.size()) != m + p) A.resize(m + p); // parameter
if (static_cast<size_t>(A.size()) != m + p)
A.resize(m + p);
VectorXi A_old(m + p), iai(m + p), iaexcl(m + p); VectorXi A_old(m + p), iai(m + p), iaexcl(m + p);
// int q; // int q;
size_t iq, iter = 0; size_t iq, iter = 0;
me = p; /* number of equality constraints */ me = p; /* number of equality constraints */
mi = m; /* number of inequality constraints */ mi = m; /* number of inequality constraints */
q = 0; /* size of the active set A (containing the indices of the active constraints) */ q = 0; /* size of the active set A (containing the indices of the active
constraints) */
/* /*
* Preprocessing phase * Preprocessing phase
...@@ -56,12 +94,13 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons ...@@ -56,12 +94,13 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
R.setZero(); R.setZero();
R_norm = 1.0; /* this variable will hold the norm of the matrix R */ R_norm = 1.0; /* this variable will hold the norm of the matrix R */
/* compute the inverse of the factorized matrix G^-1, this is the initial value for H */ /* compute the inverse of the factorized matrix G^-1, this is the initial
* value for H */
// J = L^-T // J = L^-T
J.setIdentity(); J.setIdentity();
J = chol.matrixU().solve(J); chol.matrixU().solveInPlace(J);
c2 = J.trace(); c2 = J.trace();
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("J", J, n); print_matrix("J", J, n);
#endif #endif
...@@ -72,11 +111,11 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons ...@@ -72,11 +111,11 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
* this is a feasible point in the dual space * this is a feasible point in the dual space
* x = G^-1 * g0 * x = G^-1 * g0
*/ */
x = chol.solve(g0); x = -g0;
x = -x; chol.solveInPlace(x);
/* and compute the current solution value */ /* and compute the current solution value */
f_value = 0.5 * g0.dot(x); f_value = 0.5 * g0.dot(x);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Unconstrained solution: " << f_value << std::endl; std::cerr << "Unconstrained solution: " << f_value << std::endl;
print_vector("x", x, n); print_vector("x", x, n);
#endif #endif
...@@ -88,17 +127,18 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons ...@@ -88,17 +127,18 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
compute_d(d, J, np); compute_d(d, J, np);
update_z(z, J, d, iq); update_z(z, J, d, iq);
update_r(R, r, d, iq); update_r(R, r, d, iq);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("R", R, iq); print_matrix("R", R, iq);
print_vector("z", z, n); print_vector("z", z, n);
print_vector("r", r, iq); print_vector("r", r, iq);
print_vector("d", d, n); print_vector("d", d, n);
#endif #endif
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint /* compute full step length t2: i.e., the minimum step in primal space s.t.
becomes feasible */ the contraint becomes feasible */
t2 = 0.0; t2 = 0.0;
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0 if (std::abs(z.dot(z)) >
std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-np.dot(x) - ce0(i)) / z.dot(np); t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
x += t2 * z; x += t2 * z;
...@@ -119,11 +159,12 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons ...@@ -119,11 +159,12 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
} }
/* set iai = K \ A */ /* set iai = K \ A */
for (i = 0; i < mi; i++) iai(i) = static_cast<VectorXi::Scalar>(i); for (i = 0; i < mi; i++)
iai(i) = static_cast<VectorXi::Scalar>(i);
l1: l1:
iter++; iter++;
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_vector("x", x, n); print_vector("x", x, n);
#endif #endif
/* step 1: choose a violated constraint */ /* step 1: choose a violated constraint */
...@@ -142,11 +183,13 @@ l1: ...@@ -142,11 +183,13 @@ l1:
s(i) = sum; s(i) = sum;
psi += std::min(0.0, sum); psi += std::min(0.0, sum);
} }
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_vector("s", s, mi); print_vector("s", s, mi);
#endif #endif
if (std::abs(psi) <= static_cast<double>(mi) * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) { if (std::abs(psi) <= static_cast<double>(mi) *
std::numeric_limits<double>::epsilon() * c1 * c2 *
100.0) {
/* numerically there are not infeasibilities anymore */ /* numerically there are not infeasibilities anymore */
q = iq; q = iq;
return f_value; return f_value;
...@@ -176,18 +219,20 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */ ...@@ -176,18 +219,20 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */
/* add ip to the active set A */ /* add ip to the active set A */
A(iq) = static_cast<VectorXi::Scalar>(ip); A(iq) = static_cast<VectorXi::Scalar>(ip);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Trying with constraint " << ip << std::endl; std::cerr << "Trying with constraint " << ip << std::endl;
print_vector("np", np, n); print_vector("np", np, n);
#endif #endif
l2a: /* Step 2a: determine step direction */ l2a: /* Step 2a: determine step direction */
/* compute z = H np: the step direction in the primal space (through J, see the paper) */ /* compute z = H np: the step direction in the primal space (through J, see
* the paper) */
compute_d(d, J, np); compute_d(d, J, np);
update_z(z, J, d, iq); update_z(z, J, d, iq);
/* compute N* np (if q > 0): the negative of the step direction in the dual space */ /* compute N* np (if q > 0): the negative of the step direction in the dual
* space */
update_r(R, r, d, iq); update_r(R, r, d, iq);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Step direction z" << std::endl; std::cerr << "Step direction z" << std::endl;
print_vector("z", z, n); print_vector("z", z, n);
print_vector("r", r, iq + 1); print_vector("r", r, iq + 1);
...@@ -198,7 +243,8 @@ l2a: /* Step 2a: determine step direction */ ...@@ -198,7 +243,8 @@ l2a: /* Step 2a: determine step direction */
/* Step 2b: compute step length */ /* Step 2b: compute step length */
l = 0; l = 0;
/* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */ /* Compute t1: partial step length (maximum step in dual space without
* violating dual feasibility */
t1 = inf; /* +inf */ t1 = inf; /* +inf */
/* find the index l s.t. it reaches the minimum of u+(x) / r */ /* find the index l s.t. it reaches the minimum of u+(x) / r */
for (k = me; k < iq; k++) { for (k = me; k < iq; k++) {
...@@ -208,16 +254,19 @@ l2a: /* Step 2a: determine step direction */ ...@@ -208,16 +254,19 @@ l2a: /* Step 2a: determine step direction */
l = A(k); l = A(k);
} }
} }
/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */ /* Compute t2: full step length (minimum step in primal space such that the
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0 * constraint ip becomes feasible */
if (std::abs(z.dot(z)) >
std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = -s(ip) / z.dot(np); t2 = -s(ip) / z.dot(np);
else else
t2 = inf; /* +inf */ t2 = inf; /* +inf */
/* the step is chosen as the minimum of t1 and t2 */ /* the step is chosen as the minimum of t1 and t2 */
t = std::min(t1, t2); t = std::min(t1, t2);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") "; std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2
<< ") ";
#endif #endif
/* Step 2c: determine new S-pair and take step: */ /* Step 2c: determine new S-pair and take step: */
...@@ -236,7 +285,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -236,7 +285,7 @@ l2a: /* Step 2a: determine step direction */
u(iq) += t; u(iq) += t;
iai(l) = static_cast<VectorXi::Scalar>(l); iai(l) = static_cast<VectorXi::Scalar>(l);
delete_constraint(R, J, A, u, p, iq, l); delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << " in dual space: " << f_value << std::endl; std::cerr << " in dual space: " << f_value << std::endl;
print_vector("x", x, n); print_vector("x", x, n);
print_vector("z", z, n); print_vector("z", z, n);
...@@ -253,7 +302,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -253,7 +302,7 @@ l2a: /* Step 2a: determine step direction */
u.head(iq) -= t * r.head(iq); u.head(iq) -= t * r.head(iq);
u(iq) += t; u(iq) += t;
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << " in both spaces: " << f_value << std::endl; std::cerr << " in both spaces: " << f_value << std::endl;
print_vector("x", x, n); print_vector("x", x, n);
print_vector("u", u, iq + 1); print_vector("u", u, iq + 1);
...@@ -262,7 +311,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -262,7 +311,7 @@ l2a: /* Step 2a: determine step direction */
#endif #endif
if (t == t2) { if (t == t2) {
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Full step has taken " << t << std::endl; std::cerr << "Full step has taken " << t << std::endl;
print_vector("x", x, n); print_vector("x", x, n);
#endif #endif
...@@ -271,11 +320,12 @@ l2a: /* Step 2a: determine step direction */ ...@@ -271,11 +320,12 @@ l2a: /* Step 2a: determine step direction */
if (!add_constraint(R, J, d, iq, R_norm)) { if (!add_constraint(R, J, d, iq, R_norm)) {
iaexcl(ip) = 0; iaexcl(ip) = 0;
delete_constraint(R, J, A, u, p, iq, ip); delete_constraint(R, J, A, u, p, iq, ip);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("R", R, n); print_matrix("R", R, n);
print_vector("A", A, iq); print_vector("A", A, iq);
#endif #endif
for (i = 0; i < m; i++) iai(i) = static_cast<VectorXi::Scalar>(i); for (i = 0; i < m; i++)
iai(i) = static_cast<VectorXi::Scalar>(i);
for (i = 0; i < iq; i++) { for (i = 0; i < iq; i++) {
A(i) = A_old(i); A(i) = A_old(i);
iai(A(i)) = -1; iai(A(i)) = -1;
...@@ -285,7 +335,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -285,7 +335,7 @@ l2a: /* Step 2a: determine step direction */
goto l2; /* go to step 2 */ goto l2; /* go to step 2 */
} else } else
iai(ip) = -1; iai(ip) = -1;
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("R", R, n); print_matrix("R", R, n);
print_vector("A", A, iq); print_vector("A", A, iq);
#endif #endif
...@@ -293,29 +343,30 @@ l2a: /* Step 2a: determine step direction */ ...@@ -293,29 +343,30 @@ l2a: /* Step 2a: determine step direction */
} }
/* a patial step has taken */ /* a patial step has taken */
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Partial step has taken " << t << std::endl; std::cerr << "Partial step has taken " << t << std::endl;
print_vector("x", x, n); print_vector("x", x, n);
#endif #endif
/* drop constraint l */ /* drop constraint l */
iai(l) = static_cast<VectorXi::Scalar>(l); iai(l) = static_cast<VectorXi::Scalar>(l);
delete_constraint(R, J, A, u, p, iq, l); delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("R", R, n); print_matrix("R", R, n);
print_vector("A", A, iq); print_vector("A", A, iq);
#endif #endif
s(ip) = CI.col(ip).dot(x) + ci0(ip); s(ip) = CI.col(ip).dot(x) + ci0(ip);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_vector("s", s, mi); print_vector("s", s, mi);
#endif #endif
goto l2a; goto l2a;
} }
bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R_norm) { bool add_constraint(MatrixXd &R, MatrixXd &J, VectorXd &d, size_t &iq,
double &R_norm) {
size_t n = J.rows(); size_t n = J.rows();
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Add constraint " << iq << '/'; std::cerr << "Add constraint " << iq << '/';
#endif #endif
size_t j, k; size_t j, k;
...@@ -337,7 +388,8 @@ bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R ...@@ -337,7 +388,8 @@ bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R
cc = d(j - 1); cc = d(j - 1);
ss = d(j); ss = d(j);
h = distance(cc, ss); h = distance(cc, ss);
if (h == 0.0) continue; if (h == 0.0)
continue;
d(j) = 0.0; d(j) = 0.0;
ss = ss / h; ss = ss / h;
cc = cc / h; cc = cc / h;
...@@ -361,7 +413,7 @@ bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R ...@@ -361,7 +413,7 @@ bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R
into column iq - 1 of R into column iq - 1 of R
*/ */
R.col(iq - 1).head(iq) = d.head(iq); R.col(iq - 1).head(iq) = d.head(iq);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << iq << std::endl; std::cerr << iq << std::endl;
#endif #endif
...@@ -372,9 +424,10 @@ bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R ...@@ -372,9 +424,10 @@ bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R
return true; return true;
} }
void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_t p, size_t& iq, size_t l) { void delete_constraint(MatrixXd &R, MatrixXd &J, VectorXi &A, VectorXd &u,
size_t p, size_t &iq, size_t l) {
size_t n = R.rows(); size_t n = R.rows();
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Delete constraint " << l << ' ' << iq; std::cerr << "Delete constraint " << l << ' ' << iq;
#endif #endif
size_t i, j, k, qq = 0; size_t i, j, k, qq = 0;
...@@ -398,20 +451,23 @@ void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_ ...@@ -398,20 +451,23 @@ void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_
u(iq - 1) = u(iq); u(iq - 1) = u(iq);
A(iq) = 0; A(iq) = 0;
u(iq) = 0.0; u(iq) = 0.0;
for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0; for (j = 0; j < iq; j++)
R(j, iq - 1) = 0.0;
/* constraint has been fully removed */ /* constraint has been fully removed */
iq--; iq--;
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << '/' << iq << std::endl; std::cerr << '/' << iq << std::endl;
#endif #endif
if (iq == 0) return; if (iq == 0)
return;
for (j = qq; j < iq; j++) { for (j = qq; j < iq; j++) {
cc = R(j, j); cc = R(j, j);
ss = R(j + 1, j); ss = R(j + 1, j);
h = distance(cc, ss); h = distance(cc, ss);
if (h == 0.0) continue; if (h == 0.0)
continue;
cc = cc / h; cc = cc / h;
ss = ss / h; ss = ss / h;
R(j + 1, j) = 0.0; R(j + 1, j) = 0.0;
...@@ -438,5 +494,5 @@ void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_ ...@@ -438,5 +494,5 @@ void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_
} }
} }
} // namespace solvers } // namespace solvers
} // namespace eiquadprog } // namespace eiquadprog
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment