Go to the documentation of this file.
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;
72 unsigned int commandNb;
83 double cost, previous_cost;
100 Eigen::LLT<commandMat_t> lltofQuu;
112 unsigned char completeBackwardFlag;
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
commandVec_t & getlu()
Definition: costfunction.hh:62
commandMat_t & getluu()
Definition: costfunction.hh:63
commandVec_t solveTrajectory()
Definition: ddpsolver.hh:194
DDPSolver::traj getLastSolvedTrajectory()
Definition: ddpsolver.hh:349
unsigned int getStateNb()
Definition: dynamicmodel.hh:68
Definition: dynamicmodel.hh:7
Eigen::Matrix< precision, stateSize, commandSize > stateR_commandC_stateD_t[stateSize]
Definition: ddpsolver.hh:40
double & getFinalCost()
Definition: costfunction.hh:59
unsigned int iter
Definition: ddpsolver.hh:60
void backwardLoop()
Definition: ddpsolver.hh:226
Eigen::Matrix< precision, stateSize, commandSize > stateR_commandC_commandD_t[commandSize]
Definition: ddpsolver.hh:41
stateR_commandC_t & getfu()
Definition: dynamicmodel.hh:74
Eigen::Matrix< precision, commandSize, commandSize > commandMat_t
Definition: ddpsolver.hh:36
std::vector< commandVec_t > commandVecTab_t
Definition: ddpsolver.hh:49
stateMat_t & getfx()
Definition: dynamicmodel.hh:72
Eigen::Matrix< precision, commandSize, stateSize > commandR_stateC_stateD_t[stateSize]
Definition: ddpsolver.hh:43
Definition: costfunction.hh:7
void FirstInitSolver(stateVec_t &myxInit, stateVec_t &myxDes, unsigned int &myT, double &mydt, unsigned int &myiterMax, double &mystopCrit)
Definition: ddpsolver.hh:158
Eigen::Matrix< precision, commandSize, stateSize > commandR_stateC_commandD_t[commandSize]
Definition: ddpsolver.hh:44
Eigen::Matrix< precision, commandSize, commandSize > commandR_commandC_stateD_t[stateSize]
Definition: ddpsolver.hh:46
Eigen::Matrix< precision, stateSize, stateSize > stateMat_t
Definition: ddpsolver.hh:31
double & getRunningCost()
Definition: costfunction.hh:58
Eigen::Matrix< precision, stateSize, commandSize > stateR_commandC_t
Definition: ddpsolver.hh:39
stateMat_t & getlxx()
Definition: costfunction.hh:61
CostFunction< precision, stateSize, commandSize > CostFunction_t
Definition: ddpsolver.hh:53
virtual commandMat_t computeTensorContuu(const stateVec_t &nextVx)=0
bool isQuudefinitePositive(const commandMat_t &Quu_reg)
Definition: ddpsolver.hh:359
virtual commandR_stateC_t computeTensorContux(const stateVec_t &nextVx)=0
void forwardLoop()
Definition: ddpsolver.hh:332
commandVec_t & getLowerCommandBounds()
Definition: dynamicmodel.hh:70
void initSolver(stateVec_t &myxInit, stateVec_t &myxDes)
Definition: ddpsolver.hh:188
virtual void computeCostAndDeriv(const stateVec_t &X, const stateVec_t &Xdes, const commandVec_t &U)=0
DynamicModel< precision, stateSize, commandSize > DynamicModel_t
Definition: ddpsolver.hh:52
Eigen::Matrix< precision, stateSize, stateSize > stateR_stateC_commandD_t[commandSize]
Definition: ddpsolver.hh:45
commandR_stateC_t & getlux()
Definition: costfunction.hh:64
Eigen::Matrix< precision, stateSize, 1 > stateVec_t
Definition: ddpsolver.hh:29
Eigen::Matrix< precision, commandSize, 1 > commandVec_t
Definition: ddpsolver.hh:34
Definition: ddpsolver.hh:26
stateVec_t & getlx()
Definition: costfunction.hh:60
virtual void computeFinalCostAndDeriv(const stateVec_t &X, const stateVec_t &Xdes)=0
virtual stateVec_t computeNextState(double &dt, const stateVec_t &X, const commandVec_t &U)=0
virtual void computeModelDeriv(double &dt, const stateVec_t &X, const commandVec_t &U)=0
void initTrajectory()
Definition: ddpsolver.hh:215
Eigen::Matrix< precision, 1, commandSize > commandVecTrans_t
Definition: ddpsolver.hh:35
unsigned int getCommandNb()
Definition: dynamicmodel.hh:69
DDPSolver(DynamicModel_t &myDynamicModel, CostFunction_t &myCostFunction, bool fullDDP=0, bool QPBox=0)
Definition: ddpsolver.hh:129
std::vector< commandR_stateC_t > commandR_stateC_tab_t
Definition: ddpsolver.hh:50
stateVecTab_t xList
Definition: ddpsolver.hh:58
virtual stateMat_t computeTensorContxx(const stateVec_t &nextVx)=0
commandVecTab_t uList
Definition: ddpsolver.hh:59
DDPSolver::commandVec_t getLastCommand()
Definition: ddpsolver.hh:357
commandVec_t & getUpperCommandBounds()
Definition: dynamicmodel.hh:71
Eigen::Matrix< precision, 1, stateSize > stateVecTrans_t
Definition: ddpsolver.hh:30
Definition: ddpsolver.hh:56
std::vector< stateVec_t > stateVecTab_t
Definition: ddpsolver.hh:48
Eigen::Matrix< precision, commandSize, stateSize > commandR_stateC_t
Definition: ddpsolver.hh:42