Skip to content
GitLab
Commit a75e7dbd authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Format]

parent fb6bbcd5
Show whitespace changes
Inline Side-by-side
include/eiquadprog/eiquadprog-fast.hpp
View file @ a75e7dbd
......@@ -43,32 +43,27 @@
#define DEFAULT_MAX_ITER 1000
namespace eiquadprog
{
namespace eiquadprog {
namespace solvers
{
namespace solvers {
/**
/**
* Possible states of the solver.
*/
enum EiquadprogFast_status
{
EIQUADPROG_FAST_OPTIMAL=0,
EIQUADPROG_FAST_INFEASIBLE=1,
EIQUADPROG_FAST_UNBOUNDED=2,
EIQUADPROG_FAST_MAX_ITER_REACHED=3,
EIQUADPROG_FAST_REDUNDANT_EQUALITIES=4
};
class EiquadprogFast
{
enum EiquadprogFast_status {
EIQUADPROG_FAST_OPTIMAL = 0,
EIQUADPROG_FAST_INFEASIBLE = 1,
EIQUADPROG_FAST_UNBOUNDED = 2,
EIQUADPROG_FAST_MAX_ITER_REACHED = 3,
EIQUADPROG_FAST_REDUNDANT_EQUALITIES = 4
};
class EiquadprogFast {
typedef Eigen::MatrixXd MatrixXd;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::VectorXi VectorXi;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EiquadprogFast();
......@@ -78,10 +73,8 @@ namespace eiquadprog
int getMaxIter() const { return m_maxIter; }
bool setMaxIter(int maxIter)
{
if(maxIter<0)
return false;
bool setMaxIter(int maxIter) {
if (maxIter < 0) return false;
m_maxIter = maxIter;
return true;
}
......@@ -105,7 +98,7 @@ namespace eiquadprog
/**
* @return The Lagrange multipliers
*/
const VectorXd & getLagrangeMultipliers() const { return u; }
const VectorXd& getLagrangeMultipliers() const { return u; }
/**
* Return the active set, namely the indeces of active constraints.
......@@ -115,7 +108,7 @@ namespace eiquadprog
* is the size of the active set.
* @return The set of indexes of the active constraints.
*/
const VectorXi & getActiveSet() const { return A; }
const VectorXi& getActiveSet() const { return A; }
/**
* solves the problem
......@@ -123,13 +116,8 @@ namespace eiquadprog
* s.t. CE x + ce0 = 0
* CI x + ci0 >= 0
*/
EiquadprogFast_status solve_quadprog(const MatrixXd & Hess,
const VectorXd & g0,
const MatrixXd & CE,
const VectorXd & ce0,
const MatrixXd & CI,
const VectorXd & ci0,
VectorXd & x);
EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x);
MatrixXd m_J; // J * J' = Hessian <nVars,nVars>::d
bool is_inverse_provided_;
......@@ -142,7 +130,7 @@ namespace eiquadprog
int m_maxIter; /// max number of active-set iterations
double f_value; /// current value of cost function
Eigen::LLT<MatrixXd,Eigen::Lower> chol_; // <nVars,nVars>::d
Eigen::LLT<MatrixXd, Eigen::Lower> chol_; // <nVars,nVars>::d
/// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the matrix of active constraints
MatrixXd R; // <nVars,nVars>::d
......@@ -196,10 +184,7 @@ namespace eiquadprog
/// number of active-set iterations
int iter;
inline void compute_d(VectorXd & d,
const MatrixXd & J,
const VectorXd & np)
{
inline void compute_d(VectorXd& d, const MatrixXd& J, const VectorXd& np) {
#ifdef OPTIMIZE_COMPUTE_D
d.noalias() = J.adjoint() * np;
#else
......@@ -207,41 +192,26 @@ namespace eiquadprog
#endif
}
inline void update_z(VectorXd & z,
const MatrixXd & J,
const VectorXd & d,
size_t iq)
{
inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d, size_t iq) {
#ifdef OPTIMIZE_UPDATE_Z
z.noalias() = J.rightCols(z.size()-iq) * d.tail(z.size()-iq);
z.noalias() = J.rightCols(z.size() - iq) * d.tail(z.size() - iq);
#else
z = J.rightCols(J.cols()-iq) * d.tail(J.cols()-iq);
z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
#endif
}
inline void update_r(const MatrixXd & R,
VectorXd & r,
const VectorXd & d,
size_t iq)
{
r.head(iq)= d.head(iq);
R.topLeftCorner(iq,iq).triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d, size_t iq) {
r.head(iq) = d.head(iq);
R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
}
inline bool add_constraint(MatrixXd & R,
MatrixXd & J,
VectorXd & d,
size_t& iq, double& R_norm);
inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R_norm);
inline void delete_constraint(MatrixXd & R,
MatrixXd & J,
VectorXi & A,
VectorXd & u,
size_t nEqCon, size_t& iq,
inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_t nEqCon, size_t& iq,
size_t l);
};
};
} /* namespace solvers */
} /* namespace solvers */
} /* namespace eiquadprog */
/* --- Details -------------------------------------------------------------------- */
......
include/eiquadprog/eiquadprog-fast.hxx
View file @ a75e7dbd
......@@ -5,48 +5,37 @@
#ifndef EIQUADPROGFAST_HXX_
#define EIQUADPROGFAST_HXX_
namespace eiquadprog
{
namespace solvers
{
/// Compute sqrt(a^2 + b^2)
template<typename Scalar>
inline Scalar distance(Scalar a, Scalar b)
{
namespace eiquadprog {
namespace solvers {
/// Compute sqrt(a^2 + b^2)
template <typename Scalar>
inline Scalar distance(Scalar a, Scalar b) {
Scalar a1, b1, t;
a1 = std::abs(a);
b1 = std::abs(b);
if (a1 > b1)
{
if (a1 > b1) {
t = (b1 / a1);
return a1 * std::sqrt(1.0 + t * t);
}
else
if (b1 > a1)
{
} else if (b1 > a1) {
t = (a1 / b1);
return b1 * std::sqrt(1.0 + t * t);
}
return a1 * std::sqrt(2.0);
}
}
EiquadprogFast::EiquadprogFast()
{
EiquadprogFast::EiquadprogFast() {
m_maxIter = DEFAULT_MAX_ITER;
q = 0; // size of the active set A (containing the indices of the active constraints)
is_inverse_provided_ = false;
m_nVars = 0;
m_nEqCon = 0;
m_nIneqCon = 0;
}
}
EiquadprogFast::~EiquadprogFast() {}
EiquadprogFast::~EiquadprogFast() {}
void EiquadprogFast::reset(size_t nVars,
size_t nEqCon,
size_t nIneqCon)
{
void EiquadprogFast::reset(size_t nVars, size_t nEqCon, size_t nIneqCon) {
m_nVars = nVars;
m_nEqCon = nEqCon;
m_nIneqCon = nIneqCon;
......@@ -54,30 +43,24 @@ namespace eiquadprog
chol_.compute(m_J);
R.resize(nVars, nVars);
s.resize(nIneqCon);
r.resize(nIneqCon+nEqCon);
u.resize(nIneqCon+nEqCon);
r.resize(nIneqCon + nEqCon);
u.resize(nIneqCon + nEqCon);
z.resize(nVars);
d.resize(nVars);
np.resize(nVars);
A.resize(nIneqCon+nEqCon);
A.resize(nIneqCon + nEqCon);
iai.resize(nIneqCon);
iaexcl.resize(nIneqCon);
x_old.resize(nVars);
u_old.resize(nIneqCon+nEqCon);
A_old.resize(nIneqCon+nEqCon);
u_old.resize(nIneqCon + nEqCon);
A_old.resize(nIneqCon + nEqCon);
#ifdef OPTIMIZE_ADD_CONSTRAINT
T1.resize(nVars);
#endif
}
}
bool EiquadprogFast::add_constraint(MatrixXd & R,
MatrixXd & J,
VectorXd & d,
size_t& iq,
double& R_norm)
{
bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R_norm) {
size_t nVars = J.rows();
#ifdef TRACE_SOLVER
std::cerr << "Add constraint " << iq << '/';
......@@ -93,8 +76,7 @@ namespace eiquadprog
d(j) to zero.
if it is already zero we don't have to do anything, except of
decreasing j */
for (j = d.size() - 1; j >= iq + 1; j--)
{
for (j = d.size() - 1; j >= iq + 1; j--) {
/* The Givens rotation is done with the matrix (cc cs, cs -cc).
If cc is one, then element (j) of d is zero compared with element
(j - 1). Hence we don't have to do anything.
......@@ -106,45 +88,41 @@ namespace eiquadprog
cc = d(j - 1);
ss = d(j);
h = distance(cc, ss);
if (h == 0.0)
continue;
if (h == 0.0) continue;
d(j) = 0.0;
ss = ss / h;
cc = cc / h;
if (cc < 0.0)
{
if (cc < 0.0) {
cc = -cc;
ss = -ss;
d(j - 1) = -h;
}
else
} else
d(j - 1) = h;
xny = ss / (1.0 + cc);
// #define OPTIMIZE_ADD_CONSTRAINT
#ifdef OPTIMIZE_ADD_CONSTRAINT // the optimized code is actually slower than the original
T1 = J.col(j-1);
T1 = J.col(j - 1);
cc_ss(0) = cc;
cc_ss(1) = ss;
J.col(j-1).noalias() = J.middleCols<2>(j-1) * cc_ss;
J.col(j - 1).noalias() = J.middleCols<2>(j - 1) * cc_ss;
J.col(j) = xny * (T1 + J.col(j - 1)) - J.col(j);
#else
// J.col(j-1) = J[:,j-1:j] * [cc; ss]
for (k = 0; k < nVars; k++)
{
t1 = J(k,j - 1);
t2 = J(k,j);
J(k,j - 1) = t1 * cc + t2 * ss;
J(k,j) = xny * (t1 + J(k,j - 1)) - t2;
for (k = 0; k < nVars; k++) {
t1 = J(k, j - 1);
t2 = J(k, j);
J(k, j - 1) = t1 * cc + t2 * ss;
J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
}
#endif
}
/* update the number of constraints added*/
iq++;
/* To update R we have to put the iq components of the d vector
into column iq - 1 of R
*/
R.col(iq-1).head(iq) = d.head(iq);
into column iq - 1 of R
*/
R.col(iq - 1).head(iq) = d.head(iq);
#ifdef TRACE_SOLVER
std::cerr << iq << std::endl;
#endif
......@@ -154,123 +132,100 @@ namespace eiquadprog
return false;
R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
return true;
}
void EiquadprogFast::delete_constraint(MatrixXd & R,
MatrixXd & J,
VectorXi & A,
VectorXd & u,
size_t nEqCon,
size_t& iq,
size_t l)
{
}
void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_t nEqCon, size_t& iq,
size_t l) {
size_t nVars = R.rows();
#ifdef TRACE_SOLVER
std::cerr << "Delete constraint " << l << ' ' << iq;
#endif
size_t i, j, k;
size_t qq =0;
size_t qq = 0;
double cc, ss, h, xny, t1, t2;
/* Find the index qq for active constraint l to be removed */
for (i = nEqCon; i < iq; i++)
if (A(i) == static_cast<VectorXi::Scalar>(l))
{
if (A(i) == static_cast<VectorXi::Scalar>(l)) {
qq = i;
break;
}
/* remove the constraint from the active set and the duals */
for (i = qq; i < iq - 1; i++)
{
for (i = qq; i < iq - 1; i++) {
A(i) = A(i + 1);
u(i) = u(i + 1);
R.col(i) = R.col(i+1);
R.col(i) = R.col(i + 1);
}
A(iq - 1) = A(iq);
u(iq - 1) = u(iq);
A(iq) = 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 */
iq--;
#ifdef TRACE_SOLVER
std::cerr << '/' << iq << std::endl;
#endif
if (iq == 0)
return;
if (iq == 0) return;
for (j = qq; j < iq; j++)
{
cc = R(j,j);
ss = R(j + 1,j);
for (j = qq; j < iq; j++) {
cc = R(j, j);
ss = R(j + 1, j);
h = distance(cc, ss);
if (h == 0.0)
continue;
if (h == 0.0) continue;
cc = cc / h;
ss = ss / h;
R(j + 1,j) = 0.0;
if (cc < 0.0)
{
R(j,j) = -h;
R(j + 1, j) = 0.0;
if (cc < 0.0) {
R(j, j) = -h;
cc = -cc;
ss = -ss;
}
else
R(j,j) = h;
} else
R(j, j) = h;
xny = ss / (1.0 + cc);
for (k = j + 1; k < iq; k++)
{
t1 = R(j,k);
t2 = R(j + 1,k);
R(j,k) = t1 * cc + t2 * ss;
R(j + 1,k) = xny * (t1 + R(j,k)) - t2;
}
for (k = 0; k < nVars; k++)
{
t1 = J(k,j);
t2 = J(k,j + 1);
J(k,j) = t1 * cc + t2 * ss;
J(k,j + 1) = xny * (J(k,j) + t1) - t2;
for (k = j + 1; k < iq; k++) {
t1 = R(j, k);
t2 = R(j + 1, k);
R(j, k) = t1 * cc + t2 * ss;
R(j + 1, k) = xny * (t1 + R(j, k)) - t2;
}
for (k = 0; k < nVars; k++) {
t1 = J(k, j);
t2 = J(k, j + 1);
J(k, j) = t1 * cc + t2 * ss;
J(k, j + 1) = xny * (J(k, j) + t1) - t2;
}
}
}
template<class Derived>
void print_vector(const char* name, Eigen::MatrixBase<Derived>& x, int n){
template <class Derived>
void print_vector(const char* name, Eigen::MatrixBase<Derived>& x, int n) {
// std::cerr << name << x.transpose() << std::endl;
}
template<class Derived>
void print_matrix(const char* name, Eigen::MatrixBase<Derived>& x, int n){
}
template <class Derived>
void print_matrix(const char* name, Eigen::MatrixBase<Derived>& x, int n) {
// std::cerr << name << std::endl << x << std::endl;
}
}
EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd & Hess,
const VectorXd & g0,
const MatrixXd & CE,
const VectorXd & ce0,
const MatrixXd & CI,
const VectorXd & ci0,
VectorXd & x)
{
EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0,
VectorXd& x) {
const size_t nVars = g0.size();
const size_t nEqCon = ce0.size();
const size_t nIneqCon = ci0.size();
if(nVars!=m_nVars || nEqCon!=m_nEqCon || nIneqCon!=m_nIneqCon)
reset(nVars, nEqCon, nIneqCon);
if (nVars != m_nVars || nEqCon != m_nEqCon || nIneqCon != m_nIneqCon) reset(nVars, nEqCon, nIneqCon);
assert(static_cast<size_t>(Hess.rows())==m_nVars && static_cast<size_t>(Hess.cols())==m_nVars);
assert(static_cast<size_t>(g0.size())==m_nVars);
assert(static_cast<size_t>(CE.rows())==m_nEqCon && static_cast<size_t>(CE.cols())==m_nVars);
assert(static_cast<size_t>(ce0.size())==m_nEqCon);
assert(static_cast<size_t>(CI.rows())==m_nIneqCon && static_cast<size_t>(CI.cols())==m_nVars);
assert(static_cast<size_t>(ci0.size())==m_nIneqCon);
assert(static_cast<size_t>(Hess.rows()) == m_nVars && static_cast<size_t>(Hess.cols()) == m_nVars);
assert(static_cast<size_t>(g0.size()) == m_nVars);
assert(static_cast<size_t>(CE.rows()) == m_nEqCon && static_cast<size_t>(CE.cols()) == m_nVars);
assert(static_cast<size_t>(ce0.size()) == m_nEqCon);
assert(static_cast<size_t>(CI.rows()) == m_nIneqCon && static_cast<size_t>(CI.cols()) == m_nVars);
assert(static_cast<size_t>(ci0.size()) == m_nIneqCon);
size_t i, k, l; // indices
size_t ip; // index of the chosen violated constraint
......@@ -294,14 +249,12 @@ namespace eiquadprog
c1 = Hess.trace();
/* decompose the matrix Hess in the form LL^T */
if(!is_inverse_provided_)
{
if (!is_inverse_provided_) {
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION);
chol_.compute(Hess);
STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION);
}
/* initialize the matrix R */
d.setZero(nVars);
R.setZero(nVars, nVars);
......@@ -309,8 +262,7 @@ namespace eiquadprog
/* compute the inverse of the factorized matrix Hess^-1, this is the initial value for H */
// m_J = L^-T
if(!is_inverse_provided_)
{
if (!is_inverse_provided_) {
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_INVERSE);
m_J.setIdentity(nVars, nVars);
#ifdef OPTIMIZE_HESSIAN_INVERSE
......@@ -334,12 +286,9 @@ namespace eiquadprog
* x = Hess^-1 * g0
*/
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
if(is_inverse_provided_)
{
x = m_J*(m_J.transpose()*g0);
}
else
{
if (is_inverse_provided_) {
x = m_J * (m_J.transpose() * g0);
} else {
#ifdef OPTIMIZE_UNCONSTR_MINIM
x = -g0;
chol_.solveInPlace(x);
......@@ -361,8 +310,7 @@ namespace eiquadprog
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
iq = 0;
for (i = 0; i < nEqCon; i++)
{
for (i = 0; i < nEqCon; i++) {
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
np = CE.row(i);
compute_d(d, m_J, np);
......@@ -394,8 +342,7 @@ namespace eiquadprog
STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
if (!add_constraint(R, m_J, d, iq, R_norm))
{
if (!add_constraint(R, m_J, d, iq, R_norm)) {
// Equality constraints are linearly dependent
return EIQUADPROG_FAST_REDUNDANT_EQUALITIES;
}
......@@ -404,14 +351,12 @@ namespace eiquadprog
STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
/* set iai = K \ A */
for (i = 0; i < nIneqCon; i++)
iai(i) = static_cast<VectorXi::Scalar>(i);
for (i = 0; i < nIneqCon; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
#ifdef USE_WARM_START
// DEBUG_STREAM("Gonna warm start using previous active set:\n"<<A.transpose()<<"\n")
for (i = nEqCon; i < q; i++)
{
iai(i-nEqCon) = -1;
for (i = nEqCon; i < q; i++) {
iai(i - nEqCon) = -1;
ip = A(i);
np = CI.row(ip);
compute_d(d, m_J, np);
......@@ -435,8 +380,7 @@ namespace eiquadprog
/* compute the new solution value */
f_value += 0.5 * (t2 * t2) * z.dot(np);
if (!add_constraint(R, m_J, d, iq, R_norm))
{
if (!add_constraint(R, m_J, d, iq, R_norm)) {
// constraints are linearly dependent
std::cerr << "[WARM START] Constraints are linearly dependent\n";
return RT_EIQUADPROG_REDUNDANT_EQUALITIES;
......@@ -448,8 +392,7 @@ namespace eiquadprog
l1:
iter++;
if(iter>=m_maxIter)
{
if (iter >= m_maxIter) {
q = iq;
return EIQUADPROG_FAST_MAX_ITER_REACHED;
}
......@@ -460,8 +403,7 @@ l1:
print_vector("x", x, nVars);
#endif
/* step 1: choose a violated constraint */