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

Format

parent 57d5f735
Subproject commit f389aae203c4d92649cd5eb66289fd6a17c03fde
Subproject commit efa25a9976b8a6fc9f51d26924d4238d0d4820b1
......@@ -33,261 +33,225 @@
#ifdef PROFILE_EIQUADPROG
#define START_PROFILER_EIQUADPROG_RT(x) START_PROFILER(x)
#define STOP_PROFILER_EIQUADPROG_RT(x) STOP_PROFILER(x)
#define STOP_PROFILER_EIQUADPROG_RT(x) STOP_PROFILER(x)
#else
#define START_PROFILER_EIQUADPROG_RT(x)
#define STOP_PROFILER_EIQUADPROG_RT(x)
#endif
#define PROFILE_EIQUADPROG_CHOWLESKY_DECOMPOSITION "EIQUADPROG_RT Chowlesky dec"
#define PROFILE_EIQUADPROG_CHOWLESKY_INVERSE "EIQUADPROG_RT Chowlesky inv"
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR "EIQUADPROG_RT ADD_EQ_CONSTR"
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1 "EIQUADPROG_RT ADD_EQ_CONSTR_1"
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_2 "EIQUADPROG_RT ADD_EQ_CONSTR_2"
#define PROFILE_EIQUADPROG_STEP_1 "EIQUADPROG_RT STEP_1"
#define PROFILE_EIQUADPROG_STEP_1_1 "EIQUADPROG_RT STEP_1_1"
#define PROFILE_EIQUADPROG_STEP_1_2 "EIQUADPROG_RT STEP_1_2"
#define PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM "EIQUADPROG_RT STEP_1_UNCONSTR_MINIM"
#define PROFILE_EIQUADPROG_STEP_2 "EIQUADPROG_RT STEP_2"
#define PROFILE_EIQUADPROG_STEP_2A "EIQUADPROG_RT STEP_2A"
#define PROFILE_EIQUADPROG_STEP_2B "EIQUADPROG_RT STEP_2B"
#define PROFILE_EIQUADPROG_STEP_2C "EIQUADPROG_RT STEP_2C"
#define PROFILE_EIQUADPROG_CHOWLESKY_INVERSE "EIQUADPROG_RT Chowlesky inv"
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR "EIQUADPROG_RT ADD_EQ_CONSTR"
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1 "EIQUADPROG_RT ADD_EQ_CONSTR_1"
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_2 "EIQUADPROG_RT ADD_EQ_CONSTR_2"
#define PROFILE_EIQUADPROG_STEP_1 "EIQUADPROG_RT STEP_1"
#define PROFILE_EIQUADPROG_STEP_1_1 "EIQUADPROG_RT STEP_1_1"
#define PROFILE_EIQUADPROG_STEP_1_2 "EIQUADPROG_RT STEP_1_2"
#define PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM "EIQUADPROG_RT STEP_1_UNCONSTR_MINIM"
#define PROFILE_EIQUADPROG_STEP_2 "EIQUADPROG_RT STEP_2"
#define PROFILE_EIQUADPROG_STEP_2A "EIQUADPROG_RT STEP_2A"
#define PROFILE_EIQUADPROG_STEP_2B "EIQUADPROG_RT STEP_2B"
#define PROFILE_EIQUADPROG_STEP_2C "EIQUADPROG_RT STEP_2C"
#define DEFAULT_MAX_ITER 1000
template<int Rows, int Cols>
struct RtMatrixX
{
template <int Rows, int Cols>
struct RtMatrixX {
typedef Eigen::Matrix<double, Rows, Cols> d;
};
template<int Rows>
struct RtVectorX
{
template <int Rows>
struct RtVectorX {
typedef Eigen::Matrix<double, Rows, 1> d;
typedef Eigen::Matrix<int, Rows, 1> i;
typedef Eigen::Matrix<int, Rows, 1> i;
};
namespace eiquadprog
{
namespace solvers
{
/**
* Possible states of the solver.
*/
enum RtEiquadprog_status
{
RT_EIQUADPROG_OPTIMAL=0,
RT_EIQUADPROG_INFEASIBLE=1,
RT_EIQUADPROG_UNBOUNDED=2,
RT_EIQUADPROG_MAX_ITER_REACHED=3,
RT_EIQUADPROG_REDUNDANT_EQUALITIES=4
};
template<int nVars, int nEqCon, int nIneqCon>
class RtEiquadprog
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
RtEiquadprog();
virtual ~RtEiquadprog();
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).
*/
int 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 typename RtVectorX<nIneqCon+nEqCon>::d & 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 typename RtVectorX<nIneqCon+nEqCon>::i & getActiveSet() const
{
return A;
}
/**
* solves the problem
* min. x' Hess x + 2 g0' x
* s.t. CE x + ce0 = 0
* CI x + ci0 >= 0
*/
inline RtEiquadprog_status solve_quadprog(
const typename RtMatrixX<nVars,nVars>::d & Hess,
const typename RtVectorX<nVars>::d & g0,
const typename RtMatrixX<nEqCon, nVars>::d & CE,
const typename RtVectorX<nEqCon>::d & ce0,
const typename RtMatrixX<nIneqCon, nVars>::d & CI,
const typename RtVectorX<nIneqCon>::d & ci0,
typename RtVectorX<nVars>::d & x);
typename RtMatrixX<nVars,nVars>::d m_J; // J * J' = Hessian
bool is_inverse_provided_;
private:
int m_maxIter; /// max number of active-set iterations
double f_value; /// current value of cost function
Eigen::LLT<typename RtMatrixX<nVars,nVars>::d,Eigen::Lower> chol_;
double solver_return_;
/// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the matrix of active constraints
typename RtMatrixX<nVars,nVars>::d R;
/// CI*x+ci0
typename RtVectorX<nIneqCon>::d s;
/// infeasibility multipliers, i.e. negative step direction in dual space
typename RtVectorX<nIneqCon+nEqCon>::d r;
/// Lagrange multipliers
typename RtVectorX<nIneqCon+nEqCon>::d u;
/// step direction in primal space
typename RtVectorX<nVars>::d z;
/// J' np
typename RtVectorX<nVars>::d d;
/// current constraint normal
typename RtVectorX<nVars>::d np;
/// 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
typename RtVectorX<nIneqCon+nEqCon>::i A;
/// initialized as K \ A
/// iai(i)=-1 iff inequality constraint i is in the active set
/// iai(i)=i otherwise
typename RtVectorX<nIneqCon>::i iai;
/// 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
typename RtVectorX<nIneqCon>::i iaexcl;
typename RtVectorX<nVars>::d x_old; // old value of x
typename RtVectorX<nIneqCon+nEqCon>::d u_old; // old value of u
typename RtVectorX<nIneqCon+nEqCon>::i A_old; // old value of A
namespace eiquadprog {
namespace solvers {
/**
* Possible states of the solver.
*/
enum RtEiquadprog_status {
RT_EIQUADPROG_OPTIMAL = 0,
RT_EIQUADPROG_INFEASIBLE = 1,
RT_EIQUADPROG_UNBOUNDED = 2,
RT_EIQUADPROG_MAX_ITER_REACHED = 3,
RT_EIQUADPROG_REDUNDANT_EQUALITIES = 4
};
template <int nVars, int nEqCon, int nIneqCon>
class RtEiquadprog {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
RtEiquadprog();
virtual ~RtEiquadprog();
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).
*/
int 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 typename RtVectorX<nIneqCon + nEqCon>::d& 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 typename RtVectorX<nIneqCon + nEqCon>::i& getActiveSet() const { return A; }
/**
* solves the problem
* min. x' Hess x + 2 g0' x
* s.t. CE x + ce0 = 0
* CI x + ci0 >= 0
*/
inline RtEiquadprog_status solve_quadprog(const typename RtMatrixX<nVars, nVars>::d& Hess,
const typename RtVectorX<nVars>::d& g0,
const typename RtMatrixX<nEqCon, nVars>::d& CE,
const typename RtVectorX<nEqCon>::d& ce0,
const typename RtMatrixX<nIneqCon, nVars>::d& CI,
const typename RtVectorX<nIneqCon>::d& ci0,
typename RtVectorX<nVars>::d& x);
typename RtMatrixX<nVars, nVars>::d m_J; // J * J' = Hessian
bool is_inverse_provided_;
private:
int m_maxIter; /// max number of active-set iterations
double f_value; /// current value of cost function
Eigen::LLT<typename RtMatrixX<nVars, nVars>::d, Eigen::Lower> chol_;
double solver_return_;
/// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the matrix of active constraints
typename RtMatrixX<nVars, nVars>::d R;
/// CI*x+ci0
typename RtVectorX<nIneqCon>::d s;
/// infeasibility multipliers, i.e. negative step direction in dual space
typename RtVectorX<nIneqCon + nEqCon>::d r;
/// Lagrange multipliers
typename RtVectorX<nIneqCon + nEqCon>::d u;
/// step direction in primal space
typename RtVectorX<nVars>::d z;
/// J' np
typename RtVectorX<nVars>::d d;
/// current constraint normal
typename RtVectorX<nVars>::d np;
/// 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
typename RtVectorX<nIneqCon + nEqCon>::i A;
/// initialized as K \ A
/// iai(i)=-1 iff inequality constraint i is in the active set
/// iai(i)=i otherwise
typename RtVectorX<nIneqCon>::i iai;
/// 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
typename RtVectorX<nIneqCon>::i iaexcl;
typename RtVectorX<nVars>::d x_old; // old value of x
typename RtVectorX<nIneqCon + nEqCon>::d u_old; // old value of u
typename RtVectorX<nIneqCon + nEqCon>::i A_old; // old value of A
#ifdef OPTIMIZE_ADD_CONSTRAINT
typename RtVectorX<nVars>::d T1; // tmp vector
typename RtVectorX<nVars>::d T1; // tmp vector
#endif
/// size of the active set A (containing the indices of the active constraints)
int q;
/// number of active-set iterations
int iter;
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);
}
inline void compute_d(typename RtVectorX<nVars>::d & d,
const typename RtMatrixX<nVars,nVars>::d & J,
const typename RtVectorX<nVars>::d & np)
{
/// size of the active set A (containing the indices of the active constraints)
int q;
/// number of active-set iterations
int iter;
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);
}
inline void compute_d(typename RtVectorX<nVars>::d& d, const typename RtMatrixX<nVars, nVars>::d& J,
const typename RtVectorX<nVars>::d& 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(typename RtVectorX<nVars>::d & z,
const typename RtMatrixX<nVars,nVars>::d & J,
const typename RtVectorX<nVars>::d & d,
int iq)
{
inline void update_z(typename RtVectorX<nVars>::d& z, const typename RtMatrixX<nVars, nVars>::d& J,
const typename RtVectorX<nVars>::d& d, int iq) {
#ifdef OPTIMIZE_UPDATE_Z
z.noalias() = J.rightCols(nVars-iq) * d.tail(nVars-iq);
z.noalias() = J.rightCols(nVars - iq) * d.tail(nVars - 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 typename RtMatrixX<nVars,nVars>::d & R,
typename RtVectorX<nIneqCon+nEqCon>::d& r,
const typename RtVectorX<nVars>::d& d,
int iq)
{
r.head(iq)= d.head(iq);
R.topLeftCorner(iq,iq).template triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
}
inline bool add_constraint(
typename RtMatrixX<nVars,nVars>::d & R,
typename RtMatrixX<nVars,nVars>::d & J,
typename RtVectorX<nVars>::d & d,
int& iq, double& R_norm);
inline void delete_constraint(
typename RtMatrixX<nVars,nVars>::d& R,
typename RtMatrixX<nVars,nVars>::d& J,
typename RtVectorX<nIneqCon+nEqCon>::i & A,
typename RtVectorX<nIneqCon+nEqCon>::d & u,
int& iq, int l);
};
} /* namespace solvers */
}
inline void update_r(const typename RtMatrixX<nVars, nVars>::d& R, typename RtVectorX<nIneqCon + nEqCon>::d& r,
const typename RtVectorX<nVars>::d& d, int iq) {
r.head(iq) = d.head(iq);
R.topLeftCorner(iq, iq).template triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
}
inline bool add_constraint(typename RtMatrixX<nVars, nVars>::d& R, typename RtMatrixX<nVars, nVars>::d& J,
typename RtVectorX<nVars>::d& d, int& iq, double& R_norm);
inline void delete_constraint(typename RtMatrixX<nVars, nVars>::d& R, typename RtMatrixX<nVars, nVars>::d& J,
typename RtVectorX<nIneqCon + nEqCon>::i& A,
typename RtVectorX<nIneqCon + nEqCon>::d& u, int& iq, int l);
};
} /* namespace solvers */
} /* namespace eiquadprog */
/* --- Details -------------------------------------------------------------------- */
......
This diff is collapsed.
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment