generate_urdf.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (C) 2014 Aldebaran Robotics
00004 #
00005 # Licensed under the Apache License, Version 2.0 (the "License");
00006 # you may not use this file except in compliance with the License.
00007 # You may obtain a copy of the License at
00008 #
00009 # http://www.apache.org/licenses/LICENSE-2.0
00010 #
00011 # Unless required by applicable law or agreed to in writing, software
00012 # distributed under the License is distributed on an "AS IS" BASIS,
00013 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014 # See the License for the specific language governing permissions and
00015 # limitations under the License.
00016 #
00017 #
00018 # This file takes the official URDF aldebaran files and convert them
00019 # to rep120 compliant urdf files. It also includes the meshes from nao_meshes
00020 # package allowing to display the model in RVIZ
00021 #
00022 # authors: Mikael Arguedas [mikael DOT arguedas AT gmail DOT com]
00023 # TODO Get motor information from documentation and generate transmission tags
00024 # automatically
00025 # TODO Generate automatically gazebo tags for every sensor
00026 # TODO Add toe frames for romeo (not supported yet by NAOqi)
00027 
00028 from __future__ import print_function
00029 import sys
00030 import argparse
00031 from naoqi_tools.urdf import URDF
00032 import copy
00033 import naoqi_tools.gazeboUrdf
00034 import naoqi_tools.urdf as ur
00035 import naoqi_tools.nao_dictionaries as dico
00036 import subprocess
00037 import os
00038 import math
00039 from xml.dom.minidom import Document
00040 
00041 NAO_XACRO_DICO = {
00042     'head': 'gaze',
00043     'legs': 'sole',
00044     'arms': 'gripper',
00045     'torso': 'torso',
00046     }
00047 
00048 ROMEO_XACRO_DICO = {
00049     'head': 'gaze',
00050     'legs': 'sole',
00051     'arms': 'gripper',
00052     'torso': 'body',
00053     'eyes': 'Eye',
00054     }
00055 
00056 PEPPER_XACRO_DICO = {
00057     'head': 'Head',
00058     'legs': 'base_footprint',
00059     'arms': 'gripper',
00060     'torso': 'torso',
00061     }
00062 
00063 COLLISION_SUFFIX = '_0.10.stl'
00064 
00065 parser = argparse.ArgumentParser(usage='Load an URDF file')
00066 parser.add_argument('-i', '--input', default='', help='URDF file to load')
00067 parser.add_argument('-r', '--REP120', choices=['true', 'false'],
00068     default='true', help='Rename the links to be REP120 compliant')
00069 parser.add_argument('-x', '--xacro', choices=['urdf', 'robot'],
00070 default='robot', help='Chose robot part to generate. choosing urdf create a'
00071     'single urdf file with the entire robot. robot will OUTPUT a xacro file '
00072     'for every kinematic chain on the robot')
00073 
00074 ####################
00075 ##### FUNCTIONS ####
00076 ####################
00077 
00078 
00079 def define_materials():
00080     """Create a few materials.
00081 
00082     to display geometrical shapes in a given color.
00083     """
00084     global robot
00085     robot.add_material(ur.Material('Black', ur.Color(0.1, 0.1, 0.1, 1)))
00086     robot.add_material(ur.Material('LightGrey', ur.Color(0.9, 0.9, 0.9, 1)))
00087     robot.add_material(ur.Material('Grey', ur.Color(0.6, 0.6, 0.6, 1)))
00088     robot.add_material(ur.Material('DarkGrey', ur.Color(0.3, 0.3, 0.3, 1)))
00089 
00090 
00091 def REP120_compatibility():
00092     """Add frames defined by ROS for humanoid robots.
00093 
00094     (REP120): http://www.ros.org/reps/rep-0120.html
00095     """
00096     # TODO Add toe frames for ROMEO (not supported by NAOqi yet)
00097     global robot, NAME, MESH_VERSION, VERSION, LINKS_DICO, OFFSETS_DICO
00098     print('creating and renaming joints & links to comply to REP120')
00099 
00100     # Rename links
00101     for joint in robot.joints:
00102         if robot.joints[joint].name.endswith('_joint'):
00103             robot.joints[joint].name = robot.joints[joint].name[0:-6]
00104         if robot.joints[joint].name.endswith('_actuator'):
00105             robot.joints[joint].name = robot.joints[joint].name[0:-9]
00106         if robot.joints[joint].mimic is not None:
00107             if robot.joints[joint].mimic.joint_name.endswith('_actuator'):
00108                 robot.joints[joint].mimic.joint_name = \
00109                     robot.joints[joint].mimic.joint_name[0:-9]
00110             if robot.joints[joint].mimic.joint_name.endswith('_joint'):
00111                 robot.joints[joint].mimic.joint_name = \
00112                     robot.joints[joint].mimic.joint_name[0:-6]
00113         try:
00114             robot.joints[joint].parent = LINKS_DICO[robot.joints[joint].parent]
00115         except KeyError:
00116             pass
00117         try:
00118             robot.joints[joint].child = LINKS_DICO[robot.joints[joint].child]
00119         except KeyError:
00120             pass
00121         for link in robot.links.keys():
00122             try:
00123                 robot.rename_link(link, LINKS_DICO[link])
00124             except KeyError, ValueError:
00125                 pass
00126 
00127     if NAME == 'romeo':
00128         robot.add_link(ur.Link('gaze'))
00129         robot.add_joint(ur.Joint('gaze_joint', 'HeadRoll_link',
00130             'gaze', 'fixed', None, ur.Pose(
00131                 (OFFSETS_DICO['CameraLeftEyeOffsetX'], 0,
00132                 OFFSETS_DICO['CameraLeftEyeOffsetZ']), (0, 0, 0))))
00133         MESH_VERSION = ''
00134 
00135     elif NAME == 'nao':
00136         robot.add_link(ur.Link('gaze'))
00137         robot.add_joint(ur.Joint('gaze_joint', 'Head',
00138             'gaze', 'fixed', None, ur.Pose(
00139                 (OFFSETS_DICO['CameraTopV4OffsetX'], 0,
00140                 OFFSETS_DICO['CameraTopV4OffsetZ']), (0, 0, 0))))
00141         if VERSION == 'V32':
00142             MESH_VERSION = VERSION
00143         elif VERSION == 'V33' or VERSION == 'V40' or VERSION == 'V50':
00144             MESH_VERSION = 'V40'
00145 
00146     elif NAME == 'pepper':
00147         MESH_VERSION = VERSION
00148         # add base_footprint frame
00149         robot.add_link(ur.Link('base_footprint'))
00150         robot.add_joint(ur.Joint('base_footprint_joint', 'Tibia',
00151             'base_footprint', 'fixed', None, ur.Pose(
00152                 (OFFSETS_DICO['BaseFootprintOffsetX'],
00153                 OFFSETS_DICO['BaseFootprintOffsetY'],
00154                 OFFSETS_DICO['BaseFootprintOffsetZ']),
00155                 (OFFSETS_DICO['BaseFootprintRotX'],
00156                 OFFSETS_DICO['BaseFootprintRotY'],
00157                 OFFSETS_DICO['BaseFootprintRotZ']))))
00158 
00159         # rename the laser frames to sensor frames
00160         # (they are actually not used for computation)
00161         laser_links = [c for c in robot.links.keys()
00162             if 'surrounding' in c.lower()]
00163         for joint in robot.joints.values():
00164             if joint.child in laser_links:
00165                 laser_frame = joint.child
00166                 laser_device_frame = laser_frame[:-5] + 'device_frame'
00167                 # get the old joint to have the device frame as a child
00168                 joint.child = laser_device_frame
00169                 # but also create a joint with the projected frame as a child
00170                 robot.add_link(ur.Link(laser_device_frame))
00171                 joint_new = copy.deepcopy(joint)
00172                 joint_new.name = joint.name[:-17] + \
00173                     'projected_sensor_fixedjoint'
00174                 joint_new.child = laser_frame
00175                 joint_new.origin.rotation[0] = 0
00176                 joint_new.origin.rotation[1] = 0
00177                 # set it on the ground
00178                 joint_new.origin.position[2] = -0.334
00179                 if 'left' in laser_frame.lower():
00180                     # the following line is a temporary fix
00181                     # that should be fixed upstream
00182                     joint_new.origin.rotation[2] = math.pi/2.0 + \
00183                         0.1864836732051034
00184                 elif 'right' in laser_frame.lower():
00185                     # the following line is a temporary fix
00186                     # that should be fixed upstream
00187                     joint.origin.position[0] = -0.018
00188                     joint_new.origin.position[0] = -0.018
00189                     # the following line is a temporary fix
00190                     # that should be fixed upstream
00191                     joint_new.origin.rotation[2] = -math.pi/2.0 \
00192                         - 0.1864836732051034
00193                 elif 'front' in laser_frame.lower():
00194                     joint_new.origin.rotation[2] = 0
00195                 robot.add_joint(joint_new)
00196 
00197     # add an optical frame for each robot
00198     camera_frames = [c for c in robot.links.keys() if 'camera' in c.lower()]
00199     for camera_frame in camera_frames:
00200         camera_optical_frame = camera_frame[:-6] + '_optical_frame'
00201         robot.add_link(ur.Link(camera_optical_frame))
00202         robot.add_joint(ur.Joint('%s_fixedjoint' % camera_optical_frame,
00203             camera_frame, camera_optical_frame, 'fixed', None,
00204             ur.Pose((0, 0, 0), (-math.pi/2.0, 0, -math.pi/2.0))))
00205 
00206     # add dummy physics for gazebo simulation
00207     add_dummy_inertia(['Finger', 'Thumb', 'gripper', 'Fsr'])
00208     add_dummy_collision(['Fsr'])
00209 
00210 
00211 def add_transmission_tags():
00212     """Should instanciate all transmission tags.
00213 
00214     - hardware interface
00215     - mechanical reduction ratio for each motor
00216     - joint and actuator to which the transmission tag reference
00217     for now naoTransmission.xacro has been done by hand based on the work
00218     of Konstantinos Chatzilygeroudis in his nao_dcm project
00219     https://github.com/costashatz/nao_dcm
00220     """
00221     global robot
00222     # TODO create all transmission elements : Cannot get them from the lib for now
00223     return
00224 
00225 
00226 def add_gazebo_tags():
00227     """Should instanciate all gazebo tags.
00228 
00229         - sensor plugins
00230         - mimic joints plugins
00231         - ros_control plugin
00232         - disable_links plugins
00233         - gazebo reference for every link (ok)
00234     for now naoGazebo.xacro has been done by hand based on the work of
00235     Konstantinos Chatzilygeroudis in his nao_dcm project
00236     https://github.com/costashatz/nao_dcm
00237     """
00238     global robot
00239     # TODO instantiate plugins according to sensors present in input urdf
00240     return
00241 
00242 
00243 def add_dummy_inertia(list):
00244     """Add a dummy Inertial tag to every links containing keyword in list."""
00245     global robot
00246     for string in list:
00247         for link in robot.links:
00248             if robot.links[link].name.find(string) != -1:
00249                 robot.links[link].inertial = ur.Inertial(1.1e-9, 0.0, 0.0,
00250                     1.1e-9, 0.0, 1.1e-9, 2e-06)
00251 
00252 
00253 def add_dummy_collision(list):
00254     """Add a Box collision tag to every links containing keyword in list."""
00255     global robot
00256     for string in list:
00257         for link in robot.links:
00258             if robot.links[link].name.find(string) != -1:
00259                 robot.links[link].collision = ur.Collision(
00260                     ur.Box([0.01, 0.01, 0.005]),
00261                     ur.Pose((0, 0, 0), (0, 0, 0)))
00262 
00263 ##################
00264 ##### Meshes #####
00265 ##################
00266 
00267 
00268 def create_visual_xacro():
00269     """Create a <ROBOT>_visual_collision.xacro file.
00270 
00271     with xacro macros for visual and collision tags of the urdf.
00272     This function creates xacro macros for visualisation and collisions.
00273     It checks if the meshes are on the computer and set the 'meshes_installed'
00274     property accordingly
00275     """
00276     global robot
00277     global OUTPUT
00278     global MESH_VERSION
00279     global NAME
00280     global LINKS_DICO
00281     global VISU_DICO
00282     global OFFSETS_DICO
00283     global MESHPKG
00284     prefix = 'insert_visu_'
00285     doc = Document()
00286     root = doc.createElement('robot')
00287     doc.appendChild(root)
00288     root.setAttribute("xmlns:xacro", "http://www.ros.org/wiki/xacro")
00289 
00290     cmd = 'rospack find ' + NAME + MESHPKG
00291     try:
00292         path_mesh_pkg = subprocess.check_output(cmd, stderr=subprocess.STDOUT,
00293             shell=True)[:-1]
00294     except:
00295         print('unable to find ' + NAME + MESHPKG + ' package')
00296         sys.exit(0)
00297     # Set Mesh path
00298     if NAME == 'nao':
00299         node = ur.short(doc, 'xacro:property', 'name', 'PI_2')
00300         node.setAttribute('value', str(math.pi/2.0))
00301         root.appendChild(node)
00302         node = ur.short(doc, 'xacro:property', 'name', 'meshes_installed')
00303 
00304         if os.path.isdir(os.path.join(path_mesh_pkg, 'meshes', MESH_VERSION)):
00305             node.setAttribute('value', 'true')
00306         else:
00307             node.setAttribute('value', 'false')
00308         root.appendChild(node)
00309 
00310     # Insert xacro macro
00311     for link in robot.links:
00312         (tempVisu, tempCol) = adjustMeshPath(path_mesh_pkg, link)
00313         if robot.links[link].visual is not None:
00314             robot.links[link].xacro = 'xacro:' + prefix + \
00315                 robot.links[link].name
00316             node = ur.short(doc, 'xacro:macro', 'name', prefix +
00317                 robot.links[link].name)
00318             if NAME == 'nao':
00319                 # add xacro condition macro to handle the absence of meshes
00320                 node2 = ur.short(doc, 'xacro:unless', 'value',
00321                     '${meshes_installed}')
00322                 if tempVisu is not None:
00323                     node2.appendChild(tempVisu.to_xml(doc))
00324                 if tempCol is not None:
00325                     node2.appendChild(tempCol.to_xml(doc))
00326                 node.appendChild(node2)
00327                 node3 = ur.short(doc, 'xacro:if', 'value', '${meshes_installed}')
00328                 node3.appendChild(robot.links[link].visual.to_xml(doc))
00329                 node3.appendChild(robot.links[link].collision.to_xml(doc))
00330                 node.appendChild(node3)
00331             else:
00332                 node.appendChild(robot.links[link].visual.to_xml(doc))
00333                 node.appendChild(robot.links[link].collision.to_xml(doc))
00334             root.appendChild(node)
00335         robot.links[link].visual = None
00336         robot.links[link].collision = None
00337     filename = OUTPUT[0:OUTPUT.rfind('.')] + '_visual_collisions.xacro'
00338     write_comments_in_xacro(doc, filename)
00339 
00340 
00341 #################################
00342 ######## XACRO FUNCTIONS ########
00343 #################################
00344 
00345 
00346 def export_robot_element(element):
00347     """
00348     Export the 'elements' related to the keyword 'element'.
00349 
00350     :param : element, string in ['Transmission', 'Gazebo', 'material']
00351 
00352     The output file is <ROBOT>_<element>.xacro
00353     """
00354     global robot, OUTPUT
00355     doc = Document()
00356     root = doc.createElement('robot')
00357     doc.appendChild(root)
00358     root.setAttribute("xmlns:xacro", "http://www.ros.org/wiki/xacro")
00359     for i in robot.elements:
00360         try:
00361             if element == 'Transmission':
00362                 if i.name.find(element) != -1:
00363                     root.appendChild(i.to_xml(doc))
00364             elif element == 'Gazebo':
00365                 if i.reference is not None:
00366                     root.appendChild(i.to_xml(doc))
00367                 elif i.plugins != []:
00368                     root.appendChild(i.to_xml(doc))
00369             elif element == 'material':
00370                 if type(i) == naoqi_tools.urdf.Material:
00371                     root.appendChild(i.to_xml(doc))
00372         except AttributeError:
00373             pass
00374     filename = OUTPUT[0:OUTPUT.rfind('.')] + '_' + str(element) + '.xacro'
00375     print('exporting ' + element + ' xacro')
00376     write_comments_in_xacro(doc, filename)
00377 
00378 
00379 def export_robot_to_xacro_files():
00380     """
00381     Export the entire 'robot' in several xacro files.
00382 
00383     One xacro file per kinematic chain (<ROBOT>_legs.xacro,
00384     <ROBOT>_arms.xacro, <ROBOT>_torso.xacro...)
00385     Xacro file for specific parts of the robot (<ROBOT>_fingers.xacro,
00386     <ROBOT>_sensors.xacro)
00387     One xacro file for visual elements (<ROBOT>_visual_collision.xacro,
00388     <ROBOT>_material.xacro)
00389     One xacro file per type of element needed for gazebo simulation
00390     (<ROBOT>_Gazebo.xacro, <ROBOT>_Transmission.xacro)
00391     One generic robot file which includes all the other ones
00392     (<ROBOT>_robot.xacro)
00393     """
00394     global robot, OUTPUT, NAME
00395     doc = Document()
00396     root = doc.createElement('robot')
00397     doc.appendChild(root)
00398     root.setAttribute("xmlns:xacro", "http://www.ros.org/wiki/xacro")
00399     root.setAttribute("name", robot.name)
00400     root.appendChild(ur.short(doc, 'xacro:include', 'filename', NAME +
00401         '_visual_collisions.xacro'))
00402     create_visual_xacro()
00403     for i in XACRO_DICO.keys():
00404         print('exporting ' + NAME + '_' + i + '.xacro')
00405         if i.find('eye') != -1:
00406             export_kinematic_chain_to_xacro(i, 'HeadRoll_link',
00407                 'HeadRoll_link')
00408         else:
00409             export_kinematic_chain_to_xacro(i)
00410         filenamerobot = NAME + '_' + i + '.xacro'
00411         root.appendChild(ur.short(doc, 'xacro:include', 'filename',
00412             filenamerobot))
00413     # Transmission elements not available from Aldebaran libraries yet
00414     export_robot_element('Transmission')
00415     root.appendChild(ur.short(doc, 'xacro:include', 'filename', NAME +
00416         '_Transmission.xacro'))
00417     # Gazebo Plugin not available from Aldebaran libraries yet
00418     export_robot_element('Gazebo')
00419     root.appendChild(ur.short(doc, 'xacro:include', 'filename', NAME +
00420         '_Gazebo.xacro'))
00421     root.appendChild(ur.short(doc, 'xacro:include', 'filename', NAME +
00422         '_sensors.xacro'))
00423     export_list_to_xacro(['_frame'], OUTPUT[0:OUTPUT.rfind('.')] +
00424         '_sensors.xacro')
00425     root.appendChild(ur.short(doc, 'xacro:include', 'filename', NAME +
00426         '_fingers.xacro'))
00427     export_list_to_xacro(['Finger', 'Thumb'], OUTPUT[0:OUTPUT.rfind('.')] +
00428         '_fingers.xacro')
00429     if NAME == 'pepper':
00430         root.appendChild(ur.short(doc, 'xacro:include', 'filename', NAME +
00431             '_wheels.xacro'))
00432         export_list_to_xacro(['Wheel'], OUTPUT[0:OUTPUT.rfind('.')] +
00433             '_wheels.xacro')
00434     if NAME == 'romeo':
00435         root.appendChild(ur.short(doc, 'xacro:include', 'filename',
00436             'romeo_cap.xacro'))
00437 
00438     filename = OUTPUT[0:OUTPUT.rfind('.')] + '_robot.xacro'
00439     write_comments_in_xacro(doc, filename)
00440     print('output directory : ' + OUTPUT[0:OUTPUT.rfind('/') + 1])
00441 
00442 
00443 def export_kinematic_chain_to_xacro(keyword, baseChain='base_link',
00444                                     tipRefChain='default'):
00445     """Export a specific kinematic chain to a xacro file.
00446 
00447     :param : keyword, string defining kinematic chains to export
00448     (legs,arms,head,torso)
00449     :param : baseChain, string representing the name of the link where
00450     the reference chain starts
00451     :param : tipRefChain, string representing the name of the link where
00452     the reference chain ends
00453     """
00454     global robot, OUTPUT
00455     if tipRefChain == 'default':
00456         print('applying torso to end of ref chain')
00457         tipRefChain = XACRO_DICO['torso']
00458     chainRef = robot.get_chain(baseChain, tipRefChain)
00459     print(chainRef)
00460     doc = Document()
00461     root = doc.createElement('robot')
00462     doc.appendChild(root)
00463     root.setAttribute('xmlns:xacro', 'http://www.ros.org/wiki/xacro')
00464     chainNb = 0
00465     try:
00466         chain1 = robot.get_chain(baseChain, 'l_' + XACRO_DICO[keyword])
00467         chain2 = robot.get_chain(baseChain, 'r_' + XACRO_DICO[keyword])
00468         chainNb = 2
00469     except KeyError:
00470         try:
00471             chain1 = robot.get_chain(baseChain, 'L' + XACRO_DICO[keyword])
00472             chain2 = robot.get_chain(baseChain, 'R' + XACRO_DICO[keyword])
00473             chainNb = 2
00474         except KeyError:
00475             try:
00476                 chain1 = robot.get_chain(baseChain, XACRO_DICO[keyword])
00477                 chainNb = 1
00478             except KeyError:
00479                 print('the chain ' + keyword + ' cannot be found')
00480 
00481     if chainNb != 0:
00482         duplicate = 0
00483         for i in range(len(chain1)):
00484             for j in range(len(chainRef)):
00485                 if chain1[i] == chainRef[j]:
00486                     duplicate = 1
00487             if duplicate == 0 or keyword == 'torso':
00488                 try:
00489                     root.appendChild(robot.links[chain1[i]].to_xml(doc))
00490                 except KeyError:
00491                     try:
00492                         root.appendChild(robot.joints[chain1[i]].to_xml(doc))
00493                     except KeyError:
00494                         print('unknown element' + chain1[i])
00495             else:
00496                 duplicate = 0
00497         if chainNb == 2:
00498             for i in range(len(chain2)):
00499                 for j in range(len(chainRef)):
00500                     if chain2[i] == chainRef[j]:
00501                         duplicate = 1
00502                 if duplicate == 0:
00503                     try:
00504                         root.appendChild(robot.links[chain2[i]].to_xml(doc))
00505                     except KeyError:
00506                         try:
00507                             root.appendChild(
00508                                 robot.joints[chain2[i]].to_xml(doc))
00509                         except KeyError:
00510                             print('unknown element' + chain2[i])
00511                 else:
00512                     duplicate = 0
00513         filename = OUTPUT[0:OUTPUT.rfind('.')] + '_' + keyword + str('.xacro')
00514         write_comments_in_xacro(doc, filename)
00515 
00516 
00517 def write_comments_in_xacro(doc, filename):
00518     """
00519     Write the content of the XML Document doc to a file named filename.
00520 
00521     Also add comments at the beginning of the file
00522 
00523     :param : doc, minidom Document to write
00524     :param : filename, absolute path of the file to write to
00525     """
00526     if(not os.path.isdir(filename[0:filename.rfind('/') + 1])):
00527         os.makedirs(filename[0:filename.rfind('/')])
00528 
00529     file = open(filename, 'w+')
00530     file.write(doc.toprettyxml())
00531     file.close()
00532     file = open(filename, 'r')
00533     firstline, remaining = file.readline(), file.read()
00534     file.close()
00535     file = open(filename, 'w')
00536     file.write(firstline)
00537     file.write(
00538         '<!--**************************************************************\n'
00539         ' **** File automatically generated by generate_urdf.py script ****\n'
00540         ' **************************************************************-->\n')
00541     file.write(remaining)
00542     file.close()
00543 
00544 
00545 def export_list_to_xacro(list, filename):
00546     """Export all links containing a string and its parent joint.
00547 
00548     :param : list, list of strings to look for
00549     :param : filename, absolute path of the file to write to
00550     """
00551     global robot, OUTPUT
00552     doc = Document()
00553     root = doc.createElement('robot')
00554     doc.appendChild(root)
00555     root.setAttribute("xmlns:xacro", "http://www.ros.org/wiki/xacro")
00556     print ('exporting ' + os.path.basename(filename))
00557     for string in list:
00558         for link in robot.links:
00559             if robot.links[link].name.find(string) != -1:
00560                 root.appendChild(robot.links[link].to_xml(doc))
00561                 for joint in robot.joints:
00562                     if robot.joints[joint].child == robot.links[link].name:
00563                         root.appendChild(robot.joints[joint].to_xml(doc))
00564     write_comments_in_xacro(doc, filename)
00565 
00566 
00567 def adjustMeshPath(path_mesh_pkg, link):
00568     """
00569     Find the path of a mesh according to the link definition.
00570 
00571     Set the visual and collision element of the given link
00572 
00573     :param : path_mesh_pkg, absolute path of the package where the meshes
00574     should be located
00575     :param : link, dictionary key of the link we want to set
00576     visual and collision parameters
00577 
00578     :return : tempVisu, Visual element with the NAO visual geometrical shape
00579     for people who don't have the meshes
00580     :return : tempCol, Collision element with the NAO collision geometrical
00581     shape for people who don't have the meshes
00582     """
00583     global robot, SCALE, MESHPKG, MESH_VERSION
00584     tempVisu = None
00585     tempVisuMesh = None
00586     tempCol = None
00587     tempColMesh = None
00588     if robot.links[link].visual is not None:
00589         try:
00590             meshname = str(
00591                 LINKS_DICO.keys()[list(LINKS_DICO.values()).index(link)])
00592             if meshname.endswith('_link'):
00593                 meshfile = meshname[0:-5]
00594             else:
00595                 meshfile = meshname
00596         except:
00597             meshname = link
00598             meshfile = link
00599             pass
00600         if meshfile.endswith('_link'):
00601             meshfile = meshfile[0:-5]
00602         tempVisuMesh = ur.Visual(ur.Mesh('', (SCALE, SCALE, SCALE)))
00603         tempColMesh = ur.Collision(ur.Mesh('', (SCALE, SCALE, SCALE)))
00604         tempVisuMesh.origin = robot.links[link].visual.origin
00605         tempColMesh.origin = robot.links[link].visual.origin
00606         if os.path.isfile(os.path.join(path_mesh_pkg, 'meshes', MESH_VERSION,
00607             robot.links[link].visual.geometry.filename[
00608                 robot.links[link].visual.geometry.filename.rfind('/') + 1:])):
00609             tempVisuMesh.geometry.filename = os.path.join(
00610                 'package://', NAME + MESHPKG, 'meshes', MESH_VERSION,
00611                 robot.links[link].visual.geometry.filename[
00612                     robot.links[link].visual.geometry.filename.rfind('/')+1:])
00613             tempColMesh.geometry.filename = \
00614                 tempVisuMesh.geometry.filename[0:-4] + COLLISION_SUFFIX
00615         else:
00616             tempVisuMesh.geometry.filename = os.path.join(
00617                 'package://', NAME + MESHPKG, 'meshes', MESH_VERSION,
00618                 meshfile + '.dae')
00619             tempColMesh.geometry.filename = \
00620                 tempVisuMesh.geometry.filename[0:-4] + COLLISION_SUFFIX
00621 
00622         if NAME == 'nao':
00623             try:
00624                 tempVisu = ur.Visual(
00625                     VISU_DICO[meshname], ur.Material('LightGrey'),
00626                     dico.Nao_orig[meshname])
00627                 tempCol = ur.Collision(VISU_DICO[meshname],
00628                     dico.Nao_orig[meshname])
00629             except KeyError:
00630                 tempVisu = None
00631                 tempCol = None
00632         robot.links[link].visual = tempVisuMesh
00633         robot.links[link].collision = tempColMesh
00634     return (tempVisu, tempCol)
00635 
00636 ##############
00637 #### Main ####
00638 ##############
00639 
00640 args = parser.parse_args()
00641 if args.input is '':
00642     robot = URDF.from_parameter_server()
00643 else:
00644     robot = URDF.load_xml_file(args.input)
00645 
00646 if robot.name.find('V') != -1:
00647     VERSION = robot.name[robot.name.find('V'):]
00648 else:
00649     VERSION = args.input[args.input.find('V'):args.input.find('V') + 3]
00650 
00651 if robot.name.lower().find('nao') != -1:
00652     NAME = 'nao'
00653     try:
00654         import naoqi_tools.nao_dictionaries as dico
00655         print('import nao dictionaries')
00656     except:
00657         print('unable to import nao dictionaries')
00658         sys.exit(0)
00659     LINKS_DICO = dico.Nao_links
00660     VISU_DICO = dico.Nao_visu
00661     OFFSETS_DICO = dico.Nao_offsets
00662     XACRO_DICO = NAO_XACRO_DICO
00663     MESHPKG = '_meshes'
00664     SCALE = 0.1
00665 elif robot.name.lower().find('romeo') != -1:
00666     NAME = 'romeo'
00667     try:
00668         import naoqi_tools.romeo_dictionaries as dico
00669     except:
00670         print('unable to import romeo dictionaries')
00671         sys.exit(0)
00672     LINKS_DICO = dico.Romeo_links
00673     OFFSETS_DICO = dico.Romeo_offsets
00674     VISU_DICO = ''
00675     XACRO_DICO = ROMEO_XACRO_DICO
00676     MESHPKG = '_description'
00677     SCALE = 1
00678 
00679 elif robot.name.lower().find('juliette') or robot.name.lower().find('pepper'):
00680     NAME = 'pepper'
00681     try:
00682         import naoqi_tools.pepper_dictionaries as dico
00683     except:
00684         print('unable to import pepper dictionaries')
00685         sys.exit(0)
00686     LINKS_DICO = dico.Pepper_links
00687     OFFSETS_DICO = dico.Pepper_offsets
00688     VISU_DICO = ''
00689     XACRO_DICO = PEPPER_XACRO_DICO
00690     print('PROCESSING PEPPER ROBOT')
00691     MESHPKG = '_meshes'
00692     SCALE = 0.1
00693     VERSION = '1.0'
00694 
00695 for element in robot.elements:
00696     if type(element) == naoqi_tools.urdf.Material:
00697         robot.elements.remove(element)
00698 cmd = 'rospack find ' + NAME + '_description'
00699 try:
00700     pathdescription = subprocess.check_output(cmd,
00701         stderr=subprocess.STDOUT, shell=True)[:-1]
00702 except:
00703     print('unable to find ' + NAME + '_description package')
00704     sys.exit(0)
00705 OUTPUT = os.path.join(pathdescription, 'urdf', NAME + VERSION +
00706     '_generated_urdf', NAME + '.urdf')
00707 print('processing ' + NAME + ' (' + VERSION + ") robot's urdf file in " +
00708     OUTPUT)
00709 cmd = 'rospack find ' + NAME + MESHPKG
00710 try:
00711     path_mesh_pkg = subprocess.check_output(cmd, stderr=subprocess.STDOUT,
00712         shell=True)[:-1]
00713 except:
00714     print('unable to find ' + NAME + MESHPKG + ' package')
00715     sys.exit(0)
00716 
00717 define_materials()
00718 if args.REP120 == 'true':
00719     REP120_compatibility()
00720 
00721 for link in robot.links:
00722     adjustMeshPath(path_mesh_pkg, link)
00723 
00724 if args.xacro == 'robot':
00725     export_robot_element('material')
00726     export_robot_to_xacro_files()
00727 elif args.xacro == 'urdf':
00728     robot.write_xml(OUTPUT)
00729 else:
00730     export_kinematic_chain_to_xacro(args.xacro)


naoqi_tools
Author(s): Mikael Arguedas
autogenerated on Wed Aug 16 2017 02:28:16