Commit 15a40d72 authored by flforget's avatar flforget
Browse files

add several typedef

parent 25d7b413
#ifndef CONFIG_H
#define CONFIG_H
#include <Eigen/Core>
#define stateSize 4
#define commandSize 1
// typedef for stateSize types
typedef Eigen::Matrix<double,stateSize,1> stateVec_t; // stateSize x 1
typedef Eigen::Matrix<double,1,stateSize> stateVecTrans_t; // 1 x stateSize
typedef Eigen::Matrix<double,stateSize,stateSize> stateMat_t; // stateSize x stateSize
typedef Eigen::Matrix<double,stateSize,stateSize> stateTens_t[stateSize]; // stateSize x stateSize x stateSize
// typedef for commandSize types
typedef Eigen::Matrix<double,commandSize,1> commandVec_t; // commandSize x 1
typedef Eigen::Matrix<double,1,commandSize> commandVecTrans_t; // 1 x commandSize
typedef Eigen::Matrix<double,commandSize,commandSize> commandMat_t; // commandSize x commandSize
typedef Eigen::Matrix<double,commandSize,commandSize> commandTens_t[commandSize]; // stateSize x commandSize x commandSize
// typedef for mixed stateSize and commandSize types
typedef Eigen::Matrix<double,stateSize,commandSize> stateR_commandC_t; // stateSize x commandSize
typedef Eigen::Matrix<double,stateSize,commandSize> stateR_commandC_stateD_t[stateSize]; // stateSize x commandSize x stateSize
typedef Eigen::Matrix<double,stateSize,commandSize> stateR_commandC_commandD_t[commandSize]; // stateSize x commandSize x commandSize
typedef Eigen::Matrix<double,commandSize,stateSize> commandR_stateC_t; // commandSize x stateSize
typedef Eigen::Matrix<double,commandSize,stateSize> commandR_stateC_stateD_t[stateSize]; // commandSize x stateSize x stateSize
typedef Eigen::Matrix<double,commandSize,stateSize> commandR_stateC_commandD_t[commandSize]; // commandSize x stateSize x commandSize
typedef Eigen::Matrix<double,stateSize,stateSize> stateR_stateC_commandD_t[commandSize]; // stateSize x stateSize x commandSize
typedef Eigen::Matrix<double,commandSize,commandSize> commandR_commandC_stateD_t[stateSize]; // commandSize x commandSize x stateSize
#endif // CONFIG_H
#ifndef COSTFUNCTION_H
#define COSTFUNCTION_H
#include "config.h"
class CostFunction
{
public:
CostFunction();
private:
protected:
// attributes //
public:
private:
protected:
// methods //
public:
virtual void computeAllCostDeriv(stateVec_t& X, stateVec_t& Xdes, commandVec_t& U);
virtual void commuteFinalCostDeriv(stateVec_t& X, stateVec_t& Xdes);
private:
protected:
// accessors //
public:
virtual stateVec_t getlx();
virtual stateMat_t getlxx();
virtual commandVec_t getlu();
virtual commandMat_t getluu();
virtual commandR_stateC_t getlux();
virtual stateR_commandC_t getlxu();
};
#endif // COSTFUNCTION_H
......@@ -6,7 +6,7 @@ CostFunctionRomeoActuator::CostFunctionRomeoActuator()
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->R = 0.1;
this->R << 0.1;
this->lxx = this->Q;
this->luu = this->R;
......@@ -14,44 +14,44 @@ CostFunctionRomeoActuator::CostFunctionRomeoActuator()
this->lxu << 0.0,0.0,0.0,0.0;
}
void CostFunctionRomeoActuator::computeAllCostDeriv(V4 &X, V4 &Xdes, double &U)
void CostFunctionRomeoActuator::computeAllCostDeriv(stateVec_t& X, stateVec_t& Xdes, commandVec_t& U)
{
this->lx = this->Q*(X-Xdes);
this->lu = this->R*U;
}
void CostFunctionRomeoActuator::commuteFinalCostDeriv(V4 &X, V4 &Xdes)
void CostFunctionRomeoActuator::commuteFinalCostDeriv(stateVec_t& X, stateVec_t& Xdes)
{
this->lx = this->Q*(X-Xdes);
}
// accessors //
V4 CostFunctionRomeoActuator::getlx()
stateVec_t CostFunctionRomeoActuator::getlx()
{
return this->lx;
}
M4 CostFunctionRomeoActuator::getlxx()
stateMat_t CostFunctionRomeoActuator::getlxx()
{
return this->lxx;
}
double CostFunctionRomeoActuator::getlu()
commandVec_t CostFunctionRomeoActuator::getlu()
{
return this->lu;
}
double CostFunctionRomeoActuator::getluu()
commandMat_t CostFunctionRomeoActuator::getluu()
{
return this->luu;
}
V4 CostFunctionRomeoActuator::getlxu()
stateR_commandC_t CostFunctionRomeoActuator::getlxu()
{
return this->lxu;
}
V4T CostFunctionRomeoActuator::getlux()
commandR_stateC_t CostFunctionRomeoActuator::getlux()
{
return this->lux;
}
#ifndef COSTFUNCTIONROMEOACTUATOR_H
#define COSTFUNCTIONROMEOACTUATOR_H
#include "config.h"
#include "costfunction.h"
#include <Eigen/Core>
using namespace Eigen;
typedef Matrix<double,4,4> M4;
typedef Matrix<double,4,1> V4;
typedef Matrix<double,1,4> V4T;
class CostFunctionRomeoActuator
class CostFunctionRomeoActuator : public CostFunction
{
public:
CostFunctionRomeoActuator();
private:
M4 Q;
double R;
V4 lx;
M4 lxx;
double lu,luu;
V4T lux;
V4 lxu;
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;
protected:
// attributes //
public:
......@@ -29,18 +30,18 @@ private:
protected:
// methods //
public:
void computeAllCostDeriv(V4& X, V4& Xdes, double& U);
void commuteFinalCostDeriv(V4& X, V4& Xdes);
void computeAllCostDeriv(stateVec_t& X, stateVec_t& Xdes, commandVec_t& U);
void commuteFinalCostDeriv(stateVec_t& X, stateVec_t& Xdes);
private:
protected:
// accessors //
public:
V4 getlx();
M4 getlxx();
double getlu();
double getluu();
V4T getlux();
V4 getlxu();
stateVec_t getlx();
stateMat_t getlxx();
commandVec_t getlu();
commandMat_t getluu();
commandR_stateC_t getlux();
stateR_commandC_t getlxu();
};
#endif // COSTFUNCTIONROMEOACTUATOR_H
......@@ -3,23 +3,3 @@
DynamicModel::DynamicModel()
{
}
void DynamicModel::computeAllModelDeriv(unsigned int dt, unsigned int X, unsigned int U)
{
}
void DynamicModel::computeNextState(unsigned int dt, unsigned int X, unsigned int U)
{
}
unsigned int DynamicModel::getCommandNb()
{
}
unsigned int DynamicModel::getStateNb()
{
}
#ifndef DYNAMICMODEL_H
#define DYNAMICMODEL_H
#include "config.h"
#include <Eigen/Core>
using namespace Eigen;
......@@ -14,15 +16,15 @@ public:
// attributes //
public:
private:
static unsigned int stateNb;
static unsigned int commandNb;
unsigned int stateNb;
unsigned int commandNb;
protected:
// methods //
public:
virtual void computeNextState(unsigned int dt,unsigned int X,unsigned int U);
virtual void computeAllModelDeriv(unsigned int dt,unsigned int X,unsigned int U);
virtual stateVec_t computeNextState(unsigned int dt,stateVec_t& X,commandVec_t& U);
virtual void computeAllModelDeriv(unsigned int dt,stateVec_t& X,commandVec_t& U);
virtual unsigned int getStateNb();
virtual unsigned int getCommandNb();
private:
......
......@@ -10,7 +10,7 @@ ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunctio
this->commandNb = myDynamicModel.getCommandNb();
}
void ILQRSolver::initSolver(Eigen::VectorXd myxInit, Eigen::VectorXd myxDes, unsigned int myT,
void ILQRSolver::initSolver(stateVec_t myxInit, stateVec_t myxDes, unsigned int myT,
double mydt, unsigned int myiterMax,double mystopCrit)
{
this->xInit = myxInit;
......@@ -20,10 +20,10 @@ void ILQRSolver::initSolver(Eigen::VectorXd myxInit, Eigen::VectorXd myxDes, uns
this->iterMax = myiterMax;
this->stopCrit = mystopCrit;
this->xList = new Eigen::VectorXd[myT+1];
this->uList = new Eigen::VectorXd[myT];
this->updatedxList = new Eigen::VectorXd[myT+1];
this->updateduList = new Eigen::VectorXd[myT];
this->xList = new stateVec_t[myT+1];
this->uList = new commandVec_t[myT];
this->updatedxList = new stateVec_t[myT+1];
this->updateduList = new commandVec_t[myT];
}
void ILQRSolver::solveTrajectory()
......@@ -43,9 +43,12 @@ void ILQRSolver::solveTrajectory()
void ILQRSolver::initTrajectory()
{
this->xList[0] = this->xInit;
for(unsigned int i=1;i<this->T;i++)
commandVec_t zeroCommand;
zeroCommand << commandVec_t::Zero(commandSize,1);
for(unsigned int i=0;i<this->T;i++)
{
this->uList[i] = zeroCommand;
this->xList[i+1] = this->dynamicModel.computeNextState(this->dt,this->xList[i],zeroCommand);
}
}
......
#ifndef ILQRSOLVER_H
#define ILQRSOLVER_H
#include "config.h"
#include "dynamicmodel.h"
#include "costfunction.h"
#include <Eigen/Core>
using namespace Eigen;
#ifdef stateSize
typedef Matrix<double,stateSize,1> stateVec;
#else
typedef Matrix<double,Dynamic,1> stateVec;
#endif
#ifdef commandSize
typedef Matrix<double,commandSize,1> commandVec;
#else
typedef Matrix<double,Dynamic,1> commandVec;
#endif
class ILQRSolver
{
public:
......@@ -31,8 +22,8 @@ private:
CostFunction costFunction;
unsigned int stateNb;
unsigned int commandNb;
stateVec xInit;
stateVec xDes;
stateVec_t xInit;
stateVec_t xDes;
unsigned int T;
double dt;
unsigned int iter;
......@@ -40,16 +31,16 @@ private:
double stopCrit;
double changeAmount;
stateVec* xList;
stateVec* uList;
stateVec* updatedxList;
stateVec* updateduList;
stateVec_t* xList;
commandVec_t* uList;
stateVec_t* updatedxList;
commandVec_t* updateduList;
protected:
// methods //
public:
void initSolver(stateVec myxInit, stateVec myxDes, unsigned int myT,
void initSolver(stateVec_t myxInit, stateVec_t myxDes, unsigned int myT,
double mydt, unsigned int myiterMax,double mystopCrit);
void solveTrajectory();
void initTrajectory();
......
#define stateSize 4
#define commandSize 1
#include <iostream>
#include "config.h"
#include "ilqrsolver.h"
#include "romeosimpleactuator.h"
#include "costfunctionromeoactuator.h"
......@@ -10,8 +10,6 @@
using namespace std;
using namespace Eigen;
typedef Matrix<double,4,1> V4;
int main()
{
DynamicModel testModel;
......@@ -19,6 +17,8 @@ int main()
RomeoSimpleActuator romeoActuatorModel;
CostFunctionRomeoActuator costRomeoActuator;
ILQRSolver testSolver(testModel,testCostFun);
ILQRSolver testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator);
return 0;
}
......@@ -22,6 +22,7 @@ HEADERS += \
dynamicmodel.h \
ilqrsolver.h \
romeosimpleactuator.h \
costfunctionromeoactuator.h
costfunctionromeoactuator.h \
config.h
INCLUDEPATH += /usr/include/eigen3
......@@ -47,11 +47,11 @@ RomeoSimpleActuator::RomeoSimpleActuator()
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,
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.0;
this->fxu << 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.0;
......@@ -65,18 +65,18 @@ RomeoSimpleActuator::RomeoSimpleActuator()
}
V4 RomeoSimpleActuator::computeNextState(double dt,V4& X,double& U)
stateVec_t RomeoSimpleActuator::computeNextState(double dt,stateVec_t& X,commandVec_t& U)
{
M4 Ad = (this->A*dt+this->Id);
V4 Bd = dt*this->B;
V4 result = Ad*X + Bd*U;
stateMat_t Ad = (this->A*dt+this->Id);
stateR_commandC_t Bd = dt*this->B;
stateVec_t 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(double dt,V4& X,double& U)
void RomeoSimpleActuator::computeAllModelDeriv(double dt,stateVec_t& X,commandVec_t& U)
{
this->fx = this->fxBase;
......@@ -107,32 +107,32 @@ unsigned int RomeoSimpleActuator::getCommandNb()
return this->commandNb;
}
M4 RomeoSimpleActuator::getfx()
stateMat_t RomeoSimpleActuator::getfx()
{
return this->fx;
}
M4* RomeoSimpleActuator::getfxx()
stateTens_t* RomeoSimpleActuator::getfxx()
{
return this->fxx;
return &this->fxx;
}
V4 RomeoSimpleActuator::getfu()
stateR_commandC_t RomeoSimpleActuator::getfu()
{
return this->fu;
}
V4 RomeoSimpleActuator::getfuu()
stateR_commandC_commandD_t* RomeoSimpleActuator::getfuu()
{
return this->fuu;
return &this->fuu;
}
M4 RomeoSimpleActuator::getfxu()
stateR_stateC_commandD_t* RomeoSimpleActuator::getfxu()
{
return this->fxu;
return &this->fxu;
}
M4 RomeoSimpleActuator::getfux()
stateR_commandC_stateD_t* RomeoSimpleActuator::getfux()
{
return this->fux;
return &this->fux;
}
#ifndef ROMEOSIMPLEACTUATOR_H
#define ROMEOSIMPLEACTUATOR_H
#include <iostream>
#include "config.h"
#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:
......@@ -30,31 +27,32 @@ private:
double fvm;
double Cf0;
double a;
M4 Id;
M4 A;
V4 B;
stateMat_t Id;
stateMat_t A;
stateR_commandC_t B;
double A13atan;
double A33atan;
M4 fx,fxBase;
M4 fxx[4];
V4 fu,fuBase;
V4 fuu;
M4 fxu,fux;
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:
V4 computeNextState(double dt,V4& X,double& U);
void computeAllModelDeriv(double dt,V4& X,double& U);
stateVec_t computeNextState(double dt,stateVec_t& X,commandVec_t &U);
void computeAllModelDeriv(double dt,stateVec_t& X,commandVec_t &U);
// accessors //
unsigned int getStateNb();
unsigned int getCommandNb();
M4 getfx();
M4* getfxx();
V4 getfu();
V4 getfuu();
M4 getfxu();
M4 getfux();
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();
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