From 4c4f4e1cde8e59959625759c5700c1d511568cc4 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Tue, 23 Oct 2018 21:03:41 +0200
Subject: [PATCH] Moving to Eigen.
---
.../ComAndFootRealizationByGeometry.hh | 156 ++++++++--------
src/MotionGeneration/UpperBodyMotion.hh | 6 +-
src/MotionGeneration/WaistHeightVariation.hh | 98 +++++-----
.../AnalyticalMorisawaCompact.cpp | 169 +++++++++---------
.../AnalyticalMorisawaCompact.hh | 10 +-
.../DynamicFilter.hh | 70 ++++----
...ngAnalyticalTrajectoryByPreviewControl.cpp | 2 +-
...ingAnalyticalTrajectoryByPreviewControl.hh | 2 +-
.../ZMPVelocityReferencedQP.cpp | 8 +-
.../ZMPVelocityReferencedQP.hh | 6 +-
10 files changed, 267 insertions(+), 260 deletions(-)
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.hh b/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
index 5a182490..9d30bd44 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
@@ -92,14 +92,14 @@ namespace PatternGeneratorJRL
last stage, we store some information.
*/
- bool ComputePostureForGivenCoMAndFeetPosture(MAL_VECTOR_TYPE(double) &CoMPosition,
- MAL_VECTOR_TYPE(double) & aCoMSpeed,
- MAL_VECTOR_TYPE(double) & aCoMAcc,
- MAL_VECTOR_TYPE(double) &LeftFoot,
- MAL_VECTOR_TYPE(double) &RightFoot,
- MAL_VECTOR_TYPE(double) & CurrentConfiguration,
- MAL_VECTOR_TYPE(double) & CurrentVelocity,
- MAL_VECTOR_TYPE(double) & CurrentAcceleration,
+ bool ComputePostureForGivenCoMAndFeetPosture(Eigen::VectorXd &CoMPosition,
+ Eigen::VectorXd & aCoMSpeed,
+ Eigen::VectorXd & aCoMAcc,
+ Eigen::VectorXd &LeftFoot,
+ Eigen::VectorXd &RightFoot,
+ Eigen::VectorXd & CurrentConfiguration,
+ Eigen::VectorXd & CurrentVelocity,
+ Eigen::VectorXd & CurrentAcceleration,
int IterationNumber,
int Stage);
@@ -113,8 +113,8 @@ namespace PatternGeneratorJRL
\param[in] BodyAnglesIni: The configuration vector provided by the user.
\param[out] lStartingWaistPose: The waist pose according to the user configuration vector.
*/
- bool InitializationHumanoid(MAL_VECTOR_TYPE(double) &BodyAnglesIni,
- MAL_VECTOR_TYPE(double) &lStartingWaistPose);
+ bool InitializationHumanoid(Eigen::VectorXd &BodyAnglesIni,
+ Eigen::VectorXd &lStartingWaistPose);
/*! \brief Initialize the foot position.
\param[in] aFoot: Pointer to the foot to be updated.
@@ -123,7 +123,7 @@ namespace PatternGeneratorJRL
free flyer (set to 0.0 0.0 0.0)
*/
bool InitializationFoot(PRFoot * aFoot,
- MAL_S3_VECTOR(& m_AnklePosition,double),
+ Eigen::Vector3d &m_AnklePosition,
FootAbsolutePosition & InitFootPosition);
/*! This initialization phase does the following:
@@ -135,9 +135,9 @@ namespace PatternGeneratorJRL
IMPORTANT: The jrlHumanoidDynamicRobot must have been properly set up.
*/
- bool InitializationCoM(MAL_VECTOR_TYPE(double) &BodyAnglesIni,
- MAL_S3_VECTOR_TYPE(double) & lStartingCOMPosition,
- MAL_VECTOR_TYPE(double) & lStartingWaistPosition,
+ bool InitializationCoM(Eigen::VectorXd &BodyAnglesIni,
+ Eigen::Vector3d & lStartingCOMPosition,
+ Eigen::VectorXd & lStartingWaistPosition,
FootAbsolutePosition & InitLeftFootAbsPos,
FootAbsolutePosition & InitRightFootAbsPos);
@@ -157,9 +157,9 @@ namespace PatternGeneratorJRL
Assuming that the waist is at (0,0,0)
It returns the associate initial values for the left and right foot.
*/
- int EvaluateCOMForStartingPosition(MAL_VECTOR( &BodyAngles,double),
+ int EvaluateCOMForStartingPosition(Eigen::VectorXd &BodyAngles,
double omega, double theta,
- MAL_S3_VECTOR( &lCOMPosition,double),
+ Eigen::Vector3d &lCOMPosition,
FootAbsolutePosition & LeftFootPosition,
FootAbsolutePosition & RightFootPosition);
@@ -167,29 +167,29 @@ namespace PatternGeneratorJRL
Assuming that the waist is at (0,0,0)
It returns the associate initial values for the left and right foot.*/
- int EvaluateStartingCoM(MAL_VECTOR_TYPE(double) &BodyAngles,
- MAL_S3_VECTOR_TYPE(double) & aStartingCOMPosition,
+ int EvaluateStartingCoM(Eigen::VectorXd &BodyAngles,
+ Eigen::Vector3d & aStartingCOMPosition,
FootAbsolutePosition & InitLeftFootPosition,
FootAbsolutePosition & InitRightFootPosition);
- int EvaluateStartingCoM(MAL_VECTOR(&BodyAngles,double),
- MAL_S3_VECTOR(&aStartingCOMPosition,double),
- MAL_VECTOR(&aWaistPose,double),
+ int EvaluateStartingCoM(Eigen::VectorXd &BodyAngles,
+ Eigen::Vector3d &aStartingCOMPosition,
+ Eigen::VectorXd &aWaistPose,
FootAbsolutePosition & InitLeftFootPosition,
FootAbsolutePosition & InitRightFootPosition);
/*! Method to compute the heuristic for the arms. */
- void ComputeUpperBodyHeuristicForNormalWalking(MAL_VECTOR_TYPE(double) & qArmr,
- MAL_VECTOR_TYPE(double) & qArml,
- MAL_VECTOR_TYPE(double) & aCOMPosition,
- MAL_VECTOR_TYPE(double) & RFP,
- MAL_VECTOR_TYPE(double) & LFP);
+ void ComputeUpperBodyHeuristicForNormalWalking(Eigen::VectorXd & qArmr,
+ Eigen::VectorXd & qArml,
+ Eigen::VectorXd & aCOMPosition,
+ Eigen::VectorXd & RFP,
+ Eigen::VectorXd & LFP);
/*! This method returns the final COM pose matrix after the second stage of control. */
- MAL_MATRIX_TYPE(double) GetFinalDesiredCOMPose();
+ Eigen::MatrixXd GetFinalDesiredCOMPose();
/*! Returns the position of the Waist in the the COM Frame . */
- void GetCurrentPositionofWaistInCOMFrame(MAL_VECTOR_TYPE(double) & CurPosWICF_homogeneous);
+ void GetCurrentPositionofWaistInCOMFrame(Eigen::VectorXd & CurPosWICF_homogeneous);
/*! Reimplementation of the setter of the HumanoidDynamicRobot. */
@@ -206,14 +206,14 @@ namespace PatternGeneratorJRL
@param[in] LeftOrRight: -1 for the right leg, 1 for the left.
@param[out] lq : Values of the leg which realize the position asked for.
*/
- bool KinematicsForOneLeg(MAL_S3x3_MATRIX_TYPE(double) & Body_R,
- MAL_S3_VECTOR_TYPE(double) & Body_P,
- MAL_VECTOR_TYPE(double) &aFoot,
- MAL_S3_VECTOR_TYPE(double) &lDt,
- MAL_VECTOR_TYPE(double) &aCoMPosition,
- MAL_S3_VECTOR_TYPE(double) &ToTheHip,
+ bool KinematicsForOneLeg(Eigen::Matrix3d & Body_R,
+ Eigen::Vector3d & Body_P,
+ Eigen::VectorXd &aFoot,
+ Eigen::Vector3d &lDt,
+ Eigen::VectorXd &aCoMPosition,
+ Eigen::Vector3d &ToTheHip,
int LeftOrRight,
- MAL_VECTOR_TYPE(double) &lq,
+ Eigen::VectorXd &lq,
int Stage);
/*! Compute the angles values considering two 6DOF legs for a given configuration
@@ -226,13 +226,13 @@ namespace PatternGeneratorJRL
@param qr: Angles for the right leg to achieve the positions.
@param AbsoluteWaistPosition: The waist position.
*/
- bool KinematicsForTheLegs(MAL_VECTOR_TYPE(double) & aCoMPosition,
- MAL_VECTOR_TYPE(double) & aLeftFoot,
- MAL_VECTOR_TYPE(double) & aRightFoot,
+ bool KinematicsForTheLegs(Eigen::VectorXd & aCoMPosition,
+ Eigen::VectorXd & aLeftFoot,
+ Eigen::VectorXd & aRightFoot,
int Stage,
- MAL_VECTOR_TYPE(double) & ql,
- MAL_VECTOR_TYPE(double) & qr,
- MAL_S3_VECTOR_TYPE(double) & AbsoluteWaistPosition);
+ Eigen::VectorXd & ql,
+ Eigen::VectorXd & qr,
+ Eigen::Vector3d & AbsoluteWaistPosition);
/*! \brief Implement the Plugin part to receive information from
PatternGeneratorInterface.
@@ -243,51 +243,51 @@ namespace PatternGeneratorJRL
@return a 4x4 matrix which contains the pose and the position of the waist
in the CoM reference frame.
*/
- MAL_S4x4_MATRIX_TYPE(double) GetCurrentPositionofWaistInCOMFrame();
+ Eigen::Matrix4d GetCurrentPositionofWaistInCOMFrame();
/*! \brief Getter and setter for the previous configurations and velocities */
- inline void SetPreviousConfigurationStage0(MAL_VECTOR_TYPE(double) & prev_Configuration)
+ inline void SetPreviousConfigurationStage0(Eigen::VectorXd & prev_Configuration)
{ m_prev_Configuration = prev_Configuration ;}
- inline void SetPreviousVelocityStage0(MAL_VECTOR_TYPE(double) & prev_Velocity)
+ inline void SetPreviousVelocityStage0(Eigen::VectorXd & prev_Velocity)
{ m_prev_Velocity = prev_Velocity ;}
- inline void SetPreviousConfigurationStage1(MAL_VECTOR_TYPE(double) & prev_Configuration)
+ inline void SetPreviousConfigurationStage1(Eigen::VectorXd & prev_Configuration)
{ m_prev_Configuration1 = prev_Configuration ;}
- inline void SetPreviousVelocityStage1(MAL_VECTOR_TYPE(double) & prev_Velocity)
+ inline void SetPreviousVelocityStage1(Eigen::VectorXd & prev_Velocity)
{ m_prev_Velocity1 = prev_Velocity ;}
- inline void SetPreviousConfigurationStage2(MAL_VECTOR_TYPE(double) & prev_Configuration)
+ inline void SetPreviousConfigurationStage2(Eigen::VectorXd & prev_Configuration)
{ m_prev_Configuration2 = prev_Configuration ;}
- inline void SetPreviousVelocityStage2(MAL_VECTOR_TYPE(double) & prev_Velocity)
+ inline void SetPreviousVelocityStage2(Eigen::VectorXd & prev_Velocity)
{ m_prev_Velocity2 = prev_Velocity ;}
/*! \brief Getter and setter for the previous configurations and velocities */
- inline void SetPreviousConfigurationStage0(const MAL_VECTOR_TYPE(double) & prev_Configuration)
+ inline void SetPreviousConfigurationStage0(const Eigen::VectorXd & prev_Configuration)
{ m_prev_Configuration = prev_Configuration ;}
- inline void SetPreviousVelocityStage0(const MAL_VECTOR_TYPE(double) & prev_Velocity)
+ inline void SetPreviousVelocityStage0(const Eigen::VectorXd & prev_Velocity)
{ m_prev_Velocity = prev_Velocity ;}
- inline void SetPreviousConfigurationStage1(const MAL_VECTOR_TYPE(double) & prev_Configuration)
+ inline void SetPreviousConfigurationStage1(const Eigen::VectorXd & prev_Configuration)
{ m_prev_Configuration1 = prev_Configuration ;}
- inline void SetPreviousVelocityStage1(const MAL_VECTOR_TYPE(double) & prev_Velocity)
+ inline void SetPreviousVelocityStage1(const Eigen::VectorXd & prev_Velocity)
{ m_prev_Velocity1 = prev_Velocity ;}
- inline void SetPreviousConfigurationStage2(const MAL_VECTOR_TYPE(double) & prev_Configuration)
+ inline void SetPreviousConfigurationStage2(const Eigen::VectorXd & prev_Configuration)
{ m_prev_Configuration2 = prev_Configuration ;}
- inline void SetPreviousVelocityStage2(const MAL_VECTOR_TYPE(double) & prev_Velocity)
+ inline void SetPreviousVelocityStage2(const Eigen::VectorXd & prev_Velocity)
{ m_prev_Velocity2 = prev_Velocity;}
/*! \brief Getter and setter for the previous configurations and velocities */
- inline MAL_VECTOR_TYPE(double) & GetPreviousConfigurationStage0()
+ inline Eigen::VectorXd & GetPreviousConfigurationStage0()
{ return m_prev_Configuration ;};
- inline MAL_VECTOR_TYPE(double) & GetPreviousConfigurationStage1()
+ inline Eigen::VectorXd & GetPreviousConfigurationStage1()
{ return m_prev_Configuration1 ;};
- inline MAL_VECTOR_TYPE(double) & GetPreviousVelocityStage0()
+ inline Eigen::VectorXd & GetPreviousVelocityStage0()
{ return m_prev_Velocity ;};
- inline MAL_VECTOR_TYPE(double) & GetPreviousVelocityStage1()
+ inline Eigen::VectorXd & GetPreviousVelocityStage1()
{ return m_prev_Velocity1 ;};
inline void leftLegIndexinConfiguration(std::vector<int> & leftLegMaps) const
@@ -318,7 +318,7 @@ namespace PatternGeneratorJRL
{ShiftFoot_=ShiftFoot ;}
/*! \brief Get the COG of the ankles at the starting position. */
- virtual MAL_S3_VECTOR_TYPE(double) GetCOGInitialAnkles();
+ virtual Eigen::Vector3d GetCOGInitialAnkles();
friend ostream& operator <<(ostream &os,const ComAndFootRealization &obj);
@@ -373,21 +373,21 @@ namespace PatternGeneratorJRL
/*! \brief Displacement between the hip and the foot. @{*/
/*! \brief For the right foot. */
- MAL_S3_VECTOR(m_DtRight,double);
+ Eigen::Vector3d m_DtRight;
/*! \brief For the left foot. */
- MAL_S3_VECTOR(m_DtLeft,double);
+ Eigen::Vector3d m_DtLeft;
/*! @} */
/*! \name Vector from the Waist to the left and right hip. */
/*! Static part from the waist to the left hip.. */
- MAL_S3_VECTOR(m_StaticToTheLeftHip,double);
+ Eigen::Vector3d m_StaticToTheLeftHip;
/*! Static part from the waist to the right hip. */
- MAL_S3_VECTOR(m_StaticToTheRightHip,double);
+ Eigen::Vector3d m_StaticToTheRightHip;
/*! Dynamic part from the waist to the left hip. */
- MAL_S3_VECTOR(m_TranslationToTheLeftHip,double);
+ Eigen::Vector3d m_TranslationToTheLeftHip;
/*! Dynamic part form the waist to the right hip. */
- MAL_S3_VECTOR( m_TranslationToTheRightHip,double);
+ Eigen::Vector3d m_TranslationToTheRightHip;
/*! @} */
@@ -395,45 +395,45 @@ namespace PatternGeneratorJRL
/*! \name Previous joint values. */
//@{
/*! \brief For the speed (stage 0). */
- MAL_VECTOR(m_prev_Configuration,double);
+ Eigen::VectorXd m_prev_Configuration;
/*! \brief For the speed (stage 1). */
- MAL_VECTOR( m_prev_Configuration1,double);
+ Eigen::VectorXd m_prev_Configuration1;
/*! \brief For the speed (stage 1). */
- MAL_VECTOR( m_prev_Configuration2,double);
+ Eigen::VectorXd m_prev_Configuration2;
/*! \brief For the speed (stage 0). */
- MAL_VECTOR(m_prev_Velocity,double);
+ Eigen::VectorXd m_prev_Velocity;
/*! \brief For the speed (stage 1). */
- MAL_VECTOR( m_prev_Velocity1,double);
+ Eigen::VectorXd m_prev_Velocity1;
/*! \brief For the speed (stage 1). */
- MAL_VECTOR( m_prev_Velocity2,double);
+ Eigen::VectorXd m_prev_Velocity2;
//@}
/*! COM Starting position. */
- MAL_S3_VECTOR(m_StartingCOMPosition,double);
+ Eigen::Vector3d m_StartingCOMPosition;
/*! Final COM pose. */
- MAL_S4x4_MATRIX(m_FinalDesiredCOMPose,double);
+ Eigen::Matrix4d m_FinalDesiredCOMPose;
/*! Store the position of the ankle in the right feet. */
- MAL_S3_VECTOR(m_AnklePositionRight,double);
+ Eigen::Vector3d m_AnklePositionRight;
/*! Store the position of the ankle in the left feet. */
- MAL_S3_VECTOR(m_AnklePositionLeft,double);
+ Eigen::Vector3d m_AnklePositionLeft;
/*! Difference between the CoM and the Waist
from the initialization phase,
i.e. not reevaluated while walking. */
- MAL_S3_VECTOR(m_DiffBetweenComAndWaist,double);
+ Eigen::Vector3d m_DiffBetweenComAndWaist;
/*! Difference between the CoM and the Waist
in the CoM reference frame. */
- MAL_S3_VECTOR(m_ComAndWaistInRefFrame,double);
+ Eigen::Vector3d m_ComAndWaistInRefFrame;
/*! Maximal distance along the X axis for the hand motion */
@@ -486,7 +486,7 @@ namespace PatternGeneratorJRL
/*! COG of the ankles in the waist reference frame
when evaluating the initial position.
*/
- MAL_S3_VECTOR_TYPE(double) m_COGInitialAnkles;
+ Eigen::Vector3d m_COGInitialAnkles;
/*! Store the position of the left and right shoulders. */
se3::JointIndex m_LeftShoulder, m_RightShoulder;
diff --git a/src/MotionGeneration/UpperBodyMotion.hh b/src/MotionGeneration/UpperBodyMotion.hh
index b78d2fd3..c3195664 100644
--- a/src/MotionGeneration/UpperBodyMotion.hh
+++ b/src/MotionGeneration/UpperBodyMotion.hh
@@ -30,6 +30,8 @@
#ifndef _UPPER_BODY_MOTION_
#define _UPPER_BODY_MOTION_
+
+
#include <vector>
#include <string>
//#define FULL_POLYNOME
@@ -56,9 +58,9 @@ namespace PatternGeneratorJRL
void GenerateDataFile(string aFileName, int LenghtDataArray);
- void ReadDataFile(string aFileName, MAL_MATRIX(&UpperBodyAngles,double));
+ void ReadDataFile(string aFileName, Eigen::MatrixXd &UpperBodyAngles);
- void WriteDataFile(string aFileName, MAL_MATRIX(&UpperBodyAngles,double));
+ void WriteDataFile(string aFileName, Eigen::MatrixXd &UpperBodyAngles);
protected:
diff --git a/src/MotionGeneration/WaistHeightVariation.hh b/src/MotionGeneration/WaistHeightVariation.hh
index 4fa06d1a..c733ead7 100644
--- a/src/MotionGeneration/WaistHeightVariation.hh
+++ b/src/MotionGeneration/WaistHeightVariation.hh
@@ -27,18 +27,18 @@
/** \file WaistHeightVariation.h
\brief This object generate all the values for the foot trajectories,
- and the desired ZMP based on a sequence of relative steps.
- If you want to change the reference trajectories, and the planning
- of the foot, this is the object to modify.
+ and the desired ZMP based on a sequence of relative steps.
+ If you want to change the reference trajectories, and the planning
+ of the foot, this is the object to modify.
- Copyright (c) 2005-2009,
- @author Olivier Stasse,Ramzi Sellouati, Francois Keith,
+ Copyright (c) 2005-2009,
+ @author Olivier Stasse,Ramzi Sellouati, Francois Keith,
- JRL-Japan, CNRS/AIST
+ JRL-Japan, CNRS/AIST
- All rights reserved.
+ All rights reserved.
- Please see License.txt for further information on license.
+ Please see License.txt for further information on license.
*/
#ifndef _WAISTHEIGHT_VARIATION_H_
@@ -59,21 +59,21 @@
namespace PatternGeneratorJRL
{
-class WaistPolynome : public Polynome
- {
- public:
- /// Constructor:
- /// boundCond: the different boundary conditions begin, intermediate and end of polynomial
- /// timeDistr: vector with time instants for intermediate boundary conditions and end time
+ class WaistPolynome : public Polynome
+ {
+ public:
+ /// Constructor:
+ /// boundCond: the different boundary conditions begin, intermediate and end of polynomial
+ /// timeDistr: vector with time instants for intermediate boundary conditions and end time
- WaistPolynome();
+ WaistPolynome();
- /// Set the parameters
- void SetParameters(MAL_VECTOR( boundCond,double), std::vector<double> timeDistr);
+ /// Set the parameters
+ void SetParameters( Eigen::MatrixXd boundCond, std::vector<double> timeDistr);
- /// Destructor.
- ~WaistPolynome();
- };
+ /// Destructor.
+ ~WaistPolynome();
+ };
@@ -81,48 +81,48 @@ class WaistPolynome : public Polynome
/// Object to compute new foot trajectories for the height of the waist with waist differnces as input for each step
class WaistHeightVariation
- {
- public :
+ {
+ public :
- /// Constructor
- WaistHeightVariation();
+ /// Constructor
+ WaistHeightVariation();
- /// Destructor
- ~WaistHeightVariation();
+ /// Destructor
+ ~WaistHeightVariation();
- ///call for polynomial planning of both steps during the obstacle stepover
- void PolyPlanner(deque<COMPosition> &aCOMBuffer,
- deque<RelativeFootPosition> &aFootHolds,
- deque<ZMPPosition> aZMPPosition);
+ ///call for polynomial planning of both steps during the obstacle stepover
+ void PolyPlanner(deque<COMPosition> &aCOMBuffer,
+ deque<RelativeFootPosition> &aFootHolds,
+ deque<ZMPPosition> aZMPPosition);
- protected:
+ protected:
- deque<RelativeFootPosition> m_FootHolds;
+ deque<RelativeFootPosition> m_FootHolds;
- MAL_MATRIX(mBoundCond,double);
- std::vector<double> mTimeDistr;
+ Eigen::MatrixXd mBoundCond;
+ std::vector<double> mTimeDistr;
- WaistPolynome *m_PolynomeHip;
+ WaistPolynome *m_PolynomeHip;
- ///extra COMPosition buffer calculated in ZMPMultibody class
- std::vector<COMPosition> m_ExtraCOMBuffer;
+ ///extra COMPosition buffer calculated in ZMPMultibody class
+ std::vector<COMPosition> m_ExtraCOMBuffer;
- /// buffers for first preview
- deque<COMPosition> m_COMBuffer;
- unsigned int m_ExtraBufferLength;
- double m_ModulationSupportCoefficient;
- float m_Tsingle,m_TsingleStepOver;
- float m_Tdble;
- double m_DiffBetweenComAndWaist;
- /// Sampling Period.
- double m_SamplingPeriod;
- /// Starting a new step sequences.
- bool m_StartingNewSequence;
-};
+ /// buffers for first preview
+ deque<COMPosition> m_COMBuffer;
+ unsigned int m_ExtraBufferLength;
+ double m_ModulationSupportCoefficient;
+ float m_Tsingle,m_TsingleStepOver;
+ float m_Tdble;
+ double m_DiffBetweenComAndWaist;
+ /// Sampling Period.
+ double m_SamplingPeriod;
+ /// Starting a new step sequences.
+ bool m_StartingNewSequence;
+ };
}
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index 1b383ed4..ff4069c5 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -35,6 +35,9 @@ ICRA 2007, 3989--39994
#include <fstream>
#include <ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh>
#include <iomanip>
+
+#include <boost/version.hpp>
+
typedef double doublereal;
typedef int integer;
@@ -59,6 +62,7 @@ extern "C"
integer *, /* 19 IWORK */
integer * /* 20 INFO */
);
+
#if BOOST_VERSION >=104000
extern void dgetrf_( integer * m, /* M */
integer * n, /* N */
@@ -232,11 +236,11 @@ namespace PatternGeneratorJRL
void AnalyticalMorisawaCompact::ComputePolynomialWeights()
{
- MAL_MATRIX_TYPE(double) iZ;
- MAL_INVERSE(m_Z,iZ,double);
+ Eigen::MatrixXd iZ;
+ iZ=m_Z.inverse();
// Compute the weights.
- MAL_C_eq_A_by_B(m_y,iZ,m_w);
+ m_y=iZ+m_w;
if (m_VerboseLevel>=2)
{
@@ -244,7 +248,7 @@ namespace PatternGeneratorJRL
ofs.open("YMatrix.dat",ofstream::out);
ofs.precision(10);
- for(unsigned int i=0;i<MAL_VECTOR_SIZE(m_y);i++)
+ for(unsigned int i=0;i<m_y.size();i++)
{
ofs << m_y[i]<< " ";
}
@@ -256,46 +260,47 @@ namespace PatternGeneratorJRL
void AnalyticalMorisawaCompact::ResetTheResolutionOfThePolynomial()
{
- int SizeOfZ = MAL_MATRIX_NB_ROWS(m_Z);
+ long int SizeOfZ = m_Z.rows();
- MAL_MATRIX_RESIZE(m_AF,SizeOfZ,2*SizeOfZ);
- MAL_VECTOR_RESIZE(m_IPIV,SizeOfZ);
+ m_AF.resize(SizeOfZ,2*SizeOfZ);
+ m_IPIV.resize(SizeOfZ);
- MAL_MATRIX_FILL(m_AF,0);
- MAL_VECTOR_FILL(m_IPIV,0);
+ {for(unsigned int i=0;i<m_AF.rows();i++) for(unsigned int j=0;j<m_AF.cols();j++)
+ m_AF(i,j)=0;};
+ { for(unsigned int i=0;i<m_IPIV.size();m_IPIV[i++]=0);};
m_NeedToReset = true;
}
void AnalyticalMorisawaCompact::ComputePolynomialWeights2()
{
- int SizeOfZ = MAL_MATRIX_NB_ROWS(m_Z),
+ int SizeOfZ = (int)m_Z.rows(),
LDA,LDAF,LDB;
int NRHS = 1;
char EQUED='N';
- MAL_MATRIX_TYPE(double) tZ;
- tZ = MAL_RET_TRANSPOSE(m_Z);
+ Eigen::MatrixXd tZ;
+ tZ = m_Z.transpose();
- MAL_VECTOR_TYPE(double) lR;
- MAL_VECTOR_RESIZE(lR,SizeOfZ);
- MAL_VECTOR_TYPE(double) lC;
- MAL_VECTOR_RESIZE( lC,SizeOfZ);
+ Eigen::VectorXd lR;
+ lR.resize(SizeOfZ);
+ Eigen::VectorXd lC;
+ lC.resize(SizeOfZ);
- MAL_VECTOR_RESIZE(m_y,SizeOfZ);
- //MAL_VECTOR_TYPE(double) m_X;
- //b MAL_VECTOR_RESIZE(m_X,SizeOfZ);
+ m_y.resize(SizeOfZ);
+ //Eigen::VectorXd m_X;
+ //b m_X.resize(SizeOfZ);
LDA = SizeOfZ;
LDAF = SizeOfZ;
LDB = SizeOfZ;
double lRCOND;
- MAL_VECTOR_TYPE(double) lFERR, lBERR;
- MAL_VECTOR_RESIZE(lFERR,SizeOfZ);
- MAL_VECTOR_RESIZE(lBERR,SizeOfZ);
+ Eigen::VectorXd lFERR, lBERR;
+ lFERR.resize(SizeOfZ);
+ lBERR.resize(SizeOfZ);
int lwork = 4* SizeOfZ;
double *work = new double[lwork];
@@ -305,12 +310,12 @@ namespace PatternGeneratorJRL
if (m_NeedToReset)
{
- m_AF = MAL_RET_TRANSPOSE(m_Z);
+ m_AF = m_Z.transpose();
dgetrf_(&SizeOfZ, /* M */
&SizeOfZ, /* N here M=N=SizeOfZ */
- MAL_RET_MATRIX_DATABLOCK(m_AF), /* A */
+ &m_AF(0), /* A */
&SizeOfZ, /* Leading dimension cf before */
- MAL_RET_VECTOR_DATABLOCK(m_IPIV), /* IPIV */
+ &m_IPIV(0), /* IPIV */
&info /* info */
);
m_NeedToReset = false;
@@ -322,28 +327,28 @@ namespace PatternGeneratorJRL
lN, /* A * X = B */
&SizeOfZ, /* Size of A */
&NRHS, /*Nb of columns for X et B */
- MAL_RET_MATRIX_DATABLOCK(tZ), /* Access to A */
+ &tZ(0), /* Access to A */
&LDA, /* Leading size of A */
- MAL_RET_MATRIX_DATABLOCK(m_AF),
+ &m_AF(0),
&LDAF,
- MAL_RET_VECTOR_DATABLOCK(m_IPIV),
+ &m_IPIV(0),
&EQUED,
- MAL_RET_VECTOR_DATABLOCK(lR),
- MAL_RET_VECTOR_DATABLOCK(lC),
- MAL_RET_VECTOR_DATABLOCK(m_w),
+ &lR(0),
+ &lC(0),
+ &m_w(0),
&LDB,
- MAL_RET_VECTOR_DATABLOCK(m_y),
+ &m_y(0),
&lsizeofx,
&lRCOND,
- MAL_RET_VECTOR_DATABLOCK(lFERR),
- MAL_RET_VECTOR_DATABLOCK(lBERR),
+ &lFERR(0),
+ &lBERR(0),
work,
iwork,
&info
);
// Compute the weights.
- // MAL_C_eq_A_by_B(m_y,iZ,m_w);
+ // m_y=iZ+m_w;
if (m_VerboseLevel>=2)
{
@@ -351,7 +356,7 @@ namespace PatternGeneratorJRL
ofs.open("YMatrix.dat",ofstream::out);
ofs.precision(10);
- for(unsigned int i=0;i<MAL_VECTOR_SIZE(m_y);i++)
+ for(unsigned int i=0;i<m_y.size();i++)
{
ofs << m_y[i]<< " ";
}
@@ -364,7 +369,7 @@ namespace PatternGeneratorJRL
}
- int AnalyticalMorisawaCompact::BuildAndSolveCOMZMPForASetOfSteps(MAL_S3x3_MATRIX(& lStartingCOMState,double),
+ int AnalyticalMorisawaCompact::BuildAndSolveCOMZMPForASetOfSteps(Eigen::Matrix3d &lStartingCOMState,
FootAbsolutePosition &LeftFootInitialPosition,
FootAbsolutePosition &RightFootInitialPosition,
bool IgnoreFirstRelativeFoot,
@@ -374,7 +379,7 @@ namespace PatternGeneratorJRL
if (m_RelativeFootPositions.size()==0)
return -2;
- int NbSteps = m_RelativeFootPositions.size();
+ int NbSteps = (int)m_RelativeFootPositions.size();
int NbOfIntervals=2*NbSteps+1;
SetNumberOfStepsInAdvance(NbSteps);
@@ -459,7 +464,7 @@ computing the analytical trajectories. */
// Strategy for the final CoM pos: middle of the segment
// between the two final steps, in order to be statically stable.
- unsigned int lindex = m_AbsoluteSupportFootPositions.size()-1;
+ unsigned int lindex = (unsigned int)(m_AbsoluteSupportFootPositions.size()-1);
if (DoNotPrepareLastFoot)
FinalCoMPosX = m_AbsoluteSupportFootPositions[lindex].x;
@@ -525,13 +530,13 @@ computing the analytical trajectories. */
deque<FootAbsolutePosition> &RightFootAbsolutePositions,
double ,
COMState & lStartingCOMState,
- MAL_S3_VECTOR(&,double) ,
+ Eigen::Vector3d & ,
FootAbsolutePosition & InitLeftFootAbsolutePosition,
FootAbsolutePosition & InitRightFootAbsolutePosition)
{
// INITIALIZE FEET POSITIONS:
// --------------------------
- vector3d lAnklePositionRight,lAnklePositionLeft;
+ Eigen::Vector3d lAnklePositionRight,lAnklePositionLeft;
PRFoot *LeftFoot, *RightFoot;
LeftFoot = m_PR->leftFoot();
if (LeftFoot==0)
@@ -544,21 +549,15 @@ computing the analytical trajectories. */
lAnklePositionLeft = LeftFoot->anklePosition ;
lAnklePositionRight = RightFoot->anklePosition ;
- MAL_VECTOR_DIM(CurPosWICF_homogeneous,double,4) ;
- m_kajitaDynamicFilter->getComAndFootRealization()->GetCurrentPositionofWaistInCOMFrame(CurPosWICF_homogeneous);
-
-// InitLeftFootAbsolutePosition.x += lAnklePositionLeft(0) ;
-// InitLeftFootAbsolutePosition.y += lAnklePositionLeft(1) ;
-// InitLeftFootAbsolutePosition.z += lAnklePositionLeft(2) ;
-// InitRightFootAbsolutePosition.x += lAnklePositionRight(0) ;
-// InitRightFootAbsolutePosition.y += lAnklePositionRight(1) ;
-// InitRightFootAbsolutePosition.z += lAnklePositionRight(2) ;
+ Eigen::Matrix4d CurPosWICF_homogeneous ;
+ CurPosWICF_homogeneous = m_kajitaDynamicFilter->getComAndFootRealization()
+ ->GetCurrentPositionofWaistInCOMFrame();
m_RelativeFootPositions = RelativeFootPositions;
/* This part computes the CoM and ZMP trajectory giving the foot position information.
It also creates the analytical feet trajectories.
*/
- MAL_S3x3_MATRIX(lMStartingCOMState,double);
+ Eigen::Matrix3d lMStartingCOMState;
lMStartingCOMState(0,0)= lStartingCOMState.x[0];
lMStartingCOMState(1,0)= lStartingCOMState.y[0];
@@ -605,7 +604,7 @@ computing the analytical trajectories. */
if(filterOn_)
{
/*! initialize the dynamic filter */
- unsigned int n = COMStates.size();
+ unsigned int n =(unsigned int) COMStates.size();
double KajitaPCpreviewWindow = 1.6 ;
m_kajitaDynamicFilter->init( m_SamplingPeriod,
m_SamplingPeriod,
@@ -614,9 +613,9 @@ computing the analytical trajectories. */
KajitaPCpreviewWindow,
lStartingCOMState );
/*! Set the upper body trajectory */
- MAL_VECTOR_TYPE(double) UpperConfig = m_PR->currentConfiguration() ;
- MAL_VECTOR_TYPE(double) UpperVel = m_PR->currentVelocity() ;
- MAL_VECTOR_TYPE(double) UpperAcc = m_PR->currentAcceleration() ;
+ Eigen::VectorXd UpperConfig = m_PR->currentConfiguration() ;
+ Eigen::VectorXd UpperVel = m_PR->currentVelocity() ;
+ Eigen::VectorXd UpperAcc = m_PR->currentAcceleration() ;
// carry the weight in front of him
// UpperConfig(18)= 0.0 ; // CHEST_JOINT0
// UpperConfig(19)= 0.015 ; // CHEST_JOINT1
@@ -692,9 +691,9 @@ computing the analytical trajectories. */
ZMPPositions,
LeftFootAbsolutePositions,
RightFootAbsolutePositions,
- vector< MAL_VECTOR_TYPE(double) > (1,UpperConfig),
- vector< MAL_VECTOR_TYPE(double) > (1,UpperVel),
- vector< MAL_VECTOR_TYPE(double) > (1,UpperAcc),
+ vector< Eigen::VectorXd > (1,UpperConfig),
+ vector< Eigen::VectorXd > (1,UpperVel),
+ vector< Eigen::VectorXd > (1,UpperAcc),
outputDeltaCOMTraj_deq);
#ifdef DEBUG
@@ -740,17 +739,17 @@ computing the analytical trajectories. */
FootAbsolutePosition & InitRightFootAbsolutePosition,
deque<RelativeFootPosition> &RelativeFootPositions,
COMState & lStartingCOMState,
- MAL_S3_VECTOR(&,double))
+ Eigen::Vector3d &)
{
m_OnLineMode = true;
m_RelativeFootPositions.clear();
- unsigned int r = RelativeFootPositions.size();
+ unsigned int r = (unsigned int)RelativeFootPositions.size();
unsigned int maxrelsteps = r < 3 ? r : 3;
// INITIALIZE FEET POSITIONS:
// --------------------------
- vector3d lAnklePositionRight,lAnklePositionLeft;
+ Eigen::Vector3d lAnklePositionRight,lAnklePositionLeft;
PRFoot *LeftFoot, *RightFoot;
LeftFoot = m_PR->leftFoot();
if (LeftFoot==0)
@@ -763,8 +762,10 @@ computing the analytical trajectories. */
lAnklePositionLeft = LeftFoot->anklePosition ;
lAnklePositionRight = RightFoot->anklePosition ;
- MAL_VECTOR_DIM(CurPosWICF_homogeneous,double,4) ;
- m_kajitaDynamicFilter->getComAndFootRealization()->GetCurrentPositionofWaistInCOMFrame(CurPosWICF_homogeneous);
+ Eigen::Matrix<double,4,1> CurPosWICF_homogeneous ;
+ CurPosWICF_homogeneous =
+ m_kajitaDynamicFilter->getComAndFootRealization()
+ ->GetCurrentPositionofWaistInCOMFrame();
InitLeftFootAbsolutePosition.x += lAnklePositionLeft(0) ;
InitLeftFootAbsolutePosition.y += lAnklePositionLeft(1) ;
@@ -775,7 +776,7 @@ computing the analytical trajectories. */
// INITIALIZE THE COM
// ------------------
- MAL_S3x3_MATRIX(lMStartingCOMState,double);
+ Eigen::Matrix3d lMStartingCOMState;
lMStartingCOMState(0,0)= lStartingCOMState.x[0];
lMStartingCOMState(1,0)= lStartingCOMState.y[0];
@@ -843,7 +844,7 @@ computing the analytical trajectories. */
lStartingCOMState );
- return m_RelativeFootPositions.size();
+ return (int) m_RelativeFootPositions.size();
}
@@ -1030,7 +1031,7 @@ computing the analytical trajectories. */
unsigned int StartingIndexInterval;
m_AnalyticalZMPCoGTrajectoryX->GetIntervalIndexFromTime(m_CurrentTime,StartingIndexInterval);
- unsigned int IndexInterval = m_CTIPX.ZMPProfil.size()-1;
+ unsigned int IndexInterval = (unsigned int)(m_CTIPX.ZMPProfil.size()-1);
/* If the interval detected is not a double support interval,
a shift is done to chose the earliest double support interval. */
@@ -1135,7 +1136,7 @@ When the limit is reached, and the stack exhausted this method is called again.
// Again assuming that the number of unknowns
// is based on a 4- (m-2) 3 -4 th order polynomials.
- MAL_VECTOR_RESIZE(m_w, 2 * m_NumberOfIntervals + 6);
+ m_w.resize( 2 * m_NumberOfIntervals + 6);
// Initial CoM Position
m_w(lindex)= InitialCoMPos - ZMPPosSequence[0];
@@ -1228,7 +1229,7 @@ When the limit is reached, and the stack exhausted this method is called again.
ofs.open("WCompactMatrix.dat",ofstream::out);
ofs.precision(10);
- for(unsigned int i=0;i<MAL_VECTOR_SIZE(m_w);i++)
+ for(unsigned int i=0;i<m_w.size();i++)
{
ofs << m_w[i]<< " ";
}
@@ -1413,12 +1414,15 @@ When the limit is reached, and the stack exhausted this method is called again.
NbRows = 2+4+2*(m_NumberOfIntervals-2)+4;
NbCols = 2*m_NumberOfIntervals + 6;
- MAL_MATRIX_RESIZE(m_Z,NbRows,NbCols);
+ m_Z.resize(NbRows,NbCols);
// Initial condition for the COG position and the velocity
double SquareOmega0 = m_Omegaj[0]*m_Omegaj[0];
- MAL_MATRIX_FILL(m_Z,0.0);
+ {for(unsigned int i=0;i<m_Z.rows();i++)
+ for(unsigned int j=0;j<m_Z.cols();j++)
+ m_Z(i,j)=0.0;
+ };
m_Z(0,0) = 2.0/SquareOmega0;
m_Z(0,3) = 1.0;
rowindex++;
@@ -1446,9 +1450,9 @@ When the limit is reached, and the stack exhausted this method is called again.
std::ofstream ofs;
ofs.open("ZCompactMatrix.dat",ofstream::out);
ofs.precision(10);
- for(unsigned int i=0;i<MAL_MATRIX_NB_ROWS(m_Z);i++)
+ for(unsigned int i=0;i<m_Z.rows();i++)
{
- for(unsigned int j=0;j<MAL_MATRIX_NB_COLS(m_Z);j++)
+ for(unsigned int j=0;j<m_Z.cols();j++)
{
ofs << m_Z(i,j) << " ";
}
@@ -1671,7 +1675,7 @@ and if there is a StepStack Handler available.
if (aStepStackHandler!=0)
{
/* Compute the number of steps needed. */
- int NeededSteps = (aCTIPX.ZMPProfil.size()-j+1)/2;
+ int NeededSteps = (int)((aCTIPX.ZMPProfil.size()-j+1)/2);
int r;
/* Test if there is enough step in the stack of.
@@ -1699,7 +1703,7 @@ We have to remove one, because there is still the last foot added.
lRelativeFootPositions.pop_front();
deque<FootAbsolutePosition> lAbsoluteSupportFootPositions;
- int lLastIndex = m_AbsoluteSupportFootPositions.size()-1;
+ int lLastIndex = (int)(m_AbsoluteSupportFootPositions.size()-1);
m_FeetTrajectoryGenerator->ComputeAbsoluteStepsFromRelativeSteps(lRelativeFootPositions,
m_AbsoluteSupportFootPositions[lLastIndex],
lAbsoluteSupportFootPositions);
@@ -1786,8 +1790,8 @@ and final CoM to be feed to the new system. */
else r = rnum/rden;
;
- r2 = ( m_Omegaj[0]*(aFP.CoMInit - aFP.ZMPInit) + (aFP.CoMSpeedInit - aFP.ZMPSpeedInit) )/
- (m_Omegaj[0] * ( aFP.CoMNew - aFP.ZMPNew) + (aFP.CoMSpeedNew - aFP.ZMPSpeedNew));
+ /* r2 = ( m_Omegaj[0]*(aFP.CoMInit - aFP.ZMPInit) + (aFP.CoMSpeedInit - aFP.ZMPSpeedInit) )/
+ (m_Omegaj[0] * ( aFP.CoMNew - aFP.ZMPNew) + (aFP.CoMSpeedNew - aFP.ZMPSpeedNew)); */
if (r<0.0)
DeltaTNew = DeltaTInit + m_Tsingle*0.5;
@@ -2103,7 +2107,7 @@ and final condition, and update the queue of foot prints. */
/* Initiliazing the Preview Control according to the trajectory
to filter. */
double lAbsoluteTimeReference = aAZCT.GetAbsoluteTimeReference();
- MAL_MATRIX_DIM(x,double,3,1);
+ Eigen::MatrixXd x(3,1);
/*! Initialize the state vector used by the preview controller */
x(0,0) = 0.0;//aAZCT.ComputeCOM(lAbsoluteTimeReference,x(0,0));
@@ -2137,8 +2141,9 @@ to filter. */
lsxzmp = 0.0;
for(double lx=0;lx<m_DeltaTj[0]+PreviewWindowTime;lx+= m_SamplingPeriod)
{
- m_PreviewControl->OneIterationOfPreview1D(x,lsxzmp,FIFOZMPRefPositions,lindex,
- lxzmp,false);
+ m_PreviewControl->
+ OneIterationOfPreview1D(x,lsxzmp,FIFOZMPRefPositions,lindex,
+ lxzmp);
ZMPTrajectory.push_back(lxzmp);
CoGTrajectory.push_back(x(0,0));
lindex++;
@@ -2516,7 +2521,7 @@ new step has to be generate.
}
else if (Method==":setRobotUpperPart")
{
-// MAL_VECTOR_TYPE(double) configuration ;
+// Eigen::VectorXd configuration ;
// if (strm.good())
// {
// strm >> configuration;
@@ -2680,7 +2685,7 @@ new step has to be generate.
PRFoot *aFoot = m_PR->leftFoot() ;
if (aFoot==0)
LTHROW("No foot");
- vector3d corrZ ;
+ Eigen::Vector3d corrZ ;
corrZ = aFoot->anklePosition ;
corrZ(2) = 0; //foot height no more equal to ankle height; TODO : remove corrZ
@@ -2884,7 +2889,7 @@ new step has to be generate.
PRFoot *aFoot = m_PR->leftFoot() ;
if (aFoot==0)
LTHROW("No foot");
- vector3d corrZ ;
+ Eigen::Vector3d corrZ ;
corrZ = aFoot->anklePosition ;
corrZ(2) = 0.0 ;
bool sinple_support = (IndexInterval % 2) == 0 ;
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh
index fbc49d6c..fc9e94bb 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh
@@ -154,7 +154,7 @@ namespace PatternGeneratorJRL
deque<FootAbsolutePosition> &RightFootAbsolutePositions,
double Xmax,
COMState & lStartingCOMState,
- MAL_S3_VECTOR_TYPE(double) &lStartingZMPPosition,
+ Eigen::Vector3d &lStartingZMPPosition,
FootAbsolutePosition & InitLeftFootAbsolutePosition,
FootAbsolutePosition & InitRightFootAbsolutePosition);
@@ -187,7 +187,7 @@ namespace PatternGeneratorJRL
FootAbsolutePosition & InitRightFootAbsolutePosition,
deque<RelativeFootPosition> &RelativeFootPositions,
COMState &lStartingCOMState,
- MAL_S3_VECTOR(&,double) lStartingZMPPosition
+ Eigen::Vector3d & lStartingZMPPosition
);
/* ! \brief Methods to update the stack on-line by inserting a new foot position.
@@ -520,7 +520,7 @@ namespace PatternGeneratorJRL
*/
- int BuildAndSolveCOMZMPForASetOfSteps(MAL_S3x3_MATRIX(& ,double) lStartingCOMState,
+ int BuildAndSolveCOMZMPForASetOfSteps(Eigen::Matrix3d & lStartingCOMState,
FootAbsolutePosition &LeftFootInitialPosition,
FootAbsolutePosition &RightFootInitialPosition,
bool IgnoreFirstRelativeFoot,
@@ -588,10 +588,10 @@ namespace PatternGeneratorJRL
FootAbsolutePosition & FinalRightFootAbsolutePosition);
/*! \brief LU decomposition of the Z matrix. */
- MAL_MATRIX_TYPE(double) m_AF;
+ Eigen::MatrixXd m_AF;
/*! \brief Pivots of the Z matrix LU decomposition. */
- MAL_VECTOR_TYPE(int) m_IPIV;
+ Eigen::Matrix<int, Eigen::Dynamic, 1> m_IPIV;
/*! \brief Boolean on the need to reset to the
precomputed Z matrix LU decomposition */
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
index 49d24935..102ee61b 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
@@ -50,9 +50,9 @@ namespace PatternGeneratorJRL
const deque<ZMPPosition> & inputZMPTraj_deq_,
const deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
const deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
- const vector<MAL_VECTOR_TYPE(double) > &UpperPart_q,
- const vector<MAL_VECTOR_TYPE(double) > &UpperPart_dq,
- const vector<MAL_VECTOR_TYPE(double) > &UpperPart_ddq,
+ const vector<Eigen::VectorXd > &UpperPart_q,
+ const vector<Eigen::VectorXd > &UpperPart_dq,
+ const vector<Eigen::VectorXd > &UpperPart_ddq,
deque<COMState> & outputDeltaCOMTraj_deq_);
int OnLinefilter(const deque<COMState> & inputCOMTraj_deq_,
@@ -74,9 +74,9 @@ namespace PatternGeneratorJRL
const COMState & inputCoMState,
const FootAbsolutePosition & inputLeftFoot,
const FootAbsolutePosition & inputRightFoot,
- MAL_VECTOR_TYPE(double) & configuration,
- MAL_VECTOR_TYPE(double) & velocity,
- MAL_VECTOR_TYPE(double) & acceleration,
+ Eigen::VectorXd & configuration,
+ Eigen::VectorXd & velocity,
+ Eigen::VectorXd & acceleration,
double samplingPeriod,
int stage,
int iteration);
@@ -87,7 +87,7 @@ namespace PatternGeneratorJRL
const COMState & inputCoMState,
const FootAbsolutePosition & inputLeftFoot,
const FootAbsolutePosition & inputRightFoot,
- MAL_S3_VECTOR_TYPE(double) & ZMPMB,
+ Eigen::Vector3d & ZMPMB,
unsigned int stage,
unsigned int iteration);
@@ -98,10 +98,10 @@ namespace PatternGeneratorJRL
deque<COMState> & outputDeltaCOMTraj_deq_);
/// \brief compute the zmpmb from articulated pos vel and acc
- int zmpmb(MAL_VECTOR_TYPE(double)& configuration,
- MAL_VECTOR_TYPE(double)& velocity,
- MAL_VECTOR_TYPE(double)& acceleration,
- MAL_S3_VECTOR_TYPE(double) & zmpmb);
+ int zmpmb(Eigen::VectorXd& configuration,
+ Eigen::VectorXd& velocity,
+ Eigen::VectorXd& acceleration,
+ Eigen::Vector3d & zmpmb);
void CallMethod(std::string & Method, std::istringstream &strm);
@@ -113,9 +113,9 @@ namespace PatternGeneratorJRL
public: // The accessors
- void setRobotUpperPart(const MAL_VECTOR_TYPE(double) & configuration,
- const MAL_VECTOR_TYPE(double) & velocity,
- const MAL_VECTOR_TYPE(double) & acceleration);
+ void setRobotUpperPart(const Eigen::VectorXd & configuration,
+ const Eigen::VectorXd & velocity,
+ const Eigen::VectorXd & acceleration);
/// \brief getter :
inline ComAndFootRealizationByGeometry * getComAndFootRealization()
@@ -139,7 +139,7 @@ namespace PatternGeneratorJRL
inline Clock * getClock()
{ return &clock_ ; }
- inline deque< MAL_S3_VECTOR_TYPE(double) > zmpmb()
+ inline deque< Eigen::Vector3d > zmpmb()
{ return ZMPMB_vec_ ; }
private: // Private members
@@ -168,25 +168,25 @@ namespace PatternGeneratorJRL
ComAndFootRealizationByGeometry * comAndFootRealization_;
/// \brief Buffers for the Inverse Kinematics
- MAL_VECTOR_TYPE(double) aCoMState_;
- MAL_VECTOR_TYPE(double) aCoMSpeed_;
- MAL_VECTOR_TYPE(double) aCoMAcc_;
- MAL_VECTOR_TYPE(double) aLeftFootPosition_;
- MAL_VECTOR_TYPE(double) aRightFootPosition_;
+ Eigen::VectorXd aCoMState_;
+ Eigen::VectorXd aCoMSpeed_;
+ Eigen::VectorXd aCoMAcc_;
+ Eigen::VectorXd aLeftFootPosition_;
+ Eigen::VectorXd aRightFootPosition_;
/// \brief used to compute the ZMPMB from only
/// com and feet position from outside of the class
- MAL_VECTOR_TYPE(double) ZMPMBConfiguration_ ;
- MAL_VECTOR_TYPE(double) ZMPMBVelocity_ ;
- MAL_VECTOR_TYPE(double) ZMPMBAcceleration_ ;
- MAL_VECTOR_TYPE(double) previousZMPMBConfiguration_ ;
- MAL_VECTOR_TYPE(double) previousZMPMBVelocity_ ;
-
- MAL_VECTOR_TYPE(double) upperPartConfiguration_ ;
- MAL_VECTOR_TYPE(double) previousUpperPartConfiguration_ ;
- MAL_VECTOR_TYPE(double) upperPartVelocity_ ;
- MAL_VECTOR_TYPE(double) previousUpperPartVelocity_ ;
- MAL_VECTOR_TYPE(double) upperPartAcceleration_ ;
+ Eigen::VectorXd ZMPMBConfiguration_ ;
+ Eigen::VectorXd ZMPMBVelocity_ ;
+ Eigen::VectorXd ZMPMBAcceleration_ ;
+ Eigen::VectorXd previousZMPMBConfiguration_ ;
+ Eigen::VectorXd previousZMPMBVelocity_ ;
+
+ Eigen::VectorXd upperPartConfiguration_ ;
+ Eigen::VectorXd previousUpperPartConfiguration_ ;
+ Eigen::VectorXd upperPartVelocity_ ;
+ Eigen::VectorXd previousUpperPartVelocity_ ;
+ Eigen::VectorXd upperPartAcceleration_ ;
/*! \brief left Leg Index in Configuration */
std::vector<int> llegIdxq_ ;
/*! \brief right Leg Index in Configuration */
@@ -219,9 +219,9 @@ namespace PatternGeneratorJRL
/// from the inverse Dynamics, and the difference between
/// this zmp and the reference one.
/// sampled at interpolation sampling period
- deque< MAL_S3_VECTOR_TYPE(double) > ZMPMB_vec_ ;
+ deque< Eigen::Vector3d > ZMPMB_vec_ ;
/// sampled at control sampling period
- deque< MAL_S3_VECTOR_TYPE(double) > zmpmb_i_ ;
+ deque< Eigen::Vector3d > zmpmb_i_ ;
/// sampled at control sampling period
std::deque<ZMPPosition> deltaZMP_deq_ ;
@@ -235,8 +235,8 @@ namespace PatternGeneratorJRL
double CoMHeight_ ;
/// \brief State of the Preview control.
- MAL_MATRIX(deltax_,double);
- MAL_MATRIX(deltay_,double);
+ Eigen::MatrixXd deltax_;
+ Eigen::MatrixXd deltay_;
/// \brief time measurement
Clock clock_;
diff --git a/src/ZMPRefTrajectoryGeneration/FilteringAnalyticalTrajectoryByPreviewControl.cpp b/src/ZMPRefTrajectoryGeneration/FilteringAnalyticalTrajectoryByPreviewControl.cpp
index 17e74d79..6eff458f 100644
--- a/src/ZMPRefTrajectoryGeneration/FilteringAnalyticalTrajectoryByPreviewControl.cpp
+++ b/src/ZMPRefTrajectoryGeneration/FilteringAnalyticalTrajectoryByPreviewControl.cpp
@@ -48,7 +48,7 @@ FilteringAnalyticalTrajectoryByPreviewControl::FilteringAnalyticalTrajectoryByPr
m_PreviewControl = 0;
/*! Initialize the state vector used by the preview controller */
- MAL_MATRIX_RESIZE(m_ComState,3,1);
+ m_ComState.resize(3,1);
m_ComState(0,0) = 0.0;
m_ComState(1,0) = 0.0;
m_ComState(2,0) = 0.0;
diff --git a/src/ZMPRefTrajectoryGeneration/FilteringAnalyticalTrajectoryByPreviewControl.hh b/src/ZMPRefTrajectoryGeneration/FilteringAnalyticalTrajectoryByPreviewControl.hh
index 994302af..543e8578 100644
--- a/src/ZMPRefTrajectoryGeneration/FilteringAnalyticalTrajectoryByPreviewControl.hh
+++ b/src/ZMPRefTrajectoryGeneration/FilteringAnalyticalTrajectoryByPreviewControl.hh
@@ -92,7 +92,7 @@ namespace PatternGeneratorJRL
PreviewControl * m_PreviewControl;
/*! \brief State of the CoM */
- MAL_MATRIX(m_ComState,double);
+ Eigen::MatrixXd m_ComState;
/*! \brief Starting time of the filter. */
double m_StartingTime;
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index cc76d122..bb6732ae 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -286,7 +286,7 @@ ZMPVelocityReferencedQP::~ZMPVelocityReferencedQP()
void ZMPVelocityReferencedQP::setCoMPerturbationForce(istringstream &strm)
{
- MAL_VECTOR_RESIZE(PerturbationAcceleration_,6);
+ PerturbationAcceleration_.resize(6);
strm >> PerturbationAcceleration_(2);
strm >> PerturbationAcceleration_(5);
@@ -298,7 +298,7 @@ void ZMPVelocityReferencedQP::setCoMPerturbationForce(istringstream &strm)
void ZMPVelocityReferencedQP::setCoMPerturbationForce(double x, double y)
{
- MAL_VECTOR_RESIZE(PerturbationAcceleration_,6);
+ PerturbationAcceleration_.resize(6);
PerturbationAcceleration_(2) = x/RobotMass_;
PerturbationAcceleration_(5) = y/RobotMass_;
@@ -346,7 +346,7 @@ int ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
FootAbsolutePosition & InitRightFootAbsolutePosition,
deque<RelativeFootPosition> &, // RelativeFootPositions,
COMState & lStartingCOMState,
- MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition)
+ Eigen::Vector3d & lStartingZMPPosition)
{
UpperTimeLimitToUpdate_ = 0.0;
@@ -821,7 +821,7 @@ void ZMPVelocityReferencedQP::GetZMPDiscretization(deque<ZMPPosition> & ,
deque<FootAbsolutePosition> &,
double ,
COMState &,
- MAL_S3_VECTOR(&,double),
+ Eigen::Vector3d &,
FootAbsolutePosition & ,
FootAbsolutePosition & )
{
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index d669685e..388b9ecb 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -87,7 +87,7 @@ namespace PatternGeneratorJRL
FootAbsolutePosition & InitRightFootAbsolutePosition,
deque<RelativeFootPosition> &RelativeFootPositions,
COMState & lStartingCOMState,
- MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition);
+ Eigen::Vector3d & lStartingZMPPosition);
/// \brief Update the stacks on-line
@@ -173,7 +173,7 @@ namespace PatternGeneratorJRL
double TimeBuffer_;
/// \brief Additional term on the acceleration of the CoM
- MAL_VECTOR(PerturbationAcceleration_,double);
+ Eigen::VectorXd PerturbationAcceleration_;
/// \brief Sampling period considered in the QP
double QP_T_;
@@ -290,7 +290,7 @@ namespace PatternGeneratorJRL
std::deque<FootAbsolutePosition> &RightFootAbsolutePositions,
double Xmax,
COMState & lStartingCOMState,
- MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition,
+ Eigen::Vector3d & lStartingZMPPosition,
FootAbsolutePosition & InitLeftFootAbsolutePosition,
FootAbsolutePosition & InitRightFootAbsolutePosition);
--
GitLab