diff --git a/robot_model/include/moveit/robot_model/robot_model.h b/robot_model/include/moveit/robot_model/robot_model.h index fee14157..fe0ba21b 100644 --- a/robot_model/include/moveit/robot_model/robot_model.h +++ b/robot_model/include/moveit/robot_model/robot_model.h @@ -130,10 +130,13 @@ class RobotModel /** \brief Check if a joint exists. Return true if it does. */ bool hasJointModel(const std::string &name) const; - /** \brief Get a joint by its name. Throw an exception when the joint is missing. */ + /** \brief Get a joint by its name. Output error and return NULL when the joint is missing. */ const JointModel* getJointModel(const std::string &joint) const; - /** \brief Get a joint by its name. Throw an exception when the joint is missing. */ + /** \brief Get a joint by its index. Output error and return NULL when the link is missing. */ + const JointModel* getJointModel(int index) const; + + /** \brief Get a joint by its name. Output error and return NULL when the joint is missing. */ JointModel* getJointModel(const std::string &joint); /** \brief Get the array of joints, in the order they appear @@ -228,10 +231,13 @@ class RobotModel /** \brief Check if a link exists. Return true if it does. */ bool hasLinkModel(const std::string &name) const; - /** \brief Get a link by its name. Throw an exception when the link is missing. */ + /** \brief Get a link by its name. Output error and return NULL when the link is missing. */ const LinkModel* getLinkModel(const std::string &link) const; - /** \brief Get a link by its name. Throw an exception when the link is missing. */ + /** \brief Get a link by its index. Output error and return NULL when the link is missing. */ + const LinkModel* getLinkModel(int index) const; + + /** \brief Get a link by its name. Output error and return NULL when the link is missing. */ LinkModel* getLinkModel(const std::string &link); /** \brief Get the array of links */ diff --git a/robot_model/src/robot_model.cpp b/robot_model/src/robot_model.cpp index 35ebf7c9..bcbed008 100644 --- a/robot_model/src/robot_model.cpp +++ b/robot_model/src/robot_model.cpp @@ -1053,6 +1053,17 @@ const moveit::core::JointModel* moveit::core::RobotModel::getJointModel(const st return NULL; } +const moveit::core::JointModel* moveit::core::RobotModel::getJointModel(int index) const +{ + if (index < 0 || index >= joint_model_vector_.size()) + { + logError("Joint index '%i' out of bounds of joints in model '%s'", index, model_name_.c_str()); + return NULL; + } + assert(joint_model_vector_[index]->getJointIndex() == index); + return joint_model_vector_[index]; +} + moveit::core::JointModel* moveit::core::RobotModel::getJointModel(const std::string &name) { JointModelMap::const_iterator it = joint_model_map_.find(name); @@ -1071,6 +1082,17 @@ const moveit::core::LinkModel* moveit::core::RobotModel::getLinkModel(const std: return NULL; } +const moveit::core::LinkModel* moveit::core::RobotModel::getLinkModel(int index) const +{ + if (index < 0 || index >= link_model_vector_.size()) + { + logError("Link index '%i' out of bounds of links in model '%s'", index, model_name_.c_str()); + return NULL; + } + assert(link_model_vector_[index]->getLinkIndex() == index); + return link_model_vector_[index]; +} + moveit::core::LinkModel* moveit::core::RobotModel::getLinkModel(const std::string &name) { LinkModelMap::const_iterator it = link_model_map_.find(name);