ddpsolver.hh
Go to the documentation of this file.
1 #ifndef DDPSOLVER_H
2 #define DDPSOLVER_H
3 
4 #include "dynamicmodel.hh"
5 #include "costfunction.hh"
6 
7 #include <Eigen/Dense>
8 #include <Eigen/StdVector>
9 #include <qpOASES.hpp>
10 #include <qpOASES/QProblemB.hpp>
11 #include <time.h>
12 #include <sys/time.h>
13 #include <iostream>
14 
15 #define ENABLE_QPBOX 1
16 #define DISABLE_QPBOX 0
17 #define ENABLE_FULLDDP 1
18 #define DISABLE_FULLDDP 0
19 
20 USING_NAMESPACE_QPOASES
21 
22 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::MatrixXd)
23 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)
24 
25 template<typename precision, int stateSize, int commandSize>
26 class DDPSolver
27 {
28  public:
29  typedef Eigen::Matrix<precision, stateSize, 1> stateVec_t; // 1 x stateSize
30  typedef Eigen::Matrix<precision, 1, stateSize> stateVecTrans_t; // 1 x stateSize
31  typedef Eigen::Matrix<precision, stateSize, stateSize> stateMat_t; // stateSize x stateSize
32 
33  // typedef for commandSize types
34  typedef Eigen::Matrix<precision, commandSize, 1> commandVec_t; // commandSize x 1
35  typedef Eigen::Matrix<precision, 1, commandSize> commandVecTrans_t; // 1 x commandSize
36  typedef Eigen::Matrix<precision, commandSize, commandSize> commandMat_t; // commandSize x commandSize
37 
38  // typedef for mixed stateSize and commandSize types
39  typedef Eigen::Matrix<precision, stateSize, commandSize> stateR_commandC_t; // stateSize x commandSize
40  typedef Eigen::Matrix<precision, stateSize, commandSize> stateR_commandC_stateD_t[stateSize]; // stateSize x commandSize x stateSize
41  typedef Eigen::Matrix<precision, stateSize, commandSize> stateR_commandC_commandD_t[commandSize]; // stateSize x commandSize x commandSize
42  typedef Eigen::Matrix<precision, commandSize, stateSize> commandR_stateC_t; // commandSize x stateSize
43  typedef Eigen::Matrix<precision, commandSize, stateSize> commandR_stateC_stateD_t[stateSize]; // commandSize x stateSize x stateSize
44  typedef Eigen::Matrix<precision, commandSize, stateSize> commandR_stateC_commandD_t[commandSize]; // commandSize x stateSize x commandSize
45  typedef Eigen::Matrix<precision, stateSize, stateSize> stateR_stateC_commandD_t[commandSize]; // stateSize x stateSize x commandSize
46  typedef Eigen::Matrix<precision, commandSize, commandSize> commandR_commandC_stateD_t[stateSize]; // commandSize x commandSize x stateSize
47 
48  typedef std::vector<stateVec_t> stateVecTab_t;
49  typedef std::vector<commandVec_t> commandVecTab_t;
50  typedef std::vector<commandR_stateC_t> commandR_stateC_tab_t;
51 
54 
55  public:
56  struct traj
57  {
60  unsigned int iter;
61  };
62 
63  public:
64  private:
65  protected:
66  // attributes //
67  public:
68  private:
69  DynamicModel_t * dynamicModel;
70  CostFunction_t * costFunction;
71  unsigned int stateNb;
72  unsigned int commandNb;
73  stateVec_t x;
74  commandVec_t u;
75  stateVec_t xInit;
76  stateVec_t xDes;
77  unsigned int T;
78  unsigned int iter;
79  double dt;
80  unsigned int iterMax;
81  double stopCrit;
82  double changeAmount;
83  double cost, previous_cost;
84  commandVec_t zeroCommand;
85 
86  stateVecTab_t xList;
87  commandVecTab_t uList;
88  stateVecTab_t updatedxList;
89  commandVecTab_t updateduList;
90  stateVecTab_t tmpxPtr;
91  commandVecTab_t tmpuPtr;
92  struct traj lastTraj;
93 
94  stateVec_t nextVx;
95  stateMat_t nextVxx;
96  stateVec_t Qx;
97  stateMat_t Qxx;
98  commandVec_t Qu;
99  commandMat_t Quu, Quu_reg;
100  Eigen::LLT<commandMat_t> lltofQuu;
101  commandMat_t QuuInv;
102  commandR_stateC_t Qux, Qux_reg;
103  commandVec_t k;
105  commandVecTab_t kList;
106  commandR_stateC_tab_t KList;
107  double alphaList[5];
108  double alpha;
109 
110  double mu;
111  stateMat_t muEye;
112  unsigned char completeBackwardFlag;
113 
114  /* QP variables */
115  QProblemB* qp;
116  bool enableQPBox;
117  bool enableFullDDP;
118  commandMat_t H;
119  commandVec_t g;
120  commandVec_t lowerCommandBounds;
121  commandVec_t upperCommandBounds;
122  commandVec_t lb;
123  commandVec_t ub;
124  int nWSR;
125  real_t* xOpt;
126  protected:
127  // methods //
128  public:
129  DDPSolver(DynamicModel_t& myDynamicModel, CostFunction_t& myCostFunction,
130  bool fullDDP = 0, bool QPBox = 0)
131  {
132  dynamicModel = &myDynamicModel;
133  costFunction = &myCostFunction;
134  stateNb = myDynamicModel.getStateNb();
135  commandNb = myDynamicModel.getCommandNb();
136  enableQPBox = QPBox;
137  enableFullDDP = fullDDP;
138  zeroCommand.setZero();
139  cost = 0;
140  previous_cost = 0;
141  if (QPBox)
142  {
143  qp = new QProblemB(commandNb);
144  Options myOptions;
145  myOptions.printLevel = PL_LOW;
146  myOptions.enableRegularisation = BT_TRUE;
147  myOptions.initialStatusBounds = ST_INACTIVE;
148  myOptions.numRefinementSteps = 1;
149  myOptions.enableCholeskyRefactorisation = 1;
150  qp->setOptions(myOptions);
151 
152  xOpt = new real_t[commandNb];
153  lowerCommandBounds = myDynamicModel.getLowerCommandBounds();
154  upperCommandBounds = myDynamicModel.getUpperCommandBounds();
155  }
156  }
157 
158  void FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes,
159  unsigned int& myT, double& mydt, unsigned int& myiterMax,
160  double& mystopCrit)
161  {
162  xInit = myxInit;
163  xDes = myxDes;
164  T = myT;
165  dt = mydt;
166  iterMax = myiterMax;
167  stopCrit = mystopCrit;
168 
169  xList.resize(myT + 1);
170  uList.resize(myT);
171  updatedxList.resize(myT + 1);
172  updateduList.resize(myT);
173  tmpxPtr.resize(myT + 1);
174  tmpuPtr.resize(myT);
175  k.setZero();
176  K.setZero();
177  kList.resize(myT);
178  KList.resize(myT);
179 
180  alphaList[0] = 1.0;
181  alphaList[1] = 0.8;
182  alphaList[2] = 0.6;
183  alphaList[3] = 0.4;
184  alphaList[4] = 0.2;
185  alpha = 1.0;
186  }
187 
188  void initSolver(stateVec_t& myxInit, stateVec_t& myxDes)
189  {
190  xInit = myxInit;
191  xDes = myxDes;
192  }
193 
195  {
196  initTrajectory();
197  for (iter = 1; iter < iterMax; iter++)
198  {
199  backwardLoop();
200  forwardLoop();
201  if (changeAmount < stopCrit)
202  {
203  break;
204  }
205  tmpxPtr = xList;
206  tmpuPtr = uList;
207  xList = updatedxList;
208  updatedxList = tmpxPtr;
209  uList = updateduList;
210  updateduList = tmpuPtr;
211  }
212  return updateduList[0];
213  }
214 
216  {
217  xList[0] = xInit;
218  for (unsigned int i = 0; i < T; i++)
219  {
220  uList[i] = zeroCommand;
221  xList[i + 1] = dynamicModel->computeNextState(dt, xList[i],
222  zeroCommand);
223  }
224  }
225 
227  {
228  costFunction->computeFinalCostAndDeriv(xList[T], xDes);
229  cost = costFunction->getFinalCost();
230  nextVx = costFunction->getlx();
231  nextVxx = costFunction->getlxx();
232 
233  mu = 0.0;
234  completeBackwardFlag = 0;
235 
236  while (!completeBackwardFlag)
237  {
238  completeBackwardFlag = 1;
239  muEye = stateMat_t::Constant(mu);
240  for (int i = T - 1; i >= 0; i--)
241  {
242  x = xList[i];
243  u = uList[i];
244 
245  dynamicModel->computeModelDeriv(dt, x, u);
246  costFunction->computeCostAndDeriv(x, xDes, u);
247  cost += costFunction->getRunningCost();
248 
249  Qx = costFunction->getlx()
250  + dynamicModel->getfx().transpose() * nextVx;
251  Qu = costFunction->getlu()
252  + dynamicModel->getfu().transpose() * nextVx;
253  Qxx = costFunction->getlxx()
254  + dynamicModel->getfx().transpose() * (nextVxx)
255  * dynamicModel->getfx();
256  Quu = costFunction->getluu()
257  + dynamicModel->getfu().transpose() * (nextVxx)
258  * dynamicModel->getfu();
259  Qux = costFunction->getlux()
260  + dynamicModel->getfu().transpose() * (nextVxx)
261  * dynamicModel->getfx();
262  Quu_reg = costFunction->getluu()
263  + dynamicModel->getfu().transpose() * (nextVxx + muEye)
264  * dynamicModel->getfu();
265  Qux_reg = costFunction->getlux()
266  + dynamicModel->getfu().transpose() * (nextVxx + muEye)
267  * dynamicModel->getfx();
268 
269  if (enableFullDDP)
270  {
271  Qxx += dynamicModel->computeTensorContxx(nextVx);
272  Qux += dynamicModel->computeTensorContux(nextVx);
273  Quu += dynamicModel->computeTensorContuu(nextVx);
274  Qux_reg += dynamicModel->computeTensorContux(nextVx);
275  Quu_reg += dynamicModel->computeTensorContuu(nextVx);
276  }
277 
278  if (!isQuudefinitePositive(Quu_reg))
279  {
280 
281  std::cout << "regularization" << std::endl; // to remove
282  if (mu == 0.0)
283  mu += 1e-4;
284  else
285  mu *= 10;
286  completeBackwardFlag = 0;
287  break;
288  }
289 
290  QuuInv = Quu.inverse();
291 
292  if (enableQPBox)
293  {
294  nWSR = 10;
295  H = Quu_reg;
296  g = Qu;
297  lb = lowerCommandBounds - u;
298  ub = upperCommandBounds - u;
299  qp->init(H.data(), g.data(), lb.data(), ub.data(), nWSR);
300  qp->getPrimalSolution(xOpt);
301  k = Eigen::Map<commandVec_t>(xOpt);
302  K = -QuuInv * Qux;
303  for (unsigned int i_cmd = 0; i_cmd < commandNb; i_cmd++)
304  {
305  if ((k[i_cmd] == lowerCommandBounds[i_cmd])
306  | (k[i_cmd] == upperCommandBounds[i_cmd]))
307  {
308  K.row(i_cmd).setZero();
309  }
310  }
311  }
312  else
313  {
314  k = -QuuInv * Qu;
315  K = -QuuInv * Qux;
316  }
317 
318  /*nextVx = Qx - K.transpose()*Quu*k;
319  nextVxx = Qxx - K.transpose()*Quu*K;*/
320  nextVx = Qx + K.transpose() * Quu * k + K.transpose() * Qu
321  + Qux.transpose() * k;
322  nextVxx = Qxx + K.transpose() * Quu * K + K.transpose() * Qux
323  + Qux.transpose() * K;
324  nextVxx = 0.5 * (nextVxx + nextVxx.transpose());
325 
326  kList[i] = k;
327  KList[i] = K;
328  }
329  }
330  }
331 
332  void forwardLoop()
333  {
334  changeAmount = 0.0;
335  updatedxList[0] = xInit;
336  // Line search to be implemented
337  alpha = 1.0;
338  for (unsigned int i = 0; i < T; i++)
339  {
340  updateduList[i] = uList[i] + alpha * kList[i]
341  + KList[i] * (updatedxList[i] - xList[i]);
342  updatedxList[i + 1] = dynamicModel->computeNextState(dt,
343  updatedxList[i], updateduList[i]);
344  changeAmount = fabs(previous_cost - cost) / cost;
345  }
346  previous_cost = cost;
347  }
348 
350  {
351  lastTraj.xList = updatedxList;
352  lastTraj.uList = updateduList;
353  lastTraj.iter = iter;
354  return lastTraj;
355  }
356 
357  DDPSolver::commandVec_t getLastCommand() { return updateduList[0]; }
358 
359  bool isQuudefinitePositive(const commandMat_t & Quu_reg)
360  {
361  lltofQuu.compute(Quu_reg);
362  if (lltofQuu.info() == Eigen::NumericalIssue)
363  {
364  std::cout << "not sdp" << std::endl;
365  std::cout << "#############################" << std::endl;
366  std::cout << "Quu_reg : " << Quu_reg << std::endl;
367  std::cout << "lxx : " << costFunction->getlxx() << std::endl;
368  std::cout << "lu : " << costFunction->getlu() << std::endl;
369  std::cout << "lx : " << costFunction->getlx() << std::endl;
370  std::cout << "luu" << costFunction->getluu() << std::endl;
371  std::cout << "updateduList[0] : " << updateduList[0] << std::endl;
372  std::cout << "updatedxList[0] : " << updatedxList[0] << std::endl;
373  std::cout << "#############################" << std::endl;
374  return false;
375  }
376  return true;
377  }
378  protected:
379 
380 };
381 
382 #endif // DDPSOLVER_H
CostFunction::getlu
commandVec_t & getlu()
Definition: costfunction.hh:62
CostFunction::getluu
commandMat_t & getluu()
Definition: costfunction.hh:63
DDPSolver::solveTrajectory
commandVec_t solveTrajectory()
Definition: ddpsolver.hh:194
DDPSolver::getLastSolvedTrajectory
DDPSolver::traj getLastSolvedTrajectory()
Definition: ddpsolver.hh:349
DynamicModel::getStateNb
unsigned int getStateNb()
Definition: dynamicmodel.hh:68
DynamicModel
Definition: dynamicmodel.hh:7
DDPSolver::stateR_commandC_stateD_t
Eigen::Matrix< precision, stateSize, commandSize > stateR_commandC_stateD_t[stateSize]
Definition: ddpsolver.hh:40
CostFunction::getFinalCost
double & getFinalCost()
Definition: costfunction.hh:59
DDPSolver::traj::iter
unsigned int iter
Definition: ddpsolver.hh:60
DDPSolver::backwardLoop
void backwardLoop()
Definition: ddpsolver.hh:226
DDPSolver::stateR_commandC_commandD_t
Eigen::Matrix< precision, stateSize, commandSize > stateR_commandC_commandD_t[commandSize]
Definition: ddpsolver.hh:41
DynamicModel::getfu
stateR_commandC_t & getfu()
Definition: dynamicmodel.hh:74
DDPSolver::commandMat_t
Eigen::Matrix< precision, commandSize, commandSize > commandMat_t
Definition: ddpsolver.hh:36
DDPSolver::commandVecTab_t
std::vector< commandVec_t > commandVecTab_t
Definition: ddpsolver.hh:49
DynamicModel::getfx
stateMat_t & getfx()
Definition: dynamicmodel.hh:72
DDPSolver::commandR_stateC_stateD_t
Eigen::Matrix< precision, commandSize, stateSize > commandR_stateC_stateD_t[stateSize]
Definition: ddpsolver.hh:43
CostFunction
Definition: costfunction.hh:7
DDPSolver::FirstInitSolver
void FirstInitSolver(stateVec_t &myxInit, stateVec_t &myxDes, unsigned int &myT, double &mydt, unsigned int &myiterMax, double &mystopCrit)
Definition: ddpsolver.hh:158
DDPSolver::commandR_stateC_commandD_t
Eigen::Matrix< precision, commandSize, stateSize > commandR_stateC_commandD_t[commandSize]
Definition: ddpsolver.hh:44
DDPSolver::commandR_commandC_stateD_t
Eigen::Matrix< precision, commandSize, commandSize > commandR_commandC_stateD_t[stateSize]
Definition: ddpsolver.hh:46
DDPSolver::stateMat_t
Eigen::Matrix< precision, stateSize, stateSize > stateMat_t
Definition: ddpsolver.hh:31
CostFunction::getRunningCost
double & getRunningCost()
Definition: costfunction.hh:58
DDPSolver::stateR_commandC_t
Eigen::Matrix< precision, stateSize, commandSize > stateR_commandC_t
Definition: ddpsolver.hh:39
CostFunction::getlxx
stateMat_t & getlxx()
Definition: costfunction.hh:61
DDPSolver::CostFunction_t
CostFunction< precision, stateSize, commandSize > CostFunction_t
Definition: ddpsolver.hh:53
DynamicModel::computeTensorContuu
virtual commandMat_t computeTensorContuu(const stateVec_t &nextVx)=0
DDPSolver::isQuudefinitePositive
bool isQuudefinitePositive(const commandMat_t &Quu_reg)
Definition: ddpsolver.hh:359
DynamicModel::computeTensorContux
virtual commandR_stateC_t computeTensorContux(const stateVec_t &nextVx)=0
DDPSolver::forwardLoop
void forwardLoop()
Definition: ddpsolver.hh:332
DynamicModel::getLowerCommandBounds
commandVec_t & getLowerCommandBounds()
Definition: dynamicmodel.hh:70
DDPSolver::initSolver
void initSolver(stateVec_t &myxInit, stateVec_t &myxDes)
Definition: ddpsolver.hh:188
CostFunction::computeCostAndDeriv
virtual void computeCostAndDeriv(const stateVec_t &X, const stateVec_t &Xdes, const commandVec_t &U)=0
DDPSolver::DynamicModel_t
DynamicModel< precision, stateSize, commandSize > DynamicModel_t
Definition: ddpsolver.hh:52
DDPSolver::stateR_stateC_commandD_t
Eigen::Matrix< precision, stateSize, stateSize > stateR_stateC_commandD_t[commandSize]
Definition: ddpsolver.hh:45
CostFunction::getlux
commandR_stateC_t & getlux()
Definition: costfunction.hh:64
DDPSolver::stateVec_t
Eigen::Matrix< precision, stateSize, 1 > stateVec_t
Definition: ddpsolver.hh:29
DDPSolver::commandVec_t
Eigen::Matrix< precision, commandSize, 1 > commandVec_t
Definition: ddpsolver.hh:34
DDPSolver
Definition: ddpsolver.hh:26
CostFunction::getlx
stateVec_t & getlx()
Definition: costfunction.hh:60
CostFunction::computeFinalCostAndDeriv
virtual void computeFinalCostAndDeriv(const stateVec_t &X, const stateVec_t &Xdes)=0
DynamicModel::computeNextState
virtual stateVec_t computeNextState(double &dt, const stateVec_t &X, const commandVec_t &U)=0
DynamicModel::computeModelDeriv
virtual void computeModelDeriv(double &dt, const stateVec_t &X, const commandVec_t &U)=0
DDPSolver::initTrajectory
void initTrajectory()
Definition: ddpsolver.hh:215
DDPSolver::commandVecTrans_t
Eigen::Matrix< precision, 1, commandSize > commandVecTrans_t
Definition: ddpsolver.hh:35
DynamicModel::getCommandNb
unsigned int getCommandNb()
Definition: dynamicmodel.hh:69
DDPSolver::DDPSolver
DDPSolver(DynamicModel_t &myDynamicModel, CostFunction_t &myCostFunction, bool fullDDP=0, bool QPBox=0)
Definition: ddpsolver.hh:129
costfunction.hh
DDPSolver::commandR_stateC_tab_t
std::vector< commandR_stateC_t > commandR_stateC_tab_t
Definition: ddpsolver.hh:50
DDPSolver::traj::xList
stateVecTab_t xList
Definition: ddpsolver.hh:58
DynamicModel::computeTensorContxx
virtual stateMat_t computeTensorContxx(const stateVec_t &nextVx)=0
DDPSolver::traj::uList
commandVecTab_t uList
Definition: ddpsolver.hh:59
DDPSolver::getLastCommand
DDPSolver::commandVec_t getLastCommand()
Definition: ddpsolver.hh:357
DynamicModel::getUpperCommandBounds
commandVec_t & getUpperCommandBounds()
Definition: dynamicmodel.hh:71
DDPSolver::stateVecTrans_t
Eigen::Matrix< precision, 1, stateSize > stateVecTrans_t
Definition: ddpsolver.hh:30
dynamicmodel.hh
DDPSolver::traj
Definition: ddpsolver.hh:56
DDPSolver::stateVecTab_t
std::vector< stateVec_t > stateVecTab_t
Definition: ddpsolver.hh:48
DDPSolver::commandR_stateC_t
Eigen::Matrix< precision, commandSize, stateSize > commandR_stateC_t
Definition: ddpsolver.hh:42