8 xmlr.start_namespace(
'urdf')
10 xmlr.add_type(
'element_link', xmlr.SimpleElementType(
'link', str))
11 xmlr.add_type(
'element_xyz', xmlr.SimpleElementType(
'xyz',
'vector3'))
22 assert (self.
xyz is None or len(self.
xyz) == 3)
and \
23 (self.
rpy is None or len(self.
rpy) == 3)
39 xmlr.reflect(Pose, tag=
'origin', params=[
40 xmlr.Attribute(
'xyz',
'vector3',
False, default=[0, 0, 0]),
41 xmlr.Attribute(
'rpy',
'vector3',
False, default=[0, 0, 0])
46 name_attribute = xmlr.Attribute(
'name', str)
47 origin_element = xmlr.Element(
'origin', Pose,
False)
54 if count == 4
or count == 3:
60 if self.
rgba is not None:
61 if len(self.
rgba) == 3:
63 if len(self.
rgba) != 4:
64 raise Exception(
'Invalid color argument count')
67 xmlr.reflect(Color, tag=
'color', params=[
68 xmlr.Attribute(
'rgba',
'vector4')
73 def __init__(self, damping=None, friction=None):
78 xmlr.reflect(JointDynamics, tag=
'dynamics', params=[
79 xmlr.Attribute(
'damping', float,
False),
80 xmlr.Attribute(
'friction', float,
False)
84 class Box(xmlr.Object):
89 xmlr.reflect(Box, tag=
'box', params=[
90 xmlr.Attribute(
'size',
'vector3')
100 xmlr.reflect(Cylinder, tag=
'cylinder', params=[
101 xmlr.Attribute(
'radius', float),
102 xmlr.Attribute(
'length', float)
111 xmlr.reflect(Sphere, tag=
'sphere', params=[
112 xmlr.Attribute(
'radius', float)
122 xmlr.reflect(Mesh, tag=
'mesh', params=[
123 xmlr.Attribute(
'filename', str),
124 xmlr.Attribute(
'scale',
'vector3', required=
False)
130 self.
factory = xmlr.FactoryType(
'geometric', {
132 'cylinder': Cylinder,
139 assert len(children) == 1,
'One element only for geometric'
143 name = self.
factory.get_name(obj)
152 def __init__(self, geometry=None, origin=None, name=None):
158 xmlr.reflect(Collision, tag=
'collision', params=[
159 xmlr.Attribute(
'name', str,
False),
161 xmlr.Element(
'geometry',
'geometric')
170 xmlr.reflect(Texture, tag=
'texture', params=[
171 xmlr.Attribute(
'filename', str)
176 def __init__(self, name=None, color=None, texture=None):
183 xmlr.on_error(
"Material has neither a color nor texture.")
186 xmlr.reflect(Material, tag=
'material', params=[
188 xmlr.Element(
'color', Color,
False),
189 xmlr.Element(
'texture', Texture,
False)
198 class Visual(xmlr.Object):
199 def __init__(self, geometry=None, material=None, origin=None, name=None):
206 xmlr.reflect(Visual, tag=
'visual', params=[
207 xmlr.Attribute(
'name', str,
False),
209 xmlr.Element(
'geometry',
'geometric'),
210 xmlr.Element(
'material', LinkMaterial,
False)
215 KEYS = [
'ixx',
'ixy',
'ixz',
'iyy',
'iyz',
'izz']
217 def __init__(self, ixx=0.0, ixy=0.0, ixz=0.0, iyy=0.0, iyz=0.0, izz=0.0):
232 xmlr.reflect(Inertia, tag=
'inertia',
233 params=[xmlr.Attribute(key, float)
for key
in Inertia.KEYS])
237 def __init__(self, mass=0.0, inertia=None, origin=None):
243 xmlr.reflect(Inertial, tag=
'inertial', params=[
245 xmlr.Element(
'mass',
'element_value'),
246 xmlr.Element(
'inertia', Inertia,
False)
257 xmlr.reflect(JointCalibration, tag=
'calibration', params=[
258 xmlr.Attribute(
'rising', float,
False, 0),
259 xmlr.Attribute(
'falling', float,
False, 0)
264 def __init__(self, effort=None, velocity=None, lower=None, upper=None):
271 xmlr.reflect(JointLimit, tag=
'limit', params=[
272 xmlr.Attribute(
'effort', float),
273 xmlr.Attribute(
'lower', float,
False, 0),
274 xmlr.Attribute(
'upper', float,
False, 0),
275 xmlr.Attribute(
'velocity', float)
282 def __init__(self, joint_name=None, multiplier=None, offset=None):
288 xmlr.reflect(JointMimic, tag=
'mimic', params=[
289 xmlr.Attribute(
'joint', str),
290 xmlr.Attribute(
'multiplier', float,
False),
291 xmlr.Attribute(
'offset', float,
False)
296 def __init__(self, velocity=None, position=None, lower=None, upper=None):
303 xmlr.reflect(SafetyController, tag=
'safety_controller', params=[
304 xmlr.Attribute(
'k_velocity', float),
305 xmlr.Attribute(
'k_position', float,
False, 0),
306 xmlr.Attribute(
'soft_lower_limit', float,
False, 0),
307 xmlr.Attribute(
'soft_upper_limit', float,
False, 0)
312 TYPES = [
'unknown',
'revolute',
'continuous',
'prismatic',
313 'floating',
'planar',
'fixed']
315 def __init__(self, name=None, parent=None, child=None, joint_type=None,
316 axis=None, origin=None,
317 limit=None, dynamics=None, safety_controller=None,
318 calibration=None, mimic=None):
332 assert self.
type in self.
TYPES,
"Invalid joint type: {}".format(self.
type)
341 xmlr.reflect(Joint, tag=
'joint', params=[
343 xmlr.Attribute(
'type', str),
345 xmlr.Element(
'axis',
'element_xyz',
False),
346 xmlr.Element(
'parent',
'element_link'),
347 xmlr.Element(
'child',
'element_link'),
348 xmlr.Element(
'limit', JointLimit,
False),
349 xmlr.Element(
'dynamics', JointDynamics,
False),
350 xmlr.Element(
'safety_controller', SafetyController,
False),
351 xmlr.Element(
'calibration', JointCalibration,
False),
352 xmlr.Element(
'mimic', JointMimic,
False),
357 def __init__(self, name=None, visual=None, inertial=None, collision=None,
359 self.aggregate_init()
371 """Return the first visual or None."""
376 """Set the first visual."""
382 self.add_aggregate(
'visual', visual)
385 """Return the first collision or None."""
390 """Set the first collision."""
396 self.add_aggregate(
'collision', collision)
399 visual = property(__get_visual, __set_visual)
400 collision = property(__get_collision, __set_collision)
403 xmlr.reflect(Link, tag=
'link', params=[
406 xmlr.AggregateElement(
'visual', Visual),
407 xmlr.AggregateElement(
'collision', Collision),
408 xmlr.Element(
'inertial', Inertial,
False),
413 def __init__(self, name=None, joint=None, actuator=None, type=None,
414 mechanicalReduction=1):
422 xmlr.reflect(PR2Transmission, tag=
'pr2_transmission', params=[
424 xmlr.Attribute(
'type', str),
425 xmlr.Element(
'joint',
'element_name'),
426 xmlr.Element(
'actuator',
'element_name'),
427 xmlr.Element(
'mechanicalReduction', float)
432 def __init__(self, name=None, mechanicalReduction=1):
437 xmlr.reflect(Actuator, tag=
'actuator', params=[
439 xmlr.Element(
'mechanicalReduction', float, required=
False)
445 self.aggregate_init()
453 xmlr.reflect(TransmissionJoint, tag=
'joint', params=[
455 xmlr.AggregateElement(
'hardwareInterface', str),
460 """ New format: http://wiki.ros.org/urdf/XML/Transmission """
463 self.aggregate_init()
469 assert len(self.
joints) > 0,
"no joint defined"
470 assert len(self.
actuators) > 0,
"no actuator defined"
473 xmlr.reflect(Transmission, tag=
'new_transmission', params=[
475 xmlr.Element(
'type', str),
476 xmlr.AggregateElement(
'joint', TransmissionJoint),
477 xmlr.AggregateElement(
'actuator', Actuator)
480 xmlr.add_type(
'transmission',
481 xmlr.DuckTypedFactory(
'transmission',
482 [Transmission, PR2Transmission]))
486 SUPPORTED_VERSIONS = [
"1.0"]
489 self.aggregate_init()
493 raise ValueError(
"Invalid version; only %s is supported" % (
','.join(self.
SUPPORTED_VERSIONS)))
509 xmlr.Object.add_aggregate(self, typeName, elem)
511 if typeName ==
'joint':
514 self.
parent_map[joint.child] = (joint.name, joint.parent)
516 self.
child_map[joint.parent].append((joint.name, joint.child))
518 self.
child_map[joint.parent] = [(joint.name, joint.child)]
519 elif typeName ==
'link':
529 def get_chain(self, root, tip, joints=True, links=True, fixed=True):
537 if fixed
or self.
joint_map[joint].joint_type !=
'fixed':
549 assert root
is None,
"Multiple roots detected, invalid URDF."
551 assert root
is not None,
"No roots detected, invalid URDF."
558 split = self.
version.split(
".")
560 raise ValueError(
"The version attribute should be in the form 'x.y'")
562 if split[0] ==
'' or split[1] ==
'':
563 raise ValueError(
"Empty major or minor number is not allowed")
565 if int(split[0]) < 0
or int(split[1]) < 0:
566 raise ValueError(
"Version number must be positive")
569 raise ValueError(
"Invalid version; only %s is supported" % (
','.join(self.
SUPPORTED_VERSIONS)))
574 Retrieve the robot model on the parameter server
575 and parse it to create a URDF robot structure.
577 Warning: this requires roscore to be running.
581 return cls.from_xml_string(rospy.get_param(key))
584 xmlr.reflect(Robot, tag=
'robot', params=[
585 xmlr.Attribute(
'name', str),
586 xmlr.Attribute(
'version', str,
False),
587 xmlr.AggregateElement(
'link', Link),
588 xmlr.AggregateElement(
'joint', Joint),
589 xmlr.AggregateElement(
'gazebo', xmlr.RawType()),
590 xmlr.AggregateElement(
'transmission',
'transmission'),
591 xmlr.AggregateElement(
'material', Material)