[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

cpp/include/costfunction.h

@@ -16,8 +16,8 @@ private:
 protected:
     // methods //
 public:
-    virtual void computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U)=0;
-    virtual void computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes)=0;
+    virtual void computeAllCostDeriv(const stateVec_t& X, const commandVec_t& U)=0;
+    virtual void computeFinalCostDeriv(const stateVec_t& X)=0;
 private:
 protected:
     // accessors //

cpp/include/costfunctionromeoactuator.h

@@ -31,8 +31,8 @@ private:
 protected:
     // methods //
 public:
-    void computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U);
-    void computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes);
+    void computeAllCostDeriv(const stateVec_t& X, const commandVec_t& U);
+    void computeFinalCostDeriv(const stateVec_t& X);
 private:
 protected:
     // accessors //

cpp/include/ilqrsolver.h

@@ -71,9 +71,9 @@ private:
 protected:
     // methods //
 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);
-    void initSolver(stateVec_t& myxInit, stateVec_t& myxDes);
+    void initSolver(stateVec_t& myxInit);
     void solveTrajectory();
     void initTrajectory();
     void backwardLoop();

cpp/src/costfunctionromeoactuator.cpp

@@ -15,17 +15,17 @@ CostFunctionRomeoActuator::CostFunctionRomeoActuator()
     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(0,0) = 100.0*(X(0,0)-Xdes(0,0));
+    lx(0,0) = 100.0*X(0,0);
     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(0,0) = 100.0*(X(0,0)-Xdes(0,0));
+    lx(0,0) = 100.0*X(0,0);
 }
 
 // accessors //

cpp/src/ilqrsolver.cpp

@@ -15,11 +15,10 @@ ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunctio
     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)
 {
     xInit = myxInit;
-    xDes = myxDes;
     T = myT;
     dt = mydt;
     iterMax = myiterMax;
@@ -42,10 +41,9 @@ void ILQRSolver::FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsign
     alpha = 1.0;
 }
 
-void ILQRSolver::initSolver(stateVec_t& myxInit, stateVec_t& myxDes)
+void ILQRSolver::initSolver(stateVec_t& myxInit)
 {
     xInit = myxInit;
-    xDes = myxDes;
 }
 
 void ILQRSolver::solveTrajectory()
@@ -82,10 +80,16 @@ void ILQRSolver::initTrajectory()
 
 void ILQRSolver::backwardLoop()
 {
-    costFunction->computeFinalCostDeriv(xList[T],xDes);
+    costFunction->computeFinalCostDeriv(xList[T]);
     nextVx = costFunction->getlx();
     nextVxx = costFunction->getlxx();
 
+    cout << nextVx << endl;
+    cout << "-" << endl;
+    cout << nextVxx << endl;
+    cout << "-" << endl;
+    cout << "-" << endl;
+
     mu = 0.0;
     completeBackwardFlag = 0;
 
@@ -99,7 +103,7 @@ void ILQRSolver::backwardLoop()
             u = uList[i];
 
             dynamicModel->computeAllModelDeriv(dt,x,u);
-            costFunction->computeAllCostDeriv(x,xDes,u);
+            costFunction->computeAllCostDeriv(x,u);
 
             Qx = costFunction->getlx() + dynamicModel->getfx().transpose() * nextVx;
             Qu = costFunction->getlu() + dynamicModel->getfu().transpose() * nextVx;
@@ -114,9 +118,6 @@ void ILQRSolver::backwardLoop()
 
             if(!isQuudefinitePositive(Quu))
             {
-                /*
-                  To be Implemented : Regularization (is Quu definite positive ?)
-                */
                 if(mu==0.0) mu += 1e-4;
                 else mu *= 10;
                 completeBackwardFlag = 0;

cpp/test/main.cpp

@@ -21,10 +21,10 @@ int main()
     double texec=0.0;
     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;
 
-    unsigned int T = 30;
+    unsigned int T = 300;
     double dt=1e-4;
     unsigned int iterMax = 20;
     double stopCrit = 1e-3;
@@ -35,11 +35,11 @@ int main()
     RomeoSimpleActuator romeoActuatorModel(dt);
     RomeoLinearActuator romeoLinearModel(dt);
     CostFunctionRomeoActuator costRomeoActuator;
-    ILQRSolver testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator);
-    testSolverRomeoActuator.FirstInitSolver(xinit,xDes,T,dt,iterMax,stopCrit);
+    ILQRSolver testSolverRomeoActuator(romeoLinearModel,costRomeoActuator);
+    testSolverRomeoActuator.FirstInitSolver(xinit,T,dt,iterMax,stopCrit);
 
 
-    int N = 100;
+    int N = 1;
     gettimeofday(&tbegin,NULL);
     for(int i=0;i<N;i++) testSolverRomeoActuator.solveTrajectory();
     gettimeofday(&tend,NULL);

cpp/test/mainMPC.cpp

@@ -51,12 +51,12 @@ int main()
     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);
     for(int i=0;i<M;i++)
     {
-        testSolverRomeoActuator.initSolver(xinit,xDes);
+        testSolverRomeoActuator.initSolver(xinit);
         testSolverRomeoActuator.solveTrajectory();
         lastTraj = testSolverRomeoActuator.getLastSolvedTrajectory();
         xList = lastTraj.xList;