From ca34d0cde0da72b5aca1284c2107bc4b71a07ab5 Mon Sep 17 00:00:00 2001
From: Rohan Budhiraja <budhiraja@laas.fr>
Date: Fri, 9 Sep 2016 12:34:59 +0200
Subject: [PATCH] [c++] body and BODY should both be excepted as valid names
---
src/RobotDynamics/pinocchiorobot.cpp | 15 ++++++++++++---
1 file changed, 12 insertions(+), 3 deletions(-)
diff --git a/src/RobotDynamics/pinocchiorobot.cpp b/src/RobotDynamics/pinocchiorobot.cpp
index 79625915..dcc0698d 100644
--- a/src/RobotDynamics/pinocchiorobot.cpp
+++ b/src/RobotDynamics/pinocchiorobot.cpp
@@ -102,10 +102,10 @@ bool PinocchioRobot::checkModel(se3::Model * robotModel)
throw std::invalid_argument(exception_message);
return false ;
}
- if(!robotModel->existBodyName("BODY"))
+ if(!robotModel->existBodyName("BODY") and !robotModel->existBodyName("body"))
{
m_boolModel=false;
- const std::string exception_message ("BODY is not a valid body name");
+ const std::string exception_message ("BODY/body is not a valid body name");
throw std::invalid_argument(exception_message);
return false ;
}
@@ -145,7 +145,16 @@ bool PinocchioRobot::initializeRobotModelAndData(se3::Model * robotModel,
// initialize the short cut for the joint ids
m_chest = m_robotModel->getFrameParent("torso");
- m_waist = m_robotModel->getFrameParent("BODY");
+ if(robotModel->existBodyName("BODY")) {
+ m_waist = m_robotModel->getFrameParent("BODY");
+ }
+ else if (robotModel->existBodyName("body")) {
+ m_waist = m_robotModel->getFrameParent("body");
+ }
+ else {
+ const std::string exception_message ("BODY/body not found. Check model.");
+ throw std::invalid_argument(exception_message);
+ }
m_leftFoot.associatedAnkle = m_robotModel->getFrameParent("l_ankle");
m_rightFoot.associatedAnkle = m_robotModel->getFrameParent("r_ankle");
m_leftWrist = m_robotModel->getFrameParent("l_wrist");
--
GitLab