Commit a4430f39 authored by flforget's avatar flforget
Browse files

implement MPC + minor adjustments

parent 71308c3f
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})
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)
#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
#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
#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
#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
#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
#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
#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
#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)
#include "costfunction.h"
CostFunction::CostFunction()
{
}
#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;
}
#include "dynamicmodel.h"
DynamicModel::DynamicModel()
{
}
#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];