00001 from urdf_parser_py.xml_reflection.basics import *
00002 import urdf_parser_py.xml_reflection as xmlr
00003
00004
00005
00006
00007
00008 xmlr.start_namespace('urdf')
00009
00010 xmlr.add_type('element_link', xmlr.SimpleElementType('link', str))
00011 xmlr.add_type('element_xyz', xmlr.SimpleElementType('xyz', 'vector3'))
00012
00013 verbose = True
00014
00015
00016 class Pose(xmlr.Object):
00017 def __init__(self, xyz=None, rpy=None):
00018 self.xyz = xyz
00019 self.rpy = rpy
00020
00021 def check_valid(self):
00022 assert (self.xyz is None or len(self.xyz) == 3) and \
00023 (self.rpy is None or len(self.rpy) == 3)
00024
00025
00026 @property
00027 def rotation(self): return self.rpy
00028
00029 @rotation.setter
00030 def rotation(self, value): self.rpy = value
00031
00032 @property
00033 def position(self): return self.xyz
00034
00035 @position.setter
00036 def position(self, value): self.xyz = value
00037
00038
00039 xmlr.reflect(Pose, params=[
00040 xmlr.Attribute('xyz', 'vector3', False, default=[0, 0, 0]),
00041 xmlr.Attribute('rpy', 'vector3', False, default=[0, 0, 0])
00042 ])
00043
00044
00045
00046 name_attribute = xmlr.Attribute('name', str)
00047 origin_element = xmlr.Element('origin', Pose, False)
00048
00049
00050 class Color(xmlr.Object):
00051 def __init__(self, *args):
00052
00053 count = len(args)
00054 if count == 4 or count == 3:
00055 self.rgba = args
00056 elif count == 1:
00057 self.rgba = args[0]
00058 elif count == 0:
00059 self.rgba = None
00060 if self.rgba is not None:
00061 if len(self.rgba) == 3:
00062 self.rgba += [1.]
00063 if len(self.rgba) != 4:
00064 raise Exception('Invalid color argument count')
00065
00066
00067 xmlr.reflect(Color, params=[
00068 xmlr.Attribute('rgba', 'vector4')
00069 ])
00070
00071
00072 class JointDynamics(xmlr.Object):
00073 def __init__(self, damping=None, friction=None):
00074 self.damping = damping
00075 self.friction = friction
00076
00077
00078 xmlr.reflect(JointDynamics, params=[
00079 xmlr.Attribute('damping', float, False),
00080 xmlr.Attribute('friction', float, False)
00081 ])
00082
00083
00084 class Box(xmlr.Object):
00085 def __init__(self, size=None):
00086 self.size = size
00087
00088
00089 xmlr.reflect(Box, params=[
00090 xmlr.Attribute('size', 'vector3')
00091 ])
00092
00093
00094 class Cylinder(xmlr.Object):
00095 def __init__(self, radius=0.0, length=0.0):
00096 self.radius = radius
00097 self.length = length
00098
00099
00100 xmlr.reflect(Cylinder, params=[
00101 xmlr.Attribute('radius', float),
00102 xmlr.Attribute('length', float)
00103 ])
00104
00105
00106 class Sphere(xmlr.Object):
00107 def __init__(self, radius=0.0):
00108 self.radius = radius
00109
00110
00111 xmlr.reflect(Sphere, params=[
00112 xmlr.Attribute('radius', float)
00113 ])
00114
00115
00116 class Mesh(xmlr.Object):
00117 def __init__(self, filename=None, scale=None):
00118 self.filename = filename
00119 self.scale = scale
00120
00121
00122 xmlr.reflect(Mesh, params=[
00123 xmlr.Attribute('filename', str),
00124 xmlr.Attribute('scale', 'vector3', required=False)
00125 ])
00126
00127
00128 class GeometricType(xmlr.ValueType):
00129 def __init__(self):
00130 self.factory = xmlr.FactoryType('geometric', {
00131 'box': Box,
00132 'cylinder': Cylinder,
00133 'sphere': Sphere,
00134 'mesh': Mesh
00135 })
00136
00137 def from_xml(self, node):
00138 children = xml_children(node)
00139 assert len(children) == 1, 'One element only for geometric'
00140 return self.factory.from_xml(children[0])
00141
00142 def write_xml(self, node, obj):
00143 name = self.factory.get_name(obj)
00144 child = node_add(node, name)
00145 obj.write_xml(child)
00146
00147
00148 xmlr.add_type('geometric', GeometricType())
00149
00150
00151 class Collision(xmlr.Object):
00152 def __init__(self, geometry=None, origin=None):
00153 self.geometry = geometry
00154 self.origin = origin
00155
00156
00157 xmlr.reflect(Collision, params=[
00158 origin_element,
00159 xmlr.Element('geometry', 'geometric')
00160 ])
00161
00162
00163 class Texture(xmlr.Object):
00164 def __init__(self, filename=None):
00165 self.filename = filename
00166
00167
00168 xmlr.reflect(Texture, params=[
00169 xmlr.Attribute('filename', str)
00170 ])
00171
00172
00173 class Material(xmlr.Object):
00174 def __init__(self, name=None, color=None, texture=None):
00175 self.name = name
00176 self.color = color
00177 self.texture = texture
00178
00179 def check_valid(self):
00180 if self.color is None and self.texture is None:
00181 xmlr.on_error("Material has neither a color nor texture.")
00182
00183
00184 xmlr.reflect(Material, params=[
00185 name_attribute,
00186 xmlr.Element('color', Color, False),
00187 xmlr.Element('texture', Texture, False)
00188 ])
00189
00190
00191 class LinkMaterial(Material):
00192 def check_valid(self):
00193 pass
00194
00195
00196 class Visual(xmlr.Object):
00197 def __init__(self, geometry=None, material=None, origin=None):
00198 self.geometry = geometry
00199 self.material = material
00200 self.origin = origin
00201
00202
00203 xmlr.reflect(Visual, params=[
00204 origin_element,
00205 xmlr.Element('geometry', 'geometric'),
00206 xmlr.Element('material', LinkMaterial, False)
00207 ])
00208
00209
00210 class Inertia(xmlr.Object):
00211 KEYS = ['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz']
00212
00213 def __init__(self, ixx=0.0, ixy=0.0, ixz=0.0, iyy=0.0, iyz=0.0, izz=0.0):
00214 self.ixx = ixx
00215 self.ixy = ixy
00216 self.ixz = ixz
00217 self.iyy = iyy
00218 self.iyz = iyz
00219 self.izz = izz
00220
00221 def to_matrix(self):
00222 return [
00223 [self.ixx, self.ixy, self.ixz],
00224 [self.ixy, self.iyy, self.iyz],
00225 [self.ixz, self.iyz, self.izz]]
00226
00227
00228 xmlr.reflect(Inertia,
00229 params=[xmlr.Attribute(key, float) for key in Inertia.KEYS])
00230
00231
00232 class Inertial(xmlr.Object):
00233 def __init__(self, mass=0.0, inertia=None, origin=None):
00234 self.mass = mass
00235 self.inertia = inertia
00236 self.origin = origin
00237
00238
00239 xmlr.reflect(Inertial, params=[
00240 origin_element,
00241 xmlr.Element('mass', 'element_value'),
00242 xmlr.Element('inertia', Inertia, False)
00243 ])
00244
00245
00246
00247 class JointCalibration(xmlr.Object):
00248 def __init__(self, rising=None, falling=None):
00249 self.rising = rising
00250 self.falling = falling
00251
00252
00253 xmlr.reflect(JointCalibration, params=[
00254 xmlr.Attribute('rising', float, False, 0),
00255 xmlr.Attribute('falling', float, False, 0)
00256 ])
00257
00258
00259 class JointLimit(xmlr.Object):
00260 def __init__(self, effort=None, velocity=None, lower=None, upper=None):
00261 self.effort = effort
00262 self.velocity = velocity
00263 self.lower = lower
00264 self.upper = upper
00265
00266
00267 xmlr.reflect(JointLimit, params=[
00268 xmlr.Attribute('effort', float),
00269 xmlr.Attribute('lower', float, False, 0),
00270 xmlr.Attribute('upper', float, False, 0),
00271 xmlr.Attribute('velocity', float)
00272 ])
00273
00274
00275
00276
00277 class JointMimic(xmlr.Object):
00278 def __init__(self, joint_name=None, multiplier=None, offset=None):
00279 self.joint = joint_name
00280 self.multiplier = multiplier
00281 self.offset = offset
00282
00283
00284 xmlr.reflect(JointMimic, params=[
00285 xmlr.Attribute('joint', str),
00286 xmlr.Attribute('multiplier', float, False),
00287 xmlr.Attribute('offset', float, False)
00288 ])
00289
00290
00291 class SafetyController(xmlr.Object):
00292 def __init__(self, velocity=None, position=None, lower=None, upper=None):
00293 self.k_velocity = velocity
00294 self.k_position = position
00295 self.soft_lower_limit = lower
00296 self.soft_upper_limit = upper
00297
00298
00299 xmlr.reflect(SafetyController, params=[
00300 xmlr.Attribute('k_velocity', float),
00301 xmlr.Attribute('k_position', float, False, 0),
00302 xmlr.Attribute('soft_lower_limit', float, False, 0),
00303 xmlr.Attribute('soft_upper_limit', float, False, 0)
00304 ])
00305
00306
00307 class Joint(xmlr.Object):
00308 TYPES = ['unknown', 'revolute', 'continuous', 'prismatic',
00309 'floating', 'planar', 'fixed']
00310
00311 def __init__(self, name=None, parent=None, child=None, joint_type=None,
00312 axis=None, origin=None,
00313 limit=None, dynamics=None, safety_controller=None,
00314 calibration=None, mimic=None):
00315 self.name = name
00316 self.parent = parent
00317 self.child = child
00318 self.type = joint_type
00319 self.axis = axis
00320 self.origin = origin
00321 self.limit = limit
00322 self.dynamics = dynamics
00323 self.safety_controller = safety_controller
00324 self.calibration = calibration
00325 self.mimic = mimic
00326
00327 def check_valid(self):
00328 assert self.type in self.TYPES, "Invalid joint type: {}".format(self.type)
00329
00330
00331 @property
00332 def joint_type(self): return self.type
00333
00334 @joint_type.setter
00335 def joint_type(self, value): self.type = value
00336
00337 xmlr.reflect(Joint, params=[
00338 name_attribute,
00339 xmlr.Attribute('type', str),
00340 origin_element,
00341 xmlr.Element('axis', 'element_xyz', False),
00342 xmlr.Element('parent', 'element_link'),
00343 xmlr.Element('child', 'element_link'),
00344 xmlr.Element('limit', JointLimit, False),
00345 xmlr.Element('dynamics', JointDynamics, False),
00346 xmlr.Element('safety_controller', SafetyController, False),
00347 xmlr.Element('calibration', JointCalibration, False),
00348 xmlr.Element('mimic', JointMimic, False),
00349 ])
00350
00351
00352 class Link(xmlr.Object):
00353 def __init__(self, name=None, visual=None, inertial=None, collision=None,
00354 origin=None):
00355 self.name = name
00356 self.visual = visual
00357 self.inertial = inertial
00358 self.collision = collision
00359 self.origin = origin
00360
00361 xmlr.reflect(Link, params=[
00362 name_attribute,
00363 origin_element,
00364 xmlr.Element('inertial', Inertial, False),
00365 xmlr.Element('visual', Visual, False),
00366 xmlr.Element('collision', Collision, False)
00367 ])
00368
00369
00370 class PR2Transmission(xmlr.Object):
00371 def __init__(self, name=None, joint=None, actuator=None, type=None,
00372 mechanicalReduction=1):
00373 self.name = name
00374 self.type = type
00375 self.joint = joint
00376 self.actuator = actuator
00377 self.mechanicalReduction = mechanicalReduction
00378
00379
00380 xmlr.reflect(PR2Transmission, tag='pr2_transmission', params=[
00381 name_attribute,
00382 xmlr.Attribute('type', str),
00383 xmlr.Element('joint', 'element_name'),
00384 xmlr.Element('actuator', 'element_name'),
00385 xmlr.Element('mechanicalReduction', float)
00386 ])
00387
00388
00389 class Actuator(xmlr.Object):
00390 def __init__(self, name=None, mechanicalReduction=1):
00391 self.name = name
00392 self.mechanicalReduction = None
00393
00394
00395 xmlr.reflect(Actuator, tag='actuator', params=[
00396 name_attribute,
00397 xmlr.Element('mechanicalReduction', float, required=False)
00398 ])
00399
00400
00401 class TransmissionJoint(xmlr.Object):
00402 def __init__(self, name=None):
00403 self.aggregate_init()
00404 self.name = name
00405 self.hardwareInterfaces = []
00406
00407 def check_valid(self):
00408 assert len(self.hardwareInterfaces) > 0, "no hardwareInterface defined"
00409
00410
00411 xmlr.reflect(TransmissionJoint, tag='joint', params=[
00412 name_attribute,
00413 xmlr.AggregateElement('hardwareInterface', str),
00414 ])
00415
00416
00417 class Transmission(xmlr.Object):
00418 """ New format: http://wiki.ros.org/urdf/XML/Transmission """
00419
00420 def __init__(self, name=None):
00421 self.aggregate_init()
00422 self.name = name
00423 self.joints = []
00424 self.actuators = []
00425
00426 def check_valid(self):
00427 assert len(self.joints) > 0, "no joint defined"
00428 assert len(self.actuators) > 0, "no actuator defined"
00429
00430
00431 xmlr.reflect(Transmission, tag='new_transmission', params=[
00432 name_attribute,
00433 xmlr.Element('type', str),
00434 xmlr.AggregateElement('joint', TransmissionJoint),
00435 xmlr.AggregateElement('actuator', Actuator)
00436 ])
00437
00438 xmlr.add_type('transmission',
00439 xmlr.DuckTypedFactory('transmission',
00440 [Transmission, PR2Transmission]))
00441
00442
00443 class Robot(xmlr.Object):
00444 def __init__(self, name=None):
00445 self.aggregate_init()
00446
00447 self.name = name
00448 self.joints = []
00449 self.links = []
00450 self.materials = []
00451 self.gazebos = []
00452 self.transmissions = []
00453
00454 self.joint_map = {}
00455 self.link_map = {}
00456
00457 self.parent_map = {}
00458 self.child_map = {}
00459
00460 def add_aggregate(self, typeName, elem):
00461 xmlr.Object.add_aggregate(self, typeName, elem)
00462
00463 if typeName == 'joint':
00464 joint = elem
00465 self.joint_map[joint.name] = joint
00466 self.parent_map[joint.child] = (joint.name, joint.parent)
00467 if joint.parent in self.child_map:
00468 self.child_map[joint.parent].append((joint.name, joint.child))
00469 else:
00470 self.child_map[joint.parent] = [(joint.name, joint.child)]
00471 elif typeName == 'link':
00472 link = elem
00473 self.link_map[link.name] = link
00474
00475 def add_link(self, link):
00476 self.add_aggregate('link', link)
00477
00478 def add_joint(self, joint):
00479 self.add_aggregate('joint', joint)
00480
00481 def get_chain(self, root, tip, joints=True, links=True, fixed=True):
00482 chain = []
00483 if links:
00484 chain.append(tip)
00485 link = tip
00486 while link != root:
00487 (joint, parent) = self.parent_map[link]
00488 if joints:
00489 if fixed or self.joint_map[joint].joint_type != 'fixed':
00490 chain.append(joint)
00491 if links:
00492 chain.append(parent)
00493 link = parent
00494 chain.reverse()
00495 return chain
00496
00497 def get_root(self):
00498 root = None
00499 for link in self.link_map:
00500 if link not in self.parent_map:
00501 assert root is None, "Multiple roots detected, invalid URDF."
00502 root = link
00503 assert root is not None, "No roots detected, invalid URDF."
00504 return root
00505
00506 @classmethod
00507 def from_parameter_server(cls, key='robot_description'):
00508 """
00509 Retrieve the robot model on the parameter server
00510 and parse it to create a URDF robot structure.
00511
00512 Warning: this requires roscore to be running.
00513 """
00514
00515 import rospy
00516 return cls.from_xml_string(rospy.get_param(key))
00517
00518
00519 xmlr.reflect(Robot, tag='robot', params=[
00520 xmlr.Attribute('name', str, False),
00521 xmlr.AggregateElement('link', Link),
00522 xmlr.AggregateElement('joint', Joint),
00523 xmlr.AggregateElement('gazebo', xmlr.RawType()),
00524 xmlr.AggregateElement('transmission', 'transmission'),
00525 xmlr.AggregateElement('material', Material)
00526 ])
00527
00528
00529 URDF = Robot
00530
00531 xmlr.end_namespace()