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

few optimizations

parent d295829d
Hide whitespace changes
Inline Side-by-side
cpp/include/ilqrsolver.h
View file @ 7e39e7d0
......@@ -71,8 +71,9 @@ private:
protected:
// methods //
public:
void initSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned int& myT,
void FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned int& myT,
double& mydt, unsigned int& myiterMax,double& mystopCrit);
void initSolver(stateVec_t& myxInit, stateVec_t& myxDes);
void solveTrajectory();
void initTrajectory();
void backwardLoop();
......
cpp/include/romeolinearactuator.h
View file @ 7e39e7d0
......@@ -11,7 +11,7 @@ using namespace Eigen;
class RomeoLinearActuator : public DynamicModel
{
public:
RomeoLinearActuator();
RomeoLinearActuator(double& mydt);
private:
protected:
......@@ -31,7 +31,9 @@ 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;
......
cpp/include/romeosimpleactuator.h
View file @ 7e39e7d0
......@@ -11,7 +11,7 @@ using namespace Eigen;
class RomeoSimpleActuator : public DynamicModel
{
public:
RomeoSimpleActuator();
RomeoSimpleActuator(double& mydt);
private:
protected:
......@@ -31,7 +31,9 @@ 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;
......
cpp/src/costfunctionromeoactuator.cpp
View file @ 7e39e7d0
......@@ -12,17 +12,20 @@ CostFunctionRomeoActuator::CostFunctionRomeoActuator()
luu = R;
lux << 0.0,0.0,0.0,0.0;
lxu << 0.0,0.0,0.0,0.0;
lx.setZero();
}
void CostFunctionRomeoActuator::computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U)
{
lx = Q*(X-Xdes);
// lx = Q*(X-Xdes);
lx(0,0) = 100.0*(X(0,0)-Xdes(0,0));
lu = R*U;
}
void CostFunctionRomeoActuator::computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes)
{
lx = Q*(X-Xdes);
// lx = Q*(X-Xdes);
lx(0,0) = 100.0*(X(0,0)-Xdes(0,0));
}
// accessors //
......
cpp/src/ilqrsolver.cpp
View file @ 7e39e7d0
......@@ -15,7 +15,7 @@ ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunctio
commandNb = myDynamicModel.getCommandNb();
}
void ILQRSolver::initSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned int& myT,
void ILQRSolver::FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned int& myT,
double& mydt, unsigned int& myiterMax,double& mystopCrit)
{
xInit = myxInit;
......@@ -42,6 +42,12 @@ void ILQRSolver::initSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned in
alpha = 1.0;
}
void ILQRSolver::initSolver(stateVec_t& myxInit, stateVec_t& myxDes)
{
xInit = myxInit;
xDes = myxDes;
}
void ILQRSolver::solveTrajectory()
{
stateVec_t* tmpxPtr;
......
cpp/src/romeolinearactuator.cpp
View file @ 7e39e7d0
......@@ -3,8 +3,9 @@
#define pi M_PI
RomeoLinearActuator::RomeoLinearActuator()
RomeoLinearActuator::RomeoLinearActuator(double& mydt)
{
dt = mydt;
Id.setIdentity();
A.setZero();
......@@ -12,17 +13,19 @@ RomeoLinearActuator::RomeoLinearActuator()
A(2,2) = 1.0;
A(1,0) = -(k/Jl)+(k/(Jm*R*R));
A(3,0) =1.0/Jl;
Ad = (A*dt+Id);
B << 0.0,
k/(R*Jm),
0.0,
0.0;
Bd = dt*B;
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;
fx = (A*dt+Id);
fxx[0].setZero();
fxx[1].setZero();
fxx[2].setZero();
......@@ -33,7 +36,7 @@ RomeoLinearActuator::RomeoLinearActuator()
k/(R*Jm),
0.0,
0.0;
fu.setZero();
fu = dt*fuBase;
fuu[0].setZero();
fux[0].setZero();
fxu[0].setZero();
......@@ -41,17 +44,13 @@ RomeoLinearActuator::RomeoLinearActuator()
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)
......
cpp/src/romeosimpleactuator.cpp
View file @ 7e39e7d0
......@@ -3,8 +3,9 @@
#define pi M_PI
RomeoSimpleActuator::RomeoSimpleActuator()
RomeoSimpleActuator::RomeoSimpleActuator(double& mydt)
{
dt = mydt;
Id.setIdentity();
A.setZero();
......@@ -14,20 +15,21 @@ RomeoSimpleActuator::RomeoSimpleActuator()
A(1,1) = -(fvm/Jm);
A(1,3) = -((fvm*k)/Jm);
A(3,0) = 1.0/Jl;
Ad = (A*dt+Id);
A13atan = (2.0*Jm*R/(pi*Jl))*Cf0;
A33atan = (2.0/(pi*Jl))*Cf0;
A13atan = dt*(2.0*Jm*R/(pi*Jl))*Cf0;
A33atan = dt*(2.0/(pi*Jl))*Cf0;
B << 0.0,
k/(R*Jm),
0.0,
0.0;
Bd = dt*B;
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.setZero();
fxBase << 1.0, dt, 0.0, 0.0,
dt*(-(k/Jl)-(k/(Jm*R*R))), 1 - dt*(fvm/Jm), 0.0, -dt*((fvm*k)/Jm),
0.0, 0.0, 1.0, dt,
dt/Jl, 0.0, 0.0, 1.0;
fxx[0].setZero();
fxx[1].setZero();
......@@ -40,7 +42,7 @@ RomeoSimpleActuator::RomeoSimpleActuator()
k/(R*Jm),
0.0,
0.0;
fu.setZero();
fu = dt* fuBase;
fuu[0].setZero();
fux[0].setZero();
fxu[0].setZero();
......@@ -53,11 +55,9 @@ RomeoSimpleActuator::RomeoSimpleActuator()
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)+=dt*A13atan*atan(a*X(3,0));
result(3,0)+=dt*A33atan*atan(a*X(3,0));
result(1,0)+=A13atan*atan(a*X(3,0));
result(3,0)+=A33atan*atan(a*X(3,0));
return result;
}
......@@ -65,19 +65,8 @@ stateVec_t RomeoSimpleActuator::computeNextState(double& dt, const stateVec_t& X
void RomeoSimpleActuator::computeAllModelDeriv(double& dt, const stateVec_t& X,const commandVec_t& U)
{
fx = fxBase;
fx(0,1) *= dt;
fx(1,0) *= dt;
fx(1,1) *= dt;
fx(1,1) += 1.0;
fx(1,3) *= dt;
fx(1,3) += ((2*dt*Jm*R)/(pi*Jl))*Cf0*(a/(1.0+(a*a*X(3,0)*X(3,0))));
fx(2,3) *= dt;
fx(3,0) *= dt;
fx(3,3) = 1.0-((2*dt*Cf0)/(pi*Jl))*(a/(1.0+(a*a*X(3,0)*X(3,0))));
fu = dt*fuBase;
fx(1,3) += A13atan*(a/(1+a*a*X(3,0)*X(3,0)));
fx(3,3) -= A33atan*(a/(1+(a*a*X(3,0)*X(3,0))));
fxx[3](1,3) = -((2*dt*Jm*R)/(pi*Jl))*Cf0*((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)))));
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)))));
}
......
cpp/test/main.cpp
View file @ 7e39e7d0
......@@ -32,11 +32,11 @@ int main()
commandVec_t* uList;
ILQRSolver::traj lastTraj;
RomeoSimpleActuator romeoActuatorModel;
RomeoLinearActuator romeoLinearModel;
RomeoSimpleActuator romeoActuatorModel(dt);
RomeoLinearActuator romeoLinearModel(dt);
CostFunctionRomeoActuator costRomeoActuator;
ILQRSolver testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator);
testSolverRomeoActuator.initSolver(xinit,xDes,T,dt,iterMax,stopCrit);
testSolverRomeoActuator.FirstInitSolver(xinit,xDes,T,dt,iterMax,stopCrit);
int N = 100;
......@@ -63,7 +63,7 @@ int main()
ofstream fichier("results.csv",ios::out | ios::trunc);
ofstream fichier("_build/cpp/results.csv",ios::out | ios::trunc);
if(fichier)
{
fichier << "tau,tauDot,q,qDot,u" << endl;
......
cpp/test/mainMPC.cpp
View file @ 7e39e7d0
......@@ -34,14 +34,14 @@ int main()
commandVec_t* uList;
ILQRSolver::traj lastTraj;
RomeoSimpleActuator romeoActuatorModel;
RomeoLinearActuator romeoLinearModel;
RomeoSimpleActuator romeoActuatorModel(dt);
RomeoLinearActuator romeoLinearModel(dt);
CostFunctionRomeoActuator costRomeoActuator;
ILQRSolver testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator);
ofstream fichier("resultsMPC.csv",ios::out | ios::trunc);
ofstream fichier("_build/cpp/resultsMPC.csv",ios::out | ios::trunc);
if(!fichier)
{
cerr << "erreur fichier ! " << endl;
......@@ -51,11 +51,12 @@ int main()
fichier << "tau,tauDot,q,qDot,u" << endl;
testSolverRomeoActuator.FirstInitSolver(xinit,xDes,T,dt,iterMax,stopCrit);
gettimeofday(&tbegin,NULL);
for(int i=0;i<M;i++)
{
testSolverRomeoActuator.initSolver(xinit,xDes,T,dt,iterMax,stopCrit);
testSolverRomeoActuator.initSolver(xinit,xDes);
testSolverRomeoActuator.solveTrajectory();
lastTraj = testSolverRomeoActuator.getLastSolvedTrajectory();
xList = lastTraj.xList;
......@@ -67,7 +68,7 @@ int main()
gettimeofday(&tend,NULL);
//texec=((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000.;
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 ";
......@@ -75,7 +76,7 @@ int main()
cout << "temps d'execution par pas de MPC ";
cout << texec/(T*1000000) << endl;
fichier.close();
// fichier.close();
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment