Commit 10b7b125 authored by flforget's avatar flforget
Browse files

[major]Add cmake file to find qpOASES

[minor]new executable fo test QPBox
parent 451068fb
#ifndef ROMEOACTUATORPOS_H
#define ROMEOACTUATORPOS_H
#include "config.h"
#include "dynamicmodel.h"
#include <Eigen/Dense>
using namespace Eigen;
class RomeoActuatorPos : public DynamicModel
{
public:
RomeoActuatorPos(double& mydt);
private:
protected:
// attributes //
public:
private:
double dt;
//static const unsigned int stateNb=4;
//static const unsigned int commandNb=1;
public:
static const double k;
static const double R;
static const double Jm;
static const double Jl;
static const double fvm;
static const double Cf0;
static const double a;
private:
stateVec_t Xreal;
stateMat_t Id;
stateMat_t A;
stateMat_t Ad;
stateR_commandC_t B;
stateR_commandC_t Bd;
double A13atan;
double A33atan;
stateMat_t fxBase;
stateR_commandC_t fuBase;
stateMat_t QxxCont;
commandMat_t QuuCont;
commandR_stateC_t QuxCont;
protected:
// methods //
public:
stateVec_t computeNextState(double& dt, const stateVec_t& X,const stateVec_t& Xdes, const commandVec_t &U);
void computeAllModelDeriv(double& dt, const stateVec_t& X,const stateVec_t& Xdes, const commandVec_t &U);
stateMat_t computeTensorContxx(const stateVec_t& nextVx);
commandMat_t computeTensorContuu(const stateVec_t& nextVx);
commandR_stateC_t computeTensorContux(const stateVec_t& nextVx);
private:
protected:
// accessors //
public:
};
#endif // ROMEOSIMPLEACTUATOR_H
#include "romeoactuatorpos.h"
#include <math.h>
#define pi M_PI
const double RomeoActuatorPos::k=1000.0;
const double RomeoActuatorPos::R=200.0;
const double RomeoActuatorPos::Jm=138*1e-7;
const double RomeoActuatorPos::Jl=0.1;
const double RomeoActuatorPos::fvm=0.01;
const double RomeoActuatorPos::Cf0=0.1;
const double RomeoActuatorPos::a=10.0;
RomeoActuatorPos::RomeoActuatorPos(double& mydt)
{
stateNb=4;
commandNb=1;
dt = mydt;
Id.setIdentity();
A.setZero();
A << 0.0, 1.0,0.0,0.0,
-k/(Jm*R*R),0.0,k/(R*Jm),0.0,
0.0,0.0,0.0,1.0,
k/(Jl*R),0.0,-k/Jl,0.0;
Ad = (A*dt+Id);
fx = Ad;
B << 0.0,
1.0/Jm,
0.0,
0.0;
Bd = dt*B;
fu = Bd;
fxx[0].setZero();
fxx[1].setZero();
fxx[2].setZero();
fxx[3].setZero();
fxu[0].setZero();
fxu[0].setZero();
fuu[0].setZero();
fux[0].setZero();
fxu[0].setZero();
QxxCont.setZero();
QuuCont.setZero();
QuxCont.setZero();
lowerCommandBounds << -40.0;
upperCommandBounds << 40.0;
}
stateVec_t RomeoActuatorPos::computeNextState(double& dt, const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t& U)
{
stateVec_t result = Ad*X + Bd*U;
return result;
}
void RomeoActuatorPos::computeAllModelDeriv(double& dt, const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t& U)
{
}
stateMat_t RomeoActuatorPos::computeTensorContxx(const stateVec_t& nextVx)
{
return QxxCont;
}
commandMat_t RomeoActuatorPos::computeTensorContuu(const stateVec_t& nextVx)
{
return QuuCont;
}
commandR_stateC_t RomeoActuatorPos::computeTensorContux(const stateVec_t& nextVx)
{
return QuxCont;
}
#include <iostream>
#include <fstream>
#include "config.h"
#include "ilqrsolver.h"
#include "romeosimpleactuator.h"
#include "romeolinearactuator.h"
#include "romeoactuatorpos.h"
#include "costfunctionromeoactuator.h"
#include <time.h>
#include <sys/time.h>
using namespace std;
using namespace Eigen;
int main()
{
struct timeval tbegin,tend;
double texec=0.0;
stateVec_t xinit,xDes,x;
commandVec_t u;
u << 1.0;
xinit << -3.0,0.0,0.0,0.0;
xDes << 0.0,0.0,0.0,0.0;
x = xinit;
unsigned int T = 50;
double dt=1e-4;
unsigned int iterMax = 100;
double stopCrit = 1e-5;
stateVec_t* xList;
commandVec_t* uList;
ILQRSolver::traj lastTraj;
RomeoSimpleActuator romeoActuatorModel(dt);
RomeoActuatorPos romeoActuator(dt);
RomeoLinearActuator romeoLinearModel(dt);
CostFunctionRomeoActuator costRomeoActuator;
/*cout << romeoActuatorModel.getCommandNb() << endl;
for(unsigned int i=0;i<100;i++)
{
x = romeoActuator.computeNextState(dt,x,xDes,u);
cout << x << endl<<"-"<<endl;
}*/
ILQRSolver testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator,1);
testSolverRomeoActuator.FirstInitSolver(xinit,xDes,T,dt,iterMax,stopCrit);
int N = 1;
gettimeofday(&tbegin,NULL);
for(int i=0;i<N;i++) testSolverRomeoActuator.solveTrajectory();
gettimeofday(&tend,NULL);
lastTraj = testSolverRomeoActuator.getLastSolvedTrajectory();
xList = lastTraj.xList;
uList = lastTraj.uList;
unsigned int iter = lastTraj.iter;
texec=((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000.;
texec /= N;
cout << endl;
cout << "temps d'execution total du solveur ";
cout << texec << endl;
cout << "temps d'execution par pas de temps ";
cout << texec/T << endl;
cout << "Nombre d'itérations : " << iter << endl;
ofstream fichier("results.csv",ios::out | ios::trunc);
if(fichier)
{
fichier << "tau,tauDot,q,qDot,u" << endl;
for(int i=0;i<T;i++) fichier << xList[i](0,0) << "," << xList[i](1,0) << "," << xList[i](2,0) << "," << xList[i](3,0) << "," << uList[i](0,0) << endl;
fichier << xList[T](0,0) << "," << xList[T](1,0) << "," << xList[T](2,0) << "," << xList[T](3,0) << "," << 0.0 << endl;
fichier.close();
}
else
cerr << "erreur ouverte fichier" << endl;
return 0;
}
#.rst:
# FindqpOASES
# -----------
#
# Try to find the qpOASES library.
# Once done this will define the following variables::
#
# qpOASES_FOUND - System has qpOASES
# qpOASES_INCLUDE_DIRS - qpOASES include directory
# qpOASES_LIBRARIES - qpOASES libraries
#
# qpOASES does not have an "install" step, and the includes are in the source
# tree, while the libraries are in the build tree.
# Therefore the environment and cmake variables `qpOASES_SOURCE_DIR` and
# `qpOASES_BINARY_DIR` will be used to locate the includes and libraries.
#=============================================================================
# Copyright 2014 iCub Facility, Istituto Italiano di Tecnologia
# Authors: Daniele E. Domenichelli <daniele.domenichelli@iit.it>
#
# Distributed under the OSI-approved BSD License (the "License");
# see accompanying file Copyright.txt for details.
#
# This software is distributed WITHOUT ANY WARRANTY; without even the
# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
# See the License for more information.
#=============================================================================
# (To distribute this file outside of YCM, substitute the full
# License text for the above reference.)
MACRO(SEARCH_FOR_QPOASES)
include(FindPackageHandleStandardArgs)
find_path(qpOASES_INCLUDEDIR
NAMES qpOASES.hpp
HINTS "${qpOASES_SOURCE_DIR}"
ENV qpOASES_SOURCE_DIR
PATH_SUFFIXES include)
find_library(qpOASES_LIB
NAMES qpOASES
HINTS "${qpOASES_BINARY_DIR}"
ENV qpOASES_BINARY_DIR
PATH_SUFFIXES lib
libs)
set(qpOASES_INCLUDE_DIRS ${qpOASES_INCLUDEDIR})
set(qpOASES_LIBRARIES ${qpOASES_LIB})
find_package_handle_standard_args(qpOASES DEFAULT_MSG qpOASES_LIBRARIES
qpOASES_INCLUDE_DIRS)
set(qpOASES_FOUND ${QPOASES_FOUND})
ENDMACRO(SEARCH_FOR_QPOASES)
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