WOP

qtTmp/ilqrsolver.cpp

@@ -49,8 +49,6 @@ void ILQRSolver::solveTrajectory()
     {
         backwardLoop();
         forwardLoop();
-        xList = updatedxList;
-        uList = updateduList;
         if(changeAmount<stopCrit)
             break;
         for(unsigned int i=0;i<T;i++)
@@ -126,7 +124,7 @@ void ILQRSolver::forwardLoop()
 {
     changeAmount = 0.0;
     updatedxList[0] = xInit;
-    alpha = 0.0;
+    alpha = 1.0;
     for(unsigned int i=0;i<T;i++)
     {
         updateduList[i] = uList[i] + alpha*kList[i] + KList[i]*(updatedxList[i]-xList[i]);
@@ -135,3 +133,11 @@ void ILQRSolver::forwardLoop()
             changeAmount += abs(uList[i](j,0) - updateduList[i](j,0));
     }
 }
+
+ILQRSolver::traj ILQRSolver::getLastSolvedTrajectory()
+{
+    lastTraj.xList = updatedxList;
+    lastTraj.uList = updateduList;
+
+    return lastTraj;
+}

qtTmp/ilqrsolver.h

@@ -11,6 +11,14 @@ using namespace Eigen;
 
 class ILQRSolver
 {
+public:
+    struct traj
+    {
+        stateVec_t* xList;
+        commandVec_t* uList;
+    };
+
+
 public:
     ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction);
 private:
@@ -37,6 +45,7 @@ private:
     commandVec_t* uList;
     stateVec_t* updatedxList;
     commandVec_t* updateduList;
+    struct traj lastTraj;
 
     stateVec_t nextVx;
     stateMat_t nextVxx;
@@ -68,6 +77,7 @@ public:
     void initTrajectory();
     void backwardLoop();
     void forwardLoop();
+    struct traj getLastSolvedTrajectory();
 private:
 protected:
 

qtTmp/main.cpp

@@ -19,13 +19,16 @@ 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 = 50;
     double dt=1e-4;
     int iterMax = 20;
     double stopCrit = 1e-3;
+    stateVec_t* xList;
+    commandVec_t* uList;
+    ILQRSolver::traj lastTraj;
 
     RomeoSimpleActuator romeoActuatorModel;
     CostFunctionRomeoActuator costRomeoActuator;
@@ -36,13 +39,19 @@ int main()
     testSolverRomeoActuator.solveTrajectory();
     gettimeofday(&tend,NULL);
 
+    lastTraj = testSolverRomeoActuator.getLastSolvedTrajectory();
+    xList = lastTraj.xList;
+    uList = lastTraj.uList;
+
     texec=((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000.;
 
-    cout << "temps d'execution total du solveeur ";
+    cout << "temps d'execution total du solveur ";
     cout << texec << endl;
     cout << "temps d'execution par pas de temps ";
     cout << texec/T << endl;
 
+    for(int i=0;i<T+1;i++) cout << uList[i];
+
     return 0;
 }