From 9fac5415423417718c300c94984fcc35d30fbd90 Mon Sep 17 00:00:00 2001 From: Ioan Sucan Date: Tue, 8 Nov 2011 18:27:46 +0000 Subject: [PATCH] add initial version of srdf (no tests yet) --- srdf/CMakeLists.txt | 26 +++ srdf/Makefile | 1 + srdf/include/srdf/model.h | 208 +++++++++++++++++ srdf/mainpage.dox | 13 ++ srdf/manifest.xml | 29 +++ srdf/src/model.cpp | 476 ++++++++++++++++++++++++++++++++++++++ 6 files changed, 753 insertions(+) create mode 100644 srdf/CMakeLists.txt create mode 100644 srdf/Makefile create mode 100644 srdf/include/srdf/model.h create mode 100644 srdf/mainpage.dox create mode 100644 srdf/manifest.xml create mode 100644 srdf/src/model.cpp diff --git a/srdf/CMakeLists.txt b/srdf/CMakeLists.txt new file mode 100644 index 0000000..098d672 --- /dev/null +++ b/srdf/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +set(ROS_BUILD_TYPE Debug) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +rosbuild_add_library(${PROJECT_NAME} src/model.cpp) +target_link_libraries(${PROJECT_NAME} tinyxml) diff --git a/srdf/Makefile b/srdf/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/srdf/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/srdf/include/srdf/model.h b/srdf/include/srdf/model.h new file mode 100644 index 0000000..86c9e37 --- /dev/null +++ b/srdf/include/srdf/model.h @@ -0,0 +1,208 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2011, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author Ioan Sucan */ + +#ifndef SRDF_MODEL_ +#define SRDF_MODEL_ + +#include +#include +#include +#include +#include +#include + +/// Main namespace +namespace srdf +{ + + /** \brief Representation of semantic information about the robot */ + class Model + { + public: + + Model(void) + { + } + + ~Model(void) + { + } + + /// \brief Load Model from TiXMLElement + bool initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *xml); + /// \brief Load Model from TiXMLDocument + bool initXml(const urdf::ModelInterface &urdf_model, TiXmlDocument *xml); + /// \brief Load Model given a filename + bool initFile(const urdf::ModelInterface &urdf_model, const std::string& filename); + /// \brief Load Model from a XML-string + bool initString(const urdf::ModelInterface &urdf_model, const std::string& xmlstring); + + /** \brief A group consists of a set of joints and the + corresponding descendant links. There are multiple ways to + specify a group. Directly specifying joints, links or + chains, or referring to other defined groups. */ + struct Group + { + /// The name of the group + std::string name_; + + /// Directly specified joints to be included in the + /// group. Descendent links should be implicitly + /// considered to be part of the group, although this + /// parsed does not add them to links_. The joints are + /// checked to be in the corresponding URDF. + std::vector joints_; + + /// Directly specified links to be included in the + /// group. Parent joints should be implicitly considered + /// to be part of the group. The links are checked to be + /// in the corresponding URDF. + std::vector links_; + + /// Specify a chain of links (and the implicit joints) to + /// be added to the group. Each chain is specified as a + /// pair of base link and tip link. It is checked that the + /// chain is indeed a chain in the specified URDF. + std::vector > chains_; + + /// It is sometimes convenient to refer to the content of + /// another group. A group can include the content of the + /// referenced groups + std::vector subgroups_; + }; + + /// In addition to the joints specified in the URDF it is + /// sometimes convenient to add special (virtual) joints. For + /// example, to connect the robot to the environment in a + /// meaningful way. + struct VirtualJoint + { + /// The name of the new joint + std::string name_; + + /// The type of this new joint. This can be "fixed" (0 DOF), "planar" (3 DOF: x,y,yaw) or "floating" (6DOF) + std::string type_; + + /// The transform applied by this joint to the robot model brings that model to a particular frame. + std::string parent_frame_; + + /// The link this joint applies to + std::string child_link_; + }; + + /// Representation of an end effector + struct EndEffector + { + /// The name of the end effector + std::string name_; + + /// The name of the link this end effector connects to + std::string parent_link_; + + /// The name of the group that includes the joints & links this end effector consists of + std::string component_group_; + }; + + /// A named state for a particular group + struct GroupState + { + /// The name of the state + std::string name_; + + /// The name of the group this state is specified for + std::string group_; + + /// The values of joints for this state. Each joint can have a value. We use a vector for the 'value' to support multi-DOF joints + std::map > joint_values_; + }; + + /// Get the name of this model + const std::string& getName(void) const + { + return name_; + } + + /// Get the list of pairs of links that need not be checked for collisions (because they can never touch given the geometry and kinematics of the robot) + const std::vector >& getDisabledCollisions(void) const + { + return disabled_collisions_; + } + + /// Get the list of groups defined for this model + const std::vector& getGroups(void) const + { + return groups_; + } + + /// Get the list of virtual joints defined for this model + const std::vector& getVirtualJoints(void) const + { + return virtual_joints_; + } + + /// Get the list of end effectors defined for this model + const std::vector& getEndEffectors(void) const + { + return end_effectors_; + } + + /// Get the list of group states defined for this model + const std::vector& getGroupStates(void) const + { + return group_states_; + } + + /// Clear the model + void clear(void); + + private: + + void loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml); + void loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml); + void loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml); + void loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml); + void loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml); + + std::string name_; + std::vector groups_; + std::vector group_states_; + std::vector virtual_joints_; + std::vector end_effectors_; + std::vector > disabled_collisions_; + }; + +} +#endif diff --git a/srdf/mainpage.dox b/srdf/mainpage.dox new file mode 100644 index 0000000..233eab3 --- /dev/null +++ b/srdf/mainpage.dox @@ -0,0 +1,13 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b srdf is a parser for SRDF (Semantic Robot Description Format) documents. +Please see http://www.ros.org/wiki/srdf for more details. + + + + +*/ diff --git a/srdf/manifest.xml b/srdf/manifest.xml new file mode 100644 index 0000000..029eff8 --- /dev/null +++ b/srdf/manifest.xml @@ -0,0 +1,29 @@ + + + + SRDF (Semantic Robot Description Format) is a representation of + semantic information about robots. + + + Ioan Sucan + BSD + + http://ros.org/wiki/srdf + + + + + + + + + + + + + + + + + + diff --git a/srdf/src/model.cpp b/srdf/src/model.cpp new file mode 100644 index 0000000..857a250 --- /dev/null +++ b/srdf/src/model.cpp @@ -0,0 +1,476 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2011, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author Ioan Sucan */ + +#include "srdf/model.h" +#include +#include +#include +#include + +void srdf::Model::loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml) +{ + for (TiXmlElement* vj_xml = robot_xml->FirstChildElement("virtual_joint"); vj_xml; vj_xml = vj_xml->NextSiblingElement("virtual_joint")) + { + const char *jname = vj_xml->Attribute("name"); + const char *child = vj_xml->Attribute("child_link"); + const char *parent = vj_xml->Attribute("parent_frame"); + const char *type = vj_xml->Attribute("type"); + if (!jname) + { + ROS_ERROR("Name of virtual joint is not specified"); + continue; + } + if (!child) + { + ROS_ERROR("Child link of virtual joint is not specified"); + continue; + } + if (!urdf_model.getLink(std::string(child))) + { + ROS_ERROR("Virtual joint does not attach to a link on the robot (link '%s' is not known)", child); + continue; + } + if (!parent) + { + ROS_ERROR("Parent frame of virtual joint is not specified"); + continue; + } + if (!type) + { + ROS_ERROR("Type of virtual joint is not specified"); + continue; + } + VirtualJoint vj; + vj.name_ = std::string(jname); + vj.child_link_ = std::string(child); + vj.parent_frame_ = std::string(parent); + vj.type_ = std::string(type); + virtual_joints_.push_back(vj); + } +} + +void srdf::Model::loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml) +{ + for (TiXmlElement* group_xml = robot_xml->FirstChildElement("group"); group_xml; group_xml = group_xml->NextSiblingElement("group")) + { + const char *gname = group_xml->Attribute("name"); + if (!gname) + { + ROS_ERROR("Group name not specified"); + continue; + } + Group g; + g.name_ = std::string(gname); + + // get the links in the groups + for (TiXmlElement* link_xml = group_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) + { + const char *lname = link_xml->Attribute("name"); + if (!lname) + { + ROS_ERROR("Link name not specified"); + continue; + } + std::string lname_str = std::string(lname); + if (!urdf_model.getLink(lname_str)) + { + ROS_ERROR("Link '%s' declared as part of group '%s' is not known to the URDF", lname, gname); + continue; + } + g.links_.push_back(lname_str); + } + + // get the joints in the groups + for (TiXmlElement* joint_xml = group_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) + { + const char *jname = joint_xml->Attribute("name"); + if (!jname) + { + ROS_ERROR("Joint name not specified"); + continue; + } + std::string jname_str = std::string(jname); + if (!urdf_model.getJoint(jname_str)) + { + bool missing = true; + for (std::size_t k = 0 ; k < virtual_joints_.size() ; ++k) + if (virtual_joints_[k].name_ == jname_str) + { + missing = false; + break; + } + if (missing) + { + ROS_ERROR("Joint '%s' declared as part of group '%s' is not known to the URDF", jname, gname); + continue; + } + } + g.joints_.push_back(jname_str); + } + + // get the chains in the groups + for (TiXmlElement* chain_xml = group_xml->FirstChildElement("chain"); chain_xml; chain_xml = chain_xml->NextSiblingElement("chain")) + { + const char *base = chain_xml->Attribute("base_link"); + const char *tip = chain_xml->Attribute("tip_link"); + if (!base) + { + ROS_ERROR("Base link name not specified for chain"); + continue; + } + if (!tip) + { + ROS_ERROR("Tip link name not specified for chain"); + continue; + } + std::string base_str = std::string(base); + std::string tip_str = std::string(tip); + if (!urdf_model.getLink(base_str)) + { + ROS_ERROR("Link '%s' declared as part of a chain in group '%s' is not known to the URDF", base, gname); + continue; + } + if (!urdf_model.getLink(tip_str)) + { + ROS_ERROR("Link '%s' declared as part of a chain in group '%s' is not known to the URDF", tip, gname); + continue; + } + bool found = false; + boost::shared_ptr l = urdf_model.getLink(tip_str); + while (!found && l) + { + if (l->name == base_str) + found = true; + else + l = l->getParent(); + } + if (found) + g.chains_.push_back(std::make_pair(base_str, tip_str)); + else + ROS_ERROR("Links '%s' and '%s' do not form a chain. Not included in group '%s'", base, tip, gname); + } + + // get the subgroups in the groups + for (TiXmlElement* subg_xml = group_xml->FirstChildElement("group"); subg_xml; subg_xml = subg_xml->NextSiblingElement("group")) + { + const char *sub = subg_xml->Attribute("name"); + if (!sub) + { + ROS_ERROR("Group name not specified when included as subgroup"); + continue; + } + g.subgroups_.push_back(std::string(sub)); + } + groups_.push_back(g); + } + + // check the subgroups + std::set known_groups; + bool update = true; + while (update) + { + update = false; + for (std::size_t i = 0 ; i < groups_.size() ; ++i) + { + if (known_groups.find(groups_[i].name_) != known_groups.end()) + continue; + if (groups_[i].subgroups_.empty()) + { + known_groups.insert(groups_[i].name_); + update = true; + } + else + { + bool ok = true; + for (std::size_t j = 0 ; ok && j < groups_[i].subgroups_.size() ; ++j) + if (known_groups.find(groups_[i].subgroups_[j]) == known_groups.end()) + ok = false; + if (ok) + { + known_groups.insert(groups_[i].name_); + update = true; + } + } + } + } + + // if there are erroneous groups, keep only the valid ones + if (known_groups.size() != groups_.size()) + { + std::vector correct; + for (std::size_t i = 0 ; i < groups_.size() ; ++i) + if (known_groups.find(groups_[i].name_) != known_groups.end()) + correct.push_back(groups_[i]); + else + ROS_ERROR("Group '%s' has unsatisfied subgroups", groups_[i].name_.c_str()); + groups_.swap(correct); + } +} + +void srdf::Model::loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml) +{ + for (TiXmlElement* gstate_xml = robot_xml->FirstChildElement("group_state"); gstate_xml; gstate_xml = gstate_xml->NextSiblingElement("group_state")) + { + const char *sname = gstate_xml->Attribute("name"); + const char *gname = gstate_xml->Attribute("group"); + if (!sname) + { + ROS_ERROR("Name of group state is not specified"); + continue; + } + if (!gname) + { + ROS_ERROR("Name of group for state '%s' is not specified", sname); + continue; + } + + GroupState gs; + gs.name_ = std::string(sname); + gs.group_ = std::string(gname); + + bool found = false; + for (std::size_t k = 0 ; k < groups_.size() ; ++k) + if (groups_[k].name_ == gs.group_) + { + found = true; + break; + } + if (!found) + { + ROS_ERROR("Group state '%s' specified for group '%s', but that group is not known", sname, gname); + continue; + } + + // get the joint values in the group state + for (TiXmlElement* joint_xml = gstate_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) + { + const char *jname = joint_xml->Attribute("name"); + const char *jval = joint_xml->Attribute("value"); + if (!jname) + { + ROS_ERROR("Joint name not specified in group state '%s'", sname); + continue; + } + if (!jval) + { + ROS_ERROR("Joint name not specified for joint '%s' in group state '%s'", jname, sname); + continue; + } + std::string jname_str = std::string(jname); + if (!urdf_model.getJoint(jname_str)) + { + bool missing = true; + for (std::size_t k = 0 ; k < virtual_joints_.size() ; ++k) + if (virtual_joints_[k].name_ == jname_str) + { + missing = false; + break; + } + if (missing) + { + ROS_ERROR("Joint '%s' declared as part of group state '%s' is not known to the URDF", jname, sname); + continue; + } + } + std::string jval_str = std::string(jval); + std::stringstream ss(jval_str); + while (!ss.eof()) + { + double val; ss >> val; + gs.joint_values_[jname_str].push_back(val); + } + if (gs.joint_values_.empty()) + ROS_ERROR("Unable to parse joint value ('%s') for joint '%s' in group state '%s'", jval, jname, sname); + } + group_states_.push_back(gs); + } +} + +void srdf::Model::loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml) +{ + for (TiXmlElement* eef_xml = robot_xml->FirstChildElement("end_effector"); eef_xml; eef_xml = eef_xml->NextSiblingElement("end_effector")) + { + const char *ename = eef_xml->Attribute("name"); + const char *gname = eef_xml->Attribute("group"); + const char *parent = eef_xml->Attribute("parent_link"); + if (!ename) + { + ROS_ERROR("Name of end effector is not specified"); + continue; + } + if (!gname) + { + ROS_ERROR("Group not specified for end effector '%s'", ename); + continue; + } + EndEffector e; + e.name_ = std::string(ename); + e.component_group_ = std::string(gname); + bool found = false; + for (std::size_t k = 0 ; k < groups_.size() ; ++k) + if (groups_[k].name_ == e.component_group_) + { + found = true; + break; + } + if (!found) + { + ROS_ERROR("End effector '%s' specified for group '%s', but that group is not known", ename, gname); + continue; + } + if (!parent) + { + ROS_ERROR("Parent link not specified for end effector '%s'", ename); + continue; + } + e.parent_link_ = std::string(parent); + if (!urdf_model.getLink(e.parent_link_)) + { + ROS_ERROR("Link '%s' specified as parent for end effector '%s' is not known to the URDF", parent, ename); + continue; + } + end_effectors_.push_back(e); + } +} + +void srdf::Model::loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml) +{ + for (TiXmlElement* c_xml = robot_xml->FirstChildElement("disable_collisions"); c_xml; c_xml = c_xml->NextSiblingElement("disable_collisions")) + { + const char *link1 = c_xml->Attribute("link1"); + const char *link2 = c_xml->Attribute("link2"); + if (!link1 || !link2) + { + ROS_ERROR("A pair of links needs to be specified to disable collisions"); + continue; + } + std::string link1_str = std::string(link1); + std::string link2_str = std::string(link2); + if (!urdf_model.getLink(link1_str)) + { + ROS_ERROR("Link '%s' is not known to URDF. Cannot disable collisons.", link1); + continue; + } + if (!urdf_model.getLink(link2_str)) + { + ROS_ERROR("Link '%s' is not known to URDF. Cannot disable collisons.", link2); + continue; + } + disabled_collisions_.push_back(std::make_pair(link1_str, link2_str)); + } +} + +bool srdf::Model::initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml) +{ + clear(); + if (!robot_xml || robot_xml->ValueStr() != "robot") + { + ROS_ERROR("Could not find the 'robot' element in the xml file"); + return false; + } + + // get the robot name + const char *name = robot_xml->Attribute("name"); + if (!name) + ROS_ERROR("No name given for the robot."); + else + { + name_ = std::string(name); + if (name_ != urdf_model.getName()) + ROS_ERROR("Semantic description is not specified for the same robot as the URDF"); + } + + loadVirtualJoints(urdf_model, robot_xml); + loadGroups(urdf_model, robot_xml); + loadGroupStates(urdf_model, robot_xml); + loadEndEffectors(urdf_model, robot_xml); + loadDisabledCollisions(urdf_model, robot_xml); + + return true; +} + +bool srdf::Model::initXml(const urdf::ModelInterface &urdf_model, TiXmlDocument *xml) +{ + TiXmlElement *robot_xml = xml ? xml->FirstChildElement("robot") : NULL; + if (!robot_xml) + { + ROS_ERROR("Could not find the 'robot' element in the xml file"); + return false; + } + return initXml(urdf_model, robot_xml); +} + + +bool srdf::Model::initFile(const urdf::ModelInterface &urdf_model, const std::string& filename) +{ + // get the entire file + std::string xml_string; + std::fstream xml_file(filename.c_str(), std::fstream::in); + if (xml_file.is_open()) + { + while (xml_file.good()) + { + std::string line; + std::getline(xml_file, line); + xml_string += (line + "\n"); + } + xml_file.close(); + return initString(urdf_model, xml_string); + } + else + { + ROS_ERROR("Could not open file [%s] for parsing.", filename.c_str()); + return false; + } +} + +bool srdf::Model::initString(const urdf::ModelInterface &urdf_model, const std::string& xmlstring) +{ + TiXmlDocument xml_doc; + xml_doc.Parse(xmlstring.c_str()); + return initXml(urdf_model, &xml_doc); +} + + +void srdf::Model::clear(void) +{ + name_ = ""; + groups_.clear(); + group_states_.clear(); + virtual_joints_.clear(); + end_effectors_.clear(); + disabled_collisions_.clear(); +}