libalmath  2.5.7.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
urdf.h
Go to the documentation of this file.
1 /*
2  * Copyright 2015 Aldebaran. All rights reserved.
3  *
4  */
5 
6 #ifndef LIB_ALMATH_SCENEGRAPH_URDF_H
7 #define LIB_ALMATH_SCENEGRAPH_URDF_H
8 
9 #include <string>
10 #include <iosfwd>
11 #include <memory>
12 #include <array>
13 #include <boost/property_tree/ptree_fwd.hpp>
14 #include <boost/optional.hpp>
15 #include <boost/function/function_fwd.hpp>
16 #include <boost/ref.hpp>
17 #include <boost/multi_index_container.hpp>
18 #include <boost/multi_index/ordered_index.hpp>
19 #include <boost/multi_index/identity.hpp>
20 #include <boost/multi_index/mem_fun.hpp>
21 #include <boost/multi_index/global_fun.hpp>
22 #include <boost/multi_index/sequenced_index.hpp>
23 #include <set>
24 #include <vector>
25 
26 #include <boost/property_tree/stream_translator.hpp>
27 
28 namespace boost {
29 namespace property_tree {
30 
31 // hack around ptree bug #10188 [1]: for floating point numbers they don't
32 // serialize with enough precision:
33 // they use digits10+1 instead of max_digits10
34 // [1] https://svn.boost.org/trac/boost/ticket/10188
35 template <typename Ch, typename Traits>
36 struct customize_stream<Ch, Traits, double, void> {
37  static void insert(std::basic_ostream<Ch, Traits> &s, const double &e) {
38  s.precision(std::numeric_limits<double>::max_digits10);
39  s << e;
40  }
41  static void extract(std::basic_istream<Ch, Traits> &s, double &e) {
42  s >> e;
43  if (!s.eof()) {
44  s >> std::ws;
45  }
46  }
47 };
48 }
49 }
50 
51 namespace AL {
52 
53 namespace urdf {
54 
56 
57 namespace detail {
58 class UrdfTreeP;
59 }
60 
61 typedef std::array<double, 3> Array3d;
62 
63 inline bool is_zero(const Array3d &a) {
64  return (a[0] == 0) && (a[1] == 0) && (a[2] == 0);
65 }
66 
67 class Pose;
68 
69 // return the name attribute of a joint or link element
70 std::string name(const ptree &pt);
71 
72 // return the parent link of a joint element
73 std::string parent_link(const ptree &pt);
74 
75 // return the child link of a joint element
76 std::string child_link(const ptree &pt);
77 
78 // a parser for urdf xml files [1].
79 //
80 // Contrary to the urdf parser from ROS [2], [3], this parser:
81 //
82 // * preserves joint ordering defined in XML file. This is useful if one
83 // wants to use this ordering to define the joint indexes in a
84 // state vector.
85 //
86 // * provides access to the raw XML elements, so that you can leverage the
87 // parser to handle unofficial URDF extensions and
88 // to modify the XML tree and write it back while altering the document as
89 // little as possible.
90 //
91 // * does not systematically convert roll-pitch-yaw angles to quaternions,
92 // which saves us some bugs and better fits the "alter the document as
93 // little as possible" philosophy.
94 //
95 // [1] http://wiki.ros.org/urdf/XML
96 // [2] https://github.com/ros/robot_model
97 // [3] https://github.com/ros/urdfdom_headers
98 //
99 // The UrdfTree class acts as an index for the boost::property_tree of the
100 // (URDF) XML document.
101 // Given the ptree of the root XML element, it walks the document and
102 // indexes the ptree elements for the URDF joints and links.
103 //
104 // The class is akin to a set of iterators: it does not copy nor own the
105 // XML tree, and may be invalidated if the XML tree changes.
106 //
107 // The user shall ensure that the XML root and the joint and link ptree
108 // elements stays alive and valid for the lifetime of this UrdfTree.
109 // For instance the user may use direct access to the ptree elements to alter
110 // the mass of a link, but shall no delete the link, otherwise calls to
111 // UrdfTree::link("my_link_name") would use dandling pointers and return
112 // invalid references.
113 //
114 // On the other hand, some UrdfTree functions enable the safe modification or
115 // removal of joints and links.
116 class UrdfTree {
117  public:
118  // read a XML tree given the XML root element.
119  // The XML tree is not copied, the user shall ensure that the reference
120  // is valid for the whole life of the UrdfTree object.
121  UrdfTree(ptree &pt);
122  ~UrdfTree();
123  UrdfTree(UrdfTree const &) = delete;
124  void operator=(UrdfTree const &other) = delete;
125 
126  const ptree &link(const std::string &name) const;
127  ptree &link(const std::string &name);
128 
129  const ptree &joint(const std::string &name) const;
130  ptree &joint(const std::string &name);
131 
132  const std::string &root_link() const;
133 
134  void rm_root_joint();
135 
136  // Change the root link frame of reference.
137  //
138  // Each urdf link has an implicitly defined frame of reference with respect
139  // to which the pose of the link children elements (inertia, collisions,
140  // visuals) and joints are defined.
141  //
142  // This function changes the root link frame of reference, while updating
143  // all its children elements and joints accordingly so that the physical
144  // system being described does not change.
145  //
146  // The function takes one argument "pose" which is used to define the
147  // transform from the old frame of reference of the new one according to
148  // the formula:
149  //
150  // point_in_new_frame = pose * point_in_old_frame
151  //
152  // children elements and joints poses are updated using:
153  //
154  // child_pose_in_new_frame = pose.inverse() * child_pose_in_old_frame
155  //
156  // Note: changing the frame of reference of a link which is not the root one
157  // would also be possible, but only to some extent since its origin must
158  // lie on the parent joint axis.
159  void transport_root_link_frame(const Pose &pose);
160 
161  // Modify the kinematic tree so that the given link is the root without
162  // changing the physical meaning of the described system.
163  //
164  // Flips all the joints on the path between the old and the new root.
165  //
166  // Throws is there is a multi-dof joint on the path, because flipping it
167  // would change the joint-space parametrization.
168 
169  // Change the frame of reference of the links involved so that its
170  // origin lies on the parent joint axis.
171  //
172  // Let consider a root link "a" with children links "b" and "c" and
173  // joints "ab" and "bc".
174  //
175  // The linematic tree can be written as: a --ab--> b
176  // +-ac--> c
177  //
178  // The frame may look like:
179  // / \
180  // / \
181  // / \ / /
182  // / \ / /
183  // --/ \/ /--/ \/ /--
184  // __________/ ab / \ b /
185  // / / \ /
186  // / a |_ /
187  // / \ / __________/
188  // / \ / /
189  // --/ \/ /--/ \/ /--
190  // / c / \ ac/
191  // / / \ /
192  // \ /
193  // \ /
194  //
195  // After calling define_as_root_link("b")
196  // * the kinematic tree can be written as
197  // b --ab--> a --ac--> c
198  // * the reference frame of "b" and "c" are unchanged
199  // * the reference frame of "a" moved to joint "ab" frame, so as to lie
200  // on joint "ab" axis.
201  // * joint "ab" origin pose is now defined with respecte to link "b"
202  // frame and is thus the identity
203  // * joint "ac" origin pose has been updated to acount fot the
204  //
205  // The frames look like:
206  // / \
207  // / \
208  // / \ / /
209  // / \ / /
210  // --/ \/ /--/ \/ /--
211  // __________/ a / \ b / == ba
212  // / / \ /
213  // / /
214  // / \ / __________/
215  // / \ / /
216  // --/ \/ /--/ \/ /--
217  // / c / \ ac/
218  // / / \ /
219  // \ /
220  // \ /
221  void define_as_root_link(const std::string &name);
222 
224  public:
225  // a false return value stops the walk for the current branch
226  virtual bool discover(const ptree &) { return true; }
227  virtual void finish(const ptree &) {}
228  };
229  class JointVisitor {
230  public:
231  virtual bool discover(ptree &) { return true; }
232  virtual void finish(ptree &) {}
233  };
234 
235  // Traverse the kinematic tree with a depth first traversal.
236  // Siblings joints are visited using the ordering from the urdf file.
237  void traverse_joints(JointConstVisitor &visitor) const;
238  void traverse_joints(JointVisitor &visitor);
239 
240  private:
241  std::unique_ptr<detail::UrdfTreeP> _p;
242 };
243 
246 
247 // Convenience wrapper classes around URDF ptree elements
248 
249 // helper to convert ptree to/from Array3d
251  typedef std::string internal_type;
252  typedef std::array<double, 3> external_type;
253 
254  boost::optional<external_type> get_value(const internal_type &str);
255  boost::optional<internal_type> put_value(const external_type &v);
256 };
257 
258 // Models an URDF pose/transform ("origin" XML element)
259 class Pose {
260  public:
261  Pose(const Array3d &xyz, const Array3d &rpy) : _xyz(xyz), _rpy(rpy) {}
262  Pose() : Pose({{0, 0, 0}}, {{0, 0, 0}}) {}
263  const Array3d &xyz() const { return _xyz; }
264  const Array3d &rpy() const { return _rpy; }
265  Pose inverse() const;
266  friend Pose operator*(const Pose &lhs, const Pose &rhs);
267 
268  static Pose from_ptree(const ptree &pt);
269  static Pose from_ptree(const boost::optional<const ptree &> &pt);
270  boost::optional<ptree> to_ptree() const;
271 
272  private:
273  Array3d _xyz;
274  Array3d _rpy;
275 };
276 
277 inline bool operator==(const Pose &lhs, const Pose &rhs) {
278  return (lhs.xyz() == rhs.xyz()) && (lhs.rpy() == rhs.rpy());
279 }
280 inline bool operator!=(const Pose &lhs, const Pose &rhs) {
281  return !(lhs == rhs);
282 }
283 
284 inline bool is_identity(const Pose &p) {
285  return is_zero(p.xyz()) && is_zero(p.rpy());
286 }
287 
288 // Convenience wrapper around an URDF joint XML element
289 class Joint {
290  public:
292  const ptree &pt;
293  Joint(const ptree &pt);
294  std::string name() const;
295  Type type() const;
296  std::string parent_link() const;
297  std::string child_link() const;
298  Pose origin() const;
299  Array3d axis() const;
300 };
301 
302 // Convenience wrapper around an URDF inertial XML element
303 class Inertial {
304  public:
305  const ptree &pt;
306  Inertial(const ptree &pt);
307  Pose origin() const;
308  double mass() const;
309  double ixx() const;
310  double ixy() const;
311  double ixz() const;
312  double iyy() const;
313  double iyz() const;
314  double izz() const;
315 };
316 
317 // Convenience wrapper around an URDF link XML element
318 class Link {
319  public:
320  const ptree &pt;
321  Link(const ptree &pt);
322  std::string name() const;
323  boost::optional<Inertial> inertial() const;
324 };
325 
326 // Utils
327 
328 // make the type of the joint named "name" equal to "fixed",
329 // and erase the joint axis, if any.
330 void makeJointFixed(UrdfTree &parser, const std::string &name);
331 
332 // make the type of the joint named "name" equal to "floating",
333 // and erase the joint axis, if any.
334 void makeJointFloating(UrdfTree &parser, const std::string &name);
335 
336 // apply makeJointFixed to all joints of type "continuous".
337 // Return the names of the joints whose type was changed.
338 std::vector<std::string> makeContinuousJointsFixed(UrdfTree &parser);
339 
340 // Squash a joint's child link mass into its parent link mass.
341 // The child link mass element (if any) is erased.
342 void squashJointMass(UrdfTree &parser, const std::string &name);
343 
344 // apply squashJointMass to all fixed joints.
345 void squashFixedJointsMass(UrdfTree &parser);
346 
347 // Squash all fixed joint's child link mass into their parent link mass and
348 // then convert all massless joints into fixed joints.
349 // The point is to avoid massless mobile joints, which have no physical
350 // meaning.
351 // Return the names of the joints whose type was changed.
352 std::vector<std::string> makeMasslessJointsFixed(UrdfTree &parser);
353 
354 // a visitor which prints the URDF kinematic tree as a dot graph
355 // when visiting its joints.
357  const char tab;
358  int depth;
359  bool do_indent;
360  std::ostream &os;
361 
362  public:
363  UrdfDotPrinterVisitor(std::ostream &os);
364 
365  public:
366  bool discover(const ptree &joint);
367  void finish(const ptree &joint);
368 };
369 }
370 }
371 #endif
void makeJointFloating(UrdfTree &parser, const std::string &name)
boost::optional< ptree > to_ptree() const
std::string internal_type
Definition: urdf.h:251
std::string parent_link(const ptree &pt)
std::string child_link(const ptree &pt)
double ixx() const
void squashJointMass(UrdfTree &parser, const std::string &name)
const ptree & link(const std::string &name) const
double iyz() const
double ixz() const
Type type() const
UrdfTree(ptree &pt)
double izz() const
boost::optional< internal_type > put_value(const external_type &v)
double ixy() const
bool operator==(const Pose &lhs, const Pose &rhs)
Definition: urdf.h:277
void finish(const ptree &joint)
const ptree & joint(const std::string &name) const
std::array< double, 3 > Array3d
Definition: urdf.h:61
bool is_identity(const Pose &p)
Definition: urdf.h:284
double iyy() const
void define_as_root_link(const std::string &name)
Array3d axis() const
bool discover(const ptree &joint)
friend Pose operator*(const Pose &lhs, const Pose &rhs)
Inertial(const ptree &pt)
bool operator!=(const Pose &lhs, const Pose &rhs)
Definition: urdf.h:280
const ptree & pt
Definition: urdf.h:292
void makeJointFixed(UrdfTree &parser, const std::string &name)
std::string name(const ptree &pt)
UrdfTree::JointVisitor JointVisitor
Definition: urdf.h:245
const ptree & pt
Definition: urdf.h:305
std::string child_link() const
static void extract(std::basic_istream< Ch, Traits > &s, double &e)
Definition: urdf.h:41
std::string parent_link() const
void operator=(UrdfTree const &other)=delete
double mass() const
Joint(const ptree &pt)
static void insert(std::basic_ostream< Ch, Traits > &s, const double &e)
Definition: urdf.h:37
virtual bool discover(ptree &)
Definition: urdf.h:231
static Pose from_ptree(const ptree &pt)
std::array< double, 3 > external_type
Definition: urdf.h:252
Pose origin() const
UrdfDotPrinterVisitor(std::ostream &os)
UrdfTree::JointConstVisitor JointConstVisitor
Definition: urdf.h:244
std::vector< std::string > makeMasslessJointsFixed(UrdfTree &parser)
Pose origin() const
virtual void finish(const ptree &)
Definition: urdf.h:227
virtual void finish(ptree &)
Definition: urdf.h:232
boost::property_tree::ptree ptree
Definition: urdf.h:55
boost::optional< external_type > get_value(const internal_type &str)
std::vector< std::string > makeContinuousJointsFixed(UrdfTree &parser)
void squashFixedJointsMass(UrdfTree &parser)
std::string name() const
Pose(const Array3d &xyz, const Array3d &rpy)
Definition: urdf.h:261
void transport_root_link_frame(const Pose &pose)
void traverse_joints(JointConstVisitor &visitor) const
virtual bool discover(const ptree &)
Definition: urdf.h:226
bool is_zero(const Array3d &a)
Definition: urdf.h:63
const std::string & root_link() const