8 #include <Eigen/StdVector> 10 #include <qpOASES/QProblemB.hpp> 15 #define ENABLE_QPBOX 1 16 #define DISABLE_QPBOX 0 17 #define ENABLE_FULLDDP 1 18 #define DISABLE_FULLDDP 0 20 USING_NAMESPACE_QPOASES
22 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::MatrixXd)
23 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)
25 template<
typename precision,
int stateSize,
int commandSize>
29 typedef Eigen::Matrix<precision, stateSize, 1>
stateVec_t;
31 typedef Eigen::Matrix<precision, stateSize, stateSize>
stateMat_t;
36 typedef Eigen::Matrix<precision, commandSize, commandSize>
commandMat_t;
69 DynamicModel_t * dynamicModel;
70 CostFunction_t * costFunction;
72 unsigned int commandNb;
83 double cost, previous_cost;
84 commandVec_t zeroCommand;
87 commandVecTab_t
uList;
88 stateVecTab_t updatedxList;
89 commandVecTab_t updateduList;
90 stateVecTab_t tmpxPtr;
91 commandVecTab_t tmpuPtr;
99 commandMat_t Quu, Quu_reg;
100 Eigen::LLT<commandMat_t> lltofQuu;
102 commandR_stateC_t Qux, Qux_reg;
105 commandVecTab_t kList;
106 commandR_stateC_tab_t KList;
112 unsigned char completeBackwardFlag;
120 commandVec_t lowerCommandBounds;
121 commandVec_t upperCommandBounds;
129 DDPSolver(DynamicModel_t& myDynamicModel, CostFunction_t& myCostFunction,
130 bool fullDDP = 0,
bool QPBox = 0)
132 dynamicModel = &myDynamicModel;
133 costFunction = &myCostFunction;
137 enableFullDDP = fullDDP;
138 zeroCommand.setZero();
143 qp =
new QProblemB(commandNb);
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);
152 xOpt =
new real_t[commandNb];
159 unsigned int& myT,
double& mydt,
unsigned int& myiterMax,
167 stopCrit = mystopCrit;
169 xList.resize(myT + 1);
171 updatedxList.resize(myT + 1);
172 updateduList.resize(myT);
173 tmpxPtr.resize(myT + 1);
197 for (iter = 1; iter < iterMax; iter++)
201 if (changeAmount < stopCrit)
207 xList = updatedxList;
208 updatedxList = tmpxPtr;
209 uList = updateduList;
210 updateduList = tmpuPtr;
212 return updateduList[0];
218 for (
unsigned int i = 0; i < T; i++)
220 uList[i] = zeroCommand;
230 nextVx = costFunction->
getlx();
231 nextVxx = costFunction->
getlxx();
234 completeBackwardFlag = 0;
236 while (!completeBackwardFlag)
238 completeBackwardFlag = 1;
239 muEye = stateMat_t::Constant(mu);
240 for (
int i = T - 1; i >= 0; i--)
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();
281 std::cout <<
"regularization" << std::endl;
286 completeBackwardFlag = 0;
290 QuuInv = Quu.inverse();
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);
303 for (
unsigned int i_cmd = 0; i_cmd < commandNb; i_cmd++)
305 if ((k[i_cmd] == lowerCommandBounds[i_cmd])
306 | (k[i_cmd] == upperCommandBounds[i_cmd]))
308 K.row(i_cmd).setZero();
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());
335 updatedxList[0] = xInit;
338 for (
unsigned int i = 0; i < T; i++)
340 updateduList[i] = uList[i] + alpha * kList[i]
341 + KList[i] * (updatedxList[i] - xList[i]);
343 updatedxList[i], updateduList[i]);
344 changeAmount = fabs(previous_cost - cost) / cost;
346 previous_cost = cost;
351 lastTraj.
xList = updatedxList;
352 lastTraj.uList = updateduList;
353 lastTraj.iter =
iter;
361 lltofQuu.compute(Quu_reg);
362 if (lltofQuu.info() == Eigen::NumericalIssue)
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;
382 #endif // DDPSOLVER_H DDPSolver(DynamicModel_t &myDynamicModel, CostFunction_t &myCostFunction, bool fullDDP=0, bool QPBox=0)
Definition: ddpsolver.hh:129
double & getRunningCost()
Definition: costfunction.hh:58
Eigen::Matrix< precision, commandSize, stateSize > commandR_stateC_stateD_t[stateSize]
Definition: ddpsolver.hh:43
void initTrajectory()
Definition: ddpsolver.hh:215
Definition: dynamicmodel.hh:7
unsigned int iter
Definition: ddpsolver.hh:60
Eigen::Matrix< precision, stateSize, stateSize > stateR_stateC_commandD_t[commandSize]
Definition: ddpsolver.hh:45
std::vector< commandVec_t > commandVecTab_t
Definition: ddpsolver.hh:49
Eigen::Matrix< precision, commandSize, 1 > commandVec_t
Definition: ddpsolver.hh:34
Eigen::Matrix< precision, commandSize, commandSize > commandMat_t
Definition: ddpsolver.hh:36
void backwardLoop()
Definition: ddpsolver.hh:226
void FirstInitSolver(stateVec_t &myxInit, stateVec_t &myxDes, unsigned int &myT, double &mydt, unsigned int &myiterMax, double &mystopCrit)
Definition: ddpsolver.hh:158
commandVecTab_t uList
Definition: ddpsolver.hh:59
virtual void computeModelDeriv(double &dt, const stateVec_t &X, const commandVec_t &U)=0
Eigen::Matrix< precision, commandSize, stateSize > commandR_stateC_t
Definition: ddpsolver.hh:42
Eigen::Matrix< precision, 1, stateSize > stateVecTrans_t
Definition: ddpsolver.hh:30
DDPSolver::commandVec_t getLastCommand()
Definition: ddpsolver.hh:357
std::vector< stateVec_t > stateVecTab_t
Definition: ddpsolver.hh:48
Definition: costfunction.hh:7
commandMat_t & getluu()
Definition: costfunction.hh:63
stateR_commandC_t & getfu()
Definition: dynamicmodel.hh:74
commandVec_t & getLowerCommandBounds()
Definition: dynamicmodel.hh:70
Eigen::Matrix< precision, 1, commandSize > commandVecTrans_t
Definition: ddpsolver.hh:35
virtual stateMat_t computeTensorContxx(const stateVec_t &nextVx)=0
commandVec_t & getlu()
Definition: costfunction.hh:62
Eigen::Matrix< precision, stateSize, commandSize > stateR_commandC_t
Definition: ddpsolver.hh:39
commandVec_t & getUpperCommandBounds()
Definition: dynamicmodel.hh:71
Eigen::Matrix< precision, stateSize, 1 > stateVec_t
Definition: ddpsolver.hh:29
Eigen::Matrix< precision, stateSize, commandSize > stateR_commandC_commandD_t[commandSize]
Definition: ddpsolver.hh:41
DynamicModel< precision, stateSize, commandSize > DynamicModel_t
Definition: ddpsolver.hh:52
virtual commandMat_t computeTensorContuu(const stateVec_t &nextVx)=0
DDPSolver::traj getLastSolvedTrajectory()
Definition: ddpsolver.hh:349
virtual void computeFinalCostAndDeriv(const stateVec_t &X, const stateVec_t &Xdes)=0
unsigned int getCommandNb()
Definition: dynamicmodel.hh:69
stateMat_t & getfx()
Definition: dynamicmodel.hh:72
void forwardLoop()
Definition: ddpsolver.hh:332
Eigen::Matrix< precision, stateSize, stateSize > stateMat_t
Definition: ddpsolver.hh:31
Eigen::Matrix< precision, stateSize, commandSize > stateR_commandC_stateD_t[stateSize]
Definition: ddpsolver.hh:40
commandVec_t solveTrajectory()
Definition: ddpsolver.hh:194
Eigen::Matrix< precision, commandSize, commandSize > commandR_commandC_stateD_t[stateSize]
Definition: ddpsolver.hh:46
std::vector< commandR_stateC_t > commandR_stateC_tab_t
Definition: ddpsolver.hh:50
unsigned int getStateNb()
Definition: dynamicmodel.hh:68
Definition: ddpsolver.hh:56
Eigen::Matrix< precision, commandSize, stateSize > commandR_stateC_commandD_t[commandSize]
Definition: ddpsolver.hh:44
stateVec_t & getlx()
Definition: costfunction.hh:60
virtual stateVec_t computeNextState(double &dt, const stateVec_t &X, const commandVec_t &U)=0
bool isQuudefinitePositive(const commandMat_t &Quu_reg)
Definition: ddpsolver.hh:359
CostFunction< precision, stateSize, commandSize > CostFunction_t
Definition: ddpsolver.hh:53
virtual commandR_stateC_t computeTensorContux(const stateVec_t &nextVx)=0
Definition: ddpsolver.hh:26
stateMat_t & getlxx()
Definition: costfunction.hh:61
virtual void computeCostAndDeriv(const stateVec_t &X, const stateVec_t &Xdes, const commandVec_t &U)=0
void initSolver(stateVec_t &myxInit, stateVec_t &myxDes)
Definition: ddpsolver.hh:188
stateVecTab_t xList
Definition: ddpsolver.hh:58
double & getFinalCost()
Definition: costfunction.hh:59
commandR_stateC_t & getlux()
Definition: costfunction.hh:64