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