add several typedef

qtTmp/config.h

+#ifndef CONFIG_H
+#define CONFIG_H
+
+#include <Eigen/Core>
+
+#define stateSize 4
+#define commandSize 1
+
+// typedef for stateSize types
+typedef Eigen::Matrix<double,stateSize,1> stateVec_t;                       // stateSize x 1
+typedef Eigen::Matrix<double,1,stateSize> stateVecTrans_t;                  // 1 x stateSize
+typedef Eigen::Matrix<double,stateSize,stateSize> stateMat_t;               // stateSize x stateSize
+typedef Eigen::Matrix<double,stateSize,stateSize> stateTens_t[stateSize];   // stateSize x stateSize x stateSize
+
+// typedef for commandSize types
+typedef Eigen::Matrix<double,commandSize,1> commandVec_t;                           // commandSize x 1
+typedef Eigen::Matrix<double,1,commandSize> commandVecTrans_t;                      // 1 x commandSize
+typedef Eigen::Matrix<double,commandSize,commandSize> commandMat_t;                 // commandSize x commandSize
+typedef Eigen::Matrix<double,commandSize,commandSize> commandTens_t[commandSize];   // stateSize x commandSize x commandSize
+
+
+
+// typedef for mixed stateSize and commandSize types
+typedef Eigen::Matrix<double,stateSize,commandSize> stateR_commandC_t;                          // stateSize x commandSize
+typedef Eigen::Matrix<double,stateSize,commandSize> stateR_commandC_stateD_t[stateSize];        // stateSize x commandSize x stateSize
+typedef Eigen::Matrix<double,stateSize,commandSize> stateR_commandC_commandD_t[commandSize];    // stateSize x commandSize x commandSize
+typedef Eigen::Matrix<double,commandSize,stateSize> commandR_stateC_t;                          // commandSize x stateSize
+typedef Eigen::Matrix<double,commandSize,stateSize> commandR_stateC_stateD_t[stateSize];        // commandSize x stateSize x stateSize
+typedef Eigen::Matrix<double,commandSize,stateSize> commandR_stateC_commandD_t[commandSize];    // commandSize x stateSize x commandSize
+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
+
+
+#endif // CONFIG_H

qtTmp/costfunction.h

 #ifndef COSTFUNCTION_H
 #define COSTFUNCTION_H
 
+#include "config.h"
+
 class CostFunction
 {
 public:
     CostFunction();
+private:
+protected:
+    // attributes //
+public:
+private:
+protected:
+    // methods //
+public:
+    virtual void computeAllCostDeriv(stateVec_t& X, stateVec_t& Xdes, commandVec_t& U);
+    virtual void commuteFinalCostDeriv(stateVec_t& X, stateVec_t& Xdes);
+private:
+protected:
+    // accessors //
+public:
+    virtual stateVec_t getlx();
+    virtual stateMat_t getlxx();
+    virtual commandVec_t getlu();
+    virtual commandMat_t getluu();
+    virtual commandR_stateC_t getlux();
+    virtual stateR_commandC_t getlxu();
 };
 
 #endif // COSTFUNCTION_H

qtTmp/costfunctionromeoactuator.cpp

@@ -6,7 +6,7 @@ CostFunctionRomeoActuator::CostFunctionRomeoActuator()
                 0.0,0.0,0.0,0.0,
                 0.0,0.0,0.0,0.0,
                 0.0,0.0,0.0,0.0;
-    this->R = 0.1;
+    this->R << 0.1;
 
     this->lxx = this->Q;
     this->luu = this->R;
@@ -14,44 +14,44 @@ CostFunctionRomeoActuator::CostFunctionRomeoActuator()
     this->lxu << 0.0,0.0,0.0,0.0;
 }
 
-void CostFunctionRomeoActuator::computeAllCostDeriv(V4 &X, V4 &Xdes, double &U)
+void CostFunctionRomeoActuator::computeAllCostDeriv(stateVec_t& X, stateVec_t& Xdes, commandVec_t& U)
 {
     this->lx = this->Q*(X-Xdes);
     this->lu = this->R*U;
 }
 
-void CostFunctionRomeoActuator::commuteFinalCostDeriv(V4 &X, V4 &Xdes)
+void CostFunctionRomeoActuator::commuteFinalCostDeriv(stateVec_t& X, stateVec_t& Xdes)
 {
     this->lx = this->Q*(X-Xdes);
 }
 
 // accessors //
-V4 CostFunctionRomeoActuator::getlx()
+stateVec_t CostFunctionRomeoActuator::getlx()
 {
     return this->lx;
 }
 
-M4 CostFunctionRomeoActuator::getlxx()
+stateMat_t CostFunctionRomeoActuator::getlxx()
 {
     return this->lxx;
 }
 
