Commit a75e7dbd authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Format]

parent fb6bbcd5
...@@ -43,32 +43,27 @@ ...@@ -43,32 +43,27 @@
#define DEFAULT_MAX_ITER 1000 #define DEFAULT_MAX_ITER 1000
namespace eiquadprog namespace eiquadprog {
{
namespace solvers namespace solvers {
{
/** /**
* Possible states of the solver. * Possible states of the solver.
*/ */
enum EiquadprogFast_status enum EiquadprogFast_status {
{ EIQUADPROG_FAST_OPTIMAL = 0,
EIQUADPROG_FAST_OPTIMAL=0, EIQUADPROG_FAST_INFEASIBLE = 1,
EIQUADPROG_FAST_INFEASIBLE=1, EIQUADPROG_FAST_UNBOUNDED = 2,
EIQUADPROG_FAST_UNBOUNDED=2, EIQUADPROG_FAST_MAX_ITER_REACHED = 3,
EIQUADPROG_FAST_MAX_ITER_REACHED=3, EIQUADPROG_FAST_REDUNDANT_EQUALITIES = 4
EIQUADPROG_FAST_REDUNDANT_EQUALITIES=4 };
};
class EiquadprogFast {
class EiquadprogFast
{
typedef Eigen::MatrixXd MatrixXd; typedef Eigen::MatrixXd MatrixXd;
typedef Eigen::VectorXd VectorXd; typedef Eigen::VectorXd VectorXd;
typedef Eigen::VectorXi VectorXi; typedef Eigen::VectorXi VectorXi;
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EiquadprogFast(); EiquadprogFast();
...@@ -78,10 +73,8 @@ namespace eiquadprog ...@@ -78,10 +73,8 @@ namespace eiquadprog
int getMaxIter() const { return m_maxIter; } int getMaxIter() const { return m_maxIter; }
bool setMaxIter(int maxIter) bool setMaxIter(int maxIter) {
{ if (maxIter < 0) return false;
if(maxIter<0)
return false;
m_maxIter = maxIter; m_maxIter = maxIter;
return true; return true;
} }
...@@ -105,7 +98,7 @@ namespace eiquadprog ...@@ -105,7 +98,7 @@ namespace eiquadprog
/** /**
* @return The Lagrange multipliers * @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. * Return the active set, namely the indeces of active constraints.
...@@ -115,7 +108,7 @@ namespace eiquadprog ...@@ -115,7 +108,7 @@ namespace eiquadprog
* is the size of the active set. * is the size of the active set.
* @return The set of indexes of the active constraints. * @return The set of indexes of the active constraints.
*/ */
const VectorXi & getActiveSet() const { return A; } const VectorXi& getActiveSet() const { return A; }
/** /**
* solves the problem * solves the problem
...@@ -123,13 +116,8 @@ namespace eiquadprog ...@@ -123,13 +116,8 @@ namespace eiquadprog
* s.t. CE x + ce0 = 0 * s.t. CE x + ce0 = 0
* CI x + ci0 >= 0 * CI x + ci0 >= 0
*/ */
EiquadprogFast_status solve_quadprog(const MatrixXd & Hess, EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
const VectorXd & g0, const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x);
const MatrixXd & CE,
const VectorXd & ce0,
const MatrixXd & CI,
const VectorXd & ci0,
VectorXd & x);
MatrixXd m_J; // J * J' = Hessian <nVars,nVars>::d MatrixXd m_J; // J * J' = Hessian <nVars,nVars>::d
bool is_inverse_provided_; bool is_inverse_provided_;
...@@ -142,7 +130,7 @@ namespace eiquadprog ...@@ -142,7 +130,7 @@ namespace eiquadprog
int m_maxIter; /// max number of active-set iterations int m_maxIter; /// max number of active-set iterations
double f_value; /// current value of cost function 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 /// 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 MatrixXd R; // <nVars,nVars>::d
...@@ -196,10 +184,7 @@ namespace eiquadprog ...@@ -196,10 +184,7 @@ namespace eiquadprog
/// number of active-set iterations /// number of active-set iterations
int iter; int iter;
inline void compute_d(VectorXd & d, inline void compute_d(VectorXd& d, const MatrixXd& J, const VectorXd& np) {
const MatrixXd & J,
const VectorXd & np)
{
#ifdef OPTIMIZE_COMPUTE_D #ifdef OPTIMIZE_COMPUTE_D
d.noalias() = J.adjoint() * np; d.noalias() = J.adjoint() * np;
#else #else
...@@ -207,41 +192,26 @@ namespace eiquadprog ...@@ -207,41 +192,26 @@ namespace eiquadprog
#endif #endif
} }
inline void update_z(VectorXd & z, inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d, size_t iq) {
const MatrixXd & J,
const VectorXd & d,
size_t iq)
{
#ifdef OPTIMIZE_UPDATE_Z #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 #else
z = J.rightCols(J.cols()-iq) * d.tail(J.cols()-iq); z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
#endif #endif
} }
inline void update_r(const MatrixXd & R, inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d, size_t iq) {
VectorXd & r, r.head(iq) = d.head(iq);
const VectorXd & d, R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
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, inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R_norm);
MatrixXd & J,
VectorXd & d,
size_t& iq, double& R_norm);
inline void delete_constraint(MatrixXd & R, inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_t nEqCon, size_t& iq,
MatrixXd & J,
VectorXi & A,
VectorXd & u,
size_t nEqCon, size_t& iq,
size_t l); size_t l);
}; };
} /* namespace solvers */ } /* namespace solvers */
} /* namespace eiquadprog */ } /* namespace eiquadprog */
/* --- Details -------------------------------------------------------------------- */ /* --- Details -------------------------------------------------------------------- */
......
...@@ -5,48 +5,37 @@ ...@@ -5,48 +5,37 @@
#ifndef EIQUADPROGFAST_HXX_ #ifndef EIQUADPROGFAST_HXX_
#define EIQUADPROGFAST_HXX_ #define EIQUADPROGFAST_HXX_
namespace eiquadprog namespace eiquadprog {
{ namespace solvers {
namespace solvers
{ /// Compute sqrt(a^2 + b^2)
template <typename Scalar>
/// Compute sqrt(a^2 + b^2) inline Scalar distance(Scalar a, Scalar b) {
template<typename Scalar>
inline Scalar distance(Scalar a, Scalar b)
{
Scalar a1, b1, t; Scalar a1, b1, t;
a1 = std::abs(a); a1 = std::abs(a);
b1 = std::abs(b); b1 = std::abs(b);
if (a1 > b1) if (a1 > b1) {
{
t = (b1 / a1); t = (b1 / a1);
return a1 * std::sqrt(1.0 + t * t); return a1 * std::sqrt(1.0 + t * t);
} } else if (b1 > a1) {
else
if (b1 > a1)
{
t = (a1 / b1); t = (a1 / b1);
return b1 * std::sqrt(1.0 + t * t); return b1 * std::sqrt(1.0 + t * t);
} }
return a1 * std::sqrt(2.0); return a1 * std::sqrt(2.0);
} }
EiquadprogFast::EiquadprogFast() EiquadprogFast::EiquadprogFast() {
{
m_maxIter = DEFAULT_MAX_ITER; m_maxIter = DEFAULT_MAX_ITER;
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)
is_inverse_provided_ = false; is_inverse_provided_ = false;
m_nVars = 0; m_nVars = 0;
m_nEqCon = 0; m_nEqCon = 0;
m_nIneqCon = 0; m_nIneqCon = 0;
} }
EiquadprogFast::~EiquadprogFast() {} EiquadprogFast::~EiquadprogFast() {}
void EiquadprogFast::reset(size_t nVars, void EiquadprogFast::reset(size_t nVars, size_t nEqCon, size_t nIneqCon) {
size_t nEqCon,
size_t nIneqCon)
{
m_nVars = nVars; m_nVars = nVars;
m_nEqCon = nEqCon; m_nEqCon = nEqCon;
m_nIneqCon = nIneqCon; m_nIneqCon = nIneqCon;
...@@ -54,30 +43,24 @@ namespace eiquadprog ...@@ -54,30 +43,24 @@ namespace eiquadprog
chol_.compute(m_J); chol_.compute(m_J);
R.resize(nVars, nVars); R.resize(nVars, nVars);
s.resize(nIneqCon); s.resize(nIneqCon);
r.resize(nIneqCon+nEqCon); r.resize(nIneqCon + nEqCon);
u.resize(nIneqCon+nEqCon); u.resize(nIneqCon + nEqCon);
z.resize(nVars); z.resize(nVars);
d.resize(nVars); d.resize(nVars);
np.resize(nVars); np.resize(nVars);
A.resize(nIneqCon+nEqCon); A.resize(nIneqCon + nEqCon);
iai.resize(nIneqCon); iai.resize(nIneqCon);
iaexcl.resize(nIneqCon); iaexcl.resize(nIneqCon);
x_old.resize(nVars); x_old.resize(nVars);
u_old.resize(nIneqCon+nEqCon); u_old.resize(nIneqCon + nEqCon);
A_old.resize(nIneqCon+nEqCon); A_old.resize(nIneqCon + nEqCon);
#ifdef OPTIMIZE_ADD_CONSTRAINT #ifdef OPTIMIZE_ADD_CONSTRAINT
T1.resize(nVars); T1.resize(nVars);
#endif #endif
} }
bool EiquadprogFast::add_constraint(MatrixXd & R, bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R_norm) {
MatrixXd & J,
VectorXd & d,
size_t& iq,
double& R_norm)
{
size_t nVars = J.rows(); size_t nVars = J.rows();
#ifdef TRACE_SOLVER #ifdef TRACE_SOLVER
std::cerr << "Add constraint " << iq << '/'; std::cerr << "Add constraint " << iq << '/';
...@@ -93,8 +76,7 @@ namespace eiquadprog ...@@ -93,8 +76,7 @@ namespace eiquadprog
d(j) to zero. d(j) to zero.
if it is already zero we don't have to do anything, except of if it is already zero we don't have to do anything, except of
decreasing j */ 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). /* 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 If cc is one, then element (j) of d is zero compared with element
(j - 1). Hence we don't have to do anything. (j - 1). Hence we don't have to do anything.
...@@ -106,45 +88,41 @@ namespace eiquadprog ...@@ -106,45 +88,41 @@ namespace eiquadprog
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) if (h == 0.0) continue;
continue;
d(j) = 0.0; d(j) = 0.0;
ss = ss / h; ss = ss / h;
cc = cc / h; cc = cc / h;
if (cc < 0.0) if (cc < 0.0) {
{
cc = -cc; cc = -cc;
ss = -ss; ss = -ss;
d(j - 1) = -h; d(j - 1) = -h;
} } else
else
d(j - 1) = h; d(j - 1) = h;
xny = ss / (1.0 + cc); xny = ss / (1.0 + cc);
// #define OPTIMIZE_ADD_CONSTRAINT // #define OPTIMIZE_ADD_CONSTRAINT
#ifdef OPTIMIZE_ADD_CONSTRAINT // the optimized code is actually slower than the original #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(0) = cc;
cc_ss(1) = ss; 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); J.col(j) = xny * (T1 + J.col(j - 1)) - J.col(j);
#else #else
// J.col(j-1) = J[:,j-1:j] * [cc; ss] // J.col(j-1) = J[:,j-1:j] * [cc; ss]
for (k = 0; k < nVars; k++) for (k = 0; k < nVars; k++) {
{ t1 = J(k, j - 1);
t1 = J(k,j - 1); t2 = J(k, j);
t2 = J(k,j); J(k, j - 1) = t1 * cc + t2 * ss;
J(k,j - 1) = t1 * cc + t2 * ss; J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
J(k,j) = xny * (t1 + J(k,j - 1)) - t2;
} }
#endif #endif
} }
/* update the number of constraints added*/ /* update the number of constraints added*/
iq++; iq++;
/* To update R we have to put the iq components of the d vector /* To update R we have to put the iq components of the d vector
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 TRACE_SOLVER
std::cerr << iq << std::endl; std::cerr << iq << std::endl;
#endif #endif
...@@ -154,123 +132,100 @@ namespace eiquadprog ...@@ -154,123 +132,100 @@ namespace eiquadprog
return false; return false;
R_norm = std::max<double>(R_norm, std::abs(d(iq - 1))); R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
return true; 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(); size_t nVars = R.rows();
#ifdef TRACE_SOLVER #ifdef TRACE_SOLVER
std::cerr << "Delete constraint " << l << ' ' << iq; std::cerr << "Delete constraint " << l << ' ' << iq;
#endif #endif
size_t i, j, k; size_t i, j, k;
size_t qq =0; size_t qq = 0;
double cc, ss, h, xny, t1, t2; double cc, ss, h, xny, t1, t2;
/* Find the index qq for active constraint l to be removed */ /* Find the index qq for active constraint l to be removed */
for (i = nEqCon; i < iq; i++) for (i = nEqCon; i < iq; i++)
if (A(i) == static_cast<VectorXi::Scalar>(l)) if (A(i) == static_cast<VectorXi::Scalar>(l)) {
{
qq = i; qq = i;
break; break;
} }
/* remove the constraint from the active set and the duals */ /* 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); A(i) = A(i + 1);
u(i) = u(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); A(iq - 1) = A(iq);
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++) for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0;
R(j,iq - 1) = 0.0;
/* constraint has been fully removed */ /* constraint has been fully removed */
iq--; iq--;
#ifdef TRACE_SOLVER #ifdef TRACE_SOLVER
std::cerr << '/' << iq << std::endl; std::cerr << '/' << iq << std::endl;
#endif #endif
if (iq == 0) if (iq == 0) return;
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) if (h == 0.0) continue;
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;
if (cc < 0.0) if (cc < 0.0) {
{ R(j, j) = -h;
R(j,j) = -h;
cc = -cc; cc = -cc;
ss = -ss; ss = -ss;
} } else
else R(j, j) = h;
R(j,j) = h;
xny = ss / (1.0 + cc); xny = ss / (1.0 + cc);
for (k = j + 1; k < iq; k++) for (k = j + 1; k < iq; k++) {
{ t1 = R(j, k);
t1 = R(j,k); t2 = R(j + 1, k);
t2 = R(j + 1,k); R(j, k) = t1 * cc + t2 * ss;
R(j,k) = t1 * cc + t2 * ss; R(j + 1, k) = xny * (t1 + R(j, k)) - t2;
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 = 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> template <class Derived>
void print_vector(const char* name, Eigen::MatrixBase<Derived>& x, int n){ void print_vector(const char* name, Eigen::MatrixBase<Derived>& x, int n) {
// std::cerr << name << x.transpose() << std::endl; // std::cerr << name << x.transpose() << std::endl;
} }
template<class Derived> template <class Derived>
void print_matrix(const char* name, Eigen::MatrixBase<Derived>& x, int n){ void print_matrix(const char* name, Eigen::MatrixBase<Derived>& x, int n) {
// std::cerr << name << std::endl << x << std::endl; // std::cerr << name << std::endl << x << std::endl;
} }