urdf.py
Go to the documentation of this file.
3 
4 # Add a 'namespace' for names to avoid a conflict between URDF and SDF?
5 # A type registry? How to scope that? Just make a 'global' type pointer?
6 # Or just qualify names? urdf.geometric, sdf.geometric
7 
8 xmlr.start_namespace('urdf')
9 
10 xmlr.add_type('element_link', xmlr.SimpleElementType('link', str))
11 xmlr.add_type('element_xyz', xmlr.SimpleElementType('xyz', 'vector3'))
12 
13 verbose = True
14 
15 
16 class Pose(xmlr.Object):
17  def __init__(self, xyz=None, rpy=None):
18  self.xyz = xyz
19  self.rpy = rpy
20 
21  def check_valid(self):
22  assert (self.xyz is None or len(self.xyz) == 3) and \
23  (self.rpy is None or len(self.rpy) == 3)
24 
25  # Aliases for backwards compatibility
26  @property
27  def rotation(self): return self.rpy
28 
29  @rotation.setter
30  def rotation(self, value): self.rpy = value
31 
32  @property
33  def position(self): return self.xyz
34 
35  @position.setter
36  def position(self, value): self.xyz = value
37 
38 
39 xmlr.reflect(Pose, params=[
40  xmlr.Attribute('xyz', 'vector3', False, default=[0, 0, 0]),
41  xmlr.Attribute('rpy', 'vector3', False, default=[0, 0, 0])
42 ])
43 
44 
45 # Common stuff
46 name_attribute = xmlr.Attribute('name', str)
47 origin_element = xmlr.Element('origin', Pose, False)
48 
49 
50 class Color(xmlr.Object):
51  def __init__(self, *args):
52  # What about named colors?
53  count = len(args)
54  if count == 4 or count == 3:
55  self.rgba = args
56  elif count == 1:
57  self.rgba = args[0]
58  elif count == 0:
59  self.rgba = None
60  if self.rgba is not None:
61  if len(self.rgba) == 3:
62  self.rgba += [1.]
63  if len(self.rgba) != 4:
64  raise Exception('Invalid color argument count')
65 
66 
67 xmlr.reflect(Color, params=[
68  xmlr.Attribute('rgba', 'vector4')
69 ])
70 
71 
72 class JointDynamics(xmlr.Object):
73  def __init__(self, damping=None, friction=None):
74  self.damping = damping
75  self.friction = friction
76 
77 
78 xmlr.reflect(JointDynamics, params=[
79  xmlr.Attribute('damping', float, False),
80  xmlr.Attribute('friction', float, False)
81 ])
82 
83 
84 class Box(xmlr.Object):
85  def __init__(self, size=None):
86  self.size = size
87 
88 
89 xmlr.reflect(Box, params=[
90  xmlr.Attribute('size', 'vector3')
91 ])
92 
93 
94 class Cylinder(xmlr.Object):
95  def __init__(self, radius=0.0, length=0.0):
96  self.radius = radius
97  self.length = length
98 
99 
100 xmlr.reflect(Cylinder, params=[
101  xmlr.Attribute('radius', float),
102  xmlr.Attribute('length', float)
103 ])
104 
105 
106 class Sphere(xmlr.Object):
107  def __init__(self, radius=0.0):
108  self.radius = radius
109 
110 
111 xmlr.reflect(Sphere, params=[
112  xmlr.Attribute('radius', float)
113 ])
114 
115 
116 class Mesh(xmlr.Object):
117  def __init__(self, filename=None, scale=None):
118  self.filename = filename
119  self.scale = scale
120 
121 
122 xmlr.reflect(Mesh, params=[
123  xmlr.Attribute('filename', str),
124  xmlr.Attribute('scale', 'vector3', required=False)
125 ])
126 
127 
128 class GeometricType(xmlr.ValueType):
129  def __init__(self):
130  self.factory = xmlr.FactoryType('geometric', {
131  'box': Box,
132  'cylinder': Cylinder,
133  'sphere': Sphere,
134  'mesh': Mesh
135  })
136 
137  def from_xml(self, node):
138  children = xml_children(node)
139  assert len(children) == 1, 'One element only for geometric'
140  return self.factory.from_xml(children[0])
141 
142  def write_xml(self, node, obj):
143  name = self.factory.get_name(obj)
144  child = node_add(node, name)
145  obj.write_xml(child)
146 
147 
148 xmlr.add_type('geometric', GeometricType())
149 
150 
151 class Collision(xmlr.Object):
152  def __init__(self, geometry=None, origin=None):
153  self.geometry = geometry
154  self.origin = origin
155 
156 
157 xmlr.reflect(Collision, params=[
158  origin_element,
159  xmlr.Element('geometry', 'geometric')
160 ])
161 
162 
163 class Texture(xmlr.Object):
164  def __init__(self, filename=None):
165  self.filename = filename
166 
167 
168 xmlr.reflect(Texture, params=[
169  xmlr.Attribute('filename', str)
170 ])
171 
172 
173 class Material(xmlr.Object):
174  def __init__(self, name=None, color=None, texture=None):
175  self.name = name
176  self.color = color
177  self.texture = texture
178 
179  def check_valid(self):
180  if self.color is None and self.texture is None:
181  xmlr.on_error("Material has neither a color nor texture.")
182 
183 
184 xmlr.reflect(Material, params=[
185  name_attribute,
186  xmlr.Element('color', Color, False),
187  xmlr.Element('texture', Texture, False)
188 ])
189 
190 
192  def check_valid(self):
193  pass
194 
195 
196 class Visual(xmlr.Object):
197  def __init__(self, geometry=None, material=None, origin=None):
198  self.geometry = geometry
199  self.material = material
200  self.origin = origin
201 
202 
203 xmlr.reflect(Visual, params=[
204  origin_element,
205  xmlr.Element('geometry', 'geometric'),
206  xmlr.Element('material', LinkMaterial, False)
207 ])
208 
209 
210 class Inertia(xmlr.Object):
211  KEYS = ['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz']
212 
213  def __init__(self, ixx=0.0, ixy=0.0, ixz=0.0, iyy=0.0, iyz=0.0, izz=0.0):
214  self.ixx = ixx
215  self.ixy = ixy
216  self.ixz = ixz
217  self.iyy = iyy
218  self.iyz = iyz
219  self.izz = izz
220 
221  def to_matrix(self):
222  return [
223  [self.ixx, self.ixy, self.ixz],
224  [self.ixy, self.iyy, self.iyz],
225  [self.ixz, self.iyz, self.izz]]
226 
227 
228 xmlr.reflect(Inertia,
229  params=[xmlr.Attribute(key, float) for key in Inertia.KEYS])
230 
231 
232 class Inertial(xmlr.Object):
233  def __init__(self, mass=0.0, inertia=None, origin=None):
234  self.mass = mass
235  self.inertia = inertia
236  self.origin = origin
237 
238 
239 xmlr.reflect(Inertial, params=[
240  origin_element,
241  xmlr.Element('mass', 'element_value'),
242  xmlr.Element('inertia', Inertia, False)
243 ])
244 
245 
246 # FIXME: we are missing the reference position here.
247 class JointCalibration(xmlr.Object):
248  def __init__(self, rising=None, falling=None):
249  self.rising = rising
250  self.falling = falling
251 
252 
253 xmlr.reflect(JointCalibration, params=[
254  xmlr.Attribute('rising', float, False, 0),
255  xmlr.Attribute('falling', float, False, 0)
256 ])
257 
258 
259 class JointLimit(xmlr.Object):
260  def __init__(self, effort=None, velocity=None, lower=None, upper=None):
261  self.effort = effort
262  self.velocity = velocity
263  self.lower = lower
264  self.upper = upper
265 
266 
267 xmlr.reflect(JointLimit, params=[
268  xmlr.Attribute('effort', float),
269  xmlr.Attribute('lower', float, False, 0),
270  xmlr.Attribute('upper', float, False, 0),
271  xmlr.Attribute('velocity', float)
272 ])
273 
274 # FIXME: we are missing __str__ here.
275 
276 
277 class JointMimic(xmlr.Object):
278  def __init__(self, joint_name=None, multiplier=None, offset=None):
279  self.joint = joint_name
280  self.multiplier = multiplier
281  self.offset = offset
282 
283 
284 xmlr.reflect(JointMimic, params=[
285  xmlr.Attribute('joint', str),
286  xmlr.Attribute('multiplier', float, False),
287  xmlr.Attribute('offset', float, False)
288 ])
289 
290 
291 class SafetyController(xmlr.Object):
292  def __init__(self, velocity=None, position=None, lower=None, upper=None):
293  self.k_velocity = velocity
294  self.k_position = position
295  self.soft_lower_limit = lower
296  self.soft_upper_limit = upper
297 
298 
299 xmlr.reflect(SafetyController, params=[
300  xmlr.Attribute('k_velocity', float),
301  xmlr.Attribute('k_position', float, False, 0),
302  xmlr.Attribute('soft_lower_limit', float, False, 0),
303  xmlr.Attribute('soft_upper_limit', float, False, 0)
304 ])
305 
306 
307 class Joint(xmlr.Object):
308  TYPES = ['unknown', 'revolute', 'continuous', 'prismatic',
309  'floating', 'planar', 'fixed']
310 
311  def __init__(self, name=None, parent=None, child=None, joint_type=None,
312  axis=None, origin=None,
313  limit=None, dynamics=None, safety_controller=None,
314  calibration=None, mimic=None):
315  self.name = name
316  self.parent = parent
317  self.child = child
318  self.type = joint_type
319  self.axis = axis
320  self.origin = origin
321  self.limit = limit
322  self.dynamics = dynamics
323  self.safety_controller = safety_controller
324  self.calibration = calibration
325  self.mimic = mimic
326 
327  def check_valid(self):
328  assert self.type in self.TYPES, "Invalid joint type: {}".format(self.type) # noqa
329 
330  # Aliases
331  @property
332  def joint_type(self): return self.type
333 
334  @joint_type.setter
335  def joint_type(self, value): self.type = value
336 
337 xmlr.reflect(Joint, params=[
338  name_attribute,
339  xmlr.Attribute('type', str),
340  origin_element,
341  xmlr.Element('axis', 'element_xyz', False),
342  xmlr.Element('parent', 'element_link'),
343  xmlr.Element('child', 'element_link'),
344  xmlr.Element('limit', JointLimit, False),
345  xmlr.Element('dynamics', JointDynamics, False),
346  xmlr.Element('safety_controller', SafetyController, False),
347  xmlr.Element('calibration', JointCalibration, False),
348  xmlr.Element('mimic', JointMimic, False),
349 ])
350 
351 
352 class Link(xmlr.Object):
353  def __init__(self, name=None, visual=None, inertial=None, collision=None,
354  origin=None):
355  self.name = name
356  self.visual = visual
357  self.inertial = inertial
358  self.collision = collision
359  self.origin = origin
360 
361 xmlr.reflect(Link, params=[
362  name_attribute,
363  origin_element,
364  xmlr.Element('inertial', Inertial, False),
365  xmlr.Element('visual', Visual, False),
366  xmlr.Element('collision', Collision, False)
367 ])
368 
369 
370 class PR2Transmission(xmlr.Object):
371  def __init__(self, name=None, joint=None, actuator=None, type=None,
372  mechanicalReduction=1):
373  self.name = name
374  self.type = type
375  self.joint = joint
376  self.actuator = actuator
377  self.mechanicalReduction = mechanicalReduction
378 
379 
380 xmlr.reflect(PR2Transmission, tag='pr2_transmission', params=[
381  name_attribute,
382  xmlr.Attribute('type', str),
383  xmlr.Element('joint', 'element_name'),
384  xmlr.Element('actuator', 'element_name'),
385  xmlr.Element('mechanicalReduction', float)
386 ])
387 
388 
389 class Actuator(xmlr.Object):
390  def __init__(self, name=None, mechanicalReduction=1):
391  self.name = name
393 
394 
395 xmlr.reflect(Actuator, tag='actuator', params=[
396  name_attribute,
397  xmlr.Element('mechanicalReduction', float, required=False)
398 ])
399 
400 
401 class TransmissionJoint(xmlr.Object):
402  def __init__(self, name=None):
403  self.aggregate_init()
404  self.name = name
406 
407  def check_valid(self):
408  assert len(self.hardwareInterfaces) > 0, "no hardwareInterface defined"
409 
410 
411 xmlr.reflect(TransmissionJoint, tag='joint', params=[
412  name_attribute,
413  xmlr.AggregateElement('hardwareInterface', str),
414 ])
415 
416 
417 class Transmission(xmlr.Object):
418  """ New format: http://wiki.ros.org/urdf/XML/Transmission """
419 
420  def __init__(self, name=None):
421  self.aggregate_init()
422  self.name = name
423  self.joints = []
424  self.actuators = []
425 
426  def check_valid(self):
427  assert len(self.joints) > 0, "no joint defined"
428  assert len(self.actuators) > 0, "no actuator defined"
429 
430 
431 xmlr.reflect(Transmission, tag='new_transmission', params=[
432  name_attribute,
433  xmlr.Element('type', str),
434  xmlr.AggregateElement('joint', TransmissionJoint),
435  xmlr.AggregateElement('actuator', Actuator)
436 ])
437 
438 xmlr.add_type('transmission',
439  xmlr.DuckTypedFactory('transmission',
440  [Transmission, PR2Transmission]))
441 
442 
443 class Robot(xmlr.Object):
444  def __init__(self, name=None):
445  self.aggregate_init()
446 
447  self.name = name
448  self.joints = []
449  self.links = []
450  self.materials = []
451  self.gazebos = []
452  self.transmissions = []
453 
454  self.joint_map = {}
455  self.link_map = {}
456 
457  self.parent_map = {}
458  self.child_map = {}
459 
460  def add_aggregate(self, typeName, elem):
461  xmlr.Object.add_aggregate(self, typeName, elem)
462 
463  if typeName == 'joint':
464  joint = elem
465  self.joint_map[joint.name] = joint
466  self.parent_map[joint.child] = (joint.name, joint.parent)
467  if joint.parent in self.child_map:
468  self.child_map[joint.parent].append((joint.name, joint.child))
469  else:
470  self.child_map[joint.parent] = [(joint.name, joint.child)]
471  elif typeName == 'link':
472  link = elem
473  self.link_map[link.name] = link
474 
475  def add_link(self, link):
476  self.add_aggregate('link', link)
477 
478  def add_joint(self, joint):
479  self.add_aggregate('joint', joint)
480 
481  def get_chain(self, root, tip, joints=True, links=True, fixed=True):
482  chain = []
483  if links:
484  chain.append(tip)
485  link = tip
486  while link != root:
487  (joint, parent) = self.parent_map[link]
488  if joints:
489  if fixed or self.joint_map[joint].joint_type != 'fixed':
490  chain.append(joint)
491  if links:
492  chain.append(parent)
493  link = parent
494  chain.reverse()
495  return chain
496 
497  def get_root(self):
498  root = None
499  for link in self.link_map:
500  if link not in self.parent_map:
501  assert root is None, "Multiple roots detected, invalid URDF."
502  root = link
503  assert root is not None, "No roots detected, invalid URDF."
504  return root
505 
506  @classmethod
507  def from_parameter_server(cls, key='robot_description'):
508  """
509  Retrieve the robot model on the parameter server
510  and parse it to create a URDF robot structure.
511 
512  Warning: this requires roscore to be running.
513  """
514  # Could move this into xml_reflection
515  import rospy
516  return cls.from_xml_string(rospy.get_param(key))
517 
518 
519 xmlr.reflect(Robot, tag='robot', params=[
520  xmlr.Attribute('name', str, False), # Is 'name' a required attribute?
521  xmlr.AggregateElement('link', Link),
522  xmlr.AggregateElement('joint', Joint),
523  xmlr.AggregateElement('gazebo', xmlr.RawType()),
524  xmlr.AggregateElement('transmission', 'transmission'),
525  xmlr.AggregateElement('material', Material)
526 ])
527 
528 # Make an alias
529 URDF = Robot
530 
531 xmlr.end_namespace()
def __init__(self, name=None)
Definition: urdf.py:402
def from_parameter_server(cls, key='robot_description')
Definition: urdf.py:507
def position(self)
Definition: urdf.py:33
def check_valid(self)
Definition: urdf.py:21
def add_aggregate(self, typeName, elem)
Definition: urdf.py:460
def add_joint(self, joint)
Definition: urdf.py:478
def __init__(self, xyz=None, rpy=None)
Definition: urdf.py:17
def check_valid(self)
Definition: urdf.py:327
def __init__(self, name=None, parent=None, child=None, joint_type=None, axis=None, origin=None, limit=None, dynamics=None, safety_controller=None, calibration=None, mimic=None)
Definition: urdf.py:314
def __init__(self, joint_name=None, multiplier=None, offset=None)
Definition: urdf.py:278
def __init__(self, geometry=None, origin=None)
Definition: urdf.py:152
def __init__(self, size=None)
Definition: urdf.py:85
def __init__(self, radius=0.0)
Definition: urdf.py:107
def get_chain(self, root, tip, joints=True, links=True, fixed=True)
Definition: urdf.py:481
def from_xml(self, node)
Definition: urdf.py:137
def rotation(self)
Definition: urdf.py:27
def __init__(self, name=None, joint=None, actuator=None, type=None, mechanicalReduction=1)
Definition: urdf.py:372
def __init__(self, effort=None, velocity=None, lower=None, upper=None)
Definition: urdf.py:260
def __init__(self, name=None)
Definition: urdf.py:420
def __init__(self, ixx=0.0, ixy=0.0, ixz=0.0, iyy=0.0, iyz=0.0, izz=0.0)
Definition: urdf.py:213
def __init__(self, name=None, mechanicalReduction=1)
Definition: urdf.py:390
def __init__(self, velocity=None, position=None, lower=None, upper=None)
Definition: urdf.py:292
def joint_type(self)
Definition: urdf.py:332
def __init__(self, mass=0.0, inertia=None, origin=None)
Definition: urdf.py:233
def __init__(self, name=None, color=None, texture=None)
Definition: urdf.py:174
def __init__(self, geometry=None, material=None, origin=None)
Definition: urdf.py:197
def __init__(self, filename=None, scale=None)
Definition: urdf.py:117
def __init__(self, radius=0.0, length=0.0)
Definition: urdf.py:95
def write_xml(self, node, obj)
Definition: urdf.py:142
def __init__(self, rising=None, falling=None)
Definition: urdf.py:248
def __init__(self, damping=None, friction=None)
Definition: urdf.py:73
def __init__(self, args)
Definition: urdf.py:51
def add_link(self, link)
Definition: urdf.py:475
def __init__(self, name=None)
Definition: urdf.py:444
def __init__(self, filename=None)
Definition: urdf.py:164
def get_root(self)
Definition: urdf.py:497


urdfdom_py
Author(s): Thomas Moulard, David Lu, Kelsey Hawkins, Antonio El Khoury, Eric Cousineau, Ioan Sucan , Jackie Kay
autogenerated on Fri Jun 7 2019 21:42:12