-double CostFunctionRomeoActuator::getlu()
+commandVec_t CostFunctionRomeoActuator::getlu()
 {
     return this->lu;
 }
 
-double CostFunctionRomeoActuator::getluu()
+commandMat_t CostFunctionRomeoActuator::getluu()
 {
     return this->luu;
 }
 
-V4 CostFunctionRomeoActuator::getlxu()
+stateR_commandC_t CostFunctionRomeoActuator::getlxu()
 {
     return this->lxu;
 }
 
-V4T CostFunctionRomeoActuator::getlux()
+commandR_stateC_t CostFunctionRomeoActuator::getlux()
 {
     return this->lux;
 }

qtTmp/costfunctionromeoactuator.h

 #ifndef COSTFUNCTIONROMEOACTUATOR_H
 #define COSTFUNCTIONROMEOACTUATOR_H
 
+#include "config.h"
+
+#include "costfunction.h"
+
 #include <Eigen/Core>
 
 using namespace Eigen;
 
-typedef Matrix<double,4,4> M4;
-typedef Matrix<double,4,1> V4;
-typedef Matrix<double,1,4> V4T;
-
-class CostFunctionRomeoActuator
+class CostFunctionRomeoActuator : public CostFunction
 {
 public:
     CostFunctionRomeoActuator();
 private:
-    M4 Q;
-    double R;
-    V4 lx;
-    M4 lxx;
-    double lu,luu;
-    V4T lux;
-    V4 lxu;
+    stateMat_t Q;
+    commandMat_t R;
+    stateVec_t lx;
+    stateMat_t lxx;
+    commandVec_t lu;
+    commandMat_t luu;
+    commandR_stateC_t lux;
+    stateR_commandC_t lxu;
 protected:
     // attributes //
 public:
@@ -29,18 +30,18 @@ private:
 protected:
     // methods //
 public:
-    void computeAllCostDeriv(V4& X, V4& Xdes, double& U);
-    void commuteFinalCostDeriv(V4& X, V4& Xdes);
+    void computeAllCostDeriv(stateVec_t& X, stateVec_t& Xdes, commandVec_t& U);
+    void commuteFinalCostDeriv(stateVec_t& X, stateVec_t& Xdes);
 private:
 protected:
     // accessors //
 public:
-    V4 getlx();
-    M4 getlxx();
-    double getlu();
-    double getluu();
-    V4T getlux();
-    V4 getlxu();
+    stateVec_t getlx();
+    stateMat_t getlxx();
+    commandVec_t getlu();
+    commandMat_t getluu();
+    commandR_stateC_t getlux();
+    stateR_commandC_t getlxu();
 };
 
 #endif // COSTFUNCTIONROMEOACTUATOR_H

qtTmp/dynamicmodel.cpp

@@ -3,23 +3,3 @@
 DynamicModel::DynamicModel()
 {
 }
-
-void DynamicModel::computeAllModelDeriv(unsigned int dt, unsigned int X, unsigned int U)
-{
-
-}
-
-void DynamicModel::computeNextState(unsigned int dt, unsigned int X, unsigned int U)
-{
-
-}
-
-unsigned int DynamicModel::getCommandNb()
-{
-
-}
-
-unsigned int DynamicModel::getStateNb()
-{
-
-}

qtTmp/dynamicmodel.h

 #ifndef DYNAMICMODEL_H
 #define DYNAMICMODEL_H
 
+#include "config.h"
+
 #include <Eigen/Core>
 
 using namespace Eigen;
@@ -14,15 +16,15 @@ public:
 // attributes //
 public:
 private:
-    static unsigned int stateNb;
-    static unsigned int commandNb;
+    unsigned int stateNb;
+    unsigned int commandNb;
 
 protected:
 
 // methods //
 public:
-    virtual void computeNextState(unsigned int dt,unsigned int X,unsigned int U);
-    virtual void computeAllModelDeriv(unsigned int dt,unsigned int X,unsigned int U);
+    virtual stateVec_t computeNextState(unsigned int dt,stateVec_t& X,commandVec_t& U);
+    virtual void computeAllModelDeriv(unsigned int dt,stateVec_t& X,commandVec_t& U);
     virtual unsigned int getStateNb();
     virtual unsigned int getCommandNb();
 private:

qtTmp/ilqrsolver.cpp

@@ -10,7 +10,7 @@ ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunctio
     this->commandNb = myDynamicModel.getCommandNb();
 }
 
