implement MPC + minor adjustments

cpp/CMakeLists.txt

+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+SET(CXX_DISABLE_WERROR True)
+
+SET(PROJECT_NAME DDPsolver)
+# SET(PROJECT_DESCRIPTION "DDP/iLQR solver for robotics actuators command")
+# SET(PROJECT_URL "")
+
+SET(LIBRARY_OUTPUT_PATH ${PROJECT_NAME}/lib)
+
+ADD_SUBDIRECTORY(src)
+ADD_SUBDIRECTORY(include)
+ADD_SUBDIRECTORY(test)
+
+INCLUDE_DIRECTORIES(include)
+INCLUDE_DIRECTORIES(/usr/include/eigen3)
+
+FILE(
+    GLOB_RECURSE
+    source_files
+    include/*
+    src/*
+)
+
+ADD_EXECUTABLE(main test/main.cpp ${source_files})
+ADD_EXECUTABLE(mainMPC test/mainMPC.cpp ${source_files})
+

cpp/include/CMakeLists.txt

+SET(${PROJECT_NAME}_HEADERS
+    costfunctionromeoactuator.h
+    romeolinearactuator.h
+    config.h
+    dynamicmodel.h
+    romeosimpleactuator.h
+    costfunction.h
+    ilqrsolver.h
+)
+
+INSTALL(FILES ${${PROJECT_NAME}_HEADER} DESTINATION include)
+

cpp/include/config.h

+#ifndef CONFIG_H
+#define CONFIG_H
+
+#include <Eigen/Dense>
+
+#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

cpp/include/costfunction.h

+#ifndef COSTFUNCTION_H
+#define COSTFUNCTION_H
+
+#include "config.h"
+
+class CostFunction
+{
+public:
+    CostFunction();
+private:
+protected:
+    // attributes //
+public:
+private:
+    double dt;
+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;
+private:
+protected:
+    // accessors //
+public:
+    virtual stateVec_t getlx()=0;
+    virtual stateMat_t getlxx()=0;
+    virtual commandVec_t getlu()=0;
+    virtual commandMat_t getluu()=0;
+    virtual commandR_stateC_t getlux()=0;
+    virtual stateR_commandC_t getlxu()=0;
+};
+
+#endif // COSTFUNCTION_H

cpp/include/costfunctionromeoactuator.h

+#ifndef COSTFUNCTIONROMEOACTUATOR_H
+#define COSTFUNCTIONROMEOACTUATOR_H
+
+#include "config.h"
+
+#include "costfunction.h"
+
+#include <Eigen/Dense>
+
+using namespace Eigen;
+
+class CostFunctionRomeoActuator : public CostFunction
+{
+public:
+    CostFunctionRomeoActuator();
+private:
+    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;
+    double dt;
+protected:
+    // attributes //
+public:
+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);
+private:
+protected:
+    // accessors //
+public:
+    stateVec_t getlx();
+    stateMat_t getlxx();
+    commandVec_t getlu();
+    commandMat_t getluu();
+    commandR_stateC_t getlux();
+    stateR_commandC_t getlxu();
+};
+
+#endif // COSTFUNCTIONROMEOACTUATOR_H

cpp/include/dynamicmodel.h

+#ifndef DYNAMICMODEL_H
+#define DYNAMICMODEL_H
+
+#include "config.h"
+
+#include <Eigen/Dense>
+
+using namespace Eigen;
+
+class DynamicModel
+{
+// constructors //
+public:
+    DynamicModel();
+
+// attributes //
+public:
+private:
+    unsigned int stateNb;
+    unsigned int commandNb;
+    double dt;
+
+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 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;
+private:
+protected:
+    // accessors //
+public:
+    virtual unsigned int getStateNb()=0;
+    virtual unsigned int getCommandNb()=0;
+    virtual stateMat_t &getfx()=0;
+    virtual stateTens_t& getfxx()=0;
+    virtual stateR_commandC_t &getfu()=0;
+    virtual stateR_commandC_commandD_t& getfuu()=0;
+    virtual stateR_stateC_commandD_t& getfxu()=0;
+    virtual stateR_commandC_stateD_t& getfux()=0;
+};
+
+#endif // DYNAMICMODEL_H

cpp/include/ilqrsolver.h

+#ifndef ILQRSOLVER_H
+#define ILQRSOLVER_H
+
+#include "config.h"
+
+#include "dynamicmodel.h"
+#include "costfunction.h"
+#include <Eigen/Dense>
+
+using namespace Eigen;
+
+class ILQRSolver
+{
+public:
+    struct traj
+    {
+        stateVec_t* xList;
+        commandVec_t* uList;
+        unsigned int iter;
+    };
+
+public:
+    ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction);
+private:
+protected:
+    // attributes //
+public:
+private:
+    DynamicModel* dynamicModel;
+    CostFunction* costFunction;
+    unsigned int stateNb;
+    unsigned int commandNb;
+    stateVec_t x;
+    commandVec_t u;
+    stateVec_t xInit;
+    stateVec_t xDes;
+    unsigned int T;
+    unsigned int iter;
+    double dt;
+    unsigned int iterMax;
+    double stopCrit;
+    double changeAmount;
+
+    stateVec_t* xList;
+    commandVec_t* uList;
+    stateVec_t* updatedxList;
+    commandVec_t* updateduList;
+    struct traj lastTraj;
+
+    stateVec_t nextVx;
+    stateMat_t nextVxx;
+    stateVec_t Qx;
+    stateMat_t Qxx;
+    commandVec_t Qu;
+    commandMat_t Quu;
+    commandMat_t QuuInv;
+    commandR_stateC_t Qux;
+    commandVec_t k;
+    commandR_stateC_t K;
+    commandVec_t* kList;
+    commandR_stateC_t* KList;
+    double alphaList[5];
+    double alpha;
+
+
+
+    double mu;
+    stateMat_t muEye;
+    unsigned char completeBackwardFlag;
+
+protected:
+    // methods //
+public:
+    void initSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned int& myT,
+                    double& mydt, unsigned int& myiterMax,double& mystopCrit);
+    void solveTrajectory();
+    void initTrajectory();
+    void backwardLoop();
+    void forwardLoop();
+    char isQuudefinitePositive(commandMat_t& Quu);
+    struct traj getLastSolvedTrajectory();
+private:
+protected:
+
+};
+
+#endif // ILQRSOLVER_H

cpp/include/romeolinearactuator.h

+#ifndef ROMEOLINEARACTUATOR_H
+#define ROMEOLINEARACTUATOR_H
+
+#include "config.h"
+
+#include "dynamicmodel.h"
+#include <Eigen/Dense>
+
+using namespace Eigen;
+
+class RomeoLinearActuator : public DynamicModel
+{
+public:
+    RomeoLinearActuator();
+private:
+protected:
+
+    // attributes //
+public:
+private:
+    double dt;
+    static const unsigned int stateNb=4;
+    static const unsigned int commandNb=1;
+    static const double k=1000.0;
+    static const double R=200.0;
+    static const double Jm=138*1e-7;
+    static const double Jl=0.1;
+    double fvm;
+    static const double Cf0=0.1;
+    static const double a=10.0;
+
+    stateMat_t Id;
+    stateMat_t A;
+    stateR_commandC_t B;
+    double A13atan;
+    double A33atan;
+    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:
+    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);
+    stateMat_t computeTensorContxx(const stateVec_t& nextVx);
+    commandMat_t computeTensorContuu(const stateVec_t& nextVx);
+    commandR_stateC_t computeTensorContux(const stateVec_t& nextVx);
+private:
+protected:
+        // accessors //
+public:
+    unsigned int getStateNb();
+    unsigned int getCommandNb();
+    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();
+
+};
+
+#endif // ROMEOLINEARACTUATOR_H

cpp/include/romeosimpleactuator.h

+#ifndef ROMEOSIMPLEACTUATOR_H
+#define ROMEOSIMPLEACTUATOR_H
+
+#include "config.h"
+
+#include "dynamicmodel.h"
+#include <Eigen/Dense>
+
+using namespace Eigen;
+
+class RomeoSimpleActuator : public DynamicModel
+{
+public:
+    RomeoSimpleActuator();
+private:
+protected:
+
+    // attributes //
+public:
+private:
+    double dt;
+    static const unsigned int stateNb=4;
+    static const unsigned int commandNb=1;
+    static const double k=1000.0;
+    static const double R=200.0;
+    static const double Jm=138*1e-7;
+    static const double Jl=0.1;
+    static const double fvm=0.01;
+    static const double Cf0=0.1;
+    static const double a=10.0;
+
+    stateMat_t Id;
+    stateMat_t A;
+    stateR_commandC_t B;
+    double A13atan;
+    double A33atan;
+    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;
+
+    stateMat_t QxxCont;
+    commandMat_t QuuCont;
+    commandR_stateC_t QuxCont;
+
+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);
+    stateMat_t computeTensorContxx(const stateVec_t& nextVx);
+    commandMat_t computeTensorContuu(const stateVec_t& nextVx);
+    commandR_stateC_t computeTensorContux(const stateVec_t& nextVx);
+private:
+protected:
+        // accessors //
+public:
+    unsigned int getStateNb();
+    unsigned int getCommandNb();
+    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();
+
+};
+
+#endif // ROMEOSIMPLEACTUATOR_H

cpp/src/CMakeLists.txt

+#SET(LIBRARY_NAME ${PROJECT_NAME})
+# SET(${LIBRARY_NAME}_SOURCES
+#    dynamicmodel.cpp
+#    costfunction.cpp
+#    romeolinearactuator.cpp
+#    costfunctionromeoactuator.cpp
+#    romeosimpleactuator.cpp
+#)
+
+#ADD_LIBRARY(${LIBRARY_NAME}
+#    SHARED
+#    ${${LIBRARY_NAME}_SOURCES}
+#    )
+
+#INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION lib)

cpp/src/costfunction.cpp

+#include "costfunction.h"
+
+CostFunction::CostFunction()
+{
+}

cpp/src/costfunctionromeoactuator.cpp

+#include "costfunctionromeoactuator.h"
+
+CostFunctionRomeoActuator::CostFunctionRomeoActuator()
+{
+    Q << 100.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;
+    R << 0.1;
+
+    lxx = Q;
+    luu = R;
+    lux << 0.0,0.0,0.0,0.0;
+    lxu << 0.0,0.0,0.0,0.0;
+}
+
+void CostFunctionRomeoActuator::computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U)
+{
+    lx = Q*(X-Xdes);
+    lu = R*U;
+}
+
+void CostFunctionRomeoActuator::computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes)
+{
+    lx = Q*(X-Xdes);
+}
+
+// accessors //
+stateVec_t CostFunctionRomeoActuator::getlx()
+{
+    return lx;
+}
+
+stateMat_t CostFunctionRomeoActuator::getlxx()
+{
+    return lxx;
+}
+
+commandVec_t CostFunctionRomeoActuator::getlu()
+{
+    return lu;
+}
+
+commandMat_t CostFunctionRomeoActuator::getluu()
+{
+    return luu;
+}
+
+stateR_commandC_t CostFunctionRomeoActuator::getlxu()
+{
+    return lxu;
+}
+
+commandR_stateC_t CostFunctionRomeoActuator::getlux()
+{
+    return lux;
+}

cpp/src/dynamicmodel.cpp

+#include "dynamicmodel.h"
+
+DynamicModel::DynamicModel()
+{
+}

cpp/src/ilqrsolver.cpp

+#include "ilqrsolver.h"
+
+/* Debug */
+#include <iostream>
+using namespace std;
+/* */
+
+using namespace Eigen;
+
+ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction)
+{
+    dynamicModel = &myDynamicModel;
+    costFunction = &myCostFunction;
+    stateNb = myDynamicModel.getStateNb();
+    commandNb = myDynamicModel.getCommandNb();
+}
+
+void ILQRSolver::initSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned int& myT,
+                       double& mydt, unsigned int& myiterMax,double& mystopCrit)
+{
+    xInit = myxInit;
+    xDes = myxDes;
+    T = myT;
+    dt = mydt;
+    iterMax = myiterMax;
+    stopCrit = mystopCrit;
+
+    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];
+
+    alphaList[0] = 1.0;
+    alphaList[1] = 0.8;
+    alphaList[2] = 0.6;
+    alphaList[3] = 0.4;
+    alphaList[4] = 0.2;
+    alpha = 1.0;
+}
+
+void ILQRSolver::solveTrajectory()
+{
+    stateVec_t* tmpxPtr;
+    commandVec_t* tmpuPtr;
+    initTrajectory();
+    for(iter=0;iter<iterMax;iter++)
+    {
+        backwardLoop();
+        forwardLoop();
+        if(changeAmount<stopCrit)
+            break;
+        tmpxPtr = xList;
+        tmpuPtr = uList;
+        xList = updatedxList;
+        updatedxList = tmpxPtr;
+        uList = updateduList;
+        updateduList = tmpuPtr;
+    }
+}
+
+void ILQRSolver::initTrajectory()
+{
+    xList[0] = xInit;
+    commandVec_t zeroCommand;
+    zeroCommand.setZero();
+    for(unsigned int i=0;i<T;i++)
+    {
+        uList[i] = zeroCommand;
+        xList[i+1] = dynamicModel->computeNextState(dt,xList[i],zeroCommand);
+    }
+}
+
+void ILQRSolver::backwardLoop()
+{
+    costFunction->computeFinalCostDeriv(xList[T],xDes);
+    nextVx = costFunction->getlx();
+    nextVxx = costFunction->getlxx();
+
+    mu = 0.0;
+    completeBackwardFlag = 0;
+
+    while(!completeBackwardFlag)
+    {
+        completeBackwardFlag = 1;
+        muEye = mu*stateMat_t::Zero();
+        for(int i=T-1;i>=0;i--)
+        {
+            x = xList[i];
+            u = uList[i];