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

[Format]

parent fb6bbcd5
...@@ -7,7 +7,7 @@ ...@@ -7,7 +7,7 @@
#include <Eigen/Dense> #include <Eigen/Dense>
#define OPTIMIZE_STEP_1_2 // compute s(x) = ci^T * x + ci0 #define OPTIMIZE_STEP_1_2 // compute s(x) = ci^T * x + ci0
#define OPTIMIZE_COMPUTE_D #define OPTIMIZE_COMPUTE_D
#define OPTIMIZE_UPDATE_Z #define OPTIMIZE_UPDATE_Z
#define OPTIMIZE_HESSIAN_INVERSE #define OPTIMIZE_HESSIAN_INVERSE
...@@ -21,227 +21,197 @@ ...@@ -21,227 +21,197 @@
#ifdef PROFILE_EIQUADPROG #ifdef PROFILE_EIQUADPROG
#define START_PROFILER_EIQUADPROG_FAST(x) START_PROFILER(x) #define START_PROFILER_EIQUADPROG_FAST(x) START_PROFILER(x)
#define STOP_PROFILER_EIQUADPROG_FAST(x) STOP_PROFILER(x) #define STOP_PROFILER_EIQUADPROG_FAST(x) STOP_PROFILER(x)
#else #else
#define START_PROFILER_EIQUADPROG_FAST(x) #define START_PROFILER_EIQUADPROG_FAST(x)
#define STOP_PROFILER_EIQUADPROG_FAST(x) #define STOP_PROFILER_EIQUADPROG_FAST(x)
#endif #endif
#define EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION "EIQUADPROG_FAST Cholesky dec" #define EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION "EIQUADPROG_FAST Cholesky dec"
#define EIQUADPROG_FAST_CHOLESKY_INVERSE "EIQUADPROG_FAST Cholesky inv" #define EIQUADPROG_FAST_CHOLESKY_INVERSE "EIQUADPROG_FAST Cholesky inv"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR "EIQUADPROG_FAST ADD_EQ_CONSTR" #define EIQUADPROG_FAST_ADD_EQ_CONSTR "EIQUADPROG_FAST ADD_EQ_CONSTR"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR_1 "EIQUADPROG_FAST ADD_EQ_CONSTR_1" #define EIQUADPROG_FAST_ADD_EQ_CONSTR_1 "EIQUADPROG_FAST ADD_EQ_CONSTR_1"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR_2 "EIQUADPROG_FAST ADD_EQ_CONSTR_2" #define EIQUADPROG_FAST_ADD_EQ_CONSTR_2 "EIQUADPROG_FAST ADD_EQ_CONSTR_2"
#define EIQUADPROG_FAST_STEP_1 "EIQUADPROG_FAST STEP_1" #define EIQUADPROG_FAST_STEP_1 "EIQUADPROG_FAST STEP_1"
#define EIQUADPROG_FAST_STEP_1_1 "EIQUADPROG_FAST STEP_1_1" #define EIQUADPROG_FAST_STEP_1_1 "EIQUADPROG_FAST STEP_1_1"
#define EIQUADPROG_FAST_STEP_1_2 "EIQUADPROG_FAST STEP_1_2" #define EIQUADPROG_FAST_STEP_1_2 "EIQUADPROG_FAST STEP_1_2"
#define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM "EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM" #define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM "EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM"
#define EIQUADPROG_FAST_STEP_2 "EIQUADPROG_FAST STEP_2" #define EIQUADPROG_FAST_STEP_2 "EIQUADPROG_FAST STEP_2"
#define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A" #define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
#define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B" #define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
#define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C" #define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C"
#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 {
*/ EIQUADPROG_FAST_OPTIMAL = 0,
enum EiquadprogFast_status EIQUADPROG_FAST_INFEASIBLE = 1,
{ EIQUADPROG_FAST_UNBOUNDED = 2,
EIQUADPROG_FAST_OPTIMAL=0, EIQUADPROG_FAST_MAX_ITER_REACHED = 3,
EIQUADPROG_FAST_INFEASIBLE=1, EIQUADPROG_FAST_REDUNDANT_EQUALITIES = 4
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;
class EiquadprogFast typedef Eigen::VectorXi VectorXi;
{
typedef Eigen::MatrixXd MatrixXd; public:
typedef Eigen::VectorXd VectorXd; EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef Eigen::VectorXi VectorXi;
EiquadprogFast();
public: virtual ~EiquadprogFast();
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void reset(size_t dim_qp, size_t num_eq, size_t num_ineq);
EiquadprogFast(); int getMaxIter() const { return m_maxIter; }
virtual ~EiquadprogFast();
bool setMaxIter(int maxIter) {
void reset(size_t dim_qp, size_t num_eq, size_t num_ineq); if (maxIter < 0) return false;
m_maxIter = maxIter;
int getMaxIter() const { return m_maxIter; } return true;
}
bool setMaxIter(int maxIter)
{ /**
if(maxIter<0) * @return The size of the active set, namely the number of
return false; * active constraints (including the equalities).
m_maxIter = maxIter; */
return true; size_t getActiveSetSize() const { return q; }
}
/**
/** * @return The number of active-set iteratios.
* @return The size of the active set, namely the number of */
* active constraints (including the equalities). int getIteratios() const { return iter; }
*/
size_t getActiveSetSize() const { return q; } /**
* @return The value of the objective function.
/** */
* @return The number of active-set iteratios. double getObjValue() const { return f_value; }
*/
int getIteratios() const { return iter; } /**
* @return The Lagrange multipliers
/** */
* @return The value of the objective function. const VectorXd& getLagrangeMultipliers() const { return u; }
*/
double getObjValue() const { return f_value; } /**
* Return the active set, namely the indeces of active constraints.
/** * The first nEqCon indexes are for the equalities and are negative.
* @return The Lagrange multipliers * The last nIneqCon indexes are for the inequalities and start from 0.
*/ * Only the first q elements of the return vector are valid, where q
const VectorXd & getLagrangeMultipliers() const { return u; } * is the size of the active set.
* @return The set of indexes of the active constraints.
/** */
* Return the active set, namely the indeces of active constraints. const VectorXi& getActiveSet() const { return A; }
* The first nEqCon indexes are for the equalities and are negative.
* The last nIneqCon indexes are for the inequalities and start from 0. /**
* Only the first q elements of the return vector are valid, where q * solves the problem
* is the size of the active set. * min. x' Hess x + 2 g0' x
* @return The set of indexes of the active constraints. * s.t. CE x + ce0 = 0
*/ * CI x + ci0 >= 0
const VectorXi & getActiveSet() const { return A; } */
EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
/** const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x);
* solves the problem
* min. x' Hess x + 2 g0' x MatrixXd m_J; // J * J' = Hessian <nVars,nVars>::d
* s.t. CE x + ce0 = 0 bool is_inverse_provided_;
* CI x + ci0 >= 0
*/ private:
EiquadprogFast_status solve_quadprog(const MatrixXd & Hess, size_t m_nVars;
const VectorXd & g0, size_t m_nEqCon;
const MatrixXd & CE, size_t m_nIneqCon;
const VectorXd & ce0,
const MatrixXd & CI, int m_maxIter; /// max number of active-set iterations
const VectorXd & ci0, double f_value; /// current value of cost function
VectorXd & x);
Eigen::LLT<MatrixXd, Eigen::Lower> chol_; // <nVars,nVars>::d
MatrixXd m_J; // J * J' = Hessian <nVars,nVars>::d
bool is_inverse_provided_; /// 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
private:
size_t m_nVars; /// CI*x+ci0
size_t m_nEqCon; VectorXd s; // <nIneqCon>::d
size_t m_nIneqCon;
/// infeasibility multipliers, i.e. negative step direction in dual space
int m_maxIter; /// max number of active-set iterations VectorXd r; // <nIneqCon+nEqCon>::d
double f_value; /// current value of cost function
/// Lagrange multipliers
Eigen::LLT<MatrixXd,Eigen::Lower> chol_; // <nVars,nVars>::d VectorXd u; // <nIneqCon+nEqCon>::d
/// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the matrix of active constraints /// step direction in primal space
MatrixXd R; // <nVars,nVars>::d VectorXd z; // <nVars>::d
/// CI*x+ci0 /// J' np
VectorXd s; // <nIneqCon>::d VectorXd d; //<nVars>::d
/// infeasibility multipliers, i.e. negative step direction in dual space /// current constraint normal
VectorXd r; // <nIneqCon+nEqCon>::d VectorXd np; //<nVars>::d
/// Lagrange multipliers /// active set (indeces of active constraints)
VectorXd u; // <nIneqCon+nEqCon>::d /// the first nEqCon indeces are for the equalities and are negative
/// the last nIneqCon indeces are for the inequalities are start from 0
/// step direction in primal space VectorXi A; // <nIneqCon+nEqCon>
VectorXd z; // <nVars>::d
/// initialized as K \ A
/// J' np /// iai(i)=-1 iff inequality constraint i is in the active set
VectorXd d; //<nVars>::d /// iai(i)=i otherwise
VectorXi iai; // <nIneqCon>::i
/// current constraint normal
VectorXd np; //<nVars>::d /// initialized as [1, ..., 1, .]
/// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set
/// active set (indeces of active constraints) /// if adding ineq constraint i fails => iaexcl(i)=0
/// the first nEqCon indeces are for the equalities and are negative /// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active constraints
/// the last nIneqCon indeces are for the inequalities are start from 0 /// iaexcl(i)=1 otherwise
VectorXi A; // <nIneqCon+nEqCon> VectorXi iaexcl; //<nIneqCon>::i
/// initialized as K \ A VectorXd x_old; // old value of x <nVars>::d
/// iai(i)=-1 iff inequality constraint i is in the active set VectorXd u_old; // old value of u <nIneqCon+nEqCon>::d
/// iai(i)=i otherwise VectorXi A_old; // old value of A <nIneqCon+nEqCon>::i
VectorXi iai; // <nIneqCon>::i
/// initialized as [1, ..., 1, .]
/// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set
/// if adding ineq constraint i fails => iaexcl(i)=0
/// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active constraints
/// iaexcl(i)=1 otherwise
VectorXi iaexcl; //<nIneqCon>::i
VectorXd x_old; // old value of x <nVars>::d
VectorXd u_old; // old value of u <nIneqCon+nEqCon>::d
VectorXi A_old; // old value of A <nIneqCon+nEqCon>::i
#ifdef OPTIMIZE_ADD_CONSTRAINT #ifdef OPTIMIZE_ADD_CONSTRAINT
VectorXd T1; /// tmp variable used in add_constraint VectorXd T1; /// tmp variable used in add_constraint
#endif #endif
/// size of the active set A (containing the indices of the active constraints) /// size of the active set A (containing the indices of the active constraints)
size_t q; size_t q;
/// 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
d = J.adjoint() * np; d = J.adjoint() * np;
#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); inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R_norm);
R.topLeftCorner(iq,iq).triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
} inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_t nEqCon, size_t& iq,
size_t l);
inline bool add_constraint(MatrixXd & R, };
MatrixXd & J,
VectorXd & d, } /* namespace solvers */
size_t& iq, double& R_norm);
inline void delete_constraint(MatrixXd & R,
MatrixXd & J,
VectorXi & A,
VectorXd & u,
size_t nEqCon, size_t& iq,
size_t l);
};
} /* namespace solvers */
} /* namespace eiquadprog */ } /* namespace eiquadprog */
/* --- Details -------------------------------------------------------------------- */ /* --- Details -------------------------------------------------------------------- */
......
...@@ -5,695 +5,619 @@ ...@@ -5,695 +5,619 @@
#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> Scalar a1, b1, t;
inline Scalar distance(Scalar a, Scalar b) a1 = std::abs(a);
{ b1 = std::abs(b);
Scalar a1, b1, t; if (a1 > b1) {
a1 = std::abs(a); t = (b1 / a1);
b1 = std::abs(b); return a1 * std::sqrt(1.0 + t * t);
if (a1 > b1) } else if (b1 > a1) {
{ t = (a1 / b1);
t = (b1 / a1); return b1 * std::sqrt(1.0 + t * t);
return a1 * std::sqrt(1.0 + t * t); }
} return a1 * std::sqrt(2.0);
else }
if (b1 > a1)
{ EiquadprogFast::EiquadprogFast() {
t = (a1 / b1); m_maxIter = DEFAULT_MAX_ITER;
return b1 * std::sqrt(1.0 + t * t); q = 0; // size of the active set A (containing the indices of the active constraints)
} is_inverse_provided_ = false;
return a1 * std::sqrt(2.0); m_nVars = 0;
} m_nEqCon = 0;
m_nIneqCon = 0;
EiquadprogFast::EiquadprogFast() }
{
m_maxIter = DEFAULT_MAX_ITER; EiquadprogFast::~EiquadprogFast() {}
q = 0; // size of the active set A (containing the indices of the active constraints)
is_inverse_provided_ = false; void EiquadprogFast::reset(size_t nVars, size_t nEqCon, size_t nIneqCon) {
m_nVars = 0; m_nVars = nVars;
m_nEqCon = 0; m_nEqCon = nEqCon;
m_nIneqCon = 0; m_nIneqCon = nIneqCon;
} m_J.setZero(nVars, nVars);
chol_.compute(m_J);
EiquadprogFast::~EiquadprogFast() {} R.resize(nVars, nVars);
s.resize(nIneqCon);
void EiquadprogFast::reset(size_t nVars, r.resize(nIneqCon + nEqCon);
size_t nEqCon, u.resize(nIneqCon + nEqCon);
size_t nIneqCon) z.resize(nVars);
{ d.resize(nVars);
m_nVars = nVars; np.resize(nVars);
m_nEqCon = nEqCon; A.resize(nIneqCon + nEqCon);
m_nIneqCon = nIneqCon; iai.resize(nIneqCon);
m_J.setZero(nVars, nVars); iaexcl.resize(nIneqCon);
chol_.compute(m_J); x_old.resize(nVars);
R.resize(nVars, nVars); u_old.resize(nIneqCon + nEqCon);
s.resize(nIneqCon); A_old.resize(nIneqCon + nEqCon);
r.resize(nIneqCon+nEqCon);
u.resize(nIneqCon+nEqCon);
z.resize(nVars);
d.resize(nVars);
np.resize(nVars);
A.resize(nIneqCon+nEqCon);
iai.resize(nIneqCon);
iaexcl.resize(nIneqCon);
x_old.resize(nVars);
u_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, MatrixXd& J, VectorXd& d, size_t& iq, double& R_norm) {
bool EiquadprogFast::add_constraint(MatrixXd & R, size_t nVars = J.rows();
MatrixXd & J,
VectorXd & d,
size_t& iq,
double& R_norm)
{
size_t nVars = J.rows();
#ifdef TRACE_SOLVER #ifdef TRACE_SOLVER
std::cerr << "Add constraint " << iq << '/'; std::cerr << "Add constraint " << iq << '/';
#endif #endif
size_t j, k; size_t j, k;
double cc, ss, h, t1, t2, xny; double cc, ss, h, t1, t2, xny;
#ifdef OPTIMIZE_ADD_CONSTRAINT #ifdef OPTIMIZE_ADD_CONSTRAINT
Eigen::Vector2d cc_ss; Eigen::Vector2d cc_ss;
#endif #endif
/* we have to find the Givens rotation which will reduce the element /* we have to find the Givens rotation which will reduce the element
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. If cc is zero, then we just have to switch column (j) and column (j - 1)
If cc is zero, then we just have to switch column (j) and column (j - 1) of J. Since we only switch columns in J, we have to be careful how we
of J. Since we only switch columns in J, we have to be careful how we update d depending on the sign of gs.
update d depending on the sign of gs. Otherwise we have to apply the Givens rotation to these columns.
Otherwise we have to apply the Givens rotation to these columns. The i - 1 element of d has to be updated to h. */
The i - 1 element of d has to be updated to h. */ 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) d(j) = 0.0;
continue; ss = ss / h;
d(j) = 0.0; cc = cc / h;
ss = ss / h; if (cc < 0.0) {
cc = cc / h; cc = -cc;
if (cc < 0.0) ss = -ss;
{ d(j - 1) = -h;
cc = -cc; } else
ss = -ss; d(j - 1) = h;
d(j - 1) = -h; xny = ss / (1.0 + cc);
}
else
d(j - 1) = h;
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;