-void ILQRSolver::initSolver(Eigen::VectorXd myxInit, Eigen::VectorXd myxDes, unsigned int myT,
+void ILQRSolver::initSolver(stateVec_t myxInit, stateVec_t myxDes, unsigned int myT,
                        double mydt, unsigned int myiterMax,double mystopCrit)
 {
     this->xInit = myxInit;
@@ -20,10 +20,10 @@ void ILQRSolver::initSolver(Eigen::VectorXd myxInit, Eigen::VectorXd myxDes, uns
     this->iterMax = myiterMax;
     this->stopCrit = mystopCrit;
 
-    this->xList = new Eigen::VectorXd[myT+1];
-    this->uList = new Eigen::VectorXd[myT];
-    this->updatedxList = new Eigen::VectorXd[myT+1];
-    this->updateduList = new Eigen::VectorXd[myT];
+    this->xList = new stateVec_t[myT+1];
+    this->uList = new commandVec_t[myT];
+    this->updatedxList = new stateVec_t[myT+1];
+    this->updateduList = new commandVec_t[myT];
 }
 
 void ILQRSolver::solveTrajectory()
@@ -43,9 +43,12 @@ void ILQRSolver::solveTrajectory()
 void ILQRSolver::initTrajectory()
 {
     this->xList[0] = this->xInit;
-    for(unsigned int i=1;i<this->T;i++)
+    commandVec_t zeroCommand;
+    zeroCommand << commandVec_t::Zero(commandSize,1);
+    for(unsigned int i=0;i<this->T;i++)
     {
-
+        this->uList[i] = zeroCommand;
+        this->xList[i+1] = this->dynamicModel.computeNextState(this->dt,this->xList[i],zeroCommand);
     }
 }
 

qtTmp/ilqrsolver.h

 #ifndef ILQRSOLVER_H
 #define ILQRSOLVER_H
 
+#include "config.h"
+
 #include "dynamicmodel.h"
 #include "costfunction.h"
 #include <Eigen/Core>
 
 using namespace Eigen;
 
-#ifdef stateSize
-typedef Matrix<double,stateSize,1> stateVec;
-#else
-typedef Matrix<double,Dynamic,1> stateVec;
-#endif
-#ifdef commandSize
-typedef Matrix<double,commandSize,1> commandVec;
-#else
-typedef Matrix<double,Dynamic,1> commandVec;
-#endif
-
 class ILQRSolver
 {
 public:
@@ -31,8 +22,8 @@ private:
     CostFunction costFunction;
     unsigned int stateNb;
     unsigned int commandNb;
-    stateVec xInit;
-    stateVec xDes;
+    stateVec_t xInit;
+    stateVec_t xDes;
     unsigned int T;
     double dt;
     unsigned int iter;
@@ -40,16 +31,16 @@ private:
     double stopCrit;
     double changeAmount;
 
-    stateVec* xList;
-    stateVec* uList;
-    stateVec* updatedxList;
-    stateVec* updateduList;
+    stateVec_t* xList;
+    commandVec_t* uList;
+    stateVec_t* updatedxList;
+    commandVec_t* updateduList;
 
 
 protected:
     // methods //
 public:
-    void initSolver(stateVec myxInit, stateVec myxDes, unsigned int myT,
+    void initSolver(stateVec_t myxInit, stateVec_t myxDes, unsigned int myT,
                     double mydt, unsigned int myiterMax,double mystopCrit);
     void solveTrajectory();
     void initTrajectory();

qtTmp/main.cpp

-#define stateSize 4
-#define commandSize 1
-
 #include <iostream>
+
+#include "config.h"
+
 #include "ilqrsolver.h"
 #include "romeosimpleactuator.h"
 #include "costfunctionromeoactuator.h"
@@ -10,8 +10,6 @@
 using namespace std;
 using namespace Eigen;
 
-typedef Matrix<double,4,1> V4;
-
 int main()
 {
     DynamicModel testModel;
@@ -19,6 +17,8 @@ int main()
     RomeoSimpleActuator romeoActuatorModel;
     CostFunctionRomeoActuator costRomeoActuator;
     ILQRSolver testSolver(testModel,testCostFun);
+    ILQRSolver testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator);
+
     return 0;
 }
 

qtTmp/qtTmp.pro

@@ -22,6 +22,7 @@ HEADERS += \
     dynamicmodel.h \
     ilqrsolver.h \
     romeosimpleactuator.h \
-    costfunctionromeoactuator.h
+    costfunctionromeoactuator.h \
+    config.h
 
 INCLUDEPATH += /usr/include/eigen3

qtTmp/romeosimpleactuator.cpp

@@ -47,11 +47,11 @@ RomeoSimpleActuator::RomeoSimpleActuator()
     this->fxx[1] = this->fxx[0];
     this->fxx[2] = this->fxx[0];
     this->fxx[3] = this->fxx[0];
-    this->fxu <<    0.0,      0.0,      0.0,      0.0,
+    this->fxu[0] <<    0.0,      0.0,      0.0,      0.0,
                     0.0,      0.0,      0.0,      0.0,
                     0.0,      0.0,      0.0,      0.0,
                     0.0,      0.0,      0.0,      0.0;
-    this->fxu <<    0.0,      0.0,      0.0,      0.0,
+    this->fxu[0] <<    0.0,      0.0,      0.0,      0.0,
                     0.0,      0.0,      0.0,      0.0,
                     0.0,      0.0,      0.0,      0.0,
                     0.0,      0.0,      0.0,      0.0;
@@ -65,18 +65,18 @@ RomeoSimpleActuator::RomeoSimpleActuator()
 }
 
 
-V4 RomeoSimpleActuator::computeNextState(double dt,V4& X,double& U)
+stateVec_t RomeoSimpleActuator::computeNextState(double dt,stateVec_t& X,commandVec_t& U)
 {
-    M4 Ad = (this->A*dt+this->Id);
-    V4 Bd = dt*this->B;
-    V4 result = Ad*X + Bd*U;
+    stateMat_t Ad = (this->A*dt+this->Id);
+    stateR_commandC_t Bd = dt*this->B;
+    stateVec_t result = Ad*X + Bd*U;
     result(1,0)+=this->A13atan*atan(this->a*X(3,0));
     result(3,0)+=this->A33atan*atan(this->a*X(3,0));
 
     return result;
 }
 
-void RomeoSimpleActuator::computeAllModelDeriv(double dt,V4& X,double& U)
+void RomeoSimpleActuator::computeAllModelDeriv(double dt,stateVec_t& X,commandVec_t& U)
 {
     this->fx = this->fxBase;
 
@@ -107,32 +107,32 @@ unsigned int RomeoSimpleActuator::getCommandNb()
     return this->commandNb;
 }
 
-M4 RomeoSimpleActuator::getfx()
+stateMat_t RomeoSimpleActuator::getfx()
 {
     return this->fx;
 }
 
-M4* RomeoSimpleActuator::getfxx()
+stateTens_t* RomeoSimpleActuator::getfxx()
 {
-    return this->fxx;
+    return &this->fxx;
 }
 
-V4 RomeoSimpleActuator::getfu()
+stateR_commandC_t RomeoSimpleActuator::getfu()
 {
     return this->fu;
 }
 
-V4 RomeoSimpleActuator::getfuu()
+stateR_commandC_commandD_t* RomeoSimpleActuator::getfuu()
 {
-    return this->fuu;
+    return &this->fuu;
 }
 
-M4 RomeoSimpleActuator::getfxu()
+stateR_stateC_commandD_t* RomeoSimpleActuator::getfxu()
 {
-    return this->fxu;
+    return &this->fxu;
 }
 
-M4 RomeoSimpleActuator::getfux()
+stateR_commandC_stateD_t* RomeoSimpleActuator::getfux()
 {
-    return this->fux;
+    return &this->fux;
 }

qtTmp/romeosimpleactuator.h

 #ifndef ROMEOSIMPLEACTUATOR_H
 #define ROMEOSIMPLEACTUATOR_H
 
-#include <iostream>
+#include "config.h"
 
 #include "dynamicmodel.h"
 #include <Eigen/Core>
 
 using namespace Eigen;
 
-typedef Matrix<double,4,4> M4;
-typedef Matrix<double,4,1> V4;
-
 class RomeoSimpleActuator : public DynamicModel
 {
 public:
@@ -30,31 +27,32 @@ private:
     double fvm;
     double Cf0;
     double a;
-    M4 Id;
-    M4 A;
-    V4 B;
+    stateMat_t Id;
+    stateMat_t A;
+    stateR_commandC_t B;
     double A13atan;
     double A33atan;
-    M4 fx,fxBase;
-    M4 fxx[4];
-    V4 fu,fuBase;
-    V4 fuu;
-    M4 fxu,fux;
+    stateMat_t fx,fxBase;
+    stateTens_t fxx;
+    stateR_commandC_t fu,fuBase;
+    stateR_commandC_commandD_t fuu;
+    stateR_stateC_commandD_t fxu;
+    stateR_commandC_stateD_t fux;
 
 protected:
     // methods //
 public:
-    V4 computeNextState(double dt,V4& X,double& U);
-    void computeAllModelDeriv(double dt,V4& X,double& U);
+    stateVec_t computeNextState(double dt,stateVec_t& X,commandVec_t &U);
+    void computeAllModelDeriv(double dt,stateVec_t& X,commandVec_t &U);
     // accessors //
     unsigned int getStateNb();
     unsigned int getCommandNb();
-    M4 getfx();
-    M4* getfxx();
-    V4 getfu();
-    V4 getfuu();
-    M4 getfxu();
-    M4 getfux();
+    stateMat_t getfx();
+    stateTens_t* getfxx();
+    stateR_commandC_t getfu();
+    stateR_commandC_commandD_t* getfuu();
+    stateR_stateC_commandD_t* getfxu();
+    stateR_commandC_stateD_t* getfux();
 private:
 protected: