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 
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"
25 
26 namespace eisol=eiquadprog::solvers;
27 
28 #define PROFILE_EIQUADPROG_PREPARATION "EiquadprogRT problem preparation"
29 #define PROFILE_EIQUADPROG_SOLUTION "EiquadprogRT problem solution"
30 
31 namespace tsid
32 {
33  namespace solvers
34  {
35 
36  template<int nVars, int nEqCon, int nIneqCon>
37  SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::SolverHQuadProgRT(const std::string & name)
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>
48  void SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::sendMsg(const std::string & s)
49  {
50  std::cout<<"[SolverHQuadProgRT."<<m_name<<"] "<<s<<std::endl;
51  }
52 
53  template<int nVars, int nEqCon, int nIneqCon>
54  void SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::resize(unsigned int n,
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>
66  const HQPOutput & SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::solve(const HQPData & problemData)
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 
144  EIGEN_MALLOC_ALLOWED
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());
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);
170  EIGEN_MALLOC_ALLOWED
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>
235  double SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::getObjectiveValue()
236  {
237  return m_solver.getObjValue();
238  }
239 
240  template<int nVars, int nEqCon, int nIneqCon>
241  bool SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::setMaximumIterations(unsigned int maxIter)
242  {
243  SolverHQPBase::setMaximumIterations(maxIter);
244  return m_solver.setMaxIter(maxIter);
245  }
246 
247  } // namespace solvers
248 } // namespace tsid
249 
250 #endif // ifndef __invdyn_solvers_hqp_eiquadprog_rt_hxx__
math::Vector Vector
Definition: task-motion.cpp:25
Definition: utils.cpp:22
#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