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


pr2_calibration_launch
Author(s): Vijay Pradeep
autogenerated on Wed Apr 3 2019 02:59:37