[major] use of Eigen::StdVector instead of tables

cpp/include/config.h

@@ -2,6 +2,10 @@
 #define CONFIG_H
 
 #include <Eigen/Dense>
+#include <Eigen/StdVector>
+
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::MatrixXd)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)
 
 #define stateSize 4
 #define commandSize 1
@@ -30,5 +34,8 @@ typedef Eigen::Matrix<double,commandSize,stateSize> commandR_stateC_commandD_t[c
 typedef Eigen::Matrix<double,stateSize,stateSize> stateR_stateC_commandD_t[commandSize];    // stateSize x stateSize x commandSize
 typedef Eigen::Matrix<double,commandSize,commandSize> commandR_commandC_stateD_t[stateSize];    // commandSize x commandSize x stateSize
 
+typedef std::vector<stateVec_t> stateVecTab_t;
+typedef std::vector<commandVec_t> commandVecTab_t;
+typedef std::vector<commandR_stateC_t> commandR_stateC_tab_t;
 
 #endif // CONFIG_H

cpp/include/ilqrsolver.h

@@ -6,6 +6,7 @@
 #include "dynamicmodel.h"
 #include "costfunction.h"
 #include <Eigen/Dense>
+#include <Eigen/StdVector>
 #include <qpOASES.hpp>
 #include <qpOASES/QProblemB.hpp>
 
@@ -20,8 +21,8 @@ class ILQRSolver
 public:
     struct traj
     {
-        stateVec_t* xList;
-        commandVec_t* uList;
+        stateVecTab_t xList;
+        commandVecTab_t uList;
         unsigned int iter;
     };
 
@@ -47,10 +48,12 @@ private:
     double stopCrit;
     double changeAmount;
 
-    stateVec_t* xList;
-    commandVec_t* uList;
-    stateVec_t* updatedxList;
-    commandVec_t* updateduList;
+    stateVecTab_t xList;
+    commandVecTab_t uList;
+    stateVecTab_t updatedxList;
+    commandVecTab_t updateduList;
+    stateVecTab_t tmpxPtr;
+    commandVecTab_t tmpuPtr;
     struct traj lastTraj;
 
     stateVec_t nextVx;
@@ -63,8 +66,8 @@ private:
     commandR_stateC_t Qux;
     commandVec_t k;
     commandR_stateC_t K;
-    commandVec_t* kList;
-    commandR_stateC_t* KList;
+    commandVecTab_t kList;
+    commandR_stateC_tab_t KList;
     double alphaList[5];
     double alpha;
 

cpp/src/ilqrsolver.cpp

@@ -41,14 +41,25 @@ void ILQRSolver::FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsign
     iterMax = myiterMax;
     stopCrit = mystopCrit;
 
-    xList = new stateVec_t[myT+1];
+    xList.resize(myT+1);
+    uList.resize(myT);
+    updatedxList.resize(myT+1);
+    updateduList.resize(myT);
+    tmpxPtr.resize(myT+1);
+    tmpuPtr.resize(myT);
+    k.setZero();
+    K.setZero();
+    kList.resize(myT);
+    KList.resize(myT);
+
+    /*xList = new stateVec_t[myT+1];
     uList = new commandVec_t[myT];
     updatedxList = new stateVec_t[myT+1];
     updateduList = new commandVec_t[myT];
     k.setZero();
     K.setZero();
     kList = new commandVec_t[myT];
-    KList = new commandR_stateC_t[myT];
+    KList = new commandR_stateC_t[myT];*/
 
     alphaList[0] = 1.0;
     alphaList[1] = 0.8;
@@ -66,8 +77,6 @@ void ILQRSolver::initSolver(stateVec_t& myxInit, stateVec_t& myxDes)
 
 void ILQRSolver::solveTrajectory()
 {
-    stateVec_t* tmpxPtr;
-    commandVec_t* tmpuPtr;
     initTrajectory();
     for(iter=0;iter<iterMax;iter++)
     {
@@ -160,7 +169,6 @@ void ILQRSolver::backwardLoop()
                         K.row(i_cmd).setZero();
                     }
                 }
-                cout << K << endl<<endl;
             }
             else
             {
@@ -199,10 +207,6 @@ void ILQRSolver::forwardLoop()
 
 ILQRSolver::traj ILQRSolver::getLastSolvedTrajectory()
 {
-    /* Debug */
-    //int k;
-    //for(k=0;k<T;k++) cout << updateduList[k] << '\t';
-    /**/
     lastTraj.xList = updatedxList;
     for(int i=0;i<T+1;i++)lastTraj.xList[i] += xDes;
     lastTraj.uList = updateduList;

cpp/test/main.cpp

@@ -28,8 +28,8 @@ int main()
     double dt=1e-4;
     unsigned int iterMax = 20;
     double stopCrit = 1e-5;
-    stateVec_t* xList;
-    commandVec_t* uList;
+    stateVecTab_t xList;
+    commandVecTab_t uList;
     ILQRSolver::traj lastTraj;
 
     RomeoSimpleActuator romeoActuatorModel(dt);

cpp/test/mainMPC.cpp

@@ -30,8 +30,8 @@ int main()
     double dt=1e-4;
     unsigned int iterMax = 20;
     double stopCrit = 1e-3;
-    stateVec_t* xList;
-    commandVec_t* uList;
+    stateVecTab_t xList;
+    commandVecTab_t uList;
     ILQRSolver::traj lastTraj;
 
     RomeoSimpleActuator romeoActuatorModel(dt);

cpp/test/testModel.cpp

@@ -34,8 +34,8 @@ int main()
     double dt=1e-4;
     unsigned int iterMax = 50;
     double stopCrit = 1e-5;
-    stateVec_t* xList;
-    commandVec_t* uList;
+    stateVecTab_t xList;
+    commandVecTab_t uList;
     ILQRSolver::traj lastTraj;
 
     RomeoSimpleActuator romeoActuatorModel(dt);