srdf_writer.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman */
36 
37 #include <srdfdom/srdf_writer.h>
38 
39 using namespace tinyxml2;
40 
41 namespace
42 {
43 template <class T>
44 std::string toString(const T& t)
45 {
46  // convert to string using no locale
47  std::ostringstream oss;
48  oss.imbue(std::locale::classic());
49  oss << t;
50  return oss.str();
51 }
52 } // namespace
53 
54 namespace srdf
55 {
56 // ******************************************************************************************
57 // Constructor
58 // ******************************************************************************************
59 SRDFWriter::SRDFWriter()
60 {
61  // Intialize the SRDF model
62  srdf_model_.reset(new srdf::Model());
63 }
64 
65 // ******************************************************************************************
66 // Destructor
67 // ******************************************************************************************
68 SRDFWriter::~SRDFWriter()
69 {
70 }
71 
72 // ******************************************************************************************
73 // Load SRDF data from a pre-populated string
74 // ******************************************************************************************
75 bool SRDFWriter::initString(const urdf::ModelInterface& robot_model, const std::string& srdf_string)
76 {
77  // Parse string into srdf_model_ and Error check
78  if (!srdf_model_->initString(robot_model, srdf_string))
79  {
80  return false; // error loading file. improper format?
81  }
82 
83  // copy fields into this object
84  initModel(robot_model, *srdf_model_);
85 
86  return true;
87 }
88 
89 // ******************************************************************************************
90 // Load SRDF data from a pre-populated string
91 // ******************************************************************************************
92 void SRDFWriter::initModel(const urdf::ModelInterface& robot_model, const srdf::Model& srdf_model)
93 {
94  // copy to internal srdf_model_
95  if (srdf_model_.get() != &srdf_model)
96  {
97  *srdf_model_ = srdf_model;
98  }
99 
100  // Copy all read-only data from srdf model to this object
101  no_default_collision_links_ = srdf_model_->getNoDefaultCollisionLinks();
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();
111 
112  // Copy the robot name b/c the root xml element requires this attribute
113  robot_name_ = robot_model.getName();
114 }
115 
116 // ******************************************************************************************
117 // Update the SRDF Model class using a new SRDF string
118 // ******************************************************************************************
119 void SRDFWriter::updateSRDFModel(const urdf::ModelInterface& robot_model)
120 {
121  // Get an up to date SRDF String
122  const std::string srdf_string = getSRDFString();
123 
124  // Error check
125  if (!srdf_model_->initString(robot_model, srdf_string))
126  {
127  throw std::runtime_error("Unable to update the SRDF Model");
128  }
129 }
130 
131 // ******************************************************************************************
132 // Save to file a generated SRDF document
133 // ******************************************************************************************
134 bool SRDFWriter::writeSRDF(const std::string& file_path)
135 {
136  // Generate the SRDF
137  XMLDocument document;
138  generateSRDF(document);
139 
140  // Save to file
141  return document.SaveFile(file_path.c_str()) == XML_SUCCESS;
142 }
143 
144 // ******************************************************************************************
145 // Get a string of a generated SRDF document
146 // ******************************************************************************************
147 std::string SRDFWriter::getSRDFString()
148 {
149  // Generate the SRDF
150  XMLDocument document;
151  generateSRDF(document);
152 
153  // Setup printer
154  XMLPrinter printer;
155  document.Accept(&printer);
156 
157  // Return string
158  return printer.CStr();
159 }
160 
161 // ******************************************************************************************
162 // Generate SRDF XML of all contained data
163 // ******************************************************************************************
164 void SRDFWriter::generateSRDF(XMLDocument& document)
165 {
166  XMLDeclaration* decl = document.NewDeclaration();
167  document.InsertEndChild(decl);
168 
169  // Convenience comments
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 "
172  "structure.\n "
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);
176 
177  // Root
178  XMLElement* robot_root = document.NewElement("robot");
179  robot_root->SetAttribute("name", robot_name_.c_str()); // robot name
180  document.InsertEndChild(robot_root);
181 
182  // Add Groups
183  createGroupsXML(robot_root);
184 
185  // Add Group States
186  createGroupStatesXML(robot_root);
187 
188  // Add End Effectors
189  createEndEffectorsXML(robot_root);
190 
191  // Add Virtual Joints
192  createVirtualJointsXML(robot_root);
193 
194  // Add Passive Joints
195  createPassiveJointsXML(robot_root);
196 
197  // Add Joint Properties
198  createJointPropertiesXML(robot_root);
199 
200  // Add Link Sphere approximations
201  createLinkSphereApproximationsXML(robot_root);
202 
203  // Create disable_default_collisions tags and tags to re-enable specific pairs
204  createCollisionDefaultsXML(robot_root);
205 
206  // Add Disabled Collisions
207  createDisabledCollisionPairsXML(robot_root);
208 }
209 
210 // ******************************************************************************************
211 // Generate XML for SRDF groups
212 // ******************************************************************************************
213 void SRDFWriter::createGroupsXML(XMLElement* root)
214 {
215  XMLDocument* doc = root->GetDocument();
216 
217  // Convenience comments
218  if (groups_.size()) // only show comments if there are corresponding elements
219  {
220  XMLComment* comment;
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);
237  }
238 
239  // Loop through all of the top groups
240  for (std::vector<srdf::Model::Group>::iterator group_it = groups_.begin(); group_it != groups_.end(); ++group_it)
241  {
242  // Create group element
243  XMLElement* group = doc->NewElement("group");
244  group->SetAttribute("name", group_it->name_.c_str()); // group name
245  root->InsertEndChild(group);
246 
247  // LINKS
248  for (std::vector<std::string>::const_iterator link_it = group_it->links_.begin(); link_it != group_it->links_.end();
249  ++link_it)
250  {
251  XMLElement* link = doc->NewElement("link");
252  link->SetAttribute("name", (*link_it).c_str()); // link name
253  group->InsertEndChild(link);
254  }
255 
256  // JOINTS
257  for (std::vector<std::string>::const_iterator joint_it = group_it->joints_.begin();
258  joint_it != group_it->joints_.end(); ++joint_it)
259  {
260  XMLElement* joint = doc->NewElement("joint");
261  joint->SetAttribute("name", (*joint_it).c_str()); // joint name
262  group->InsertEndChild(joint);
263  }
264 
265  // CHAINS
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)
268  {
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);
273  }
274 
275  // SUBGROUPS
276  for (std::vector<std::string>::const_iterator subgroup_it = group_it->subgroups_.begin();
277  subgroup_it != group_it->subgroups_.end(); ++subgroup_it)
278  {
279  XMLElement* subgroup = doc->NewElement("group");
280  subgroup->SetAttribute("name", (*subgroup_it).c_str()); // subgroup name
281  group->InsertEndChild(subgroup);
282  }
283  }
284 }
285 
286 // ******************************************************************************************
287 // Generate XML for SRDF link collision spheres
288 // ******************************************************************************************
289 void SRDFWriter::createLinkSphereApproximationsXML(XMLElement* root)
290 {
291  if (link_sphere_approximations_.empty()) // skip it if there are none
292  return;
293 
294  XMLDocument* doc = root->GetDocument();
295 
296  // Convenience comments
297  XMLComment* comment = doc->NewComment("COLLISION SPHERES: Purpose: Define a set of spheres that bounds a link.");
298  root->InsertEndChild(comment);
299 
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)
302  {
303  if (link_sphere_it->spheres_.empty()) // skip if no spheres for this link
304  continue;
305 
306  // Create new element for the link
307  XMLElement* link = doc->NewElement("link_sphere_approximation");
308  link->SetAttribute("link", link_sphere_it->link_.c_str());
309  root->InsertEndChild(link);
310 
311  // Add all spheres for the 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)
314  {
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);
322  }
323  }
324 }
325 
326 // ******************************************************************************************
327 // Generate XML for SRDF collision defaults of robot links
328 // ******************************************************************************************
329 void SRDFWriter::createCollisionDefaultsXML(XMLElement* root)
330 {
331  XMLDocument* doc = root->GetDocument();
332 
333  // Convenience comments
334  if (!no_default_collision_links_.empty()) // only show comments if there are corresponding elements
335  {
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);
340  }
341 
342  for (const std::string& name : no_default_collision_links_)
343  {
344  XMLElement* entry = doc->NewElement("disable_default_collisions");
345  entry->SetAttribute("link", name.c_str());
346  root->InsertEndChild(entry);
347  }
348  // Write enabled collision pairs
349  createCollisionPairsXML(root, "enable_collisions", enabled_collision_pairs_);
350 }
351 
352 // ******************************************************************************************
353 // Generate XML for SRDF disabled/enabled collisions of robot link pairs
354 // ******************************************************************************************
355 void SRDFWriter::createCollisionPairsXML(XMLElement* root, const char* tag_name,
356  const std::vector<Model::CollisionPair>& pairs)
357 {
358  XMLDocument* doc = root->GetDocument();
359 
360  for (const srdf::Model::CollisionPair& pair : pairs)
361  {
362  // Create new element for each link pair
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());
367 
368  root->InsertEndChild(entry);
369  }
370 }
371 
372 // ******************************************************************************************
373 // Generate XML for SRDF disabled collisions of robot link pairs
374 // ******************************************************************************************
375 void SRDFWriter::createDisabledCollisionPairsXML(XMLElement* root)
376 {
377  XMLDocument* doc = root->GetDocument();
378 
379  // Convenience comments
380  if (disabled_collision_pairs_.empty())
381  return;
382 
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_);
388 }
389 
390 // ******************************************************************************************
391 // Generate XML for SRDF group states
392 // ******************************************************************************************
393 void SRDFWriter::createGroupStatesXML(XMLElement* root)
394 {
395  XMLDocument* doc = root->GetDocument();
396 
397  // Convenience comments
398  if (group_states_.size()) // only show comments if there are corresponding elements
399  {
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);
403  }
404 
405  for (std::vector<srdf::Model::GroupState>::const_iterator state_it = group_states_.begin();
406  state_it != group_states_.end(); ++state_it)
407  {
408  // Create new element for each group state
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);
413 
414  // Add all joints
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)
417  {
418  XMLElement* joint = doc->NewElement("joint");
419  joint->SetAttribute("name", value_it->first.c_str()); // joint name
420  joint->SetAttribute("value", toString(value_it->second[0]).c_str()); // joint value
421 
422  // TODO: use the vector to support multi-DOF joints
423  state->InsertEndChild(joint);
424  }
425  }
426 }
427 
428 // ******************************************************************************************
429 // Generate XML for SRDF end effectors
430 // ******************************************************************************************
431 void SRDFWriter::createEndEffectorsXML(XMLElement* root)
432 {
433  XMLDocument* doc = root->GetDocument();
434 
435  // Convenience comments
436  if (end_effectors_.size()) // only show comments if there are corresponding elements
437  {
438  XMLComment* comment = doc->NewComment("END EFFECTOR: Purpose: Represent information about an end effector.");
439  root->InsertEndChild(comment);
440  }
441 
442  for (std::vector<srdf::Model::EndEffector>::const_iterator effector_it = end_effectors_.begin();
443  effector_it != end_effectors_.end(); ++effector_it)
444  {
445  // Create new element for each link pair
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);
453  }
454 }
455 
456 // ******************************************************************************************
457 // Generate XML for SRDF virtual joints
458 // ******************************************************************************************
459 void SRDFWriter::createVirtualJointsXML(XMLElement* root)
460 {
461  XMLDocument* doc = root->GetDocument();
462 
463  // Convenience comments
464  if (virtual_joints_.size()) // only show comments if there are corresponding elements
465  {
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);
470  }
471 
472  for (std::vector<srdf::Model::VirtualJoint>::const_iterator virtual_it = virtual_joints_.begin();
473  virtual_it != virtual_joints_.end(); ++virtual_it)
474  {
475  // Create new element for each link pair
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());
481 
482  root->InsertEndChild(virtual_joint);
483  }
484 }
485 
486 void SRDFWriter::createPassiveJointsXML(XMLElement* root)
487 {
488  XMLDocument* doc = root->GetDocument();
489 
490  if (passive_joints_.size())
491  {
492  XMLComment* comment = doc->NewComment("PASSIVE JOINT: Purpose: this element is used to mark joints that are not "
493  "actuated");
494  root->InsertEndChild(comment);
495  }
496  for (std::vector<srdf::Model::PassiveJoint>::const_iterator p_it = passive_joints_.begin();
497  p_it != passive_joints_.end(); ++p_it)
498  {
499  // Create new element for each link pair
500  XMLElement* p_joint = doc->NewElement("passive_joint");
501  p_joint->SetAttribute("name", p_it->name_.c_str());
502  root->InsertEndChild(p_joint);
503  }
504 }
505 
506 void SRDFWriter::createJointPropertiesXML(tinyxml2::XMLElement* root)
507 {
508  XMLDocument* doc = root->GetDocument();
509 
510  if (!joint_properties_.empty())
511  {
512  XMLComment* comment = doc->NewComment("JOINT PROPERTIES: Define properties for individual joints");
513  root->InsertEndChild(comment);
514  }
515  for (const auto& joint_properties : joint_properties_)
516  {
517  for (const auto& joint_property : joint_properties.second)
518  {
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);
524  }
525  }
526 }
527 } // namespace srdf
srdf_writer.h
srdf::Model::getNoDefaultCollisionLinks
const std::vector< std::string > & getNoDefaultCollisionLinks() const
Get the list of links that should have collision checking disabled by default (and only selectively e...
Definition: model.h:272
srdf
Main namespace.
Definition: model.h:49
srdf::Model::CollisionPair
The definition of a disabled/enabled collision between two links.
Definition: model.h:246
srdf::Model
Representation of semantic information about the robot.
Definition: model.h:84


srdfdom
Author(s): Ioan Sucan , Guillaume Walck
autogenerated on Wed Oct 16 2024 02:22:59