bool srdf::Model::initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml) { clear(); if (!robot_xml || robot_xml->ValueStr() != "robot") { logError("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) logError("No name given for the robot."); else { name_ = std::string(name); boost::trim(name_); if (name_ != urdf_model.getName()) logError("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); loadLinkSphereApproximations(urdf_model, robot_xml); loadDisabledCollisions(urdf_model, robot_xml); loadPassiveJoints(urdf_model, robot_xml); return true; }
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) { logError("A pair of links needs to be specified to disable collisions"); continue; } DisabledCollision dc; dc.link1_ = boost::trim_copy(std::string(link1)); dc.link2_ = boost::trim_copy(std::string(link2)); if (!urdf_model.getLink(dc.link1_)) { logError("Link '%s' is not known to URDF. Cannot disable collisons.", link1); continue; } if (!urdf_model.getLink(dc.link2_)) { logError("Link '%s' is not known to URDF. Cannot disable collisons.", link2); continue; } const char *reason = c_xml->Attribute("reason"); if (reason) dc.reason_ = std::string(reason); disabled_collisions_.push_back(dc); } }
bool URDFtoRSG::rsgFromUrdfModel(const urdf::ModelInterface& robot_model) { std::cout << "Found a robot with root :" << robot_model.getRoot()->name << std::endl; // add all children for (size_t i = 0; i < robot_model.getRoot()->child_links.size(); i++) if (!addChildrenToRSG(robot_model.getRoot()->child_links[i], wm->getRootNodeId())) return false; return true; }
bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree) { tree = Tree(robot_model.getRoot()->name); // warn if root link has inertia. KDL does not support this if (robot_model.getRoot()->inertial) std::cout<<"The root link "<<robot_model.getRoot()->name.c_str()<<" has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF."<<std::endl; // ROS_WARN("The root link %s has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.", robot_model.getRoot()->name.c_str()); // add all children for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++) if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree)) return false; return true; }
void srdf::Model::loadPassiveJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml) { for (TiXmlElement* c_xml = robot_xml->FirstChildElement("passive_joint"); c_xml; c_xml = c_xml->NextSiblingElement("passive_joint")) { const char *name = c_xml->Attribute("name"); if (!name) { logError("No name specified for passive joint. Ignoring."); continue; } PassiveJoint pj; pj.name_ = boost::trim_copy(std::string(name)); // see if a virtual joint was marked as passive bool vjoint = false; for (std::size_t i = 0 ; !vjoint && i < virtual_joints_.size() ; ++i) if (virtual_joints_[i].name_ == pj.name_) vjoint = true; if (!vjoint && !urdf_model.getJoint(pj.name_)) { logError("Joint '%s' marked as passive is not known to the URDF. Ignoring.", name); continue; } passive_joints_.push_back(pj); } }
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"); const char *parent_group = eef_xml->Attribute("parent_group"); if (!ename) { logError("Name of end effector is not specified"); continue; } if (!gname) { logError("Group not specified for end effector '%s'", ename); continue; } EndEffector e; e.name_ = std::string(ename); boost::trim(e.name_); e.component_group_ = std::string(gname); boost::trim(e.component_group_); 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) { logError("End effector '%s' specified for group '%s', but that group is not known", ename, gname); continue; } if (!parent) { logError("Parent link not specified for end effector '%s'", ename); continue; } e.parent_link_ = std::string(parent); boost::trim(e.parent_link_); if (!urdf_model.getLink(e.parent_link_)) { logError("Link '%s' specified as parent for end effector '%s' is not known to the URDF", parent, ename); continue; } if (parent_group) { e.parent_group_ = std::string(parent_group); boost::trim(e.parent_group_); } end_effectors_.push_back(e); } }
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) { logError("Name of virtual joint is not specified"); continue; } if (!child) { logError("Child link of virtual joint is not specified"); continue; } if (!urdf_model.getLink(boost::trim_copy(std::string(child)))) { logError("Virtual joint does not attach to a link on the robot (link '%s' is not known)", child); continue; } if (!parent) { logError("Parent frame of virtual joint is not specified"); continue; } if (!type) { logError("Type of virtual joint is not specified"); continue; } VirtualJoint vj; vj.type_ = std::string(type); boost::trim(vj.type_); std::transform(vj.type_.begin(), vj.type_.end(), vj.type_.begin(), ::tolower); if (vj.type_ != "planar" && vj.type_ != "floating" && vj.type_ != "fixed") { logError("Unknown type of joint: '%s'. Assuming 'fixed' instead. Other known types are 'planar' and 'floating'.", type); vj.type_ = "fixed"; } vj.name_ = std::string(jname); boost::trim(vj.name_); vj.child_link_ = std::string(child); boost::trim(vj.child_link_); vj.parent_frame_ = std::string(parent); boost::trim(vj.parent_frame_); virtual_joints_.push_back(vj); } }
bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree, const bool consider_root_link_inertia) { if (consider_root_link_inertia) { //For giving a name to the root of KDL using the robot name, //as it is not used elsewhere in the KDL tree std::string fake_root_name = "__kdl_import__" + robot_model.getName()+"__fake_root__"; std::string fake_root_fixed_joint_name = "__kdl_import__" + robot_model.getName()+"__fake_root_fixed_joint__"; tree = Tree(fake_root_name); const urdf::ConstLinkPtr root = robot_model.getRoot(); // constructs the optional inertia RigidBodyInertia inert(0); if (root->inertial) inert = toKdl(root->inertial); // constructs the kdl joint Joint jnt = Joint(fake_root_fixed_joint_name, Joint::None); // construct the kdl segment Segment sgm(root->name, jnt, Frame::Identity(), inert); // add segment to tree tree.addSegment(sgm, fake_root_name); } else { tree = Tree(robot_model.getRoot()->name); // warn if root link has inertia. KDL does not support this if (robot_model.getRoot()->inertial) std::cerr << "The root link " << robot_model.getRoot()->name << " has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF." << std::endl; } // add all children for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++) if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree)) return false; return true; }
void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision ) { // clear out any data (properties, shapes, etc) from a previously loaded robot. clear(); // the root link is discovered below. Set to NULL until found. root_link_ = NULL; // Create properties for each link. { typedef std::map<std::string, boost::shared_ptr<urdf::Link> > M_NameToUrdfLink; M_NameToUrdfLink::const_iterator link_it = urdf.links_.begin(); M_NameToUrdfLink::const_iterator link_end = urdf.links_.end(); for( ; link_it != link_end; ++link_it ) { const boost::shared_ptr<const urdf::Link>& urdf_link = link_it->second; std::string parent_joint_name; if (urdf_link != urdf.getRoot() && urdf_link->parent_joint) { parent_joint_name = urdf_link->parent_joint->name; } RobotLink* link = new RobotLink(this, urdf_link, parent_joint_name, visual, collision, link_pointclouds_); if (urdf_link == urdf.getRoot()) { root_link_ = link; } links_[urdf_link->name] = link; } } // Create properties for each joint. { typedef std::map<std::string, boost::shared_ptr<urdf::Joint> > M_NameToUrdfJoint; M_NameToUrdfJoint::const_iterator joint_it = urdf.joints_.begin(); M_NameToUrdfJoint::const_iterator joint_end = urdf.joints_.end(); for( ; joint_it != joint_end; ++joint_it ) { const boost::shared_ptr<const urdf::Joint>& urdf_joint = joint_it->second; RobotJoint* joint = new RobotJoint(this, urdf_joint); joints_[urdf_joint->name] = joint; } } link_pointclouds_.syncToDevice(); // after all links have been created, we sync them to the GPU // finally create a KDL representation of the kinematic tree: if (!kdl_parser::treeFromUrdfModel(urdf, tree)){ LOGGING_ERROR_C(RobotLog, Robot, "Failed to extract kdl tree from xml robot description!" << endl); exit(-1); } LOGGING_INFO_C(RobotLog, Robot, "Constructed KDL tree has " << tree.getNrOfJoints() << " Joints and " << tree.getNrOfSegments() << " segments." << endl); }
void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision ) { link_tree_->hide(); // hide until loaded robot_loaded_ = false; // clear out any data (properties, shapes, etc) from a previously loaded robot. clear(); // the root link is discovered below. Set to NULL until found. root_link_ = NULL; // Create properties for each link. // Properties are not added to display until changedLinkTreeStyle() is called (below). { typedef std::map<std::string, boost::shared_ptr<urdf::Link> > M_NameToUrdfLink; M_NameToUrdfLink::const_iterator link_it = urdf.links_.begin(); M_NameToUrdfLink::const_iterator link_end = urdf.links_.end(); for( ; link_it != link_end; ++link_it ) { const boost::shared_ptr<const urdf::Link>& urdf_link = link_it->second; std::string parent_joint_name; if (urdf_link != urdf.getRoot() && urdf_link->parent_joint) { parent_joint_name = urdf_link->parent_joint->name; } RobotLink* link = link_factory_->createLink( this, urdf_link, parent_joint_name, visual, collision ); if (urdf_link == urdf.getRoot()) { root_link_ = link; } links_[urdf_link->name] = link; link->setRobotAlpha( alpha_ ); } } // Create properties for each joint. // Properties are not added to display until changedLinkTreeStyle() is called (below). { typedef std::map<std::string, boost::shared_ptr<urdf::Joint> > M_NameToUrdfJoint; M_NameToUrdfJoint::const_iterator joint_it = urdf.joints_.begin(); M_NameToUrdfJoint::const_iterator joint_end = urdf.joints_.end(); for( ; joint_it != joint_end; ++joint_it ) { const boost::shared_ptr<const urdf::Joint>& urdf_joint = joint_it->second; RobotJoint* joint = link_factory_->createJoint( this, urdf_joint ); joints_[urdf_joint->name] = joint; joint->setRobotAlpha( alpha_ ); } } // robot is now loaded robot_loaded_ = true; link_tree_->show(); // set the link tree style and add link/joint properties to rviz pane. setLinkTreeStyle(LinkTreeStyle(link_tree_style_->getOptionInt())); changedLinkTreeStyle(); // at startup the link tree is collapsed since it is large and not often needed. link_tree_->collapse(); setVisualVisible( isVisualVisible() ); setCollisionVisible( isCollisionVisible() ); }
bool treeToUrdfModel(const KDL::Tree& tree, const std::string & robot_name, urdf::ModelInterface& robot_model) { robot_model.clear(); robot_model.name_ = robot_name; //Add all links KDL::SegmentMap::iterator seg; KDL::SegmentMap segs; KDL::SegmentMap::const_iterator root_seg; root_seg = tree.getRootSegment(); segs = tree.getSegments(); for( seg = segs.begin(); seg != segs.end(); seg++ ) { if (robot_model.getLink(seg->first)) { std::cerr << "[ERR] link " << seg->first << " is not unique." << std::endl; robot_model.clear(); return false; } else { urdf::LinkPtr link; resetPtr(link, new urdf::Link); //add name link->name = seg->first; //insert link robot_model.links_.insert(make_pair(seg->first,link)); std::cerr << "[DEBUG] successfully added a new link " << link->name << std::endl; } //inserting joint //The fake root segment has no joint to add if( seg->first != root_seg->first ) { KDL::Joint jnt; jnt = GetTreeElementSegment(seg->second).getJoint(); if (robot_model.getJoint(jnt.getName())) { std::cerr << "[ERR] joint " << jnt.getName() << " is not unique." << std::endl; robot_model.clear(); return false; } else { urdf::JointPtr joint; urdf::LinkPtr link = robot_model.links_[seg->first]; //This variable will be set by toUrdf KDL::Frame H_new_old_successor; KDL::Frame H_new_old_predecessor = getH_new_old(GetTreeElementSegment(GetTreeElementParent(seg->second)->second)); urdf::resetPtr(joint, new urdf::Joint()); //convert joint *joint = toUrdf(jnt, GetTreeElementSegment(seg->second).getFrameToTip(),H_new_old_predecessor,H_new_old_successor); //insert parent joint->parent_link_name = GetTreeElementParent(seg->second)->first; //insert child joint->child_link_name = seg->first; //insert joint robot_model.joints_.insert(make_pair(seg->first,joint)); std::cerr << "[DEBUG] successfully added a new joint" << jnt.getName() << std::endl; //add inertial, taking in account an eventual change in the link frame resetPtr(link->inertial, new urdf::Inertial()); *(link->inertial) = toUrdf(H_new_old_successor * GetTreeElementSegment(seg->second).getInertia()); } } } // every link has children links and joints, but no parents, so we create a // local convenience data structure for keeping child->parent relations std::map<std::string, std::string> parent_link_tree; parent_link_tree.clear(); // building tree: name mapping //try //{ robot_model.initTree(parent_link_tree); //} /* catch(ParseError &e) { logError("Failed to build tree: %s", e.what()); robot_model.clear(); return false; }*/ // find the root link //try //{ robot_model.initRoot(parent_link_tree); //} /* catch(ParseError &e) { logError("Failed to find root link: %s", e.what()); robot_model.reset(); return false; } */ return true; }
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) { logError("Group name not specified"); continue; } Group g; g.name_ = std::string(gname); boost::trim(g.name_); // 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) { logError("Link name not specified"); continue; } std::string lname_str = boost::trim_copy(std::string(lname)); if (!urdf_model.getLink(lname_str)) { logError("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) { logError("Joint name not specified"); continue; } std::string jname_str = boost::trim_copy(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) { logError("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) { logError("Base link name not specified for chain"); continue; } if (!tip) { logError("Tip link name not specified for chain"); continue; } std::string base_str = boost::trim_copy(std::string(base)); std::string tip_str = boost::trim_copy(std::string(tip)); if (!urdf_model.getLink(base_str)) { logError("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)) { logError("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<const urdf::Link> l = urdf_model.getLink(tip_str); std::set<std::string> seen; while (!found && l) { seen.insert(l->name); if (l->name == base_str) found = true; else l = l->getParent(); } if (!found) { l = urdf_model.getLink(base_str); while (!found && l) { if (seen.find(l->name) != seen.end()) found = true; else l = l->getParent(); } } if (found) g.chains_.push_back(std::make_pair(base_str, tip_str)); else logError("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) { logError("Group name not specified when included as subgroup"); continue; } g.subgroups_.push_back(boost::trim_copy(std::string(sub))); } if (g.links_.empty() && g.joints_.empty() && g.chains_.empty() && g.subgroups_.empty()) logWarn("Group '%s' is empty.", gname); groups_.push_back(g); } // check the subgroups std::set<std::string> 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<Group> 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 logError("Group '%s' has unsatisfied subgroups", groups_[i].name_.c_str()); groups_.swap(correct); } }
void srdf::Model::loadLinkSphereApproximations(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml) { for (TiXmlElement* cslink_xml = robot_xml->FirstChildElement("link_sphere_approximation"); cslink_xml; cslink_xml = cslink_xml->NextSiblingElement("link_sphere_approximation")) { int non_0_radius_sphere_cnt = 0; const char *link_name = cslink_xml->Attribute("link"); if (!link_name) { logError("Name of link is not specified in link_collision_spheres"); continue; } LinkSpheres link_spheres; link_spheres.link_ = boost::trim_copy(std::string(link_name)); if (!urdf_model.getLink(link_spheres.link_)) { logError("Link '%s' is not known to URDF.", link_name); continue; } // get the spheres for this link int cnt = 0; for (TiXmlElement* sphere_xml = cslink_xml->FirstChildElement("sphere"); sphere_xml; sphere_xml = sphere_xml->NextSiblingElement("sphere"), cnt++) { const char *s_center = sphere_xml->Attribute("center"); const char *s_r = sphere_xml->Attribute("radius"); if (!s_center || !s_r) { logError("Link collision sphere %d for link '%s' does not have both center and radius.", cnt, link_name); continue; } Sphere sphere; try { std::stringstream center(s_center); center.exceptions(std::stringstream::failbit | std::stringstream::badbit); center >> sphere.center_x_ >> sphere.center_y_ >> sphere.center_z_; sphere.radius_ = boost::lexical_cast<double>(s_r); } catch (std::stringstream::failure &e) { logError("Link collision sphere %d for link '%s' has bad center attribute value.", cnt, link_name); continue; } catch (boost::bad_lexical_cast &e) { logError("Link collision sphere %d for link '%s' has bad radius attribute value.", cnt, link_name); continue; } // ignore radius==0 spheres unless there is only 1 of them // // NOTE: // - If a link has no sphere_approximation then one will be generated // automatically containing a single sphere which encloses the entire // collision geometry. Internally this is represented by not having // a link_sphere_approximations_ entry for this link. // - If a link has only spheres with radius 0 then it will not be // considered for collision detection. In this case the internal // representation is a single radius=0 sphere. // - If a link has at least one sphere with radius>0 then those spheres // are used. Any radius=0 spheres are eliminated. if (sphere.radius_ > std::numeric_limits<double>::epsilon()) { if (non_0_radius_sphere_cnt == 0) link_spheres.spheres_.clear(); link_spheres.spheres_.push_back(sphere); non_0_radius_sphere_cnt++; } else if (non_0_radius_sphere_cnt == 0) { sphere.center_x_ = 0.0; sphere.center_y_ = 0.0; sphere.center_z_ = 0.0; sphere.radius_ = 0.0; link_spheres.spheres_.clear(); link_spheres.spheres_.push_back(sphere); } } if (!link_spheres.spheres_.empty()) link_sphere_approximations_.push_back(link_spheres); } }
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) { logError("Name of group state is not specified"); continue; } if (!gname) { logError("Name of group for state '%s' is not specified", sname); continue; } GroupState gs; gs.name_ = boost::trim_copy(std::string(sname)); gs.group_ = boost::trim_copy(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) { logError("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) { logError("Joint name not specified in group state '%s'", sname); continue; } if (!jval) { logError("Joint name not specified for joint '%s' in group state '%s'", jname, sname); continue; } std::string jname_str = boost::trim_copy(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) { logError("Joint '%s' declared as part of group state '%s' is not known to the URDF", jname, sname); continue; } } try { std::string jval_str = std::string(jval); std::stringstream ss(jval_str); while (ss.good() && !ss.eof()) { std::string val; ss >> val >> std::ws; gs.joint_values_[jname_str].push_back(boost::lexical_cast<double>(val)); } } catch (boost::bad_lexical_cast &e) { logError("Unable to parse joint value '%s'", jval); } if (gs.joint_values_.empty()) logError("Unable to parse joint value ('%s') for joint '%s' in group state '%s'", jval, jname, sname); } group_states_.push_back(gs); } }