Skip to content
GitLab
Commit 451068fb authored by flforget's avatar flforget
Browse files

[major]Add Qpbox 

[minor]several modifications on class handle
parent 7d076143
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 451068fb
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
# INCLUDE(cmake/base.cmake)
# INCLUDE(cmake/eigen.cmake)
SET(CXX_DISABLE_WERROR True)
# SET(CMAKE_VERBOSE_MAKEFILE True)
SET(PROJECT_NAME DDPsolver)
SET(PROJECT_DESCRIPTION "DDP/iLQR solver for robotics actuators command")
SET(PROJECT_URL "")
# ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.0.5")
# SETUP_PROJECT()
LIST(APPEND CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_SOURCE_DIR}/private_cmake)
MESSAGE(STATUS "CMAKE_MODULE_PATH: ${CMAKE_MODULE_PATH}")
FIND_PACKAGE(qpOASES)
SEARCH_FOR_QPOASES()
SET(LIBRARY_OUTPUT_PATH ${PROJECT_NAME}/lib)
ADD_SUBDIRECTORY(cpp)
cpp/CMakeLists.txt
View file @ 451068fb
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)
......@@ -23,3 +14,8 @@ FILE(
ADD_EXECUTABLE(main test/main.cpp ${source_files})
ADD_EXECUTABLE(mainMPC test/mainMPC.cpp ${source_files})
ADD_EXECUTABLE(testModel test/testModel.cpp ${source_files})
TARGET_LINK_LIBRARIES(main ${qpOASES_LIBRARIES})
TARGET_LINK_LIBRARIES(mainMPC ${qpOASES_LIBRARIES})
TARGET_LINK_LIBRARIES(testModel ${qpOASES_LIBRARIES})
cpp/include/CMakeLists.txt
View file @ 451068fb
......@@ -4,6 +4,7 @@ SET(${PROJECT_NAME}_HEADERS
config.h
dynamicmodel.h
romeosimpleactuator.h
romeoactuatorpos.h
costfunction.h
ilqrsolver.h
)
......
cpp/include/costfunction.h
View file @ 451068fb
......@@ -12,8 +12,15 @@ protected:
// attributes //
public:
private:
double dt;
protected:
double dt;
stateVec_t lx;
stateMat_t lxx;
commandVec_t lu;
commandMat_t luu;
commandR_stateC_t lux;
stateR_commandC_t lxu;
// methods //
public:
virtual void computeAllCostDeriv(const stateVec_t& X, const commandVec_t& U)=0;
......@@ -22,12 +29,12 @@ 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;
stateVec_t& getlx();
stateMat_t& getlxx();
commandVec_t& getlu();
commandMat_t& getluu();
commandR_stateC_t& getlux();
stateR_commandC_t& getlxu();
};
#endif // COSTFUNCTION_H
cpp/include/costfunctionromeoactuator.h
View file @ 451068fb
......@@ -16,12 +16,6 @@ public:
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 //
......@@ -37,12 +31,7 @@ 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
View file @ 451068fb
......@@ -11,17 +11,29 @@ class DynamicModel
{
// constructors //
public:
DynamicModel();
//DynamicModel();
// attributes //
public:
private:
protected:
unsigned int stateNb;
unsigned int commandNb;
double dt;
commandVec_t lowerCommandBounds;
commandVec_t upperCommandBounds;
stateMat_t fx;
stateTens_t fxx;
stateR_commandC_t fu;
stateR_commandC_commandD_t fuu;
stateR_stateC_commandD_t fxu;
stateR_commandC_stateD_t fux;
public:
protected:
// methods //
public:
virtual stateVec_t computeNextState(double& dt, const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t& U)=0;
......@@ -33,14 +45,16 @@ 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;
unsigned int getStateNb();
unsigned int getCommandNb();
commandVec_t& getLowerCommandBounds();
commandVec_t& getUpperCommandBounds();
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 // DYNAMICMODEL_H
cpp/include/ilqrsolver.h
View file @ 451068fb
......@@ -6,8 +6,11 @@
#include "dynamicmodel.h"
#include "costfunction.h"
#include <Eigen/Dense>
#include <qpOASES.hpp>
#include <qpOASES/QProblemB.hpp>
using namespace Eigen;
USING_NAMESPACE_QPOASES
class ILQRSolver
{
......@@ -20,7 +23,7 @@ public:
};
public:
ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction);
ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction,bool QPBox=0);
private:
protected:
// attributes //
......@@ -68,6 +71,17 @@ private:
stateMat_t muEye;
unsigned char completeBackwardFlag;
/* QP variables */
QProblemB* qp;
bool enableQPBox;
commandMat_t H;
commandVec_t g;
commandVec_t lowerCommandBounds;
commandVec_t upperCommandBounds;
commandVec_t lb;
commandVec_t ub;
int nWSR;
real_t* xOpt;
protected:
// methods //
public:
......@@ -75,12 +89,12 @@ public:
double& mydt, unsigned int& myiterMax,double& mystopCrit);
void initSolver(stateVec_t& myxInit, stateVec_t& myxDes);
void solveTrajectory();
struct traj getLastSolvedTrajectory();
//private:
void initTrajectory();
void backwardLoop();
void forwardLoop();
bool isQuudefinitePositive(const commandMat_t & Quu);
struct traj getLastSolvedTrajectory();
private:
bool isQuudefinitePositive(const commandMat_t & Quu);
protected:
};
......
cpp/include/romeolinearactuator.h
View file @ 451068fb
......@@ -19,29 +19,20 @@ protected:
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 unsigned int stateNb=4;
//static const unsigned int commandNb=1;
public:
static const double k;
static const double R;
static const double Jm;
static const double Jl;
double fvm;
static const double Cf0=0.1;
static const double a=10.0;
static const double Cf0;
static const double a;
private:
stateMat_t Id;
stateMat_t A;
stateMat_t Ad;
stateR_commandC_t B;
stateR_commandC_t Bd;
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 //
......@@ -55,14 +46,6 @@ 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();
};
......
cpp/include/romeosimpleactuator.h
View file @ 451068fb
......@@ -19,16 +19,19 @@ protected:
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;
//static const unsigned int stateNb=4;
//static const unsigned int commandNb=1;
public:
static const double k;
static const double R;
static const double Jm;
static const double Jl;
static const double fvm;
static const double Cf0;
static const double a;
//commandVec_t lowerCommandBounds;
//commandVec_t upperCommandBounds;
private:
stateVec_t Xreal;
stateMat_t Id;
stateMat_t A;
......@@ -37,12 +40,8 @@ private:
stateR_commandC_t Bd;
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 fxBase;
stateR_commandC_t fuBase;
stateMat_t QxxCont;
commandMat_t QuuCont;
......@@ -60,14 +59,6 @@ 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();
};
......
cpp/src/costfunction.cpp
View file @ 451068fb
......@@ -3,3 +3,33 @@
CostFunction::CostFunction()
{
}
stateVec_t& CostFunction::getlx()
{
return lx;
}
stateMat_t& CostFunction::getlxx()
{
return lxx;
}
commandVec_t& CostFunction::getlu()
{
return lu;
}
commandMat_t& CostFunction::getluu()
{
return luu;
}
commandR_stateC_t& CostFunction::getlux()
{
return lux;
}
stateR_commandC_t& CostFunction::getlxu()
{
return lxu;
}
cpp/src/costfunctionromeoactuator.cpp
View file @ 451068fb
......@@ -18,43 +18,12 @@ CostFunctionRomeoActuator::CostFunctionRomeoActuator()
void CostFunctionRomeoActuator::computeAllCostDeriv(const stateVec_t& X, const commandVec_t& U)
{
// lx = Q*(X-Xdes);
lx(0,0) = 100.0*X(0,0);
lx = Q*X;
lu = R*U;
}
void CostFunctionRomeoActuator::computeFinalCostDeriv(const stateVec_t& X)
{
// lx = Q*(X-Xdes);
lx(0,0) = 100.0*X(0,0);
}
// accessors //
stateVec_t& CostFunctionRomeoActuator::getlx()
{
return lx;
}
stateMat_t& CostFunctionRomeoActuator::getlxx()
{
return lxx;
}
commandVec_t& CostFunctionRomeoActuator::getlu()
{
return lu;
lx = Q*X;
}
commandMat_t& CostFunctionRomeoActuator::getluu()
{
return luu;
}
stateR_commandC_t& CostFunctionRomeoActuator::getlxu()
{
return lxu;
}
commandR_stateC_t& CostFunctionRomeoActuator::getlux()
{
return lux;
}
cpp/src/dynamicmodel.cpp
View file @ 451068fb
#include "dynamicmodel.h"
DynamicModel::DynamicModel()
/*DynamicModel::DynamicModel()
{
}*/
unsigned int DynamicModel::getStateNb()
{
return stateNb;
}
unsigned int DynamicModel::getCommandNb()
{
return commandNb;
}
commandVec_t& DynamicModel::getLowerCommandBounds()
{
return lowerCommandBounds;
}
commandVec_t& DynamicModel::getUpperCommandBounds()
{
return upperCommandBounds;
}
stateMat_t& DynamicModel::getfx()
{
return fx;
}
stateTens_t& DynamicModel::getfxx()
{
return fxx;
}
stateR_commandC_t& DynamicModel::getfu()
{
return fu;
}
stateR_commandC_commandD_t& DynamicModel::getfuu()
{
return fuu;
}
stateR_stateC_commandD_t& DynamicModel::getfxu()
{
return fxu;
}
stateR_commandC_stateD_t& DynamicModel::getfux()
{
return fux;
}
cpp/src/ilqrsolver.cpp
View file @ 451068fb
......@@ -7,12 +7,28 @@ using namespace std;
using namespace Eigen;
ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction)
ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction,bool QPBox)
{
dynamicModel = &myDynamicModel;
costFunction = &myCostFunction;
stateNb = myDynamicModel.getStateNb();
commandNb = myDynamicModel.getCommandNb();
enableQPBox = QPBox;
if(QPBox)
{
qp = new QProblemB(commandNb);
Options myOptions;
myOptions.printLevel = PL_LOW;
myOptions.enableRegularisation = BT_TRUE;
myOptions.initialStatusBounds = ST_INACTIVE;
myOptions.numRefinementSteps = 1;
myOptions.enableCholeskyRefactorisation = 1;
qp->setOptions(myOptions);
xOpt = new real_t[commandNb];
lowerCommandBounds = myDynamicModel.getLowerCommandBounds();
upperCommandBounds = myDynamicModel.getUpperCommandBounds();
}
}
void ILQRSolver::FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned int& myT,
......@@ -58,7 +74,9 @@ void ILQRSolver::solveTrajectory()
backwardLoop();
forwardLoop();
if(changeAmount<stopCrit)
{
break;
}
tmpxPtr = xList;
tmpuPtr = uList;
xList = updatedxList;
......@@ -111,6 +129,7 @@ void ILQRSolver::backwardLoop()
Qux += dynamicModel->computeTensorContux(nextVx);
Quu += dynamicModel->computeTensorContuu(nextVx);
QuuInv = Quu.inverse();
if(!isQuudefinitePositive(Quu))
{
......@@ -123,12 +142,39 @@ void ILQRSolver::backwardLoop()
break;
}
QuuInv = Quu.inverse();
k = -QuuInv*Qu;
K = -QuuInv*Qux;
if(enableQPBox)
{
nWSR = 10;
H = Quu;
g = Qu;
lb = lowerCommandBounds - u;
ub = upperCommandBounds - u;
qp->init(H.data(),g.data(),lb.data(),ub.data(),nWSR);
//cout << i<<'\t' <<H <<'\t'<< g<<'\t' << lb<<'\t' << ub <<endl;
qp->getPrimalSolution(xOpt);
k = Map<commandVec_t>(xOpt);
//cout << k << endl<<'-'<<endl;
if((k == lowerCommandBounds) | (k == upperCommandBounds))
{
K.setZero();
cout << iter << " : "<< i << " -> " <<"bounded" << endl;
}
else
{
K = -QuuInv*Qux; // to be modified
}
}
else
{
k = -QuuInv*Qu;
K = -QuuInv*Qux;
}
nextVx = Qx - K.transpose()*Quu*k;
nextVxx = Qxx - K.transpose()*Quu*K;
/*nextVx = Qx - K.transpose()*Quu*k;
nextVxx = Qxx - K.transpose()*Quu*K;*/
nextVx = Qx + K.transpose()*Quu*k + K.transpose()*Qu + Qux.transpose()*k;
nextVxx = Qxx + K.transpose()*Quu*K+ K.transpose()*Qux + Qux.transpose()*K;
nextVxx = 0.5*(nextVxx + nextVxx.transpose());
kList[i] = k;
KList[i] = K;
......@@ -155,6 +201,10 @@ 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/src/romeolinearactuator.cpp
View file @ 451068fb
......@@ -3,48 +3,50 @@
#define pi M_PI
const double RomeoLinearActuator::k=1000.0;
const double RomeoLinearActuator::R=200.0;
const double RomeoLinearActuator::Jm=138*1e-7;
const double RomeoLinearActuator::Jl=0.1;
const double RomeoLinearActuator::Cf0=0.1;
const double RomeoLinearActuator::a=10.0;
RomeoLinearActuator::RomeoLinearActuator(double& mydt)
{
stateNb=4;
commandNb=1;
dt = mydt;
Id.setIdentity();
A.setZero();
A(0,1) = 1.0;
A(2,2) = 1.0;
A(1,0) = -(k/Jl)+(k/(Jm*R*R));
A(3,0) =1.0/Jl;
Ad = (A*dt+Id);
A << 0.0, 1.0, 0.0, 0.0;
-(k/Jl)-(k/(Jm*R*R)), 0.0, 0.0, 0.0;
0.0, 0.0, 0.0, 1.0;
1.0/Jl, 0.0, 0.0, 0.0;
fx = (A*dt+Id);
B << 0.0,
k/(R*Jm),