urdf.py
Go to the documentation of this file.
00001 from urdf_parser_py.xml_reflection.basics import *
00002 import urdf_parser_py.xml_reflection as xmlr
00003 
00004 # Add a 'namespace' for names to avoid a conflict between URDF and SDF?
00005 # A type registry? How to scope that? Just make a 'global' type pointer?
00006 # Or just qualify names? urdf.geometric, sdf.geometric
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     # Aliases for backwards compatibility
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 # Common stuff
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         # What about named colors?
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 # FIXME: we are missing the reference position here.
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 # FIXME: we are missing __str__ here.
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)  # noqa
00329 
00330     # Aliases
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         # Could move this into xml_reflection
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),  # Is 'name' a required attribute?
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 # Make an alias
00529 URDF = Robot
00530 
00531 xmlr.end_namespace()


urdfdom_py
Author(s): Thomas Moulard, David Lu, Kelsey Hawkins, Antonio El Khoury, Eric Cousineau, Ioan Sucan , Jackie Kay
autogenerated on Fri Jul 28 2017 05:47:59