Commit 16a34d04 authored by flforget's avatar flforget
Browse files

[major] add possibility to use DDP as a simple iLQR

in this case, thge solver doesn't use the 2nd order derivatives of the dynamics
parent 2fe0748b
......@@ -12,6 +12,8 @@
#define ENABLE_QPBOX 1
#define DISABLE_QPBOX 0
#define ENABLE_FULLDDP 1
#define DISABLE_FULLDDP 0
using namespace Eigen;
USING_NAMESPACE_QPOASES
......@@ -27,7 +29,7 @@ public:
};
public:
ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction,bool QPBox=0);
ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction,bool fullDDP=0,bool QPBox=0);
private:
protected:
// attributes //
......@@ -80,6 +82,7 @@ private:
/* QP variables */
QProblemB* qp;
bool enableQPBox;
bool enableFullDDP;
commandMat_t H;
commandVec_t g;
commandVec_t lowerCommandBounds;
......
......@@ -7,13 +7,14 @@ using namespace std;
using namespace Eigen;
ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction,bool QPBox)
ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction,bool fullDDP,bool QPBox)
{
dynamicModel = &myDynamicModel;
costFunction = &myCostFunction;
stateNb = myDynamicModel.getStateNb();
commandNb = myDynamicModel.getCommandNb();
enableQPBox = QPBox;
enableFullDDP = fullDDP;
if(QPBox)
{
qp = new QProblemB(commandNb);
......@@ -134,9 +135,12 @@ void ILQRSolver::backwardLoop()
Quu = costFunction->getluu() + dynamicModel->getfu().transpose() * (nextVxx+muEye) * dynamicModel->getfu();
Qux = costFunction->getlux() + dynamicModel->getfu().transpose() * (nextVxx+muEye) * dynamicModel->getfx();
Qxx += dynamicModel->computeTensorContxx(nextVx);
Qux += dynamicModel->computeTensorContux(nextVx);
Quu += dynamicModel->computeTensorContuu(nextVx);
if(enableFullDDP)
{
Qxx += dynamicModel->computeTensorContxx(nextVx);
Qux += dynamicModel->computeTensorContux(nextVx);
Quu += dynamicModel->computeTensorContuu(nextVx);
}
QuuInv = Quu.inverse();
......
......@@ -35,7 +35,7 @@ int main()
RomeoSimpleActuator romeoActuatorModel(dt);
RomeoLinearActuator romeoLinearModel(dt);
CostFunctionRomeoActuator costRomeoActuator;
ILQRSolver testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator,0);
ILQRSolver testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator,ENABLE_FULLDDP,ENABLE_QPBOX);
testSolverRomeoActuator.FirstInitSolver(xinit,xDes,T,dt,iterMax,stopCrit);
......
......@@ -37,7 +37,7 @@ int main()
RomeoSimpleActuator romeoActuatorModel(dt);
RomeoLinearActuator romeoLinearModel(dt);
CostFunctionRomeoActuator costRomeoActuator;
ILQRSolver testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator);
ILQRSolver testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator,ENABLE_FULLDDP,DISABLE_QPBOX);
......
......@@ -51,7 +51,7 @@ int main()
cout << x << endl<<"-"<<endl;
}*/
ILQRSolver testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator,ENABLE_QPBOX);
ILQRSolver testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator,ENABLE_FULLDDP,DISABLE_QPBOX);
testSolverRomeoActuator.FirstInitSolver(xinit,xDes,T,dt,iterMax,stopCrit);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment