From 4e77eca4e8648cf814e51ce7d8b2750e2eaf9feb Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Fri, 23 Sep 2016 11:19:12 +0200
Subject: [PATCH] fix the initialization of the jointId in pinnochioRobot
---
src/RobotDynamics/pinocchiorobot.cpp | 31 +++++++++++++++++-----------
tests/TestObject.cpp | 2 +-
2 files changed, 20 insertions(+), 13 deletions(-)
diff --git a/src/RobotDynamics/pinocchiorobot.cpp b/src/RobotDynamics/pinocchiorobot.cpp
index 80f73be6..951e0b51 100644
--- a/src/RobotDynamics/pinocchiorobot.cpp
+++ b/src/RobotDynamics/pinocchiorobot.cpp
@@ -88,42 +88,42 @@ PinocchioRobot::~PinocchioRobot()
bool PinocchioRobot::checkModel(se3::Model * robotModel)
{
- if(!robotModel->existBodyName("r_ankle"))
+ if(!robotModel->existFrame("r_ankle"))
{
m_boolModel=false;
const std::string exception_message ("r_ankle is not a valid body name");
throw std::invalid_argument(exception_message);
return false ;
}
- if(!robotModel->existBodyName("l_ankle"))
+ if(!robotModel->existFrame("l_ankle"))
{
m_boolModel=false;
const std::string exception_message ("l_ankle is not a valid body name");
throw std::invalid_argument(exception_message);
return false ;
}
- if(!robotModel->existBodyName("BODY"))
+ if(!robotModel->existFrame("BODY"))
{
m_boolModel=false;
const std::string exception_message ("BODY is not a valid body name");
throw std::invalid_argument(exception_message);
return false ;
}
- if(!robotModel->existBodyName("torso"))
+ if(!robotModel->existFrame("torso"))
{
m_boolModel=false;
const std::string exception_message ("torso is not a valid body name");
throw std::invalid_argument(exception_message);
return false ;
}
- if(!robotModel->existBodyName("r_wrist"))
+ if(!robotModel->existFrame("r_wrist"))
{
m_boolModel=false;
const std::string exception_message ("r_wrist is not a valid body name");
throw std::invalid_argument(exception_message);
return false ;
}
- if(!robotModel->existBodyName("l_wrist"))
+ if(!robotModel->existFrame("l_wrist"))
{
const std::string exception_message ("l_wrist is not a valid body name");
throw std::invalid_argument(exception_message);
@@ -144,12 +144,19 @@ bool PinocchioRobot::initializeRobotModelAndData(se3::Model * robotModel,
m_robotModel = robotModel;
// initialize the short cut for the joint ids
- m_chest = m_robotModel->getFrameId("torso");
- m_waist = m_robotModel->getFrameId("BODY");
- m_leftFoot.associatedAnkle = m_robotModel->getFrameId("l_ankle");
- m_rightFoot.associatedAnkle = m_robotModel->getFrameId("r_ankle");
- m_leftWrist = m_robotModel->getFrameId("l_wrist");
- m_rightWrist = m_robotModel->getFrameId("r_wrist");
+ se3::FrameIndex chest = m_robotModel->getFrameId("torso");
+ m_chest = m_robotModel->frames[chest].parent ;
+ se3::FrameIndex waist = m_robotModel->getFrameId("BODY");
+ m_waist = m_robotModel->frames[waist].parent ;
+ se3::FrameIndex ra = m_robotModel->getFrameId("r_ankle");
+ m_rightFoot.associatedAnkle = m_robotModel->frames[ra].parent ;
+ se3::FrameIndex la = m_robotModel->getFrameId("l_ankle");
+ m_leftFoot.associatedAnkle = m_robotModel->frames[la].parent ;
+ se3::FrameIndex rw = m_robotModel->getFrameId("r_wrist");
+ m_rightWrist = m_robotModel->frames[rw].parent ;
+ se3::FrameIndex lw = m_robotModel->getFrameId("l_wrist");
+ m_leftWrist = m_robotModel->frames[lw].parent ;
+
DetectAutomaticallyShoulders();
// intialize the "initial pose" (q=[0]) data
diff --git a/tests/TestObject.cpp b/tests/TestObject.cpp
index dcfe4fff..4000de45 100644
--- a/tests/TestObject.cpp
+++ b/tests/TestObject.cpp
@@ -326,7 +326,7 @@ namespace PatternGeneratorJRL
aFoot.anklePosition(1) = v.second.get<double>("y");
aFoot.anklePosition(2) = v.second.get<double>("z");
}
- se3::FrameIndex la = aModel->getBodyId("l_ankle");
+ se3::FrameIndex la = aModel->getFrameId("l_ankle");
aFoot.associatedAnkle = aModel->frames[la].parent ;
aPR.initializeLeftFoot(aFoot);
}
--
GitLab