Loading...
Searching...
No Matches
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
20USING_NAMESPACE_QPOASES
21
22EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::MatrixXd)
23EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)
24
25template <typename precision, int stateSize, int commandSize>
26class 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;
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;
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
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
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
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
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
double & getRunningCost()
Definition: costfunction.hh:62
commandMat_t & getluu()
Definition: costfunction.hh:67
virtual void computeCostAndDeriv(const stateVec_t &X, const stateVec_t &Xdes, const commandVec_t &U)=0
double & getFinalCost()
Definition: costfunction.hh:63
commandVec_t & getlu()
Definition: costfunction.hh:66
commandR_stateC_t & getlux()
Definition: costfunction.hh:68
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
stateMat_t & getfx()
Definition: dynamicmodel.hh:74
virtual stateVec_t computeNextState(double &dt, const stateVec_t &X, const commandVec_t &U)=0
commandVec_t & getUpperCommandBounds()
Definition: dynamicmodel.hh:73
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:71
virtual stateMat_t computeTensorContxx(const stateVec_t &nextVx)=0
commandVec_t & getLowerCommandBounds()
Definition: dynamicmodel.hh:72
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