diff --git a/cmake b/cmake
index 8e7bedfcbd8524c0401a58fd74edc07c3d4308d0..8474524906099169b05f5c2d0523519d8e383df1 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit 8e7bedfcbd8524c0401a58fd74edc07c3d4308d0
+Subproject commit 8474524906099169b05f5c2d0523519d8e383df1
diff --git a/include/jrl/walkgen/pinocchiorobot.hh b/include/jrl/walkgen/pinocchiorobot.hh
index e182c75bd48d59079b26d692448294bc9f79bd99..875c4429a4e9aec951495e5db11838e3e34af8c9 100644
--- a/include/jrl/walkgen/pinocchiorobot.hh
+++ b/include/jrl/walkgen/pinocchiorobot.hh
@@ -33,6 +33,7 @@ frame work */
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include <jrl/mal/matrixabstractlayer.hh>
+
namespace PatternGeneratorJRL
{
struct PinocchioRobotFoot_t{
@@ -40,7 +41,7 @@ namespace PatternGeneratorJRL
double soleDepth ; // z axis
double soleWidth ; // y axis
double soleHeight ;// x axis
- vector3d anklePosition ;
+ Eigen::Vector3d anklePosition ;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
typedef PinocchioRobotFoot_t PRFoot ;
@@ -57,12 +58,12 @@ namespace PatternGeneratorJRL
/// Functions computing kinematics and dynamics
void computeInverseDynamics();
- void computeInverseDynamics(MAL_VECTOR_TYPE(double) & q,
- MAL_VECTOR_TYPE(double) & v,
- MAL_VECTOR_TYPE(double) & a);
+ void computeInverseDynamics(Eigen::VectorXd & q,
+ Eigen::VectorXd & v,
+ Eigen::VectorXd & a);
void computeForwardKinematics();
- void computeForwardKinematics(MAL_VECTOR_TYPE(double) & q);
+ void computeForwardKinematics(Eigen::VectorXd & q);
void RPYToSpatialFreeFlyer(Eigen::Vector3d & rpy,
Eigen::Vector3d & drpy,
@@ -89,9 +90,9 @@ namespace PatternGeneratorJRL
virtual bool ComputeSpecializedInverseKinematics(
const se3::JointIndex &jointRoot,
const se3::JointIndex &jointEnd,
- const MAL_S4x4_MATRIX_TYPE(double) & jointRootPosition,
- const MAL_S4x4_MATRIX_TYPE(double) & jointEndPosition,
- MAL_VECTOR_TYPE(double) &q);
+ const Eigen::Matrix4d & jointRootPosition,
+ const Eigen::Matrix4d & jointEndPosition,
+ Eigen::VectorXd &q);
///
/// \brief testInverseKinematics :
@@ -118,14 +119,14 @@ namespace PatternGeneratorJRL
private :
// needed for the inverse geometry (ComputeSpecializedInverseKinematics)
- void getWaistFootKinematics(const matrix4d & jointRootPosition,
- const matrix4d & jointEndPosition,
- vectorN &q,
- vector3d Dt);
+ void getWaistFootKinematics(const Eigen::Matrix4d & jointRootPosition,
+ const Eigen::Matrix4d & jointEndPosition,
+ Eigen::VectorXd &q,
+ Eigen::Vector3d Dt);
double ComputeXmax(double & Z);
- void getShoulderWristKinematics(const matrix4d & jointRootPosition,
- const matrix4d & jointEndPosition,
- vectorN &q,
+ void getShoulderWristKinematics(const Eigen::Matrix4d & jointRootPosition,
+ const Eigen::Matrix4d & jointEndPosition,
+ Eigen::VectorXd &q,
int side);
void DetectAutomaticallyShoulders();
void DetectAutomaticallyOneShoulder(se3::JointIndex aWrist,
@@ -163,17 +164,17 @@ namespace PatternGeneratorJRL
inline se3::JointModelVector & getActuatedJoints()
{return m_robotModel->joints;}
- inline MAL_VECTOR_TYPE(double) currentConfiguration()
+ inline Eigen::VectorXd currentConfiguration()
{return m_qmal;}
- inline MAL_VECTOR_TYPE(double) currentVelocity()
+ inline Eigen::VectorXd currentVelocity()
{return m_vmal;}
- inline MAL_VECTOR_TYPE(double) currentAcceleration()
+ inline Eigen::VectorXd currentAcceleration()
{return m_amal;}
inline unsigned numberDof()
{return m_robotModel->nv;}
- inline void zeroMomentumPoint(MAL_S3_VECTOR_TYPE(double) & zmp)
+ inline void zeroMomentumPoint(Eigen::Vector3d & zmp)
{
m_externalForces = m_robotData->liMi[1].act(m_robotData->f[1]);
m_f = m_externalForces.linear() ;
@@ -183,16 +184,16 @@ namespace PatternGeneratorJRL
zmp(2) = 0.0 ; // by default
}
- inline void positionCenterOfMass(MAL_S3_VECTOR_TYPE(double) & com)
+ inline void positionCenterOfMass(Eigen::Vector3d & com)
{
m_com = m_robotData->com[0] ;
com(0) = m_com(0) ;
com(1) = m_com(1) ;
com(2) = m_com(2) ;
}
- inline void CenterOfMass(MAL_S3_VECTOR_TYPE(double) & com,
- MAL_S3_VECTOR_TYPE(double) & dcom,
- MAL_S3_VECTOR_TYPE(double) & ddcom)
+ inline void CenterOfMass(Eigen::Vector3d & com,
+ Eigen::Vector3d & dcom,
+ Eigen::Vector3d & ddcom)
{
m_com = m_robotData->acom[0] ;
ddcom(0) = m_com(0) ;
@@ -212,11 +213,11 @@ namespace PatternGeneratorJRL
/// SETTERS
/// ///////
- inline void currentConfiguration(MAL_VECTOR_TYPE(double) conf)
+ inline void currentConfiguration(Eigen::VectorXd conf)
{m_qmal=conf;}
- inline void currentVelocity(MAL_VECTOR_TYPE(double) vel)
+ inline void currentVelocity(Eigen::VectorXd vel)
{m_vmal=vel;}
- inline void currentAcceleration(MAL_VECTOR_TYPE(double) acc)
+ inline void currentAcceleration(Eigen::VectorXd acc)
{m_amal=acc;}
/// Initialization functions
@@ -242,9 +243,9 @@ namespace PatternGeneratorJRL
se3::JointIndex m_chest, m_waist, m_leftShoulder, m_rightShoulder ;
se3::JointIndex m_leftWrist , m_rightWrist ;
- MAL_VECTOR_TYPE(double) m_qmal ;
- MAL_VECTOR_TYPE(double) m_vmal ;
- MAL_VECTOR_TYPE(double) m_amal ;
+ Eigen::VectorXd m_qmal ;
+ Eigen::VectorXd m_vmal ;
+ Eigen::VectorXd m_amal ;
Eigen::VectorXd m_q ;
Eigen::VectorXd m_v ;
Eigen::VectorXd m_a ;
@@ -265,7 +266,7 @@ namespace PatternGeneratorJRL
bool m_isArmInverseKinematic ;
// length between the waist and the hip
- MAL_S3_VECTOR_TYPE(double) m_leftDt, m_rightDt ;
+ Eigen::Vector3d m_leftDt, m_rightDt ;
double m_femurLength ;
double m_tibiaLengthZ ;
double m_tibiaLengthY ;
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index f5eea821bcb01e555a5527665d1f136e0e7cbce5..4b3d2f510bd62a36bfc7fff17a259952b221d3de 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -207,7 +207,6 @@ ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCES} ${${PROJECT_NAME}_ABSOLUTE_HEADERS
# Define dependencies
SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-msse -msse2 -msse3 -march=core2 -mfpmath=sse -fivopts -ftree-loop-im -fipa-pta ")
-PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} jrl-mal)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} pinocchio)
IF(USE_LSSOL)
diff --git a/src/PatternGeneratorInterfacePrivate.cpp b/src/PatternGeneratorInterfacePrivate.cpp
index 265a494013ce0ab177eff385cfa5a3aa87d74bfb..e737c4a5cee471dcc916c7f11e4e608de1d8a68a 100644
--- a/src/PatternGeneratorInterfacePrivate.cpp
+++ b/src/PatternGeneratorInterfacePrivate.cpp
@@ -559,7 +559,7 @@ namespace PatternGeneratorJRL {
memset(&InitLeftFootAbsPos,0,sizeof(InitLeftFootAbsPos));
memset(&InitRightFootAbsPos,0,sizeof(InitRightFootAbsPos));
- MAL_VECTOR(lStartingWaistPose,double);
+ Eigen::VectorXd lStartingWaistPose;
EvaluateStartingState(lStartingCOMState,
lStartingZMPPosition,
@@ -602,7 +602,7 @@ namespace PatternGeneratorJRL {
memset(&InitLeftFootAbsPos,0,sizeof(InitLeftFootAbsPos));
memset(&InitRightFootAbsPos,0,sizeof(InitRightFootAbsPos));
- MAL_VECTOR(lStartingWaistPose,double);
+ Eigen::VectorXd lStartingWaistPose;
EvaluateStartingState(lStartingCOMState,
lStartingZMPPosition,
@@ -659,7 +659,7 @@ namespace PatternGeneratorJRL {
void PatternGeneratorInterfacePrivate::EvaluateStartingCOM(MAL_VECTOR( & Configuration,double),
MAL_S3_VECTOR( & lStartingCOMState,double))
{
- MAL_VECTOR(Velocity,double);
+ Eigen::VectorXd Velocity;
MAL_VECTOR_RESIZE(Velocity,MAL_VECTOR_SIZE(Configuration));
for(unsigned int i=0;i<MAL_VECTOR_SIZE(Configuration);i++)
Velocity[i] = 0.0;
@@ -673,11 +673,11 @@ namespace PatternGeneratorJRL {
void PatternGeneratorInterfacePrivate::EvaluateStartingState(COMState & lStartingCOMState,
MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition,
- MAL_VECTOR_TYPE(double) & lStartingWaistPose,
+ Eigen::VectorXd & lStartingWaistPose,
FootAbsolutePosition & InitLeftFootAbsPos,
FootAbsolutePosition & InitRightFootAbsPos)
{
- MAL_VECTOR(lBodyInit,double);
+ Eigen::VectorXd lBodyInit;
MAL_VECTOR_RESIZE(lBodyInit,m_CurrentActuatedJointValues.size());
for(unsigned int j=0; j<m_CurrentActuatedJointValues.size();j++)
@@ -810,7 +810,7 @@ namespace PatternGeneratorJRL {
// Initialize consequently the ComAndFoot Realization object.
- MAL_VECTOR(lStartingWaistPose,double);
+ Eigen::VectorXd lStartingWaistPose;
m_GlobalStrategyManager->EvaluateStartingState(BodyAnglesIni,
lStartingCOMState,
lStartingZMPPosition,
@@ -983,7 +983,7 @@ namespace PatternGeneratorJRL {
- MAL_VECTOR_TYPE(double) lCurrentConfiguration;
+ Eigen::VectorXd lCurrentConfiguration;
lCurrentConfiguration = m_PinocchioRobot->currentConfiguration();
@@ -1306,9 +1306,9 @@ namespace PatternGeneratorJRL {
- bool PatternGeneratorInterfacePrivate::RunOneStepOfTheControlLoop(MAL_VECTOR_TYPE(double) & CurrentConfiguration,
- MAL_VECTOR_TYPE(double) & CurrentVelocity,
- MAL_VECTOR_TYPE(double) & CurrentAcceleration,
+ bool PatternGeneratorInterfacePrivate::RunOneStepOfTheControlLoop(Eigen::VectorXd & CurrentConfiguration,
+ Eigen::VectorXd & CurrentVelocity,
+ Eigen::VectorXd & CurrentAcceleration,
MAL_VECTOR( &ZMPTarget,double))
{
COMState finalCOMState;
@@ -1329,9 +1329,9 @@ namespace PatternGeneratorJRL {
ZMPPosition &ZMPRefPos,
COMPosition &COMRefPos)
{
- MAL_VECTOR_TYPE(double) CurrentConfiguration;
- MAL_VECTOR_TYPE(double) CurrentVelocity;
- MAL_VECTOR_TYPE(double) CurrentAcceleration;
+ Eigen::VectorXd CurrentConfiguration;
+ Eigen::VectorXd CurrentVelocity;
+ Eigen::VectorXd CurrentAcceleration;
MAL_VECTOR( ZMPTarget,double);
COMState aCOMRefState;
@@ -1351,9 +1351,9 @@ namespace PatternGeneratorJRL {
}
- bool PatternGeneratorInterfacePrivate::RunOneStepOfTheControlLoop(MAL_VECTOR_TYPE(double) & CurrentConfiguration,
- MAL_VECTOR_TYPE(double) & CurrentVelocity,
- MAL_VECTOR_TYPE(double) & CurrentAcceleration,
+ bool PatternGeneratorInterfacePrivate::RunOneStepOfTheControlLoop(Eigen::VectorXd & CurrentConfiguration,
+ Eigen::VectorXd & CurrentVelocity,
+ Eigen::VectorXd & CurrentAcceleration,
MAL_VECTOR( &ZMPTarget,double),
COMPosition &finalCOMPosition,
FootAbsolutePosition &LeftFootPosition,
@@ -1371,9 +1371,9 @@ namespace PatternGeneratorJRL {
return m_Running;
}
- bool PatternGeneratorInterfacePrivate::RunOneStepOfTheControlLoop(MAL_VECTOR_TYPE(double) & CurrentConfiguration,
- MAL_VECTOR_TYPE(double) & CurrentVelocity,
- MAL_VECTOR_TYPE(double) & CurrentAcceleration,
+ bool PatternGeneratorInterfacePrivate::RunOneStepOfTheControlLoop(Eigen::VectorXd & CurrentConfiguration,
+ Eigen::VectorXd & CurrentVelocity,
+ Eigen::VectorXd & CurrentAcceleration,
MAL_VECTOR( &ZMPTarget,double),
COMState &finalCOMState,
FootAbsolutePosition &LeftFootPosition,
diff --git a/src/PreviewControl/LinearizedInvertedPendulum2D.cpp b/src/PreviewControl/LinearizedInvertedPendulum2D.cpp
index 8d5d1888a9660a04fc12c41c261029542fe8ab92..6f2bf45227a039b1973d4d1af58e2aeec25e0122 100644
--- a/src/PreviewControl/LinearizedInvertedPendulum2D.cpp
+++ b/src/PreviewControl/LinearizedInvertedPendulum2D.cpp
@@ -43,14 +43,14 @@ LinearizedInvertedPendulum2D::LinearizedInvertedPendulum2D()
m_ComHeight = -1.0;
m_SamplingPeriod = -1.0;
m_InterpolationInterval = -1;
- MAL_MATRIX_RESIZE(m_A,3,3);
- MAL_MATRIX_RESIZE(m_B,3,1);
- MAL_MATRIX_RESIZE(m_C,1,3);
+ m_A.resize(3,3);
+ m_B.resize(3,1);
+ m_C.resize(1,3);
- MAL_VECTOR_RESIZE(m_xk,6);
- MAL_VECTOR_RESIZE(m_CoM.x,3);
- MAL_VECTOR_RESIZE(m_CoM.y,3);
- MAL_VECTOR_RESIZE(m_zk,2);
+ m_xk.resize(6);
+ m_CoM.x.resize(3);
+ m_CoM.y.resize(3);
+ m_zk.resize(2);
RESETDEBUG4("Debug2DLIPM.dat");
}
@@ -105,7 +105,7 @@ void LinearizedInvertedPendulum2D::SetRobotControlPeriod(const double & aT)
}
-void LinearizedInvertedPendulum2D::GetState(MAL_VECTOR_TYPE(double) &lxk)
+void LinearizedInvertedPendulum2D::GetState(Eigen::VectorXd &lxk)
{
//For compability reasons
m_xk[0] = m_CoM.x[0];m_xk[1] = m_CoM.x[1];m_xk[2] = m_CoM.x[2];
@@ -258,8 +258,8 @@ int LinearizedInvertedPendulum2D::Interpolation(deque<COMState> &COMStates,
com_t LinearizedInvertedPendulum2D::OneIteration(double ux, double uy)
{
- MAL_VECTOR_DIM(Bux,double,3);
- MAL_VECTOR_DIM(Buy,double,3);
+ Eigen::VectorXd Bux(3);
+ Eigen::VectorXd Buy(3);
Bux[0] = ux*m_B(0,0);
Bux[1] = ux*m_B(1,0);
@@ -270,15 +270,12 @@ com_t LinearizedInvertedPendulum2D::OneIteration(double ux, double uy)
Buy[2] = uy*m_B(2,0);
// Simulate the dynamical system
- m_CoM.x = MAL_RET_A_by_B(m_A,m_CoM.x);
+ m_CoM.x = m_A*m_CoM.x;
m_CoM.x = m_CoM.x + Bux;
- m_CoM.y = MAL_RET_A_by_B(m_A,m_CoM.y);
+ m_CoM.y = m_A*m_CoM.y;
m_CoM.y = m_CoM.y + Buy;
// Modif. from Dimitar: Initially a mistake regarding the ordering.
- //MAL_C_eq_A_by_B(m_zk,m_C,m_xk);
-
-
ODEBUG4( m_xk[0] << " " << m_xk[1] << " " << m_xk[2] << " " <<
m_xk[3] << " " << m_xk[4] << " " << m_xk[5] << " " <<
m_CoM.x << " " << m_CoM.y << " " <<
diff --git a/src/PreviewControl/LinearizedInvertedPendulum2D.hh b/src/PreviewControl/LinearizedInvertedPendulum2D.hh
index b4ae95d041dccc34b67e87f763d1dc881cd69505..a809478d006c80a88a2d1ddf0be3edbe6d8dc0b0 100644
--- a/src/PreviewControl/LinearizedInvertedPendulum2D.hh
+++ b/src/PreviewControl/LinearizedInvertedPendulum2D.hh
@@ -101,20 +101,20 @@ namespace PatternGeneratorJRL
@{
*/
/* ! Matrix regarding the state of the CoM (pos, velocity, acceleration) */
- MAL_MATRIX(m_A,double);
+ Eigen::MatrixXd m_A;
/* ! Vector for the command */
- MAL_MATRIX(m_B,double);
+ Eigen::MatrixXd m_B;
/* ! Vector for the ZMP. */
- MAL_MATRIX(m_C,double);
+ Eigen::MatrixXd m_C;
/*! \brief State of the LIPM at the \f$k\f$ eme iteration
\f$ x_k = [ c_x \dot{c}_x \ddot{c}_x c_y \dot{c}_y \ddot{c}_y\f$ */
- MAL_VECTOR(m_xk,double);
+ Eigen::VectorXd m_xk;
com_t m_CoM;
/* ! \brief Vector of ZMP */
- MAL_VECTOR(m_zk,double);
+ Eigen::VectorXd m_zk;
/* ! @} */
@@ -150,7 +150,7 @@ namespace PatternGeneratorJRL
{m_CoM = CoM;};
/*! Get state. */
- void GetState(MAL_VECTOR_TYPE(double) &lxk);
+ void GetState(Eigen::VectorXd &lxk);
COMState GetState();
inline com_t getState()
diff --git a/src/PreviewControl/OptimalControllerSolver.cpp b/src/PreviewControl/OptimalControllerSolver.cpp
index 2e22aa4b6622b2e42271fdf80f4b184b5a64340e..1e28ef6eb4cf89e638a7a8c471a6a249655e4a3c 100644
--- a/src/PreviewControl/OptimalControllerSolver.cpp
+++ b/src/PreviewControl/OptimalControllerSolver.cpp
@@ -95,9 +95,9 @@ logical sb02ow (double *_alphar, double * /* _alphai */, double *_beta)
}
-OptimalControllerSolver::OptimalControllerSolver(MAL_MATRIX( &A, double),
- MAL_MATRIX( &b, double),
- MAL_MATRIX( &c, double),
+OptimalControllerSolver::OptimalControllerSolver(Eigen::MatrixXd &A, double),
+ Eigen::MatrixXd &b, double),
+ Eigen::MatrixXd &c, double),
double Q, double R,
unsigned int Nl)
{
@@ -130,13 +130,13 @@ OptimalControllerSolver::~OptimalControllerSolver()
}
-bool OptimalControllerSolver::GeneralizedSchur(MAL_MATRIX( &A,double),
- MAL_MATRIX( &B,double),
+bool OptimalControllerSolver::GeneralizedSchur(Eigen::MatrixXd &A,double),
+ Eigen::MatrixXd &B,double),
MAL_VECTOR( &alphar,double),
MAL_VECTOR( &alphai,double),
MAL_VECTOR( &beta,double),
- MAL_MATRIX( &L,double),
- MAL_MATRIX( &R,double))
+ Eigen::MatrixXd &L,double),
+ Eigen::MatrixXd &R,double))
{
ODEBUG("A:" << A);
ODEBUG("B:" << A);
@@ -207,8 +207,8 @@ void OptimalControllerSolver::ComputeWeights(unsigned int Mode)
// [-c'Qc E]]
// ODEBUG("m_A:" << m_A);
// And each sub-matrix is n x n matrix.
- MAL_MATRIX(H,double);
- MAL_MATRIX(tm_b,double);
+ Eigen::MatrixXd H;
+ Eigen::MatrixXd tm_b;
// Store the transpose of m_b;
tm_b = MAL_RET_TRANSPOSE(m_b);
@@ -227,7 +227,7 @@ void OptimalControllerSolver::ComputeWeights(unsigned int Mode)
for(int j=0;j<n;j++)
H(i,j) = m_A(i,j);
- MAL_MATRIX(H21,double);
+ Eigen::MatrixXd H21;
H21 = MAL_RET_TRANSPOSE(m_c);
ODEBUG("H21 (1):" << H21);
H21 = H21 * m_Q;
@@ -245,10 +245,10 @@ void OptimalControllerSolver::ComputeWeights(unsigned int Mode)
MAL_MATRIX_DIM(E,double,2*n,2*n);
MAL_MATRIX_SET_IDENTITY(E);
- MAL_MATRIX(G,double);
+ Eigen::MatrixXd G;
G = MAL_RET_A_by_B(m_b * (1/m_R) , tm_b);
- MAL_MATRIX(At,double);
+ Eigen::MatrixXd At;
At= MAL_RET_TRANSPOSE(m_A);
for(int i=0;i< n;i++)
for(int j=0;j<n;j++)
@@ -277,7 +277,7 @@ void OptimalControllerSolver::ComputeWeights(unsigned int Mode)
// Computes P the solution of the Riccati equation.
- MAL_MATRIX(P,double);
+ Eigen::MatrixXd P;
MAL_MATRIX_DIM(Z11,double,n,n);
MAL_MATRIX_DIM(Z21,double,n,n);
@@ -293,14 +293,14 @@ void OptimalControllerSolver::ComputeWeights(unsigned int Mode)
ODEBUG( "Z11:" << endl << Z11 << endl
<< "Z21:" << endl << Z21 );
- MAL_MATRIX(iZ11,double);
- MAL_INVERSE(Z11,iZ11,double);
+ Eigen::MatrixXd iZ11;
+ MAL_INVERSE(Z11,iZ11;
P = MAL_RET_A_by_B(Z21, iZ11);
ODEBUG( "P: " << endl << P);
// Compute the weights.
- MAL_MATRIX(r,double);
+ Eigen::MatrixXd r;
double la;
r = tm_b; // b^T
@@ -318,11 +318,11 @@ void OptimalControllerSolver::ComputeWeights(unsigned int Mode)
ODEBUG("K: "<< endl << m_K);
// Computes the weights for the future.
- MAL_MATRIX(PreMatrix,double);
- MAL_MATRIX(Recursive,double);
- MAL_MATRIX(BaseOfRecursion,double);
- MAL_MATRIX(PostMatrix,double);
- MAL_MATRIX(Intermediate,double);
+ Eigen::MatrixXd PreMatrix;
+ Eigen::MatrixXd Recursive;
+ Eigen::MatrixXd BaseOfRecursion;
+ Eigen::MatrixXd PostMatrix;
+ Eigen::MatrixXd Intermediate;
PreMatrix = la * tm_b;
BaseOfRecursion = m_A - MAL_RET_A_by_B(m_b , m_K);
@@ -357,12 +357,12 @@ void OptimalControllerSolver::DisplayWeights()
std::cout << "F:" << m_F << std::endl;
}
-void OptimalControllerSolver::GetF(MAL_MATRIX(& lF, double))
+void OptimalControllerSolver::GetF(Eigen::MatrixXd & lF, double))
{
lF = m_F;
}
-void OptimalControllerSolver::GetK(MAL_MATRIX(& lK,double))
+void OptimalControllerSolver::GetK(Eigen::MatrixXd & lK,double))
{
lK = m_K;
}
diff --git a/src/PreviewControl/OptimalControllerSolver.hh b/src/PreviewControl/OptimalControllerSolver.hh
index 2f7b098d83a9d6a6b6e92e93fd77f1f01912b9ff..d50020632829995d0a20909d692e1c61ada04b6d 100644
--- a/src/PreviewControl/OptimalControllerSolver.hh
+++ b/src/PreviewControl/OptimalControllerSolver.hh
@@ -144,9 +144,9 @@ The augmented system is then
static const unsigned int MODE_WITH_INITIALPOS=0;
/*! A constructor */
- OptimalControllerSolver(MAL_MATRIX(&A,double),
- MAL_MATRIX(&b,double),
- MAL_MATRIX(&c,double),
+ OptimalControllerSolver(Eigen::MatrixXd &A,
+ Eigen::MatrixXd &b,
+ Eigen::MatrixXd &c,
double Q, double R,
unsigned int Nl);
@@ -162,19 +162,19 @@ The augmented system is then
/*! Display the weights */
void DisplayWeights();
- bool GeneralizedSchur(MAL_MATRIX(&A,double),
- MAL_MATRIX(&B,double),
- MAL_VECTOR(&alphar,double),
- MAL_VECTOR(&alphai,double),
- MAL_VECTOR(&beta,double),
- MAL_MATRIX(&L,double),
- MAL_MATRIX(&R,double));
+ bool GeneralizedSchur(Eigen::MatrixXd &A,
+ Eigen::MatrixXd &B,
+ Eigen::VectorXd &alphar,
+ Eigen::VectorXd &alphai,
+ Eigen::VectorXd &beta,
+ Eigen::MatrixXd &L,
+ Eigen::MatrixXd &R);
/*! To take matrix F aka the weights of the preview window . */
- void GetF(MAL_MATRIX(& LF,double) );
+ void GetF(Eigen::MatrixXd & LF );
/*! To take matrix K, aka the weight of the other part of the command */
- void GetK(MAL_MATRIX(& LK,double) );
+ void GetK(Eigen::MatrixXd & LK );
protected:
@@ -186,9 +186,9 @@ The augmented system is then
\f}
*/
- MAL_MATRIX(m_A,double);
- MAL_MATRIX(m_b,double);
- MAL_MATRIX(m_c,double);
+ Eigen::MatrixXd m_A;
+ Eigen::MatrixXd m_b;
+ Eigen::MatrixXd m_c;
/*! The coefficent of the index criteria:
\f[
@@ -199,8 +199,8 @@ The augmented system is then
/*! The weights themselves */
- MAL_MATRIX(m_K,double);
- MAL_MATRIX(m_F,double);
+ Eigen::MatrixXd m_K;
+ Eigen::MatrixXd m_F;
/*! The size of the window for the preview */
int m_Nl;
diff --git a/src/PreviewControl/PreviewControl.cpp b/src/PreviewControl/PreviewControl.cpp
index 82076ec217c8fc746799639df8cf8691f605c6b4..0033c477c3e936cf221d450275214e3dccf822ed 100644
--- a/src/PreviewControl/PreviewControl.cpp
+++ b/src/PreviewControl/PreviewControl.cpp
@@ -51,11 +51,11 @@ PreviewControl::PreviewControl(SimplePluginManager *lSPM,
m_Zc = 0.0;
m_SizeOfPreviewWindow = 0;
- MAL_MATRIX_RESIZE(m_A,3,3);
- MAL_MATRIX_RESIZE(m_B,3,1);
- MAL_MATRIX_RESIZE(m_C,1,3);
+ Eigen::MatrixXd m_A.resize(3,3);
+ Eigen::MatrixXd m_B.resize(3,1);
+ Eigen::MatrixXd m_C.resize(1,3);
- MAL_MATRIX_RESIZE(m_Kx,1,3);
+ Eigen::MatrixXd m_Kx.resize(1,3);
m_Ks = 0;
@@ -166,7 +166,7 @@ void PreviewControl::ReadPrecomputedFile(string aFileName)
m_SizeOfPreviewWindow = (unsigned int)(m_PreviewControlTime/
m_SamplingPeriod);
- MAL_MATRIX_RESIZE(m_F,m_SizeOfPreviewWindow,1);
+ Eigen::MatrixXd m_F.resize(m_SizeOfPreviewWindow,1);
for(unsigned int i=0;i<m_SizeOfPreviewWindow;i++)
{
@@ -216,7 +216,7 @@ void PreviewControl::ComputeOptimalWeights(unsigned int mode)
m_C(0,2) = -m_Zc/9.81;
ODEBUG(" m_Zc: " << m_Zc << " m_C(0,2)" << m_C(0,2));
- MAL_MATRIX_TYPE( double) lF,lK;
+ Eigen::MatrixXd lF,lK;
double Q=0.0,R=0.0;
int Nl;
@@ -236,14 +236,14 @@ void PreviewControl::ComputeOptimalWeights(unsigned int mode)
R = 1e-6;
// Build the derivated system
- MAL_MATRIX_DIM(Ax,double,4,4);
- MAL_MATRIX_FILL(Ax,0.0);
- MAL_MATRIX(tmpA,double);
- MAL_MATRIX_DIM(bx,double,4,1);
- MAL_MATRIX(tmpb,double);
- MAL_MATRIX_DIM(cx,double,1,4);
+ Eigen::Matrix<double,4,4> Ax;
+ Ax::Zero();
+ Eigen::MatrixXd tmpA;
+ Eigen::Matrix<double,4,1> bx;
+ Eigen::MatrixXd tmpb;
+ Eigen::Matrix<double,1,4> cx;
- tmpA = MAL_RET_A_by_B(m_C,m_A);
+ tmpA = m_C*m_A;
Ax(0,0)= 1.0;
for(int i=0;i<3;i++)
@@ -254,7 +254,7 @@ void PreviewControl::ComputeOptimalWeights(unsigned int mode)
Ax(i+1,j+1) = m_A(i,j);
}
- tmpb = MAL_RET_A_by_B(m_C,m_B);
+ tmpb = m_C*m_B;
bx(0,0) = tmpb(0,0);
for(int i=0;i<3;i++)
{
@@ -318,13 +318,13 @@ void PreviewControl::ComputeOptimalWeights(unsigned int mode)
m_SizeOfPreviewWindow = (unsigned int)(m_PreviewControlTime/
m_SamplingPeriod);
- MAL_MATRIX_RESIZE(m_F,m_SizeOfPreviewWindow,1);
+ Eigen::MatrixXd m_F.resize(m_SizeOfPreviewWindow,1);
m_Coherent = true;
}
-int PreviewControl::OneIterationOfPreview(MAL_MATRIX( &x, double),
- MAL_MATRIX(& y, double),
+int PreviewControl::OneIterationOfPreview(Eigen::MatrixXd &x,
+ Eigen::MatrixXd& y,
double & sxzmp, double & syzmp,
deque<PatternGeneratorJRL::ZMPPosition> & ZMPPositions,
unsigned int lindex,
@@ -334,10 +334,10 @@ int PreviewControl::OneIterationOfPreview(MAL_MATRIX( &x, double),
double ux=0.0, uy=0.0;
- MAL_MATRIX_DIM(r,double,1,1);
+ Eigen::Matrix<double,1,1> r;
// Compute the command.
- r = MAL_RET_A_by_B(m_Kx,x);
+ r = m_Kx * x;
ux = - r(0,0) + m_Ks * sxzmp ;
if(ZMPPositions.size()<m_SizeOfPreviewWindow)
@@ -348,20 +348,20 @@ int PreviewControl::OneIterationOfPreview(MAL_MATRIX( &x, double),
for(unsigned int i=0;i<m_SizeOfPreviewWindow;i++)
ux += m_F(i,0)* ZMPPositions[lindex+i].px;
- r = MAL_RET_A_by_B(m_Kx, y);
+ r = m_Kx * y;
uy = - r(0,0) + m_Ks * syzmp;
for(unsigned int i=0;i<m_SizeOfPreviewWindow;i++)
uy += m_F(i,0)* ZMPPositions[lindex+i].py;
- x = MAL_RET_A_by_B(m_A,x) + ux * m_B;
- y = MAL_RET_A_by_B(m_A,y) + uy * m_B;
+ x = m_A*x + ux * m_B;
+ y = m_A*y + uy * m_B;
zmpx2 = 0.0;
- for(unsigned int i=0;i<MAL_MATRIX_NB_ROWS(x);i++)
+ for(unsigned int i=0;i<x.rows();i++)
zmpx2 += m_C(0,i)*x(i,0);
zmpy2 = 0.0;
- for(unsigned int i=0;i<MAL_MATRIX_NB_ROWS(y);i++)
+ for(unsigned int i=0;i<y.rows();i++)
zmpy2 += m_C(0,i)*y(i,0);
if (Simulation)
@@ -375,7 +375,7 @@ int PreviewControl::OneIterationOfPreview(MAL_MATRIX( &x, double),
return 0;
}
-int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
+int PreviewControl::OneIterationOfPreview1D(Eigen::MatrixXd &x,
double & sxzmp,
deque<double> & ZMPPositions,
unsigned int lindex,
@@ -385,10 +385,10 @@ int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
double ux=0.0;
- MAL_MATRIX_DIM(r,double,1,1);
+ Eigen::MatrixXd<double,1,1> r;
// Compute the command.
- r = MAL_RET_A_by_B(m_Kx,x);
+ r = m_Kx * x;
ux = - r(0,0) + m_Ks * sxzmp ;
ODEBUG( "x: " << x);
@@ -403,10 +403,10 @@ int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
for(unsigned int i=0;i<m_SizeOfPreviewWindow;i++)
ux += m_F(i,0)* ZMPPositions[lindex+i];
ODEBUG(" ux preview window phase: " << ux );
- x = MAL_RET_A_by_B(m_A,x) + ux * m_B;
+ x = m_A*x + ux * m_B;
zmpx2 = 0.0;
- for(unsigned int i=0;i<MAL_MATRIX_NB_ROWS(x);i++)
+ for(unsigned int i=0;i<x.rows();i++)
zmpx2 += m_C(0,i)*x(i,0);
@@ -422,7 +422,7 @@ int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
return 0;
}
-int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
+int PreviewControl::OneIterationOfPreview1D(Eigen::MatrixXd &x,
double & sxzmp,
vector<double> & ZMPPositions,
unsigned int lindex,
@@ -432,10 +432,10 @@ int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
double ux=0.0;
- MAL_MATRIX_DIM(r,double,1,1);
+ Eigen::MatrixXd<double,1,1> r;
// Compute the command.
- r = MAL_RET_A_by_B(m_Kx,x);
+ r = m_Kx * x;
ux = - r(0,0) + m_Ks * sxzmp ;
ODEBUG( "x: " << x);
@@ -467,10 +467,10 @@ int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
ux += m_F(i,0)* ZMPPositions[i];
}
ODEBUG(" ux preview window phase: " << ux );
- x = MAL_RET_A_by_B(m_A,x) + ux * m_B;
+ x = m_A*x + ux * m_B;
zmpx2 = 0.0;
- for(unsigned int i=0;i<MAL_MATRIX_NB_ROWS(x);i++)
+ for(unsigned int i=0;i<x.rows();i++)
zmpx2 += m_C(0,i)*x(i,0);
if (Simulation)
diff --git a/src/PreviewControl/PreviewControl.hh b/src/PreviewControl/PreviewControl.hh
index b6fb4a6dc2a93a7ae4992852c2bcbd8918d5cbcc..aba1a9e4d1c7a576a8eeda6a21578b570e90f116 100644
--- a/src/PreviewControl/PreviewControl.hh
+++ b/src/PreviewControl/PreviewControl.hh
@@ -69,8 +69,8 @@ namespace PatternGeneratorJRL
/*! \brief One iteration of the preview control. */
- int OneIterationOfPreview(MAL_MATRIX(& x,double),
- MAL_MATRIX(& y,double),
+ int OneIterationOfPreview(Eigen::MatrixXd &x,
+ Eigen::MatrixXd &y,
double & sxzmp, double & syzmp,
deque<PatternGeneratorJRL::ZMPPosition> & ZMPPositions,
unsigned int lindex,
@@ -79,7 +79,7 @@ namespace PatternGeneratorJRL
/*! \brief One iteration of the preview control along one axis (using queues)*/
- int OneIterationOfPreview1D(MAL_MATRIX( &x, double),
+ int OneIterationOfPreview1D(Eigen::MatrixXd &x,
double & sxzmp,
deque<double> & ZMPPositions,
unsigned int lindex,
@@ -94,7 +94,7 @@ namespace PatternGeneratorJRL
\param [out] zmpx2: Resulting ZMP value.
\param [in] Simulation: This should be set to false.
*/
- int OneIterationOfPreview1D(MAL_MATRIX( &x, double),
+ int OneIterationOfPreview1D(Eigen::MatrixXd &x,
double & sxzmp,
vector<double> & ZMPPositions,
unsigned int lindex,
@@ -141,20 +141,20 @@ namespace PatternGeneratorJRL
private:
/*! \brief Matrices for preview control. */
- MAL_MATRIX(m_A,double);
- MAL_MATRIX(m_B,double);
- MAL_MATRIX(m_C,double);
+ Eigen::MatrixXd m_A;
+ Eigen::MatrixXd m_B;
+ Eigen::MatrixXd m_C;
/** \name Control parameters.
@{ */
/*! Gain on the current state of the CoM. */
- MAL_MATRIX(m_Kx,double);
+ Eigen::MatrixXd m_Kx;
/*! Gain on the current ZMP. */
double m_Ks;
/*! Window */
- MAL_MATRIX(m_F,double);
+ Eigen::MatrixXd m_F;
//@}
/* \name Preview parameters. */
diff --git a/src/PreviewControl/ZMPPreviewControlWithMultiBodyZMP.cpp b/src/PreviewControl/ZMPPreviewControlWithMultiBodyZMP.cpp
index f26765cdada45d48881d038ca6c8acb943352c91..ed1a21aa57c314df875e48943351abf08d3f90ba 100644
--- a/src/PreviewControl/ZMPPreviewControlWithMultiBodyZMP.cpp
+++ b/src/PreviewControl/ZMPPreviewControlWithMultiBodyZMP.cpp
@@ -110,9 +110,9 @@ void ZMPPreviewControlWithMultiBodyZMP::SetPreviewControl(PreviewControl *aPC)
void ZMPPreviewControlWithMultiBodyZMP::CallToComAndFootRealization(COMState &acomp,
FootAbsolutePosition &aLeftFAP,
FootAbsolutePosition &aRightFAP,
- MAL_VECTOR_TYPE(double) &CurrentConfiguration,
- MAL_VECTOR_TYPE(double) &CurrentVelocity,
- MAL_VECTOR_TYPE(double) &CurrentAcceleration,
+ Eigen::VectorXd &CurrentConfiguration,
+ Eigen::VectorXd &CurrentVelocity,
+ Eigen::VectorXd &CurrentAcceleration,
int IterationNumber,
int StageOfTheAlgorithm)
{
@@ -197,9 +197,9 @@ void ZMPPreviewControlWithMultiBodyZMP::SetPreviewControl(PreviewControl *aPC)
FootAbsolutePosition &RightFootPosition,
ZMPPosition & ,
COMState &refandfinalCOMState,
- MAL_VECTOR_TYPE(double) & CurrentConfiguration,
- MAL_VECTOR_TYPE(double) & CurrentVelocity,
- MAL_VECTOR_TYPE(double) & CurrentAcceleration)
+ Eigen::VectorXd & CurrentConfiguration,
+ Eigen::VectorXd & CurrentVelocity,
+ Eigen::VectorXd & CurrentAcceleration)
{
FirstStageOfControl(LeftFootPosition,RightFootPosition,refandfinalCOMState);
// This call is suppose to initialize
@@ -467,7 +467,7 @@ void ZMPPreviewControlWithMultiBodyZMP::SetPreviewControl(PreviewControl *aPC)
aZMPpos.stepType = 1;
aZMPpos.time = m_FIFOZMPRefPositions[0].time;
ODEBUG("Stage 3");
- MAL_VECTOR_TYPE(double) CurrentConfiguration;
+ Eigen::VectorXd CurrentConfiguration;
/* Get the current configuration vector */
CurrentConfiguration = m_PinocchioRobot->currentConfiguration();
@@ -486,9 +486,9 @@ void ZMPPreviewControlWithMultiBodyZMP::SetPreviewControl(PreviewControl *aPC)
deque<FootAbsolutePosition> &RightFootPositions)
{
m_NumberOfIterations = 0;
- MAL_VECTOR_TYPE(double) CurrentConfiguration = m_PinocchioRobot->currentConfiguration();
- MAL_VECTOR_TYPE(double) CurrentVelocity = m_PinocchioRobot->currentVelocity();
- MAL_VECTOR_TYPE(double) CurrentAcceleration = m_PinocchioRobot->currentAcceleration();
+ Eigen::VectorXd CurrentConfiguration = m_PinocchioRobot->currentConfiguration();
+ Eigen::VectorXd CurrentVelocity = m_PinocchioRobot->currentVelocity();
+ Eigen::VectorXd CurrentAcceleration = m_PinocchioRobot->currentAcceleration();
m_PC->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITHOUT_INITIALPOS);
@@ -593,9 +593,9 @@ void ZMPPreviewControlWithMultiBodyZMP::SetPreviewControl(PreviewControl *aPC)
deque<COMState> &COMStates,
deque<FootAbsolutePosition> &LeftFootPositions,
deque<FootAbsolutePosition> &RightFootPositions,
- MAL_VECTOR_TYPE(double) & CurrentConfiguration,
- MAL_VECTOR_TYPE(double) & CurrentVelocity,
- MAL_VECTOR_TYPE(double) & CurrentAcceleration,
+ Eigen::VectorXd & CurrentConfiguration,
+ Eigen::VectorXd & CurrentVelocity,
+ Eigen::VectorXd & CurrentAcceleration,
int localindex)
{
diff --git a/src/RobotDynamics/pinocchiorobot.cpp b/src/RobotDynamics/pinocchiorobot.cpp
index f588216dc5603e73f256f721e1aac3d320cdd5f3..ca89746dfd3d05c0623f8dc96f2efa0de9943573 100644
--- a/src/RobotDynamics/pinocchiorobot.cpp
+++ b/src/RobotDynamics/pinocchiorobot.cpp
@@ -43,12 +43,12 @@ PinocchioRobot::PinocchioRobot()
m_v.fill(0.0);
m_a.fill(0.0);
m_tau.fill(0.0);
- MAL_VECTOR_RESIZE(m_qmal,50);
- MAL_VECTOR_RESIZE(m_vmal,50);
- MAL_VECTOR_RESIZE(m_amal,50);
- MAL_VECTOR_FILL(m_qmal,0.0);
- MAL_VECTOR_FILL(m_vmal,0.0);
- MAL_VECTOR_FILL(m_amal,0.0);
+ m_qmal.resize( 50 );
+ m_vmal.resize( 50 );
+ m_amal.resize( 50 );
+ m_qmal.Zero(50);
+ m_vmal.Zero(50);
+ m_amal.Zero(50);
m_f.fill(0.0);
m_n.fill(0.0);
@@ -171,12 +171,12 @@ bool PinocchioRobot::initializeRobotModelAndData(se3::Model * robotModel,
m_tau.resize(m_robotModel->nv,1);
se3::forwardKinematics(*m_robotModel,*m_robotDataInInitialePose,m_q);
- MAL_VECTOR_RESIZE(m_qmal,m_robotModel->nv);
- MAL_VECTOR_RESIZE(m_vmal,m_robotModel->nv);
- MAL_VECTOR_RESIZE(m_amal,m_robotModel->nv);
- MAL_VECTOR_FILL(m_qmal,0.0);
- MAL_VECTOR_FILL(m_vmal,0.0);
- MAL_VECTOR_FILL(m_amal,0.0);
+ m_qmal.resize(m_robotModel->nv);
+ m_vmal.resize(m_robotModel->nv);
+ m_amal.resize(m_robotModel->nv);
+ m_qmal.Zero(m_robotModel->nv);
+ m_vmal.Zero(m_robotModel->nv);
+ m_amal.Zero(m_robotModel->nv);
// compute the global mass of the robot
m_mass=0.0;
@@ -297,8 +297,8 @@ void PinocchioRobot::initializeInverseKinematics()
std::vector<se3::JointIndex> rightLeg =
jointsBetween(m_waist,m_rightFoot.associatedAnkle);
- MAL_S3_VECTOR_CLEAR(m_leftDt);
- MAL_S3_VECTOR_CLEAR(m_rightDt);
+ m_leftDt.Zero();
+ m_rightDt.Zero();
se3::SE3 waist_M_leftHip , waist_M_rightHip ;
waist_M_leftHip = m_robotModel->jointPlacements[leftLeg[0]].act(
m_robotModel->jointPlacements[leftLeg[1]]).act(
@@ -358,7 +358,7 @@ void PinocchioRobot::computeForwardKinematics()
computeForwardKinematics(m_qmal);
}
-void PinocchioRobot::computeForwardKinematics(MAL_VECTOR_TYPE(double) & q)
+void PinocchioRobot::computeForwardKinematics(Eigen::VectorXd & q)
{
// euler to quaternion :
m_quat = Eigen::Quaterniond(
@@ -388,9 +388,9 @@ void PinocchioRobot::computeInverseDynamics()
PinocchioRobot::computeInverseDynamics(m_qmal,m_vmal,m_amal);
}
-void PinocchioRobot::computeInverseDynamics(MAL_VECTOR_TYPE(double) & q,
- MAL_VECTOR_TYPE(double) & v,
- MAL_VECTOR_TYPE(double) & a)
+void PinocchioRobot::computeInverseDynamics(Eigen::VectorXd & q,
+ Eigen::VectorXd & v,
+ Eigen::VectorXd & a)
{
// for(unsigned i=0;i<3;++i)
// {
@@ -476,7 +476,7 @@ std::vector<se3::JointIndex> PinocchioRobot::jointsBetween
{
out.push_back(fromRootToSecond[0]);
}
- for(unsigned k=lastCommonRank+1 ; k<fromRootToSecond.size() ; ++k)
+ for(se3::JointIndex k=lastCommonRank+1 ; k<fromRootToSecond.size() ; ++k)
{
out.push_back(fromRootToSecond[k]);
}
@@ -489,11 +489,11 @@ bool PinocchioRobot::
ComputeSpecializedInverseKinematics(
const se3::JointIndex &jointRoot,
const se3::JointIndex &jointEnd,
- const MAL_S4x4_MATRIX_TYPE(double) & jointRootPosition,
- const MAL_S4x4_MATRIX_TYPE(double) & jointEndPosition,
- MAL_VECTOR_TYPE(double) &q )
+ const Eigen::Matrix4d & jointRootPosition,
+ const Eigen::Matrix4d & jointEndPosition,
+ Eigen::VectorXd &q )
{
- MAL_VECTOR_FILL(q,0.0);
+ q.Zero(q.size());
/*! Try to find out which kinematics chain the user
send to the method.*/
if (jointRoot==m_waist)
@@ -541,10 +541,10 @@ ComputeSpecializedInverseKinematics(
return false;
}
-void PinocchioRobot::getWaistFootKinematics(const matrix4d & jointRootPosition,
- const matrix4d & jointEndPosition,
- vectorN &q,
- vector3d Dt)
+void PinocchioRobot::getWaistFootKinematics(const Eigen::Matrix4d & jointRootPosition,
+ const Eigen::Matrix4d & jointEndPosition,
+ Eigen::VectorXd &q,
+ Eigen::Vector3d Dt)
{
double _epsilon=1.0e-6;
// definition des variables relatif au design du robot
@@ -554,30 +554,28 @@ void PinocchioRobot::getWaistFootKinematics(const matrix4d & jointRootPosition,
double c5 = 0.0;
double q6a = 0.0;
- //vector3d r;
+ //Eigen::Vector3d r;
/* Build sub-matrices */
- matrix3d Foot_R,Body_R;
- vector3d Foot_P,Body_P;
+ Eigen::Matrix3d Foot_R,Body_R;
+ Eigen::Vector3d Foot_P,Body_P;
for(unsigned int i=0;i<3;i++)
{
for(unsigned int j=0;j<3;j++)
{
- MAL_S3x3_MATRIX_ACCESS_I_J(Body_R,i,j) =
- MAL_S4x4_MATRIX_ACCESS_I_J(jointRootPosition,i,j);
- MAL_S3x3_MATRIX_ACCESS_I_J(Foot_R,i,j) =
- MAL_S4x4_MATRIX_ACCESS_I_J(jointEndPosition,i,j);
+ Body_R(i,j) = jointRootPosition(i,j);
+ Foot_R(i,j) = jointEndPosition(i,j);
}
- Body_P(i) = MAL_S4x4_MATRIX_ACCESS_I_J(jointRootPosition,i,3);
- Foot_P(i) = MAL_S4x4_MATRIX_ACCESS_I_J(jointEndPosition,i,3);
+ Body_P(i) = jointRootPosition(i,3);
+ Foot_P(i) = jointEndPosition(i,3);
}
- matrix3d Foot_Rt;
- MAL_S3x3_TRANSPOSE_A_in_At(Foot_R,Foot_Rt);
+ Eigen::Matrix3d Foot_Rt;
+ Foot_Rt=Foot_R.transpose();
// Initialisation of q
- if (MAL_VECTOR_SIZE(q)!=6)
- MAL_VECTOR_RESIZE(q,6);
+ if (q.size()!=6)
+ q.resize(6);
for(unsigned int i=0;i<6;i++)
q(i)=0.0;
@@ -585,7 +583,7 @@ void PinocchioRobot::getWaistFootKinematics(const matrix4d & jointRootPosition,
// if Dt(1)<0.0 then Opp=1.0 else Opp=-1.0
double OppSignOfDtY = Dt(1) < 0.0 ? 1.0 : -1.0;
- vector3d d2,d3;
+ Eigen::Vector3d d2,d3;
d2 = Body_P + Body_R * Dt;
d3 = d2 - Foot_P;
@@ -605,7 +603,7 @@ void PinocchioRobot::getWaistFootKinematics(const matrix4d & jointRootPosition,
q[3] = acos(c5);
}
- vector3d r3;
+ Eigen::Vector3d r3;
r3 = Foot_Rt * d3;
q6a = asin((A/l0)*sin(M_PI- q[3]));
@@ -630,41 +628,41 @@ void PinocchioRobot::getWaistFootKinematics(const matrix4d & jointRootPosition,
q[5] += M_PI;
}
- matrix3d R;
- matrix3d BRt;
- MAL_S3x3_TRANSPOSE_A_in_At(Body_R,BRt);
+ Eigen::Matrix3d R;
+ Eigen::Matrix3d BRt;
+ BRt = Body_R.transpose();
- matrix3d Rroll;
+ Eigen::Matrix3d Rroll;
double c = cos(q[5]);
double s = sin(q[5]);
- MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,0,0) = 1.0;
- MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,0,1) = 0.0;
- MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,0,2) = 0.0;
+ Rroll(0,0) = 1.0;
+ Rroll(0,1) = 0.0;
+ Rroll(0,2) = 0.0;
- MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,1,0) = 0.0;
- MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,1,1) = c;
- MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,1,2) = s;
+ Rroll(1,0) = 0.0;
+ Rroll(1,1) = c;
+ Rroll(1,2) = s;
- MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,2,0) = 0.0;
- MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,2,1) = -s;
- MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,2,2) = c;
+ Rroll(2,0) = 0.0;
+ Rroll(2,1) = -s;
+ Rroll(2,2) = c;
- matrix3d Rpitch;
+ Eigen::Matrix3d Rpitch;
c = cos(q[4]+q[3]);
s = sin(q[4]+q[3]);
- MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,0,0) = c;
- MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,0,1) = 0.0;
- MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,0,2) = -s;
+ Rpitch(0,0) = c;
+ Rpitch(0,1) = 0.0;
+ Rpitch(0,2) = -s;
- MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,1,0) = 0.0;
- MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,1,1) = 1.0;
- MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,1,2) = 0.0;
+ Rpitch(1,0) = 0.0;
+ Rpitch(1,1) = 1.0;
+ Rpitch(1,2) = 0.0;
- MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,2,0) = s;
- MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,2,1) = 0.0;
- MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,2,2) = c;
+ Rpitch(2,0) = s;
+ Rpitch(2,1) = 0.0;
+ Rpitch(2,2) = c;
R = BRt * Foot_R * Rroll * Rpitch;
q[0] = atan2(-R(0,1),R(1,1));
@@ -687,24 +685,24 @@ double PinocchioRobot::ComputeXmax(double & Z)
return Xmax;
}
-void PinocchioRobot::getShoulderWristKinematics(const matrix4d & jointRootPosition,
- const matrix4d & jointEndPosition,
- vectorN &q,
+void PinocchioRobot::getShoulderWristKinematics(const Eigen::Matrix4d & jointRootPosition,
+ const Eigen::Matrix4d & jointEndPosition,
+ Eigen::VectorXd &q,
int side)
{
// Initialisation of q
- if (MAL_VECTOR_SIZE(q)!=6)
- MAL_VECTOR_RESIZE(q,6);
+ if (q.size()!=6)
+ q.resize(6);
double Alpha,Beta;
for(unsigned int i=0;i<6;i++)
q(i)=0.0;
- double X = MAL_S4x4_MATRIX_ACCESS_I_J(jointEndPosition,0,3)
- - MAL_S4x4_MATRIX_ACCESS_I_J(jointRootPosition,0,3);
- double Z = MAL_S4x4_MATRIX_ACCESS_I_J(jointEndPosition,2,3)
- - MAL_S4x4_MATRIX_ACCESS_I_J(jointRootPosition,2,3);
+ double X = jointEndPosition(0,3)
+ - jointRootPosition(0,3);
+ double Z = jointEndPosition(2,3)
+ - jointRootPosition(2,3);
double Xmax = ComputeXmax(Z);
X = X*Xmax;
diff --git a/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp b/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
index 244ba2c64f15fc1e27e0680f59e04125fcd37859..696edb176f82a51511226522bcd7e216edd3fa74 100644
--- a/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
+++ b/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
@@ -33,7 +33,7 @@
//#define DEBUG_COUT
//#ifdef DEBUG
-void DumpMatrix(std::string fileName, MAL_MATRIX_TYPE(double) & M)
+void DumpMatrix(std::string fileName, Eigen::MatrixXf & M)
{
std::ofstream aof;
std::ostringstream oss(std::ostringstream::ate);
@@ -64,7 +64,7 @@ void DumpMatrix(std::string fileName, MAL_MATRIX_TYPE(double) & M)
}
}
-void DumpVector(std::string fileName, MAL_VECTOR_TYPE(double) & M)
+void DumpVector(std::string fileName, Eigen::VectorXf & M)
{
std::ofstream aof;
std::ostringstream oss(std::ostringstream::ate);
@@ -429,7 +429,7 @@ void NMPCgenerator::updateConstraint()
}
-void NMPCgenerator::evalConstraint(MAL_VECTOR_TYPE(double) & U)
+void NMPCgenerator::evalConstraint(Eigen::VectorXf & U)
{
//
// Eval real problem bounds : lb_ < g(U) < ub_
@@ -452,7 +452,7 @@ void NMPCgenerator::evalConstraint(MAL_VECTOR_TYPE(double) & U)
{
for(unsigned n=0 ; n<nf_ ; ++n)
{
- MAL_VECTOR_TYPE(double) HobsUxy = MAL_RET_A_by_B(Hobs_[obs][n],Uxy_);
+ Eigen::VectorXf HobsUxy = MAL_RET_A_by_B(Hobs_[obs][n],Uxy_);
double deltaObs = 0 ;
for(unsigned i=0 ; i<MAL_VECTOR_SIZE(HobsUxy) ; ++i)
deltaObs += Uxy_(i) * (HobsUxy(i) + Aobs_[obs][n](i)) ;
@@ -1338,7 +1338,7 @@ void NMPCgenerator::initializeCoPConstraint()
return ;
}
-void NMPCgenerator::updateCoPconstraint(MAL_VECTOR_TYPE(double) &U)
+void NMPCgenerator::updateCoPconstraint(Eigen::VectorXf &U)
{
if(nc_cop_==0)
return ;
@@ -1396,7 +1396,7 @@ void NMPCgenerator::updateCoPconstraint(MAL_VECTOR_TYPE(double) &U)
return ;
}
-void NMPCgenerator::evalCoPconstraint(MAL_VECTOR_TYPE(double) & U)
+void NMPCgenerator::evalCoPconstraint(Eigen::VectorXf & U)
{
if(nc_cop_==0)
return ;
@@ -1514,7 +1514,7 @@ void NMPCgenerator::initializeFootPoseConstraint()
return ;
}
-void NMPCgenerator::updateFootPoseConstraint(MAL_VECTOR_TYPE(double) &U)
+void NMPCgenerator::updateFootPoseConstraint(Eigen::VectorXf &U)
{
// if(nc_foot_==0)
// return ;
@@ -1619,7 +1619,7 @@ void NMPCgenerator::updateFootPoseConstraint(MAL_VECTOR_TYPE(double) &U)
return ;
}
-void NMPCgenerator::evalFootPoseConstraint(MAL_VECTOR_TYPE(double) & U)
+void NMPCgenerator::evalFootPoseConstraint(Eigen::VectorXf & U)
{
if(nc_foot_==0)
return ;
@@ -1846,8 +1846,8 @@ void NMPCgenerator::updateObstacleConstraint()
MAL_VECTOR_FILL(A,0.0);
MAL_VECTOR_FILL(B,0.0);
- Hobs_.resize(obstacles_.size() , std::vector<MAL_MATRIX_TYPE(double)>(nc_obs_,H) );
- Aobs_.resize(obstacles_.size() , std::vector<MAL_VECTOR_TYPE(double)>(nc_obs_,A) );
+ Hobs_.resize(obstacles_.size() , std::vector<Eigen::MatrixXf>(nc_obs_,H) );
+ Aobs_.resize(obstacles_.size() , std::vector<Eigen::VectorXf>(nc_obs_,A) );
UBobs_.resize(obstacles_.size() , B );
unsigned nc = nf_ ;
diff --git a/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh b/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh
index c306051569e966c7df6268685700a79480be6e90..7e0326010a210f502a110ad411f40b9256d5dbb2 100644
--- a/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh
+++ b/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh
@@ -87,14 +87,14 @@ namespace PatternGeneratorJRL
// build the constraints :
void initializeConstraint();
void updateConstraint();
- void evalConstraint(MAL_VECTOR_TYPE(double) & U);
+ void evalConstraint(Eigen::VectorXf & U);
void initializeCoPConstraint();
- void evalCoPconstraint(MAL_VECTOR_TYPE(double) & U);
- void updateCoPconstraint(MAL_VECTOR_TYPE(double) & U);
+ void evalCoPconstraint(Eigen::VectorXf & U);
+ void updateCoPconstraint(Eigen::VectorXf & U);
void initializeFootPoseConstraint();
- void evalFootPoseConstraint(MAL_VECTOR_TYPE(double) & U);
- void updateFootPoseConstraint(MAL_VECTOR_TYPE(double) & U);
+ void evalFootPoseConstraint(Eigen::VectorXf & U);
+ void updateFootPoseConstraint(Eigen::VectorXf & U);
void initializeFootVelIneqConstraint();
void updateFootVelIneqConstraint();
void initializeRotIneqConstraint();
@@ -220,17 +220,17 @@ namespace PatternGeneratorJRL
double time_;
// [x, dx, ddx], com (pos, vel, acc) at instant t_k on axis X
- MAL_VECTOR_TYPE(double) c_k_x_;
+ Eigen::VectorXf c_k_x_;
// [y, dy, ddy], com (pos, vel, acc) at instant t_k on axis Y
- MAL_VECTOR_TYPE(double) c_k_y_;
+ Eigen::VectorXf c_k_y_;
/**
NOTE number of foot steps in prediction horizon changes between
nf and nf+1, because when robot takes first step nf steps are
planned on the prediction horizon, which makes a total of nf+1 steps.
*/
- MAL_VECTOR_TYPE(double) v_kp1_ ;
- MAL_MATRIX_TYPE(double) V_kp1_ ;
+ Eigen::VectorXf v_kp1_ ;
+ Eigen::MatrixXd V_kp1_ ;
// Usefull for managing the PG
@@ -246,18 +246,18 @@ namespace PatternGeneratorJRL
// Constraint Matrix
// Center of Pressure constraint
unsigned nc_cop_ ;
- MAL_MATRIX_TYPE(double) Acop_xy_, Acop_theta_ ;
- MAL_VECTOR_TYPE(double) UBcop_ ;
- MAL_MATRIX_TYPE(double) D_kp1_xy_, D_kp1_theta_, Pzuv_, derv_Acop_map_ ;
- MAL_MATRIX_TYPE(double) derv_Acop_map2_ ;
- MAL_VECTOR_TYPE(double) b_kp1_, Pzsc_, Pzsc_x_, Pzsc_y_, v_kp1f_, v_kp1f_x_, v_kp1f_y_ ;
- MAL_VECTOR_TYPE(double) v_kf_x_, v_kf_y_ ;
- MAL_MATRIX_TYPE(double) diffMat_ ;
- MAL_MATRIX_TYPE(double) rotMat_xy_, rotMat_theta_, rotMat_;
- MAL_MATRIX_TYPE(double) A0_xy_, A0_theta_;
- MAL_VECTOR_TYPE(double) B0_;
- MAL_MATRIX_TYPE(double) Acop_theta_dummy0_;
- MAL_VECTOR_TYPE(double) Acop_theta_dummy1_;
+ Eigen::MatrixXd Acop_xy_, Acop_theta_ ;
+ Eigen::VectorXf UBcop_ ;
+ Eigen::MatrixXd D_kp1_xy_, D_kp1_theta_, Pzuv_, derv_Acop_map_ ;
+ Eigen::MatrixXd derv_Acop_map2_ ;
+ Eigen::VectorXf b_kp1_, Pzsc_, Pzsc_x_, Pzsc_y_, v_kp1f_, v_kp1f_x_, v_kp1f_y_ ;
+ Eigen::VectorXf v_kf_x_, v_kf_y_ ;
+ Eigen::MatrixXd diffMat_ ;
+ Eigen::MatrixXd rotMat_xy_, rotMat_theta_, rotMat_;
+ Eigen::MatrixXd A0_xy_, A0_theta_;
+ Eigen::VectorXf B0_;
+ Eigen::MatrixXd Acop_theta_dummy0_;
+ Eigen::VectorXf Acop_theta_dummy1_;
// Foot position constraint
unsigned nc_foot_ ;
@@ -265,77 +265,77 @@ namespace PatternGeneratorJRL
unsigned itBeforeLanding_ ;
bool useItBeforeLanding_ ;
int itMax_;
- std::vector<MAL_MATRIX_TYPE(double)> Afoot_xy_, Afoot_theta_ ;
- std::vector<MAL_VECTOR_TYPE(double)> UBfoot_ ;
- std::vector<MAL_MATRIX_TYPE(double)> SelecMat_;
- std::vector<MAL_MATRIX_TYPE(double)> A0f_xy_, A0f_theta_ ;
- std::vector<MAL_VECTOR_TYPE(double)> B0f_;
- std::vector<MAL_MATRIX_TYPE(double)> rotMat_vec_, drotMat_vec_ ;
- MAL_MATRIX_TYPE(double) tmpRotMat_;
- std::vector<MAL_VECTOR_TYPE(double)> deltaF_ ;
- std::vector<MAL_VECTOR_TYPE(double)> AdRdF_ ;
- MAL_MATRIX_TYPE(double) Afoot_xy_full_, Afoot_theta_full_ ;
- MAL_VECTOR_TYPE(double) UBfoot_full_ ;
+ std::vector<Eigen::MatrixXd> Afoot_xy_, Afoot_theta_ ;
+ std::vector<Eigen::VectorXf> UBfoot_ ;
+ std::vector<Eigen::MatrixXd> SelecMat_;
+ std::vector<Eigen::MatrixXd> A0f_xy_, A0f_theta_ ;
+ std::vector<Eigen::VectorXf> B0f_;
+ std::vector<Eigen::MatrixXd> rotMat_vec_, drotMat_vec_ ;
+ Eigen::MatrixXd tmpRotMat_;
+ std::vector<Eigen::VectorXf> deltaF_ ;
+ std::vector<Eigen::VectorXf> AdRdF_ ;
+ Eigen::MatrixXd Afoot_xy_full_, Afoot_theta_full_ ;
+ Eigen::VectorXf UBfoot_full_ ;
// Foot Velocity constraint
unsigned nc_vel_ ;
std::deque <RelativeFootPosition> desiredNextSupportFootRelativePosition ;
std::vector<support_state_t> desiredNextSupportFootAbsolutePosition ;
- MAL_MATRIX_TYPE(double) Avel_ ;
- MAL_VECTOR_TYPE(double) Bvel_ ;
+ Eigen::MatrixXd Avel_ ;
+ Eigen::VectorXf Bvel_ ;
// Foot Position constraint
unsigned nc_pos_ ;
- MAL_MATRIX_TYPE(double) Apos_ ;
- MAL_VECTOR_TYPE(double) Bpos_ ;
+ Eigen::MatrixXd Apos_ ;
+ Eigen::VectorXf Bpos_ ;
// Rotation linear constraint
unsigned nc_rot_ ;
- MAL_MATRIX_TYPE(double) Arot_ ;
- MAL_VECTOR_TYPE(double) UBrot_,LBrot_ ;
+ Eigen::MatrixXd Arot_ ;
+ Eigen::VectorXf UBrot_,LBrot_ ;
// Obstacle constraint
unsigned nc_obs_ ;
- std::vector< std::vector<MAL_MATRIX_TYPE(double)> > Hobs_ ;
- std::vector< std::vector<MAL_VECTOR_TYPE(double)> > Aobs_ ;
- std::vector< MAL_VECTOR_TYPE(double) > UBobs_ ;
+ std::vector< std::vector<Eigen::MatrixXd> > Hobs_ ;
+ std::vector< std::vector<Eigen::VectorXf> > Aobs_ ;
+ std::vector< Eigen::VectorXf > UBobs_ ;
std::vector<Circle> obstacles_ ;
- MAL_VECTOR_TYPE(double) qp_J_obs_i_ ;
+ Eigen::VectorXf qp_J_obs_i_ ;
// Standing constraint :
unsigned nc_stan_ ;
- MAL_MATRIX_TYPE(double) Astan_ ;
- MAL_VECTOR_TYPE(double) UBstan_, LBstan_ ;
+ Eigen::MatrixXd Astan_ ;
+ Eigen::VectorXf UBstan_, LBstan_ ;
// evaluate constraint
// real problem bounds : lb_ < g(U) < ub_
- MAL_VECTOR_TYPE(double) ub_ ;
- MAL_VECTOR_TYPE(double) gU_ ;
- MAL_VECTOR_TYPE(double) Uxy_ ;
- MAL_VECTOR_TYPE(double) gU_cop_, gU_vel_ ;
- MAL_VECTOR_TYPE(double) gU_foot_ ;
- MAL_VECTOR_TYPE(double) gU_obs_, gU_rot_ , gU_stan_ ;
+ Eigen::VectorXf ub_ ;
+ Eigen::VectorXf gU_ ;
+ Eigen::VectorXf Uxy_ ;
+ Eigen::VectorXf gU_cop_, gU_vel_ ;
+ Eigen::VectorXf gU_foot_ ;
+ Eigen::VectorXf gU_obs_, gU_rot_ , gU_stan_ ;
// Cost Function
unsigned nv_ ; // number of degrees of freedom
// initial problem matrix
- MAL_MATRIX_TYPE(double) Q_theta_, I_NN_, I_FF_ ;
+ Eigen::MatrixXd Q_theta_, I_NN_, I_FF_ ;
// decomposition of p_xy_
// p_xy_ = ( p_xy_X_, p_xy_Fx_, p_xy_Y_, p_xy_Fy_ )
- MAL_VECTOR_TYPE(double) p_xy_X_, p_xy_Fx_, p_xy_Y_, p_xy_Fy_ ;
- MAL_VECTOR_TYPE(double) Pvsc_x_ , Pvsc_y_ ;
+ Eigen::VectorXf p_xy_X_, p_xy_Fx_, p_xy_Y_, p_xy_Fy_ ;
+ Eigen::VectorXf Pvsc_x_ , Pvsc_y_ ;
// decomposition of Q_x_=Q_y_
// Q_x = ( Q_x_XX Q_x_XF ) = Q_y
// ( Q_x_FX Q_x_FF )
- MAL_MATRIX_TYPE(double) Q_x_XX_, Q_x_XF_, Q_x_FX_, Q_x_FF_ ;
- MAL_MATRIX_TYPE(double) Q_y_XX_;// Q_x_XX_ != Q_y_XX_
+ Eigen::MatrixXd Q_x_XX_, Q_x_XF_, Q_x_FX_, Q_x_FF_ ;
+ Eigen::MatrixXd Q_y_XX_;// Q_x_XX_ != Q_y_XX_
// Line Search
bool useLineSearch_ ;
- MAL_VECTOR_TYPE(double) p_ , U_n_, selectActiveConstraint ;
- MAL_VECTOR_TYPE(double) JdU_, contraintValue ;
- MAL_VECTOR_TYPE(double) HUn_ ;
+ Eigen::VectorXf p_ , U_n_, selectActiveConstraint ;
+ Eigen::VectorXf JdU_, contraintValue ;
+ Eigen::VectorXf HUn_ ;
double lineStep_, lineStep0_, stepParam_ ; // step searched
double mu_ ; // weight between cost function and constraints
double cm_, c_ ; // Merit Function Jacobian
@@ -348,12 +348,12 @@ namespace PatternGeneratorJRL
unsigned nceq_ ;
unsigned ncineq_ ;
unsigned nc_ ;
- MAL_MATRIX_TYPE(double) qp_H_ ;
- MAL_VECTOR_TYPE(double) qp_g_ ;
- MAL_MATRIX_TYPE(double) qp_J_ ; //constraint Jacobian
- MAL_VECTOR_TYPE(double) qp_ubJ_ ; //constraint Jacobian
+ Eigen::MatrixXd qp_H_ ;
+ Eigen::VectorXf qp_g_ ;
+ Eigen::MatrixXd qp_J_ ; //constraint Jacobian
+ Eigen::VectorXf qp_ubJ_ ; //constraint Jacobian
// temporary usefull variable for matrix manipulation
- MAL_VECTOR_TYPE(double) qp_g_x_, qp_g_y_, qp_g_theta_ ;
+ Eigen::VectorXf qp_g_x_, qp_g_y_, qp_g_theta_ ;
// Free variable of the system
// U_ = [C_kp1_x_ F_kp1_x_ C_kp1_y_ F_kp1_y_ F_kp1_theta_]^T
@@ -361,13 +361,13 @@ namespace PatternGeneratorJRL
// U_x = [C_kp1_x_ F_kp1_x_]^T
// U_y = [C_kp1_y_ F_kp1_y_]^T
// U_theta_ = [F_kp1_theta_]^T
- MAL_VECTOR_TYPE(double) U_ ;
- MAL_VECTOR_TYPE(double) U_xy_ ;
- MAL_VECTOR_TYPE(double) U_x_ ;
- MAL_VECTOR_TYPE(double) U_y_ ;
- MAL_VECTOR_TYPE(double) F_kp1_x_ ;
- MAL_VECTOR_TYPE(double) F_kp1_y_ ;
- MAL_VECTOR_TYPE(double) F_kp1_theta_ ;
+ Eigen::VectorXf U_ ;
+ Eigen::VectorXf U_xy_ ;
+ Eigen::VectorXf U_x_ ;
+ Eigen::VectorXf U_y_ ;
+ Eigen::VectorXf F_kp1_x_ ;
+ Eigen::VectorXf F_kp1_y_ ;
+ Eigen::VectorXf F_kp1_theta_ ;
// Time constant parameter
//////////////////////////
@@ -406,17 +406,17 @@ namespace PatternGeneratorJRL
// Z_k+1_% = position of the ZMP on the whole preview (free variable)
// C_k+1_% = Pps * c_k_% + Ppu dddC_k+1_% (%={x,y})
- MAL_MATRIX_TYPE(double) Pps_ ;
- MAL_MATRIX_TYPE(double) Ppu_ ;
+ Eigen::MatrixXd Pps_ ;
+ Eigen::MatrixXd Ppu_ ;
// dC_k+1_% = Pvs * c_k_% + Pvu dddC_k+1_%
- MAL_MATRIX_TYPE(double) Pvs_ ;
- MAL_MATRIX_TYPE(double) Pvu_ ;
+ Eigen::MatrixXd Pvs_ ;
+ Eigen::MatrixXd Pvu_ ;
// ddC_k+1_% = Pas * c_k_% + Pau dddC_k+1_%
- MAL_MATRIX_TYPE(double) Pas_ ;
- MAL_MATRIX_TYPE(double) Pau_ ;
+ Eigen::MatrixXd Pas_ ;
+ Eigen::MatrixXd Pau_ ;
// Z_k+1_% = Pzs * c_k_% + Pzu dddC_k+1_%
- MAL_MATRIX_TYPE(double) Pzs_ ;
- MAL_MATRIX_TYPE(double) Pzu_ ;
+ Eigen::MatrixXd Pzs_ ;
+ Eigen::MatrixXd Pzu_ ;
// Convex Hulls for ZMP and FootStep constraints :
support_state_t dummySupp_ ;
@@ -425,23 +425,23 @@ namespace PatternGeneratorJRL
double FeetDistance_;
// linear system corresponding to the foot step constraints
// right foot polygonal hull
- MAL_MATRIX_TYPE(double) A0r_ ;
- MAL_VECTOR_TYPE(double) ubB0r_;
+ Eigen::MatrixXd A0r_ ;
+ Eigen::VectorXf ubB0r_;
// left foot polygonal hull
- MAL_MATRIX_TYPE(double) A0l_ ;
- MAL_VECTOR_TYPE(double) ubB0l_;
+ Eigen::MatrixXd A0l_ ;
+ Eigen::VectorXf ubB0l_;
// linear systems corresponding to the CoP constraints
// right foot hull minus security margin
- MAL_MATRIX_TYPE(double) A0rf_ ;
- MAL_VECTOR_TYPE(double) ubB0rf_ ;
+ Eigen::MatrixXd A0rf_ ;
+ Eigen::VectorXf ubB0rf_ ;
// left foot hull minus security margin
- MAL_MATRIX_TYPE(double) A0lf_ ;
- MAL_VECTOR_TYPE(double) ubB0lf_ ;
+ Eigen::MatrixXd A0lf_ ;
+ Eigen::VectorXf ubB0lf_ ;
// foot hull minus security margin for standing still
// or dealing with the switching mass phase
- MAL_MATRIX_TYPE(double) A0ds_ ;
- MAL_VECTOR_TYPE(double) ubB0ds_ ;
+ Eigen::MatrixXd A0ds_ ;
+ Eigen::VectorXf ubB0ds_ ;
// [Vx, Vy, Vtheta], reference velocity express in local frame
// [Vx, Vy, Vtheta], reference velocity express in global frame
@@ -455,7 +455,7 @@ namespace PatternGeneratorJRL
Eigen::QuadProgDense * QP_ ;
Eigen::MatrixXd QuadProg_H_, QuadProg_J_eq_, QuadProg_J_ineq_;
Eigen::VectorXd QuadProg_g_, QuadProg_bJ_eq_, QuadProg_lbJ_ineq_, deltaU_;
- MAL_VECTOR_TYPE(double) deltaU_thresh_ ;
+ Eigen::VectorXf deltaU_thresh_ ;
};
diff --git a/src/patterngeneratorinterfaceprivate.hh b/src/patterngeneratorinterfaceprivate.hh
index cc0f34264d7d9c16c1ad1000097bb901892f17fe..d911f935501ee313d433422901f719ed2050eab3 100644
--- a/src/patterngeneratorinterfaceprivate.hh
+++ b/src/patterngeneratorinterfaceprivate.hh
@@ -177,10 +177,10 @@ namespace PatternGeneratorJRL
@param[out] ZMPTarget The target ZMP in the waist reference frame.
@return True is there is still some data to send, false otherwise.
*/
- bool RunOneStepOfTheControlLoop(MAL_VECTOR_TYPE(double) & CurrentConfiguration,
- MAL_VECTOR_TYPE(double) & CurrentVelocity,
- MAL_VECTOR_TYPE(double) & CurrentAcceleration,
- MAL_VECTOR_TYPE(double) & ZMPTarget);
+ bool RunOneStepOfTheControlLoop(Eigen::VectorXd & CurrentConfiguration,
+ Eigen::VectorXd & CurrentVelocity,
+ Eigen::VectorXd & CurrentAcceleration,
+ Eigen::VectorXd & ZMPTarget);
/*! \brief Run One Step of the global control loop aka The Main Method To Be Used.
@param[out] CurrentConfiguration The current configuration of the robot according to
@@ -196,10 +196,10 @@ namespace PatternGeneratorJRL
@param[out] RightFootPosition: Absolute position of the right foot.
@return True is there is still some data to send, false otherwise.
*/
- bool RunOneStepOfTheControlLoop(MAL_VECTOR_TYPE(double) & CurrentConfiguration,
- MAL_VECTOR_TYPE(double) & CurrentVelocity,
- MAL_VECTOR_TYPE(double) & CurrentAcceleration,
- MAL_VECTOR_TYPE(double) &ZMPTarget,
+ bool RunOneStepOfTheControlLoop(Eigen::VectorXd & CurrentConfiguration,
+ Eigen::VectorXd & CurrentVelocity,
+ Eigen::VectorXd & CurrentAcceleration,
+ Eigen::VectorXd &ZMPTarget,
COMState &COMState,
FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition);
@@ -218,10 +218,10 @@ namespace PatternGeneratorJRL
@param[out] RightFootPosition: Absolute position of the right foot.
@return True is there is still some data to send, false otherwise.
*/
- bool RunOneStepOfTheControlLoop(MAL_VECTOR_TYPE(double) & CurrentConfiguration,
- MAL_VECTOR_TYPE(double) & CurrentVelocity,
- MAL_VECTOR_TYPE(double) & CurrentAcceleration,
- MAL_VECTOR_TYPE(double) &ZMPTarget,
+ bool RunOneStepOfTheControlLoop(Eigen::VectorXd & CurrentConfiguration,
+ Eigen::VectorXd & CurrentVelocity,
+ Eigen::VectorXd & CurrentAcceleration,
+ Eigen::VectorXd &ZMPTarget,
COMPosition &aCOMPosition,
FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition);
@@ -367,7 +367,7 @@ namespace PatternGeneratorJRL
for the initiale pose.*/
void EvaluateStartingState(COMState & lStartingCOMState,
MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition,
- MAL_VECTOR_TYPE(double) & lStartingWaistPose,
+ Eigen::VectorXd & lStartingWaistPose,
FootAbsolutePosition & InitLeftFootAbsPos,
FootAbsolutePosition & InitRightFootAbsPos);
@@ -661,7 +661,7 @@ namespace PatternGeneratorJRL
//CjrlHumanoidDynamicRobot * m_HumanoidDynamicRobot, * m_2HumanoidDynamicRobot;
/*! Speed of the leg. */
- MAL_VECTOR_TYPE(double) m_dqr,m_dql;
+ Eigen::VectorXd m_dqr,m_dql;
/*! Objet to realize the generate the posture for given CoM
and feet positions. */
diff --git a/src/privatepgtypes.hh b/src/privatepgtypes.hh
index 34606a5249cec3d106d6b1fa213aaeffec482f86..6cea46e2dc5003ebda0c62b65bde075a0e334b6c 100644
--- a/src/privatepgtypes.hh
+++ b/src/privatepgtypes.hh
@@ -135,9 +135,9 @@ namespace PatternGeneratorJRL
/// \brief State of the center of mass
struct com_t
{
- MAL_VECTOR(x,double);
- MAL_VECTOR(y,double);
- MAL_VECTOR(z,double);
+ Eigen::VectorXd x;
+ Eigen::VectorXd y;
+ Eigen::VectorXd z;
struct com_t & operator=(const com_t &aCS);
@@ -150,13 +150,13 @@ namespace PatternGeneratorJRL
// Support state of the robot at a certain point in time
struct trunk_t
{
- MAL_VECTOR(x,double);
- MAL_VECTOR(y,double);
- MAL_VECTOR(z,double);
+ Eigen::VectorXd x;
+ Eigen::VectorXd y;
+ Eigen::VectorXd z;
- MAL_VECTOR(yaw,double);
- MAL_VECTOR(pitch,double);
- MAL_VECTOR(roll,double);
+ Eigen::VectorXd yaw;
+ Eigen::VectorXd pitch;
+ Eigen::VectorXd roll;
struct trunk_t & operator=(const trunk_t &aTS);
@@ -181,9 +181,9 @@ namespace PatternGeneratorJRL
double X, Y, Yaw;
/// \brief Reference vectors
- MAL_VECTOR(X_vec,double);
- MAL_VECTOR(Y_vec,double);
- MAL_VECTOR(Yaw_vec,double);
+ Eigen::VectorXd Xvec;
+ Eigen::VectorXd Yvec;
+ Eigen::VectorXd Yawvec;
frame_t();
frame_t(const frame_t &);
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 5553ea40d52b31a0ffcff3532e6a0338528308a1..8fbd6c0330415809a9c0209ea132522633c641bb 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -15,8 +15,6 @@
INCLUDE(CTest)
-# Import jrl-mal flags.
-ADD_DEFINITIONS(${JRL_MAL_CFLAGS})
# Make sure private headers can be used.
INCLUDE_DIRECTORIES(${CMAKE_SOURCE_DIR}/src)
@@ -26,7 +24,7 @@ CONFIGURE_FILE(${CMAKE_SOURCE_DIR}/private_cmake/config_private.hh.cmake
${CMAKE_BINARY_DIR}/include/${HEADER_DIR}/config_private.hh)
ADD_REQUIRED_DEPENDENCY("simple_humanoid_description >= 1.0.1")
-ADD_OPTIONAL_DEPENDENCY("hrp2_14_description >= 1.0.2")
+#ADD_OPTIONAL_DEPENDENCY("hrp2_14_description >= 1.0.2")
#################
## Generic test #
@@ -41,7 +39,6 @@ IF(GENERIC_TEST)
)
TARGET_LINK_LIBRARIES(TestFootPrintPGInterface ${PROJECT_NAME})
- PKG_CONFIG_USE_DEPENDENCY(TestFootPrintPGInterface jrl-mal)
ADD_DEPENDENCIES(TestFootPrintPGInterface ${PROJECT_NAME})
ENDIF(GENERIC_TEST)
@@ -73,8 +70,6 @@ ADD_EXECUTABLE(TestRiccatiEquation
TestRiccatiEquation.cpp
../src/PreviewControl/OptimalControllerSolver.cpp
)
-# Need this dependency for the use of the fortran functions (it seems)
-PKG_CONFIG_USE_DEPENDENCY(TestRiccatiEquation jrl-mal)
# Add test on the ricatti equation
ADD_TEST(TestRiccatiEquation TestRiccatiEquation)
@@ -110,7 +105,6 @@ MACRO(ADD_JRL_WALKGEN_EXE test_arg test_file_name)
)
TARGET_LINK_LIBRARIES(${test_name} ${PROJECT_NAME})
- PKG_CONFIG_USE_DEPENDENCY(${test_name} jrl-mal)
PKG_CONFIG_USE_DEPENDENCY(${test_name} pinocchio)
ENDMACRO(ADD_JRL_WALKGEN_EXE)