18 #ifndef __invdyn_solvers_hqp_eiquadprog_rt_hxx__
19 #define __invdyn_solvers_hqp_eiquadprog_rt_hxx__
22 #include "eiquadprog/eiquadprog-rt.hxx"
26 namespace eisol=eiquadprog::solvers;
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();
130 resize(m_n, neq, nin);
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());
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)
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)
226 else if(status==eisol::RT_EIQUADPROG_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>
237 return m_solver.getObjValue();
240 template<
int nVars,
int nEqCon,
int nIneqCon>
244 return m_solver.setMaxIter(maxIter);
250 #endif // ifndef __invdyn_solvers_hqp_eiquadprog_rt_hxx__