Commit 25d7b413 authored by flforget's avatar flforget
Browse files

implementation in progress

parent ac181610
......@@ -2,7 +2,7 @@
using namespace Eigen;
ILQRSolver::ILQRSolver(DynamicModel myDynamicModel, CostFunction myCostFunction)
ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction)
{
this->dynamicModel = myDynamicModel;
this->costFunction = myCostFunction;
......@@ -24,7 +24,6 @@ void ILQRSolver::initSolver(Eigen::VectorXd myxInit, Eigen::VectorXd myxDes, uns
this->uList = new Eigen::VectorXd[myT];
this->updatedxList = new Eigen::VectorXd[myT+1];
this->updateduList = new Eigen::VectorXd[myT];
}
void ILQRSolver::solveTrajectory()
......@@ -44,6 +43,10 @@ void ILQRSolver::solveTrajectory()
void ILQRSolver::initTrajectory()
{
this->xList[0] = this->xInit;
for(unsigned int i=1;i<this->T;i++)
{
}
}
void ILQRSolver::backwardLoop()
......
......@@ -21,7 +21,7 @@ typedef Matrix<double,Dynamic,1> commandVec;
class ILQRSolver
{
public:
ILQRSolver(DynamicModel myDynamicModel, CostFunction myCostFunction);
ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction);
private:
protected:
// attributes //
......
......@@ -3,17 +3,22 @@
#include <iostream>
#include "ilqrsolver.h"
#include "romeosimpleactuator.h"
#include "costfunctionromeoactuator.h"
#include <Eigen/Core>
using namespace std;
using namespace Eigen;
typedef Matrix<double,4,1> V4;
int main()
{
DynamicModel testModel;
CostFunction testCostFun;
RomeoSimpleActuator romeoActuatorModel;
CostFunctionRomeoActuator costRomeoActuator;
ILQRSolver testSolver(testModel,testCostFun);
cout << testModel.getCommandNb() << endl;
return 0;
}
......@@ -7,7 +7,8 @@ SOURCES += main.cpp \
costfunction.cpp \
dynamicmodel.cpp \
ilqrsolver.cpp \
romeosimpleactuator.cpp
romeosimpleactuator.cpp \
costfunctionromeoactuator.cpp
SOURCES +=
include(deployment.pri)
......@@ -20,6 +21,7 @@ HEADERS += \
costfunction.h \
dynamicmodel.h \
ilqrsolver.h \
romeosimpleactuator.h
romeosimpleactuator.h \
costfunctionromeoactuator.h
INCLUDEPATH += /usr/include/eigen3
#include "romeosimpleactuator.h"
#include <math.h>
#define pi 3,141593
RomeoSimpleActuator::RomeoSimpleActuator()
{
......@@ -8,19 +11,89 @@ RomeoSimpleActuator::RomeoSimpleActuator()
this->R = 200.0;
this->Jm = 138*1e-7;
this->Jl = 0.1;
this->fvm = 0.01;
this->Cf0 = 0.1;
this->a = 10.0;
this->Id << 1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0;
this->A << 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 0.0;
this->A(1,0) = -(this->k/this->Jl)+(this->k/(this->Jm*this->R*this->R));
this->A(1,3) = -this->fvm*this->k/this->Jm;
this->A(3,0) =1.0/this->Jl;
this->A13atan = 2.0*this->Jm*this->R/(pi*this->Jl)*this->Cf0;
this->A33atan = 2.0/(pi*this->Jl)*this->Cf0;
this->B << 0.0,
this->k/(this->R*this->Jm),
0.0,
0.0;
this->fxBase << 1.0, 1.0, 0.0, 0.0,
-(this->k/this->Jl)-(this->k/(this->Jm*this->R*this->R)), -this->fvm/this->Jm, 0.0, -this->fvm*this->k/this->Jm,
0.0, 0.0, 1.0, 1.0,
1.0/this->Jl, 0.0, 0.0, 0.0;
this->fx = this->fxBase;
this->fxx[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, 0.0;
this->fxx[1] = this->fxx[0];
this->fxx[2] = this->fxx[0];
this->fxx[3] = this->fxx[0];
this->fxu << 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.0;
this->fxu << 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.0;
this->fuBase << 0.0,
this->k/(this->R*this->Jm),
0.0,
0.0;
this->fu << 0.0,0.0,0.0,0.0;
}
void RomeoSimpleActuator::computeNextState(unsigned int dt,Eigen::VectorXd X,unsigned int U)
V4 RomeoSimpleActuator::computeNextState(double dt,V4& X,double& U)
{
M4 Ad = (this->A*dt+this->Id);
V4 Bd = dt*this->B;
V4 result = Ad*X + Bd*U;
result(1,0)+=this->A13atan*atan(this->a*X(3,0));
result(3,0)+=this->A33atan*atan(this->a*X(3,0));
return result;
}
void RomeoSimpleActuator::computeAllModelDeriv(unsigned int dt,unsigned int X,unsigned int U)
void RomeoSimpleActuator::computeAllModelDeriv(double dt,V4& X,double& U)
{
this->fx = this->fxBase;
this->fx(0,1) = dt;
this->fx(1,0) *= dt;
this->fx(1,1) *= dt;
this->fx(1,1) += 1.0;
this->fx(1,3) *= dt;
this->fx(1,3) += ((2*dt*this->Jm*this->R)/(pi*this->Jl))*this->Cf0*(this->a/(1.0+(this->a*this->a*X(3,0)*X(3,0))));
this->fx(2,3) *= dt;
this->fx(3,0) *= dt;
this->fx(3,3) += 1.0-((2*dt*this->Cf0)/(pi*this->Jl))*(this->a/(1.0+(this->a*this->a*X(3,0)*X(3,0))));
this->fu = dt*this->fuBase;
this->fxx[3](3,3) = -((2*dt*this->Jm*this->R)/(pi*this->Jl))*this->Cf0*((2*this->a*this->a*this->a*X(3,0))/((1+(this->a*this->a*X(3,0)*X(3,0)))*(1+(this->a*this->a*X(3,0)*X(3,0)))));
this->fxx[3](3,3) = +((2*dt*this->Cf0)/(pi*this->Jl))*((2*this->a*this->a*this->a*X(3,0))/((1+(this->a*this->a*X(3,0)*X(3,0)))*(1+(this->a*this->a*X(3,0)*X(3,0)))));
}
/// accessors ///
......@@ -33,3 +106,33 @@ unsigned int RomeoSimpleActuator::getCommandNb()
{
return this->commandNb;
}
M4 RomeoSimpleActuator::getfx()
{
return this->fx;
}
M4* RomeoSimpleActuator::getfxx()
{
return this->fxx;
}
V4 RomeoSimpleActuator::getfu()
{
return this->fu;
}
V4 RomeoSimpleActuator::getfuu()
{
return this->fuu;
}
M4 RomeoSimpleActuator::getfxu()
{
return this->fxu;
}
M4 RomeoSimpleActuator::getfux()
{
return this->fux;
}
#ifndef ROMEOSIMPLEACTUATOR_H
#define ROMEOSIMPLEACTUATOR_H
#include <iostream>
#include "dynamicmodel.h"
#include <Eigen/Core>
using namespace Eigen;
typedef Matrix<double,4,4> M4;
typedef Matrix<double,4,1> V4;
class RomeoSimpleActuator : public DynamicModel
{
public:
......@@ -20,15 +27,34 @@ private:
double R;
double Jm;
double Jl;
double fvm;
double Cf0;
double a;
M4 Id;
M4 A;
V4 B;
double A13atan;
double A33atan;
M4 fx,fxBase;
M4 fxx[4];
V4 fu,fuBase;
V4 fuu;
M4 fxu,fux;
protected:
//methods //
// methods //
public:
void computeNextState(unsigned int dt,Eigen::VectorXd X,unsigned int U);
void computeAllModelDeriv(unsigned int dt,unsigned int X,unsigned int U);
V4 computeNextState(double dt,V4& X,double& U);
void computeAllModelDeriv(double dt,V4& X,double& U);
// accessors //
unsigned int getStateNb();
unsigned int getCommandNb();
M4 getfx();
M4* getfxx();
V4 getfu();
V4 getfuu();
M4 getfxu();
M4 getfux();
private:
protected:
......
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