Commit 64b5aa92 authored by flforget's avatar flforget
Browse files

several modifications + testModel updated

parent 49ff5709
SET(${PROJECT_NAME}_HEADERS
costfunctionromeoactuator.h
romeolinearactuator.h
config.h
dynamicmodel.h
romeosimpleactuator.h
romeoactuatorpos.h
costfunction.h
ilqrsolver.h
costfunctionromeoactuator.h
romeolinearactuator.h
config.h
dynamicmodel.h
romeosimpleactuator.h
romeoactuatorpos.h
costfunction.h
ilqrsolver.h
costfunctionpneumaticarmelbow.h
pneumaticarm_2linkmodel.hh
)
INSTALL(FILES ${${PROJECT_NAME}_HEADER} DESTINATION include)
......
......@@ -7,8 +7,8 @@
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::MatrixXd)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)
#define stateSize 4
#define commandSize 1
#define stateSize 8
#define commandSize 2
// typedef for stateSize types
typedef Eigen::Matrix<double,stateSize,1> stateVec_t; // stateSize x 1
......
......@@ -14,14 +14,9 @@ class CostFunctionPneumaticarmElbow : public CostFunction
public:
CostFunctionPneumaticarmElbow();
private:
stateMat_t Q, Qf;
stateMat_t Q;
stateMat_t Qf;
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 //
......@@ -31,18 +26,13 @@ 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);
void computeAllCostDeriv(const stateVec_t& X, const commandVec_t& U);
void computeFinalCostDeriv(const stateVec_t& X);
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
......@@ -5,14 +5,14 @@
/* This is a program to model the pneumatic muscle based joint of the Pneumatic 7R arm */
#ifndef PNEUMATICARMNONLINEARMODEL_H
#define PNEUMATICARMNONLINEARMODEL_H
#include <iostream>
#include "config.h"
#include <vector>
#include "dynamicmodel.h"
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;
class PneumaticarmNonlinearModel : public DynamicModel
{
public:
......@@ -24,34 +24,24 @@ protected:
public:
private:
double dt;
unsigned int stateNb;
unsigned int commandNb;
// Muscle parameters
// Muscle parameters
double lo, alphao, k,ro,R,a,b,emax,lb,lt,epsb,epst;
double time_constant1, time_constant2;
// Joint parameters
double m;
double link_l;
double g;
double I;
double fv;
stateMat_t Id;
std::vector<double> x1;
/*stateMat_t Id;
stateMat_t A;
stateMat_t Ad;
stateR_commandC_t B;
stateR_commandC_t Bd;
double A13atan,A10;
double A33atan;
double A33atan;*/
stateVec_t Xreal;
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;
stateMat_t QxxCont;
commandMat_t QuuCont;
commandR_stateC_t QuxCont;
......@@ -59,23 +49,16 @@ private:
protected:
// methods //
public:
stateVec_t computeNextState(double& dt, const stateVec_t& X, const commandVec_t &U);
void computeAllModelDeriv(double& dt, const stateVec_t& X, const commandVec_t &U);
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);
double getX(unsigned int i);
private:
protected:
// accessors //
public:
unsigned int getStateNb();
unsigned int getCommandNb();
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();
};
......
......@@ -3,8 +3,8 @@
CostFunctionPneumaticarmElbow::CostFunctionPneumaticarmElbow()
{
Q <<1e4*1.0, 0.0, 0.0, 0.0, 0,0,0,0,
0.0,1e4*1.0, 0.0, 0.0, 0,0,0,0,
Q <<1e-1*1, 0.0, 0.0, 0.0, 0,0,0,0,
0.0,1e-1*1, 0.0, 0.0, 0,0,0,0,
2.0e-2*0.0, 0.0, 1e-5*0.0, 0.0, 0,0,0,0,
0.0, 0.0, 0.0, 1e-5*0.0, 0,0,0,0,
0,0,0,0, 0,0,0,0,
......@@ -15,9 +15,17 @@ CostFunctionPneumaticarmElbow::CostFunctionPneumaticarmElbow()
0.0,1e-3*0.0, 0.0, 0.0,
0.0, 0.0, 5e-5*1.0, 0.0,
0.0, 0.0, 0.0, 5e-5*1.0;*/
Qf = Q;
R << 1e-2,0,
0, 1e-4;
/*Qf <<1e0*5, 0.0, 0.0, 0.0, 0,0,0,0,
0.0,1e0*8, 0.0, 0.0, 0,0,0,0,
2.0e-2*0.0, 0.0, 1e-5*0.0, 0.0, 0,0,0,0,
0.0, 0.0, 0.0, 1e-5*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,0, 0,0,0,0;*/
Qf = Q; Qf(0,0) = 0; Qf(1,1) = 0;
R << 5e-3,0,
0, 5e-3;
lxx = Q;
luu = R;
//lux << 0.0,0.0;
......@@ -25,46 +33,15 @@ CostFunctionPneumaticarmElbow::CostFunctionPneumaticarmElbow()
lx.setZero();
}
void CostFunctionPneumaticarmElbow::computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U)
void CostFunctionPneumaticarmElbow::computeAllCostDeriv(const stateVec_t& X, const commandVec_t& U)
{
lx = Q*(X-Xdes);
//lx(0,0) = 1e-3*8*(X(0,0)-Xdes(0,0));
lx = Q*X;
lu = R*U;
}
void CostFunctionPneumaticarmElbow::computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes)
{
lx = Qf*(X-Xdes);
//lx(0,0) = 1e-3*8*1.0*(X(0,0)-Xdes(0,0));
}
// accessors //
stateVec_t CostFunctionPneumaticarmElbow::getlx()
{
return lx;
}
stateMat_t CostFunctionPneumaticarmElbow::getlxx()
void CostFunctionPneumaticarmElbow::computeFinalCostDeriv(const stateVec_t& X)
{
return lxx;
lx = Q*X;
}
commandVec_t CostFunctionPneumaticarmElbow::getlu()
{
return lu;
}
commandMat_t CostFunctionPneumaticarmElbow::getluu()
{
return luu;
}
stateR_commandC_t CostFunctionPneumaticarmElbow::getlxu()
{
return lxu;
}
commandR_stateC_t CostFunctionPneumaticarmElbow::getlux()
{
return lux;
}
......@@ -6,7 +6,7 @@
#include "pneumaticarm_2linkmodel.hh"
#include <math.h>
using namespace std;
//#define pi M_PI
double PI = 3.14;
// Joint 1 parameters
......@@ -24,7 +24,7 @@ double j2fv1 = 3.0;
double j2Pmax1 = 3.0;
// Joint 3 parameters
double j4lo = 0.185;
double j4alphao = 20*PI/180;
double j4alphao = 23*PI/180;
double j4k = 1.25;
double j4ro = 0.0085;
double j4R = 0.015;
......@@ -67,15 +67,19 @@ double F_triceps = P2*ftterm;
double Torque_pneumatics = (F_biceps -F_triceps )*R;
return (Torque_pneumatics);
}
stateVec_t computejointderiv(double& dt, const stateVec_t& X,const commandVec_t& U)
stateVec_t computejointderiv(double& dt, const stateVec_t& Xe,const stateVec_t& Xdes,const commandVec_t& U)
{
// result(1,0)-=A10*sin(X(0));
//result(3,0)+=A33atan*atan(a*X(3,0));
stateVec_t X;
X = Xe + Xdes;
stateVec_t jointstate_deriv;
double m = -0.0023;
double c = 0.0136;
double p1 = -0.009338; //%(-0.01208, -0.006597)
double p2 = 0.01444;
j2R1 = m*X(4) + c;
j2R1 = p1*X(0) + p2;
//P1_ = X(4);
//P2_ = X(5);
double wnb1 = 9;
......@@ -98,15 +102,15 @@ stateVec_t computejointderiv(double& dt, const stateVec_t& X,const commandVec_t&
double m11_const = j2link1_I + j2m1*pow(j2link1_lc,2) + j4link2_I + j4m2*(pow(j2link1_l,2) +
pow(j4link2_lc,2)) + mb*(pow(j2link1_l,2) + pow(j4link2_l,2));
double m11_var = j4m2*2*j2link1_l*j4link2_lc*cos(X(2)) +
double m11_var = j4m2*2*j2link1_l*j4link2_lc*cos(X(1)) +
mb*2*j2link1_l*j4link2_l*cos(X(1));
double m11 = (m11_var - m11_const);
double m11 = (m11_var + m11_const);
double m12_const = j4link2_I + j4m2*pow(j4link2_lc,2) + mb*pow(j4link2_l,2);
double m12_var = j4m2*j2link1_l*j4link2_lc*cos(X(1)) +
mb*j2link1_l*j4link2_l*cos(X(1));
double m12 = (m12_var-m12_const);
double m12 = (m12_var + m12_const);
double m22 = j4link2_I + j4m2*pow(j4link2_lc,2) + mb*pow(j4link2_l,2);
double det = m11*m22 - m12*m12;
double inv_m11 = m22/det;
......@@ -127,24 +131,24 @@ stateVec_t computejointderiv(double& dt, const stateVec_t& X,const commandVec_t&
double g1 = (j2m1*j2link1_lc + j4m2*j2link1_l + mb*j2link1_l)*sin(X(0)) + (
j4m2*j4link2_lc + mb*j4link2_l)*sin(X(0) + X(1));
double g2 = (j4m2*j4link2_lc + mb*j4link2_l)*sin(X(0) + X(1));
double g2 = 9.8*(j4m2*j4link2_lc + mb*j4link2_l)*sin(X(0) + X(1));
//%% viscous friction matrix
double tf1 = -j2fv1*X(2);
double tf2 = -j4fv2*X(3);
double Mat1 = inv_m11*(T1 + tf1 - c1 -g1) + inv_m12*(T2+tf2 -c2 -g2);
double Mat2 = inv_m21*(T1 + tf1 - c1 -g1) + inv_m22*(T2+tf2 -c2 -g2);
double Mat1 = inv_m11*(T1 + tf1 - c1 - 9.8*g1) + inv_m12*(T2+tf2 -c2 -g2);
double Mat2 = inv_m21*(T1 + tf1 - c1 - 9.8*g1) + inv_m22*(T2+tf2 -c2 -g2);
jointstate_deriv(0) = X(2); //%joint_state(2);
jointstate_deriv(1) = X(3); //%joint_state(2);
jointstate_deriv(2) = Mat1;
jointstate_deriv(3) = Mat2;
//stateVec_t result = X + dt*jointstate_deriv;
stateVec_t result = X + dt*jointstate_deriv;
//fx = jointstate_deriv;
return jointstate_deriv;
return result;
}
......@@ -153,6 +157,8 @@ PneumaticarmNonlinearModel::PneumaticarmNonlinearModel(double& mydt)
{
stateNb=8;
commandNb=2;
x1.resize(2);
dt = mydt;
//state_vector_.resize(8);
//state_derivative_.resize(8);
//control_vector_.resize(2);
......@@ -176,15 +182,19 @@ PneumaticarmNonlinearModel::PneumaticarmNonlinearModel(double& mydt)
QuxCont.setZero();*/
}
stateVec_t PneumaticarmNonlinearModel::computeNextState(double& dt, const stateVec_t& X,const commandVec_t& U)
stateVec_t PneumaticarmNonlinearModel::computeNextState(double& dt, const stateVec_t& Xe,const stateVec_t& Xdes,const commandVec_t& U)
{
// result(1,0)-=A10*sin(X(0));
//result(3,0)+=A33atan*atan(a*X(3,0));
stateVec_t jointstate_deriv;
stateVec_t X = Xe + Xdes;
double m = -0.0023;
double c = 0.0136;
double p1 = -0.009338; //%(-0.01208, -0.006597)
double p2 = 0.01444;
j2R1 = m*X(4) + c;
j2R1 = p1*X(0) + p2;
//double P1_ = X(4);
//double P2_ = X(5);
double wnb1 = 9;
......@@ -207,15 +217,16 @@ stateVec_t PneumaticarmNonlinearModel::computeNextState(double& dt, const stateV
double m11_const = j2link1_I + j2m1*pow(j2link1_lc,2) + j4link2_I + j4m2*(pow(j2link1_l,2) +
pow(j4link2_lc,2)) + mb*(pow(j2link1_l,2) + pow(j4link2_l,2));
double m11_var = j4m2*2*j2link1_l*j4link2_lc*cos(X(2)) +
double m11_var = j4m2*2*j2link1_l*j4link2_lc*cos(X(1)) +
mb*2*j2link1_l*j4link2_l*cos(X(1));
double m11 = (m11_var - m11_const);
double m11 = (m11_var + m11_const);
double m12_const = j4link2_I + j4m2*pow(j4link2_lc,2) + mb*pow(j4link2_l,2);
double m12_var = j4m2*j2link1_l*j4link2_lc*cos(X(1)) +
mb*j2link1_l*j4link2_l*cos(X(1));
double m12 = (m12_var-m12_const);
double m12 = (m12_var + m12_const);
double m22 = j4link2_I + j4m2*pow(j4link2_lc,2) + mb*pow(j4link2_l,2);
double det = m11*m22 - m12*m12;
double inv_m11 = m22/det;
......@@ -236,66 +247,67 @@ stateVec_t PneumaticarmNonlinearModel::computeNextState(double& dt, const stateV
double g1 = (j2m1*j2link1_lc + j4m2*j2link1_l + mb*j2link1_l)*sin(X(0)) + (
j4m2*j4link2_lc + mb*j4link2_l)*sin(X(0) + X(1));
double g2 = (j4m2*j4link2_lc + mb*j4link2_l)*sin(X(0) + X(1));
double g2 = 9.8*(j4m2*j4link2_lc + mb*j4link2_l)*sin(X(0) + X(1));
//%% viscous friction matrix
double tf1 = -j2fv1*X(2);
double tf2 = -j4fv2*X(3);
double Mat1 = inv_m11*(T1 + tf1 - c1 -g1) + inv_m12*(T2+tf2 -c2 -g2);
double Mat2 = inv_m21*(T1 + tf1 - c1 -g1) + inv_m22*(T2+tf2 -c2 -g2);
double Mat1 = inv_m11*(T1 + tf1 - c1 - 9.8*g1) + inv_m12*(T2+tf2 -c2 -g2);
double Mat2 = inv_m21*(T1 + tf1 - c1 - 9.8*g1) + inv_m22*(T2+tf2 -c2 -g2);
jointstate_deriv(0) = X(2); //%joint_state(2);
jointstate_deriv(1) = X(3); //%joint_state(2);
jointstate_deriv(2) = Mat1;
jointstate_deriv(3) = Mat2;
stateVec_t result = X + dt*jointstate_deriv;
x1[0] = X(0); x1[1] = X(1);
//fx = jointstate_deriv;
return result;
}
void PneumaticarmNonlinearModel::computeAllModelDeriv(double& dt, const stateVec_t& X,const commandVec_t& U)
void PneumaticarmNonlinearModel::computeAllModelDeriv(double& dt, const stateVec_t& Xe,const stateVec_t& Xdes,const commandVec_t& U)
{
//fx = fxBase;
double dh = 1e-4;;
Xreal = Xe + Xdes;
double dh = 5e-3;
stateVec_t tempX, derivplus, derivminus;
commandVec_t tempU;
tempX = X;
tempX = Xe;
for(unsigned int i = 0; i<8; i++)
{
for(unsigned int j=0; j<8;j++)
{
tempX = X;
tempX(j) = X(j) + dh;
derivplus = this->computeNextState(dt, tempX, U);
tempX = X;
tempX(j) = X(j)-dh;
derivminus = this->computeNextState(dt, tempX, U);
tempX = Xe;
tempX(j) = Xe(j) + dh;
derivplus = computejointderiv(dt, tempX,Xdes, U); //cout<< "deriv:" <<derivplus(0)<< '\n' << endl;
tempX = Xe;
tempX(j) = Xe(j) - dh;
derivminus = computejointderiv(dt, tempX,Xdes, U);
fx(i,j) = (derivplus(i) - derivminus(i))/(2*dh);
}
}
derivplus.setZero(); derivminus.setZero();
for(unsigned int i = 0; i<8; i++)
{
for(unsigned int j=0; j<2;j++)
{
tempU = U;
tempU(j) = U(j) + dh;
derivplus = computejointderiv(dt, X, tempU);
derivplus = computejointderiv(dt, Xe,Xdes, tempU);
tempU = U;
tempU(j) = U(j)-dh;
derivminus = computejointderiv(dt, tempX, tempU);
derivminus = computejointderiv(dt, Xe,Xdes, tempU);
fu(i,j) = (derivplus(i) - derivminus(i))/(2*dh);
}
}
//fx.setZero(); fu.setZero();
}
stateMat_t PneumaticarmNonlinearModel::computeTensorContxx(const stateVec_t& nextVx)
{
QxxCont = nextVx[0]*fxx[0] + nextVx[1]*fxx[1] + nextVx[2]*fxx[2] + nextVx[3]*fxx[3];
QxxCont.setZero();//nextVx[0]*fxx[0] + nextVx[1]*fxx[1] + nextVx[2]*fxx[2] + nextVx[3]*fxx[3];
return QxxCont;
}
......@@ -310,44 +322,18 @@ commandR_stateC_t PneumaticarmNonlinearModel::computeTensorContux(const stateVec
}
/// accessors ///
unsigned int PneumaticarmNonlinearModel::getStateNb()
{
return stateNb;
}
unsigned int PneumaticarmNonlinearModel::getCommandNb()
{
return commandNb;
}
stateMat_t& PneumaticarmNonlinearModel::getfx()
/*stateMat_t& PneumaticarmNonlinearModel::getfx()
{
return fx;
}
stateTens_t& PneumaticarmNonlinearModel::getfxx()
{
return fxx;
}
stateR_commandC_t& PneumaticarmNonlinearModel::getfu()
{
return fu;
}
stateR_commandC_commandD_t& PneumaticarmNonlinearModel::getfuu()
{
return fuu;
}
stateR_stateC_commandD_t& PneumaticarmNonlinearModel::getfxu()
{
return fxu;
}
stateR_commandC_stateD_t& PneumaticarmNonlinearModel::getfux()
double PneumaticarmNonlinearModel::getX(unsigned int i)
{
return fux;
return x1[i];
}
*/
......@@ -4,9 +4,8 @@
#include "config.h"
#include "ilqrsolver.h"
#include "romeosimpleactuator.h"
#include "romeolinearactuator.h"
#include "costfunctionromeoactuator.h"
#include "pneumaticarm_2linkmodel.hh"
#include "costfunctionpneumaticarmelbow.h"
#include <time.h>
#include <sys/time.h>
......@@ -21,30 +20,31 @@ int main()
double texec=0.0;
stateVec_t xinit,xDes;
xinit << -3.0,0.0,0.0,0.0;
xDes << 0.0,0.0,0.0,0.0;
xinit << 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0;
xDes << 0.5,1.0,0.0,0.0,0.0,0.0,0.0,0.0;
unsigned int T = 50;
double dt=1e-4;
unsigned int iterMax = 20;
double dt=5e-3;
unsigned int iterMax = 100;
double stopCrit = 1e-5;
stateVecTab_t xList;
commandVecTab_t uList;
ILQRSolver::traj lastTraj;
RomeoSimpleActuator romeoActuatorModel(dt);
RomeoLinearActuator romeoLinearModel(dt);
CostFunctionRomeoActuator costRomeoActuator;
ILQRSolver testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator,ENABLE_FULLDDP,ENABLE_QPBOX);
testSolverRomeoActuator.FirstInitSolver(xinit,xDes,T,dt,iterMax,stopCrit);
PneumaticarmNonlinearModel model(dt);
CostFunctionPneumaticarmElbow cost;
int N = 100;
ILQRSolver solver(model,cost,DISABLE_FULLDDP,DISABLE_QPBOX);
solver.FirstInitSolver(xinit,xDes,T,dt,iterMax,stopCrit);
int N = 10;
gettimeofday(&tbegin,NULL);
for(int i=0;i<N;i++) testSolverRomeoActuator.solveTrajectory();
for(int i=0;i<N;i++) solver.solveTrajectory();
gettimeofday(&tend,NULL);
lastTraj = testSolverRomeoActuator.getLastSolvedTrajectory();
lastTraj = solver.getLastSolvedTrajectory();
xList = lastTraj.xList;
uList = lastTraj.uList;
unsigned int iter = lastTraj.iter;
......
......@@ -4,10 +4,8 @@
#include "config.h"
#include "ilqrsolver.h"
#include "romeosimpleactuator.h"
#include "romeolinearactuator.h"
#include "romeoactuatorpos.h"
#include "costfunctionromeoactuator.h"
#include "pneumaticarm_2linkmodel.hh"
#include "costfunctionpneumaticarmelbow.h"
#include <time.h>
#include <sys/time.h>
......@@ -23,35 +21,48 @@ int main()
stateVec_t xinit,xDes,x;
commandVec_t u;
u << 1.0;
u << 2.0,2.0;
xinit << -3.0,0.0,0.0,0.0;
xDes << 0.0,0.0,0.0,0.0;
xinit << 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0;
xDes << 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0;
x = xinit;
unsigned int T = 50;
double dt=1e-4;
double dt=3e-5;