Commit b906bab2 authored by flforget's avatar flforget
Browse files

bug fix

parent b0b9a885
......@@ -45,7 +45,7 @@ class ILQRSolver:
if(self.changeAmount < self.stopCrit):
break
return XList,UList
return XList,UList,iter
def initTrajectory(self):
......@@ -77,6 +77,7 @@ class ILQRSolver:
Kappend = KList.append
nextVx,nextVxx = \
self.costfunction.computeFinalCostDeriv(XList[self.T],self.Xdes)
Xdes = self.Xdes
stateNb = self.model.stateNumber
dt = self.dt
......@@ -109,6 +110,7 @@ class ILQRSolver:
# Quu += nextVx[j].item()*fuu[j]
Quut += nextVx[j].item()*fuu[j]
if(np.all(np.linalg.eigvals(Quut) <= 0)): # check out if Quut is definite positive
if(mu==0):
mu += 1e-4
......
No preview for this file type
__author__ = 'fforget'
import numpy as np
from matplotlib import pyplot as pl
# __author__ = 'fforget'
#
# import numpy as np
# from matplotlib import pyplot as pl
from dynamicModel import *
from scipy.integrate import odeint
from costFunction import *
# from scipy.integrate import odeint
#
# model = simpleRomeoActuatorDynamicModel()
#
# dt = 1e-4
# X = np.matrix([[1.0,0.0,0.0,0.0]]).T
# U = 0.0
#
#
# # lists init
# XList = list()
# XrealList = list()
# tauList = list()
# tauDotList = list()
# qList = list()
# qDotList = list()
# taurealList = list()
# tauDotrealList = list()
# qrealList = list()
# qDotrealList = list()
# errList = list()
# fxList = list()
# XList.append(X)
# XrealList.append(X)
# tauList.append(X[0].item())
# tauDotList.append(X[1].item())
# qList.append(X[2].item())
# qDotList.append(X[3].item())
# taurealList.append(X[0].item())
# tauDotrealList.append(X[1].item())
# qrealList.append(X[2].item())
# qDotrealList.append(X[3].item())
# errList.append(0.0)
#
#
# N = 1000
# for i in range(N):
# X = model.computeNextState(dt,X,U)
# XList.append(X)
# tauList.append(X[0].item())
# tauDotList.append(X[1].item())
# qList.append(X[2].item())
# qDotList.append(X[3].item())
#
#
# # solving ODE to compare discrete model with continuous one
# time = np.linspace((i)*dt, (i+1)*dt)
# # example for odeint -> Xreal = odeint(romeoActuator,[X[0].item(),X[1].item(),X[2].item(),X[3].item()],time)
# Xrealfull = odeint(model.continuousModel,[X[0].item(),X[1].item(),X[2].item(),X[3].item()],time,args=(U,))
# Xreal = Xrealfull[Xrealfull.shape[0]-1,:]
# XrealList.append(Xreal)
# taurealList.append(Xreal[0].item())
# tauDotrealList.append(Xreal[1].item())
# qrealList.append(Xreal[2].item())
# qDotrealList.append(Xreal[3].item())
#
# errList.append(X[0].item()-Xreal[0].item())
#
# fig = pl.figure ()
# ax = fig.add_subplot ('311')
# ax.set_title('tau (b) - real (dot) ')
# ax.plot ( range(N+1),tauList,
# range(N+1),taurealList,':')
# bx = fig.add_subplot ('312')
# bx.set_title('error continuous-discrete')
# bx.plot ( range(N+1),errList)
# cx = fig.add_subplot ('313')
# cx.set_title('angle q (b) - angular velocity (r) - real (dot)')
# cx.plot ( range(N+1),qList,'b', range(N+1),qDotList,'r',
# range(N+1),qrealList,'b:', range(N+1),qDotrealList,'r:')
# ax.grid()
# bx.grid()
# cx.grid()
#
#
#
#
# pl.show()
model = simpleRomeoActuatorDynamicModel()
model = SimpleRomeoActuatorDynamicModel()
costfunction = CostFunctionSimpleRomeoActuator()
X = np.matrix([[100.0],[1.0],[100.0],[13.0]])
Xdes = np.matrix([[1.0],[1.0],[1.0],[1.0]])
U = 120.0
dt = 1e-4
X = np.matrix([[1.0,0.0,0.0,0.0]]).T
U = 0.0
# lists init
XList = list()
XrealList = list()
tauList = list()
tauDotList = list()
qList = list()
qDotList = list()
taurealList = list()
tauDotrealList = list()
qrealList = list()
qDotrealList = list()
errList = list()
fxList = list()
XList.append(X)
XrealList.append(X)
tauList.append(X[0].item())
tauDotList.append(X[1].item())
qList.append(X[2].item())
qDotList.append(X[3].item())
taurealList.append(X[0].item())
tauDotrealList.append(X[1].item())
qrealList.append(X[2].item())
qDotrealList.append(X[3].item())
errList.append(0.0)
N = 1000
for i in range(N):
X = model.computeNextState(dt,X,U)
XList.append(X)
tauList.append(X[0].item())
tauDotList.append(X[1].item())
qList.append(X[2].item())
qDotList.append(X[3].item())
# solving ODE to compare discrete model with continuous one
time = np.linspace((i)*dt, (i+1)*dt)
# example for odeint -> Xreal = odeint(romeoActuator,[X[0].item(),X[1].item(),X[2].item(),X[3].item()],time)
Xrealfull = odeint(model.continuousModel,[X[0].item(),X[1].item(),X[2].item(),X[3].item()],time,args=(U,))
Xreal = Xrealfull[Xrealfull.shape[0]-1,:]
XrealList.append(Xreal)
taurealList.append(Xreal[0].item())
tauDotrealList.append(Xreal[1].item())
qrealList.append(Xreal[2].item())
qDotrealList.append(Xreal[3].item())
errList.append(X[0].item()-Xreal[0].item())
fig = pl.figure ()
ax = fig.add_subplot ('311')
ax.set_title('tau (b) - real (dot) ')
ax.plot ( range(N+1),tauList,
range(N+1),taurealList,':')
bx = fig.add_subplot ('312')
bx.set_title('error continuous-discrete')
bx.plot ( range(N+1),errList)
cx = fig.add_subplot ('313')
cx.set_title('angle q (b) - angular velocity (r) - real (dot)')
cx.plot ( range(N+1),qList,'b', range(N+1),qDotList,'r',
range(N+1),qrealList,'b:', range(N+1),qDotrealList,'r:')
ax.grid()
bx.grid()
cx.grid()
print model.computeNextState(dt,X,U)
pl.show()
\ No newline at end of file
......@@ -46,8 +46,9 @@ solver = ILQRSolver(model,costFunction)
for i in range(M):
Xdes = trajList[i]
initTime = time.time()
XListtmp,UListtmp = solver.solveTrajectory(Xinit,Xdes,N,dt,20,1e-3)
XListtmp,UListtmp,iter = solver.solveTrajectory(Xinit,Xdes,N,dt,20,1e-3)
endTime = time.time() - initTime
print iter
print costFunction.computeCostValue(N,XListtmp,Xdes,UListtmp)
timeList.append(endTime/N)
XList += XListtmp
......
......@@ -17,7 +17,7 @@ protected:
// methods //
public:
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;
virtual void computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes)=0;
private:
protected:
// accessors //
......
......@@ -20,7 +20,7 @@ void CostFunctionRomeoActuator::computeAllCostDeriv(const stateVec_t& X, const s
lu = R*U;
}
void CostFunctionRomeoActuator::commuteFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes)
void CostFunctionRomeoActuator::computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes)
{
lx = Q*(X-Xdes);
}
......
......@@ -32,7 +32,7 @@ protected:
// methods //
public:
void computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U);
void commuteFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes);
void computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes);
private:
protected:
// accessors //
......
......@@ -24,8 +24,8 @@ 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 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;
......@@ -36,11 +36,11 @@ public:
virtual unsigned int getStateNb()=0;
virtual unsigned int getCommandNb()=0;
virtual stateMat_t &getfx()=0;
virtual stateTens_t* getfxx()=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;
virtual stateR_commandC_commandD_t& getfuu()=0;
virtual stateR_stateC_commandD_t& getfxu()=0;
virtual stateR_commandC_stateD_t& getfux()=0;
};
#endif // DYNAMICMODEL_H
......@@ -39,7 +39,7 @@ void ILQRSolver::initSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned in
alphaList[2] = 0.6;
alphaList[3] = 0.4;
alphaList[4] = 0.2;
alpha = 0.0;
alpha = 1.0;
}
void ILQRSolver::solveTrajectory()
......@@ -76,16 +76,17 @@ void ILQRSolver::initTrajectory()
void ILQRSolver::backwardLoop()
{
costFunction->commuteFinalCostDeriv(xList[T],xDes);
costFunction->computeFinalCostDeriv(xList[T],xDes);
nextVx = costFunction->getlx();
nextVxx = costFunction->getlxx();
muEye.setZero();
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];
......@@ -104,9 +105,18 @@ void ILQRSolver::backwardLoop()
Qux += dynamicModel->computeTensorContux(nextVx);
Quu += dynamicModel->computeTensorContuu(nextVx);
/*
To be Implemented : Regularization (is Quu definite positive ?)
*/
if(!isQuudefinitePositive(Quu))
{
/*
To be Implemented : Regularization (is Quu definite positive ?)
*/
if(mu==0.0) mu += 1e-4;
else mu *= 10;
completeBackwardFlag = 0;
break;
}
QuuInv = Quu.inverse();
k = -QuuInv*Qu;
K = -QuuInv*Qux;
......@@ -144,3 +154,11 @@ ILQRSolver::traj ILQRSolver::getLastSolvedTrajectory()
lastTraj.iter = iter;
return lastTraj;
}
char ILQRSolver::isQuudefinitePositive(commandMat_t& Quu)
{
/*
Todo : check if Quu is definite positive
*/
return 1;
}
......@@ -77,6 +77,7 @@ public:
void initTrajectory();
void backwardLoop();
void forwardLoop();
char isQuudefinitePositive(commandMat_t& Quu);
struct traj getLastSolvedTrajectory();
private:
protected:
......
......@@ -25,7 +25,7 @@ int main()
xinit << 0.0,0.0,0.0,0.0;
xDes << 1.0,0.0,0.0,0.0;
unsigned int T = 3;
unsigned int T = 30;
double dt=1e-4;
unsigned int iterMax = 20;
double stopCrit = 1e-3;
......@@ -39,8 +39,10 @@ int main()
ILQRSolver testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator);
testSolverRomeoActuator.initSolver(xinit,xDes,T,dt,iterMax,stopCrit);
int N = 1000;
gettimeofday(&tbegin,NULL);
testSolverRomeoActuator.solveTrajectory();
for(int i=0;i<N;i++) testSolverRomeoActuator.solveTrajectory();
gettimeofday(&tend,NULL);
lastTraj = testSolverRomeoActuator.getLastSolvedTrajectory();
......@@ -49,6 +51,7 @@ int main()
unsigned int iter = lastTraj.iter;
texec=((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000.;
texec /= N;
cout << "temps d'execution total du solveur ";
cout << texec << endl;
......@@ -56,6 +59,10 @@ int main()
cout << texec/T << endl;
cout << "Nombre d'itérations : " << iter << endl;
ofstream fichier("results.csv",ios::out | ios::trunc);
if(fichier)
{
......@@ -66,7 +73,6 @@ int main()
}
else
cerr << "erreur ouverte fichier" << endl;
return 0;
}
#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;
int mainMPC()
{
struct timeval tbegin,tend;
double texec=0.0;
stateVec_t xinit,xDes;
xinit << 0.0,0.0,0.0,0.0;
xDes << 1.0,0.0,0.0,0.0;
unsigned int T = 5;
unsigned int M = 30;
double dt=1e-4;
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);
ofstream fichier("results.csv",ios::out | ios::trunc);
if(!fichier)
{
cerr << "erreur fichier ! " << endl;
return 1;
}
fichier << "tau,tauDot,q,qDot,u" << endl;
gettimeofday(&tbegin,NULL);
for(int i=0;i<M;i++)
{
cout << i << endl;
testSolverRomeoActuator.initSolver(xinit,xDes,T,dt,iterMax,stopCrit);
testSolverRomeoActuator.solveTrajectory();
lastTraj = testSolverRomeoActuator.getLastSolvedTrajectory();
xList = lastTraj.xList;
uList = lastTraj.uList;
xinit = xList[1];
for(int j=0;j<T;j++) 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 << endl;
}
gettimeofday(&tend,NULL);
//texec=((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000.;
texec = (double)(tend.tv_usec - tbegin.tv_usec);
cout << "temps d'execution total du solveur ";
cout << texec << endl;
cout << "temps d'execution par pas de MPC ";
cout << texec/T << endl;
fichier.close();
return 0;
}
......@@ -9,8 +9,8 @@ SOURCES += main.cpp \
ilqrsolver.cpp \
romeosimpleactuator.cpp \
costfunctionromeoactuator.cpp \
romeolinearactuator.cpp
SOURCES +=
romeolinearactuator.cpp \
mainMPC.cpp
include(deployment.pri)
qtcAddDeployment()
......
......@@ -91,9 +91,9 @@ stateMat_t& RomeoLinearActuator::getfx()
return fx;
}
stateTens_t* RomeoLinearActuator::getfxx()
stateTens_t& RomeoLinearActuator::getfxx()
{
return &fxx;
return fxx;
}
stateR_commandC_t& RomeoLinearActuator::getfu()
......@@ -101,17 +101,17 @@ stateR_commandC_t& RomeoLinearActuator::getfu()
return fu;
}
stateR_commandC_commandD_t* RomeoLinearActuator::getfuu()
stateR_commandC_commandD_t& RomeoLinearActuator::getfuu()
{
return &fuu;
return fuu;
}
stateR_stateC_commandD_t* RomeoLinearActuator::getfxu()
stateR_stateC_commandD_t& RomeoLinearActuator::getfxu()
{
return &fxu;
return fxu;
}
stateR_commandC_stateD_t* RomeoLinearActuator::getfux()
stateR_commandC_stateD_t& RomeoLinearActuator::getfux()
{
return &fux;
return fux;
}
......@@ -45,7 +45,7 @@ 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);
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);
......@@ -56,11 +56,11 @@ public:
unsigned int getStateNb();
unsigned int getCommandNb();
stateMat_t &getfx();
stateTens_t* getfxx();
stateTens_t& getfxx();
stateR_commandC_t &getfu();
stateR_commandC_commandD_t* getfuu();
stateR_stateC_commandD_t* getfxu();
stateR_commandC_stateD_t* getfux();
stateR_commandC_commandD_t& getfuu();
stateR_stateC_commandD_t& getfxu();
stateR_commandC_stateD_t& getfux();
};
......
......@@ -9,34 +9,38 @@ RomeoSimpleActuator::RomeoSimpleActuator()
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(3,0) =1.0/Jl;
A13atan = 2.0*Jm*R/(pi*Jl)*Cf0;
A33atan = 2.0/(pi*Jl)*Cf0;
A(2,3) = 1.0;
A(1,0) = -((k/Jl)+(k/(Jm*R*R)));
A(1,1) = -(fvm/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;
B << 0.0,
k/(R*Jm),
0.0,
0.0;
k/(R*Jm),
0.0,
0.0;
fxBase << 1.0, 1.0, 0.0, 0.0,
-(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;
-(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.setZero();
fxx[0].setZero();
fxx[1].setZero();
fxx[2].setZero();
fxx[3].setZero();
fxu[0].setZero();
fxu[0].setZero();
fuBase << 0.0,
k/(R*Jm),
0.0,
0.0;
fu.setZero();;
k/(R*Jm),
0.0,
0.0;
fu.setZero();
fuu[0].setZero();
fux[0].setZero();
fxu[0].setZero();
......@@ -62,7 +66,7 @@ void RomeoSimpleActuator::computeAllModelDeriv(double& dt, const stateVec_t& X,c
{
fx = fxBase;
fx(0,1) = dt;