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

[Format]

parent fb6bbcd5
......@@ -7,7 +7,7 @@
#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_UPDATE_Z
#define OPTIMIZE_HESSIAN_INVERSE
......@@ -21,227 +21,197 @@
#ifdef PROFILE_EIQUADPROG
#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
#define START_PROFILER_EIQUADPROG_FAST(x)
#define STOP_PROFILER_EIQUADPROG_FAST(x)
#endif
#define EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION "EIQUADPROG_FAST Cholesky dec"
#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_1 "EIQUADPROG_FAST ADD_EQ_CONSTR_1"
#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_1 "EIQUADPROG_FAST STEP_1_1"
#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_2 "EIQUADPROG_FAST STEP_2"
#define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
#define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
#define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C"
#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_1 "EIQUADPROG_FAST ADD_EQ_CONSTR_1"
#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_1 "EIQUADPROG_FAST STEP_1_1"
#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_2 "EIQUADPROG_FAST STEP_2"
#define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
#define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
#define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C"
#define DEFAULT_MAX_ITER 1000
namespace eiquadprog
{
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
{
typedef Eigen::MatrixXd MatrixXd;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::VectorXi VectorXi;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EiquadprogFast();
virtual ~EiquadprogFast();
void reset(size_t dim_qp, size_t num_eq, size_t num_ineq);
int getMaxIter() const { return m_maxIter; }
bool setMaxIter(int maxIter)
{
if(maxIter<0)
return false;
m_maxIter = maxIter;
return true;
}
/**
* @return The size of the active set, namely the number of
* active constraints (including the equalities).
*/
size_t getActiveSetSize() const { return q; }
/**
* @return The number of active-set iteratios.
*/
int getIteratios() const { return iter; }
/**
* @return The value of the objective function.
*/
double getObjValue() const { return f_value; }
/**
* @return The Lagrange multipliers
*/
const VectorXd & getLagrangeMultipliers() const { return u; }
/**
* Return the active set, namely the indeces of active constraints.
* 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
* is the size of the active set.
* @return The set of indexes of the active constraints.
*/
const VectorXi & getActiveSet() const { return A; }
/**
* solves the problem
* min. x' Hess x + 2 g0' x
* 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);
MatrixXd m_J; // J * J' = Hessian <nVars,nVars>::d
bool is_inverse_provided_;
private:
size_t m_nVars;
size_t m_nEqCon;
size_t m_nIneqCon;
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
/// 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
/// CI*x+ci0
VectorXd s; // <nIneqCon>::d
/// infeasibility multipliers, i.e. negative step direction in dual space
VectorXd r; // <nIneqCon+nEqCon>::d
/// Lagrange multipliers
VectorXd u; // <nIneqCon+nEqCon>::d
/// step direction in primal space
VectorXd z; // <nVars>::d
/// J' np
VectorXd d; //<nVars>::d
/// current constraint normal
VectorXd np; //<nVars>::d
/// active set (indeces of active constraints)
/// the first nEqCon indeces are for the equalities and are negative
/// the last nIneqCon indeces are for the inequalities are start from 0
VectorXi A; // <nIneqCon+nEqCon>
/// initialized as K \ A
/// iai(i)=-1 iff inequality constraint i is in the active set
/// iai(i)=i otherwise
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
namespace eiquadprog {
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 {
typedef Eigen::MatrixXd MatrixXd;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::VectorXi VectorXi;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EiquadprogFast();
virtual ~EiquadprogFast();
void reset(size_t dim_qp, size_t num_eq, size_t num_ineq);
int getMaxIter() const { return m_maxIter; }
bool setMaxIter(int maxIter) {
if (maxIter < 0) return false;
m_maxIter = maxIter;
return true;
}
/**
* @return The size of the active set, namely the number of
* active constraints (including the equalities).
*/
size_t getActiveSetSize() const { return q; }
/**
* @return The number of active-set iteratios.
*/
int getIteratios() const { return iter; }
/**
* @return The value of the objective function.
*/
double getObjValue() const { return f_value; }
/**
* @return The Lagrange multipliers
*/
const VectorXd& getLagrangeMultipliers() const { return u; }
/**
* Return the active set, namely the indeces of active constraints.
* 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
* is the size of the active set.
* @return The set of indexes of the active constraints.
*/
const VectorXi& getActiveSet() const { return A; }
/**
* solves the problem
* min. x' Hess x + 2 g0' x
* 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);
MatrixXd m_J; // J * J' = Hessian <nVars,nVars>::d
bool is_inverse_provided_;
private:
size_t m_nVars;
size_t m_nEqCon;
size_t m_nIneqCon;
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
/// 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
/// CI*x+ci0
VectorXd s; // <nIneqCon>::d
/// infeasibility multipliers, i.e. negative step direction in dual space
VectorXd r; // <nIneqCon+nEqCon>::d
/// Lagrange multipliers
VectorXd u; // <nIneqCon+nEqCon>::d
/// step direction in primal space
VectorXd z; // <nVars>::d
/// J' np
VectorXd d; //<nVars>::d
/// current constraint normal
VectorXd np; //<nVars>::d
/// active set (indeces of active constraints)
/// the first nEqCon indeces are for the equalities and are negative
/// the last nIneqCon indeces are for the inequalities are start from 0
VectorXi A; // <nIneqCon+nEqCon>
/// initialized as K \ A
/// iai(i)=-1 iff inequality constraint i is in the active set
/// iai(i)=i otherwise
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
VectorXd T1; /// tmp variable used in add_constraint
VectorXd T1; /// tmp variable used in add_constraint
#endif
/// size of the active set A (containing the indices of the active constraints)
size_t q;
/// size of the active set A (containing the indices of the active constraints)
size_t q;
/// number of active-set iterations
int iter;
/// 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;
d.noalias() = J.adjoint() * np;
#else
d = J.adjoint() * np;
d = J.adjoint() * np;
#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 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,
size_t l);
};
} /* namespace solvers */
}
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 void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_t nEqCon, size_t& iq,
size_t l);
};
} /* namespace solvers */
} /* namespace eiquadprog */
/* --- Details -------------------------------------------------------------------- */
......
This diff is collapsed.
This diff is collapsed.
......@@ -20,25 +20,24 @@
// ci0: m
// x: n
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
// min ||x||^2
BOOST_AUTO_TEST_CASE ( test_unbiased )
{
Eigen::MatrixXd Q(2,2);
BOOST_AUTO_TEST_CASE(test_unbiased) {
Eigen::MatrixXd Q(2, 2);
Q.setZero();
Q(0,0) = 1.0;
Q(1,1) = 1.0;
Q(0, 0) = 1.0;
Q(1, 1) = 1.0;
Eigen::VectorXd C(2);
C.setZero();
Eigen::MatrixXd Aeq(2,0);
Eigen::MatrixXd Aeq(2, 0);
Eigen::VectorXd Beq(0);
Eigen::MatrixXd Aineq(2,0);
Eigen::MatrixXd Aineq(2, 0);
Eigen::VectorXd Bineq(0);
......@@ -53,29 +52,28 @@ BOOST_AUTO_TEST_CASE ( test_unbiased )
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));
}
// min ||x-x_0||^2, x_0 = (1 1)^T
BOOST_AUTO_TEST_CASE ( test_biased )
{
Eigen::MatrixXd Q(2,2);
BOOST_AUTO_TEST_CASE(test_biased) {
Eigen::MatrixXd Q(2, 2);
Q.setZero();
Q(0,0) = 1.0;
Q(1,1) = 1.0;
Q(0, 0) = 1.0;
Q(1, 1) = 1.0;
Eigen::VectorXd C(2);
C(0) = -1.;
C(1) = -1.;
Eigen::MatrixXd Aeq(2,0);
Eigen::MatrixXd Aeq(2, 0);
Eigen::VectorXd Beq(0);
Eigen::MatrixXd Aineq(2,0);
Eigen::MatrixXd Aineq(2, 0);
Eigen::VectorXd Bineq(0);
......@@ -91,7 +89,7 @@ BOOST_AUTO_TEST_CASE ( test_biased )
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));
}
......@@ -100,24 +98,23 @@ BOOST_AUTO_TEST_CASE ( test_biased )
// s.t.
// x[1] = 1 - x[0]
BOOST_AUTO_TEST_CASE ( test_equality_constraints )
{
Eigen::MatrixXd Q(2,2);
BOOST_AUTO_TEST_CASE(test_equality_constraints) {
Eigen::MatrixXd Q(2, 2);
Q.setZero();
Q(0,0) = 1.0;
Q(1,1) = 1.0;
Q(0, 0) = 1.0;
Q(1, 1) = 1.0;
Eigen::VectorXd C(2);
C.setZero();
Eigen::MatrixXd Aeq(2,1);
Aeq(0,0) = 1.;
Aeq(1,0) = 1.;
Eigen::MatrixXd Aeq(2, 1);
Aeq(0, 0) = 1.;
Aeq(1, 0) = 1.;
Eigen::VectorXd Beq(1);
Beq(0) = -1.;
Eigen::MatrixXd Aineq(2,0);
Eigen::MatrixXd Aineq(2, 0);
Eigen::VectorXd Bineq(0);
......@@ -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);
BOOST_CHECK_CLOSE(out,val,1e-6);
BOOST_CHECK_CLOSE(out, val, 1e-6);
BOOST_CHECK(x.isApprox(solution));
}
......@@ -142,24 +139,23 @@ BOOST_AUTO_TEST_CASE ( test_equality_constraints )
// s.t.
// x[i] >= 1
BOOST_AUTO_TEST_CASE ( test_inequality_constraints )
{
Eigen::MatrixXd Q(2,2);
BOOST_AUTO_TEST_CASE(test_inequality_constraints) {
Eigen::MatrixXd Q(2, 2);
Q.setZero();
Q(0,0) = 1.0;
Q(1,1) = 1.0;
Q(0, 0) = 1.0;
Q(1, 1) = 1.0;
Eigen::VectorXd C(2);
C.setZero();
Eigen::MatrixXd Aeq(2,0);
Eigen::MatrixXd Aeq(2, 0);
Eigen::VectorXd Beq(0);
Eigen::MatrixXd Aineq(2,2);
Eigen::MatrixXd Aineq(2, 2);
Aineq.setZero();
Aineq(0,0) = 1.;
Aineq(1,1) = 1.;
Aineq(0, 0) = 1.;
Aineq(1, 1) = 1.;
Eigen::VectorXd Bineq(2);
Bineq(0) = -1.;
......@@ -177,7 +173,7 @@ BOOST_AUTO_TEST_CASE ( test_inequality_constraints )
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));
}
......@@ -187,27 +183,26 @@ BOOST_AUTO_TEST_CASE ( test_inequality_constraints )
// x[1] = 5 - x[0]
// x[1] >= 3
BOOST_AUTO_TEST_CASE ( test_full )
{
Eigen::MatrixXd Q(2,2);
BOOST_AUTO_TEST_CASE(test_full) {
Eigen::MatrixXd Q(2, 2);
Q.setZero();
Q(0,0) = 1.0;
Q(1,1) = 1.0;
Q(0, 0) = 1.0;
Q(1, 1) = 1.0;
Eigen::VectorXd C(2);
C(0) = -1.;
C(1) = -1.;
Eigen::MatrixXd Aeq(2,1);
Aeq(0,0) = 1.;
Aeq(1,0) = 1.;
Eigen::MatrixXd Aeq(2, 1);
Aeq(0, 0) = 1.;
Aeq(1, 0) = 1.;
Eigen::VectorXd Beq(1);
Beq(0) = -5.;
Eigen::MatrixXd Aineq(2,1);
Eigen::MatrixXd Aineq(2, 1);
Aineq.setZero();
Aineq(1,0) = 1.;
Aineq(1, 0) = 1.;
Eigen::VectorXd Bineq(1);
Bineq(0) = -3.;
......@@ -224,7 +219,7 @@ BOOST_AUTO_TEST_CASE ( test_full )
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));
}
...