[Major] fix bug

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/dynamicmodel.h

@@ -24,8 +24,8 @@ protected:
 
 // methods //
 public:
-    virtual stateVec_t computeNextState(double& dt, const stateVec_t& X,const commandVec_t& U)=0;
-    virtual void computeAllModelDeriv(double& dt, const stateVec_t& X,const commandVec_t& U)=0;
+    virtual stateVec_t computeNextState(double& dt, const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t& U)=0;
+    virtual void computeAllModelDeriv(double& dt, const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t& U)=0;
     virtual stateMat_t computeTensorContxx(const stateVec_t& nextVx)=0;
     virtual commandMat_t computeTensorContuu(const stateVec_t& nextVx)=0;
     virtual commandR_stateC_t computeTensorContux(const stateVec_t& nextVx)=0;

cpp/include/romeolinearactuator.h

@@ -46,8 +46,8 @@ private:
 protected:
     // methods //
 public:
-    stateVec_t computeNextState(double& dt,const stateVec_t& X,const commandVec_t &U);
-    void computeAllModelDeriv(double& dt, const stateVec_t& X,const commandVec_t &U);
+    stateVec_t computeNextState(double& dt,const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t &U);
+    void computeAllModelDeriv(double& dt, const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t &U);
     stateMat_t computeTensorContxx(const stateVec_t& nextVx);
     commandMat_t computeTensorContuu(const stateVec_t& nextVx);
     commandR_stateC_t computeTensorContux(const stateVec_t& nextVx);

cpp/include/romeosimpleactuator.h

@@ -29,6 +29,7 @@ private:
     static const double Cf0=0.1;
     static const double a=10.0;
 
+    stateVec_t Xreal;
     stateMat_t Id;
     stateMat_t A;
     stateMat_t Ad;
@@ -50,8 +51,8 @@ private:
 protected:
     // methods //
 public:
-    stateVec_t computeNextState(double& dt, const stateVec_t& X, const commandVec_t &U);
-    void computeAllModelDeriv(double& dt, const stateVec_t& X, const commandVec_t &U);
+    stateVec_t computeNextState(double& dt, const stateVec_t& X,const stateVec_t& Xdes, const commandVec_t &U);
+    void computeAllModelDeriv(double& dt, const stateVec_t& X,const stateVec_t& Xdes, const commandVec_t &U);
     stateMat_t computeTensorContxx(const stateVec_t& nextVx);
     commandMat_t computeTensorContuu(const stateVec_t& nextVx);
     commandR_stateC_t computeTensorContux(const stateVec_t& nextVx);

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

