Commit 83f2990f authored by flforget's avatar flforget
Browse files

[major] fix a bug

the use of x and xDes was creating a computation problem
It is now necessary to use only x = xCurrent-xDes in DDP
parent b0b07ef9
...@@ -16,8 +16,8 @@ private: ...@@ -16,8 +16,8 @@ private:
protected: protected:
// methods // // methods //
public: public:
virtual void computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U)=0; virtual void computeAllCostDeriv(const stateVec_t& X, const commandVec_t& U)=0;
virtual void computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes)=0; virtual void computeFinalCostDeriv(const stateVec_t& X)=0;
private: private:
protected: protected:
// accessors // // accessors //
......
...@@ -31,8 +31,8 @@ private: ...@@ -31,8 +31,8 @@ private:
protected: protected:
// methods // // methods //
public: public:
void computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U); void computeAllCostDeriv(const stateVec_t& X, const commandVec_t& U);
void computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes); void computeFinalCostDeriv(const stateVec_t& X);
private: private:
protected: protected:
// accessors // // accessors //
......
...@@ -71,9 +71,9 @@ private: ...@@ -71,9 +71,9 @@ private:
protected: protected:
// methods // // methods //
public: public:
void FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned int& myT, void FirstInitSolver(stateVec_t& myxInit, unsigned int& myT,
double& mydt, unsigned int& myiterMax,double& mystopCrit); double& mydt, unsigned int& myiterMax,double& mystopCrit);
void initSolver(stateVec_t& myxInit, stateVec_t& myxDes); void initSolver(stateVec_t& myxInit);
void solveTrajectory(); void solveTrajectory();
void initTrajectory(); void initTrajectory();
void backwardLoop(); void backwardLoop();
......
...@@ -15,17 +15,17 @@ CostFunctionRomeoActuator::CostFunctionRomeoActuator() ...@@ -15,17 +15,17 @@ CostFunctionRomeoActuator::CostFunctionRomeoActuator()
lx.setZero(); lx.setZero();
} }
void CostFunctionRomeoActuator::computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U) void CostFunctionRomeoActuator::computeAllCostDeriv(const stateVec_t& X, const commandVec_t& U)
{ {
// lx = Q*(X-Xdes); // lx = Q*(X-Xdes);
lx(0,0) = 100.0*(X(0,0)-Xdes(0,0)); lx(0,0) = 100.0*X(0,0);
lu = R*U; lu = R*U;
} }
void CostFunctionRomeoActuator::computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes) void CostFunctionRomeoActuator::computeFinalCostDeriv(const stateVec_t& X)
{ {
// lx = Q*(X-Xdes); // lx = Q*(X-Xdes);
lx(0,0) = 100.0*(X(0,0)-Xdes(0,0)); lx(0,0) = 100.0*X(0,0);
} }
// accessors // // accessors //
......
...@@ -15,11 +15,10 @@ ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunctio ...@@ -15,11 +15,10 @@ ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunctio
commandNb = myDynamicModel.getCommandNb(); commandNb = myDynamicModel.getCommandNb();
} }
void ILQRSolver::FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned int& myT, void ILQRSolver::FirstInitSolver(stateVec_t& myxInit, unsigned int& myT,
double& mydt, unsigned int& myiterMax,double& mystopCrit) double& mydt, unsigned int& myiterMax,double& mystopCrit)
{ {
xInit = myxInit; xInit = myxInit;
xDes = myxDes;
T = myT; T = myT;
dt = mydt; dt = mydt;
iterMax = myiterMax; iterMax = myiterMax;
...@@ -42,10 +41,9 @@ void ILQRSolver::FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsign ...@@ -42,10 +41,9 @@ void ILQRSolver::FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsign
alpha = 1.0; alpha = 1.0;
} }
void ILQRSolver::initSolver(stateVec_t& myxInit, stateVec_t& myxDes) void ILQRSolver::initSolver(stateVec_t& myxInit)
{ {
xInit = myxInit; xInit = myxInit;
xDes = myxDes;
} }
void ILQRSolver::solveTrajectory() void ILQRSolver::solveTrajectory()
...@@ -82,10 +80,16 @@ void ILQRSolver::initTrajectory() ...@@ -82,10 +80,16 @@ void ILQRSolver::initTrajectory()
void ILQRSolver::backwardLoop() void ILQRSolver::backwardLoop()
{ {
costFunction->computeFinalCostDeriv(xList[T],xDes); costFunction->computeFinalCostDeriv(xList[T]);
nextVx = costFunction->getlx(); nextVx = costFunction->getlx();
nextVxx = costFunction->getlxx(); nextVxx = costFunction->getlxx();
cout << nextVx << endl;
cout << "-" << endl;
cout << nextVxx << endl;
cout << "-" << endl;
cout << "-" << endl;
mu = 0.0; mu = 0.0;
completeBackwardFlag = 0; completeBackwardFlag = 0;
...@@ -99,7 +103,7 @@ void ILQRSolver::backwardLoop() ...@@ -99,7 +103,7 @@ void ILQRSolver::backwardLoop()
u = uList[i]; u = uList[i];
dynamicModel->computeAllModelDeriv(dt,x,u); dynamicModel->computeAllModelDeriv(dt,x,u);
costFunction->computeAllCostDeriv(x,xDes,u); costFunction->computeAllCostDeriv(x,u);
Qx = costFunction->getlx() + dynamicModel->getfx().transpose() * nextVx; Qx = costFunction->getlx() + dynamicModel->getfx().transpose() * nextVx;
Qu = costFunction->getlu() + dynamicModel->getfu().transpose() * nextVx; Qu = costFunction->getlu() + dynamicModel->getfu().transpose() * nextVx;
...@@ -114,9 +118,6 @@ void ILQRSolver::backwardLoop() ...@@ -114,9 +118,6 @@ void ILQRSolver::backwardLoop()
if(!isQuudefinitePositive(Quu)) if(!isQuudefinitePositive(Quu))
{ {
/*
To be Implemented : Regularization (is Quu definite positive ?)
*/
if(mu==0.0) mu += 1e-4; if(mu==0.0) mu += 1e-4;
else mu *= 10; else mu *= 10;
completeBackwardFlag = 0; completeBackwardFlag = 0;
......
...@@ -21,10 +21,10 @@ int main() ...@@ -21,10 +21,10 @@ int main()
double texec=0.0; double texec=0.0;
stateVec_t xinit,xDes; stateVec_t xinit,xDes;
xinit << 0.0,0.0,0.0,0.0; xinit << -1.0,0.0,0.0,0.0;
xDes << 1.0,0.0,0.0,0.0; xDes << 1.0,0.0,0.0,0.0;
unsigned int T = 30; unsigned int T = 300;
double dt=1e-4; double dt=1e-4;
unsigned int iterMax = 20; unsigned int iterMax = 20;
double stopCrit = 1e-3; double stopCrit = 1e-3;
...@@ -35,11 +35,11 @@ int main() ...@@ -35,11 +35,11 @@ int main()
RomeoSimpleActuator romeoActuatorModel(dt); RomeoSimpleActuator romeoActuatorModel(dt);
RomeoLinearActuator romeoLinearModel(dt); RomeoLinearActuator romeoLinearModel(dt);
CostFunctionRomeoActuator costRomeoActuator; CostFunctionRomeoActuator costRomeoActuator;
ILQRSolver testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator); ILQRSolver testSolverRomeoActuator(romeoLinearModel,costRomeoActuator);
testSolverRomeoActuator.FirstInitSolver(xinit,xDes,T,dt,iterMax,stopCrit); testSolverRomeoActuator.FirstInitSolver(xinit,T,dt,iterMax,stopCrit);
int N = 100; int N = 1;
gettimeofday(&tbegin,NULL); gettimeofday(&tbegin,NULL);
for(int i=0;i<N;i++) testSolverRomeoActuator.solveTrajectory(); for(int i=0;i<N;i++) testSolverRomeoActuator.solveTrajectory();
gettimeofday(&tend,NULL); gettimeofday(&tend,NULL);
......
...@@ -51,12 +51,12 @@ int main() ...@@ -51,12 +51,12 @@ int main()
fichier << "tau,tauDot,q,qDot,u" << endl; fichier << "tau,tauDot,q,qDot,u" << endl;
testSolverRomeoActuator.FirstInitSolver(xinit,xDes,T,dt,iterMax,stopCrit); testSolverRomeoActuator.FirstInitSolver(xinit,T,dt,iterMax,stopCrit);
gettimeofday(&tbegin,NULL); gettimeofday(&tbegin,NULL);
for(int i=0;i<M;i++) for(int i=0;i<M;i++)
{ {
testSolverRomeoActuator.initSolver(xinit,xDes); testSolverRomeoActuator.initSolver(xinit);
testSolverRomeoActuator.solveTrajectory(); testSolverRomeoActuator.solveTrajectory();
lastTraj = testSolverRomeoActuator.getLastSolvedTrajectory(); lastTraj = testSolverRomeoActuator.getLastSolvedTrajectory();
xList = lastTraj.xList; xList = lastTraj.xList;
......
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