Commit a9e54ae4 authored by flforget's avatar flforget
Browse files

[major]bug fix with peznumaticarm model

parent be542fa1
SET(${PROJECT_NAME}_HEADERS
costfunctionromeoactuator.h
romeolinearactuator.h
config.h
dynamicmodel.h
romeosimpleactuator.h
romeoactuatorpos.h
costfunction.h
ilqrsolver.h
costfunctionpneumaticarmelbow.h
pneumaticarm_2linkmodel.hh
costfunctionromeoactuator.h
romeolinearactuator.h
config.h
dynamicmodel.h
romeosimpleactuator.h
romeoactuatorpos.h
costfunction.h
ilqrsolver.h
)
INSTALL(FILES ${${PROJECT_NAME}_HEADER} DESTINATION include)
......
......@@ -31,17 +31,8 @@ private:
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;*/
stateVec_t Xreal;
stateMat_t fx,fxBase;
stateR_commandC_t fu,fuBase;
stateMat_t QxxCont;
commandMat_t QuuCont;
commandR_stateC_t QuxCont;
......
// Copydight (c) 2015 LAAS_CNRS //
// Author: Ganesh Kumar //
/* This is a program to model the pneumatic muscle based joint of the Pneumatic 7R arm */
#ifndef PNEUMATICARMMODEL_HH
#define PNEUMATICARMMODEL_HH
//#include <Eigen/Dense>
//#include <Eigen/Core>
#include <math.h>
#include <vector>
#define GRAVITY 9.81
#define PI 3.14159265
#include "config.h"
#include "dynamicmodel.h"
#include <Eigen/Dense>
using namespace std;
//using namespace Eigen;
class PneumaticarmModel
{
protected:
double length_;
double mass_;
double friction_;
double Torque_, TorqueDes_;
float pressure_muscle1_, pressure_muscle2_, pressure_musclebase_;
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;
int nDOF_;
unsigned int n_;
std::vector<double> state_vector_;
std::vector<double> state_derivative_;
std::vector<double> control_vector_;
public:
/// Constructor
PneumaticarmModel();
virtual ~PneumaticarmModel();
void setProblemDimension (int n);
void setParameters (void);
//void setpidcoeff(int p, int i, int d);
void computeStateDerivative (double time);
void integrateRK4 (double time, double timeStep);
double InverseModel(vector<double>& reference);
// vector<double> integrateEuler (double time, double timeStep);
void Set_ControlVector(double value, unsigned int idx);
void Set_StateVector(double value, unsigned int idx);
double Get_StateVector(unsigned idx);
double Get_ControlVector(unsigned int idx);
double Get_Torque();
double Get_TorqueDes();
//VectorXd getControl (VectorXd statevector, double reference_position, double position);
};
#endif
#ifndef PNEUMATICARMNONLINEARMODEL_H
#define PNEUMATICARMNONLINEARMODEL_H
#include "config.h"
#include "dynamicmodel.h"
#include <Eigen/Dense>
using namespace Eigen;
class PneumaticarmNonlinearModel : public DynamicModel
{
public:
PneumaticarmNonlinearModel(double& mydt);
private:
protected:
// attributes //
public:
private:
double dt;
unsigned int stateNb;
unsigned int commandNb;
// 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;
stateMat_t A;
stateMat_t Ad;
stateR_commandC_t B;
stateR_commandC_t Bd;
double A13atan,A10;
double A33atan;
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;
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);
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:
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();
};
#endif // PNEUMATICARMNONLINEARMODEL_H
#include "costfunctionpneumaticarmelbow.h"
#include "iostream"
using namespace std;
CostFunctionPneumaticarmElbow::CostFunctionPneumaticarmElbow()
{
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,
Q <<1e4*1, 0.0, 0.0, 0.0, 0,0,0,0,
0.0,1e4*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,
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 <<8e4*1.0, 0.0, 0.0, 0.0,
/* Qf <<8e4*1.0, 0.0, 0.0, 0.0,
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 <<1e0*5, 0.0, 0.0, 0.0, 0,0,0,0,
/*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,
......@@ -23,9 +26,9 @@ CostFunctionPneumaticarmElbow::CostFunctionPneumaticarmElbow()
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;
Qf = Q; //Qf(0,0) = 0; Qf(1,1) = 0;
R << 1e-5,0,
0, 1e-5;
lxx = Q;
luu = R;
//lux << 0.0,0.0;
......@@ -42,6 +45,5 @@ void CostFunctionPneumaticarmElbow::computeAllCostDeriv(const stateVec_t& X, con
void CostFunctionPneumaticarmElbow::computeFinalCostDeriv(const stateVec_t& X)
{
lx = Q*X;
}
}
......@@ -72,7 +72,7 @@ void ILQRSolver::FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsign
void ILQRSolver::initSolver(stateVec_t& myxInit, stateVec_t& myxDes)
{
xInit = myxInit-myxDes;
xInit = myxInit - myxDes;
xDes = myxDes;
}
......@@ -85,7 +85,7 @@ void ILQRSolver::solveTrajectory()
forwardLoop();
if(changeAmount<stopCrit)
{
break;
break;
}
tmpxPtr = xList;
tmpuPtr = uList;
......@@ -100,11 +100,14 @@ void ILQRSolver::initTrajectory()
{
xList[0] = xInit;
commandVec_t zeroCommand;
stateVec_t Xe;
Xe = xInit;
zeroCommand.setZero();
for(unsigned int i=0;i<T;i++)
{
uList[i] = zeroCommand;
xList[i+1] = dynamicModel->computeNextState(dt,xList[i],xDes,zeroCommand);
xList[i+1] = dynamicModel->computeNextState(dt,xList[i] ,xDes,zeroCommand) - xDes;
//Xe = xList[i] - xDes;
}
}
......@@ -178,7 +181,7 @@ void ILQRSolver::backwardLoop()
{
k = -QuuInv*Qu;
K = -QuuInv*Qux;
}
}
/*nextVx = Qx - K.transpose()*Quu*k;
nextVxx = Qxx - K.transpose()*Quu*K;*/
......@@ -200,8 +203,8 @@ void ILQRSolver::forwardLoop()
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],xDes,updateduList[i]);
updateduList[i] = uList[i] + alpha*kList[i] + KList[i]*(updatedxList[i] - xList[i]);
updatedxList[i+1] = dynamicModel->computeNextState(dt,updatedxList[i],xDes,updateduList[i]) - xDes;
for(unsigned int j=0;j<commandNb;j++)
{
changeAmount += abs(uList[i](j,0) - updateduList[i](j,0));
......
// Authors: Ganesh Kumar
/* ******** It is a trial program for modelling a Pneumatic muscle and
/* ******** It is a trial program for modelling a Pneumatic muscle and
******** controlling it in closeloop under xenomai realtime kernel ********/
......@@ -54,7 +54,6 @@ double epsb = (1-(lb/lo));
double lt = lo*(1-emax_triceps) + R*theta;
double epst = (1-(lt/lo));
double P1 = x(i+4);
double P2 = pmax-x(i+4);
//P = [P1;P2];
......@@ -72,14 +71,14 @@ stateVec_t computejointderiv(double& dt, const stateVec_t& Xe,const stateVec_t&
// result(1,0)-=A10*sin(X(0));
//result(3,0)+=A33atan*atan(a*X(3,0));
stateVec_t X;
X = Xe + Xdes;
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 = p1*X(0) + p2;
j2R1 = p1*X(0) + p2;
//P1_ = X(4);
//P2_ = X(5);
double wnb1 = 9;
......@@ -93,23 +92,23 @@ stateVec_t computejointderiv(double& dt, const stateVec_t& Xe,const stateVec_t&
jointstate_deriv(5) = X(7);
jointstate_deriv(6) = -pow(wnb1,2)*X(4) - 2*wnb1*X(6) + pow(wnb1,2)*Pdes1;
jointstate_deriv(7) = -pow(wnb2,2)*X(5) - 2*wnb2*X(7) + pow(wnb2,2)*Pdes2;
//%% Force calculation
double T1 = Torque_net(X,j2lo1,j2alphao1,j2k1,j2ro1,j2R1,0,j2Pmax1);
double T2 = Torque_net(X,j4lo,j4alphao,j4k,j4ro,j4R,1,j4Pmax);
//%% Mass Inertia Matrix
double m11_const = j2link1_I + j2m1*pow(j2link1_lc,2) + j4link2_I + j4m2*(pow(j2link1_l,2) +
//%% Mass Inertia Matrix
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(1)) +
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 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)) +
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 m22 = j4link2_I + j4m2*pow(j4link2_lc,2) + mb*pow(j4link2_l,2);
double det = m11*m22 - m12*m12;
......@@ -126,17 +125,17 @@ stateVec_t computejointderiv(double& dt, const stateVec_t& Xe,const stateVec_t&
double c2_var1 = sin(X(1));
double c2_var2 = pow(X(2),2);
double c2 = c2_const*(c2_var1*c2_var2);
//%% Gravity Matrix
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 = 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 - 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);
......@@ -144,8 +143,8 @@ stateVec_t computejointderiv(double& dt, const stateVec_t& Xe,const stateVec_t&
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 result;
......@@ -170,7 +169,7 @@ PneumaticarmNonlinearModel::PneumaticarmNonlinearModel(double& mydt)
fxu[0].setZero();
fxu[1].setZero();
fuBase = B;
fu = dt* fuBase;
fuu[0].setZero();
......@@ -182,19 +181,19 @@ PneumaticarmNonlinearModel::PneumaticarmNonlinearModel(double& mydt)
QuxCont.setZero();*/
}
stateVec_t PneumaticarmNonlinearModel::computeNextState(double& dt, const stateVec_t& Xe,const stateVec_t& Xdes,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;
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 = p1*X(0) + p2;
j2R1 = p1*X(0) + p2;
//double P1_ = X(4);
//double P2_ = X(5);
double wnb1 = 9;
......@@ -208,24 +207,24 @@ stateVec_t PneumaticarmNonlinearModel::computeNextState(double& dt, const stateV
jointstate_deriv(5) = X(7);
jointstate_deriv(6) = -pow(wnb1,2)*X(4) - 2*wnb1*X(6) + pow(wnb1,2)*Pdes1;
jointstate_deriv(7) = -pow(wnb2,2)*X(5) - 2*wnb2*X(7) + pow(wnb2,2)*Pdes2;
//%% Force calculation
double T1 = Torque_net(X,j2lo1,j2alphao1,j2k1,j2ro1,j2R1,0,j2Pmax1);
double T2 = Torque_net(X,j4lo,j4alphao,j4k,j4ro,j4R,1,j4Pmax);
//%% Mass Inertia Matrix
double m11_const = j2link1_I + j2m1*pow(j2link1_lc,2) + j4link2_I + j4m2*(pow(j2link1_l,2) +
//%% Mass Inertia Matrix
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(1)) +
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 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)) +
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 m22 = j4link2_I + j4m2*pow(j4link2_lc,2) + mb*pow(j4link2_l,2);
double det = m11*m22 - m12*m12;
......@@ -242,17 +241,17 @@ stateVec_t PneumaticarmNonlinearModel::computeNextState(double& dt, const stateV
double c2_var1 = sin(X(1));
double c2_var2 = pow(X(2),2);
double c2 = c2_const*(c2_var1*c2_var2);
//%% Gravity Matrix
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 = 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 - 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);
......@@ -260,7 +259,7 @@ stateVec_t PneumaticarmNonlinearModel::computeNextState(double& dt, const stateV
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 ;
x1[0] = X(0); x1[1] = X(1);
//fx = jointstate_deriv;
return result;
......@@ -269,13 +268,13 @@ stateVec_t PneumaticarmNonlinearModel::computeNextState(double& dt, const stateV
void PneumaticarmNonlinearModel::computeAllModelDeriv(double& dt, const stateVec_t& Xe,const stateVec_t& Xdes,const commandVec_t& U)
{
//fx = fxBase;
Xreal = Xe + Xdes;
stateVec_t X = Xe + Xdes;
double dh = 1e-5;
stateVec_t tempX, derivplus, derivminus;
commandVec_t tempU;
tempX = X;
for(unsigned int i = 0; i<8; i++)
{
for(unsigned int j=0; j<8;j++)
......@@ -296,10 +295,10 @@ void PneumaticarmNonlinearModel::computeAllModelDeriv(double& dt, const stateVec
{
tempU = U;
tempU(j) = U(j) + dh;
derivplus = computejointderiv(dt, Xe,Xdes, tempU);
derivplus = computejointderiv(dt, X,Xdes, tempU);
tempU = U;
tempU(j) = U(j)-dh;
derivminus = computejointderiv(dt, Xe,Xdes, tempU);
tempU(j) = U(j) - dh;
derivminus = computejointderiv(dt, X,Xdes, tempU);
fu(i,j) = (derivplus(i) - derivminus(i))/(2*dh);
}
}
......@@ -327,7 +326,7 @@ commandR_stateC_t PneumaticarmNonlinearModel::computeTensorContux(const stateVec
{
return fx;
}
stateR_commandC_t& PneumaticarmNonlinearModel::getfu()
{
return fu;
......
// Authors: Ganesh Kumar
/* ******** It is a trial program for modelling a Pneumatic muscle and
******** controlling it in closeloop under xenomai realtime kernel ********/
#include <stdio.h>
#include <signal.h>
#include <unistd.h>
#include <sys/mman.h>
#include <iostream>
#include <cmath>
//#include <native/task.h>
//#include <native/timer.h>
#include <time.h>
#include <string.h>
#include <sstream>
//#include <debug.hh>
#include "pneumaticarm_model.h"
double pi1 =3.14;
PneumaticarmModel::PneumaticarmModel()
{
state_vector_.resize(4);
state_derivative_.resize(4);
control_vector_.resize(2);
}
/* Setting Number of Joints or degree of freedom*/
void PneumaticarmModel::setProblemDimension (int n)
{
nDOF_ = n;
}
/* Initialization or setting the parameters */
void PneumaticarmModel::setParameters (void)
{
length_ = 1.0; // m
mass_ = 1.0; // kg
friction_ = 0.1; // kg/s
pressure_musclebase_ = 2.5; //bar
n_ = 4; // Number of states in the state spase model
/* M=1; //Mass (kg)
K = 30000; //stiffness
L = 1; //Length of the rod
g = 9.8;
I = 1; //Inertia of the primary motor
J = 1; */ // Inertia of the other m*/
}
/*PAM system dynamics and Compute state derivatives*/
void PneumaticarmModel::computeStateDerivative(double time)
{
//VectorXd state_derivative(statevector.size());
//Parameters Muscles
//double Tmax, fk,fs, a, b, K0, K1, K2, P_m1, P_m2;
lo = 0.185;
alphao = 20.0*PI/180;
//double epsilono = 0.15;
k = 1.25;
ro = 0.0085;
// Parameters Joint
R = 0.015;
m = 2.6;
link_l = 0.32;
g = 9.81;
time_constant1 = 0.18;
time_constant2 = 0.13;
//double velocity_constant = 0.15;
I = m*link_l*link_l/3; //0.0036;
fv = 0.25;
double F1, F2, P1, P2, Tc1,Tc2, u1,u2;
P1 = state_vector_[2];
P2 = state_vector_[3];
u1 = control_vector_[0];
u2 = control_vector_[1];
Tc1 = time_constant1;