diff --git a/src/RobotDynamics/pinocchiorobot.cpp b/src/RobotDynamics/pinocchiorobot.cpp index 80f73be6120a48d77034c0ac4a481ad9fa33ed8e..951e0b513fc032288be86631d1a17f0855faab0b 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 dcfe4fffbc9ce6ef09200ff27d764f203f81b954..4000de4568c4f52358f9390f72398f1b58e4b707 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); }