parse.py
Go to the documentation of this file.
00001 from __future__ import print_function
00002 
00003 import itertools
00004 import os
00005 import xml.etree.ElementTree as ET
00006 import xml.dom.minidom
00007 
00008 from tf.transformations import *
00009 
00010 from naming import *
00011 from conversions import *
00012 
00013 models_path = os.path.expanduser('~/.gazebo/models/')
00014 catkin_ws_path = os.path.expanduser('~') + '/catkin_ws/src/'
00015 supported_sdf_versions = [1.4, 1.5]
00016 
00017 
00018 
00019 def find_mesh_in_catkin_ws(filename):
00020   if not find_mesh_in_catkin_ws.cache:
00021     result = ''
00022     for root, dirs, files in os.walk(catkin_ws_path, followlinks=True):
00023       for currfile in files:
00024         if currfile.endswith('.stl') or currfile.endswith('.dae'):
00025           partial_path = ''
00026           for path_part in root.split('/'):
00027             partial_path += path_part + '/'
00028             if os.path.exists(partial_path + '/package.xml'):
00029               break
00030           catkin_stack_path = partial_path.replace(path_part + '/', '')
00031           filename_path = os.path.join(root, currfile).replace(catkin_stack_path, '')
00032           find_mesh_in_catkin_ws.cache.append(filename_path)
00033   matching = [path for path in find_mesh_in_catkin_ws.cache if filename in path]
00034   return ' OR '.join(matching)
00035 find_mesh_in_catkin_ws.cache = []
00036 
00037 
00038 def find_model_in_gazebo_dir(modelname):
00039   if not find_model_in_gazebo_dir.cache:
00040     for root, dirs, files in os.walk(models_path, followlinks=True):
00041       for currfile in files:
00042         if currfile != 'model.sdf':
00043           continue
00044         filename_path = os.path.join(root, currfile)
00045         tree = ET.parse(filename_path)
00046         root = tree.getroot()
00047         if root.tag != 'sdf':
00048           continue
00049         modelnode = get_node(root, 'model')
00050         if modelnode == None:
00051           continue
00052         modelname_in_file = modelnode.attrib['name']
00053         find_model_in_gazebo_dir.cache[modelname_in_file] = filename_path
00054     #print(find_model_in_gazebo_dir.cache)
00055   return find_model_in_gazebo_dir.cache.get(modelname)
00056 find_model_in_gazebo_dir.cache = {}
00057 
00058 
00059 def pose2origin(node, pose):
00060   xyz, rpy = homogeneous2translation_rpy(pose)
00061   ET.SubElement(node, 'origin', {'xyz': array2string(rounded(xyz)), 'rpy': array2string(rounded(rpy))})
00062 
00063 
00064 def prettyXML(uglyXML):
00065   return xml.dom.minidom.parseString(uglyXML).toprettyxml(indent='  ')
00066 
00067 
00068 def get_tag(node, tagname, default = None):
00069   tag = node.findall(tagname)
00070   if tag:
00071     return tag[0].text
00072   else:
00073     return default
00074 
00075 
00076 def get_node(node, tagname, default = None):
00077   tag = node.findall(tagname)
00078   if tag:
00079     return tag[0]
00080   else:
00081     return default
00082 
00083 
00084 def get_tag_pose(node):
00085   pose = get_tag(node, 'pose', '0 0 0  0 0 0')
00086   return pose_string2homogeneous(pose)
00087 
00088 
00089 def indent(string, spaces):
00090   return string.replace('\n', '\n' + ' ' * spaces).strip()
00091 
00092 
00093 def model_from_include(parent, include_node):
00094     submodel_uri = get_tag(include_node, 'uri')
00095     submodel_path = submodel_uri.replace('model://', models_path) + os.path.sep + 'model.sdf'
00096     submodel_name = get_tag(include_node, 'name')
00097     submodel_pose = get_tag_pose(include_node)
00098     return Model(parent, name=submodel_name, pose=submodel_pose, file=submodel_path)
00099 
00100 
00101 def homogeneous_times_vector(homogeneous, vector):
00102   vector_as_hom = identity_matrix()
00103   vector_as_hom[:3,3] = vector.T
00104   res = numpy.dot(homogeneous, vector_as_hom)
00105   return res[:3,3].T 
00106 
00107 
00108 
00109 
00110 
00111 class SDF(object):
00112   def __init__(self, **kwargs):
00113     self.world = World()
00114     if 'file' in kwargs:
00115       self.from_file(kwargs['file'])
00116     elif 'model' in kwargs:
00117       self.from_model(kwargs['model'])
00118 
00119 
00120   def from_file(self, filename):
00121     if not os.path.exists(filename):
00122       print('Failed to open SDF because %s does not exist' % filename)
00123       return
00124     tree = ET.parse(filename)
00125     root = tree.getroot()
00126     if root.tag != 'sdf':
00127       print('Not a SDF file. Aborting.')
00128       return
00129     self.version = float(root.attrib['version'])
00130     if not self.version in supported_sdf_versions:
00131       print('Unsupported SDF version in %s. Aborting.\n' % filename)
00132       return
00133     self.world.from_tree(root, version=self.version)
00134 
00135 
00136   def from_model(self, modelname):
00137     sdf_file = models_path + modelname + '/model.sdf'
00138     if not os.path.exists(sdf_file):
00139       sdf_file = find_model_in_gazebo_dir(modelname)
00140     if not sdf_file:
00141       print('Could not resolve modelname=%s to its SDF file in %s' % (modelname, models_path))
00142       return
00143     self.from_file(sdf_file)
00144 
00145 
00146 
00147 class World(object):
00148   def __init__(self, **kwargs):
00149     self.name = '__default__'
00150     self.models = []
00151     self.lights = []
00152     self.version = kwargs.get('version', 0.0)
00153 
00154 
00155   def from_tree(self, node, **kwargs):
00156     self.version = kwargs.get('version', self.version)
00157     if node.findall('world'):
00158       node = node.findall('world')[0]
00159       for include_node in node.iter('include'):
00160         self.models.append(model_from_include(None, include_node))
00161       # TODO lights
00162     self.models += [Model(tree=model_node, version=self.version) for model_node in node.findall('model')]
00163 
00164 
00165   def plot_to_file(self, plot_filename):
00166     import pygraphviz as pgv
00167     graph = pgv.AGraph(directed=True)
00168     self.plot(graph)
00169     graph.draw(plot_filename, prog='dot')
00170 
00171 
00172   def plot(self, graph):
00173     graph.add_node('world')
00174 
00175     for model in self.models:
00176       model.plot(graph)
00177       graph.add_edge('world', model.name + '::' + model.root_link.name)
00178 
00179 
00180   def get_link(self, requested_linkname):
00181     #print('World.get_link: rl=%s' % requested_linkname)
00182     for model in self.models:
00183       link = model.get_link(requested_linkname, model.name)
00184       if link:
00185         return link
00186 
00187 
00188   def for_all_links(self, func, **kwargs):
00189     for model in self.models:
00190       model.for_all_links(func, **kwargs)
00191 
00192 
00193   def for_all_joints(self, func, **kwargs):
00194     for model in self.models:
00195       model.for_all_joints(func, **kwargs)
00196 
00197 
00198   def for_all_submodels(self, func, **kwargs):
00199     for model in self.models:
00200       model.for_all_submodels(func, **kwargs)
00201 
00202 
00203 
00204 class SpatialEntity(object):
00205   def __init__(self, **kwargs):
00206     self.name = ''
00207     self.pose = identity_matrix()
00208     self.pose_world = identity_matrix()
00209 
00210 
00211   def __repr__(self):
00212     return ''.join((
00213       'name: %s\n' % self.name,
00214       'pose: %s\n' % homogeneous2tq_string_rounded(self.pose),
00215       'pose_world: %s\n' % homogeneous2tq_string_rounded(self.pose_world),
00216     ))
00217 
00218 
00219   def from_tree(self, node, **kwargs):
00220     if node == None:
00221       return
00222     self.name = node.attrib['name']
00223     self.pose = get_tag_pose(node)
00224 
00225 
00226 
00227 class Model(SpatialEntity):
00228   def __init__(self, parent_model = None, **kwargs):
00229     super(Model, self).__init__(**kwargs)
00230     self.parent_model = parent_model
00231     self.version = kwargs.get('version', 0.0)
00232     self.submodels = []
00233     self.links = []
00234     self.joints = []
00235     self.root_link = None
00236     if 'tree' in kwargs:
00237       self.from_tree(kwargs['tree'], **kwargs)
00238     elif 'file' in kwargs:
00239       self.from_file(kwargs['file'], **kwargs)
00240 
00241     if not self.parent_model:
00242       self.root_link = self.find_root_link()
00243       self.build_tree()
00244       self.calculate_absolute_pose()
00245 
00246 
00247   def __repr__(self):
00248     return ''.join((
00249       'Model(\n', 
00250       '  %s\n' % indent(super(Model, self).__repr__(), 2),
00251       '  version: %s\n' % self.version,
00252       '  root_link: %s\n' % self.root_link.name if self.root_link else '',
00253       '  links:\n',
00254       '    %s' % '\n    '.join([indent(str(l), 4) for l in self.links]),
00255       '\n',
00256       '  joints:\n',
00257       '    %s' % '\n    '.join([indent(str(j), 4) for j in self.joints]),
00258       '\n',
00259       '  submodels:\n',
00260       '    %s' % '\n    '.join([indent(str(m), 4) for m in self.submodels]),
00261       '\n',
00262       ')'
00263     ))
00264 
00265 
00266   def from_file(self, filename, **kwargs):
00267     if not os.path.exists(filename):
00268       print('Failed to open Model because %s does not exist' % filename)
00269       return
00270     tree = ET.parse(filename)
00271     root = tree.getroot()
00272     if root.tag != 'sdf':
00273       print('Not a SDF file. Aborting.')
00274       return
00275     self.version = float(root.attrib['version'])
00276     if not self.version in supported_sdf_versions:
00277       print('Unsupported SDF version in %s. Aborting.\n' % filename)
00278       return
00279     modelnode = get_node(root, 'model')
00280     self.from_tree(modelnode, **kwargs)
00281 
00282     # Override name (e.g. for <include>)
00283     kwargs_name = kwargs.get('name')
00284     if kwargs_name:
00285       self.name = kwargs_name
00286 
00287     # External pose offset (from <include>)
00288     self.pose = numpy.dot(kwargs.get('pose', identity_matrix()), self.pose)
00289 
00290 
00291   def from_tree(self, node, **kwargs):
00292     if node == None:
00293       return
00294     if node.tag != 'model':
00295       print('Invalid node of type %s instead of model. Aborting.' % node.tag)
00296       return
00297     self.version = kwargs.get('version', self.version)
00298     super(Model, self).from_tree(node, **kwargs)
00299     self.links = [Link(self, tree=link_node) for link_node in node.iter('link')]
00300     self.joints = [Joint(self, tree=joint_node) for joint_node in node.iter('joint')]
00301 
00302     for include_node in node.iter('include'):
00303       self.submodels.append(model_from_include(self, include_node))
00304 
00305 
00306   def add_urdf_elements(self, node, prefix = ''):
00307     full_prefix = prefix + '::' + self.name if prefix else self.name
00308     for entity in self.joints + self.links:
00309       entity.add_urdf_elements(node, full_prefix)
00310     for submodel in self.submodels:
00311       submodel.add_urdf_elements(node, full_prefix)
00312 
00313 
00314   def to_urdf_string(self):
00315     urdfnode = ET.Element('robot', {'name': self.name})
00316     self.add_urdf_elements(urdfnode)
00317     return ET.tostring(urdfnode)
00318 
00319 
00320   def save_urdf(self, filename):
00321     urdf_file = open(filename, 'w')
00322     pretty_urdf_string = prettyXML(self.to_urdf_string())
00323     urdf_file.write(pretty_urdf_string)
00324     urdf_file.close()
00325 
00326 
00327   def get_joint(self, requested_jointname, prefix = ''):
00328     #print('get_joint: n=%s rj=%s p=%s' % (self.name, requested_jointname, prefix))
00329     full_prefix = prefix + '::' if prefix else ''
00330     for joint in self.joints:
00331       if full_prefix + joint.name == requested_jointname:
00332         return joint
00333     for submodel in self.submodels:
00334       res = submodel.get_joint(requested_jointname, submodel.name)
00335       if res:
00336         return res
00337 
00338 
00339   def get_link(self, requested_linkname, prefix = ''):
00340     #print('Model.get_link: n=%s rl=%s p=%s' % (self.name, requested_linkname, prefix))
00341     full_prefix = prefix + '::' if prefix else ''
00342     for link in self.links:
00343       if full_prefix + link.name == requested_linkname:
00344         return link
00345     for submodel in self.submodels:
00346       res = submodel.get_link(requested_linkname, prefix + '::' + submodel.name if prefix else submodel.name)
00347       if res:
00348         return res
00349 
00350 
00351   def build_tree(self):
00352     for joint in self.joints:
00353       joint.tree_parent_link = self.get_link(joint.parent)
00354       if not joint.tree_parent_link:
00355         print('Failed to find parent %s of joint %s. Aborting' % (joint.parent, joint.name))
00356       joint.tree_child_link = self.get_link(joint.child)
00357       if not joint.tree_child_link:
00358         print('Failed to find child %s of joint %s. Aborting' % (joint.child, joint.name))
00359         return None
00360       joint.tree_parent_link.tree_child_joints.append(joint)
00361       joint.tree_child_link.tree_parent_joint = joint
00362     for submodel in self.submodels:
00363       submodel.build_tree()
00364 
00365 
00366   def calculate_absolute_pose(self, worldMVparent = identity_matrix()):
00367     worldMVmodel = concatenate_matrices(worldMVparent, self.pose)
00368     self.pose_world = worldMVmodel
00369     for submodel in self.submodels:
00370       submodel.calculate_absolute_pose(worldMVmodel)
00371     for link in self.links:
00372       link.pose_world = concatenate_matrices(worldMVmodel, link.pose)
00373     for joint in self.joints:
00374       joint.pose_world = concatenate_matrices(joint.tree_child_link.pose_world, joint.pose)
00375 
00376 
00377   def find_root_link(self):
00378     if not self.links:
00379       print('Model %s has no links and therefore no root link. Aborting' % self.name)
00380       return None
00381     curr_link = self.links[0]
00382     while True:
00383       parent_link = self.get_parent(curr_link.name)
00384       if not parent_link:
00385         return curr_link
00386       curr_link = parent_link
00387 
00388 
00389   def get_parent(self, requested_linkname, prefix = '', visited = []):
00390     if self.name in visited:
00391       return None
00392     full_prefix = prefix + '::' if prefix else ''
00393     for joint in self.joints:
00394       if joint.child == full_prefix + requested_linkname:
00395         return self.get_link(joint.parent)
00396     # Parent links can also be in submodels
00397     for submodel in self.submodels:
00398       res = submodel.get_parent(requested_linkname, full_prefix + submodel.name, visited + [self.name])
00399       if res:
00400         return res
00401     if self.parent_model:
00402       return self.parent_model.get_parent(requested_linkname, self.name, visited + [self.name])
00403 
00404 
00405   def plot(self, graph, prefix = ''):
00406     full_prefix = prefix + '::' + self.name if prefix else self.name
00407     full_prefix += '::'
00408     for link in self.links:
00409       graph.add_node(full_prefix + link.name, label=full_prefix + link.name + '\\nrel: ' + homogeneous2tq_string_rounded(link.pose) + '\\nabs: ' + homogeneous2tq_string_rounded(link.pose_world))
00410     subgraph = graph.add_subgraph([full_prefix + link.name for link in self.links], 'cluster_' + self.name, color='gray', label=self.name + '\\nrel: ' + homogeneous2tq_string_rounded(self.pose) + '\\nabs: ' + homogeneous2tq_string_rounded(self.pose_world))
00411 
00412     for submodel in self.submodels:
00413       submodel.plot(graph, full_prefix.rstrip('::'))
00414 
00415     for joint in self.joints:
00416       graph.add_edge(full_prefix + joint.parent, full_prefix + joint.child, label=full_prefix + joint.name + '\\nrel: ' + homogeneous2tq_string_rounded(joint.pose) + '\\nabs: ' + homogeneous2tq_string_rounded(joint.pose_world))
00417 
00418 
00419   def get_root_model(self):
00420     curr_model = self
00421     while True:
00422       if not curr_model.parent_model:
00423         return curr_model
00424       curr_model = curr_model.parent_model
00425 
00426 
00427   def for_all_links(self, func, prefix = '', **kwargs):
00428     full_prefix = prefix + '::' + self.name if prefix else self.name
00429     for link in self.links:
00430       func(link, full_prefix + '::' + link.name, **kwargs)
00431     for submodel in self.submodels:
00432       submodel.for_all_links(func, full_prefix, **kwargs)
00433 
00434 
00435   def for_all_joints(self, func, prefix = '', **kwargs):
00436     full_prefix = prefix + '::' + self.name if prefix else self.name
00437     for joint in self.joints:
00438       func(joint, full_prefix + '::' + joint.name, **kwargs)
00439     for submodel in self.submodels:
00440       submodel.for_all_joints(func, full_prefix, **kwargs)
00441 
00442 
00443   def for_all_submodels(self, func, prefix = '', **kwargs):
00444     full_prefix = prefix + '::' + self.name if prefix else self.name
00445     func(self, full_prefix, **kwargs)
00446     for submodel in self.submodels:
00447       submodel.for_all_submodels(func, full_prefix, **kwargs)
00448 
00449 
00450   def get_full_name(self):
00451     name = self.name
00452     curr_model = self
00453     while True:
00454       if not curr_model.parent_model:
00455         break
00456       curr_model = curr_model.parent_model
00457       name = curr_model.name + '::' + name
00458     return name
00459 
00460 
00461 
00462 class Link(SpatialEntity):
00463   def __init__(self, parent_model, **kwargs):
00464     super(Link, self).__init__(**kwargs)
00465     self.parent_model = parent_model
00466     self.gravity = True
00467     self.inertial = Inertial()
00468     self.collision = Collision()
00469     self.visual = Visual()
00470     self.tree_parent_joint = None
00471     self.tree_child_joints = []
00472     if 'tree' in kwargs:
00473       self.from_tree(kwargs['tree'])
00474 
00475 
00476   def __repr__(self):
00477     return ''.join((
00478       'Link(\n',
00479       '  %s\n' % indent(super(Link, self).__repr__(), 2),
00480       '  %s\n' % indent(str(self.inertial), 2),
00481       '  collision: %s\n' % self.collision,
00482       '  visual: %s\n' % self.visual,
00483       ')'
00484     ))
00485 
00486 
00487   def from_tree(self, node):
00488     if node == None:
00489       return
00490     if node.tag != 'link':
00491       print('Invalid node of type %s instead of link. Aborting.' % node.tag)
00492       return
00493     super(Link, self).from_tree(node)
00494     self.inertial = Inertial(tree=get_node(node, 'inertial'))
00495     self.collision = Collision(tree=get_node(node, 'collision'))
00496     self.visual = Visual(tree=get_node(node, 'visual'))
00497 
00498   def is_empty(self):
00499     return not self.visual.geometry_type and not self.collision.geometry_type
00500 
00501   def add_urdf_elements(self, node, prefix):
00502     full_prefix = prefix + '::' if prefix else ''
00503     linknode = ET.SubElement(node, 'link', {'name': sdf2tfname(full_prefix + self.name)})
00504     # urdf links do not have a coordinate system themselves, only their parts (inertial, collision, visual) have one
00505     if self.tree_parent_joint:
00506       if self.tree_parent_joint.parent_model == self.parent_model:
00507         urdf_pose = concatenate_matrices(inverse_matrix(self.tree_parent_joint.pose_world), self.pose_world)
00508       else: # joint crosses includes
00509         urdf_pose = identity_matrix()
00510     else: # root
00511       urdf_pose = self.pose_world
00512     self.inertial.add_urdf_elements(linknode, urdf_pose)
00513     self.collision.add_urdf_elements(linknode, prefix, urdf_pose)
00514     self.visual.add_urdf_elements(linknode, prefix, urdf_pose)
00515 
00516 
00517   def get_full_name(self):
00518     return self.parent_model.get_full_name() + '::' + self.name
00519     
00520 
00521 
00522 
00523 class Joint(SpatialEntity):
00524   def __init__(self, parent_model, **kwargs):
00525     super(Joint, self).__init__(**kwargs)
00526     self.parent_model = parent_model
00527     self.type = ''
00528     self.parent = ''
00529     self.child = ''
00530     self.axis = Axis(self)
00531     self.axis2 = None
00532     self.tree_parent_link = None
00533     self.tree_child_link = None
00534     if 'tree' in kwargs:
00535       self.from_tree(kwargs['tree'])
00536 
00537 
00538   def __repr__(self):
00539     return ''.join((
00540       'Joint(\n',
00541       '  %s\n' % indent(super(Joint, self).__repr__(), 2),
00542       '  type: %s\n' % self.type,
00543       '  parent: %s\n' % self.parent,
00544       '  child: %s\n' % self.child,
00545       '  axis: %s\n' % self.axis,
00546       '  axis2: %s\n' % self.axis2 if self.axis2 else '',
00547       ')'
00548     ))
00549 
00550 
00551   def from_tree(self, node):
00552     if node == None:
00553       return
00554     if node.tag != 'joint':
00555       print('Invalid node of type %s instead of joint. Aborting.' % node.tag)
00556       return
00557     super(Joint, self).from_tree(node)
00558     self.type = node.attrib['type']
00559     self.parent = get_tag(node, 'parent', '')
00560     self.child = get_tag(node, 'child', '')
00561     self.axis = Axis(self, tree=get_node(node, 'axis'))
00562     if get_node(node, 'axis2') != None:
00563       self.axis2 = Axis(self, tree=get_node(node, 'axis2'))
00564 
00565 
00566   def add_urdf_elements(self, node, prefix):
00567     full_prefix = prefix + '::' if prefix else ''
00568     jointnode = ET.SubElement(node, 'joint', {'name': sdf2tfname(full_prefix + self.name)})
00569     parentnode = ET.SubElement(jointnode, 'parent', {'link': sdf2tfname(full_prefix + self.parent)})
00570     childnode = ET.SubElement(jointnode, 'child', {'link': sdf2tfname(full_prefix + self.child)})
00571     # in SDF a joint's pose is given in child link frame, in URDF joint frame = child link frame, i.e. specifiy relative to parent joint (not parent link)
00572     parent_pose_world = self.tree_parent_link.tree_parent_joint.pose_world if self.tree_parent_link.tree_parent_joint else identity_matrix()
00573     if self.tree_parent_link.parent_model == self.parent_model:
00574       pose2origin(jointnode, concatenate_matrices(inverse_matrix(parent_pose_world), self.pose_world))
00575     else: # joint crosses includes
00576       pose2origin(jointnode, concatenate_matrices(inverse_matrix(parent_pose_world), self.tree_child_link.pose_world))
00577     if self.type == 'revolute' and self.axis.lower_limit == 0 and self.axis.upper_limit == 0:
00578       jointnode.attrib['type'] = 'fixed'
00579     elif self.type == 'universal':
00580       # Simulate universal robot as
00581       # self.parent -> revolute joint (self) -> dummy link -> revolute joint -> self.child
00582       jointnode.attrib['type'] = 'revolute'
00583       dummylinknode = ET.SubElement(node, 'link', {'name': sdf2tfname(full_prefix + self.name + '::revolute_dummy_link')})
00584       childnode.attrib['link'] = dummylinknode.attrib['name']
00585       dummyjointnode = ET.SubElement(node, 'joint', {'name': sdf2tfname(full_prefix + self.name + '::revolute_dummy_joint')})
00586       ET.SubElement(dummyjointnode, 'parent', {'link': dummylinknode.attrib['name']})
00587       ET.SubElement(dummyjointnode, 'child', {'link': sdf2tfname(full_prefix + self.child)})
00588       dummyjointnode.attrib['type'] = 'revolute'
00589       self.axis2.add_urdf_elements(dummyjointnode, concatenate_matrices(inverse_matrix(self.pose_world), self.parent_model.pose_world))
00590     else:
00591       jointnode.attrib['type'] = self.type
00592     #print('self.pose_world\n', self.pose_world)
00593     #print('self.parent_model.pose_world\n', self.parent_model.pose_world)
00594     self.axis.add_urdf_elements(jointnode, concatenate_matrices(inverse_matrix(self.pose_world), self.parent_model.pose_world))
00595 
00596 
00597   def get_full_name(self):
00598     return self.parent_model.get_full_name() + '::' + self.name
00599 
00600 
00601 
00602 
00603 class Axis(object):
00604   def __init__(self, joint, **kwargs):
00605     self.joint = joint
00606     self.version = self.joint.parent_model.version
00607     self.xyz = numpy.array([0, 0, 0])
00608     self.lower_limit = 0
00609     self.upper_limit = 0
00610     self.effort_limit = 0
00611     self.velocity_limit = 0
00612     if self.version >= 1.5:
00613       self.use_parent_model_frame = False
00614     if 'tree' in kwargs:
00615       self.from_tree(kwargs['tree'])
00616 
00617 
00618   def __repr__(self):
00619     return 'Axis(xyz=%s,%s lower_limit=%s, upper_limit=%s, effort=%s, velocity=%s)' % (self.xyz, ' use_parent_model_frame=%s,' % self.use_parent_model_frame if self.version >= 1.5 else '', self.lower_limit, self.upper_limit, self.effort_limit, self.velocity_limit)
00620 
00621 
00622   def from_tree(self, node):
00623     if node == None:
00624       return
00625     if node.tag != 'axis' and node.tag != 'axis2':
00626       print('Invalid node of type %s instead of axis(2). Aborting.' % node.tag)
00627       return
00628     self.xyz = numpy.array(get_tag(node, 'xyz').split())
00629     self.use_parent_model_frame = bool(get_tag(node, 'use_parent_model_frame'))
00630     limitnode = get_node(node, 'limit')
00631     if limitnode == None:
00632       print('limit Tag missing from joint. Aborting.')
00633       return
00634     self.lower_limit = float(get_tag(limitnode, 'lower', 0))
00635     self.upper_limit = float(get_tag(limitnode, 'upper', 0))
00636     self.effort_limit = float(get_tag(limitnode, 'effort', 0))
00637     self.velocity_limit = float(get_tag(limitnode, 'velocity', 0))
00638 
00639 
00640   def add_urdf_elements(self, node, modelCBTjoint):
00641     if (self.version <= 1.4) or (self.version >= 1.5 and self.use_parent_model_frame): # SDF 1.4 axis is specified in model frame
00642       rotation_modelCBTjoint = rotation_only(modelCBTjoint)
00643       xyz_joint = homogeneous_times_vector(rotation_modelCBTjoint, self.xyz)
00644       xyz_joint /= numpy.linalg.norm(xyz_joint)
00645       #print('self.xyz=%s\nmodelCBTjoint:\n%s\nrotation_modelCBT_joint:\n%s\nxyz_joint=%s' % (self.xyz, modelCBTjoint, rotation_modelCBTjoint, xyz_joint))
00646     else: # SDF 1.5 axis is specified in joint frame unless the use_parent_model_frame flag is set to true
00647       print('UNTESTED')
00648       xyz_joint = self.xyz
00649     axisnode = ET.SubElement(node, 'axis', {'xyz': array2string(rounded(xyz_joint))})
00650     limitnode = ET.SubElement(node, 'limit', {'lower': str(self.lower_limit), 'upper': str(self.upper_limit), 'effort': str(self.effort_limit), 'velocity': str(self.velocity_limit)})
00651 
00652 
00653 
00654 class Inertial(object):
00655   def __init__(self, **kwargs):
00656     self.pose = identity_matrix()
00657     self.mass = 0
00658     self.inertia = Inertia()
00659     if 'tree' in kwargs:
00660       self.from_tree(kwargs['tree'])
00661 
00662 
00663   def __repr__(self):
00664     return ''.join((
00665       'Inertial(\n',
00666       '  pose: %s\n' % homogeneous2tq_string_rounded(self.pose),
00667       '  mass: %s\n' % self.mass,
00668       '  inertia: %s\n' % self.inertia,
00669       ')'
00670     ))
00671 
00672 
00673   def from_tree(self, node):
00674     if node == None:
00675       return
00676     if node.tag != 'inertial':
00677       print('Invalid node of type %s instead of inertial. Aborting.' % node.tag)
00678       return
00679     self.pose = get_tag_pose(node)
00680     self.mass = get_tag(node, 'mass', 0)
00681     self.inertia = Inertia(tree=get_node(node, 'inertia'))
00682 
00683 
00684   def add_urdf_elements(self, node, link_pose):
00685     inertialnode = ET.SubElement(node, 'inertial')
00686     massnode = ET.SubElement(inertialnode, 'mass', {'value': str(self.mass)})
00687     pose2origin(inertialnode, concatenate_matrices(link_pose, self.pose))
00688     self.inertia.add_urdf_elements(inertialnode)
00689 
00690 
00691 
00692 class Inertia(object):
00693   def __init__(self, **kwargs):
00694     self.ixx = 0
00695     self.ixy = 0
00696     self.ixz = 0
00697     self.iyy = 0
00698     self.iyz = 0
00699     self.izz = 0
00700     self.coords = 'ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz'
00701     if 'tree' in kwargs:
00702       self.from_tree(kwargs['tree'])
00703 
00704 
00705   def __repr__(self):
00706     return 'Inertia(ixx=%s, ixy=%s, ixz=%s, iyy=%s, iyz=%s, izz=%s)' % (self.ixx, self.ixy, self.ixz, self.iyy, self.iyz, self.izz)
00707 
00708 
00709   def from_tree(self, node):
00710     if node == None:
00711       return
00712     if node.tag != 'inertia':
00713       print('Invalid node of type %s instead of inertia. Aborting.' % node.tag)
00714       return
00715     for coord in self.coords:
00716       setattr(self, coord, get_tag(node, coord, 0))
00717 
00718 
00719   def add_urdf_elements(self, node):
00720     inertianode = ET.SubElement(node, 'inertia')
00721     for coord in self.coords:
00722       inertianode.attrib[coord] = str(getattr(self, coord))
00723 
00724 
00725 
00726 class LinkPart(SpatialEntity):
00727   def __init__(self, **kwargs):
00728     super(LinkPart, self).__init__(**kwargs)
00729     self.geometry_type = None
00730     self.geometry_data = {}
00731     self.gtypes = 'box', 'cylinder', 'sphere', 'mesh'
00732     if 'tree' in kwargs:
00733       self.from_tree(kwargs['tree'])
00734 
00735 
00736   def from_tree(self, node):
00737     if node == None:
00738       return
00739     if node.tag != 'visual' and node.tag != 'collision':
00740       print('Invalid node of type %s instead of visual or collision. Aborting.' % node.tag)
00741       return
00742     super(LinkPart, self).from_tree(node)
00743     gnode = get_node(node, 'geometry')
00744     if gnode == None:
00745       return
00746     for gtype in self.gtypes:
00747       typenode = get_node(gnode, gtype)
00748       if typenode != None:
00749         self.geometry_type = gtype
00750         if gtype == 'box':
00751           self.geometry_data = {'size': get_tag(typenode, 'size')}
00752         elif gtype == 'cylinder':
00753           self.geometry_data = {'radius': get_tag(typenode, 'radius'), 'length': get_tag(typenode, 'length')}
00754         elif gtype == 'sphere':
00755           self.geometry_data = {'radius': get_tag(typenode, 'radius')}
00756         elif gtype == 'mesh':
00757           self.geometry_data = {'uri': get_tag(typenode, 'uri'), 'scale': get_tag(typenode, 'scale', '1.0 1.0 1.0')}
00758 
00759 
00760   def __repr__(self):
00761     return '%s geometry_type: %s, geometry_data: %s' % (super(LinkPart, self).__repr__().replace('\n', ', ').strip(), self.geometry_type, self.geometry_data)
00762 
00763 
00764   def add_urdf_elements(self, node, prefix, link_pose, part_type):
00765     if not self.geometry_type:
00766       return
00767     partnode = ET.SubElement(node, part_type, {'name': sdf2tfname(prefix + '::' + self.name)})
00768     pose2origin(partnode, concatenate_matrices(link_pose, self.pose))
00769     geometrynode = ET.SubElement(partnode, 'geometry')
00770     if self.geometry_type == 'box':
00771       boxnode = ET.SubElement(geometrynode, 'box', {'size': self.geometry_data['size']})
00772     elif self.geometry_type == 'cylinder':
00773       cylindernode = ET.SubElement(geometrynode, 'cylinder', {'radius': self.geometry_data['radius'], 'length': self.geometry_data['length']})
00774     elif self.geometry_type == 'sphere':
00775       spherenode = ET.SubElement(geometrynode, 'sphere', {'radius': self.geometry_data['radius']})
00776     elif self.geometry_type == 'mesh':
00777       mesh_file = '/'.join(self.geometry_data['uri'].split('/')[3:])
00778       mesh_found = find_mesh_in_catkin_ws(mesh_file)
00779       if mesh_found:
00780         mesh_path = 'package://' + mesh_found
00781       else:
00782         print('Could not find mesh %s in %s' % (mesh_file, catkin_ws_path))
00783         mesh_path = 'package://PATHTOMESHES/' + mesh_file
00784       meshnode = ET.SubElement(geometrynode, 'mesh', {'filename': mesh_path, 'scale': self.geometry_data['scale']})
00785 
00786 
00787 
00788 class Collision(LinkPart):
00789   def __init__(self, **kwargs):
00790     super(Collision, self).__init__(**kwargs)
00791 
00792 
00793   def __repr__(self):
00794     return 'Collision(%s)' % super(Collision, self).__repr__()
00795 
00796 
00797   def add_urdf_elements(self, node, prefix, link_pose):
00798     super(Collision, self).add_urdf_elements(node, prefix, link_pose, 'collision')
00799 
00800 
00801 
00802 class Visual(LinkPart):
00803   def __init__(self, **kwargs):
00804     super(Visual, self).__init__(**kwargs)
00805 
00806 
00807   def __repr__(self):
00808     return 'Visual(%s)' % super(Visual, self).__repr__()
00809 
00810 
00811   def add_urdf_elements(self, node, prefix, link_pose):
00812     super(Visual, self).add_urdf_elements(node, prefix, link_pose, 'visual')


pysdf
Author(s): Andreas Bihlmaier
autogenerated on Thu Aug 27 2015 14:35:07