@@ -18,7 +18,7 @@ ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunctio
 void ILQRSolver::FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned int& myT,
                        double& mydt, unsigned int& myiterMax,double& mystopCrit)
 {
-    xInit = myxInit;
+    xInit = myxInit-myxDes;
     xDes = myxDes;
     T = myT;
     dt = mydt;
@@ -76,13 +76,13 @@ void ILQRSolver::initTrajectory()
     for(unsigned int i=0;i<T;i++)
     {
         uList[i] = zeroCommand;
-        xList[i+1] = dynamicModel->computeNextState(dt,xList[i],zeroCommand);
+        xList[i+1] = dynamicModel->computeNextState(dt,xList[i],xDes,zeroCommand);
     }
 }
 
 void ILQRSolver::backwardLoop()
 {
-    costFunction->computeFinalCostDeriv(xList[T],xDes);
+    costFunction->computeFinalCostDeriv(xList[T]);
     nextVx = costFunction->getlx();
     nextVxx = costFunction->getlxx();
 
@@ -98,8 +98,8 @@ void ILQRSolver::backwardLoop()
             x = xList[i];
             u = uList[i];
 
-            dynamicModel->computeAllModelDeriv(dt,x,u);
-            costFunction->computeAllCostDeriv(x,xDes,u);
+            dynamicModel->computeAllModelDeriv(dt,x,xDes,u);
+            costFunction->computeAllCostDeriv(x,u);
 
             Qx = costFunction->getlx() + dynamicModel->getfx().transpose() * nextVx;
             Qu = costFunction->getlu() + dynamicModel->getfu().transpose() * nextVx;
@@ -145,7 +145,7 @@ void ILQRSolver::forwardLoop()
     for(unsigned int i=0;i<T;i++)
     {
         updateduList[i] = uList[i] + alpha*kList[i] + KList[i]*(updatedxList[i]-xList[i]);
-        updatedxList[i+1] = dynamicModel->computeNextState(dt,updatedxList[i],updateduList[i]);
+        updatedxList[i+1] = dynamicModel->computeNextState(dt,updatedxList[i],xDes,updateduList[i]);
         for(unsigned int j=0;j<commandNb;j++)
         {
             changeAmount += abs(uList[i](j,0) - updateduList[i](j,0));
@@ -156,6 +156,7 @@ void ILQRSolver::forwardLoop()
 ILQRSolver::traj ILQRSolver::getLastSolvedTrajectory()
 {
     lastTraj.xList = updatedxList;
+    for(int i=0;i<T+1;i++)lastTraj.xList[i] += xDes;
     lastTraj.uList = updateduList;
     lastTraj.iter = iter;
     return lastTraj;

cpp/src/romeolinearactuator.cpp

@@ -42,13 +42,13 @@ RomeoLinearActuator::RomeoLinearActuator(double& mydt)
     fxu[0].setZero();
 }
 
-stateVec_t RomeoLinearActuator::computeNextState(double& dt, const stateVec_t& X,const commandVec_t& U)
+stateVec_t RomeoLinearActuator::computeNextState(double& dt, const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t& U)
 {
-    stateVec_t result = Ad*X + Bd*U;
+    stateVec_t result = Ad*(X+Xdes) + Bd*U;
     return result;
 }
 
-void RomeoLinearActuator::computeAllModelDeriv(double& dt, const stateVec_t& X,const commandVec_t& U)
+void RomeoLinearActuator::computeAllModelDeriv(double& dt, const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t& U)
 {
 
 }

cpp/src/romeosimpleactuator.cpp

@@ -53,22 +53,23 @@ RomeoSimpleActuator::RomeoSimpleActuator(double& mydt)
 }
 
 
-stateVec_t RomeoSimpleActuator::computeNextState(double& dt, const stateVec_t& X,const commandVec_t& U)
+stateVec_t RomeoSimpleActuator::computeNextState(double& dt, const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t& U)
 {
     stateVec_t result = Ad*X + Bd*U;
-    result(1,0)+=A13atan*atan(a*X(3,0));
-    result(3,0)+=A33atan*atan(a*X(3,0));
+    result(1,0)+=A13atan*atan(a*(X(3,0)+Xdes(3,0)));
+    result(3,0)+=A33atan*atan(a*(X(3,0)+Xdes(3,0)));
 
     return result;
 }
 
-void RomeoSimpleActuator::computeAllModelDeriv(double& dt, const stateVec_t& X,const commandVec_t& U)
+void RomeoSimpleActuator::computeAllModelDeriv(double& dt, const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t& U)
 {
+    Xreal = X + Xdes;
     fx = fxBase;
-    fx(1,3) += A13atan*(a/(1+a*a*X(3,0)*X(3,0)));
-    fx(3,3) -= A33atan*(a/(1+(a*a*X(3,0)*X(3,0))));
-    fxx[3](1,3) = -((2*dt*Jm*R)/(pi*Jl))*Cf0*((2*a*a*a*X(3,0))/((1+(a*a*X(3,0)*X(3,0)))*(1+(a*a*X(3,0)*X(3,0)))));
-    fxx[3](3,3) = +((2*dt*Cf0)/(pi*Jl))*((2*a*a*a*X(3,0))/((1+(a*a*X(3,0)*X(3,0)))*(1+(a*a*X(3,0)*X(3,0)))));
+    fx(1,3) += A13atan*(a/(1+a*a*Xreal(3,0)*Xreal(3,0)));
+    fx(3,3) -= A33atan*(a/(1+(a*a*Xreal(3,0)*Xreal(3,0))));
+    fxx[3](1,3) = -((2*dt*Jm*R)/(pi*Jl))*Cf0*((2*a*a*a*Xreal(3,0))/((1+(a*a*Xreal(3,0)*Xreal(3,0)))*(1+(a*a*Xreal(3,0)*Xreal(3,0)))));
+    fxx[3](3,3) = +((2*dt*Cf0)/(pi*Jl))*((2*a*a*a*Xreal(3,0))/((1+(a*a*Xreal(3,0)*Xreal(3,0)))*(1+(a*a*Xreal(3,0)*Xreal(3,0)))));
 }
 
 stateMat_t RomeoSimpleActuator::computeTensorContxx(const stateVec_t& nextVx)

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;
-    xDes << 1.0,0.0,0.0,0.0;
+    xinit << -1.0,0.0,0.0,0.0;
+    xDes << 2.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;

cpp/test/mainMPC.cpp

@@ -25,7 +25,7 @@ int main()
     xinit << 0.0,0.0,0.0,0.0;
     xDes << 1.0,0.0,0.0,0.0;
 
-    unsigned int T = 8;
+    unsigned int T = 300;
     unsigned int M = 30;
     double dt=1e-4;
     unsigned int iterMax = 20;