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

implementation finalization + tests

parent e6bbaa6c
Hide whitespace changes
Inline Side-by-side
python/testiLQR.py
View file @ b0b9a885
......@@ -13,10 +13,10 @@ Xinit = np.matrix([ [0.0],
[0.0],
[0.0],
[0.0]])
Xinit = np.matrix([ [uniform(-10,10)],
[uniform(-1.0,1.0)],
[uniform(-10,10)],
[uniform(-1.0,1.0)]])
# Xinit = np.matrix([ [uniform(-10,10)],
# [uniform(-1.0,1.0)],
# [uniform(-10,10)],
# [uniform(-1.0,1.0)]])
Xdes = np.matrix([ [1.0],
[0.0],
[0.0],
......@@ -28,7 +28,7 @@ XListtmp = list()
UListtmp = list()
timeList = list()
N = 20
N = 3
"""Debug"""
traj = False
......
qtTmp/costfunction.h
View file @ b0b9a885
......@@ -12,11 +12,12 @@ protected:
// attributes //
public:
private:
double dt;
protected:
// methods //
public:
virtual void computeAllCostDeriv(stateVec_t& X, stateVec_t& Xdes, commandVec_t& U)=0;
virtual void commuteFinalCostDeriv(stateVec_t& X, stateVec_t& Xdes)=0;
virtual void computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U)=0;
virtual void commuteFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes)=0;
private:
protected:
// accessors //
......
qtTmp/costfunctionromeoactuator.cpp
View file @ b0b9a885
......@@ -14,13 +14,13 @@ CostFunctionRomeoActuator::CostFunctionRomeoActuator()
lxu << 0.0,0.0,0.0,0.0;
}
void CostFunctionRomeoActuator::computeAllCostDeriv(stateVec_t& X, stateVec_t& Xdes, commandVec_t& U)
void CostFunctionRomeoActuator::computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U)
{
lx = Q*(X-Xdes);
lu = R*U;
}
void CostFunctionRomeoActuator::commuteFinalCostDeriv(stateVec_t& X, stateVec_t& Xdes)
void CostFunctionRomeoActuator::commuteFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes)
{
lx = Q*(X-Xdes);
}
......
qtTmp/costfunctionromeoactuator.h
View file @ b0b9a885
......@@ -22,6 +22,7 @@ private:
commandMat_t luu;
commandR_stateC_t lux;
stateR_commandC_t lxu;
double dt;
protected:
// attributes //
public:
......@@ -30,8 +31,8 @@ private:
protected:
// methods //
public:
void computeAllCostDeriv(stateVec_t& X, stateVec_t& Xdes, commandVec_t& U);
void commuteFinalCostDeriv(stateVec_t& X, stateVec_t& Xdes);
void computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U);
void commuteFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes);
private:
protected:
// accessors //
......
qtTmp/dynamicmodel.h
View file @ b0b9a885
......@@ -18,13 +18,21 @@ public:
private:
unsigned int stateNb;
unsigned int commandNb;
double dt;
protected:
// methods //
public:
virtual stateVec_t computeNextState(double dt,stateVec_t& X,commandVec_t& U)=0;
virtual void computeAllModelDeriv(double dt,stateVec_t& X,commandVec_t& U)=0;
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;
......@@ -33,8 +41,6 @@ public:
virtual stateR_commandC_commandD_t* getfuu()=0;
virtual stateR_stateC_commandD_t* getfxu()=0;
virtual stateR_commandC_stateD_t* getfux()=0;
private:
protected:
};
#endif // DYNAMICMODEL_H
qtTmp/ilqrsolver.cpp
View file @ b0b9a885
......@@ -15,8 +15,8 @@ ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunctio
commandNb = myDynamicModel.getCommandNb();
}
void ILQRSolver::initSolver(stateVec_t myxInit, stateVec_t 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)
{
xInit = myxInit;
xDes = myxDes;
......@@ -44,6 +44,8 @@ void ILQRSolver::initSolver(stateVec_t myxInit, stateVec_t myxDes, unsigned int
void ILQRSolver::solveTrajectory()
{
stateVec_t* tmpxPtr;
commandVec_t* tmpuPtr;
initTrajectory();
for(iter=0;iter<iterMax;iter++)
{
......@@ -51,12 +53,12 @@ void ILQRSolver::solveTrajectory()
forwardLoop();
if(changeAmount<stopCrit)
break;
for(unsigned int i=0;i<T;i++)
{
xList[i] = updatedxList[i];
uList[i] = updateduList[i];
}
xList[T] = updatedxList[T];
tmpxPtr = xList;
tmpuPtr = uList;
xList = updatedxList;
updatedxList = tmpxPtr;
uList = updateduList;
updateduList = tmpuPtr;
}
}
......@@ -64,7 +66,7 @@ void ILQRSolver::initTrajectory()
{
xList[0] = xInit;
commandVec_t zeroCommand;
zeroCommand << commandVec_t::Zero(commandSize,1);
zeroCommand.setZero();
for(unsigned int i=0;i<T;i++)
{
uList[i] = zeroCommand;
......@@ -78,7 +80,6 @@ void ILQRSolver::backwardLoop()
nextVx = costFunction->getlx();
nextVxx = costFunction->getlxx();
mu = 0.0;
muEye.setZero();
completeBackwardFlag = 0;
......@@ -95,14 +96,13 @@ void ILQRSolver::backwardLoop()
Qx = costFunction->getlx() + dynamicModel->getfx().transpose() * nextVx;
Qu = costFunction->getlu() + dynamicModel->getfu().transpose() * nextVx;
Qxx = costFunction->getlxx() + dynamicModel->getfx().transpose() * nextVxx * dynamicModel->getfx();
Qxx = costFunction->getlxx() + dynamicModel->getfx().transpose() * (nextVxx+muEye) * dynamicModel->getfx();
Quu = costFunction->getluu() + dynamicModel->getfu().transpose() * (nextVxx+muEye) * dynamicModel->getfu();
Qux = costFunction->getlux() + dynamicModel->getfu().transpose() * (nextVxx+muEye) * dynamicModel->getfx();
/*
To be Implemented : dynamic second order derivatives
*/
Qxx += dynamicModel->computeTensorContxx(nextVx);
Qux += dynamicModel->computeTensorContux(nextVx);
Quu += dynamicModel->computeTensorContuu(nextVx);
/*
To be Implemented : Regularization (is Quu definite positive ?)
......@@ -124,13 +124,16 @@ void ILQRSolver::forwardLoop()
{
changeAmount = 0.0;
updatedxList[0] = xInit;
// Line search to be implemented
alpha = 1.0;
for(unsigned int i=0;i<T;i++)
{
updateduList[i] = uList[i] + alpha*kList[i] + KList[i]*(updatedxList[i]-xList[i]);
updatedxList[i+1] = dynamicModel->computeNextState(dt,updatedxList[i],updateduList[i]);
for(unsigned int j=0;j<commandNb;j++)
{
changeAmount += abs(uList[i](j,0) - updateduList[i](j,0));
}
}
}
......@@ -138,6 +141,6 @@ ILQRSolver::traj ILQRSolver::getLastSolvedTrajectory()
{
lastTraj.xList = updatedxList;
lastTraj.uList = updateduList;
lastTraj.iter = iter;
return lastTraj;
}
qtTmp/ilqrsolver.h
View file @ b0b9a885
......@@ -16,9 +16,9 @@ public:
{
stateVec_t* xList;
commandVec_t* uList;
unsigned int iter;
};
public:
ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction);
private:
......@@ -35,8 +35,8 @@ private:
stateVec_t xInit;
stateVec_t xDes;
unsigned int T;
double dt;
unsigned int iter;
double dt;
unsigned int iterMax;
double stopCrit;
double changeAmount;
......@@ -71,8 +71,8 @@ private:
protected:
// methods //
public:
void initSolver(stateVec_t myxInit, stateVec_t 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();
......
qtTmp/main.cpp
View file @ b0b9a885
#include <iostream>
#include <fstream>
#include "config.h"
#include "ilqrsolver.h"
#include "romeosimpleactuator.h"
#include "romeolinearactuator.h"
#include "costfunctionromeoactuator.h"
#include <Eigen/Dense>
#include <time.h>
#include <sys/time.h>
using namespace std;
using namespace Eigen;
......@@ -19,18 +22,19 @@ int main()
double texec=0.0;
stateVec_t xinit,xDes;
xinit << 1.0,0.0,0.0,0.0;
xDes << 2.0,0.0,0.0,0.0;
xinit << 0.0,0.0,0.0,0.0;
xDes << 1.0,0.0,0.0,0.0;
unsigned int T = 50;
unsigned int T = 3;
double dt=1e-4;
int iterMax = 20;
unsigned int iterMax = 20;
double stopCrit = 1e-3;
stateVec_t* xList;
commandVec_t* uList;
ILQRSolver::traj lastTraj;
RomeoSimpleActuator romeoActuatorModel;
RomeoLinearActuator romeoLinearModel;
CostFunctionRomeoActuator costRomeoActuator;
ILQRSolver testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator);
testSolverRomeoActuator.initSolver(xinit,xDes,T,dt,iterMax,stopCrit);
......@@ -42,6 +46,7 @@ int main()
lastTraj = testSolverRomeoActuator.getLastSolvedTrajectory();
xList = lastTraj.xList;
uList = lastTraj.uList;
unsigned int iter = lastTraj.iter;
texec=((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000.;
......@@ -49,9 +54,19 @@ int main()
cout << texec << endl;
cout << "temps d'execution par pas de temps ";
cout << texec/T << endl;
cout << "Nombre d'itérations : " << iter << endl;
ofstream fichier("results.csv",ios::out | ios::trunc);
if(fichier)
{
fichier << "tau,tauDot,q,qDot,u" << endl;
for(int i=0;i<T;i++) fichier << xList[i](0,0) << "," << xList[i](1,0) << "," << xList[i](2,0) << "," << xList[i](3,0) << "," << uList[i](0,0) << endl;
fichier << xList[T](0,0) << "," << xList[T](1,0) << "," << xList[T](2,0) << "," << xList[T](3,0) << "," << 0.0 << endl;
fichier.close();
}
else
cerr << "erreur ouverte fichier" << endl;
for(int i=0;i<T+1;i++) cout << uList[i];
return 0;
}
qtTmp/qtTmp.pro
View file @ b0b9a885
......@@ -8,7 +8,8 @@ SOURCES += main.cpp \
dynamicmodel.cpp \
ilqrsolver.cpp \
romeosimpleactuator.cpp \
costfunctionromeoactuator.cpp
costfunctionromeoactuator.cpp \
romeolinearactuator.cpp
SOURCES +=
include(deployment.pri)
......@@ -23,6 +24,8 @@ HEADERS += \
ilqrsolver.h \
romeosimpleactuator.h \
costfunctionromeoactuator.h \
config.h
config.h \
romeolinearactuator.h
INCLUDEPATH += /usr/include/eigen3
INCLUDEPATH += /usr/include/python2.7
qtTmp/romeolinearactuator.cpp 0 → 100644
View file @ b0b9a885
#include "romeolinearactuator.h"
#include <math.h>
#define pi M_PI
RomeoLinearActuator::RomeoLinearActuator()
{
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;
B << 0.0,
k/(R*Jm),
0.0,
0.0;
fxBase << 1.0, 1.0, 0.0, 0.0,
-(k/Jl)-(k/(Jm*R*R)), 0.0, 0.0, 0.0,
0.0, 0.0, 1.0, 1.0,
1.0/Jl, 0.0, 0.0, 0.0;
fx = fxBase;
fxx[0].setZero();
fxx[1].setZero();
fxx[2].setZero();
fxx[3].setZero();
fuBase << 0.0,
k/(R*Jm),
0.0,
0.0;
fu.setZero();
fuu[0].setZero();
fux[0].setZero();
fxu[0].setZero();
}
stateVec_t RomeoLinearActuator::computeNextState(double& dt, const stateVec_t& X,const commandVec_t& U)
{
const stateMat_t Ad = (A*dt+Id);
const stateR_commandC_t Bd = dt*B;
stateVec_t result = Ad*X + Bd*U;
return result;
}
void RomeoLinearActuator::computeAllModelDeriv(double& dt, const stateVec_t& X,const commandVec_t& U)
{
fu = dt*fuBase;
fx = (A*dt+Id);
}
stateMat_t RomeoLinearActuator::computeTensorContxx(const stateVec_t& nextVx)
{
stateMat_t QxxCont;
QxxCont.setZero();
return QxxCont;
}
commandMat_t RomeoLinearActuator::computeTensorContuu(const stateVec_t& nextVx)
{
commandMat_t QuuCont;
QuuCont.setZero();
return QuuCont;
}
commandR_stateC_t RomeoLinearActuator::computeTensorContux(const stateVec_t& nextVx)
{
commandR_stateC_t QuxCont;
QuxCont.setZero();
return QuxCont;
}
/// accessors ///
unsigned int RomeoLinearActuator::getStateNb()
{
return stateNb;
}
unsigned int RomeoLinearActuator::getCommandNb()
{
return commandNb;
}
stateMat_t& RomeoLinearActuator::getfx()
{
return fx;
}
stateTens_t* RomeoLinearActuator::getfxx()
{
return &fxx;
}
stateR_commandC_t& RomeoLinearActuator::getfu()
{
return fu;
}
stateR_commandC_commandD_t* RomeoLinearActuator::getfuu()
{
return &fuu;
}
stateR_stateC_commandD_t* RomeoLinearActuator::getfxu()
{
return &fxu;
}
stateR_commandC_stateD_t* RomeoLinearActuator::getfux()
{
return &fux;
}
qtTmp/romeolinearactuator.h 0 → 100644
View file @ b0b9a885
#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
qtTmp/romeosimpleactuator.cpp
View file @ b0b9a885
......@@ -5,23 +5,13 @@
RomeoSimpleActuator::RomeoSimpleActuator()
{
commandNb = 1;
stateNb = 4;
k = 1000.0;
R = 200.0;
Jm = 138*1e-7;
Jl = 0.1;
fvm = 0.01;
Cf0 = 0.1;
a = 10.0;
Id.setIdentity();
A.setZero();
A(0,1) = 1.0;
A(2,2) = 1.0;
A(1,0) = -(k/Jl)+(k/(Jm*R*R));
A(1,3) = -fvm*k/Jm;
A(1,3) = -(fvm*k)/Jm;
A(3,0) =1.0/Jl;
A13atan = 2.0*Jm*R/(pi*Jl)*Cf0;
A33atan = 2.0/(pi*Jl)*Cf0;
......@@ -32,7 +22,7 @@ RomeoSimpleActuator::RomeoSimpleActuator()
0.0;
fxBase << 1.0, 1.0, 0.0, 0.0,
-(k/Jl)-(k/(Jm*R*R)), -fvm/Jm, 0.0, -fvm*k/Jm,
-(k/Jl)-(k/(Jm*R*R)), -(fvm/Jm), 0.0, -(fvm*k)/Jm,
0.0, 0.0, 1.0, 1.0,
1.0/Jl, 0.0, 0.0, 0.0;
fx = fxBase;
......@@ -47,23 +37,28 @@ RomeoSimpleActuator::RomeoSimpleActuator()
0.0,
0.0;
fu.setZero();;
fuu[0].setZero();
fux[0].setZero();
fxu[0].setZero();
QxxCont.setZero();
QuuCont.setZero();
QuxCont.setZero();
}
stateVec_t RomeoSimpleActuator::computeNextState(double dt,stateVec_t& X,commandVec_t& U)
stateVec_t RomeoSimpleActuator::computeNextState(double& dt, const stateVec_t& X,const commandVec_t& U)
{
stateMat_t Ad = (A*dt+Id);
stateR_commandC_t Bd = dt*B;
stateVec_t result = Ad*X + Bd*U;
result(1,0)+=A13atan*atan(a*X(3,0));
result(3,0)+=A33atan*atan(a*X(3,0));
result(1,0)+=dt*A13atan*atan(a*X(3,0));
result(3,0)+=dt*A33atan*atan(a*X(3,0));
return result;
}
void RomeoSimpleActuator::computeAllModelDeriv(double dt,stateVec_t& X,commandVec_t& U)
void RomeoSimpleActuator::computeAllModelDeriv(double& dt, const stateVec_t& X,const commandVec_t& U)
{
fx = fxBase;
......@@ -83,6 +78,22 @@ void RomeoSimpleActuator::computeAllModelDeriv(double dt,stateVec_t& X,commandVe
fxx[3](3,3) = +((2*dt*Cf0)/(pi*Jl))*((2*a*a*a*X(3,0))/((1+(a*a*X(3,0)*X(3,0)))*(1+(a*a*X(3,0)*X(3,0)))));
}
stateMat_t RomeoSimpleActuator::computeTensorContxx(const stateVec_t& nextVx)
{
QxxCont = nextVx[3]*fxx[3];
return QxxCont;
}
commandMat_t RomeoSimpleActuator::computeTensorContuu(const stateVec_t& nextVx)
{
return QuuCont;
}
commandR_stateC_t RomeoSimpleActuator::computeTensorContux(const stateVec_t& nextVx)
{
return QuxCont;
}
/// accessors ///
unsigned int RomeoSimpleActuator::getStateNb()
{
......
qtTmp/romeosimpleactuator.h
View file @ b0b9a885
......@@ -18,15 +18,17 @@ protected:
// attributes //
public:
private:
unsigned int stateNb;