Commit d295829d authored by flforget's avatar flforget
Browse files

Merge pull request #1 from flforget/qtTmp

Qt tmp
parents d2451934 ba7e1e46
_build
*.user.*
*.user
*/*.user
.idea
project(DDP C CXX)
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
cmake_minimum_required(VERSION 2.8)
add_subdirectory(cpp)
ADD_SUBDIRECTORY(cpp)
add_subdirectory(src)
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})
include_directories(include)
file(GLOB include_H . *.h)
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)
INSTALL(FILES ${include_H} 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 <Eigen/Core>
#include "config.h"
#include <Eigen/Dense>
using namespace Eigen;
class DynamicModel
{
......@@ -14,16 +18,29 @@ public:
private:
unsigned int stateNb;
unsigned int commandNb;
double dt;
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 unsigned int getStateNb();
virtual unsigned int getCommandNb();
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/Core>
#include <Eigen/Dense>
using namespace Eigen;
class ILQRSolver
{
public:
ILQRSolver(DynamicModel myDynamicModel, CostFunction myCostFunction);
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;
DynamicModel* dynamicModel;
CostFunction* costFunction;
unsigned int stateNb;
unsigned int commandNb;
Eigen::VectorXf xInit;
Eigen::VectorXf xDes;
stateVec_t x;
commandVec_t u;
stateVec_t xInit;
stateVec_t xDes;
unsigned int T;
double dt;
unsigned int iter;
double dt;
unsigned int iterMax;
double stopCrit;
double changeAmount;
Eigen::VectorXf* xList;
Eigen::VectorXf* uList;
Eigen::VectorXf* updatedxList;
Eigen::VectorXf* updateduList;
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(Eigen::VectorXf myxInit, Eigen::VectorXf myxDes, unsigned int myT,
double mydt, unsigned int myiterMax,double mystopCrit);
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:
......
#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/Core>
#include <Eigen/Dense>
using namespace Eigen;
class RomeoSimpleActuator : public DynamicModel
{
......@@ -14,23 +18,53 @@ protected:
// attributes //
public:
private:
unsigned int stateNb;
unsigned int commandNb;
double k;
double R;
double Jm;
double Jl;
double Cf0;
double a;
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:
//methods //
// accessors //
public:
void computeNextState(unsigned int dt,Eigen::VectorXd X,unsigned int U);
void computeAllModelDeriv(unsigned int dt,unsigned int X,unsigned int U);
unsigned int getStateNb();
unsigned int getCommandNb();
private:
protected:
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();
};
......
include_directories(include)
ADD_LIBRARY(DDP ${SRC})
install(TARGETS DDP DESTINATION lib)
SET(PROJECT_LIBS DDP)
add_executable(execmain main.cpp)
TARGET_LINK_LIBRARIES(execmain ${PROJECT_LIBS})
install(TARGETS execmain DESTINATION bin)
#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 "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;
}
......@@ -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()
{
}
#include "ilqrsolver.h"
ILQRSolver::ILQRSolver(DynamicModel myDynamicModel, CostFunction myCostFunction)
/* Debug */
#include <iostream>
using namespace std;
/* */
using namespace Eigen;
ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction)
{
this->dynamicModel = myDynamicModel;
this->costFunction = myCostFunction;
this->stateNb = myDynamicModel.getStateNb();
this->commandNb = myDynamicModel.getCommandNb();
dynamicModel = &myDynamicModel;
costFunction = &myCostFunction;
stateNb = myDynamicModel.getStateNb();
commandNb = myDynamicModel.getCommandNb();
}
void ILQRSolver::initSolver(Eigen::VectorXf myxInit, Eigen::VectorXf myxDes, unsigned int myT,
double mydt, unsigned int myiterMax,double mystopCrit)
void ILQRSolver::initSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned int& myT,
double& mydt, unsigned int& myiterMax,double& mystopCrit)
{
this->xInit = myxInit;
this->xDes = myxDes;