18 #ifndef __invdyn_solvers_hqp_eiquadprog_rt_hxx__ 19 #define __invdyn_solvers_hqp_eiquadprog_rt_hxx__ 22 #include "eiquadprog/eiquadprog-rt.hxx" 28 #define PROFILE_EIQUADPROG_PREPARATION "EiquadprogRT problem preparation" 29 #define PROFILE_EIQUADPROG_SOLUTION "EiquadprogRT problem solution" 36 template<
int nVars,
int nEqCon,
int nIneqCon>
47 template<
int nVars,
int nEqCon,
int nIneqCon>
50 std::cout<<
"[SolverHQuadProgRT."<<
m_name<<
"] "<<s<<std::endl;
53 template<
int nVars,
int nEqCon,
int nIneqCon>
60 assert(nin==nIneqCon);
61 if ((n!=nVars) || (neq!=nEqCon) || (nin!=nIneqCon))
62 std::cerr <<
"[SolverHQuadProgRT] (n!=nVars) || (neq!=nEqCon) || (nin!=nIneqCon)" << std::endl;
65 template<
int nVars,
int nEqCon,
int nIneqCon>
76 if(problemData.size()>2)
78 assert(
false &&
"Solver not implemented for more than 2 hierarchical levels.");
82 unsigned int neq = 0, nin = 0;
86 const unsigned int n = cl0[0].second->cols();
87 for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
89 auto constr = it->second;
90 assert(n==constr->cols());
91 if(constr->isEquality())
92 neq += constr->rows();
94 nin += constr->rows();
100 for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
102 auto constr = it->second;
103 if(constr->isEquality())
105 m_CE.middleRows(i_eq, constr->rows()) = constr->matrix();
106 m_ce0.segment(i_eq, constr->rows()) = -constr->vector();
107 i_eq += constr->rows();
109 else if(constr->isInequality())
111 m_CI.middleRows(i_in, constr->rows()) = constr->matrix();
112 m_ci0.segment(i_in, constr->rows()) = -constr->lowerBound();
113 i_in += constr->rows();
114 m_CI.middleRows(i_in, constr->rows()) = -constr->matrix();
115 m_ci0.segment(i_in, constr->rows()) = constr->upperBound();
116 i_in += constr->rows();
118 else if(constr->isBound())
120 m_CI.middleRows(i_in, constr->rows()).setIdentity();
121 m_ci0.segment(i_in, constr->rows()) = -constr->lowerBound();
122 i_in += constr->rows();
123 m_CI.middleRows(i_in, constr->rows()) = -Matrix::Identity(
m_n,
m_n);
124 m_ci0.segment(i_in, constr->rows()) = constr->upperBound();
125 i_in += constr->rows();
132 if(problemData.size()>1)
137 for(ConstraintLevel::const_iterator it=cl1.begin(); it!=cl1.end(); it++)
139 const double & w = it->first;
140 auto constr = it->second;
141 if(!constr->isEquality())
142 assert(
false &&
"Inequalities in the cost function are not implemented yet");
145 m_H.noalias() += w*constr->matrix().transpose()*constr->matrix();
147 m_g.noalias() -= w*(constr->matrix().transpose()*constr->vector());
169 typename RtVectorX<nVars>::d sol(
m_n);
171 eisol::RtEiquadprog_status
184 if(status==eisol::RT_EIQUADPROG_OPTIMAL)
197 for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
199 auto constr = it->second;
200 if(constr->checkConstraint(x)==
false)
202 if(constr->isEquality())
204 sendMsg(
"Equality "+constr->name()+
" violated: "+
205 toString((constr->matrix()*x-constr->vector()).norm()));
207 else if(constr->isInequality())
209 sendMsg(
"Inequality "+constr->name()+
" violated: "+
210 toString((constr->matrix()*x-constr->lowerBound()).minCoeff())+
"\n"+
211 toString((constr->upperBound()-constr->matrix()*x).minCoeff()));
213 else if(constr->isBound())
215 sendMsg(
"Bound "+constr->name()+
" violated: "+
216 toString((x-constr->lowerBound()).minCoeff())+
"\n"+
217 toString((constr->upperBound()-x).minCoeff()));
224 else if(status==eisol::RT_EIQUADPROG_UNBOUNDED)
226 else if(status==eisol::RT_EIQUADPROG_MAX_ITER_REACHED)
228 else if(status==eisol::RT_EIQUADPROG_REDUNDANT_EQUALITIES)
234 template<
int nVars,
int nEqCon,
int nIneqCon>
240 template<
int nVars,
int nEqCon,
int nIneqCon>
244 return m_solver.setMaxIter(maxIter);
250 #endif // ifndef __invdyn_solvers_hqp_eiquadprog_rt_hxx__ Vector x
solver status
Definition: solver-HQP-output.hpp:41
SolverHQuadProgRT(const std::string &name)
Definition: solver-HQP-eiquadprog-rt.hxx:37
eiquadprog::solvers::RtEiquadprog< nVars, nEqCon, 2 *nIneqCon > m_solver
Definition: solver-HQP-eiquadprog-rt.hpp:64
Definition: solver-HQP-output.hpp:32
HQP_STATUS_OPTIMAL
Definition: fwd.hpp:57
math::Vector Vector
Definition: solver-HQP-eiquadprog-rt.hpp:41
std::string toString(const T &v)
Definition: utils.hpp:36
const HQPOutput & solve(const HQPData &problemData)
Definition: solver-HQP-eiquadprog-rt.hxx:66
#define DEFAULT_HESSIAN_REGULARIZATION
Definition: fwd.hpp:27
RtMatrixX< nEqCon, nVars >::d m_CE
Definition: solver-HQP-eiquadprog-rt.hpp:68
#define EIGEN_MALLOC_ALLOWED
Definition: fwd.hpp:27
int m_neq
Definition: solver-HQP-eiquadprog-rt.hpp:82
Vector lambda
solution
Definition: solver-HQP-output.hpp:42
HQPOutput m_output
Definition: solver-HQP-base.hpp:84
void resize(unsigned int n, unsigned int neq, unsigned int nin)
Definition: solver-HQP-eiquadprog-rt.hxx:54
#define EIGEN_MALLOC_NOT_ALLOWED
Definition: fwd.hpp:28
void sendMsg(const std::string &s)
Definition: solver-HQP-eiquadprog-rt.hxx:48
RtVectorX< nVars >::d m_g
Definition: solver-HQP-eiquadprog-rt.hpp:67
int m_nin
number of equality constraints
Definition: solver-HQP-eiquadprog-rt.hpp:83
RtVectorX< nEqCon >::d m_ce0
Definition: solver-HQP-eiquadprog-rt.hpp:69
pinocchio::container::aligned_vector< ConstraintLevel > HQPData
Definition: fwd.hpp:91
int m_n
number of inequality constraints
Definition: solver-HQP-eiquadprog-rt.hpp:84
HQP_STATUS_MAX_ITER_REACHED
Definition: fwd.hpp:60
Definition: constraint-base.hpp:26
RtMatrixX< nVars, nVars >::d m_H
Definition: solver-HQP-eiquadprog-rt.hpp:66
pinocchio::container::aligned_vector< aligned_pair< double, std::shared_ptr< math::ConstraintBase > > > ConstraintLevel
Definition: fwd.hpp:89
void resize(int nVars, int nEqCon, int nInCon)
Definition: solver-HQP-output.hpp:53
#define PROFILE_EIQUADPROG_SOLUTION
Definition: solver-HQP-eiquadprog-rt.hxx:29
HQPStatus status
Definition: solver-HQP-output.hpp:40
virtual bool setMaximumIterations(unsigned int maxIter)
Definition: solver-HQP-base.cpp:41
double m_hessian_regularization
Definition: solver-HQP-eiquadprog-rt.hpp:74
bool setMaximumIterations(unsigned int maxIter)
Definition: solver-HQP-eiquadprog-rt.hxx:241
Definition: constraint-bound.hpp:26
VectorXi activeSet
Lagrange multipliers.
Definition: solver-HQP-output.hpp:43
int iterations
indexes of active inequalities
Definition: solver-HQP-output.hpp:44
RtMatrixX< 2 *nIneqCon, nVars >::d m_CI
Definition: solver-HQP-eiquadprog-rt.hpp:70
#define PROFILE_EIQUADPROG_PREPARATION
Definition: solver-HQP-eiquadprog-rt.hxx:28
double getObjectiveValue()
Definition: solver-HQP-eiquadprog-rt.hxx:235
RtVectorX< 2 *nIneqCon >::d m_ci0
twice the rows because inequality constraints are bilateral
Definition: solver-HQP-eiquadprog-rt.hpp:71
HQP_STATUS_INFEASIBLE
Definition: fwd.hpp:58
Abstract interface for a Quadratic Program (HQP) solver.
Definition: solver-HQP-base.hpp:36
std::string m_name
Definition: solver-HQP-base.hpp:80