Commit 6604ab7d authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Transition from jrl_mal to Eigen

parent 78f90508
......@@ -42,7 +42,9 @@
#endif
#include <iostream>
#include <fstream>
#include <jrl/mal/matrixabstractlayer.hh>
#include <Eigen/Dense>
#include <vector>
namespace PatternGeneratorJRL
{
......@@ -198,9 +200,9 @@ namespace PatternGeneratorJRL
// Linear constraint.
struct LinearConstraintInequality_s
{
MAL_MATRIX(A,double);
MAL_MATRIX(B,double);
MAL_VECTOR(Center,double);
Eigen::MatrixXd A;
Eigen::MatrixXd B;
Eigen::VectorXd Center;
std::vector<int> SimilarConstraints;
double StartingTime, EndingTime;
};
......@@ -210,8 +212,8 @@ namespace PatternGeneratorJRL
/// Linear constraints with variable feet placement.
struct LinearConstraintInequalityFreeFeet_s
{
MAL_MATRIX(D,double);
MAL_MATRIX(Dc,double);
Eigen::MatrixXd D;
Eigen::MatrixXd Dc;
int StepNumber;
};
typedef struct LinearConstraintInequalityFreeFeet_s
......@@ -240,9 +242,9 @@ namespace PatternGeneratorJRL
double x,y,z, dYaw;
/*! reference values for the whole preview window */
MAL_VECTOR(RefVectorX,double);
MAL_VECTOR(RefVectorY,double);
MAL_VECTOR(RefVectorTheta,double);
Eigen::VectorXd RefVectorX;
Eigen::VectorXd RefVectorY;
Eigen::VectorXd RefVectorTheta;
};
typedef struct ReferenceAbsoluteVelocity_t ReferenceAbsoluteVelocity;
......
......@@ -35,8 +35,6 @@
/* System related inclusions */
#include <deque>
/*! MatrixAbstractLayer */
#include <jrl/mal/matrixabstractlayer.hh>
/* dynamics pinocchio related inclusions */
#include <jrl/walkgen/pinocchiorobot.hh>
......
......@@ -63,7 +63,7 @@ FootTrajectoryGenerationStandard::FootTrajectoryGenerationStandard(SimplePluginM
{
cerr << "Pb no ref Foot." << endl;
}
vector3d AnklePosition;
Eigen::Vector3d AnklePosition;
if (m_Foot->associatedAnkle!=0)
AnklePosition = m_Foot->anklePosition;
else
......@@ -859,7 +859,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
aoflocal << dFX << " " << dFY << " " << dFZ << " " << lOmega << endl;
aoflocal.close();
#endif
MAL_S3_VECTOR(Foot_Shift,double);
Eigen::Vector3d Foot_Shift;
#if 0
double co,so;
......@@ -867,7 +867,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
so = sin(lOmega);
// COM Orientation
MAL_S3x3_MATRIX(Foot_R,double);
Eigen::Matrix3d Foot_R;
Foot_R(0,0) = c*co; Foot_R(0,1) = -s; Foot_R(0,2) = c*so;
Foot_R(1,0) = s*co; Foot_R(1,1) = c; Foot_R(1,2) = s*so;
......@@ -875,10 +875,10 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
if (LeftOrRight==-1)
{
MAL_S3x3_C_eq_A_by_B(Foot_Shift, Foot_R,m_AnklePositionRight);
Foot_Shift=Foot_R+m_AnklePositionRight;
}
else if (LeftOrRight==1)
MAL_S3x3_C_eq_A_by_B(Foot_Shift, Foot_R,m_AnklePositionLeft);
Foot_Shift=Foot_R+m_AnklePositionLeft;
// Modification of the foot position.
curr_NSFAP.x += (dFX + Foot_Shift(0));
......@@ -1070,7 +1070,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
aoflocal << dFX << " " << dFY << " " << dFZ << " " << lOmega << endl;
aoflocal.close();
#endif
MAL_S3_VECTOR(Foot_Shift,double);
Eigen::Vector3d Foot_Shift;
#if 0
double co,so;
......@@ -1078,7 +1078,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
so = sin(lOmega);
// COM Orientation
MAL_S3x3_MATRIX(Foot_R,double);
Eigen::Matrix3d Foot_R;
Foot_R(0,0) = c*co; Foot_R(0,1) = -s; Foot_R(0,2) = c*so;
Foot_R(1,0) = s*co; Foot_R(1,1) = c; Foot_R(1,2) = s*so;
......@@ -1086,10 +1086,10 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
if (LeftOrRight==-1)
{
MAL_S3x3_C_eq_A_by_B(Foot_Shift, Foot_R,m_AnklePositionRight);
Foot_Shift=Foot_R+m_AnklePositionRight;
}
else if (LeftOrRight==1)
MAL_S3x3_C_eq_A_by_B(Foot_Shift, Foot_R,m_AnklePositionLeft);
Foot_Shift=Foot_R+m_AnklePositionLeft;
// Modification of the foot position.
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x += (dFX + Foot_Shift(0));
......@@ -1120,12 +1120,12 @@ void FootTrajectoryGenerationStandard::ComputingAbsFootPosFromQueueOfRelPos(dequ
/*! Compute the absolute coordinates of the steps. */
double CurrentAbsTheta=0.0,c=0.0,s=0.0;
MAL_MATRIX_DIM(MM,double,2,2);
MAL_MATRIX_DIM(CurrentSupportFootPosition,double,3,3);
MAL_MATRIX_DIM(Orientation,double,2,2);
MAL_MATRIX_SET_IDENTITY(CurrentSupportFootPosition);
MAL_MATRIX_DIM(v,double,2,1);
MAL_MATRIX_DIM(v2,double,2,1);
Eigen::Matrix<double,2,2> MM;;
Eigen::Matrix<double,3,3> CurrentSupportFootPosition;;
Eigen::Matrix<double,2,2> Orientation;;
CurrentSupportFootPosition.setIdentity();
Eigen::Matrix<double,2,1> v;;
Eigen::Matrix<double,2,1> v2;;
for(unsigned int i=0;i<RelativeFootPositions.size();i++)
{
......@@ -1150,8 +1150,8 @@ void FootTrajectoryGenerationStandard::ComputingAbsFootPosFromQueueOfRelPos(dequ
v(1,0) = RelativeFootPositions[i].sy;
/*! Compute the new orientation of the foot vector. */
Orientation = MAL_RET_A_by_B(MM , Orientation);
v2 = MAL_RET_A_by_B(Orientation, v);
Orientation = MM*Orientation;
v2 = Orientation*v;
/*! Update the world coordinates of the support foot. */
for(int k=0;k<2;k++)
......
......@@ -263,10 +263,10 @@ namespace PatternGeneratorJRL
double m_FootB, m_FootH, m_FootF;
/*! \brief Position of the ankle in the left foot. */
MAL_S3_VECTOR(m_AnklePositionLeft,double);
Eigen::Vector3d m_AnklePositionLeft;
/*! \brief Position of the ankle in the right foot. */
MAL_S3_VECTOR(m_AnklePositionRight,double);
Eigen::Vector3d m_AnklePositionRight;
};
......
......@@ -62,7 +62,7 @@ LeftAndRightFootTrajectoryGenerationMultiple(SimplePluginManager *lSPM,
}
}
MAL_VECTOR_FILL(m_MiddleWayPoint,0.0) ;
{ for(unsigned int i=0;i<m_MiddleWayPoint.size();m_MiddleWayPoint[i++]=0.0);} ;
wayPoint.resize(2,0.0);
}
......@@ -204,7 +204,8 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
RelativeFootPositions.size())
SupportFootAbsoluteFootPositions.resize(RelativeFootPositions.size());
unsigned int lNbOfIntervals = RelativeFootPositions.size();
deque<RelativeFootPosition>::size_type
lNbOfIntervals = RelativeFootPositions.size();
/*! It is assumed that a set of relative positions for the support foot
are given as an input. */
deque<FootAbsolutePosition> AbsoluteFootPositions;
......@@ -226,13 +227,13 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
/*! Compute the absolute coordinates of the steps. */
double CurrentAbsTheta=0.0,c=0.0,s=0.0;
MAL_MATRIX_DIM(MM,double,2,2);
MAL_MATRIX_DIM(CurrentSupportFootPosition,double,3,3);
MAL_MATRIX_SET_IDENTITY(CurrentSupportFootPosition);
MAL_MATRIX_DIM(Orientation,double,2,2);
MAL_MATRIX_SET_IDENTITY(Orientation);
MAL_MATRIX_DIM(v,double,2,1);
MAL_MATRIX_DIM(v2,double,2,1);
Eigen::Matrix<double,2,2> MM;;
Eigen::Matrix<double,3,3> CurrentSupportFootPosition;;
CurrentSupportFootPosition.setIdentity();
Eigen::Matrix<double,2,2> Orientation;;
Orientation.setIdentity();
Eigen::Matrix<double,2,1> v;;
Eigen::Matrix<double,2,1> v2;;
if (m_DeltaTj.size()!=lNbOfIntervals)
m_DeltaTj.resize(lNbOfIntervals);
......@@ -380,9 +381,9 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
v(1,0) = RelativeFootPositions[i].sy;
// v(2,0) = RelativeFootPositions[i].sz;
/*! Compute the new orientation of the foot vector. */
Orientation = MAL_RET_A_by_B(MM , Orientation);
Orientation = MM*Orientation;
v2 = MAL_RET_A_by_B(Orientation, v);
v2 = Orientation*v;
/*! Update the world coordinates of the support foot. */
if ((!IgnoreFirst) || (i>0))
......@@ -482,10 +483,10 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
if ((i!=0)|| (Continuity))
{
{// verify auto collision
MAL_VECTOR_DIM(currSupp,double,2);
MAL_VECTOR_DIM(InitPos,double,2);
MAL_VECTOR_DIM(FinalPos,double,2);
MAL_VECTOR_DIM(relWayPoint,double,2);
Eigen::Matrix<double,2,1> currSupp;
Eigen::Matrix<double,2,1> InitPos;
Eigen::Matrix<double,2,1> FinalPos;
Eigen::Matrix<double,2,1> relWayPoint;
if (SupportFoot==1)
{
currSupp(0) = LeftFootTmpInitPos.x ;
......@@ -512,8 +513,8 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
double dx , dy , dc , distSquareToLine ;
dx = InitPos(1) - FinalPos(1) ;
dy = FinalPos(0) - InitPos(0) ;
MAL_VECTOR_RESIZE(m_MiddleWayPoint,2);
MAL_VECTOR_FILL(m_MiddleWayPoint,0.0);
m_MiddleWayPoint.resize(2);
{ for(unsigned int i=0;i<m_MiddleWayPoint.size();m_MiddleWayPoint[i++]=0.0);};
if ( dx*dx>=1e-6 || dy*dy>=1e-6 )// not moving implies no collision
{
dc = -(dx * InitPos(0) + dy *InitPos(1)) ;
......@@ -526,7 +527,7 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
bool autocollision = (x-x0)*(x-x0)+(y-y0)*(y-y0)<=R2;
if( autocollision )
{
m_MiddleWayPoint = MAL_RET_A_by_B(Orientation, relWayPoint) + currSupp ;
m_MiddleWayPoint = Orientation*relWayPoint + currSupp ;
}
}
}
......@@ -724,7 +725,7 @@ ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootP
RelativeFootPositions.size())
SupportFootAbsoluteFootPositions.resize(RelativeFootPositions.size());
unsigned int lNbOfIntervals = RelativeFootPositions.size();
long unsigned int lNbOfIntervals = RelativeFootPositions.size();
/*! It is assumed that a set of relative positions for the support foot
are given as an input. */
deque<FootAbsolutePosition> AbsoluteFootPositions;
......@@ -734,13 +735,13 @@ ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootP
/*! Compute the absolute coordinates of the steps. */
double CurrentAbsTheta=0.0,c=0.0,s=0.0;
MAL_MATRIX_DIM(MM,double,2,2);
MAL_MATRIX_DIM(CurrentSupportFootPosition,double,3,3);
MAL_MATRIX_SET_IDENTITY(CurrentSupportFootPosition);
MAL_MATRIX_DIM(Orientation,double,2,2);
MAL_MATRIX_SET_IDENTITY(Orientation);
MAL_MATRIX_DIM(v,double,2,1);
MAL_MATRIX_DIM(v2,double,2,1);
Eigen::Matrix<double,2,2> MM;;
Eigen::Matrix<double,3,3> CurrentSupportFootPosition;;
CurrentSupportFootPosition.setIdentity();
Eigen::Matrix<double,2,2> Orientation;;
Orientation.setIdentity();
Eigen::Matrix<double,2,1> v;;
Eigen::Matrix<double,2,1> v2;;
ODEBUG("Detect support foot on the right.");
CurrentAbsTheta = SupportFootInitialAbsolutePosition.theta;
......@@ -795,8 +796,8 @@ ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootP
v(1,0) = RelativeFootPositions[i].sy;
/*! Compute the new orientation of the foot vector. */
Orientation = MAL_RET_A_by_B(MM , Orientation);
v2 = MAL_RET_A_by_B(Orientation, v);
Orientation = MM*Orientation;
v2 = Orientation*v;
/*! Update the world coordinates of the support foot. */
for(int k=0;k<2;k++)
......@@ -838,12 +839,12 @@ ChangeRelStepsFromAbsSteps(deque<RelativeFootPosition> &RelativeFootPositions,
return;
}
MAL_S3x3_MATRIX(KM1,double);
MAL_S3x3_MATRIX_SET_IDENTITY(KM1);
MAL_S3x3_MATRIX(K,double);
MAL_S3x3_MATRIX_SET_IDENTITY(K);
MAL_S3x3_MATRIX(KP1,double);
MAL_S3x3_MATRIX_SET_IDENTITY(KP1);
Eigen::Matrix3d KM1;
KM1.setIdentity();
Eigen::Matrix3d K;
K.setIdentity();
Eigen::Matrix3d KP1;
KP1.setIdentity();
double thetakm1,xkm1,ykm1,c,s;
......@@ -878,11 +879,11 @@ ChangeRelStepsFromAbsSteps(deque<RelativeFootPosition> &RelativeFootPositions,
K(0,0) = c; K(0,1) = -s; K(0,2) = xk;
K(1,0) = s; K(1,1) = c; K(1,2) = yk;
MAL_S3x3_MATRIX(iKM1,double);
MAL_S3x3_INVERSE(KM1,iKM1,double);
MAL_S3x3_MATRIX(relMotionM1,double);
Eigen::Matrix3d iKM1;
iKM1=KM1.inverse();
Eigen::Matrix3d relMotionM1;
MAL_S3x3_C_eq_A_by_B(relMotionM1,iKM1,K);
relMotionM1=iKM1+K;
RelativeFootPositions[ChangedInterval].sx = relMotionM1(0,2);
RelativeFootPositions[ChangedInterval].sy = relMotionM1(1,2);
......@@ -902,11 +903,11 @@ ChangeRelStepsFromAbsSteps(deque<RelativeFootPosition> &RelativeFootPositions,
KP1(0,0) = c; KP1(0,1) = -s; KP1(0,2) = xkp1;
KP1(1,0) = s; KP1(1,1) = c; KP1(1,2) = ykp1;
MAL_S3x3_MATRIX(iK,double);
MAL_S3x3_INVERSE(K,iK,double);
MAL_S3x3_MATRIX(relMotionP1,double);
Eigen::Matrix3d iK;
iK=K.inverse();
Eigen::Matrix3d relMotionP1;
MAL_S3x3_C_eq_A_by_B(relMotionP1,iK,KP1);
relMotionP1=iK+KP1;
RelativeFootPositions[ChangedInterval+1].sx = relMotionP1(0,2);
RelativeFootPositions[ChangedInterval+1].sy = relMotionP1(1,2);
......
......@@ -30,8 +30,6 @@
#ifndef _LEFT_AND_RIGHT_FOOT_TRAJECTORY_GENERATION_MULTIPLE_H_
#define _LEFT_AND_RIGHT_FOOT_TRAJECTORY_GENERATION_MULTIPLE_H_
#include <jrl/mal/matrixabstractlayer.hh>
/* abstractRobotDynamics inclusion */
#include <jrl/walkgen/pinocchiorobot.hh>
......@@ -203,7 +201,7 @@ namespace PatternGeneratorJRL
double m_StepCurving;
/*! Middle Way Point of foot trajectories */
MAL_VECTOR(m_MiddleWayPoint, double) ;
Eigen::VectorXd m_MiddleWayPoint ;
/*! Single support time. */
double m_SingleSupportTime;
......
......@@ -55,11 +55,11 @@ int CoMAndFootOnlyStrategy::InitInterObjects(PinocchioRobot * /* aPR */,
int CoMAndFootOnlyStrategy::OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition,
MAL_VECTOR_TYPE(double) & ZMPRefPos,
Eigen::VectorXd & ZMPRefPos,
COMState & finalCOMPosition,
MAL_VECTOR_TYPE(double) & ,//CurrentConfiguration,
MAL_VECTOR_TYPE(double) & ,//CurrentVelocity,
MAL_VECTOR_TYPE(double) & )//CurrentAcceleration)
Eigen::VectorXd & ,//CurrentConfiguration,
Eigen::VectorXd & ,//CurrentVelocity,
Eigen::VectorXd & )//CurrentAcceleration)
{
ODEBUG("Begin OneGlobalStepOfControl "
<< m_LeftFootPositions->size() << " "
......@@ -124,14 +124,14 @@ int CoMAndFootOnlyStrategy::OneGlobalStepOfControl(FootAbsolutePosition &LeftFoo
}
int CoMAndFootOnlyStrategy::EvaluateStartingState(MAL_VECTOR(&,double) BodyAngles,
int CoMAndFootOnlyStrategy::EvaluateStartingState(Eigen::VectorXd & BodyAngles,
COMState & aStartingCOMState,
MAL_S3_VECTOR(&,double) aStartingZMPPosition,
MAL_VECTOR(&,double) lStartingWaistPose,
Eigen::Vector3d & aStartingZMPPosition,
Eigen::VectorXd & lStartingWaistPose,
FootAbsolutePosition & InitLeftFootPosition,
FootAbsolutePosition & InitRightFootPosition)
{
MAL_S3_VECTOR(lStartingCOMState,double);
Eigen::Vector3d lStartingCOMState;
lStartingCOMState(0) = aStartingCOMState.x[0];
lStartingCOMState(1) = aStartingCOMState.y[0];
......
......@@ -69,11 +69,11 @@ namespace PatternGeneratorJRL
*/
int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition,
MAL_VECTOR_TYPE(double) & ZMPRefPos,
Eigen::VectorXd & ZMPRefPos,
COMState & finalCOMState,
MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration);
Eigen::VectorXd & CurrentConfiguration,
Eigen::VectorXd & CurrentVelocity,
Eigen::VectorXd & CurrentAcceleration);
......@@ -92,10 +92,10 @@ namespace PatternGeneratorJRL
@param[out] InitRightFootPosition: Returns the position of the right foot
in the waist coordinates frame.
*/
int EvaluateStartingState(MAL_VECTOR( &,double) BodyAngles,
int EvaluateStartingState(Eigen::VectorXd & BodyAngles,
COMState & aStartingCOMState,
MAL_S3_VECTOR(& ,double) aStartingZMPPosition,
MAL_VECTOR(& ,double) aStartingWaistPose,
Eigen::Vector3d & aStartingZMPPosition,
Eigen::VectorXd & aStartingWaistPose,
FootAbsolutePosition & InitLeftFootPosition,
FootAbsolutePosition & InitRightFootPosition);
......
......@@ -83,11 +83,11 @@ int DoubleStagePreviewControlStrategy::InitInterObjects(PinocchioRobot *aPR,
int DoubleStagePreviewControlStrategy::OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition,
MAL_VECTOR_TYPE(double) & ZMPRefPos,
Eigen::VectorXd & ZMPRefPos,
COMState & finalCOMState,
MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration)
Eigen::VectorXd & CurrentConfiguration,
Eigen::VectorXd & CurrentVelocity,
Eigen::VectorXd & CurrentAcceleration)
{
// New scheme:
// Update the queue of ZMP ref
......@@ -129,7 +129,7 @@ int DoubleStagePreviewControlStrategy::OneGlobalStepOfControl(FootAbsolutePositi
RightFootPosition = (*m_RightFootPositions)[0];
// Compute the waist position in the current motion global reference frame.
MAL_S4x4_MATRIX( PosOfWaistInCOMF,double);
Eigen::Matrix4d PosOfWaistInCOMF;
PosOfWaistInCOMF = m_ZMPpcwmbz->GetCurrentPositionofWaistInCOMFrame();
COMState outWaistPosition;
......@@ -155,7 +155,7 @@ int DoubleStagePreviewControlStrategy::OneGlobalStepOfControl(FootAbsolutePositi
ZMPRefPos(0) = cos(temp3)*temp1+sin(temp3)*temp2 ;
ZMPRefPos(1) = -sin(temp3)*temp1+cos(temp3)*temp2;
ZMPRefPos(2) = -finalCOMState.z[0] - MAL_S4x4_MATRIX_ACCESS_I_J(PosOfWaistInCOMF, 2,3) - (*m_ZMPPositions)[0].pz;
ZMPRefPos(2) = -finalCOMState.z[0] - PosOfWaistInCOMF(2,3) - (*m_ZMPPositions)[0].pz;
}
else if (m_ZMPFrame==ZMPFRAME_WORLD)
{
......@@ -190,14 +190,14 @@ int DoubleStagePreviewControlStrategy::OneGlobalStepOfControl(FootAbsolutePositi
return 0;
}
int DoubleStagePreviewControlStrategy::EvaluateStartingState(MAL_VECTOR( &,double) BodyAngles,
int DoubleStagePreviewControlStrategy::EvaluateStartingState(Eigen::VectorXd & BodyAngles,
COMState & aStartingCOMState,
MAL_S3_VECTOR(&,double) aStartingZMPPosition,
MAL_VECTOR(&,double) aStartingWaistPose,
Eigen::Vector3d & aStartingZMPPosition,
Eigen::VectorXd & aStartingWaistPose,
FootAbsolutePosition & InitLeftFootPosition,
FootAbsolutePosition & InitRightFootPosition)
{
MAL_S3_VECTOR(lStartingCOMState,double);
Eigen::Vector3d lStartingCOMState;
lStartingCOMState(0) = aStartingCOMState.x[0];
lStartingCOMState(1) = aStartingCOMState.y[0];
lStartingCOMState(2) = aStartingCOMState.z[0];
......
......@@ -64,11 +64,11 @@ namespace PatternGeneratorJRL
*/
int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition,
MAL_VECTOR_TYPE(double) & ZMPRefPos,
Eigen::VectorXd & ZMPRefPos,
COMState & COMState,
MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration);
Eigen::VectorXd & CurrentConfiguration,
Eigen::VectorXd & CurrentVelocity,
Eigen::VectorXd & CurrentAcceleration);
......@@ -87,10 +87,10 @@ namespace PatternGeneratorJRL
@param[out] InitRightFootPosition: Returns the position of the right foot
in the waist coordinates frame.
*/
int EvaluateStartingState(MAL_VECTOR( &,double) BodyAngles,
int EvaluateStartingState(Eigen::VectorXd & BodyAngles,
COMState & aStartingCOMState,
MAL_S3_VECTOR(& ,double) aStartingZMPPosition,
MAL_VECTOR(& ,double) aStartingWaistPose,
Eigen::Vector3d & aStartingZMPPosition,
Eigen::VectorXd & aStartingWaistPose,
FootAbsolutePosition & InitLeftFootPosition,
FootAbsolutePosition & InitRightFootPosition);
......
......@@ -29,9 +29,6 @@
/*! JRL inclusion */
// MAL
#include <jrl/mal/matrixabstractlayer.hh>
// Dynamics
#include <jrl/walkgen/pinocchiorobot.hh>
......@@ -85,11 +82,11 @@ namespace PatternGeneratorJRL
*/
virtual int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition,
MAL_VECTOR_TYPE(double) & ZMPRefPos,
Eigen::VectorXd & ZMPRefPos,
COMState & aCOMState,
MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration)=0;
Eigen::VectorXd & CurrentConfiguration,
Eigen::VectorXd & CurrentVelocity,
Eigen::VectorXd & CurrentAcceleration)=0;
......@@ -108,10 +105,10 @@ namespace PatternGeneratorJRL
@param[out] InitRightFootPosition: Returns the position of the right foot
in the waist coordinates frame.
*/
virtual int EvaluateStartingState(MAL_VECTOR( &,double) BodyAngles,
virtual int EvaluateStartingState(Eigen::VectorXd & BodyAngles,
COMState & aStartingCOMState,
MAL_S3_VECTOR(& ,double) aStartingZMPPosition,
MAL_VECTOR(& ,double) aStartingWaistPose,
Eigen::Vector3d & aStartingZMPPosition,
Eigen::VectorXd & aStartingWaistPose,
FootAbsolutePosition & InitLeftFootPosition,
FootAbsolutePosition & InitRightFootPosition)=0;
......
......@@ -52,14 +52,14 @@ FootConstraintsAsLinearSystem::~FootConstraintsAsLinearSystem()
{
}
int FootConstraintsAsLinearSystem::FindSimilarConstraints(MAL_MATRIX(&A,double),
int FootConstraintsAsLinearSystem::FindSimilarConstraints(Eigen::MatrixXd &A,
vector<int> &SimilarConstraints)
{
SimilarConstraints.resize(MAL_MATRIX_NB_ROWS(A));