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 -------------------------------------------------------------------- */
......
......@@ -5,695 +5,619 @@
#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)
{
Scalar a1, b1, t;
a1 = std::abs(a);
b1 = std::abs(b);
if (a1 > b1)
{
t = (b1 / a1);
return a1 * std::sqrt(1.0 + t * t);
}
else
if (b1 > a1)
{
t = (a1 / b1);
return b1 * std::sqrt(1.0 + t * t);
}
return a1 * std::sqrt(2.0);
}
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() {}
void EiquadprogFast::reset(size_t nVars,
size_t nEqCon,
size_t nIneqCon)
{
m_nVars = nVars;
m_nEqCon = nEqCon;
m_nIneqCon = nIneqCon;
m_J.setZero(nVars, nVars);
chol_.compute(m_J);
R.resize(nVars, nVars);
s.resize(nIneqCon);
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);
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) {
t = (b1 / a1);
return a1 * std::sqrt(1.0 + t * t);
} else if (b1 > a1) {
t = (a1 / b1);
return b1 * std::sqrt(1.0 + t * t);
}
return a1 * std::sqrt(2.0);
}
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() {}
void EiquadprogFast::reset(size_t nVars, size_t nEqCon, size_t nIneqCon) {
m_nVars = nVars;
m_nEqCon = nEqCon;
m_nIneqCon = nIneqCon;
m_J.setZero(nVars, nVars);
chol_.compute(m_J);
R.resize(nVars, nVars);
s.resize(nIneqCon);
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
T1.resize(nVars);
T1.resize(nVars);
#endif
}
}
bool EiquadprogFast::add_constraint(MatrixXd & R,
MatrixXd & J,
VectorXd & d,
size_t& iq,
double& R_norm)
{
size_t nVars = J.rows();
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 << '/';
std::cerr << "Add constraint " << iq << '/';
#endif
size_t j, k;
double cc, ss, h, t1, t2, xny;
size_t j, k;
double cc, ss, h, t1, t2, xny;
#ifdef OPTIMIZE_ADD_CONSTRAINT
Eigen::Vector2d cc_ss;
Eigen::Vector2d cc_ss;
#endif
/* we have to find the Givens rotation which will reduce the element
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--)
{
/* 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.
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
update d depending on the sign of gs.
Otherwise we have to apply the Givens rotation to these columns.
The i - 1 element of d has to be updated to h. */
cc = d(j - 1);
ss = d(j);
h = distance(cc, ss);
if (h == 0.0)
continue;
d(j) = 0.0;
ss = ss / h;
cc = cc / h;
if (cc < 0.0)
{
cc = -cc;
ss = -ss;
d(j - 1) = -h;
}
else
d(j - 1) = h;
xny = ss / (1.0 + cc);
/* we have to find the Givens rotation which will reduce the element
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--) {
/* 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.
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
update d depending on the sign of gs.
Otherwise we have to apply the Givens rotation to these columns.
The i - 1 element of d has to be updated to h. */
cc = d(j - 1);
ss = d(j);
h = distance(cc, ss);
if (h == 0.0) continue;
d(j) = 0.0;
ss = ss / h;
cc = cc / h;
if (cc < 0.0) {
cc = -cc;
ss = -ss;
d(j - 1) = -h;
} else
d(j - 1) = h;
xny = ss / (1.0 + cc);
// #define OPTIMIZE_ADD_CONSTRAINT