urdf.py
Go to the documentation of this file.
00001 # Based on the python URDF implementation by Antonio El Khoury
00002 # Available at https://github.com/laas/robot_model_py
00003 
00004 import string
00005 from naoqi_tools.gazeboUrdf import *
00006 from xml.dom.minidom import Document
00007 from xml.dom import minidom
00008 import sys
00009 from numpy import array, pi
00010 import re
00011 import copy
00012 
00013 HUBO_JOINT_SUFFIX_MASK = r'([HKASEWF][RPY12345])'
00014 
00015 
00016 class Collision(object):
00017     """Collision node stores collision geometry for a link."""
00018     def __init__(self, geometry=None, origin=None):
00019         self.geometry = geometry
00020         self.origin = origin
00021 
00022     @staticmethod
00023     def parse(node, verbose=True):
00024         c = Collision()
00025         for child in children(node):
00026             if child.localName == 'geometry':
00027                 c.geometry = Geometry.parse(child, verbose)
00028             elif child.localName == 'origin':
00029                 c.origin = Pose.parse(child)
00030             else:
00031                 print("Unknown collision element '%s'" % child.localName)
00032         return c
00033 
00034     def to_xml(self, doc):
00035         xml = doc.createElement("collision")
00036         add(doc, xml, self.geometry)
00037         add(doc, xml, self.origin)
00038         return xml
00039 
00040     def to_openrave_xml(self, doc):
00041 
00042         xml = self.geometry.to_openrave_xml(doc)
00043         add_openrave(doc, xml, self.origin)
00044         return xml
00045 
00046     def __str__(self):
00047         s = "Origin:\n{0}\n".format(reindent(str(self.origin), 1))
00048         s += "Geometry:\n{0}\n".format(reindent(str(self.geometry), 1))
00049         return s
00050 
00051 
00052 class Color(object):
00053     """Color node stores color definition for a material or a visual."""
00054     def __init__(self, r=0.0, g=0.0, b=0.0, a=0.0):
00055         self.rgba = (r, g, b, a)
00056 
00057     @staticmethod
00058     def parse(node):
00059         rgba = node.getAttribute("rgba").split()
00060         (r, g, b, a) = [float(x) for x in rgba]
00061         return Color(r, g, b, a)
00062 
00063     def to_xml(self, doc):
00064         xml = doc.createElement("color")
00065         set_attribute(xml, "rgba", self.rgba)
00066         return xml
00067 
00068     def to_openrave_xml(self, doc):
00069         return None
00070 
00071     def __str__(self):
00072         return "r: {0}, g: {1}, b: {2}, a: {3}, ".format(
00073             self.rgba[0], self.rgba[1], self.rgba[2], self.rgba[3])
00074 
00075 
00076 class Dynamics(object):
00077     """Dynamics node stores coefficients that define the dynamics of a joint"""
00078     def __init__(self, damping=None, friction=None):
00079         self.damping = damping
00080         self.friction = friction
00081 
00082     @staticmethod
00083     def parse(node):
00084         d = Dynamics()
00085         if node.hasAttribute('damping'):
00086             d.damping = node.getAttribute('damping')
00087         if node.hasAttribute('friction'):
00088             d.friction = node.getAttribute('friction')
00089         return d
00090 
00091     def to_xml(self, doc):
00092         xml = doc.createElement('dynamics')
00093         set_attribute(xml, 'damping', self.damping)
00094         set_attribute(xml, 'friction', self.friction)
00095         return xml
00096 
00097     def to_openrave_xml(self, doc):
00098         return None
00099 
00100     def __str__(self):
00101         return "Damping: {0}\nFriction: {1}\n".format(self.damping,
00102                                                       self.friction)
00103 
00104 
00105 class Geometry(object):
00106     """Geometry abstract class define the type of geomerical shape for a visual or collision element"""
00107     def __init__(self):
00108         None
00109 
00110     @staticmethod
00111     def parse(node, verbose=True):
00112         shape = children(node)[0]
00113         if shape.localName == 'box':
00114             return Box.parse(shape)
00115         elif shape.localName == 'cylinder':
00116             return Cylinder.parse(shape)
00117         elif shape.localName == 'sphere':
00118             return Sphere.parse(shape)
00119         elif shape.localName == 'mesh':
00120             return Mesh.parse(shape)
00121         else:
00122             if verbose:
00123                 print("Unknown shape %s" % shape.localName)
00124 
00125     def __str__(self):
00126         return "Geometry abstract class"
00127 
00128 
00129 class Box(Geometry):
00130     """Box node stores the dimensions for a rectangular geometry element"""
00131     def __init__(self, dims=None):
00132         if dims is None:
00133             self.dims = None
00134         else:
00135             self.dims = (dims[0], dims[1], dims[2])
00136 
00137     @staticmethod
00138     def parse(node):
00139         dims = node.getAttribute('size').split()
00140         return Box([float(a) for a in dims])
00141 
00142     def to_xml(self, doc):
00143         xml = doc.createElement("box")
00144         set_attribute(xml, "size", self.dims)
00145         geom = doc.createElement('geometry')
00146         geom.appendChild(xml)
00147         return geom
00148 
00149     def to_openrave_xml(self, doc):
00150         xml = short(doc, "geometry", "type", "box")
00151         xml.appendChild(create_element(xml, "extents", self.dims))
00152         return xml
00153 
00154     def __str__(self):
00155         return "Dimension: {0}".format(self.dims)
00156 
00157 
00158 class Cylinder(Geometry):
00159     """Cylinder node stores the dimensions for a z-rotation invariant geometry element"""
00160     def __init__(self, radius=0.0, length=0.0):
00161         self.radius = radius
00162         self.length = length
00163 
00164     @staticmethod
00165     def parse(node):
00166         r = node.getAttribute('radius')
00167         l = node.getAttribute('length')
00168         return Cylinder(float(r), float(l))
00169 
00170     def to_xml(self, doc):
00171         xml = doc.createElement("cylinder")
00172         set_attribute(xml, "radius", self.radius)
00173         set_attribute(xml, "length", self.length)
00174         geom = doc.createElement('geometry')
00175         geom.appendChild(xml)
00176         return geom
00177 
00178     def to_openrave_xml(self, doc):
00179         xml = short(doc, "geometry", "type", "cylinder")
00180         xml.appendChild(create_element(doc, "height", self.length))
00181         xml.appendChild(create_element(doc, "radius", self.radius))
00182         return xml
00183 
00184     def __str__(self):
00185         return "Radius: {0}\nLength: {1}".format(self.radius,
00186                                                  self.length)
00187 
00188 
00189 class Sphere(Geometry):
00190     """Sphere node stores the dimensions for a rotation invariant geometry element"""
00191     def __init__(self, radius=0.0):
00192         self.radius = radius
00193 
00194     @staticmethod
00195     def parse(node):
00196         r = node.getAttribute('radius')
00197         return Sphere(float(r))
00198 
00199     def to_xml(self, doc):
00200         xml = doc.createElement("sphere")
00201         set_attribute(xml, "radius", self.radius)
00202         geom = doc.createElement('geometry')
00203         geom.appendChild(xml)
00204         return geom
00205 
00206     def to_openrave_xml(self, doc):
00207         xml = short(doc, "geometry", "type", "sphere")
00208         xml.create_child(xml, "radius", self.radius)
00209         return xml
00210 
00211     def __str__(self):
00212         return "Radius: {0}".format(self.radius)
00213 
00214 
00215 class Mesh(Geometry):
00216     """Mesh node stores the path of the mesh file to be displayed"""
00217     def __init__(self, filename=None, scale=1):
00218         self.filename = filename
00219         self.scale = scale
00220 
00221     @staticmethod
00222     def parse(node):
00223         fn = node.getAttribute('filename')
00224         s = node.getAttribute('scale')
00225         if s == '':
00226             s = 1
00227         else:
00228             xyz = node.getAttribute('scale').split()
00229             s = map(float, xyz)
00230         return Mesh(fn, s)
00231 
00232     def to_xml(self, doc):
00233         xml = doc.createElement("mesh")
00234         set_attribute(xml, "filename", self.filename)
00235         if self.scale != 1:
00236             set_attribute(xml, "scale", self.scale)
00237         geom = doc.createElement('geometry')
00238         geom.appendChild(xml)
00239         return geom
00240 
00241     def to_openrave_xml(self, doc):
00242         xml = short(doc, "geometry", "type", "trimesh")
00243         f = '../meshes/' + self.filename.split('/')[-1]
00244         fhull = re.sub(r"Body", "convhull", f)
00245         # xml.appendChild(create_element(doc, "render", [f, self.scale]))
00246         xml.appendChild(create_element(doc, "data", [fhull, self.scale]))
00247         # set_attribute(xml, "render", "true")
00248         return xml
00249 
00250     def __str__(self):
00251         return "Filename: {0}\nScale: {1}".format(self.filename, self.scale)
00252 
00253 
00254 class Inertial(object):
00255     """Inertial node stores mecanical information of a Link : mass, inertia matrix and origin"""
00256     def __init__(self, ixx=0.0, ixy=0.0, ixz=0.0, iyy=0.0, iyz=0.0, izz=0.0,
00257                  mass=0.0, origin=None):
00258         self.matrix = {}
00259         self.matrix['ixx'] = ixx
00260         self.matrix['ixy'] = ixy
00261         self.matrix['ixz'] = ixz
00262         self.matrix['iyy'] = iyy
00263         self.matrix['iyz'] = iyz
00264         self.matrix['izz'] = izz
00265         self.mass = mass
00266         self.origin = origin
00267 
00268     @staticmethod
00269     def parse(node):
00270         inert = Inertial()
00271         for child in children(node):
00272             if child.localName == 'inertia':
00273                 for v in ['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz']:
00274                     inert.matrix[v] = float(child.getAttribute(v))
00275             elif child.localName == 'mass':
00276                 inert.mass = float(child.getAttribute('value'))
00277             elif child.localName == 'origin':
00278                 inert.origin = Pose.parse(child)
00279         return inert
00280 
00281     def to_xml(self, doc):
00282         xml = doc.createElement("inertial")
00283 
00284         xml.appendChild(short(doc, "mass", "value", self.mass))
00285 
00286         inertia = doc.createElement("inertia")
00287         for (n, v) in self.matrix.items():
00288             set_attribute(inertia, n, v)
00289         xml.appendChild(inertia)
00290 
00291         add(doc, xml, self.origin)
00292         return xml
00293 
00294     def to_openrave_xml(self, doc):
00295         xml = doc.createElement("mass")
00296         set_attribute(xml, "type", "custom")
00297         xml.appendChild(create_element(doc, "total", self.mass))
00298         text = '{ixx} {ixy} {ixz}\n{ixy} {iyy} {iyz}\n{ixz} {iyz} {izz}'.format(**self.matrix)
00299         xml.appendChild(create_element(doc, "inertia", text))
00300         add_openrave(doc, xml, self.origin)
00301         xml.getElementsByTagName('translation')[0].tagName = "com"
00302         return xml
00303 
00304     def __str__(self):
00305         s = "Origin:\n{0}\n".format(reindent(str(self.origin), 1))
00306         s += "Mass: {0}\n".format(self.mass)
00307         s += "ixx: {0}\n".format(self.matrix['ixx'])
00308         s += "ixy: {0}\n".format(self.matrix['ixy'])
00309         s += "ixz: {0}\n".format(self.matrix['ixz'])
00310         s += "iyy: {0}\n".format(self.matrix['iyy'])
00311         s += "iyz: {0}\n".format(self.matrix['iyz'])
00312         s += "izz: {0}\n".format(self.matrix['izz'])
00313         return s
00314 
00315 
00316 class Joint(object):
00317     """Joint node stores articulation information coupling two links"""
00318     UNKNOWN = 'unknown'
00319     REVOLUTE = 'revolute'
00320     CONTINUOUS = 'continuous'
00321     PRISMATIC = 'prismatic'
00322     FLOATING = 'floating'
00323     PLANAR = 'planar'
00324     FIXED = 'fixed'
00325 
00326     def __init__(self, name, parent, child, joint_type, axis=None, origin=None,
00327                  limits=None, dynamics=None, safety=None, calibration=None,
00328                  mimic=None):
00329         self.name = name
00330         self.parent = parent
00331         self.child = child
00332         self.joint_type = joint_type
00333         self.axis = axis
00334         self.origin = origin
00335         self.limits = limits
00336         self.dynamics = dynamics
00337         self.safety = safety
00338         self.calibration = calibration
00339         self.mimic = mimic
00340 
00341     @staticmethod
00342     def parse(node, verbose=True):
00343         joint = Joint(node.getAttribute('name'), None, None,
00344                       node.getAttribute('type'))
00345         for child in children(node):
00346             if child.localName == 'parent':
00347                 joint.parent = child.getAttribute('link')
00348             elif child.localName == 'child':
00349                 joint.child = child.getAttribute('link')
00350             elif child.localName == 'axis':
00351                 joint.axis = array([float(x) for x in child.getAttribute('xyz').split(' ')])
00352             elif child.localName == 'origin':
00353                 joint.origin = Pose.parse(child)
00354             elif child.localName == 'limit':
00355                 joint.limits = JointLimit.parse(child)
00356             elif child.localName == 'dynamics':
00357                 joint.dynamics = Dynamics.parse(child)
00358             elif child.localName == 'safety_controller':
00359                 joint.safety = SafetyController.parse(child)
00360             elif child.localName == 'calibration':
00361                 joint.calibration = JointCalibration.parse(child)
00362             elif child.localName == 'mimic':
00363                 joint.mimic = JointMimic.parse(child)
00364             else:
00365                 if verbose:
00366                     print("Unknown joint element '%s'" % child.localName)
00367         return joint
00368 
00369     def to_xml(self, doc):
00370         xml = doc.createElement("joint")
00371         set_attribute(xml, "name", self.name)
00372         set_attribute(xml, "type", self.joint_type)
00373         xml.appendChild(short(doc, "parent", "link", self.parent))
00374         xml.appendChild(short(doc, "child", "link", self.child))
00375         add(doc, xml, self.origin)
00376         if self.axis is not None:
00377             xml.appendChild(short(doc, "axis", "xyz", to_string(self.axis)))
00378         add(doc, xml, self.limits)
00379         add(doc, xml, self.dynamics)
00380         add(doc, xml, self.safety)
00381         add(doc, xml, self.calibration)
00382         add(doc, xml, self.mimic)
00383 
00384         return xml
00385 
00386     def to_openrave_xml(self, doc):
00387         xml = doc.createElement("joint")
00388         set_attribute(xml, "name", self.name)
00389         s = ""
00390         if self.joint_type == Joint.UNKNOWN:
00391             s = "unknown"
00392         elif self.joint_type == Joint.REVOLUTE:
00393             s = "hinge"
00394         elif self.joint_type == Joint.CONTINUOUS:
00395             s = "hinge"
00396             set_attribute(xml, "circular", "true")
00397         elif self.joint_type == Joint.PRISMATIC:
00398             s = "slider"
00399         elif self.joint_type == Joint.FIXED:
00400             s = "hinge"
00401             set_attribute(xml, "enable", "false")
00402             xml.appendChild(create_element(doc, "limits", "0 0"))
00403 
00404         set_attribute(xml, "type", s)
00405         if self.mimic is not None:
00406             multiplier = self.mimic.multiplier if self.mimic.multiplier is not None else 1.0
00407             offset = self.mimic.offset if self.mimic.offset is not None else 0.0
00408             # 1) Follow openrave mimic joint format, disable joint:
00409             set_attribute(xml, "enable", "false")
00410             # 2) Create the position equation
00411             set_attribute(xml, "mimic_pos", "{0} * {1} + {2}".format(self.mimic.joint_name, multiplier, offset))
00412             set_attribute(xml, "mimic_vel", "|{0} {1}".format(self.mimic.joint_name, multiplier))
00413 
00414         xml.appendChild(create_element(doc, "body", self.parent))
00415         xml.appendChild(create_element(doc, "body", self.child))
00416         xml.appendChild(create_element(doc, "offsetfrom", self.parent))
00417         add_openrave(doc, xml, self.origin)
00418         xml.getElementsByTagName('translation')[0].tagName = "anchor"
00419         if self.axis is not None:
00420             xml.appendChild(create_element(doc, "axis", self.axis))
00421         add_openrave(doc, xml, self.limits)
00422         return xml
00423 
00424     def __str__(self):
00425         s = "Name: {0}\n".format(self.name)
00426 
00427         s += "Child link name: {0}\n".format(self.child)
00428         s += "Parent link name: {0}\n".format(self.parent)
00429 
00430         if self.joint_type == Joint.UNKNOWN:
00431             s += "Type: unknown\n"
00432         elif self.joint_type == Joint.REVOLUTE:
00433             s += "Type: revolute\n"
00434         elif self.joint_type == Joint.CONTINUOUS:
00435             s += "Type: continuous\n"
00436         elif self.joint_type == Joint.PRISMATIC:
00437             s += "Type: prismatic\n"
00438         elif self.joint_type == Joint.FLOATING:
00439             s += "Type: floating\n"
00440         elif self.joint_type == Joint.PLANAR:
00441             s += "Type: planar\n"
00442         elif self.joint_type == Joint.FIXED:
00443             s += "Type: fixed\n"
00444         else:
00445             print("unknown joint type")
00446 
00447         s += "Axis: {0}\n".format(self.axis)
00448         s += "Origin:\n{0}\n".format(reindent(str(self.origin), 1))
00449         s += "Limits:\n"
00450         s += reindent(str(self.limits), 1) + "\n"
00451         s += "Dynamics:\n"
00452         s += reindent(str(self.dynamics), 1) + "\n"
00453         s += "Safety:\n"
00454         s += reindent(str(self.safety), 1) + "\n"
00455         s += "Calibration:\n"
00456         s += reindent(str(self.calibration), 1) + "\n"
00457         s += "Mimic:\n"
00458         s += reindent(str(self.mimic), 1) + "\n"
00459         return s
00460 
00461 
00462 # FIXME: we are missing the reference position here.
00463 class JointCalibration(object):
00464     def __init__(self, rising=None, falling=None):
00465         self.rising = rising
00466         self.falling = falling
00467 
00468     @staticmethod
00469     def parse(node):
00470         jc = JointCalibration()
00471         if node.hasAttribute('rising'):
00472             jc.rising = float(node.getAttribute('rising'))
00473         if node.hasAttribute('falling'):
00474             jc.falling = float(node.getAttribute('falling'))
00475         return jc
00476 
00477     def to_xml(self, doc):
00478         xml = doc.createElement('calibration')
00479         set_attribute(xml, 'rising', self.rising)
00480         set_attribute(xml, 'falling', self.falling)
00481         return xml
00482 
00483     def to_openrave_xml(self, doc):
00484         # No implementation
00485         return None
00486 
00487     def __str__(self):
00488         s = "Raising: {0}\n".format(self.rising)
00489         s += "Falling: {0}\n".format(self.falling)
00490         return s
00491 
00492 
00493 class JointLimit(object):
00494     """JointLimit node stores mechanical limits of a given joint"""
00495     def __init__(self, effort, velocity, lower=None, upper=None):
00496         self.effort = effort
00497         self.velocity = velocity
00498         self.lower = lower
00499         self.upper = upper
00500 
00501     @staticmethod
00502     def parse(node):
00503         jl = JointLimit(float(node.getAttribute('effort')),
00504                         float(node.getAttribute('velocity')))
00505         if node.hasAttribute('lower'):
00506             jl.lower = float(node.getAttribute('lower'))
00507         if node.hasAttribute('upper'):
00508             jl.upper = float(node.getAttribute('upper'))
00509         return jl
00510 
00511     def get_from_table(self, node):
00512         jl = JointLimit(float(node.getAttribute('effort')),
00513                         float(node.getAttribute('velocity')))
00514         if node.hasAttribute('lower'):
00515             jl.lower = float(node.getAttribute('lower'))
00516         if node.hasAttribute('upper'):
00517             jl.upper = float(node.getAttribute('upper'))
00518         return jl
00519 
00520     def to_xml(self, doc):
00521         xml = doc.createElement('limit')
00522         set_attribute(xml, 'effort', self.effort)
00523         set_attribute(xml, 'velocity', self.velocity)
00524         set_attribute(xml, 'lower', self.lower)
00525         set_attribute(xml, 'upper', self.upper)
00526         return xml
00527 
00528     def to_openrave_xml(self, doc):
00529         limit = create_element(doc, 'limitsdeg', [round(self.lower*180/pi, 1), round(self.upper*180/pi, 1)])
00530         maxvel = create_element(doc, 'maxvel', self.velocity)
00531         maxtrq = create_element(doc, 'maxtorque', self.effort)
00532         return [limit, maxvel, maxtrq]
00533 
00534     def __str__(self):
00535         s = "Effort: {0}\n".format(self.effort)
00536         s += "Lower: {0}\n".format(self.lower)
00537         s += "Upper: {0}\n".format(self.upper)
00538         s += "Velocity: {0}\n".format(self.velocity)
00539         return s
00540 
00541 
00542 class JointMimic(object):
00543     """JointMimic node stores information coupling an actuated joint to a not controllable joint"""
00544     def __init__(self, joint_name, multiplier=None, offset=None):
00545         self.joint_name = joint_name
00546         self.multiplier = multiplier
00547         self.offset = offset
00548 
00549     @staticmethod
00550     def parse(node):
00551         mimic = JointMimic(node.getAttribute('joint'))
00552         if node.hasAttribute('multiplier'):
00553             mimic.multiplier = float(node.getAttribute('multiplier'))
00554         if node.hasAttribute('offset'):
00555             mimic.offset = float(node.getAttribute('offset'))
00556         return mimic
00557 
00558     def to_xml(self, doc):
00559         xml = doc.createElement('mimic')
00560         set_attribute(xml, 'joint', self.joint_name)
00561         set_attribute(xml, 'multiplier', self.multiplier)
00562         set_attribute(xml, 'offset', self.offset)
00563         return xml
00564 
00565     def __str__(self):
00566         s = "Joint: {0}\n".format(self.joint_name)
00567         s += "Multiplier: {0}\n".format(self.multiplier)
00568         s += "Offset: {0}\n".format(self.offset)
00569         return s
00570 
00571 
00572 class Link(object):
00573     """Link node stores information defining the mechanics and the visualization of a link"""
00574     def __init__(self, name, visual=None, inertial=None, collision=None, xacro=None):
00575         self.name = name
00576         self.visual = visual
00577         self.inertial = inertial
00578         self.collision = collision
00579         self.xacro = xacro
00580 
00581     @staticmethod
00582     def parse(node, verbose=True):
00583         link = Link(node.getAttribute('name'))
00584         for child in children(node):
00585             if child.localName == 'visual':
00586                 link.visual = Visual.parse(child, verbose)
00587             elif child.localName == 'collision':
00588                 link.collision = Collision.parse(child, verbose)
00589             elif child.localName == 'inertial':
00590                 link.inertial = Inertial.parse(child)
00591             elif child.localName.startswith('xacro'):
00592                 link.xacro = xacro
00593             else:
00594                 if verbose:
00595                     print("Unknown link element '%s'" % child.localName)
00596         return link
00597 
00598     def to_xml(self, doc):
00599         xml = doc.createElement("link")
00600         xml.setAttribute("name", self.name)
00601         add(doc, xml, self.visual)
00602         add(doc, xml, self.collision)
00603         add(doc, xml, self.inertial)
00604         if self.xacro is not None:
00605             text = doc.createElement(self.xacro)
00606             xml.appendChild(text)
00607         return xml
00608 
00609     def to_openrave_xml(self, doc):
00610         xml = doc.createElement("body")
00611         xml.setAttribute("name", self.name)
00612         add_openrave(doc, xml, self.collision)
00613         add_openrave(doc, xml, self.inertial)
00614         return xml
00615 
00616     def __str__(self):
00617         s = "Name: {0}\n".format(self.name)
00618         s += "Inertial:\n"
00619         s += reindent(str(self.inertial), 1) + "\n"
00620         s += "Visual:\n"
00621         s += reindent(str(self.visual), 1) + "\n"
00622         s += "Collision:\n"
00623         s += reindent(str(self.collision), 1) + "\n"
00624         s += "Xacro:\n"
00625         s += reindent(str(self.xacro), 1) + "\n"
00626         return s
00627 
00628 
00629 class Material(object):
00630     """Material node stores visual information of a mechanical material : color, texture"""
00631     def __init__(self, name=None, color=None, texture=None):
00632         self.name = name
00633         self.color = color
00634         self.texture = texture
00635 
00636     @staticmethod
00637     def parse(node, verbose=True):
00638         material = Material()
00639         if node.hasAttribute('name'):
00640             material.name = node.getAttribute('name')
00641         for child in children(node):
00642             if child.localName == 'color':
00643                 material.color = Color.parse(child)
00644             elif child.localName == 'texture':
00645                 material.texture = child.getAttribute('filename')
00646             else:
00647                 if verbose:
00648                     print("Unknown material element '%s'" % child.localName)
00649 
00650         return material
00651 
00652     def to_xml(self, doc):
00653         xml = doc.createElement("material")
00654         set_attribute(xml, "name", self.name)
00655         add(doc, xml, self.color)
00656 
00657         if self.texture is not None:
00658             text = doc.createElement("texture")
00659             text.setAttribute('filename', self.texture)
00660             xml.appendChild(text)
00661         return xml
00662 
00663     def __str__(self):
00664         s = "Name: {0}\n".format(self.name)
00665         s += "Color: {0}\n".format(self.color)
00666         s += "Texture:\n"
00667         s += reindent(str(self.texture), 1)
00668         return s
00669 
00670 
00671 class Pose(object):
00672     """Pose node stores a cartesian 6-D pose :
00673     xyz : array
00674     rpy : array
00675     """
00676     def __init__(self, position=None, rotation=None):
00677         self.position = array(position)
00678         self.rotation = array(rotation)
00679 
00680     @staticmethod
00681     def parse(node):
00682         pose = Pose()
00683         if node.hasAttribute("xyz"):
00684             xyz = node.getAttribute('xyz').split()
00685             pose.position = array(map(float, xyz))
00686         if node.hasAttribute("rpy"):
00687             rpy = node.getAttribute('rpy').split()
00688             pose.rotation = array(map(float, rpy))
00689         return pose
00690 
00691     def to_xml(self, doc):
00692         xml = doc.createElement("origin")
00693         set_attribute(xml, 'xyz', self.position)
00694         set_attribute(xml, 'rpy', self.rotation)
00695         return xml
00696 
00697     def to_openrave_xml(self, doc):
00698         xml = doc.createElement("translation")
00699         set_content(doc, xml, self.position)
00700         elements = [xml]
00701         if not self.rotation[0] == 0:
00702             rotr = doc.createElement("rotationaxis")
00703             set_content(doc, rotr, [1, 0, 0, self.rotation[0] * 180.0/pi])
00704             elements.append(rotr)
00705         if not self.rotation[1] == 0:
00706             rotp = doc.createElement("rotationaxis")
00707             set_content(doc, rotp, [0, 1, 0, self.rotation[1] * 180.0/pi])
00708             elements.append(rotp)
00709         if not self.rotation[2] == 0:
00710             roty = doc.createElement("rotationaxis")
00711             set_content(doc, roty, [0, 0, 1, self.rotation[2] * 180.0/pi])
00712             elements.append(roty)
00713 
00714         return elements
00715 
00716     def __str__(self):
00717         return "Position: {0}\nRotation: {1}".format(self.position,
00718                                                      self.rotation)
00719 
00720 
00721 class SafetyController(object):
00722     def __init__(self, velocity, position=None, lower=None, upper=None):
00723         self.velocity = velocity
00724         self.position = position
00725         self.lower = lower
00726         self.upper = upper
00727 
00728     @staticmethod
00729     def parse(node):
00730         sc = SafetyController(float(node.getAttribute('k_velocity')))
00731         if node.hasAttribute('soft_lower_limit'):
00732             sc.lower = float(node.getAttribute('soft_lower_limit'))
00733         if node.hasAttribute('soft_upper_limit'):
00734             sc.upper = float(node.getAttribute('soft_upper_limit'))
00735         if node.hasAttribute('k_position'):
00736             sc.position = float(node.getAttribute('k_position'))
00737         return sc
00738 
00739     def to_xml(self, doc):
00740         xml = doc.createElement('safety_controller')
00741         set_attribute(xml, 'k_velocity', self.velocity)
00742         set_attribute(xml, 'k_position', self.position)
00743         set_attribute(xml, 'soft_upper_limit', self.upper)
00744         set_attribute(xml, 'soft_lower_limit', self.lower)
00745         return xml
00746 
00747     def __str__(self):
00748         s = "Safe lower limit: {0}\n".format(self.lower)
00749         s += "Safe upper limit: {0}\n".format(self.upper)
00750         s += "K position: {0}\n".format(self.position)
00751         s += "K velocity: {0}\n".format(self.velocity)
00752         return s
00753 
00754 
00755 class Visual(object):
00756     """Visual node stores information defining shape and material of the link to be displayed"""
00757     def __init__(self, geometry=None, material=None, origin=None):
00758         self.geometry = geometry
00759         self.material = material
00760         self.origin = origin
00761 
00762     @staticmethod
00763     def parse(node, verbose=True):
00764         v = Visual()
00765         for child in children(node):
00766             if child.localName == 'geometry':
00767                 v.geometry = Geometry.parse(child, verbose)
00768             elif child.localName == 'origin':
00769                 v.origin = Pose.parse(child)
00770             elif child.localName == 'material':
00771                 v.material = Material.parse(child, verbose)
00772             else:
00773                 if verbose:
00774                     print("Unknown visual element '%s'" % child.localName)
00775         return v
00776 
00777     def to_xml(self, doc):
00778         xml = doc.createElement("visual")
00779         add(doc, xml, self.geometry)
00780         add(doc, xml, self.origin)
00781         add(doc, xml, self.material)
00782         return xml
00783 
00784     def __str__(self):
00785         s = "Origin:\n{0}\n".format(reindent(str(self.origin), 1))
00786         s += "Geometry:\n"
00787         s += reindent(str(self.geometry), 1) + "\n"
00788         s += "Material:\n"
00789         s += reindent(str(self.material), 1) + "\n"
00790         return s
00791 
00792 
00793 class Actuator(object):
00794     """Actuator node stores information about how a motor controls a joint : used in transmission tags"""
00795     def __init__(self, name=None, hardwareInterface=None, mechanicalReduction=1):
00796         self.name = name
00797         self.hardwareInterface = hardwareInterface
00798         self.mechanicalReduction = mechanicalReduction
00799 
00800     @staticmethod
00801     def parse(node, verbose=True):
00802         actuator = Actuator()
00803         if node.hasAttribute('name'):
00804             actuator.name = node.getAttribute('name')
00805         for child in children(node):
00806             if child.localName == 'hardwareInterface':
00807                 actuator.hardwareInterface = str(child.childNodes[0].nodeValue)
00808             if child.localName == 'mechanicalReduction':
00809                 actuator.mechanicalReduction = str(child.childNodes[0].nodeValue)
00810             else:
00811                 print'Unknown actuator element ' + str(child.localName)
00812         return actuator
00813 
00814     def to_xml(self, doc):
00815         xml = doc.createElement('actuator')
00816         set_attribute(xml, 'name', self.name)
00817         xml.appendChild(create_element(doc, 'hardwareInterface', self.hardwareInterface))
00818         xml.appendChild(create_element(doc, "mechanicalReduction", str(self.mechanicalReduction)))
00819         return xml
00820 
00821     def __str__(self):
00822         s = "Name: {0}\n".format(self.name)
00823         s += "HardwareInterface : {0}\n".format(self.hardwareInterface)
00824         s += "Mechanical Reduction: {0}\n".format(self.mechanicalReduction)
00825         return s
00826 
00827 
00828 class Transmission(object):
00829     """Transmission node stores information linking a joint to an actuator"""
00830     def __init__(self, name=None, type=None, joint=None, actuator=None):
00831         self.name = name
00832         self.joint = joint
00833         self.actuator = actuator
00834         self.type = type
00835 
00836     @staticmethod
00837     def parse(node, verbose=True):
00838         trans = Transmission()
00839         if node.hasAttribute('name'):
00840             trans.name = node.getAttribute('name')
00841         for child in children(node):
00842             if child.localName == 'joint':
00843                 trans.joint = child.getAttribute('name')
00844             if child.localName == 'actuator':
00845                 trans.actuator = Actuator.parse(child, verbose)
00846             if child.localName == 'type':
00847                 trans.type = str(child.childNodes[0].nodeValue)
00848                 # str(child.getAttribute('type'))
00849         return trans
00850 
00851     def to_xml(self, doc):
00852         xml = doc.createElement('transmission')
00853         set_attribute(xml, 'name', self.name)
00854         xml.appendChild(short(doc, "joint", "name", self.joint))
00855         xml.appendChild(create_element(doc, 'type', self.type))
00856         add(doc, xml, self.actuator)
00857 
00858         return xml
00859 
00860     def __str__(self):
00861         s = "Name: {0}\n".format(self.name)
00862         s += "Type: {0}\n".format(self.type)
00863         s += "Joint: {0}\n".format(self.joint)
00864         s += "Actuator:\n"
00865         s += reindent(str(self.actuator), 1) + "\n"
00866 
00867         return s
00868 
00869 
00870 ########################################
00871 ######### URDF Global Class ############
00872 ########################################
00873 class URDF(object):
00874     """URDF node stores every information about a robot's model"""
00875     ZERO_THRESHOLD = 0.000000001
00876 
00877     def __init__(self, name=""):
00878         self.name = name
00879         self.elements = []
00880         self.gazebos = {}
00881         self.links = {}
00882         self.joints = {}
00883         self.materials = {}
00884         self.parent_map = {}
00885         self.child_map = {}
00886 
00887     @staticmethod
00888     def parse_xml_string(xml_string, verbose=True):
00889         """Parse a string to create a URDF robot structure."""
00890         urdf = URDF()
00891         base = minidom.parseString(xml_string)
00892         robot = children(base)[0]
00893         urdf.name = robot.getAttribute('name')
00894 
00895         for node in children(robot):
00896             if node.nodeType is node.TEXT_NODE:
00897                 continue
00898             if node.localName == 'joint':
00899                 urdf.add_joint(Joint.parse(node, verbose))
00900             elif node.localName == 'link':
00901                 urdf.add_link(Link.parse(node, verbose))
00902             elif node.localName == 'material':
00903                 urdf.add_material(Material.parse(node, verbose))
00904             elif node.localName == 'gazebo':
00905                 urdf.add_gazebo(Gazebo.parse(node, verbose))
00906             elif node.localName == 'transmission':
00907                 urdf.elements.append(Transmission.parse(node, verbose))
00908             else:
00909                 if verbose:
00910                     print("Unknown robot element '%s'" % node.localName)
00911         return urdf
00912 
00913     @staticmethod
00914     def load_xml_file(filename, verbose=True):
00915         """Parse a file to create a URDF robot structure."""
00916         f = open(filename, 'r')
00917         return URDF.parse_xml_string(f.read(), verbose)
00918 
00919     @staticmethod
00920     def load_from_parameter_server(key='robot_description', verbose=True):
00921         """
00922         Retrieve the robot model on the parameter server
00923         and parse it to create a URDF robot structure.
00924 
00925         Warning: this requires roscore to be running.
00926         """
00927         import rospy
00928         return URDF.parse_xml_string(rospy.get_param(key), verbose)
00929 
00930     def add_link(self, link):
00931         self.elements.append(link)
00932         self.links[link.name] = link
00933 
00934     def add_joint(self, joint):
00935         self.elements.append(joint)
00936         self.joints[joint.name] = joint
00937 
00938         self.parent_map[joint.child] = (joint.name, joint.parent)
00939         if joint.parent in self.child_map:
00940             self.child_map[joint.parent].append((joint.name, joint.child))
00941         else:
00942             self.child_map[joint.parent] = [(joint.name, joint.child)]
00943 
00944     def add_material(self, material):
00945         """
00946         Add a material to the URDF.elements list
00947         """
00948         self.elements.append(material)
00949         self.materials[material.name] = material
00950 
00951     def add_gazebo(self, gazebo):
00952         """
00953         Add gazebo data to the URDF.elements list
00954         """
00955 
00956         self.elements.append(gazebo)
00957         self.gazebos[gazebo.reference] = gazebo
00958 
00959     def get_chain(self, root, tip, joints=True, links=True, fixed=True):
00960         """Based on given link names root and tip, return either a joint or link chain as a list of names."""
00961         chain = []
00962         if links:
00963             chain.append(tip)
00964         link = tip
00965         while link != root:
00966             (joint, parent) = self.parent_map[link]
00967             if joints:
00968                 if fixed or self.joints[joint].joint_type != 'fixed':
00969                     chain.append(joint)
00970             if links:
00971                 chain.append(parent)
00972             link = parent
00973         chain.reverse()
00974         return chain
00975 
00976     def get_root(self):
00977         root = None
00978         for link in self.links:
00979             if link not in self.parent_map:
00980                 assert root is None, "Multiple roots detected, invalid URDF."
00981                 root = link
00982         assert root is not None, "No roots detected, invalid URDF."
00983         return root
00984 
00985     def to_xml(self, orderbytree=False, orderbytype=False):
00986         doc = Document()
00987         root = doc.createElement("robot")
00988         doc.appendChild(root)
00989         root.setAttribute("name", self.name)
00990 
00991         baselink = self.parent_map
00992         if orderbytree:
00993             # Walk child map sequentially and export
00994             pass
00995         if orderbytype:
00996             pass
00997 
00998         for element in self.elements:
00999             root.appendChild(element.to_xml(doc))
01000 
01001         return doc.toprettyxml()
01002 
01003     def write_xml(self, outfile=None):
01004         if outfile is None:
01005             outfile = self.name + '.urdf'
01006 
01007         self.write_reformatted(self.to_xml(), outfile)
01008 
01009     def make_openrave_kinbody(self, doc):
01010         kinbody = doc.createElement("kinbody")
01011         doc.appendChild(kinbody)
01012         kinbody.setAttribute("name", self.name)
01013 
01014         for element in self.elements:
01015             kinbody.appendChild(element.to_openrave_xml(doc))
01016 
01017         # Post-processing to add offsetfrom statements
01018 
01019         for j in self.joints.keys():
01020             for l in kinbody.getElementsByTagName('body'):
01021                 if l.getAttribute('name') == self.joints[j].child:
01022                     # Add offsetfrom declarration and joint anchor as transform
01023                     l.appendChild(create_element(doc, "offsetfrom", self.joints[j].parent))
01024                     add_openrave(doc, l, self.joints[j].origin)
01025                     break
01026 
01027         # Add adjacencies
01028         for j in self.joints.values():
01029             kinbody.appendChild(create_element(doc, "adjacent", [j.parent, j.child]))
01030 
01031         # Add known extra adjacencies
01032         badjoints = {'LAR': 'LKP', 'LWR': 'LWY', 'LSY': 'LSP', 'LHY': 'LHP',
01033                      'RAR': 'RKP', 'RWR': 'RWY', 'RSY': 'RSP', 'RHY': 'RHP',
01034                      'NK1': 'Torso'}
01035         for k, v in badjoints.items():
01036             # TODO: search all bodies for above pairs and add extra adjacency tags
01037             b1 = None
01038             b2 = None
01039             for b in kinbody.getElementsByTagName('body'):
01040                 if re.search(k, b.getAttribute('name')):
01041                     b1 = b.getAttribute('name')
01042                 if re.search(v, b.getAttribute('name')):
01043                     b2 = b.getAttribute('name')
01044             if b1 and b2:
01045                 kinbody.appendChild(create_element(doc, "adjacent", [b1, b2]))
01046 
01047         return kinbody
01048 
01049     def to_openrave_xml(self):
01050         doc = Document()
01051         root = doc.createElement("robot")
01052         doc.appendChild(root)
01053         root.setAttribute("name", self.name)
01054         root.appendChild(self.make_openrave_kinbody(doc))
01055 
01056         return doc.toprettyxml()
01057 
01058     def write_reformatted(self, data, name):
01059         if name is None:
01060             name = self.name
01061         outdata = re.sub(r'\t', '    ', data)
01062         with open(name, 'w+') as f:
01063             f.write(outdata)
01064 
01065     def write_openrave_files(self, outname=None, writerobot=False):
01066         if outname is None:
01067             outname = self.name
01068 
01069         kinfile = outname + '.kinbody.xml'
01070 
01071         if writerobot:
01072             robotfile = outname + '.robot.xml'
01073             doc = Document()
01074             root = doc.createElement("robot")
01075             doc.appendChild(root)
01076             root.setAttribute("name", outname)
01077             kinbody = doc.createElement("kinbody")
01078             root.appendChild(kinbody)
01079             kinbody.setAttribute("name", outname)
01080             kinbody.setAttribute("file", kinfile)
01081             self.write_reformatted(doc.toprettyxml(), robotfile)
01082 
01083         doc2 = Document()
01084         doc2.appendChild(self.make_openrave_kinbody(doc2))
01085 
01086         self.write_reformatted(doc2.toprettyxml(), kinfile)
01087 
01088     def __str__(self):
01089         s = "Name: {0}\n".format(self.name)
01090         s += "\n"
01091         s += "Links:\n"
01092         if len(self.links) == 0:
01093             s += "None\n"
01094         else:
01095             for k, v in self.links.iteritems():
01096                 s += "- Link '{0}':\n{1}\n".format(k, reindent(str(v), 1))
01097         s += "\n"
01098         s += "Joints:\n"
01099         if len(self.joints) == 0:
01100             s += "None\n"
01101         else:
01102             for k, v in self.joints.iteritems():
01103                 s += "- Joint '{0}':\n{1}\n".format(k, reindent(str(v), 1))
01104         s += "\n"
01105         s += "Materials:\n"
01106         if len(self.materials) == 0:
01107             s += "None\n"
01108         else:
01109             for k, v in self.materials.iteritems():
01110                 s += "- Material '{0}':\n{1}\n".format(k, reindent(str(v), 1))
01111 
01112         return s
01113 
01114     def walk_chain(self, link, branchorder=None):
01115         """Walk along the first branch of urdf tree to find last link. Optionally specify which fork to take globally)."""
01116         child = link
01117         if branchorder is None:
01118             branchorder = 0
01119 
01120         while self.child_map in child:
01121             children = self.child_map[link]
01122             l = len(children)
01123             child = children[min(branchorder, l - 1)][1]
01124 
01125         return child
01126 
01127     def rename_link(self, link, newlink):
01128         """Rename a link, updating all internal references to the name, and
01129         removing the old name from the links dict."""
01130         self.links[link].name = newlink
01131         self.links[newlink] = self.links[link]
01132         self.links.pop(link)
01133         for k, v in self.parent_map.items():
01134             if k == link:
01135                 self.parent_map[newlink] = v
01136                 self.parent_map.pop(k)
01137                 k = newlink
01138             if v[0] == link:
01139                 new0 = newlink
01140                 v = (new0, v[1])
01141             if v[1] == link:
01142                 new1 = newlink
01143                 v = (v[0], new1)
01144             self.parent_map[k] = v
01145         for k, v in self.child_map.items():
01146             if k == link:
01147                 self.child_map[newlink] = v
01148                 self.child_map.pop(k)
01149                 k = newlink
01150             vnew = []
01151             for el in v:
01152                 if el[1] == link:
01153                     el = (el[0], newlink)
01154                 vnew.append(el)
01155             # print(vnew)
01156             self.child_map[k] = vnew
01157         for n, j in self.joints.items():
01158             if j.parent == link:
01159                 j.parent = newlink
01160             if j.child == link:
01161                 j.child = newlink
01162         # print(self.child_map)
01163 
01164     def rename_joint(self, joint, newjoint):
01165         """Find a joint and rename it to newjoint, updating all internal
01166         structures and mimic joints with the new name. Removes the old joint
01167         reference from the joints list."""
01168         self.joints[joint].name = newjoint
01169         self.joints[newjoint] = self.joints[joint]
01170         self.joints.pop(joint)
01171         for k, v in self.child_map.items():
01172             vnew = []
01173             for el in v:
01174                 if el[0] == joint:
01175                     el = (newjoint, el[1])
01176                 print(el)
01177                 vnew.append(el)
01178             self.child_map[k] = vnew
01179         for k, v in self.parent_map.items():
01180             if v[0] == joint:
01181                 v = (newjoint, v[1])
01182             print(el)
01183             self.parent_map[k] = v
01184         for n, j in self.joints.items():
01185             if j.mimic is not None:
01186                 j.mimic.joint_name = newjoint if j.mimic.joint_name == joint else j.mimic.joint_name
01187 
01188     def copy_joint(self, joint, f, r):
01189         """Copy and rename a joint and it's parent/child by the f / r strings. Assumes links exist.
01190         Note that it renames the parent and child, so use this with copy_link"""
01191         newjoint = copy.deepcopy(self.joints[joint])
01192         newjoint.name = re.sub(f, r, newjoint.name)
01193         newjoint.parent = re.sub(f, r, newjoint.parent)
01194         newjoint.child = re.sub(f, r, newjoint.child)
01195 
01196         self.add_joint(newjoint)
01197         return newjoint
01198 
01199     def move_chain_with_rottrans(self, root, tip, rpy, xyz, f, r):
01200         """Find and rename a kinematic chain based on the given root and top
01201         names. Renames all links and joints according to find and replace
01202         terms.
01203         """
01204         linkchain = self.get_chain(root, tip, links=True, joints=False)
01205         jointchain = self.get_chain(root, tip, joints=True, links=False)
01206         print(linkchain)
01207         print(jointchain)
01208 
01209         for l in linkchain[1:]:
01210             newlink = re.sub(f, r, l)
01211             self.rename_link(l, newlink)
01212         for j in jointchain:
01213             newname = re.sub(f, r, j)
01214             self.rename_joint(j, newname)
01215             self.joints[newname].origin.position += array(xyz)
01216             self.joints[newname].origin.rotation += array(rpy)
01217             if self.joints[newname].mimic is not None:
01218                 self.joints[newname].mimic.joint_name = re.sub(f, r, self.joints[newname].mimic.joint_name)
01219 
01220     def copy_chain_with_rottrans(self, root, tip, rpy, xyz, f, r, mir_ax=None):
01221         """Copy a kinematic chain, renaming joints and links according to a regular expression.
01222 
01223         Note that this is designed to work with the Hubo joint / body
01224         convention, which has an easy pattern for joint and body names. If your
01225         model has joints and links that are not systematically named, this
01226         function won't be much use.
01227         """
01228 
01229         linkchain = self.get_chain(root, tip, links=True, joints=False)
01230         jointchain = self.get_chain(root, tip, joints=True, links=False)
01231         print(linkchain)
01232         print(jointchain)
01233         newjoints = []
01234         newlinks = []
01235         for l in linkchain[1:]:
01236             newlink = copy.deepcopy(self.links[l])
01237             newlink.name = re.sub(f, r, newlink.name)
01238             if newlink.collision is not None:
01239                 newlink.collision.geometry.filename = re.sub(f, r, newlink.collision.geometry.filename)
01240                 newlink.visual.geometry.filename = re.sub(f, r, newlink.visual.geometry.filename)
01241             self.add_link(newlink)
01242             newlinks.append(newlink)
01243             if mir_ax == 'x':
01244                 newlink.inertial.matrix['ixy'] *= -1.
01245                 newlink.inertial.matrix['ixz'] *= -1.
01246                 newlink.inertial.origin.position[0] *= -1.
01247             if mir_ax == 'y':
01248                 newlink.inertial.matrix['ixy'] *= -1.
01249                 newlink.inertial.matrix['iyz'] *= -1.
01250                 newlink.inertial.origin.position[1] *= -1.
01251             if mir_ax == 'z':
01252                 newlink.inertial.matrix['ixz'] *= -1.
01253                 newlink.inertial.matrix['iyz'] *= -1.
01254                 newlink.inertial.origin.position[2] *= -1.
01255         # Hack to rotate just first joint
01256         for j in jointchain:
01257             newjoints.append(self.copy_joint(j, f, r))
01258             if mir_ax == 'x':
01259                 newjoints[-1].origin.position[0] *= -1.0
01260                 newjoints[-1].origin.rotation[1] *= -1.0
01261                 newjoints[-1].origin.rotation[2] *= -1.0
01262             if mir_ax == 'y':
01263                 newjoints[-1].origin.position[1] *= -1.0
01264                 newjoints[-1].origin.rotation[0] *= -1.0
01265                 newjoints[-1].origin.rotation[2] *= -1.0
01266             if mir_ax == 'z':
01267                 newjoints[-1].origin.position[2] *= -1.0
01268                 newjoints[-1].origin.rotation[0] *= -1.0
01269                 newjoints[-1].origin.rotation[1] *= -1.0
01270 
01271         if mir_ax == 'rotx':
01272             newjoints[0].origin.position[1] *= -1.0
01273 
01274         for j in newjoints:
01275             if j.mimic is not None:
01276                 j.mimic.joint_name = re.sub(f, r, j.mimic.joint_name)
01277 
01278         newjoints[0].origin.position += array(xyz)
01279         newjoints[0].origin.rotation += array(rpy)
01280 
01281     def fix_mesh_case(self):
01282         for l in self.links:
01283             fname = l.collision.geometry.filename
01284             l.collision.geometry.filename = re.sub(r'\.[Ss][Tt][Ll]', '.stl', fname)
01285     # TODO: merge function to tie two chains together from disparate models
01286 
01287     def update_mesh_paths(self, package_name):
01288         """Search and replace package paths in urdf with chosen package name"""
01289         for n, l in self.links.items():
01290             # TODO: check if mesh
01291             for g in [l.collision.geometry, l.visual.geometry]:
01292                 # save STL file name only
01293                 meshfile = g.filename.split('/')[-1]
01294                 newpath = [package_name, 'meshes', meshfile]
01295                 cleanpath = re.sub('/+', '/', '/'.join(newpath))
01296                 g.filename = 'package://' + cleanpath
01297             l.collision.geometry.filename = re.sub('Body', 'convhull', l.collision.geometry.filename)
01298 
01299     def apply_default_limits(self, effort, vel, lower, upper, mask=None):
01300         """Apply default limits to all joints and convert continous joints to revolute. Ignores fixed and other joint types."""
01301         for n, j in self.joints.items():
01302             if mask is None or re.search(mask, n):
01303                 if j.joint_type == Joint.CONTINUOUS or j.joint_type == Joint.REVOLUTE:
01304                     j.limits = JointLimit(effort, vel, lower, upper)
01305                     j.joint_type = Joint.REVOLUTE
01306 
01307 if __name__ == '__main__':
01308     # try:
01309         # from openhubo import startup
01310     # except ImportError:
01311         # pass
01312 
01313     try:
01314         filename = sys.argv[1]
01315     except IndexError:
01316         print('Please supply a URDF filename to convert!')
01317 
01318     try:
01319         outname = sys.argv[2]
01320     except IndexError:
01321         outname = None
01322 
01323     model = URDF.load_xml_file(filename)
01324 
01325     model.write_openrave_files(outname)


naoqi_tools
Author(s): Mikael Arguedas
autogenerated on Sat Jun 8 2019 20:30:21