39 using namespace tinyxml2;
44 std::string toString(
const T& t)
47 std::ostringstream oss;
48 oss.imbue(std::locale::classic());
59 SRDFWriter::SRDFWriter()
68 SRDFWriter::~SRDFWriter()
75 bool SRDFWriter::initString(
const urdf::ModelInterface& robot_model,
const std::string& srdf_string)
78 if (!srdf_model_->initString(robot_model, srdf_string))
84 initModel(robot_model, *srdf_model_);
92 void SRDFWriter::initModel(
const urdf::ModelInterface& robot_model,
const srdf::Model& srdf_model)
95 if (srdf_model_.get() != &srdf_model)
97 *srdf_model_ = srdf_model;
102 enabled_collision_pairs_ = srdf_model_->getEnabledCollisionPairs();
103 disabled_collision_pairs_ = srdf_model_->getDisabledCollisionPairs();
104 link_sphere_approximations_ = srdf_model_->getLinkSphereApproximations();
105 groups_ = srdf_model_->getGroups();
106 virtual_joints_ = srdf_model_->getVirtualJoints();
107 end_effectors_ = srdf_model_->getEndEffectors();
108 group_states_ = srdf_model_->getGroupStates();
109 passive_joints_ = srdf_model_->getPassiveJoints();
110 joint_properties_ = srdf_model_->getJointProperties();
113 robot_name_ = robot_model.getName();
119 void SRDFWriter::updateSRDFModel(
const urdf::ModelInterface& robot_model)
122 const std::string srdf_string = getSRDFString();
125 if (!srdf_model_->initString(robot_model, srdf_string))
127 throw std::runtime_error(
"Unable to update the SRDF Model");
134 bool SRDFWriter::writeSRDF(
const std::string& file_path)
137 XMLDocument document;
138 generateSRDF(document);
141 return document.SaveFile(file_path.c_str()) == XML_SUCCESS;
147 std::string SRDFWriter::getSRDFString()
150 XMLDocument document;
151 generateSRDF(document);
155 document.Accept(&printer);
158 return printer.CStr();
164 void SRDFWriter::generateSRDF(XMLDocument& document)
166 XMLDeclaration* decl = document.NewDeclaration();
167 document.InsertEndChild(decl);
170 XMLComment* comment = document.NewComment(
"This does not replace URDF, and is not an extension of URDF.\n "
171 "This is a format for representing semantic information about the robot "
173 "A URDF file must exist for this robot as well, "
174 "where the joints and the links that are referenced are defined\n");
175 document.InsertEndChild(comment);
178 XMLElement* robot_root = document.NewElement(
"robot");
179 robot_root->SetAttribute(
"name", robot_name_.c_str());
180 document.InsertEndChild(robot_root);
183 createGroupsXML(robot_root);
186 createGroupStatesXML(robot_root);
189 createEndEffectorsXML(robot_root);
192 createVirtualJointsXML(robot_root);
195 createPassiveJointsXML(robot_root);
198 createJointPropertiesXML(robot_root);
201 createLinkSphereApproximationsXML(robot_root);
204 createCollisionDefaultsXML(robot_root);
207 createDisabledCollisionPairsXML(robot_root);
213 void SRDFWriter::createGroupsXML(XMLElement* root)
215 XMLDocument* doc = root->GetDocument();
221 comment = doc->NewComment(
"GROUPS: Representation of a set of joints and links. This can be useful for specifying "
222 "DOF to plan for, defining arms, end effectors, etc");
223 root->InsertEndChild(comment);
224 comment = doc->NewComment(
"LINKS: When a link is specified, the parent joint of that link (if it exists) is "
225 "automatically included");
226 root->InsertEndChild(comment);
227 comment = doc->NewComment(
"JOINTS: When a joint is specified, the child link of that joint (which will always "
228 "exist) is automatically included");
229 root->InsertEndChild(comment);
230 comment = doc->NewComment(
"CHAINS: When a chain is specified, all the links along the chain (including endpoints) "
231 "are included in the group. Additionally, all the joints that are parents to included "
232 "links are also included. This means that joints along the chain and the parent joint "
233 "of the base link are included in the group");
234 root->InsertEndChild(comment);
235 comment = doc->NewComment(
"SUBGROUPS: Groups can also be formed by referencing to already defined group names");
236 root->InsertEndChild(comment);
240 for (std::vector<srdf::Model::Group>::iterator group_it = groups_.begin(); group_it != groups_.end(); ++group_it)
243 XMLElement* group = doc->NewElement(
"group");
244 group->SetAttribute(
"name", group_it->name_.c_str());
245 root->InsertEndChild(group);
248 for (std::vector<std::string>::const_iterator link_it = group_it->links_.begin(); link_it != group_it->links_.end();
251 XMLElement* link = doc->NewElement(
"link");
252 link->SetAttribute(
"name", (*link_it).c_str());
253 group->InsertEndChild(link);
257 for (std::vector<std::string>::const_iterator joint_it = group_it->joints_.begin();
258 joint_it != group_it->joints_.end(); ++joint_it)
260 XMLElement* joint = doc->NewElement(
"joint");
261 joint->SetAttribute(
"name", (*joint_it).c_str());
262 group->InsertEndChild(joint);
266 for (std::vector<std::pair<std::string, std::string> >::const_iterator chain_it = group_it->chains_.begin();
267 chain_it != group_it->chains_.end(); ++chain_it)
269 XMLElement* chain = doc->NewElement(
"chain");
270 chain->SetAttribute(
"base_link", chain_it->first.c_str());
271 chain->SetAttribute(
"tip_link", chain_it->second.c_str());
272 group->InsertEndChild(chain);
276 for (std::vector<std::string>::const_iterator subgroup_it = group_it->subgroups_.begin();
277 subgroup_it != group_it->subgroups_.end(); ++subgroup_it)
279 XMLElement* subgroup = doc->NewElement(
"group");
280 subgroup->SetAttribute(
"name", (*subgroup_it).c_str());
281 group->InsertEndChild(subgroup);
289 void SRDFWriter::createLinkSphereApproximationsXML(XMLElement* root)
291 if (link_sphere_approximations_.empty())
294 XMLDocument* doc = root->GetDocument();
297 XMLComment* comment = doc->NewComment(
"COLLISION SPHERES: Purpose: Define a set of spheres that bounds a link.");
298 root->InsertEndChild(comment);
300 for (std::vector<srdf::Model::LinkSpheres>::const_iterator link_sphere_it = link_sphere_approximations_.begin();
301 link_sphere_it != link_sphere_approximations_.end(); ++link_sphere_it)
303 if (link_sphere_it->spheres_.empty())
307 XMLElement* link = doc->NewElement(
"link_sphere_approximation");
308 link->SetAttribute(
"link", link_sphere_it->link_.c_str());
309 root->InsertEndChild(link);
312 for (std::vector<srdf::Model::Sphere>::const_iterator sphere_it = link_sphere_it->spheres_.begin();
313 sphere_it != link_sphere_it->spheres_.end(); ++sphere_it)
315 XMLElement* sphere = doc->NewElement(
"sphere");
316 std::stringstream center;
317 center.precision(20);
318 center << sphere_it->center_x_ <<
" " << sphere_it->center_y_ <<
" " << sphere_it->center_z_;
319 sphere->SetAttribute(
"center", center.str().c_str());
320 sphere->SetAttribute(
"radius", toString(sphere_it->radius_).c_str());
321 link->InsertEndChild(sphere);
329 void SRDFWriter::createCollisionDefaultsXML(XMLElement* root)
331 XMLDocument* doc = root->GetDocument();
334 if (!no_default_collision_links_.empty())
336 XMLComment* comment = doc->NewComment(
"DEFAULT COLLISIONS: By default it is assumed that any link of the robot "
337 "could potentially come into collision with any other link in the robot. "
338 "This tag allows to revert this behavior and disable collisions by default.");
339 root->InsertEndChild(comment);
342 for (
const std::string& name : no_default_collision_links_)
344 XMLElement* entry = doc->NewElement(
"disable_default_collisions");
345 entry->SetAttribute(
"link", name.c_str());
346 root->InsertEndChild(entry);
349 createCollisionPairsXML(root,
"enable_collisions", enabled_collision_pairs_);
355 void SRDFWriter::createCollisionPairsXML(XMLElement* root,
const char* tag_name,
356 const std::vector<Model::CollisionPair>& pairs)
358 XMLDocument* doc = root->GetDocument();
363 XMLElement* entry = doc->NewElement(tag_name);
364 entry->SetAttribute(
"link1", pair.link1_.c_str());
365 entry->SetAttribute(
"link2", pair.link2_.c_str());
366 entry->SetAttribute(
"reason", pair.reason_.c_str());
368 root->InsertEndChild(entry);
375 void SRDFWriter::createDisabledCollisionPairsXML(XMLElement* root)
377 XMLDocument* doc = root->GetDocument();
380 if (disabled_collision_pairs_.empty())
383 XMLComment* comment = doc->NewComment(
"DISABLE COLLISIONS: By default it is assumed that any link of the robot "
384 "could potentially come into collision with any other link in the robot. "
385 "This tag disables collision checking between a specified pair of links. ");
386 root->InsertEndChild(comment);
387 createCollisionPairsXML(root,
"disable_collisions", disabled_collision_pairs_);
393 void SRDFWriter::createGroupStatesXML(XMLElement* root)
395 XMLDocument* doc = root->GetDocument();
398 if (group_states_.size())
400 XMLComment* comment = doc->NewComment(
"GROUP STATES: Purpose: Define a named state for a particular group, in "
401 "terms of joint values. This is useful to define states like 'folded arms'");
402 root->InsertEndChild(comment);
405 for (std::vector<srdf::Model::GroupState>::const_iterator state_it = group_states_.begin();
406 state_it != group_states_.end(); ++state_it)
409 XMLElement* state = doc->NewElement(
"group_state");
410 state->SetAttribute(
"name", state_it->name_.c_str());
411 state->SetAttribute(
"group", state_it->group_.c_str());
412 root->InsertEndChild(state);
415 for (std::map<std::string, std::vector<double> >::const_iterator value_it = state_it->joint_values_.begin();
416 value_it != state_it->joint_values_.end(); ++value_it)
418 XMLElement* joint = doc->NewElement(
"joint");
419 joint->SetAttribute(
"name", value_it->first.c_str());
420 joint->SetAttribute(
"value", toString(value_it->second[0]).c_str());
423 state->InsertEndChild(joint);
431 void SRDFWriter::createEndEffectorsXML(XMLElement* root)
433 XMLDocument* doc = root->GetDocument();
436 if (end_effectors_.size())
438 XMLComment* comment = doc->NewComment(
"END EFFECTOR: Purpose: Represent information about an end effector.");
439 root->InsertEndChild(comment);
442 for (std::vector<srdf::Model::EndEffector>::const_iterator effector_it = end_effectors_.begin();
443 effector_it != end_effectors_.end(); ++effector_it)
446 XMLElement* effector = doc->NewElement(
"end_effector");
447 effector->SetAttribute(
"name", effector_it->name_.c_str());
448 effector->SetAttribute(
"parent_link", effector_it->parent_link_.c_str());
449 effector->SetAttribute(
"group", effector_it->component_group_.c_str());
450 if (!effector_it->parent_group_.empty())
451 effector->SetAttribute(
"parent_group", effector_it->parent_group_.c_str());
452 root->InsertEndChild(effector);
459 void SRDFWriter::createVirtualJointsXML(XMLElement* root)
461 XMLDocument* doc = root->GetDocument();
464 if (virtual_joints_.size())
466 XMLComment* comment = doc->NewComment(
"VIRTUAL JOINT: Purpose: this element defines a virtual joint between a "
467 "robot link and an external frame of reference (considered fixed with "
468 "respect to the robot)");
469 root->InsertEndChild(comment);
472 for (std::vector<srdf::Model::VirtualJoint>::const_iterator virtual_it = virtual_joints_.begin();
473 virtual_it != virtual_joints_.end(); ++virtual_it)
476 XMLElement* virtual_joint = doc->NewElement(
"virtual_joint");
477 virtual_joint->SetAttribute(
"name", virtual_it->name_.c_str());
478 virtual_joint->SetAttribute(
"type", virtual_it->type_.c_str());
479 virtual_joint->SetAttribute(
"parent_frame", virtual_it->parent_frame_.c_str());
480 virtual_joint->SetAttribute(
"child_link", virtual_it->child_link_.c_str());
482 root->InsertEndChild(virtual_joint);
486 void SRDFWriter::createPassiveJointsXML(XMLElement* root)
488 XMLDocument* doc = root->GetDocument();
490 if (passive_joints_.size())
492 XMLComment* comment = doc->NewComment(
"PASSIVE JOINT: Purpose: this element is used to mark joints that are not "
494 root->InsertEndChild(comment);
496 for (std::vector<srdf::Model::PassiveJoint>::const_iterator p_it = passive_joints_.begin();
497 p_it != passive_joints_.end(); ++p_it)
500 XMLElement* p_joint = doc->NewElement(
"passive_joint");
501 p_joint->SetAttribute(
"name", p_it->name_.c_str());
502 root->InsertEndChild(p_joint);
506 void SRDFWriter::createJointPropertiesXML(tinyxml2::XMLElement* root)
508 XMLDocument* doc = root->GetDocument();
510 if (!joint_properties_.empty())
512 XMLComment* comment = doc->NewComment(
"JOINT PROPERTIES: Define properties for individual joints");
513 root->InsertEndChild(comment);
515 for (
const auto& joint_properties : joint_properties_)
517 for (
const auto& joint_property : joint_properties.second)
519 XMLElement* p_joint = doc->NewElement(
"joint_property");
520 p_joint->SetAttribute(
"joint_name", joint_properties.first.c_str());
521 p_joint->SetAttribute(
"property_name", joint_property.first.c_str());
522 p_joint->SetAttribute(
"value", joint_property.second.c_str());
523 root->InsertEndChild(p_joint);