00001 import rospy
00002 import xml.dom.minidom
00003 from xml.dom.minidom import Document
00004 from geometry_msgs.msg import Pose as GPose, Vector3, Point, Quaternion
00005 import tf.transformations
00006
00007 def add(doc, base, element):
00008 if element is not None:
00009 base.appendChild( element.to_xml(doc) )
00010
00011 def pfloat(x):
00012 return ('%f'%x).rstrip('0').rstrip('.')
00013
00014 def set_attribute(node, name, value):
00015 if value is None:
00016 return
00017 if type([]) == type(value) or type(()) == type(value):
00018 value = " ".join( [pfloat(a) for a in value] )
00019 elif type(value) == type(0.0):
00020 value = pfloat(value)
00021 elif type(value) != type(''):
00022 value = str(value)
00023 node.setAttribute(name, value)
00024
00025 def short(doc, name, key, value):
00026 element = doc.createElement(name)
00027 set_attribute(element, key, value)
00028 return element
00029
00030 def children(node):
00031 children = []
00032 for child in node.childNodes:
00033 if child.nodeType is node.TEXT_NODE or child.nodeType is node.COMMENT_NODE:
00034 continue
00035 else:
00036 children.append(child)
00037 return children
00038
00039 class Collision:
00040 def __init__(self, geometry=None, origin=None):
00041 self.geometry = geometry
00042 self.origin = origin
00043
00044 @staticmethod
00045 def parse(node):
00046 c = Collision()
00047 for child in children(node):
00048 if child.localName == 'geometry':
00049 c.geometry = Geometry.parse(child)
00050 elif child.localName == 'origin':
00051 c.origin = Pose.parse(child)
00052 else:
00053 rospy.logwarn("Unknown collision element '%s'"%child.localName)
00054 return c
00055
00056 def to_xml(self, doc):
00057 xml = doc.createElement("collision")
00058 add(doc, xml, self.geometry)
00059 add(doc, xml, self.origin)
00060 return xml
00061
00062 class Color:
00063 def __init__(self, r=0.0, g=0.0, b=0.0, a=0.0):
00064 self.rgba=(r,g,b,a)
00065
00066 @staticmethod
00067 def parse(node):
00068 rgba = node.getAttribute("rgba").split()
00069 (r,g,b,a) = [ float(x) for x in rgba ]
00070 return Color(r,g,b,a)
00071
00072 def to_xml(self, doc):
00073 xml = doc.createElement("color")
00074 set_attribute(xml, "rgba", self.rgba)
00075 return xml
00076
00077 class Dynamics:
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
00098 class Geometry:
00099 def __init__(self):
00100 None
00101
00102 @staticmethod
00103 def parse(node):
00104 shape = children(node)[0]
00105 if shape.localName=='box':
00106 return Box.parse(shape)
00107 elif shape.localName=='cylinder':
00108 return Cylinder.parse(shape)
00109 elif shape.localName=='sphere':
00110 return Sphere.parse(shape)
00111 elif shape.localName=='mesh':
00112 return Mesh.parse(shape)
00113 else:
00114 rospy.logwarn("Unknown shape %s"%child.localName)
00115
00116 class Box(Geometry):
00117 def __init__(self, dims=None):
00118 if dims is None:
00119 self.dims = None
00120 else:
00121 self.dims = (dims[0], dims[1], dims[2])
00122
00123 @staticmethod
00124 def parse(node):
00125 dims = node.getAttribute('size').split()
00126 return Box([float(a) for a in dims])
00127
00128 def to_xml(self, doc):
00129 xml = doc.createElement("box")
00130 set_attribute(xml, "size", self.dims)
00131 geom = doc.createElement('geometry')
00132 geom.appendChild(xml)
00133 return geom
00134
00135 class Cylinder(Geometry):
00136 def __init__(self, radius=0.0, length=0.0):
00137 self.radius = radius
00138 self.length = length
00139
00140 @staticmethod
00141 def parse(node):
00142 r = node.getAttribute('radius')
00143 l = node.getAttribute('length')
00144 return Cylinder(float(r), float(l))
00145
00146 def to_xml(self, doc):
00147 xml = doc.createElement("cylinder")
00148 set_attribute(xml, "radius", self.radius)
00149 set_attribute(xml, "length", self.length)
00150 geom = doc.createElement('geometry')
00151 geom.appendChild(xml)
00152 return geom
00153
00154 class Sphere(Geometry):
00155 def __init__(self, radius=0.0):
00156 self.radius = radius
00157
00158 @staticmethod
00159 def parse(node):
00160 r = node.getAttribute('radius')
00161 return Sphere(float(r))
00162
00163 def to_xml(self, doc):
00164 xml = doc.createElement("sphere")
00165 set_attribute(xml, "radius", self.radius)
00166 geom = doc.createElement('geometry')
00167 geom.appendChild(xml)
00168 return geom
00169
00170 class Mesh(Geometry):
00171 def __init__(self, filename=None, scale=None):
00172 self.filename = filename
00173 self.scale = scale
00174
00175 @staticmethod
00176 def parse(node):
00177 fn = node.getAttribute('filename')
00178 s = node.getAttribute('scale')
00179 if s == "":
00180 s = None
00181 else:
00182 xyz = node.getAttribute('scale').split()
00183 scale = map(float, xyz)
00184 return Mesh(fn, s)
00185
00186 def to_xml(self, doc):
00187 xml = doc.createElement("mesh")
00188 set_attribute(xml, "filename", self.filename)
00189 set_attribute(xml, "scale", self.scale)
00190 geom = doc.createElement('geometry')
00191 geom.appendChild(xml)
00192 return geom
00193
00194 class Inertial:
00195 def __init__(self, ixx=0.0, ixy=0.0, ixz=0.0, iyy=0.0, iyz=0.0, izz=0.0, mass=0.0, origin=None):
00196 self.matrix = {}
00197 self.matrix['ixx'] = ixx
00198 self.matrix['ixy'] = iyy
00199 self.matrix['ixz'] = ixz
00200 self.matrix['iyy'] = iyy
00201 self.matrix['iyz'] = iyz
00202 self.matrix['izz'] = izz
00203 self.mass = mass
00204 self.origin = origin
00205
00206 @staticmethod
00207 def parse(node):
00208 inert = Inertial()
00209 for child in children(node):
00210 if child.localName=='inertia':
00211 for v in ['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz']:
00212 inert.matrix[v] = float(child.getAttribute(v))
00213 elif child.localName=='mass':
00214 inert.mass = float(child.getAttribute('value'))
00215 elif child.localName=='origin':
00216 inert.origin = Pose.parse(child)
00217 return inert
00218
00219 def to_xml(self, doc):
00220 xml = doc.createElement("inertial")
00221
00222 xml.appendChild(short(doc, "mass", "value", self.mass))
00223
00224 inertia = doc.createElement("inertia")
00225 for (n,v) in self.matrix.items():
00226 set_attribute(inertia, n, v)
00227 xml.appendChild(inertia)
00228
00229 add(doc, xml, self.origin)
00230 return xml
00231
00232 class Joint:
00233 def __init__(self, name, parent, child, joint_type, axis=None, origin=None, limits=None,
00234 dynamics=None, safety=None, calibration=None, mimic=None):
00235 self.name = name
00236 self.parent = parent
00237 self.child = child
00238 self.joint_type = joint_type
00239 self.axis = axis
00240 self.origin = origin
00241 self.limits = limits
00242 self.dynamics = dynamics
00243 self.safety = safety
00244 self.calibration = calibration
00245 self.mimic = mimic
00246
00247 @staticmethod
00248 def parse(node):
00249 joint = Joint(node.getAttribute('name'), None, None, node.getAttribute('type'))
00250 for child in children(node):
00251 if child.localName == 'parent':
00252 joint.parent = child.getAttribute('link')
00253 elif child.localName == 'child':
00254 joint.child = child.getAttribute('link')
00255 elif child.localName == 'axis':
00256 joint.axis = child.getAttribute('xyz')
00257 elif child.localName == 'origin':
00258 joint.origin = Pose.parse(child)
00259 elif child.localName == 'limit':
00260 joint.limits = JointLimit.parse(child)
00261 elif child.localName == 'dynamics':
00262 joint.dynamics = Dynamics.parse(child)
00263 elif child.localName == 'safety_controller':
00264 joint.safety = SafetyController.parse(child)
00265 elif child.localName == 'calibration':
00266 joint.calibration = JointCalibration.parse(child)
00267 elif child.localName == 'mimic':
00268 joint.mimic = JointMimic.parse(child)
00269 else:
00270 rospy.logwarn("Unknown joint element '%s'"%child.localName)
00271 return joint
00272
00273 def to_xml(self, doc):
00274 xml = doc.createElement("joint")
00275 set_attribute(xml, "name", self.name)
00276 set_attribute(xml, "type", self.joint_type)
00277 xml.appendChild( short(doc, "parent", "link", self.parent) )
00278 xml.appendChild( short(doc, "child" , "link", self.child ) )
00279 add(doc, xml, self.origin)
00280 if self.axis is not None:
00281 xml.appendChild( short(doc, "axis", "xyz", self.axis) )
00282 add(doc, xml, self.limits)
00283 add(doc, xml, self.dynamics)
00284 add(doc, xml, self.safety)
00285 add(doc, xml, self.calibration)
00286 return xml
00287
00288 class JointCalibration:
00289 def __init__(self, rising=None, falling=None, reference=None):
00290 self.rising = rising
00291 self.falling = falling
00292 self.reference = reference
00293
00294 @staticmethod
00295 def parse(node):
00296 jc = JointCalibration()
00297 if node.hasAttribute('rising'):
00298 jc.rising = float( node.getAttribute('rising') )
00299 if node.hasAttribute('falling'):
00300 jc.falling = float( node.getAttribute('falling') )
00301 if node.hasAttribute('reference'):
00302 jc.reference = float( node.getAttribute('reference') )
00303 return jc
00304
00305 def to_xml(self, doc):
00306 xml = doc.createElement('calibration')
00307 set_attribute(xml, 'rising', self.rising)
00308 set_attribute(xml, 'falling', self.falling)
00309 set_attribute(xml, 'reference', self.reference)
00310 return xml
00311
00312
00313 class JointLimit:
00314 def __init__(self, effort, velocity, lower=None, upper=None):
00315 self.effort = effort
00316 self.velocity = velocity
00317 self.lower = lower
00318 self.upper = upper
00319
00320 @staticmethod
00321 def parse(node):
00322 jl = JointLimit( float( node.getAttribute('effort') ) ,
00323 float( node.getAttribute('velocity')))
00324 if node.hasAttribute('lower'):
00325 jl.lower = float( node.getAttribute('lower') )
00326 if node.hasAttribute('upper'):
00327 jl.upper = float( node.getAttribute('upper') )
00328 return jl
00329
00330 def to_xml(self, doc):
00331 xml = doc.createElement('limit')
00332 set_attribute(xml, 'effort', self.effort)
00333 set_attribute(xml, 'velocity', self.velocity)
00334 set_attribute(xml, 'lower', self.lower)
00335 set_attribute(xml, 'upper', self.upper)
00336 return xml
00337
00338 def __str__(self):
00339 if self.lower is not None:
00340 return "[%f, %f]"%(self.lower, self.upper)
00341 else:
00342 return "limit"
00343
00344 class JointMimic:
00345 def __init__(self, joint_name, multiplier=None, offset=None):
00346 self.joint_name = joint_name
00347 self.multiplier = multiplier
00348 self.offset = offset
00349
00350 @staticmethod
00351 def parse(node):
00352 mimic = JointMimic( node.getAttribute('joint') )
00353 if node.hasAttribute('multiplier'):
00354 mimic.multiplier = float( node.getAttribute('multiplier') )
00355 if node.hasAttribute('offset'):
00356 mimic.offset = float( node.getAttribute('offset') )
00357 return mimic
00358
00359 def to_xml(self, doc):
00360 xml = doc.createElement('mimic')
00361 set_attribute(xml, 'joint', self.joint_name)
00362 set_attribute(xml, 'multiplier', self.multiplier)
00363 set_attribute(xml, 'offset', self.offset)
00364 return xml
00365
00366 class Link:
00367 def __init__(self, name, visual=None, inertial=None, collision=None):
00368 self.name = name
00369 self.visual = visual
00370 self.inertial=inertial
00371 self.collision=collision
00372
00373 @staticmethod
00374 def parse(node):
00375 link = Link(node.getAttribute('name'))
00376 for child in children(node):
00377 if child.localName == 'visual':
00378 link.visual = Visual.parse(child)
00379 elif child.localName == 'collision':
00380 link.collision = Collision.parse(child)
00381 elif child.localName == 'inertial':
00382 link.inertial = Inertial.parse(child)
00383 else:
00384 rospy.logwarn("Unknown link element '%s'"%child.localName)
00385 return link
00386
00387 def to_xml(self, doc):
00388 xml = doc.createElement("link")
00389 xml.setAttribute("name", self.name)
00390 add( doc, xml, self.visual)
00391 add( doc, xml, self.collision)
00392 add( doc, xml, self.inertial)
00393 return xml
00394
00395 class Material:
00396 def __init__(self, name=None, color=None, texture=None):
00397 self.name = name
00398 self.color = color
00399 self.texture = texture
00400
00401 @staticmethod
00402 def parse(node):
00403 material = Material()
00404 if node.hasAttribute('name'):
00405 material.name = node.getAttribute('name')
00406 for child in children(node):
00407 if child.localName == 'color':
00408 material.color = Color.parse(child)
00409 elif child.localName == 'texture':
00410 material.texture = child.getAttribute('filename')
00411 else:
00412 rospy.logwarn("Unknown material element '%s'"%child.localName)
00413
00414 return material
00415
00416 def to_xml(self, doc):
00417 xml = doc.createElement("material")
00418 set_attribute(xml, "name", self.name)
00419 add( doc, xml, self.color )
00420
00421 if self.texture is not None:
00422 text = doc.createElement("texture")
00423 text.setAttribute('filename', self.texture)
00424 xml.appendChild(text)
00425 return xml
00426
00427 class Pose(GPose):
00428 def __init__(self, position=None, quaternion=None):
00429 self.position = position
00430 self.orientation = quaternion
00431
00432 @staticmethod
00433 def parse(node):
00434 pose = Pose()
00435 if node.hasAttribute("xyz"):
00436 xyz = node.getAttribute('xyz').split()
00437 pose.position = Point(*map(float, xyz))
00438 if node.hasAttribute("rpy"):
00439 rpy = node.getAttribute('rpy').split()
00440 pose.orientation = Quaternion(*tf.transformations.quaternion_from_euler(*map(float, rpy)))
00441 return pose
00442
00443 def to_xml(self, doc):
00444 xml = doc.createElement("origin")
00445 set_attribute(xml, 'xyz', self.position)
00446 if self.orientation:
00447 quat = self.orientation.x, self.orientation.y, self.orientation.z, self.orientation.z
00448 set_attribute(xml, 'rpy', tf.transformations.euler_from_quaternion(quat))
00449 return xml
00450
00451 class SafetyController:
00452 def __init__(self, velocity, position=None, lower=None, upper=None):
00453 self.velocity = velocity
00454 self.position = position
00455 self.lower = lower
00456 self.upper = upper
00457
00458 @staticmethod
00459 def parse(node):
00460 sc = SafetyController( float(node.getAttribute('k_velocity')) )
00461 if node.hasAttribute('soft_lower_limit'):
00462 sc.lower = float( node.getAttribute('soft_lower_limit') )
00463 if node.hasAttribute('soft_upper_limit'):
00464 sc.upper = float( node.getAttribute('soft_upper_limit') )
00465 if node.hasAttribute('k_position'):
00466 sc.position = float( node.getAttribute('k_position') )
00467 return sc
00468
00469 def to_xml(self, doc):
00470 xml = doc.createElement('safety_controller')
00471 set_attribute(xml, 'k_velocity', self.velocity)
00472 set_attribute(xml, 'k_position', self.position)
00473 set_attribute(xml, 'soft_upper_limit', self.upper)
00474 set_attribute(xml, 'soft_lower_limit', self.lower)
00475 return xml
00476
00477
00478 class Visual:
00479 def __init__(self, geometry=None, material=None, origin=None):
00480 self.geometry = geometry
00481 self.material = material
00482 self.origin = origin
00483
00484 @staticmethod
00485 def parse(node):
00486 v = Visual()
00487 for child in children(node):
00488 if child.localName == 'geometry':
00489 v.geometry = Geometry.parse(child)
00490 elif child.localName == 'origin':
00491 v.origin = Pose.parse(child)
00492 elif child.localName == 'material':
00493 v.material = Material.parse(child)
00494 else:
00495 rospy.logwarn("Unknown visual element '%s'"%child.localName)
00496 return v
00497
00498 def to_xml(self, doc):
00499 xml = doc.createElement("visual")
00500 add( doc, xml, self.geometry )
00501 add( doc, xml, self.origin )
00502 add( doc, xml, self.material )
00503 return xml
00504
00505 class URDF:
00506 def __init__(self, name=""):
00507 self.name = name
00508 self.elements = []
00509 self.links = {}
00510 self.joints = {}
00511 self.joint_list = []
00512 self.materials = {}
00513
00514 self.parent_map = {}
00515 self.child_map = {}
00516
00517 def parse(self, xml_string):
00518 base = xml.dom.minidom.parseString(xml_string)
00519 robot = children(base)[0]
00520 self.name = robot.getAttribute('name')
00521 for node in children(robot):
00522 if node.nodeType is node.TEXT_NODE:
00523 continue
00524 if node.localName == 'joint':
00525 self.add_joint( Joint.parse(node) )
00526 elif node.localName == 'link':
00527 self.add_link( Link.parse(node) )
00528 elif node.localName == 'material':
00529 self.elements.append( Material.parse(node) )
00530 elif node.localName == 'gazebo':
00531 None
00532 elif node.localName == 'transmission':
00533 None
00534 else:
00535 rospy.logwarn("Unknown robot element '%s'"%node.localName)
00536 return self
00537
00538 def load(self, filename):
00539 f = open(filename, 'r')
00540 return self.parse(f.read())
00541
00542 def add_link(self, link):
00543 self.elements.append(link)
00544 self.links[link.name] = link
00545
00546 def add_joint(self, joint):
00547 self.elements.append(joint)
00548 self.joint_list.append(joint.name)
00549 self.joints[joint.name] = joint
00550
00551 self.parent_map[ joint.child ] = (joint.name, joint.parent)
00552 if joint.parent in self.child_map:
00553 self.child_map[joint.parent].append( (joint.name, joint.child) )
00554 else:
00555 self.child_map[joint.parent] = [ (joint.name, joint.child) ]
00556
00557
00558 def get_chain(self, root, tip, joints=True, links=True):
00559 chain = []
00560 if links:
00561 chain.append(tip)
00562 link = tip
00563 while link != root:
00564 (joint, parent) = self.parent_map[link]
00565 if joints:
00566 chain.append(joint)
00567 if links:
00568 chain.append(parent)
00569 link = parent
00570 chain.reverse()
00571 return chain
00572
00573 def to_xml(self):
00574 doc = Document()
00575 root = doc.createElement("robot")
00576 doc.appendChild(root)
00577 root.setAttribute("name", self.name)
00578
00579 for element in self.elements:
00580 root.appendChild(element.to_xml(doc))
00581
00582 return doc.toprettyxml()