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 -------------------------------------------------------------------- */
......
This diff is collapsed.
This diff is collapsed.
...@@ -20,25 +20,24 @@ ...@@ -20,25 +20,24 @@
// ci0: m // ci0: m
// x: n // x: n
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
// min ||x||^2 // min ||x||^2
BOOST_AUTO_TEST_CASE ( test_unbiased ) BOOST_AUTO_TEST_CASE(test_unbiased) {
{ Eigen::MatrixXd Q(2, 2);
Eigen::MatrixXd Q(2,2);
Q.setZero(); Q.setZero();
Q(0,0) = 1.0; Q(0, 0) = 1.0;
Q(1,1) = 1.0; Q(1, 1) = 1.0;
Eigen::VectorXd C(2); Eigen::VectorXd C(2);
C.setZero(); C.setZero();
Eigen::MatrixXd Aeq(2,0); Eigen::MatrixXd Aeq(2, 0);
Eigen::VectorXd Beq(0); Eigen::VectorXd Beq(0);
Eigen::MatrixXd Aineq(2,0); Eigen::MatrixXd Aineq(2, 0);
Eigen::VectorXd Bineq(0); Eigen::VectorXd Bineq(0);
...@@ -53,29 +52,28 @@ BOOST_AUTO_TEST_CASE ( test_unbiased ) ...@@ -53,29 +52,28 @@ BOOST_AUTO_TEST_CASE ( test_unbiased )
double out = Eigen::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize); double out = Eigen::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize);
BOOST_CHECK_CLOSE(out,val,1e-6); BOOST_CHECK_CLOSE(out, val, 1e-6);
BOOST_CHECK(x.isApprox(solution)); BOOST_CHECK(x.isApprox(solution));
} }
// min ||x-x_0||^2, x_0 = (1 1)^T // min ||x-x_0||^2, x_0 = (1 1)^T
BOOST_AUTO_TEST_CASE ( test_biased ) BOOST_AUTO_TEST_CASE(test_biased) {
{ Eigen::MatrixXd Q(2, 2);
Eigen::MatrixXd Q(2,2);
Q.setZero(); Q.setZero();
Q(0,0) = 1.0; Q(0, 0) = 1.0;
Q(1,1) = 1.0; Q(1, 1) = 1.0;
Eigen::VectorXd C(2); Eigen::VectorXd C(2);
C(0) = -1.; C(0) = -1.;
C(1) = -1.; C(1) = -1.;
Eigen::MatrixXd Aeq(2,0); Eigen::MatrixXd Aeq(2, 0);
Eigen::VectorXd Beq(0); Eigen::VectorXd Beq(0);
Eigen::MatrixXd Aineq(2,0); Eigen::MatrixXd Aineq(2, 0);
Eigen::VectorXd Bineq(0); Eigen::VectorXd Bineq(0);
...@@ -91,7 +89,7 @@ BOOST_AUTO_TEST_CASE ( test_biased ) ...@@ -91,7 +89,7 @@ BOOST_AUTO_TEST_CASE ( test_biased )
double out = Eigen::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize); double out = Eigen::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize);
BOOST_CHECK_CLOSE(out,val,1e-6); BOOST_CHECK_CLOSE(out, val, 1e-6);
BOOST_CHECK(x.isApprox(solution)); BOOST_CHECK(x.isApprox(solution));
} }
...@@ -100,24 +98,23 @@ BOOST_AUTO_TEST_CASE ( test_biased ) ...@@ -100,24 +98,23 @@ BOOST_AUTO_TEST_CASE ( test_biased )
// s.t. // s.t.
// x[1] = 1 - x[0] // x[1] = 1 - x[0]
BOOST_AUTO_TEST_CASE ( test_equality_constraints ) BOOST_AUTO_TEST_CASE(test_equality_constraints) {
{ Eigen::MatrixXd Q(2, 2);
Eigen::MatrixXd Q(2,2);
Q.setZero(); Q.setZero();
Q(0,0) = 1.0; Q(0, 0) = 1.0;
Q(1,1) = 1.0; Q(1, 1) = 1.0;
Eigen::VectorXd C(2); Eigen::VectorXd C(2);
C.setZero(); C.setZero();
Eigen::MatrixXd Aeq(2,1); Eigen::MatrixXd Aeq(2, 1);
Aeq(0,0) = 1.; Aeq(0, 0) = 1.;
Aeq(1,0) = 1.; Aeq(1, 0) = 1.;
Eigen::VectorXd Beq(1); Eigen::VectorXd Beq(1);
Beq(0) = -1.; Beq(0) = -1.;
Eigen::MatrixXd Aineq(2,0); Eigen::MatrixXd Aineq(2, 0);
Eigen::VectorXd Bineq(0); Eigen::VectorXd Bineq(0);
...@@ -133,7 +130,7 @@ BOOST_AUTO_TEST_CASE ( test_equality_constraints ) ...@@ -133,7 +130,7 @@ BOOST_AUTO_TEST_CASE ( test_equality_constraints )
double out = Eigen::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize); double out = Eigen::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize);
BOOST_CHECK_CLOSE(out,val,1e-6); BOOST_CHECK_CLOSE(out, val, 1e-6);
BOOST_CHECK(x.isApprox(solution)); BOOST_CHECK(x.isApprox(solution));
} }
...@@ -142,24 +139,23 @@ BOOST_AUTO_TEST_CASE ( test_equality_constraints ) ...@@ -142,24 +139,23 @@ BOOST_AUTO_TEST_CASE ( test_equality_constraints )
// s.t. // s.t.
// x[i] >= 1 // x[i] >= 1
BOOST_AUTO_TEST_CASE ( test_inequality_constraints ) BOOST_AUTO_TEST_CASE(test_inequality_constraints) {
{ Eigen::MatrixXd Q(2, 2);
Eigen::MatrixXd Q(2,2);
Q.setZero(); Q.setZero();
Q(0,0) = 1.0; Q(0, 0) = 1.0;
Q(1,1) = 1.0; Q(1, 1) = 1.0;
Eigen::VectorXd C(2); Eigen::VectorXd C(2);
C.setZero(); C.setZero();
Eigen::MatrixXd Aeq(2,0); Eigen::MatrixXd Aeq(2, 0);
Eigen::VectorXd Beq(0); Eigen::VectorXd Beq(0);
Eigen::MatrixXd Aineq(2,2); Eigen::MatrixXd Aineq(2, 2);
Aineq.setZero(); Aineq.setZero();
Aineq(0,0) = 1.; Aineq(0, 0) = 1.;
Aineq(1,1) = 1.; Aineq(1, 1) = 1.;
Eigen::VectorXd Bineq(2); Eigen::VectorXd Bineq(2);
Bineq(0) = -1.; Bineq(0) = -1.;
...@@ -177,7 +173,7 @@ BOOST_AUTO_TEST_CASE ( test_inequality_constraints ) ...@@ -177,7 +173,7 @@ BOOST_AUTO_TEST_CASE ( test_inequality_constraints )