Loading...
Searching...
No Matches
solver-HQP-eiquadprog-rt.hxx
Go to the documentation of this file.
1//
2// Copyright (c) 2017 CNRS
3//
4// This file is part of tsid
5// tsid is free software: you can redistribute it
6// and/or modify it under the terms of the GNU Lesser General Public
7// License as published by the Free Software Foundation, either version
8// 3 of the License, or (at your option) any later version.
9// tsid is distributed in the hope that it will be
10// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12// General Lesser Public License for more details. You should have
13// received a copy of the GNU Lesser General Public License along with
14// tsid If not, see
15// <http://www.gnu.org/licenses/>.
16//
17
18#ifndef __invdyn_solvers_hqp_eiquadprog_rt_hxx__
19#define __invdyn_solvers_hqp_eiquadprog_rt_hxx__
20
22#include "eiquadprog/eiquadprog-rt.hxx"
24#include "tsid/math/utils.hpp"
25
26namespace eisol=eiquadprog::solvers;
27
28#define PROFILE_EIQUADPROG_PREPARATION "EiquadprogRT problem preparation"
29#define PROFILE_EIQUADPROG_SOLUTION "EiquadprogRT problem solution"
30
31namespace tsid
32{
33 namespace solvers
34 {
35
36 template<int nVars, int nEqCon, int nIneqCon>
38 : SolverHQPBase(name)
39 , m_hessian_regularization(DEFAULT_HESSIAN_REGULARIZATION)
40 {
41 m_n = nVars;
42 m_neq = nEqCon;
43 m_nin = nIneqCon;
44 m_output.resize(nVars, nEqCon, 2*nIneqCon);
45 }
46
47 template<int nVars, int nEqCon, int nIneqCon>
49 {
50 std::cout<<"[SolverHQuadProgRT."<<m_name<<"] "<<s<<std::endl;
51 }
52
53 template<int nVars, int nEqCon, int nIneqCon>
55 unsigned int neq,
56 unsigned int nin)
57 {
58 assert(n==nVars);
59 assert(neq==nEqCon);
60 assert(nin==nIneqCon);
61 if ((n!=nVars) || (neq!=nEqCon) || (nin!=nIneqCon))
62 std::cerr << "[SolverHQuadProgRT] (n!=nVars) || (neq!=nEqCon) || (nin!=nIneqCon)" << std::endl;
63 }
64
65 template<int nVars, int nEqCon, int nIneqCon>
67 {
68 using namespace tsid::math;
69
70//#ifndef EIGEN_RUNTIME_NO_MALLOC
71 // Eigen::internal::set_is_malloc_allowed(false);
72//#endif
73
74 START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_PREPARATION);
75
76 if(problemData.size()>2)
77 {
78 assert(false && "Solver not implemented for more than 2 hierarchical levels.");
79 }
80
81 // Compute the constraint matrix sizes
82 unsigned int neq = 0, nin = 0;
83 const ConstraintLevel & cl0 = problemData[0];
84 if(cl0.size()>0)
85 {
86 const unsigned int n = cl0[0].second->cols();
87 for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
88 {
89 auto constr = it->second;
90 assert(n==constr->cols());
91 if(constr->isEquality())
92 neq += constr->rows();
93 else
94 nin += constr->rows();
95 }
96 // If necessary, resize the constraint matrices
97 resize(n, neq, nin);
98
99 int i_eq=0, i_in=0;
100 for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
101 {
102 auto constr = it->second;
103 if(constr->isEquality())
104 {
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();
108 }
109 else if(constr->isInequality())
110 {
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();
117 }
118 else if(constr->isBound())
119 {
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();
126 }
127 }
128 }
129 else
130 resize(m_n, neq, nin);
131
132 if(problemData.size()>1)
133 {
134 const ConstraintLevel & cl1 = problemData[1];
135 m_H.setZero();
136 m_g.setZero();
137 for(ConstraintLevel::const_iterator it=cl1.begin(); it!=cl1.end(); it++)
138 {
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");
143
145 m_H.noalias() += w*constr->matrix().transpose()*constr->matrix();
147 m_g.noalias() -= w*(constr->matrix().transpose()*constr->vector());
148 }
149 m_H.diagonal().noalias() += m_hessian_regularization*Vector::Ones(m_n);
150 }
151
152 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_PREPARATION);
153
154 // // eliminate equality constraints
155 // if(m_neq>0)
156 // {
157 // m_CE_lu.compute(m_CE);
158 // sendMsg("The rank of CD is "+toString(m_CE_lu.rank());
159 // const MatrixXd & Z = m_CE_lu.kernel();
160
161 // }
162
163 START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_SOLUTION);
164
165 // min 0.5 * x G x + g0 x
166 // s.t.
167 // CE x + ce0 = 0
168 // CI x + ci0 >= 0
169 typename RtVectorX<nVars>::d sol(m_n);
171 eisol::RtEiquadprog_status
172 status = m_solver.solve_quadprog(m_H, m_g,
173 m_CE, m_ce0,
174 m_CI, m_ci0,
175 sol);
176 STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_SOLUTION);
177
178 m_output.x = sol;
179
180//#ifndef EIGEN_RUNTIME_NO_MALLOC
181 // Eigen::internal::set_is_malloc_allowed(true);
182//#endif
183
184 if(status==eisol::RT_EIQUADPROG_OPTIMAL)
185 {
186 m_output.status = HQP_STATUS_OPTIMAL;
187 m_output.lambda = m_solver.getLagrangeMultipliers();
188 // m_output.activeSet = m_solver.getActiveSet().template tail< 2*nIneqCon >().head(m_solver.getActiveSetSize());
189 m_output.activeSet = m_solver.getActiveSet().segment(m_neq, m_solver.getActiveSetSize()-m_neq);
190 m_output.iterations = m_solver.getIteratios();
191
192#ifndef NDEBUG
193 const Vector & x = m_output.x;
194
195 if(cl0.size()>0)
196 {
197 for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
198 {
199 auto constr = it->second;
200 if(constr->checkConstraint(x)==false)
201 {
202 if(constr->isEquality())
203 {
204 sendMsg("Equality "+constr->name()+" violated: "+
205 toString((constr->matrix()*x-constr->vector()).norm()));
206 }
207 else if(constr->isInequality())
208 {
209 sendMsg("Inequality "+constr->name()+" violated: "+
210 toString((constr->matrix()*x-constr->lowerBound()).minCoeff())+"\n"+
211 toString((constr->upperBound()-constr->matrix()*x).minCoeff()));
212 }
213 else if(constr->isBound())
214 {
215 sendMsg("Bound "+constr->name()+" violated: "+
216 toString((x-constr->lowerBound()).minCoeff())+"\n"+
217 toString((constr->upperBound()-x).minCoeff()));
218 }
219 }
220 }
221 }
222#endif
223 }
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;
230
231 return m_output;
232 }
233
234 template<int nVars, int nEqCon, int nIneqCon>
236 {
237 return m_solver.getObjValue();
238 }
239
240 template<int nVars, int nEqCon, int nIneqCon>
242 {
244 return m_solver.setMaxIter(maxIter);
245 }
246
247 } // namespace solvers
248} // namespace tsid
249
250#endif // ifndef __invdyn_solvers_hqp_eiquadprog_rt_hxx__
Definition: solver-HQP-output.hpp:33
void resize(int nVars, int nEqCon, int nInCon)
Definition: solver-HQP-output.hpp:53
Abstract interface for a Quadratic Program (HQP) solver.
Definition: solver-HQP-base.hpp:37
virtual bool setMaximumIterations(unsigned int maxIter)
Definition: solver-HQP-base.cpp:41
HQPOutput m_output
Definition: solver-HQP-base.hpp:84
int m_n
number of inequality constraints
Definition: solver-HQP-eiquadprog-rt.hpp:84
double getObjectiveValue()
Definition: solver-HQP-eiquadprog-rt.hxx:235
const HQPOutput & solve(const HQPData &problemData)
Definition: solver-HQP-eiquadprog-rt.hxx:66
void resize(unsigned int n, unsigned int neq, unsigned int nin)
Definition: solver-HQP-eiquadprog-rt.hxx:54
void sendMsg(const std::string &s)
Definition: solver-HQP-eiquadprog-rt.hxx:48
int m_nin
number of equality constraints
Definition: solver-HQP-eiquadprog-rt.hpp:83
bool setMaximumIterations(unsigned int maxIter)
Definition: solver-HQP-eiquadprog-rt.hxx:241
SolverHQuadProgRT(const std::string &name)
Definition: solver-HQP-eiquadprog-rt.hxx:37
int m_neq
Definition: solver-HQP-eiquadprog-rt.hpp:82
math::Vector Vector
Definition: solver-HQP-eiquadprog-rt.hpp:41
#define EIGEN_MALLOC_ALLOWED
Definition: fwd.hpp:27
#define EIGEN_MALLOC_NOT_ALLOWED
Definition: fwd.hpp:28
Definition: constraint-base.hpp:27
pinocchio::container::aligned_vector< ConstraintLevel > HQPData
Definition: fwd.hpp:91
pinocchio::container::aligned_vector< aligned_pair< double, std::shared_ptr< math::ConstraintBase > > > ConstraintLevel
Definition: fwd.hpp:89
Definition: constraint-bound.hpp:27
std::string toString(const T &v)
Definition: utils.hpp:36
#define PROFILE_EIQUADPROG_SOLUTION
Definition: solver-HQP-eiquadprog-rt.hxx:29
#define PROFILE_EIQUADPROG_PREPARATION
Definition: solver-HQP-eiquadprog-rt.hxx:28
#define DEFAULT_HESSIAN_REGULARIZATION
Definition: fwd.hpp:27
HQP_STATUS_MAX_ITER_REACHED
Definition: fwd.hpp:60
HQP_STATUS_OPTIMAL
Definition: fwd.hpp:57
HQP_STATUS_INFEASIBLE
Definition: fwd.hpp:58