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>
28 typedef Eigen::Matrix<precision, stateSize, 1>
stateVec_t;
30 typedef Eigen::Matrix<precision, stateSize, stateSize>
stateMat_t;
35 typedef Eigen::Matrix<precision, commandSize, commandSize>
commandMat_t;
39 typedef Eigen::Matrix<precision, stateSize, commandSize>
41 typedef Eigen::Matrix<precision, stateSize, commandSize>
44 typedef Eigen::Matrix<precision, commandSize, stateSize>
46 typedef Eigen::Matrix<precision, commandSize, stateSize>
48 typedef Eigen::Matrix<precision, stateSize, stateSize>
50 typedef Eigen::Matrix<precision, commandSize, commandSize>
76 unsigned int commandNb;
87 double cost, previous_cost;
104 Eigen::LLT<commandMat_t> lltofQuu;
116 unsigned char completeBackwardFlag;
135 dynamicModel = &myDynamicModel;
136 costFunction = &myCostFunction;
140 enableFullDDP = fullDDP;
141 zeroCommand.setZero();
145 qp =
new QProblemB(commandNb);
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);
154 xOpt =
new real_t[commandNb];
161 unsigned int& myiterMax,
double& mystopCrit) {
167 stopCrit = mystopCrit;
169 xList.resize(myT + 1);
171 updatedxList.resize(myT + 1);
172 updateduList.resize(myT);
173 tmpxPtr.resize(myT + 1);
195 for (iter = 1; iter < iterMax; iter++) {
198 if (changeAmount < stopCrit) {
203 xList = updatedxList;
204 updatedxList = tmpxPtr;
205 uList = updateduList;
206 updateduList = tmpuPtr;
208 return updateduList[0];
213 for (
unsigned int i = 0; i < T; i++) {
214 uList[i] = zeroCommand;
222 nextVx = costFunction->
getlx();
223 nextVxx = costFunction->
getlxx();
226 completeBackwardFlag = 0;
228 while (!completeBackwardFlag) {
229 completeBackwardFlag = 1;
230 muEye = stateMat_t::Constant(mu);
231 for (
int i = T - 1; i >= 0; i--) {
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();
245 costFunction->
getluu() + dynamicModel->
getfu().transpose() * (nextVxx + muEye) * dynamicModel->
getfu();
247 costFunction->
getlux() + dynamicModel->
getfu().transpose() * (nextVxx + muEye) * dynamicModel->
getfx();
258 std::cout <<
"regularization" << std::endl;
263 completeBackwardFlag = 0;
267 QuuInv = Quu.inverse();
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);
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();
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());
303 updatedxList[0] = xInit;
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;
311 previous_cost = cost;
315 lastTraj.
xList = updatedxList;
316 lastTraj.
uList = updateduList;
317 lastTraj.
iter = iter;
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;
344 #endif // DDPSOLVER_H
commandVec_t & getlu()
Definition: costfunction.hh:66
commandMat_t & getluu()
Definition: costfunction.hh:67
commandVec_t solveTrajectory()
Definition: ddpsolver.hh:193
DDPSolver::traj getLastSolvedTrajectory()
Definition: ddpsolver.hh:314
unsigned int getStateNb()
Definition: dynamicmodel.hh:70
Definition: dynamicmodel.hh:7
Eigen::Matrix< precision, stateSize, commandSize > stateR_commandC_stateD_t[stateSize]
Definition: ddpsolver.hh:40
double & getFinalCost()
Definition: costfunction.hh:63
unsigned int iter
Definition: ddpsolver.hh:64
void backwardLoop()
Definition: ddpsolver.hh:219
Eigen::Matrix< precision, stateSize, commandSize > stateR_commandC_commandD_t[commandSize]
Definition: ddpsolver.hh:42
stateR_commandC_t & getfu()
Definition: dynamicmodel.hh:76
Eigen::Matrix< precision, commandSize, commandSize > commandMat_t
Definition: ddpsolver.hh:35
std::vector< commandVec_t > commandVecTab_t
Definition: ddpsolver.hh:54
stateMat_t & getfx()
Definition: dynamicmodel.hh:74
Eigen::Matrix< precision, commandSize, stateSize > commandR_stateC_stateD_t[stateSize]
Definition: ddpsolver.hh:45
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:160
Eigen::Matrix< precision, commandSize, stateSize > commandR_stateC_commandD_t[commandSize]
Definition: ddpsolver.hh:47
Eigen::Matrix< precision, commandSize, commandSize > commandR_commandC_stateD_t[stateSize]
Definition: ddpsolver.hh:51
Eigen::Matrix< precision, stateSize, stateSize > stateMat_t
Definition: ddpsolver.hh:30
double & getRunningCost()
Definition: costfunction.hh:62
Eigen::Matrix< precision, stateSize, commandSize > stateR_commandC_t
Definition: ddpsolver.hh:38
stateMat_t & getlxx()
Definition: costfunction.hh:65
CostFunction< precision, stateSize, commandSize > CostFunction_t
Definition: ddpsolver.hh:58
virtual commandMat_t computeTensorContuu(const stateVec_t &nextVx)=0
bool isQuudefinitePositive(const commandMat_t &Quu_reg)
Definition: ddpsolver.hh:323
virtual commandR_stateC_t computeTensorContux(const stateVec_t &nextVx)=0
void forwardLoop()
Definition: ddpsolver.hh:301
commandVec_t & getLowerCommandBounds()
Definition: dynamicmodel.hh:72
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:57
Eigen::Matrix< precision, stateSize, stateSize > stateR_stateC_commandD_t[commandSize]
Definition: ddpsolver.hh:49
commandR_stateC_t & getlux()
Definition: costfunction.hh:68
Eigen::Matrix< precision, stateSize, 1 > stateVec_t
Definition: ddpsolver.hh:28
Eigen::Matrix< precision, commandSize, 1 > commandVec_t
Definition: ddpsolver.hh:33
Definition: ddpsolver.hh:26
stateVec_t & getlx()
Definition: costfunction.hh:64
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:211
Eigen::Matrix< precision, 1, commandSize > commandVecTrans_t
Definition: ddpsolver.hh:34
unsigned int getCommandNb()
Definition: dynamicmodel.hh:71
DDPSolver(DynamicModel_t &myDynamicModel, CostFunction_t &myCostFunction, bool fullDDP=0, bool QPBox=0)
Definition: ddpsolver.hh:134
std::vector< commandR_stateC_t > commandR_stateC_tab_t
Definition: ddpsolver.hh:55
stateVecTab_t xList
Definition: ddpsolver.hh:62
virtual stateMat_t computeTensorContxx(const stateVec_t &nextVx)=0
commandVecTab_t uList
Definition: ddpsolver.hh:63
DDPSolver::commandVec_t getLastCommand()
Definition: ddpsolver.hh:321
commandVec_t & getUpperCommandBounds()
Definition: dynamicmodel.hh:73
Eigen::Matrix< precision, 1, stateSize > stateVecTrans_t
Definition: ddpsolver.hh:29
Definition: ddpsolver.hh:61
std::vector< stateVec_t > stateVecTab_t
Definition: ddpsolver.hh:53
Eigen::Matrix< precision, commandSize, stateSize > commandR_stateC_t
Definition: ddpsolver.hh:43