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