urdf.py
Go to the documentation of this file.
1 from __future__ import print_function
2 
3 import urdf_parser_py.urdf as urdf
4 
5 import PyKDL as kdl
6 
7 def treeFromFile(filename):
8  """
9  Construct a PyKDL.Tree from an URDF file.
10  :param filename: URDF file path
11  """
12 
13  with open(filename) as urdf_file:
14  return treeFromUrdfModel(urdf.URDF.from_xml_string(urdf_file.read()))
15 
16 def treeFromParam(param):
17  """
18  Construct a PyKDL.Tree from an URDF in a ROS parameter.
19  :param param: Parameter name, ``str``
20  """
21 
22  return treeFromUrdfModel(urdf.URDF.from_parameter_server())
23 
24 def treeFromString(xml):
25  """
26  Construct a PyKDL.Tree from an URDF xml string.
27  :param xml: URDF xml string, ``str``
28  """
29 
30  return treeFromUrdfModel(urdf.URDF.from_xml_string(xml))
31 
32 def _toKdlPose(pose):
33  # URDF might have RPY OR XYZ unspecified. Both default to zeros
34  rpy = pose.rpy if pose and pose.rpy and len(pose.rpy) == 3 else [0, 0, 0]
35  xyz = pose.xyz if pose and pose.xyz and len(pose.xyz) == 3 else [0, 0, 0]
36 
37  return kdl.Frame(
38  kdl.Rotation.RPY(*rpy),
39  kdl.Vector(*xyz))
40 
41 
43  # kdl specifies the inertia in the reference frame of the link, the urdf
44  # specifies the inertia in the inertia reference frame
45  origin = _toKdlPose(i.origin)
46  inertia = i.inertia
47  return origin.M * kdl.RigidBodyInertia(
48  i.mass, origin.p,
49  kdl.RotationalInertia(inertia.ixx, inertia.iyy, inertia.izz, inertia.ixy, inertia.ixz, inertia.iyz));
50 
51 def _toKdlJoint(jnt):
52 
53  fixed = lambda j,F: kdl.Joint(j.name, kdl.Joint.None)
54  rotational = lambda j,F: kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.RotAxis)
55  translational = lambda j,F: kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.TransAxis)
56 
57  type_map = {
58  'fixed': fixed,
59  'revolute': rotational,
60  'continuous': rotational,
61  'prismatic': translational,
62  'floating': fixed,
63  'planar': fixed,
64  'unknown': fixed,
65  }
66 
67  return type_map[jnt.type](jnt, _toKdlPose(jnt.origin))
68 
69 def _add_children_to_tree(robot_model, root, tree):
70 
71 
72  # constructs the optional inertia
73  inert = kdl.RigidBodyInertia(0)
74  if root.inertial:
75  inert = _toKdlInertia(root.inertial)
76 
77  # constructs the kdl joint
78  (parent_joint_name, parent_link_name) = robot_model.parent_map[root.name]
79  parent_joint = robot_model.joint_map[parent_joint_name]
80 
81  # construct the kdl segment
82  sgm = kdl.Segment(
83  root.name,
84  _toKdlJoint(parent_joint),
85  _toKdlPose(parent_joint.origin),
86  inert)
87 
88  # add segment to tree
89  if not tree.addSegment(sgm, parent_link_name):
90  return False
91 
92  if root.name not in robot_model.child_map:
93  return True
94 
95  children = [robot_model.link_map[l] for (j,l) in robot_model.child_map[root.name]]
96 
97  # recurslively add all children
98  for child in children:
99  if not _add_children_to_tree(robot_model, child, tree):
100  return False
101 
102  return True;
103 
104 def treeFromUrdfModel(robot_model, quiet=False):
105  """
106  Construct a PyKDL.Tree from an URDF model from urdf_parser_python.
107 
108  :param robot_model: URDF xml string, ``str``
109  :param quiet: If true suppress messages to stdout, ``bool``
110  """
111 
112  root = robot_model.link_map[robot_model.get_root()]
113 
114  if root.inertial and not quiet:
115  print("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." % root.name);
116 
117  ok = True
118  tree = kdl.Tree(root.name)
119 
120  # add all children
121  for (joint,child) in robot_model.child_map[root.name]:
122  if not _add_children_to_tree(robot_model, robot_model.link_map[child], tree):
123  ok = False
124  break
125 
126  return (ok, tree)
def treeFromUrdfModel(robot_model, quiet=False)
Definition: urdf.py:104
def _toKdlInertia(i)
Definition: urdf.py:42
def treeFromFile(filename)
Definition: urdf.py:7
def _toKdlPose(pose)
Definition: urdf.py:32
def _toKdlJoint(jnt)
Definition: urdf.py:51
def treeFromParam(param)
Definition: urdf.py:16
def treeFromString(xml)
Definition: urdf.py:24
def _add_children_to_tree(robot_model, root, tree)
Definition: urdf.py:69


kdl_parser_py
Author(s): Jonathan Bohren , Jackie Kay
autogenerated on Mon Jun 10 2019 13:42:38