Class
List
Hierarchy
Modules
Common
Events
Math
Messages
Physics
Rendering
Sensors
Transport
Links
Gazebo Website
Wiki
Tutorials
Download
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
parser_urdf.hh
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, 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 the 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 #ifndef URDF2GAZEBO_HH
35 #define URDF2GAZEBO_HH
36 
37 #include <urdf_model/model.h>
38 #include <urdf_model/link.h>
39 #include <tinyxml.h>
40 
41 #include <cstdio>
42 #include <cstdlib>
43 #include <cmath>
44 #include <vector>
45 #include <string>
46 #include <sstream>
47 #include <map>
48 
49 #include "ode/mass.h"
50 #include "ode/rotation.h"
51 
52 #include "math/Pose.hh"
53 #include "common/Console.hh"
54 
57 namespace urdf2gazebo
58 {
61 
62  typedef boost::shared_ptr<urdf::Collision> CollisionPtr;
63  typedef boost::shared_ptr<urdf::Visual> VisualPtr;
64 
65  class GazeboExtension
66  {
67  public:
68  // for reducing fixed joints and removing links
69  std::string old_link_name;
70  gazebo::math::Pose reduction_transform;
71 
72  // visual
73  std::string material;
74 
75  // body, default off
76  bool setStaticFlag;
77  bool gravity;
78  bool is_damping_factor;
79  double damping_factor;
80  bool is_maxVel;
81  double maxVel;
82  bool is_minDepth;
83  double minDepth;
84  bool self_collide;
85 
86  // geom, contact dynamics
87  bool is_mu1, is_mu2, is_kp, is_kd;
88  double mu1, mu2, kp, kd;
89  std::string fdir1;
90  bool is_laser_retro;
91  double laser_retro;
92 
93  // joint, joint limit dynamics
94  bool is_stop_cfm, is_stop_erp, is_initial_joint_position, is_fudge_factor;
95  double stop_cfm, stop_erp, initial_joint_position, fudge_factor;
96  bool provideFeedback;
97 
98  // blobs into body or robot
99  std::vector<TiXmlElement*> blobs;
100 
101  GazeboExtension()
102  {
103  material.clear();
104  setStaticFlag = false;
105  gravity = true;
106  is_damping_factor = false;
107  is_maxVel = false;
108  is_minDepth = false;
109  fdir1.clear();
110  is_mu1 = false;
111  is_mu2 = false;
112  is_kp = false;
113  is_kd = false;
114  self_collide = false;
115  is_laser_retro = false;
116  is_stop_cfm = false;
117  is_stop_erp = false;
118  is_initial_joint_position = false;
119  is_fudge_factor = false;
120  provideFeedback = false;
121  blobs.clear();
122 
123  damping_factor = 0;
124  maxVel = 0;
125  minDepth = 0;
126  mu1 = 0;
127  mu2 = 0;
128  kp = 100000000;
129  kd = 1;
130  laser_retro = 101;
131  stop_cfm = 0;
132  stop_erp = 0.1;
133  initial_joint_position = 0;
134  fudge_factor = 1;
135  };
136 
137  GazeboExtension(const GazeboExtension &ge)
138  {
139  material = ge.material;
140  setStaticFlag = ge.setStaticFlag;
141  gravity = ge.gravity;
142  is_damping_factor = ge.is_damping_factor;
143  is_maxVel = ge.is_maxVel;
144  is_minDepth = ge.is_minDepth;
145  fdir1 = ge.fdir1;
146  is_mu1 = ge.is_mu1;
147  is_mu2 = ge.is_mu2;
148  is_kp = ge.is_kp;
149  is_kd = ge.is_kd;
150  self_collide = ge.self_collide;
151  is_laser_retro = ge.is_laser_retro;
152  is_stop_cfm = ge.is_stop_cfm;
153  is_stop_erp = ge.is_stop_erp;
154  is_initial_joint_position = ge.is_initial_joint_position;
155  is_fudge_factor = ge.is_fudge_factor;
156  provideFeedback = ge.provideFeedback;
157  old_link_name = ge.old_link_name;
158  reduction_transform = ge.reduction_transform;
159  blobs = ge.blobs;
160 
161  damping_factor = ge.damping_factor;
162  maxVel = ge.maxVel;
163  minDepth = ge.minDepth;
164  mu1 = ge.mu1;
165  mu2 = ge.mu2;
166  kp = ge.kp;
167  kd = ge.kd;
168  laser_retro = ge.laser_retro;
169  stop_cfm = ge.stop_cfm;
170  stop_erp = ge.stop_erp;
171  initial_joint_position = ge.initial_joint_position;
172  fudge_factor = ge.fudge_factor;
173  };
174  };
175 
176  class URDF2Gazebo
177  {
178  public:
179  URDF2Gazebo();
180  ~URDF2Gazebo();
181 
183  urdf::Vector3 parseVector3(TiXmlNode* key, double scale = 1.0);
184 
186  std::string values2str(unsigned int count, const double *values);
187 
189  std::string vector32str(const urdf::Vector3 vector);
190 
192  void addKeyValue(TiXmlElement *elem, const std::string& key,
193  const std::string &value);
194 
196  void addTransform(TiXmlElement *elem,
197  const::gazebo::math::Pose& transform);
198 
200  void printMass(boost::shared_ptr<urdf::Link> link);
201 
203  void printMass(std::string link_name, dMass mass);
204 
206  std::string getKeyValueAsString(TiXmlElement* elem);
207 
210  void parseGazeboExtension(TiXmlDocument &urdf_xml);
211 
213  void insertGazeboExtensionCollision(TiXmlElement *elem,
214  std::string link_name);
215 
217  void insertGazeboExtensionVisual(TiXmlElement *elem,
218  std::string link_name);
219 
221  void insertGazeboExtensionLink(TiXmlElement *elem,
222  std::string link_name);
223 
225  void insertGazeboExtensionJoint(TiXmlElement *elem,
226  std::string joint_name);
227 
229  void insertGazeboExtensionRobot(TiXmlElement *elem);
230 
232  void listGazeboExtensions();
233 
235  void listGazeboExtensions(std::string reference);
236 
238  // collision elements of the child link into the parent link
239  void reduceFixedJoints(TiXmlElement *root,
240  boost::shared_ptr<urdf::Link> link);
241 
243  void reduceInertialToParent(boost::shared_ptr<urdf::Link> link);
244 
246  void reduceVisualsToParent(boost::shared_ptr<urdf::Link> link);
247 
249  void reduceCollisionsToParent(boost::shared_ptr<urdf::Link> link);
250 
252  void reduceJointsToParent(boost::shared_ptr<urdf::Link> link);
253 
255  void reduceVisualToParent(boost::shared_ptr<urdf::Link> link,
256  std::string group_name,
257  boost::shared_ptr<urdf::Visual> visual);
258 
260  void reduceCollisionToParent(boost::shared_ptr<urdf::Link> link,
261  std::string group_name,
262  boost::shared_ptr<urdf::Collision> collision);
263 
266  void reduceGazeboExtensionToParent(boost::shared_ptr<urdf::Link> link);
267 
270  void reduceGazeboExtensionFrameReplace(GazeboExtension* ge,
271  boost::shared_ptr<urdf::Link> link);
272 
275  void reduceGazeboExtensionPluginFrameReplace(
276  std::vector<TiXmlElement*>::iterator blob_it,
277  boost::shared_ptr<urdf::Link> link, std::string plugin_name,
278  std::string element_name, gazebo::math::Pose reduction_transform);
279 
282  void reduceGazeboExtensionProjectorFrameReplace(
283  std::vector<TiXmlElement*>::iterator blob_it,
284  boost::shared_ptr<urdf::Link> link);
285 
288  void reduceGazeboExtensionGripperFrameReplace(
289  std::vector<TiXmlElement*>::iterator blob_it,
290  boost::shared_ptr<urdf::Link> link);
291 
294  void reduceGazeboExtensionJointFrameReplace(
295  std::vector<TiXmlElement*>::iterator blob_it,
296  boost::shared_ptr<urdf::Link> link);
297 
300  void reduceGazeboExtensionContactSensorFrameReplace(
301  std::vector<TiXmlElement*>::iterator blob_it,
302  boost::shared_ptr<urdf::Link> link);
303 
306  void reduceGazeboExtensionsTransformReduction(GazeboExtension* ge);
307 
310  void reduceGazeboExtensionSensorTransformReduction(
311  std::vector<TiXmlElement*>::iterator blob_it,
312  gazebo::math::Pose reduction_transform);
313 
316  void reduceGazeboExtensionProjectorTransformReduction(
317  std::vector<TiXmlElement*>::iterator blob_it,
318  gazebo::math::Pose reduction_transform);
319 
321  urdf::Pose transformToParentFrame(
322  urdf::Pose transform_in_link_frame,
323  urdf::Pose parent_to_link_transform);
325  gazebo::math::Pose transformToParentFrame(
326  gazebo::math::Pose transform_in_link_frame,
327  urdf::Pose parent_to_link_transform);
329  gazebo::math::Pose transformToParentFrame(
330  gazebo::math::Pose transform_in_link_frame,
331  gazebo::math::Pose parent_to_link_transform);
333  gazebo::math::Pose inverseTransformToParentFrame(
334  gazebo::math::Pose transform_in_link_frame,
335  urdf::Pose parent_to_link_transform);
338  gazebo::math::Pose copyPose(urdf::Pose pose);
341  urdf::Pose copyPose(gazebo::math::Pose pose);
342 
343  std::string getGeometryBoundingBox(
344  boost::shared_ptr<urdf::Geometry> geometry, double *sizeVals);
345 
346 
348  void printCollisionGroups(boost::shared_ptr<urdf::Link> link);
349 
351  void createSDF(TiXmlElement *root,
352  boost::shared_ptr<const urdf::Link> link,
353  const gazebo::math::Pose &transform);
354 
356  void createLink(TiXmlElement *root,
357  boost::shared_ptr<const urdf::Link> link,
358  gazebo::math::Pose &currentTransform);
359 
361  void createCollisions(TiXmlElement* elem,
362  boost::shared_ptr<const urdf::Link> link);
363 
365  void createVisuals(TiXmlElement* elem,
366  boost::shared_ptr<const urdf::Link> link);
367 
369  void createInertial(TiXmlElement *elem,
370  boost::shared_ptr<const urdf::Link> link);
371 
373  void createCollision(TiXmlElement* elem,
374  boost::shared_ptr<const urdf::Link> link,
375  boost::shared_ptr<urdf::Collision> collision,
376  std::string old_link_name = std::string(""));
377 
379  void createVisual(TiXmlElement *elem,
380  boost::shared_ptr<const urdf::Link> link,
381  boost::shared_ptr<urdf::Visual> visual,
382  std::string old_link_name = std::string(""));
383 
385  void createJoint(TiXmlElement *root,
386  boost::shared_ptr<const urdf::Link> link,
387  gazebo::math::Pose &currentTransform);
388 
390  void createGeometry(TiXmlElement* elem,
391  boost::shared_ptr<urdf::Geometry> geometry);
392 
393  TiXmlDocument initModelString(std::string urdf_str);
394  TiXmlDocument initModelDoc(TiXmlDocument* _xmlDoc);
395  TiXmlDocument initModelFile(std::string filename);
396  TiXmlDocument initModelString(std::string urdf_str, bool _enforce_limits);
397 
398  std::map<std::string, std::vector<GazeboExtension*> > gazebo_extensions_;
399 
400  private: bool enforce_limits;
401  private: bool reduce_fixed_joints;
402  };
404 }
405 
406 #endif