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

several modifications + testModel updated

parent 49ff5709
SET(${PROJECT_NAME}_HEADERS SET(${PROJECT_NAME}_HEADERS
costfunctionromeoactuator.h costfunctionromeoactuator.h
romeolinearactuator.h romeolinearactuator.h
config.h config.h
dynamicmodel.h dynamicmodel.h
romeosimpleactuator.h romeosimpleactuator.h
romeoactuatorpos.h romeoactuatorpos.h
costfunction.h costfunction.h
ilqrsolver.h ilqrsolver.h
costfunctionpneumaticarmelbow.h
pneumaticarm_2linkmodel.hh
) )
INSTALL(FILES ${${PROJECT_NAME}_HEADER} DESTINATION include) INSTALL(FILES ${${PROJECT_NAME}_HEADER} DESTINATION include)
......
...@@ -7,8 +7,8 @@ ...@@ -7,8 +7,8 @@
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::MatrixXd) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::MatrixXd)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)
#define stateSize 4 #define stateSize 8
#define commandSize 1 #define commandSize 2
// typedef for stateSize types // typedef for stateSize types
typedef Eigen::Matrix<double,stateSize,1> stateVec_t; // stateSize x 1 typedef Eigen::Matrix<double,stateSize,1> stateVec_t; // stateSize x 1
......
...@@ -14,14 +14,9 @@ class CostFunctionPneumaticarmElbow : public CostFunction ...@@ -14,14 +14,9 @@ class CostFunctionPneumaticarmElbow : public CostFunction
public: public:
CostFunctionPneumaticarmElbow(); CostFunctionPneumaticarmElbow();
private: private:
stateMat_t Q, Qf; stateMat_t Q;
stateMat_t Qf;
commandMat_t R; 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; double dt;
protected: protected:
// attributes // // attributes //
...@@ -31,18 +26,13 @@ private: ...@@ -31,18 +26,13 @@ private:
protected: protected:
// methods // // methods //
public: public:
void computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U); void computeAllCostDeriv(const stateVec_t& X, const commandVec_t& U);
void computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes); void computeFinalCostDeriv(const stateVec_t& X);
private: private:
protected: protected:
// accessors // // accessors //
public: public:
stateVec_t getlx();
stateMat_t getlxx();
commandVec_t getlu();
commandMat_t getluu();
commandR_stateC_t getlux();
stateR_commandC_t getlxu();
}; };
#endif #endif
...@@ -5,14 +5,14 @@ ...@@ -5,14 +5,14 @@
/* This is a program to model the pneumatic muscle based joint of the Pneumatic 7R arm */ /* This is a program to model the pneumatic muscle based joint of the Pneumatic 7R arm */
#ifndef PNEUMATICARMNONLINEARMODEL_H #ifndef PNEUMATICARMNONLINEARMODEL_H
#define PNEUMATICARMNONLINEARMODEL_H #define PNEUMATICARMNONLINEARMODEL_H
#include <iostream>
#include "config.h" #include "config.h"
#include <vector>
#include "dynamicmodel.h" #include "dynamicmodel.h"
#include <Eigen/Dense> #include <Eigen/Dense>
using namespace Eigen; using namespace Eigen;
using namespace std;
class PneumaticarmNonlinearModel : public DynamicModel class PneumaticarmNonlinearModel : public DynamicModel
{ {
public: public:
...@@ -24,34 +24,24 @@ protected: ...@@ -24,34 +24,24 @@ protected:
public: public:
private: private:
double dt; double dt;
unsigned int stateNb; // Muscle parameters
unsigned int commandNb;
// Muscle parameters
double lo, alphao, k,ro,R,a,b,emax,lb,lt,epsb,epst; double lo, alphao, k,ro,R,a,b,emax,lb,lt,epsb,epst;
double time_constant1, time_constant2; 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 A;
stateMat_t Ad; stateMat_t Ad;
stateR_commandC_t B; stateR_commandC_t B;
stateR_commandC_t Bd; stateR_commandC_t Bd;
double A13atan,A10; double A13atan,A10;
double A33atan; double A33atan;*/
stateVec_t Xreal;
stateMat_t fx,fxBase; stateMat_t fx,fxBase;
stateTens_t fxx;
stateR_commandC_t fu,fuBase; stateR_commandC_t fu,fuBase;
stateR_commandC_commandD_t fuu;
stateR_stateC_commandD_t fxu;
stateR_commandC_stateD_t fux;
stateMat_t QxxCont; stateMat_t QxxCont;
commandMat_t QuuCont; commandMat_t QuuCont;
commandR_stateC_t QuxCont; commandR_stateC_t QuxCont;
...@@ -59,23 +49,16 @@ private: ...@@ -59,23 +49,16 @@ private:
protected: protected:
// methods // // methods //
public: public:
stateVec_t computeNextState(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 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); stateMat_t computeTensorContxx(const stateVec_t& nextVx);
commandMat_t computeTensorContuu(const stateVec_t& nextVx); commandMat_t computeTensorContuu(const stateVec_t& nextVx);
commandR_stateC_t computeTensorContux(const stateVec_t& nextVx); commandR_stateC_t computeTensorContux(const stateVec_t& nextVx);
double getX(unsigned int i);
private: private:
protected: protected:
// accessors // // accessors //
public: 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 @@ ...@@ -3,8 +3,8 @@
CostFunctionPneumaticarmElbow::CostFunctionPneumaticarmElbow() CostFunctionPneumaticarmElbow::CostFunctionPneumaticarmElbow()
{ {
Q <<1e4*1.0, 0.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,1e4*1.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, 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, 1e-5*0.0, 0,0,0,0,
0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0,
...@@ -15,9 +15,17 @@ CostFunctionPneumaticarmElbow::CostFunctionPneumaticarmElbow() ...@@ -15,9 +15,17 @@ CostFunctionPneumaticarmElbow::CostFunctionPneumaticarmElbow()
0.0,1e-3*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, 5e-5*1.0, 0.0,
0.0, 0.0, 0.0, 5e-5*1.0;*/ 0.0, 0.0, 0.0, 5e-5*1.0;*/
Qf = Q; /*Qf <<1e0*5, 0.0, 0.0, 0.0, 0,0,0,0,
R << 1e-2,0, 0.0,1e0*8, 0.0, 0.0, 0,0,0,0,
0, 1e-4; 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; lxx = Q;
luu = R; luu = R;
//lux << 0.0,0.0; //lux << 0.0,0.0;
...@@ -25,46 +33,15 @@ CostFunctionPneumaticarmElbow::CostFunctionPneumaticarmElbow() ...@@ -25,46 +33,15 @@ CostFunctionPneumaticarmElbow::CostFunctionPneumaticarmElbow()
lx.setZero(); 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 = Q*X;
//lx(0,0) = 1e-3*8*(X(0,0)-Xdes(0,0));
lu = R*U; lu = R*U;
} }
void CostFunctionPneumaticarmElbow::computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes) void CostFunctionPneumaticarmElbow::computeFinalCostDeriv(const stateVec_t& X)
{
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()
{ {
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 @@ ...@@ -6,7 +6,7 @@
#include "pneumaticarm_2linkmodel.hh" #include "pneumaticarm_2linkmodel.hh"
#include <math.h> #include <math.h>
using namespace std;
//#define pi M_PI //#define pi M_PI
double PI = 3.14; double PI = 3.14;
// Joint 1 parameters // Joint 1 parameters
...@@ -24,7 +24,7 @@ double j2fv1 = 3.0; ...@@ -24,7 +24,7 @@ double j2fv1 = 3.0;
double j2Pmax1 = 3.0; double j2Pmax1 = 3.0;
// Joint 3 parameters // Joint 3 parameters
double j4lo = 0.185; double j4lo = 0.185;
double j4alphao = 20*PI/180; double j4alphao = 23*PI/180;
double j4k = 1.25; double j4k = 1.25;
double j4ro = 0.0085; double j4ro = 0.0085;
double j4R = 0.015; double j4R = 0.015;
...@@ -67,15 +67,19 @@ double F_triceps = P2*ftterm; ...@@ -67,15 +67,19 @@ double F_triceps = P2*ftterm;
double Torque_pneumatics = (F_biceps -F_triceps )*R; double Torque_pneumatics = (F_biceps -F_triceps )*R;
return (Torque_pneumatics); 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(1,0)-=A10*sin(X(0));
//result(3,0)+=A33atan*atan(a*X(3,0)); //result(3,0)+=A33atan*atan(a*X(3,0));
stateVec_t X;
X = Xe + Xdes;
stateVec_t jointstate_deriv; stateVec_t jointstate_deriv;
double m = -0.0023; double m = -0.0023;
double c = 0.0136; 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); //P1_ = X(4);
//P2_ = X(5); //P2_ = X(5);
double wnb1 = 9; double wnb1 = 9;
...@@ -98,15 +102,15 @@ stateVec_t computejointderiv(double& dt, const stateVec_t& X,const commandVec_t& ...@@ -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) + 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)); 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)); 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_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)); 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 m22 = j4link2_I + j4m2*pow(j4link2_lc,2) + mb*pow(j4link2_l,2);
double det = m11*m22 - m12*m12; double det = m11*m22 - m12*m12;
double inv_m11 = m22/det; double inv_m11 = m22/det;
...@@ -127,24 +131,24 @@ stateVec_t computejointderiv(double& dt, const stateVec_t& X,const commandVec_t& ...@@ -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)) + ( 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)); 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 //%% viscous friction matrix
double tf1 = -j2fv1*X(2); double tf1 = -j2fv1*X(2);
double tf2 = -j4fv2*X(3); double tf2 = -j4fv2*X(3);
double Mat1 = inv_m11*(T1 + tf1 - c1 -g1) + inv_m12*(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 -g1) + inv_m22*(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(0) = X(2); //%joint_state(2);
jointstate_deriv(1) = X(3); //%joint_state(2); jointstate_deriv(1) = X(3); //%joint_state(2);
jointstate_deriv(2) = Mat1; jointstate_deriv(2) = Mat1;
jointstate_deriv(3) = Mat2; jointstate_deriv(3) = Mat2;
//stateVec_t result = X + dt*jointstate_deriv; stateVec_t result = X + dt*jointstate_deriv;
//fx = jointstate_deriv; //fx = jointstate_deriv;
return jointstate_deriv; return result;
} }
...@@ -153,6 +157,8 @@ PneumaticarmNonlinearModel::PneumaticarmNonlinearModel(double& mydt) ...@@ -153,6 +157,8 @@ PneumaticarmNonlinearModel::PneumaticarmNonlinearModel(double& mydt)
{ {
stateNb=8; stateNb=8;
commandNb=2; commandNb=2;
x1.resize(2);
dt = mydt;
//state_vector_.resize(8); //state_vector_.resize(8);
//state_derivative_.resize(8); //state_derivative_.resize(8);
//control_vector_.resize(2); //control_vector_.resize(2);
...@@ -176,15 +182,19 @@ PneumaticarmNonlinearModel::PneumaticarmNonlinearModel(double& mydt) ...@@ -176,15 +182,19 @@ PneumaticarmNonlinearModel::PneumaticarmNonlinearModel(double& mydt)
QuxCont.setZero();*/ 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(1,0)-=A10*sin(X(0));
//result(3,0)+=A33atan*atan(a*X(3,0)); //result(3,0)+=A33atan*atan(a*X(3,0));
stateVec_t jointstate_deriv; stateVec_t jointstate_deriv;
stateVec_t X = Xe + Xdes;
double m = -0.0023; double m = -0.0023;
double c = 0.0136; 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 P1_ = X(4);
//double P2_ = X(5); //double P2_ = X(5);
double wnb1 = 9; double wnb1 = 9;
...@@ -207,15 +217,16 @@ stateVec_t PneumaticarmNonlinearModel::computeNextState(double& dt, const stateV ...@@ -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) + 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)); 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)); 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_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)); 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 m22 = j4link2_I + j4m2*pow(j4link2_lc,2) + mb*pow(j4link2_l,2);
double det = m11*m22 - m12*m12; double det = m11*m22 - m12*m12;
double inv_m11 = m22/det; double inv_m11 = m22/det;
...@@ -236,66 +247,67 @@ stateVec_t PneumaticarmNonlinearModel::computeNextState(double& dt, const stateV ...@@ -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)) + ( 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)); 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 //%% viscous friction matrix
double tf1 = -j2fv1*X(2); double tf1 = -j2fv1*X(2);
double tf2 = -j4fv2*X(3); double tf2 = -j4fv2*X(3);
double Mat1 = inv_m11*(T1 + tf1 - c1 -g1) + inv_m12*(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 -g1) + inv_m22*(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(0) = X(2); //%joint_state(2);
jointstate_deriv(1) = X(3); //%joint_state(2); jointstate_deriv(1) = X(3); //%joint_state(2);
jointstate_deriv(2) = Mat1; jointstate_deriv(2) = Mat1;
jointstate_deriv(3) = Mat2; 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; //fx = jointstate_deriv;
return result; 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; //fx = fxBase;
Xreal = Xe + Xdes;
double dh = 1e-4;; double dh = 5e-3;
stateVec_t tempX, derivplus, derivminus; stateVec_t tempX, derivplus, derivminus;
commandVec_t tempU; commandVec_t tempU;
tempX = X; tempX = Xe;
for(unsigned int i = 0; i<8; i++) for(unsigned int i = 0; i<8; i++)
{ {
for(unsigned int j=0; j<8;j++) for(unsigned int j=0; j<8;j++)
{ {
tempX = X; tempX = Xe;
tempX(j) = X(j) + dh; tempX(j) = Xe(j) + dh;
derivplus = this->computeNextState(dt, tempX, U); derivplus = computejointderiv(dt, tempX,Xdes, U); //cout<< "deriv:" <<derivplus(0)<< '\n' << endl;
tempX = X; tempX = Xe;
tempX(j) = X(j)-dh; tempX(j) = Xe(j) - dh;
derivminus = this->computeNextState(dt, tempX, U); derivminus = computejointderiv(dt, tempX,Xdes, U);
fx(i,j) = (derivplus(i) - derivminus(i))/(2*dh); fx(i,j) = (derivplus(i) - derivminus(i))/(2*dh);
} }
}