Commit 3f117e72 authored by flforget's avatar flforget
Browse files

remove duplications

parent b906bab2
project(DDP C CXX)
cmake_minimum_required(VERSION 2.8)
add_subdirectory(cpp)
add_subdirectory(src)
include_directories(include)
file(GLOB include_H . *.h)
INSTALL(FILES ${include_H} DESTINATION include)
#ifndef COSTFUNCTION_H
#define COSTFUNCTION_H
class CostFunction
{
public:
CostFunction();
};
#endif // COSTFUNCTION_H
#ifndef DYNAMICMODEL_H
#define DYNAMICMODEL_H
#include <Eigen/Core>
class DynamicModel
{
// constructors //
public:
DynamicModel();
// attributes //
public:
private:
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 unsigned int getStateNb();
virtual unsigned int getCommandNb();
private:
protected:
};
#endif // DYNAMICMODEL_H
#ifndef ILQRSOLVER_H
#define ILQRSOLVER_H
#include "dynamicmodel.h"
#include "costfunction.h"
#include <Eigen/Core>
class ILQRSolver
{
public:
ILQRSolver(DynamicModel myDynamicModel, CostFunction myCostFunction);
private:
protected:
// attributes //
public:
private:
DynamicModel dynamicModel;
CostFunction costFunction;
unsigned int stateNb;
unsigned int commandNb;
Eigen::VectorXf xInit;
Eigen::VectorXf xDes;
unsigned int T;
double dt;
unsigned int iter;
unsigned int iterMax;
double stopCrit;
double changeAmount;
Eigen::VectorXf* xList;
Eigen::VectorXf* uList;
Eigen::VectorXf* updatedxList;
Eigen::VectorXf* updateduList;
protected:
// methods //
public:
void initSolver(Eigen::VectorXf myxInit, Eigen::VectorXf myxDes, unsigned int myT,
double mydt, unsigned int myiterMax,double mystopCrit);
void solveTrajectory();
void initTrajectory();
void backwardLoop();
void forwardLoop();
private:
protected:
};
#endif // ILQRSOLVER_H
#ifndef ROMEOSIMPLEACTUATOR_H
#define ROMEOSIMPLEACTUATOR_H
#include "dynamicmodel.h"
#include <Eigen/Core>
class RomeoSimpleActuator : public DynamicModel
{
public:
RomeoSimpleActuator();
private:
protected:
// attributes //
public:
private:
unsigned int stateNb;
unsigned int commandNb;
double k;
double R;
double Jm;
double Jl;
double Cf0;
double a;
protected:
//methods //
public:
void computeNextState(unsigned int dt,Eigen::VectorXd X,unsigned int U);
void computeAllModelDeriv(unsigned int dt,unsigned int X,unsigned int U);
unsigned int getStateNb();
unsigned int getCommandNb();
private:
protected:
};
#endif // ROMEOSIMPLEACTUATOR_H
include_directories(include)
ADD_LIBRARY(DDP ${SRC})
install(TARGETS DDP DESTINATION lib)
SET(PROJECT_LIBS DDP)
add_executable(execmain main.cpp)
TARGET_LINK_LIBRARIES(execmain ${PROJECT_LIBS})
install(TARGETS execmain DESTINATION bin)
#include "costfunction.h"
CostFunction::CostFunction()
{
}
#include "dynamicmodel.h"
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()
{
}
#include "ilqrsolver.h"
ILQRSolver::ILQRSolver(DynamicModel myDynamicModel, CostFunction myCostFunction)
{
this->dynamicModel = myDynamicModel;
this->costFunction = myCostFunction;
this->stateNb = myDynamicModel.getStateNb();
this->commandNb = myDynamicModel.getCommandNb();
}
void ILQRSolver::initSolver(Eigen::VectorXf myxInit, Eigen::VectorXf myxDes, unsigned int myT,
double mydt, unsigned int myiterMax,double mystopCrit)
{
this->xInit = myxInit;
this->xDes = myxDes;
this->T = myT;
this->dt = mydt;
this->iterMax = myiterMax;
this->stopCrit = mystopCrit;
this->xList = new Eigen::VectorXf[myT+1];
this->uList = new Eigen::VectorXf[myT];
this->updatedxList = new Eigen::VectorXf[myT+1];
this->updateduList = new Eigen::VectorXf[myT];
}
void ILQRSolver::solveTrajectory()
{
this->initTrajectory();
for(this->iter=0;this->iter<this->iterMax;this->iter++)
{
this->backwardLoop();
this->forwardLoop();
this->xList = this->updatedxList;
this->uList = this->updateduList;
if(this->changeAmount<this->stopCrit)
break;
}
}
void ILQRSolver::initTrajectory()
{
this->xList[0] = this->xInit;
Eigen::Vector<double,this->commandNb> zerosCommand;
for(int i=0;i<this->T;i++)
{
uList[i] = zerosCommand;
}
}
void ILQRSolver::backwardLoop()
{
}
void ILQRSolver::forwardLoop()
{
}
#include <iostream>
#include "ilqrsolver.h"
#include "romeosimpleactuator.h"
#include <Eigen/Core>
using namespace std;
int main()
{
RomeoSimpleActuator romeoActuatorModel;
CostFunction costFunction;
ILQRSolver iLQRSolver(romeoActuatorModel,costFunction);
Eigen::VectorXf xInit(4);
Eigen::VectorXf xDes(4);
Eigen::VectorXf xList[2];
xDes(0) = 1.0;
xList[0] = xInit;
xList[1] = xDes;
cout << xList[0]<< endl;
cout << xList[1] << endl;
cout << "done" << endl;
}
#include "romeosimpleactuator.h"
RomeoSimpleActuator::RomeoSimpleActuator()
{
this->commandNb = 1;
this->stateNb = 4;
this->k = 1000.0;
this->R = 200.0;
this->Jm = 138*1e-7;
this->Jl = 0.1;
this->Cf0 = 0.1;
this->a = 10.0;
}
void RomeoSimpleActuator::computeNextState(unsigned int dt,Eigen::VectorXd X,unsigned int U)
{
}
void RomeoSimpleActuator::computeAllModelDeriv(unsigned int dt,unsigned int X,unsigned int U)
{
}
/// accessors ///
unsigned int RomeoSimpleActuator::getStateNb()
{
return this->stateNb;
}
unsigned int RomeoSimpleActuator::getCommandNb()
{
return this->commandNb;
}
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