[major]Add cmake file to find qpOASES

[minor]new executable fo test QPBox

cpp/include/romeoactuatorpos.h

+#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

cpp/src/romeoactuatorpos.cpp

+#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;
+}

cpp/test/testModel.cpp

+#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;
+
+}

private_cmake/FindqpOASES.cmake

+#.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)