Commit ba7e1e46 authored by flforget's avatar flforget
Browse files

delete old files using qt project manager

parent a4430f39
#ifndef CONFIG_H
#define CONFIG_H
#include <Eigen/Dense>
#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
#include "costfunction.h"
CostFunction::CostFunction()
{
}
#ifndef COSTFUNCTION_H
#define COSTFUNCTION_H
#include "config.h"
class CostFunction
{
public:
CostFunction();
private:
protected:
// attributes //
public:
private:
double dt;
protected:
// methods //
public:
virtual void computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U)=0;
virtual void computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes)=0;
private:
protected:
// accessors //
public:
virtual stateVec_t getlx()=0;
virtual stateMat_t getlxx()=0;
virtual commandVec_t getlu()=0;
virtual commandMat_t getluu()=0;
virtual commandR_stateC_t getlux()=0;
virtual stateR_commandC_t getlxu()=0;
};
#endif // COSTFUNCTION_H
#include "costfunctionromeoactuator.h"
CostFunctionRomeoActuator::CostFunctionRomeoActuator()
{
Q << 100.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;
R << 0.1;
lxx = Q;
luu = R;
lux << 0.0,0.0,0.0,0.0;
lxu << 0.0,0.0,0.0,0.0;
}
void CostFunctionRomeoActuator::computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U)
{
lx = Q*(X-Xdes);
lu = R*U;
}
void CostFunctionRomeoActuator::computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes)
{
lx = Q*(X-Xdes);
}
// accessors //
stateVec_t CostFunctionRomeoActuator::getlx()
{
return lx;
}
stateMat_t CostFunctionRomeoActuator::getlxx()
{
return lxx;
}
commandVec_t CostFunctionRomeoActuator::getlu()
{
return lu;
}
commandMat_t CostFunctionRomeoActuator::getluu()
{
return luu;
}
stateR_commandC_t CostFunctionRomeoActuator::getlxu()
{
return lxu;
}
commandR_stateC_t CostFunctionRomeoActuator::getlux()
{
return lux;
}
#ifndef COSTFUNCTIONROMEOACTUATOR_H
#define COSTFUNCTIONROMEOACTUATOR_H
#include "config.h"
#include "costfunction.h"
#include <Eigen/Dense>
using namespace Eigen;
class CostFunctionRomeoActuator : public CostFunction
{
public:
CostFunctionRomeoActuator();
private:
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;
double dt;
protected:
// attributes //
public:
private:
protected:
// methods //
public:
void computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U);
void computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes);
private:
protected:
// accessors //
public:
stateVec_t getlx();
stateMat_t getlxx();
commandVec_t getlu();
commandMat_t getluu();
commandR_stateC_t getlux();
stateR_commandC_t getlxu();
};
#endif // COSTFUNCTIONROMEOACTUATOR_H
# This file was generated by an application wizard of Qt Creator.
# The code below handles deployment to Android and Maemo, aswell as copying
# of the application data to shadow build directories on desktop.
# It is recommended not to modify this file, since newer versions of Qt Creator
# may offer an updated version of it.
defineTest(qtcAddDeployment) {
for(deploymentfolder, DEPLOYMENTFOLDERS) {
item = item$${deploymentfolder}
greaterThan(QT_MAJOR_VERSION, 4) {
itemsources = $${item}.files
} else {
itemsources = $${item}.sources
}
$$itemsources = $$eval($${deploymentfolder}.source)
itempath = $${item}.path
$$itempath= $$eval($${deploymentfolder}.target)
export($$itemsources)
export($$itempath)
DEPLOYMENT += $$item
}
MAINPROFILEPWD = $$PWD
android-no-sdk {
for(deploymentfolder, DEPLOYMENTFOLDERS) {
item = item$${deploymentfolder}
itemfiles = $${item}.files
$$itemfiles = $$eval($${deploymentfolder}.source)
itempath = $${item}.path
$$itempath = /data/user/qt/$$eval($${deploymentfolder}.target)
export($$itemfiles)
export($$itempath)
INSTALLS += $$item
}
target.path = /data/user/qt
export(target.path)
INSTALLS += target
} else:android {
for(deploymentfolder, DEPLOYMENTFOLDERS) {
item = item$${deploymentfolder}
itemfiles = $${item}.files
$$itemfiles = $$eval($${deploymentfolder}.source)
itempath = $${item}.path
$$itempath = /assets/$$eval($${deploymentfolder}.target)
export($$itemfiles)
export($$itempath)
INSTALLS += $$item
}
x86 {
target.path = /libs/x86
} else: armeabi-v7a {
target.path = /libs/armeabi-v7a
} else {
target.path = /libs/armeabi
}
export(target.path)
INSTALLS += target
} else:win32 {
copyCommand =
for(deploymentfolder, DEPLOYMENTFOLDERS) {
source = $$MAINPROFILEPWD/$$eval($${deploymentfolder}.source)
source = $$replace(source, /, \\)
sourcePathSegments = $$split(source, \\)
target = $$OUT_PWD/$$eval($${deploymentfolder}.target)/$$last(sourcePathSegments)
target = $$replace(target, /, \\)
target ~= s,\\\\\\.?\\\\,\\,
!isEqual(source,$$target) {
!isEmpty(copyCommand):copyCommand += &&
isEqual(QMAKE_DIR_SEP, \\) {
copyCommand += $(COPY_DIR) \"$$source\" \"$$target\"
} else {
source = $$replace(source, \\\\, /)
target = $$OUT_PWD/$$eval($${deploymentfolder}.target)
target = $$replace(target, \\\\, /)
copyCommand += test -d \"$$target\" || mkdir -p \"$$target\" && cp -r \"$$source\" \"$$target\"
}
}
}
!isEmpty(copyCommand) {
copyCommand = @echo Copying application data... && $$copyCommand
copydeploymentfolders.commands = $$copyCommand
first.depends = $(first) copydeploymentfolders
export(first.depends)
export(copydeploymentfolders.commands)
QMAKE_EXTRA_TARGETS += first copydeploymentfolders
}
} else:ios {
copyCommand =
for(deploymentfolder, DEPLOYMENTFOLDERS) {
source = $$MAINPROFILEPWD/$$eval($${deploymentfolder}.source)
source = $$replace(source, \\\\, /)
target = $CODESIGNING_FOLDER_PATH/$$eval($${deploymentfolder}.target)
target = $$replace(target, \\\\, /)
sourcePathSegments = $$split(source, /)
targetFullPath = $$target/$$last(sourcePathSegments)
targetFullPath ~= s,/\\.?/,/,
!isEqual(source,$$targetFullPath) {
!isEmpty(copyCommand):copyCommand += &&
copyCommand += mkdir -p \"$$target\"
copyCommand += && cp -r \"$$source\" \"$$target\"
}
}
!isEmpty(copyCommand) {
copyCommand = echo Copying application data... && $$copyCommand
!isEmpty(QMAKE_POST_LINK): QMAKE_POST_LINK += ";"
QMAKE_POST_LINK += "$$copyCommand"
export(QMAKE_POST_LINK)
}
} else:unix {
maemo5 {
desktopfile.files = $${TARGET}.desktop
desktopfile.path = /usr/share/applications/hildon
icon.files = $${TARGET}64.png
icon.path = /usr/share/icons/hicolor/64x64/apps
} else:!isEmpty(MEEGO_VERSION_MAJOR) {
desktopfile.files = $${TARGET}_harmattan.desktop
desktopfile.path = /usr/share/applications
icon.files = $${TARGET}80.png
icon.path = /usr/share/icons/hicolor/80x80/apps
} else { # Assumed to be a Desktop Unix
copyCommand =
for(deploymentfolder, DEPLOYMENTFOLDERS) {
source = $$MAINPROFILEPWD/$$eval($${deploymentfolder}.source)
source = $$replace(source, \\\\, /)
macx {
target = $$OUT_PWD/$${TARGET}.app/Contents/Resources/$$eval($${deploymentfolder}.target)
} else {
target = $$OUT_PWD/$$eval($${deploymentfolder}.target)
}
target = $$replace(target, \\\\, /)
sourcePathSegments = $$split(source, /)
targetFullPath = $$target/$$last(sourcePathSegments)
targetFullPath ~= s,/\\.?/,/,
!isEqual(source,$$targetFullPath) {
!isEmpty(copyCommand):copyCommand += &&
copyCommand += $(MKDIR) \"$$target\"
copyCommand += && $(COPY_DIR) \"$$source\" \"$$target\"
}
}
!isEmpty(copyCommand) {
copyCommand = @echo Copying application data... && $$copyCommand
copydeploymentfolders.commands = $$copyCommand
first.depends = $(first) copydeploymentfolders
export(first.depends)
export(copydeploymentfolders.commands)
QMAKE_EXTRA_TARGETS += first copydeploymentfolders
}
}
!isEmpty(target.path) {
installPrefix = $${target.path}
} else {
installPrefix = /opt/$${TARGET}
}
for(deploymentfolder, DEPLOYMENTFOLDERS) {
item = item$${deploymentfolder}
itemfiles = $${item}.files
$$itemfiles = $$eval($${deploymentfolder}.source)
itempath = $${item}.path
$$itempath = $${installPrefix}/$$eval($${deploymentfolder}.target)
export($$itemfiles)
export($$itempath)
INSTALLS += $$item
}
!isEmpty(desktopfile.path) {
export(icon.files)
export(icon.path)
export(desktopfile.files)
export(desktopfile.path)
INSTALLS += icon desktopfile
}
isEmpty(target.path) {
target.path = $${installPrefix}/bin
export(target.path)
}
INSTALLS += target
}
export (ICON)
export (INSTALLS)
export (DEPLOYMENT)
export (LIBS)
export (QMAKE_EXTRA_TARGETS)
}
#include "dynamicmodel.h"
DynamicModel::DynamicModel()
{
}
#ifndef DYNAMICMODEL_H
#define DYNAMICMODEL_H
#include "config.h"
#include <Eigen/Dense>
using namespace Eigen;
class DynamicModel
{
// constructors //
public:
DynamicModel();
// attributes //
public:
private:
unsigned int stateNb;
unsigned int commandNb;
double dt;
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 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;
private:
protected:
// accessors //
public:
virtual unsigned int getStateNb()=0;
virtual unsigned int getCommandNb()=0;
virtual stateMat_t &getfx()=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;
};
#endif // DYNAMICMODEL_H
#include "ilqrsolver.h"
/* Debug */
#include <iostream>
using namespace std;
/* */
using namespace Eigen;
ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction)
{
dynamicModel = &myDynamicModel;
costFunction = &myCostFunction;
stateNb = myDynamicModel.getStateNb();
commandNb = myDynamicModel.getCommandNb();
}
void ILQRSolver::initSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned int& myT,
double& mydt, unsigned int& myiterMax,double& mystopCrit)
{
xInit = myxInit;
xDes = myxDes;
T = myT;
dt = mydt;
iterMax = myiterMax;
stopCrit = mystopCrit;
xList = new stateVec_t[myT+1];
uList = new commandVec_t[myT];
updatedxList = new stateVec_t[myT+1];
updateduList = new commandVec_t[myT];
k.setZero();
K.setZero();
kList = new commandVec_t[myT];
KList = new commandR_stateC_t[myT];
alphaList[0] = 1.0;
alphaList[1] = 0.8;
alphaList[2] = 0.6;
alphaList[3] = 0.4;
alphaList[4] = 0.2;
alpha = 1.0;
}
void ILQRSolver::solveTrajectory()
{
stateVec_t* tmpxPtr;
commandVec_t* tmpuPtr;
initTrajectory();
for(iter=0;iter<iterMax;iter++)
{
backwardLoop();
forwardLoop();
if(changeAmount<stopCrit)
break;
tmpxPtr = xList;
tmpuPtr = uList;
xList = updatedxList;
updatedxList = tmpxPtr;
uList = updateduList;
updateduList = tmpuPtr;
}
}
void ILQRSolver::initTrajectory()
{
xList[0] = xInit;
commandVec_t zeroCommand;
zeroCommand.setZero();
for(unsigned int i=0;i<T;i++)
{
uList[i] = zeroCommand;
xList[i+1] = dynamicModel->computeNextState(dt,xList[i],zeroCommand);
}
}
void ILQRSolver::backwardLoop()
{
costFunction->computeFinalCostDeriv(xList[T],xDes);
nextVx = costFunction->getlx();
nextVxx = costFunction->getlxx();
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];
u = uList[i];
dynamicModel->computeAllModelDeriv(dt,x,u);
costFunction->computeAllCostDeriv(x,xDes,u);
Qx = costFunction->getlx() + dynamicModel->getfx().transpose() * nextVx;
Qu = costFunction->getlu() + dynamicModel->getfu().transpose() * nextVx;
Qxx = costFunction->getlxx() + dynamicModel->getfx().transpose() * (nextVxx+muEye) * dynamicModel->getfx();
Quu = costFunction->getluu() + dynamicModel->getfu().transpose() * (nextVxx+muEye) * dynamicModel->getfu();
Qux = costFunction->getlux() + dynamicModel->getfu().transpose() * (nextVxx+muEye) * dynamicModel->getfx();
Qxx += dynamicModel->computeTensorContxx(nextVx);
Qux += dynamicModel->computeTensorContux(nextVx);
Quu += dynamicModel->computeTensorContuu(nextVx);
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;
nextVx = Qx - K.transpose()*Quu*k;
nextVxx = Qxx - K.transpose()*Quu*K;
kList[i] = k;
KList[i] = K;
}
}
}
void ILQRSolver::forwardLoop()
{
changeAmount = 0.0;
updatedxList[0] = xInit;
// Line search to be implemented
alpha = 1.0;
for(unsigned int i=0;i<T;i++)
{
updateduList[i] = uList[i] + alpha*kList[i] + KList[i]*(updatedxList[i]-xList[i]);
updatedxList[i+1] = dynamicModel->computeNextState(dt,updatedxList[i],updateduList[i]);
for(unsigned int j=0;j<commandNb;j++)
{
changeAmount += abs(uList[i](j,0) - updateduList[i](j,0));
}
}
}
ILQRSolver::traj ILQRSolver::getLastSolvedTrajectory()
{
lastTraj.xList = updatedxList;
lastTraj.uList = updateduList;
lastTraj.iter = iter;
return lastTraj;
}
char ILQRSolver::isQuudefinitePositive(commandMat_t& Quu)
{
/*
Todo : check if Quu is definite positive
*/
return 1;
}
#ifndef ILQRSOLVER_H
#define ILQRSOLVER_H
#include "config.h"
#include "dynamicmodel.h"
#include "costfunction.h"
#include <Eigen/Dense>
using namespace Eigen;
class ILQRSolver
{
public:
struct traj
{
stateVec_t* xList;
commandVec_t* uList;
unsigned int iter;
};
public:
ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction);
private:
protected:
// attributes //
public:
private:
DynamicModel* dynamicModel;
CostFunction* costFunction;
</