18 #ifndef __invdyn_solvers_hqp_eiquadprog_rt_hxx__ 19 #define __invdyn_solvers_hqp_eiquadprog_rt_hxx__ 21 #include "tsid/solvers/solver-HQP-eiquadprog-rt.hpp" 22 #include "eiquadprog/eiquadprog-rt.hxx" 23 #include "tsid/utils/stop-watch.hpp" 24 #include "tsid/math/utils.hpp" 28 #define PROFILE_EIQUADPROG_PREPARATION "EiquadprogRT problem preparation" 29 #define PROFILE_EIQUADPROG_SOLUTION "EiquadprogRT problem solution" 36 template<
int nVars,
int nEqCon,
int nIneqCon>
37 SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::SolverHQuadProgRT(
const std::string & name)
39 , m_hessian_regularization(DEFAULT_HESSIAN_REGULARIZATION)
44 m_output.resize(nVars, nEqCon, 2*nIneqCon);
47 template<
int nVars,
int nEqCon,
int nIneqCon>
48 void SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::sendMsg(
const std::string & s)
50 std::cout<<
"[SolverHQuadProgRT."<<m_name<<
"] "<<s<<std::endl;
53 template<
int nVars,
int nEqCon,
int nIneqCon>
54 void SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::resize(
unsigned int n,
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>
66 const HQPOutput & SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::solve(
const HQPData & problemData)
76 if(problemData.size()>2)
78 assert(
false &&
"Solver not implemented for more than 2 hierarchical levels.");
82 unsigned int neq = 0, nin = 0;
83 const ConstraintLevel & cl0 = problemData[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();
130 resize(m_n, neq, nin);
132 if(problemData.size()>1)
134 const ConstraintLevel & cl1 = problemData[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();
146 EIGEN_MALLOC_NOT_ALLOWED
147 m_g.noalias() -= w*(constr->matrix().transpose()*constr->vector());
149 m_H.diagonal().noalias() += m_hessian_regularization*Vector::Ones(m_n);
169 typename RtVectorX<nVars>::d sol(m_n);
171 eisol::RtEiquadprog_status
172 status = m_solver.solve_quadprog(m_H, m_g,
184 if(status==eisol::RT_EIQUADPROG_OPTIMAL)
186 m_output.status = HQP_STATUS_OPTIMAL;
187 m_output.lambda = m_solver.getLagrangeMultipliers();
189 m_output.activeSet = m_solver.getActiveSet().segment(m_neq, m_solver.getActiveSetSize()-m_neq);
190 m_output.iterations = m_solver.getIteratios();
193 const Vector & x = m_output.x;
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)
225 m_output.status = HQP_STATUS_INFEASIBLE;
226 else if(status==eisol::RT_EIQUADPROG_MAX_ITER_REACHED)
227 m_output.status = HQP_STATUS_MAX_ITER_REACHED;
228 else if(status==eisol::RT_EIQUADPROG_REDUNDANT_EQUALITIES)
229 m_output.status = HQP_STATUS_ERROR;
234 template<
int nVars,
int nEqCon,
int nIneqCon>
235 double SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::getObjectiveValue()
237 return m_solver.getObjValue();
240 template<
int nVars,
int nEqCon,
int nIneqCon>
241 bool SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::setMaximumIterations(
unsigned int maxIter)
243 SolverHQPBase::setMaximumIterations(maxIter);
244 return m_solver.setMaxIter(maxIter);
250 #endif // ifndef __invdyn_solvers_hqp_eiquadprog_rt_hxx__
math::Vector Vector
Definition: task-motion.cpp:25
#define PROFILE_EIQUADPROG_SOLUTION
Definition: solver-HQP-eiquadprog-rt.hxx:29
Definition: solver-HQP-eiquadprog-rt.hxx:31
#define PROFILE_EIQUADPROG_PREPARATION
Definition: solver-HQP-eiquadprog-rt.hxx:28