From de14ed6d88f82d61d13297bee10901547002cb4a Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Thu, 7 Mar 2019 20:09:37 +0100
Subject: [PATCH] Update to pinocchio v2.1.0 Pb with pinocchio v2.1.0 and
retrieveCollisionGeometrey. It works only if this method is inline.
---
include/jrl/walkgen/pinocchiorobot.hh | 50 +++++-----
.../FootTrajectoryGenerationStandard.cpp | 5 +-
.../ComAndFootRealizationByGeometry.cpp | 52 +++++-----
.../ComAndFootRealizationByGeometry.hh | 14 +--
src/MotionGeneration/StepOverPlanner.cpp | 18 ++--
src/PreviewControl/PreviewControl.cpp | 2 +-
src/RobotDynamics/pinocchiorobot.cpp | 96 +++++++++----------
.../AnalyticalMorisawaCompact.cpp | 18 ++--
tests/TestNaveau2015.cpp | 22 ++---
tests/TestObject.cpp | 32 +++----
tests/TestObject.hh | 18 ++--
11 files changed, 160 insertions(+), 167 deletions(-)
diff --git a/include/jrl/walkgen/pinocchiorobot.hh b/include/jrl/walkgen/pinocchiorobot.hh
index 8592abe4..a99befe6 100644
--- a/include/jrl/walkgen/pinocchiorobot.hh
+++ b/include/jrl/walkgen/pinocchiorobot.hh
@@ -37,7 +37,7 @@ frame work */
namespace PatternGeneratorJRL
{
struct PinocchioRobotFoot_t{
- se3::JointIndex associatedAnkle ;
+ pinocchio::JointIndex associatedAnkle ;
double soleDepth ; // z axis
double soleWidth ; // y axis
double soleHeight ;// x axis
@@ -88,8 +88,8 @@ namespace PatternGeneratorJRL
/// param 6D vector output, filled with zeros if the robot is not compatible
///
virtual bool ComputeSpecializedInverseKinematics(
- const se3::JointIndex &jointRoot,
- const se3::JointIndex &jointEnd,
+ const pinocchio::JointIndex &jointRoot,
+ const pinocchio::JointIndex &jointEnd,
const Eigen::Matrix4d & jointRootPosition,
const Eigen::Matrix4d & jointEndPosition,
Eigen::VectorXd &q);
@@ -113,9 +113,9 @@ namespace PatternGeneratorJRL
public :
/// tools :
- std::vector<se3::JointIndex> jointsBetween
- ( se3::JointIndex first, se3::JointIndex second);
- std::vector<se3::JointIndex> fromRootToIt (se3::JointIndex it);
+ std::vector<pinocchio::JointIndex> jointsBetween
+ ( pinocchio::JointIndex first, pinocchio::JointIndex second);
+ std::vector<pinocchio::JointIndex> fromRootToIt (pinocchio::JointIndex it);
private :
// needed for the inverse geometry (ComputeSpecializedInverseKinematics)
@@ -129,18 +129,18 @@ namespace PatternGeneratorJRL
Eigen::VectorXd &q,
int side);
void DetectAutomaticallyShoulders();
- void DetectAutomaticallyOneShoulder(se3::JointIndex aWrist,
- se3::JointIndex & aShoulder);
+ void DetectAutomaticallyOneShoulder(pinocchio::JointIndex aWrist,
+ pinocchio::JointIndex & aShoulder);
public :
/// Getters
/// ///////
- inline se3::Data * Data()
+ inline pinocchio::Data * Data()
{return m_robotData;}
- inline se3::Data * DataInInitialePose()
+ inline pinocchio::Data * DataInInitialePose()
{return m_robotDataInInitialePose;}
- inline se3::Model * Model()
+ inline pinocchio::Model * Model()
{return m_robotModel;}
inline PRFoot * leftFoot()
@@ -148,20 +148,20 @@ namespace PatternGeneratorJRL
inline PRFoot * rightFoot()
{return &m_rightFoot;}
- inline se3::JointIndex leftWrist()
+ inline pinocchio::JointIndex leftWrist()
{return m_leftWrist;}
- inline se3::JointIndex rightWrist()
+ inline pinocchio::JointIndex rightWrist()
{return m_rightWrist;}
- inline se3::JointIndex chest()
+ inline pinocchio::JointIndex chest()
{return m_chest;}
- inline se3::JointIndex waist()
+ inline pinocchio::JointIndex waist()
{return m_waist;}
inline double mass()
{return m_mass;}
- inline se3::JointModelVector & getActuatedJoints()
+ inline pinocchio::JointModelVector & getActuatedJoints()
{return m_robotModel->joints;}
inline Eigen::VectorXd currentConfiguration()
@@ -226,22 +226,22 @@ namespace PatternGeneratorJRL
{
return m_boolModel && m_boolData && m_boolLeftFoot && m_boolRightFoot;
}
- bool checkModel(se3::Model * robotModel);
- bool initializeRobotModelAndData(se3::Model * robotModel,
- se3::Data * robotData);
+ bool checkModel(pinocchio::Model * robotModel);
+ bool initializeRobotModelAndData(pinocchio::Model * robotModel,
+ pinocchio::Data * robotData);
bool initializeLeftFoot(PRFoot leftFoot);
bool initializeRightFoot(PRFoot rightFoot);
/// Attributes
/// //////////
private :
- se3::Model * m_robotModel ;
- se3::Data * m_robotDataInInitialePose ; // internal variable
- se3::Data * m_robotData ;
+ pinocchio::Model * m_robotModel ;
+ pinocchio::Data * m_robotDataInInitialePose ; // internal variable
+ pinocchio::Data * m_robotData ;
PRFoot m_leftFoot , m_rightFoot ;
double m_mass ;
- se3::JointIndex m_chest, m_waist, m_leftShoulder, m_rightShoulder ;
- se3::JointIndex m_leftWrist , m_rightWrist ;
+ pinocchio::JointIndex m_chest, m_waist, m_leftShoulder, m_rightShoulder ;
+ pinocchio::JointIndex m_leftWrist , m_rightWrist ;
Eigen::VectorXd m_qmal ;
Eigen::VectorXd m_vmal ;
@@ -253,7 +253,7 @@ namespace PatternGeneratorJRL
// tmp variables
Eigen::Quaterniond m_quat ;
Eigen::Matrix3d m_rot ;
- se3::Force m_externalForces ; // external forces and torques
+ pinocchio::Force m_externalForces ; // external forces and torques
Eigen::VectorXd m_tau ; // external forces and torques
Eigen::Vector3d m_f,m_n; // external forces and torques
Eigen::Vector3d m_com; // multibody CoM
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
index 8dc2a5cb..0c8bb79b 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
@@ -53,17 +53,17 @@ FootTrajectoryGenerationStandard::FootTrajectoryGenerationStandard(SimplePluginM
/* Computes information on foot dimension
from humanoid specific informations. */
- double lWidth,lHeight,lDepth;
+ double lWidth=0.0,lDepth=0.0;
if (m_Foot->associatedAnkle!=0)
{
lWidth = m_Foot->soleWidth ;
- lHeight = m_Foot->soleHeight ;
}
else
{
cerr << "Pb no ref Foot." << endl;
}
Eigen::Vector3d AnklePosition;
+ AnklePosition.Zero();
if (m_Foot->associatedAnkle!=0)
AnklePosition = m_Foot->anklePosition;
else
@@ -87,7 +87,6 @@ FootTrajectoryGenerationStandard::FootTrajectoryGenerationStandard(SimplePluginM
if (m_Foot->associatedAnkle!=0)
{
lWidth = m_Foot->soleWidth ;
- lHeight = m_Foot->soleHeight ;
AnklePosition = m_Foot->anklePosition;
}
else
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
index dfc78fe5..60ff3b36 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
@@ -108,8 +108,8 @@ void ComAndFootRealizationByGeometry::
}
void ComAndFootRealizationByGeometry::
-InitializationMaps(std::vector<se3::Index> &FromRootToJoint,
- se3::JointModelVector & actuatedJoints,
+InitializationMaps(std::vector<pinocchio::Index> &FromRootToJoint,
+ pinocchio::JointModelVector & actuatedJoints,
std::vector<int> &IndexinConfiguration,
std::vector<int> &IndexinVelocity)
{
@@ -127,33 +127,33 @@ InitializationMaps(std::vector<se3::Index> &FromRootToJoint,
{
// -1 because pinocchio uses quaternion instead of roll pitch yaw
// assuming the model possess only revolute joint
- IndexinConfiguration[lindex] = se3::idx_q(actuatedJoints[FromRootToJoint[i]])-1 ;
- IndexinVelocity[lindex] = se3::idx_v(actuatedJoints[FromRootToJoint[i]]) ;
+ IndexinConfiguration[lindex] = pinocchio::idx_q(actuatedJoints[FromRootToJoint[i]])-1 ;
+ IndexinVelocity[lindex] = pinocchio::idx_v(actuatedJoints[FromRootToJoint[i]]) ;
lindex++;
}
}
}
void ComAndFootRealizationByGeometry::
- InitializeMapsForAHand(se3::JointIndex aWrist,
- se3::JointModelVector & ActuatedJoints,
+ InitializeMapsForAHand(pinocchio::JointIndex aWrist,
+ pinocchio::JointModelVector & ActuatedJoints,
vector<int> & IndexesInConfiguration,
vector<int> & IndexesInVelocity,
- se3::JointIndex & associateShoulder)
+ pinocchio::JointIndex & associateShoulder)
{
if (aWrist==0)
return;
// Find back the path from the shoulder to the left hand.
- se3::JointIndex Chest = getPinocchioRobot()->chest();
+ pinocchio::JointIndex Chest = getPinocchioRobot()->chest();
if (Chest==0)
return;
- const se3::JointIndex associatedWrist = aWrist;
+ const pinocchio::JointIndex associatedWrist = aWrist;
if (associatedWrist==0)
return;
- std::vector<se3::JointIndex> FromRootToJoint =
+ std::vector<pinocchio::JointIndex> FromRootToJoint =
getPinocchioRobot()->jointsBetween(Chest, associatedWrist);
associateShoulder = FromRootToJoint[0];
@@ -164,22 +164,22 @@ void ComAndFootRealizationByGeometry::
}
void ComAndFootRealizationByGeometry::
- InitializeMapForChest(se3::JointModelVector & ActuatedJoints)
+ InitializeMapForChest(pinocchio::JointModelVector & ActuatedJoints)
{
- se3::JointIndex Chest = getPinocchioRobot()->chest();
+ pinocchio::JointIndex Chest = getPinocchioRobot()->chest();
if (Chest==0)
return;
- std::vector<se3::JointIndex> /*FromRootToJoint2,*/FromRootToJoint;
+ std::vector<pinocchio::JointIndex> /*FromRootToJoint2,*/FromRootToJoint;
FromRootToJoint = getPinocchioRobot()->fromRootToIt(Chest);
// erase the 1rst element as it is the root
FromRootToJoint.erase (FromRootToJoint.begin());/*
- std::vector<se3::JointIndex>::iterator itJoint = FromRootToJoint.begin();
+ std::vector<pinocchio::JointIndex>::iterator itJoint = FromRootToJoint.begin();
bool startadding=false;
while(itJoint!=FromRootToJoint.end())
{
- std::vector<se3::JointIndex>::iterator current = itJoint;
+ std::vector<pinocchio::JointIndex>::iterator current = itJoint;
if (*current==Chest)
{
@@ -244,13 +244,13 @@ void ComAndFootRealizationByGeometry::
// to the VRML ID.
ODEBUG("Enter 5.0 ");
// Extract the indexes of the Right leg.
- se3::JointIndex waist = getPinocchioRobot()->waist();
+ pinocchio::JointIndex waist = getPinocchioRobot()->waist();
if (RightFoot->associatedAnkle==0)
LTHROW("No right ankle");
// Build global map.
- se3::JointModelVector & ActuatedJoints =
+ pinocchio::JointModelVector & ActuatedJoints =
getPinocchioRobot()->getActuatedJoints();
ODEBUG5("Size of ActuatedJoints"<<ActuatedJoints.size(),
"DebugDataStartingCOM.dat");
@@ -261,12 +261,12 @@ void ComAndFootRealizationByGeometry::
m_GlobalVRMLIDtoConfiguration.resize(ActuatedJoints.size()-2);
for(unsigned int i=0; i<m_GlobalVRMLIDtoConfiguration.size(); ++i)
{
- m_GlobalVRMLIDtoConfiguration[i] = se3::idx_q(ActuatedJoints[i+2]);
+ m_GlobalVRMLIDtoConfiguration[i] = pinocchio::idx_q(ActuatedJoints[i+2]);
}
// Build right and left leg map.
// Remove the first element
- std::vector<se3::JointIndex> FromRootToJoint =
+ std::vector<pinocchio::JointIndex> FromRootToJoint =
getPinocchioRobot()->jointsBetween(waist, RightFoot->associatedAnkle);
FromRootToJoint.erase(FromRootToJoint.begin()); // be careful with that line potential bug
InitializationMaps(FromRootToJoint,ActuatedJoints,
@@ -392,18 +392,18 @@ bool ComAndFootRealizationByGeometry::
Eigen::Vector3d &m_AnklePosition,
FootAbsolutePosition & InitFootPosition)
{
- se3::JointIndex AnkleJoint = aFoot->associatedAnkle;
- se3::SE3 lAnkleSE3 = getPinocchioRobot()->Data()->oMi[AnkleJoint] ;
+ pinocchio::JointIndex AnkleJoint = aFoot->associatedAnkle;
+ pinocchio::SE3 lAnkleSE3 = getPinocchioRobot()->Data()->oMi[AnkleJoint] ;
Eigen::Vector3d translation ;
translation << -m_AnklePosition[0], -m_AnklePosition[1], -m_AnklePosition[2];
- se3::SE3 FootTranslation = se3::SE3(Eigen::Matrix3d::Identity(),translation);
- se3::SE3 lFootSE3 = lAnkleSE3.act(FootTranslation);
+ pinocchio::SE3 FootTranslation = pinocchio::SE3(Eigen::Matrix3d::Identity(),translation);
+ pinocchio::SE3 lFootSE3 = lAnkleSE3.act(FootTranslation);
InitFootPosition.x = lFootSE3.translation()[0];
InitFootPosition.y = lFootSE3.translation()[1];
InitFootPosition.z = lFootSE3.translation()[2];
- se3::SE3 lAnkleInitSE3inv = getPinocchioRobot()
+ pinocchio::SE3 lAnkleInitSE3inv = getPinocchioRobot()
->DataInInitialePose()->oMi[AnkleJoint];
Eigen::Matrix3d normalizedRotation =
lFootSE3.rotation() * lAnkleInitSE3inv.rotation() ;
@@ -697,7 +697,7 @@ bool ComAndFootRealizationByGeometry::
Eigen::Vector3d Foot_Shift;
- se3::JointIndex Ankle = 0;
+ pinocchio::JointIndex Ankle = 0;
if (LeftOrRight==-1)
{
Foot_Shift=Foot_R*m_AnklePositionRight;
@@ -752,7 +752,7 @@ bool ComAndFootRealizationByGeometry::
BodyPose(3,3) = 1.0 ;
FootPose(3,3) = 1.0 ;
- se3::JointIndex Waist = getPinocchioRobot()->waist();
+ pinocchio::JointIndex Waist = getPinocchioRobot()->waist();
ODEBUG("Typeid of humanoid: " << typeid(getHumanoidDynamicRobot()).name() );
// Call specialized dynamics.
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.hh b/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
index eb4142d6..e5498475 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
@@ -325,8 +325,8 @@ namespace PatternGeneratorJRL
protected:
/*! \brief Initialization of internal maps of indexes */
- void InitializationMaps(std::vector<se3::JointIndex> &FromRootToFoot,
- se3::JointModelVector & ActuatedJoints,
+ void InitializationMaps(std::vector<pinocchio::JointIndex> &FromRootToFoot,
+ pinocchio::JointModelVector & ActuatedJoints,
std::vector<int> &IndexinConfiguration,
std::vector<int> &IndexinVelocity);
@@ -340,14 +340,14 @@ namespace PatternGeneratorJRL
\param[out] associateShoulder: The shoulder extracted from
the kinematic chain.
*/
- void InitializeMapsForAHand(se3::JointIndex aWrist,
- se3::JointModelVector & ActuatedJoints,
+ void InitializeMapsForAHand(pinocchio::JointIndex aWrist,
+ pinocchio::JointModelVector & ActuatedJoints,
vector<int> & IndexesInConfiguration,
vector<int> & IndexesInVelocity,
- se3::JointIndex & associateShoulder);
+ pinocchio::JointIndex & associateShoulder);
/*! Create the map of indexes for the shoulders and wrist */
- void InitializeMapForChest(se3::JointModelVector & ActuatedJoints);
+ void InitializeMapForChest(pinocchio::JointModelVector & ActuatedJoints);
/* Register methods. */
void RegisterMethods();
@@ -485,7 +485,7 @@ namespace PatternGeneratorJRL
Eigen::Vector3d m_COGInitialAnkles;
/*! Store the position of the left and right shoulders. */
- se3::JointIndex m_LeftShoulder, m_RightShoulder;
+ pinocchio::JointIndex m_LeftShoulder, m_RightShoulder;
bool ShiftFoot_ ;
};
diff --git a/src/MotionGeneration/StepOverPlanner.cpp b/src/MotionGeneration/StepOverPlanner.cpp
index 85d3a3bd..3604e1ad 100644
--- a/src/MotionGeneration/StepOverPlanner.cpp
+++ b/src/MotionGeneration/StepOverPlanner.cpp
@@ -44,14 +44,13 @@ StepOverPlanner::StepOverPlanner(ObstaclePar &ObstacleParameters,
m_PR = aPR;
// Get information specific to the humanoid.
- double lWidth,lHeight;
+ double lWidth;
Eigen::Vector3d AnklePosition;
if (m_PR!=0)
{
PRFoot * leftFoot = m_PR->leftFoot();
lWidth = leftFoot->soleWidth ;
- lHeight = leftFoot->soleHeight ;
AnklePosition = leftFoot->anklePosition ;
m_AnkleSoilDistance = AnklePosition[2];
m_tipToAnkle = lWidth-AnklePosition[0];
@@ -60,7 +59,7 @@ StepOverPlanner::StepOverPlanner(ObstaclePar &ObstacleParameters,
}
else
{
- lWidth = 0.2; lHeight=0.15;
+ lWidth = 0.2;
cerr << "WARNING: no object with humanoid specificities properly defined." << endl;
m_AnkleSoilDistance = 0.1;
m_tipToAnkle = 0.1;
@@ -354,7 +353,7 @@ void StepOverPlanner::DoubleSupportFeasibility()
double StepOverStepWidth;
double StepOverStepLenght, StepOverStepLenghtMin, StepOverStepLenghtMax;
double StepOverCOMHeight, StepOverCOMHeightMin, StepOverCOMHeightMax;
- double OrientationHipToObstacle, OrientationFeetToObstacle = 0.0, OmegaAngleFeet = 0.0;
+ double OrientationFeetToObstacle = 0.0, OmegaAngleFeet = 0.0;
//this is the factor determining aproximately the COM position due to preview control during double support
double DoubleSupportCOMPosFactor;
@@ -406,7 +405,6 @@ void StepOverPlanner::DoubleSupportFeasibility()
IncrementStepLenght = double ((StepOverStepLenghtMax - StepOverStepLenghtMin)/((EvaluationNumber)));
IncrementCOMHeight = double ((StepOverCOMHeightMax - StepOverCOMHeightMin)/((EvaluationNumber)));
- OrientationHipToObstacle = 0.0*M_PI/180.0;
/*! this angle can be used to extend the steplength during stepover but currently it is set to 0 convinience*/
@@ -763,16 +761,15 @@ void StepOverPlanner::PolyPlannerFirstStep(deque<FootAbsolutePosition> &aStepOve
double StepTime;
double StepLenght;
double Omega1,Omega2,OmegaImpact;
- double xOffset,zOffset;
- double Point1X, Point1Y, Point1Z;
- double Point2X, Point2Y, Point2Z;
+ double xOffset;
+ double Point1X, Point1Y=0.0,Point1Z;
+ double Point2X, Point2Y=0.0,Point2Z;
double Point3Z;
StepTime = aStepOverFootBuffer[m_StartDoubleSupp].time-aStepOverFootBuffer[m_StartStepOver].time;
StepLenght = aStepOverFootBuffer[m_StartDoubleSupp].x-aStepOverFootBuffer[m_StartStepOver].x;
xOffset=0.00;
- zOffset=0.0;
Omega1=0.0;
Omega2=0.0;
@@ -1076,7 +1073,7 @@ void StepOverPlanner::PolyPlannerSecondStep(deque<FootAbsolutePosition> &aStepOv
double StepTime;
double StepLenght;
double Omega1,Omega2,OmegaImpact;
- double xOffset,zOffset;
+ double xOffset;
double Point1X,Point1Y,Point1Z;
double Point2X,Point2Y,Point2Z;
double Point3Z;
@@ -1085,7 +1082,6 @@ void StepOverPlanner::PolyPlannerSecondStep(deque<FootAbsolutePosition> &aStepOv
StepLenght = aStepOverFootBuffer[m_EndStepOver].x-aStepOverFootBuffer[m_StartSecondStep].x;
xOffset=0.0;
- zOffset=0.0;
Omega1=120.0*m_ObstacleParameters.h; //in degrees
Omega2=120.0*m_ObstacleParameters.h;
diff --git a/src/PreviewControl/PreviewControl.cpp b/src/PreviewControl/PreviewControl.cpp
index e4973a8f..e0a87314 100644
--- a/src/PreviewControl/PreviewControl.cpp
+++ b/src/PreviewControl/PreviewControl.cpp
@@ -463,7 +463,7 @@ int PreviewControl::OneIterationOfPreview1D(Eigen::MatrixXd &x,
for(unsigned int i=lindex;i<ZMPPositions.size();i++)
ux += m_F(i,0)* ZMPPositions[i];
- int StillToRealized = m_SizeOfPreviewWindow - (int)ZMPPositions.size()
+ int StillToRealized = (int)m_SizeOfPreviewWindow - (int)ZMPPositions.size()
+ (int)lindex;
for(unsigned int i=0;i<(unsigned int)StillToRealized ;i++)
ux += m_F(i,0)* ZMPPositions[i];
diff --git a/src/RobotDynamics/pinocchiorobot.cpp b/src/RobotDynamics/pinocchiorobot.cpp
index 92b77f04..69602506 100644
--- a/src/RobotDynamics/pinocchiorobot.cpp
+++ b/src/RobotDynamics/pinocchiorobot.cpp
@@ -8,13 +8,13 @@ class Joint_shortname : public boost::static_visitor<std::string>
{
public:
template<typename D>
- std::string operator()(const se3::JointModelBase<D> & jmodel) const
+ std::string operator()(const pinocchio::JointModelBase<D> & jmodel) const
{ return jmodel.shortname(); }
- static std::string run( const se3::JointModelVariant & jmodel)
+ static std::string run( const pinocchio::JointModelVariant & jmodel)
{ return boost::apply_visitor( Joint_shortname(), jmodel ); }
};
-inline std::string shortname(const se3::JointModelVariant & jmodel)
+inline std::string shortname(const pinocchio::JointModelVariant & jmodel)
{ return Joint_shortname::run(jmodel); }
PinocchioRobot::PinocchioRobot()
@@ -86,7 +86,7 @@ PinocchioRobot::~PinocchioRobot()
}
}
-bool PinocchioRobot::checkModel(se3::Model * robotModel)
+bool PinocchioRobot::checkModel(pinocchio::Model * robotModel)
{
if(!robotModel->existFrame("r_ankle"))
{
@@ -132,8 +132,8 @@ bool PinocchioRobot::checkModel(se3::Model * robotModel)
return true ;
}
-bool PinocchioRobot::initializeRobotModelAndData(se3::Model * robotModel,
- se3::Data * robotData)
+bool PinocchioRobot::initializeRobotModelAndData(pinocchio::Model * robotModel,
+ pinocchio::Data * robotData)
{
m_boolModel=checkModel(robotModel);
if(!m_boolModel)
@@ -144,24 +144,24 @@ bool PinocchioRobot::initializeRobotModelAndData(se3::Model * robotModel,
m_robotModel = robotModel;
// initialize the short cut for the joint ids
- se3::FrameIndex chest = m_robotModel->getFrameId("torso");
+ pinocchio::FrameIndex chest = m_robotModel->getFrameId("torso");
m_chest = m_robotModel->frames[chest].parent ;
- se3::FrameIndex waist = (robotModel->existFrame("BODY"))?m_robotModel->getFrameId("BODY"):m_robotModel->getFrameId("body");
+ pinocchio::FrameIndex waist = (robotModel->existFrame("BODY"))?m_robotModel->getFrameId("BODY"):m_robotModel->getFrameId("body");
m_waist = m_robotModel->frames[waist].parent ;
- se3::FrameIndex ra = m_robotModel->getFrameId("r_ankle");
+ pinocchio::FrameIndex ra = m_robotModel->getFrameId("r_ankle");
m_rightFoot.associatedAnkle = m_robotModel->frames[ra].parent ;
- se3::FrameIndex la = m_robotModel->getFrameId("l_ankle");
+ pinocchio::FrameIndex la = m_robotModel->getFrameId("l_ankle");
m_leftFoot.associatedAnkle = m_robotModel->frames[la].parent ;
- se3::FrameIndex rw = m_robotModel->getFrameId("r_wrist");
+ pinocchio::FrameIndex rw = m_robotModel->getFrameId("r_wrist");
m_rightWrist = m_robotModel->frames[rw].parent ;
- se3::FrameIndex lw = m_robotModel->getFrameId("l_wrist");
+ pinocchio::FrameIndex lw = m_robotModel->getFrameId("l_wrist");
m_leftWrist = m_robotModel->frames[lw].parent ;
DetectAutomaticallyShoulders();
// intialize the "initial pose" (q=[0]) data
- m_robotDataInInitialePose = new se3::Data(*m_robotModel);
- m_robotDataInInitialePose->v[0] = se3::Motion::Zero();
+ m_robotDataInInitialePose = new pinocchio::Data(*m_robotModel);
+ m_robotDataInInitialePose->v[0] = pinocchio::Motion::Zero();
m_robotDataInInitialePose->a[0] = -m_robotModel->gravity;
m_q.resize(m_robotModel->nq,1);
m_q.fill(0.0);
@@ -169,7 +169,7 @@ bool PinocchioRobot::initializeRobotModelAndData(se3::Model * robotModel,
m_v.resize(m_robotModel->nv,1);
m_a.resize(m_robotModel->nv,1);
m_tau.resize(m_robotModel->nv,1);
- se3::forwardKinematics(*m_robotModel,*m_robotDataInInitialePose,m_q);
+ pinocchio::forwardKinematics(*m_robotModel,*m_robotDataInInitialePose,m_q);
m_qmal.resize(m_robotModel->nv);
m_vmal.resize(m_robotModel->nv);
@@ -195,7 +195,7 @@ bool PinocchioRobot::initializeRobotModelAndData(se3::Model * robotModel,
else
m_boolData=true;
m_robotData = robotData;
- m_robotData->v[0] = se3::Motion::Zero();
+ m_robotData->v[0] = pinocchio::Motion::Zero();
m_robotData->a[0] = -m_robotModel->gravity;
if(testInverseKinematics())
@@ -220,13 +220,13 @@ bool PinocchioRobot::initializeRightFoot(PRFoot rightFoot)
bool PinocchioRobot::testInverseKinematics()
{
- std::vector<se3::JointIndex> leftLeg =
+ std::vector<pinocchio::JointIndex> leftLeg =
jointsBetween(m_waist,m_leftFoot.associatedAnkle);
- std::vector<se3::JointIndex> rightLeg =
+ std::vector<pinocchio::JointIndex> rightLeg =
jointsBetween(m_waist,m_rightFoot.associatedAnkle);
- std::vector<se3::JointIndex> leftArm =
+ std::vector<pinocchio::JointIndex> leftArm =
jointsBetween(m_chest,m_leftWrist);
- std::vector<se3::JointIndex> rightArm =
+ std::vector<pinocchio::JointIndex> rightArm =
jointsBetween(m_chest,m_rightWrist);
std::vector<std::string> leftLegJointName,rightLegJointName,
@@ -292,14 +292,14 @@ bool PinocchioRobot::testInverseKinematics()
void PinocchioRobot::initializeInverseKinematics()
{
- std::vector<se3::JointIndex> leftLeg =
+ std::vector<pinocchio::JointIndex> leftLeg =
jointsBetween(m_waist,m_leftFoot.associatedAnkle);
- std::vector<se3::JointIndex> rightLeg =
+ std::vector<pinocchio::JointIndex> rightLeg =
jointsBetween(m_waist,m_rightFoot.associatedAnkle);
m_leftDt.Zero();
m_rightDt.Zero();
- se3::SE3 waist_M_leftHip , waist_M_rightHip ;
+ pinocchio::SE3 waist_M_leftHip , waist_M_rightHip ;
waist_M_leftHip = m_robotModel->jointPlacements[leftLeg[0]].act(
m_robotModel->jointPlacements[leftLeg[1]]).act(
m_robotModel->jointPlacements[leftLeg[2]]).act(
@@ -342,9 +342,9 @@ void PinocchioRobot::RPYToSpatialFreeFlyer(Eigen::Vector3d & rpy,
Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()) ) ;
quat.normalize();
- double c0,s0; se3::SINCOS (rpy(2), &s0, &c0);
- double c1,s1; se3::SINCOS (rpy(1), &s1, &c1);
- double c2,s2; se3::SINCOS (rpy(0), &s2, &c2);
+ double c0,s0; pinocchio::SINCOS (rpy(2), &s0, &c0);
+ double c1,s1; pinocchio::SINCOS (rpy(1), &s1, &c1);
+ double c2,s2; pinocchio::SINCOS (rpy(0), &s2, &c2);
m_S << -s1, 0., 1., c1 * s2, c2, 0, c1 * c2, -s2, 0;
omega = m_S * drpy ;
domega = m_S * ddrpy ;
@@ -379,8 +379,8 @@ void PinocchioRobot::computeForwardKinematics(Eigen::VectorXd & q)
{
m_q(7+i) = q(6+i);
}
- se3::forwardKinematics(*m_robotModel,*m_robotData,m_q);
- se3::centerOfMass(*m_robotModel,*m_robotData,m_q);
+ pinocchio::forwardKinematics(*m_robotModel,*m_robotData,m_q);
+ pinocchio::centerOfMass(*m_robotModel,*m_robotData,m_q);
}
void PinocchioRobot::computeInverseDynamics()
@@ -432,14 +432,14 @@ void PinocchioRobot::computeInverseDynamics(Eigen::VectorXd & q,
m_a(i) = a(i);
}
// performing the inverse dynamics
- m_tau = se3::rnea(*m_robotModel,*m_robotData,m_q,m_v,m_a);
+ m_tau = pinocchio::rnea(*m_robotModel,*m_robotData,m_q,m_v,m_a);
}
-std::vector<se3::JointIndex> PinocchioRobot::fromRootToIt(se3::JointIndex it)
+std::vector<pinocchio::JointIndex> PinocchioRobot::fromRootToIt(pinocchio::JointIndex it)
{
- std::vector<se3::JointIndex> fromRootToIt ;
+ std::vector<pinocchio::JointIndex> fromRootToIt ;
fromRootToIt.clear();
- se3::JointIndex i = it ;
+ pinocchio::JointIndex i = it ;
while(i!=0)
{
fromRootToIt.insert(fromRootToIt.begin(),i);
@@ -448,16 +448,16 @@ std::vector<se3::JointIndex> PinocchioRobot::fromRootToIt(se3::JointIndex it)
return fromRootToIt ;
}
-std::vector<se3::JointIndex> PinocchioRobot::jointsBetween
-( se3::JointIndex first, se3::JointIndex second)
+std::vector<pinocchio::JointIndex> PinocchioRobot::jointsBetween
+( pinocchio::JointIndex first, pinocchio::JointIndex second)
{
- std::vector<se3::JointIndex> fromRootToFirst = fromRootToIt(first);
- std::vector<se3::JointIndex> fromRootToSecond = fromRootToIt(second);
+ std::vector<pinocchio::JointIndex> fromRootToFirst = fromRootToIt(first);
+ std::vector<pinocchio::JointIndex> fromRootToSecond = fromRootToIt(second);
- std::vector<se3::JointIndex> out ;
+ std::vector<pinocchio::JointIndex> out ;
out.clear();
- se3::JointIndex lastCommonRank = 0 ;
- se3::JointIndex minChainLength =
+ pinocchio::JointIndex lastCommonRank = 0 ;
+ pinocchio::JointIndex minChainLength =
fromRootToFirst.size() < fromRootToSecond.size()
? fromRootToFirst.size() : fromRootToSecond.size() ;
@@ -467,7 +467,7 @@ std::vector<se3::JointIndex> PinocchioRobot::jointsBetween
++lastCommonRank;
}
- for(std::vector<se3::JointIndex>::size_type k=fromRootToFirst.size()-1;
+ for(std::vector<pinocchio::JointIndex>::size_type k=fromRootToFirst.size()-1;
k>lastCommonRank ; --k)
{
out.push_back(fromRootToFirst[k]);
@@ -476,7 +476,7 @@ std::vector<se3::JointIndex> PinocchioRobot::jointsBetween
{
out.push_back(fromRootToSecond[0]);
}
- for(se3::JointIndex k=lastCommonRank+1 ; k<fromRootToSecond.size() ; ++k)
+ for(pinocchio::JointIndex k=lastCommonRank+1 ; k<fromRootToSecond.size() ; ++k)
{
out.push_back(fromRootToSecond[k]);
}
@@ -487,8 +487,8 @@ std::vector<se3::JointIndex> PinocchioRobot::jointsBetween
///////////////////////////////////////////////////////////////////////////////
bool PinocchioRobot::
ComputeSpecializedInverseKinematics(
- const se3::JointIndex &jointRoot,
- const se3::JointIndex &jointEnd,
+ const pinocchio::JointIndex &jointRoot,
+ const pinocchio::JointIndex &jointEnd,
const Eigen::Matrix4d & jointRootPosition,
const Eigen::Matrix4d & jointEndPosition,
Eigen::VectorXd &q )
@@ -738,19 +738,19 @@ void PinocchioRobot::DetectAutomaticallyShoulders()
}
void PinocchioRobot::DetectAutomaticallyOneShoulder(
- se3::JointIndex aWrist,
- se3::JointIndex & aShoulder)
+ pinocchio::JointIndex aWrist,
+ pinocchio::JointIndex & aShoulder)
{
- std::vector<se3::JointIndex>FromRootToJoint;
+ std::vector<pinocchio::JointIndex>FromRootToJoint;
FromRootToJoint.clear();
FromRootToJoint = fromRootToIt(aWrist);
- std::vector<se3::JointIndex>::iterator itJoint = FromRootToJoint.begin();
+ std::vector<pinocchio::JointIndex>::iterator itJoint = FromRootToJoint.begin();
bool found=false;
while(itJoint!=FromRootToJoint.end())
{
- std::vector<se3::JointIndex>::iterator current = itJoint;
+ std::vector<pinocchio::JointIndex>::iterator current = itJoint;
if (*current==m_chest)
found=true;
else
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index 928ac976..5e3e72d4 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -2771,8 +2771,6 @@ new step has to be generate.
double absFootx_1 = m_AbsoluteSupportFootPositions[Index-1].x ;
double absFooty_0 = m_AbsoluteSupportFootPositions[Index].y ;
double absFooty_1 = m_AbsoluteSupportFootPositions[Index-1].y ;
- double dx = absFootx_0 - absFootx_1 ;
- double dy = absFooty_0 - absFooty_1 ;
// climbing
// put first leg on the stairs with decrease of CoM //up// of stair height
@@ -2862,14 +2860,14 @@ new step has to be generate.
}
}
-// cout << "relative position : "
-// << sx << " "
-// << sy << " "
-// << dx << " "
-// << dy << " "
-// << SStime << " "
-// << m_AbsoluteSupportFootPositions[Index].time << " "
-// << m_AbsoluteSupportFootPositions[Index-1].time << " " << endl ;
+ // cout << "relative position : "
+ // << sx << " "
+ // << sy << " "
+ // << dx << " "
+ // << dy << " "
+ // << SStime << " "
+ // << m_AbsoluteSupportFootPositions[Index].time << " "
+ // << m_AbsoluteSupportFootPositions[Index-1].time << " " << endl ;
InitPos = LastCoM.z[0];
InitSpeed = LastCoM.z[1];
InitAcc = LastCoM.z[2];
diff --git a/tests/TestNaveau2015.cpp b/tests/TestNaveau2015.cpp
index c5fb9fa4..bb81293c 100644
--- a/tests/TestNaveau2015.cpp
+++ b/tests/TestNaveau2015.cpp
@@ -61,12 +61,12 @@ private:
Eigen::VectorXd m_vel ;
Eigen::VectorXd m_acc ;
int iteration;
- std::vector<se3::JointIndex> m_leftLeg ;
- std::vector<se3::JointIndex> m_rightLeg ;
- std::vector<se3::JointIndex> m_leftArm ;
- std::vector<se3::JointIndex> m_rightArm ;
- se3::JointIndex m_leftGripper ;
- se3::JointIndex m_rightGripper ;
+ std::vector<pinocchio::JointIndex> m_leftLeg ;
+ std::vector<pinocchio::JointIndex> m_rightLeg ;
+ std::vector<pinocchio::JointIndex> m_leftArm ;
+ std::vector<pinocchio::JointIndex> m_rightArm ;
+ pinocchio::JointIndex m_leftGripper ;
+ pinocchio::JointIndex m_rightGripper ;
public:
TestNaveau2015(int argc, char *argv[], string &aString, int TestProfile):
@@ -166,15 +166,15 @@ public:
m_leftLeg.erase( m_leftLeg.begin() );
m_rightLeg.erase( m_rightLeg.begin() );
- se3::JointModelVector & ActuatedJoints = m_PR->getActuatedJoints();
+ pinocchio::JointModelVector & ActuatedJoints = m_PR->getActuatedJoints();
for(unsigned i=0 ; i <m_leftLeg.size() ; ++i)
- m_leftLeg[i] = se3::idx_q(ActuatedJoints[m_leftLeg[i]])-1;
+ m_leftLeg[i] = pinocchio::idx_q(ActuatedJoints[m_leftLeg[i]])-1;
for(unsigned i=0 ; i <m_rightLeg.size() ; ++i)
- m_rightLeg[i] = se3::idx_q(ActuatedJoints[m_rightLeg[i]])-1;
+ m_rightLeg[i] = pinocchio::idx_q(ActuatedJoints[m_rightLeg[i]])-1;
for(unsigned i=0 ; i <m_leftArm.size() ; ++i)
- m_leftArm[i] = se3::idx_q(ActuatedJoints[m_leftArm[i]])-1;
+ m_leftArm[i] = pinocchio::idx_q(ActuatedJoints[m_leftArm[i]])-1;
for(unsigned i=0 ; i <m_rightArm.size() ; ++i)
- m_rightArm[i] = se3::idx_q(ActuatedJoints[m_rightArm[i]])-1;
+ m_rightArm[i] = pinocchio::idx_q(ActuatedJoints[m_rightArm[i]])-1;
if((m_robotModel.parents.size() >= m_rightArm.back()+1) &&
m_robotModel.parents[m_rightArm.back()+1] == m_rightArm.back())
diff --git a/tests/TestObject.cpp b/tests/TestObject.cpp
index b325e7ec..264b08d5 100644
--- a/tests/TestObject.cpp
+++ b/tests/TestObject.cpp
@@ -260,15 +260,15 @@ namespace PatternGeneratorJRL
m_leftLeg.erase( m_leftLeg.begin() );
m_rightLeg.erase( m_rightLeg.begin() );
- se3::JointModelVector & ActuatedJoints = m_PR->getActuatedJoints();
+ pinocchio::JointModelVector & ActuatedJoints = m_PR->getActuatedJoints();
for(unsigned i=0 ; i <m_leftLeg.size() ; ++i)
- m_leftLeg[i] = se3::idx_q(ActuatedJoints[m_leftLeg[i]])-1;
+ m_leftLeg[i] = pinocchio::idx_q(ActuatedJoints[m_leftLeg[i]])-1;
for(unsigned i=0 ; i <m_rightLeg.size() ; ++i)
- m_rightLeg[i] = se3::idx_q(ActuatedJoints[m_rightLeg[i]])-1;
+ m_rightLeg[i] = pinocchio::idx_q(ActuatedJoints[m_rightLeg[i]])-1;
for(unsigned i=0 ; i <m_leftArm.size() ; ++i)
- m_leftArm[i] = se3::idx_q(ActuatedJoints[m_leftArm[i]])-1;
+ m_leftArm[i] = pinocchio::idx_q(ActuatedJoints[m_leftArm[i]])-1;
for(unsigned i=0 ; i <m_rightArm.size() ; ++i)
- m_rightArm[i] = se3::idx_q(ActuatedJoints[m_rightArm[i]])-1;
+ m_rightArm[i] = pinocchio::idx_q(ActuatedJoints[m_rightArm[i]])-1;
if((m_robotModel.parents.size() >= m_rightArm.back()+1) &&
m_robotModel.parents[m_rightArm.back()+1] == m_rightArm.back())
@@ -292,11 +292,11 @@ namespace PatternGeneratorJRL
{
// Creating the humanoid robot via the URDF.
// try{
- se3::urdf::buildModel(URDFFile,
- se3::JointModelFreeFlyer(),
+ pinocchio::urdf::buildModel(URDFFile,
+ pinocchio::JointModelFreeFlyer(),
m_robotModel);
- m_robotData = new se3::Data(m_robotModel) ;
- m_DebugRobotData = new se3::Data(m_robotModel) ;
+ m_robotData = new pinocchio::Data(m_robotModel) ;
+ m_DebugRobotData = new pinocchio::Data(m_robotModel) ;
// }catch(std::invalid_argument e)
// {
// cout << e.what() ;
@@ -352,7 +352,7 @@ namespace PatternGeneratorJRL
m_HalfSitting.resize(aPR.numberDof()-6);
m_HalfSitting.setZero();
- se3::Model * aModel = aPR.Model();
+ pinocchio::Model * aModel = aPR.Model();
BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot.group_state"))
{
if(v.first=="joint")
@@ -363,8 +363,8 @@ namespace PatternGeneratorJRL
v.second.get<double>("<xmlattr>.value");
if(aModel->existJointName(jointName))
{
- se3::JointIndex id = aModel->getJointId(jointName);
- unsigned idq = se3::idx_q(aModel->joints[id]);
+ pinocchio::JointIndex id = aModel->getJointId(jointName);
+ unsigned idq = pinocchio::idx_q(aModel->joints[id]);
// we assume only revolute joint here.
m_HalfSitting(idq-7) = jointValue ;
}
@@ -405,7 +405,7 @@ namespace PatternGeneratorJRL
aFoot.anklePosition(1) = v.second.get<double>("y");
aFoot.anklePosition(2) = v.second.get<double>("z");
}
- se3::FrameIndex ra = aModel->getFrameId("r_ankle");
+ pinocchio::FrameIndex ra = aModel->getFrameId("r_ankle");
aFoot.associatedAnkle = aModel->frames[ra].parent ;
aPR.initializeRightFoot(aFoot);
@@ -424,7 +424,7 @@ namespace PatternGeneratorJRL
aFoot.anklePosition(1) = v.second.get<double>("y");
aFoot.anklePosition(2) = v.second.get<double>("z");
}
- se3::FrameIndex la = aModel->getFrameId("l_ankle");
+ pinocchio::FrameIndex la = aModel->getFrameId("l_ankle");
aFoot.associatedAnkle = aModel->frames[la].parent ;
aPR.initializeLeftFoot(aFoot);
@@ -437,8 +437,8 @@ namespace PatternGeneratorJRL
std::cerr << "Found mapURDFToOpenHRP"<< std::endl;
const std::string jointName =
v.second.get<std::string>("<xmlattr>.name");
- se3::JointIndex id = aModel->getJointId(jointName);
- unsigned idq = se3::idx_q(aModel->joints[id]);
+ pinocchio::JointIndex id = aModel->getJointId(jointName);
+ unsigned idq = pinocchio::idx_q(aModel->joints[id]);
m_fromURDFToOpenHRP.push_back(idq-1);
}
lindex++;
diff --git a/tests/TestObject.hh b/tests/TestObject.hh
index d8fa5467..a910a192 100644
--- a/tests/TestObject.hh
+++ b/tests/TestObject.hh
@@ -150,25 +150,25 @@ namespace PatternGeneratorJRL
@{
*/
/*! \brief Abstract model of the humanoid robot considered */
- se3::Model m_robotModel ;
+ pinocchio::Model m_robotModel ;
/*! \brief Abstract model of the humanoid robot considered */
PinocchioRobot * m_PR ;
- se3::Data *m_robotData;
+ pinocchio::Data *m_robotData;
/*! \brief Abstract model of the humanoid robot for debugging purposes. */
PinocchioRobot * m_DebugPR ;
- se3::Data *m_DebugRobotData;
+ pinocchio::Data *m_DebugRobotData;
/*! \brief Indexes for left and right legs and arms. */
- std::vector<se3::JointIndex> m_leftLeg ;
- std::vector<se3::JointIndex> m_rightLeg ;
- std::vector<se3::JointIndex> m_leftArm ;
- std::vector<se3::JointIndex> m_rightArm ;
+ std::vector<pinocchio::JointIndex> m_leftLeg ;
+ std::vector<pinocchio::JointIndex> m_rightLeg ;
+ std::vector<pinocchio::JointIndex> m_leftArm ;
+ std::vector<pinocchio::JointIndex> m_rightArm ;
/*! \brief Indexes for left and right grippers. */
- se3::JointIndex m_leftGripper ;
- se3::JointIndex m_rightGripper ;
+ pinocchio::JointIndex m_leftGripper ;
+ pinocchio::JointIndex m_rightGripper ;
/*! \brief From URDF to OpenHRP indexes. */
std::vector<unsigned int> m_fromURDFToOpenHRP;
--
GitLab