create_ikfast_moveit_plugin.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 '''
00003 IKFast Plugin Generator for MoveIt!
00004 
00005 Creates a kinematics plugin using the output of IKFast from OpenRAVE.
00006 This plugin and the move_group node can be used as a general
00007 kinematics service, from within the moveit planning environment, or in
00008 your own ROS node.
00009 
00010 Author: Dave Coleman, CU Boulder
00011         Based heavily on the arm_kinematic_tools package by Jeremy Zoss, SwRI
00012         and the arm_navigation plugin generator by David Butterworth, KAIST
00013 Date: March 2013
00014 
00015 '''
00016 '''
00017 Copyright (c) 2013, Jeremy Zoss, SwRI
00018 All rights reserved.
00019 
00020 Redistribution and use in source and binary forms, with or without
00021 modification, are permitted provided that the following conditions are met:
00022 
00023 * Redistributions of source code must retain the above copyright
00024 notice, this list of conditions and the following disclaimer.
00025 * Redistributions in binary form must reproduce the above copyright
00026 notice, this list of conditions and the following disclaimer in the
00027 documentation and/or other materials provided with the distribution.
00028 * Neither the name of the Willow Garage, Inc. nor the names of its
00029 contributors may be used to endorse or promote products derived from
00030 this software without specific prior written permission.
00031 
00032 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00033 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00034 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00035 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00036 IABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00037 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00038 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00039 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00040 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00041 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00042 POSSIBILITY OF SUCH DAMAGE.
00043 '''
00044 
00045 import glob
00046 import sys
00047 import roslib
00048 import re
00049 import os
00050 import yaml
00051 from lxml import etree
00052 import shutil
00053 
00054 plugin_gen_pkg = 'moveit_kinematics'  # package containing this file
00055 plugin_sub_dir = 'ikfast_kinematics_plugin' # sub directory which contains the template directory
00056 # Allowed search modes, see SEARCH_MODE enum in template file
00057 search_modes = ['OPTIMIZE_MAX_JOINT', 'OPTIMIZE_FREE_JOINT' ]
00058 
00059 if __name__ == '__main__':
00060    # Check input arguments
00061    try:
00062       robot_name = sys.argv[1]
00063       planning_group_name = sys.argv[2]
00064       moveit_plugin_pkg = sys.argv[3]
00065       if len(sys.argv) == 6:
00066          ikfast_output_file = sys.argv[5]
00067          search_mode = sys.argv[4]
00068          if search_mode not in search_modes:
00069             print 'Invalid search mode. Allowed values: ', search_modes
00070             raise Exception()
00071       elif len(sys.argv) == 5:
00072          search_mode = search_modes[0];
00073          print "Warning: The default search has changed from OPTIMIZE_FREE_JOINT to now %s!" % (search_mode)
00074          ikfast_output_file = sys.argv[4]
00075       else:
00076          raise Exception()
00077    except:
00078       print("\nUsage: create_ikfast_plugin.py <yourrobot_name> <planning_group_name> <moveit_plugin_pkg> [<search_mode>] <ikfast_output_path>\n")
00079       sys.exit(-1)
00080    print '\nIKFast Plugin Generator'
00081 
00082    # Setup key package directories
00083    try:
00084       plan_pkg = robot_name + '_moveit_config'
00085       plan_pkg_dir = roslib.packages.get_pkg_dir(plan_pkg)
00086       print 'Loading robot from \''+plan_pkg+'\' package ... '
00087    except:
00088       print '\nERROR: can\'t find package '+plan_pkg+'\n'
00089       sys.exit(-1)
00090    try:
00091       plugin_pkg = moveit_plugin_pkg
00092       plugin_pkg_dir = roslib.packages.get_pkg_dir(plugin_pkg)
00093       print 'Creating plugin in \''+plugin_pkg+'\' package ... '
00094    except:
00095       print '\nERROR: can\'t find package '+plugin_pkg+'\n'
00096       sys.exit(-1)
00097 
00098    # Check for at least 1 planning group
00099    try:
00100       srdf_files = glob.glob(plan_pkg_dir+'/config/*.srdf')
00101       if (len(srdf_files) == 1):
00102          srdf_file_name = srdf_files[0]
00103       else:
00104          srdf_file_name = plan_pkg_dir + '/config/' + robot_name + '.srdf'
00105       srdf = etree.parse(srdf_file_name).getroot()
00106    except:
00107       print("\nERROR: unable to parse robot configuration file\n" + srdf_file_name + "\n")
00108       sys.exit(-1)
00109    try:
00110       if (robot_name != srdf.get('name')):
00111          print '\nERROR: non-matching robot name found in ' + srdf_file_name + '.' \
00112              + '  Expected \'' + robot_name + '\',' + ' found \''+srdf.get('name')+'\''
00113          raise
00114 
00115       groups = srdf.findall('group')
00116       if(len(groups) < 1) : # No groups
00117          raise
00118       if groups[0].get('name') == None: # Group name is blank
00119          raise
00120    except:
00121       print("\nERROR: need at least 1 planning group in robot planning description ")
00122       print srdf_file_name + '\n'
00123       sys.exit(-1)
00124    print '  found ' + str(len(groups)) + ' planning groups: ' \
00125        + ", ".join([g.get('name') for g in groups])
00126 
00127    # Select manipulator arm group
00128    planning_group = None
00129    for g in groups:
00130       foundName  = (g.get('name').lower() == planning_group_name.lower())
00131 
00132       if (foundName):
00133          planning_group = g
00134          break
00135    if planning_group is None:
00136       print '\nERROR: could not find planning group ' + planning_group_name + ' in SRDF.\n'
00137       sys.exit(-1)
00138    print '  found group \'' + planning_group_name + '\''
00139 
00140    # Create src and include folders in target package
00141    plugin_pkg_src_dir = plugin_pkg_dir+'/src/'
00142    plugin_pkg_include_dir = plugin_pkg_dir+'/include/'
00143 
00144    if not os.path.exists(plugin_pkg_src_dir):
00145       os.makedirs(plugin_pkg_src_dir)
00146    if not os.path.exists(plugin_pkg_include_dir):
00147       os.makedirs(plugin_pkg_include_dir)
00148 
00149    # Check for source code generated by IKFast
00150    if not os.path.exists(ikfast_output_file):
00151       print '\nERROR: can\'t find IKFast source code at \'' + \
00152           ikfast_output_file + '\'\n'
00153       print 'Make sure this input argument is correct \n'
00154       sys.exit(-1)
00155 
00156    # Copy the source code generated by IKFast into our src folder
00157    solver_file_name = plugin_pkg_dir+'/src/'+robot_name+'_'+planning_group_name+'_ikfast_solver.cpp'
00158    # Check if they are the same file - if so, skip
00159    skip = False
00160    if os.path.exists(ikfast_output_file) & os.path.exists(solver_file_name):
00161       if os.path.samefile(ikfast_output_file, solver_file_name):
00162          print 'Skipping copying ' + solver_file_name + ' since it is already in the correct location'
00163          skip = True
00164    if not skip:
00165       shutil.copy2(ikfast_output_file,solver_file_name)
00166    # Error check
00167    if not os.path.exists(solver_file_name):
00168       print '\nERROR: Unable to copy IKFast source code from \'' + ikfast_output_file + '\'' + ' to \'' + solver_file_name + '\''
00169       print 'Manually copy the source file generated by IKFast to this location \n'
00170       sys.exit(-1)
00171 
00172    # replace unqualified isnan()s with qualified ones (issue 584)
00173    with open(solver_file_name, 'r') as f:
00174       solver_file_contents = f.read()
00175    solver_file_contents = re.sub(r"(?<!std::)isnan\s*\(", "std::isnan(", solver_file_contents)
00176    with open(solver_file_name, 'w') as f:
00177       f.write(solver_file_contents)
00178 
00179    # Detect version of IKFast used to generate solver code
00180    solver_version = 0
00181    with open(solver_file_name,'r') as src:
00182       for line in src:
00183          if line.startswith('/// ikfast version'):
00184             line_search = re.search('ikfast version (.*) generated', line)
00185             if line_search:
00186                solver_version = int(line_search.group(1), 0)
00187             break
00188    print '  found source code generated by IKFast version ' + str(solver_version)
00189 
00190    # Get template folder location
00191    try:
00192       plugin_gen_dir = os.path.join(roslib.packages.get_pkg_dir(plugin_gen_pkg), plugin_sub_dir)
00193    except:
00194       print '\nERROR: can\'t find package '+plugin_gen_pkg+' \n'
00195       sys.exit(-1)
00196 
00197    # Chose template depending on IKFast version
00198    if solver_version >= 56:
00199       template_version = 61
00200    else:
00201       print '\nERROR this converter is not made for IKFast 54 or anything but 61'
00202       sys.exit(-1)
00203 
00204    # Check if IKFast header file exists
00205    template_header_file = plugin_gen_dir + '/templates/ikfast.h'
00206    if not os.path.exists(template_header_file):
00207       print '\nERROR: can\'t find ikfast header file at \'' + template_header_file + '\'\n'
00208       sys.exit(-1)
00209 
00210    # Copy the IKFast header file into the include directory
00211    header_file_name = plugin_pkg_dir+'/include/ikfast.h'
00212    shutil.copy2(template_header_file,header_file_name)
00213    if not os.path.exists(header_file_name):
00214       print '\nERROR: Unable to copy IKFast header file from \'' + \
00215           template_header_file + '\'' + ' to \'' + header_file_name + '\' \n'
00216       print 'Manually copy ikfast.h to this location \n'
00217       sys.exit(-1)
00218 
00219    # Check if template exists
00220    template_file_name = plugin_gen_dir + '/templates/ikfast' + str(template_version) + '_moveit_plugin_template.cpp'
00221 
00222    if not os.path.exists(template_file_name):
00223       print '\nERROR: can\'t find template file at \'' + template_file_name + '\'\n'
00224       sys.exit(-1)
00225 
00226    # Create plugin source from template
00227    template_file_data = open(template_file_name, 'r')
00228    template_text = template_file_data.read()
00229    template_text = re.sub('_ROBOT_NAME_', robot_name, template_text)
00230    template_text = re.sub('_GROUP_NAME_', planning_group_name, template_text)
00231    template_text = re.sub('_SEARCH_MODE_', search_mode, template_text)
00232    plugin_file_base = robot_name + '_' + planning_group_name + '_ikfast_moveit_plugin.cpp'
00233 
00234    plugin_file_name = plugin_pkg_dir + '/src/' + plugin_file_base
00235    with open(plugin_file_name,'w') as f:
00236       f.write(template_text)
00237    print '\nCreated plugin file at \'' + plugin_file_name + '\''
00238 
00239    # Create plugin definition .xml file
00240    ik_library_name = robot_name + "_" + planning_group_name + "_moveit_ikfast_plugin"
00241    plugin_name = robot_name + '_' + planning_group_name + \
00242        '_kinematics/IKFastKinematicsPlugin'
00243    plugin_def = etree.Element("library", path="lib/lib"+ik_library_name)
00244    cl = etree.SubElement(plugin_def, "class")
00245    cl.set("name", plugin_name)
00246    cl.set("type", 'ikfast_kinematics_plugin::IKFastKinematicsPlugin')
00247    cl.set("base_class_type", "kinematics::KinematicsBase")
00248    desc = etree.SubElement(cl, "description")
00249    desc.text = 'IKFast'+str(template_version)+' plugin for closed-form kinematics'
00250 
00251    # Write plugin definition to file
00252    def_file_base = ik_library_name + "_description.xml"
00253    def_file_name = plugin_pkg_dir + "/" + def_file_base
00254    with open(def_file_name,'w') as f:
00255       etree.ElementTree(plugin_def).write(f, xml_declaration=True, pretty_print=True)
00256    print '\nCreated plugin definition at: \''+def_file_name+'\''
00257 
00258 
00259 
00260 
00261    # Check if CMakeLists file exists
00262    cmake_template_file = plugin_gen_dir+"/templates/CMakeLists.txt"
00263    if not os.path.exists(cmake_template_file):
00264       print '\nERROR: can\'t find CMakeLists template file at \'' + cmake_template_file + '\'\n'
00265       sys.exit(-1)
00266 
00267    # Create new CMakeLists file
00268    cmake_file = plugin_pkg_dir+'/CMakeLists.txt'
00269 
00270    # Create plugin source from template
00271    template_file_data = open(cmake_template_file, 'r')
00272    template_text = template_file_data.read()
00273    template_text = re.sub('_ROBOT_NAME_', robot_name, template_text)
00274    template_text = re.sub('_GROUP_NAME_', planning_group_name, template_text)
00275    template_text = re.sub('_PACKAGE_NAME_', moveit_plugin_pkg, template_text)
00276    template_text = re.sub('_LIBRARY_NAME_', ik_library_name, template_text)
00277 
00278    with open(cmake_file,'w') as f:
00279       f.write(template_text)
00280    print '\nOverwrote CMakeLists file at \'' + cmake_file + '\''
00281 
00282    # Add plugin export to package manifest
00283    parser = etree.XMLParser(remove_blank_text=True)
00284    package_file_name = plugin_pkg_dir+"/package.xml"
00285    package_xml = etree.parse(package_file_name, parser)
00286 
00287    # Make sure at least all required dependencies are in the depends lists
00288    build_deps = ["liblapack-dev", "moveit_core", "pluginlib", "roscpp", "tf_conversions"]
00289    run_deps   = ["liblapack-dev", "moveit_core", "pluginlib", "roscpp", "tf_conversions"]
00290 
00291    def update_deps(reqd_deps, req_type, e_parent):
00292       curr_deps = [e.text for e in e_parent.findall(req_type)]
00293       missing_deps = set(reqd_deps) - set(curr_deps)
00294       for d in missing_deps:
00295          etree.SubElement(e_parent, req_type).text = d
00296       return missing_deps
00297 
00298    # empty sets evaluate to false
00299    modified_pkg  = update_deps(build_deps, "build_depend", package_xml.getroot())
00300    modified_pkg |= update_deps(run_deps, "run_depend", package_xml.getroot())
00301 
00302    if modified_pkg:
00303       with open(package_file_name,"w") as f:
00304          package_xml.write(f, xml_declaration=True, pretty_print=True)
00305 
00306       print '\nModified package.xml at \''+package_file_name+'\''
00307 
00308    # Check that plugin definition file is in the export list
00309    new_export = etree.Element("moveit_core", \
00310                                  plugin="${prefix}/"+def_file_base)
00311    export_element = package_xml.getroot().find("export")
00312    if export_element == None:
00313       export_element = etree.SubElement(package_xml.getroot(), "export")
00314    found = False
00315    for el in export_element.findall("moveit_core"):
00316       found = (etree.tostring(new_export) == etree.tostring(el))
00317       if found: break
00318 
00319    if not found:
00320       export_element.append(new_export)
00321       with open(package_file_name,"w") as f:
00322          package_xml.write(f, xml_declaration=True, pretty_print=True)
00323       print '\nModified package.xml at \''+package_file_name+'\''
00324 
00325    # Modify kinematics.yaml file
00326    kin_yaml_file_name = plan_pkg_dir+"/config/kinematics.yaml"
00327    with open(kin_yaml_file_name, 'r') as f:
00328       kin_yaml_data = yaml.safe_load(f)
00329    kin_yaml_data[planning_group_name]["kinematics_solver"] = plugin_name
00330    with open(kin_yaml_file_name, 'w') as f:
00331       yaml.dump(kin_yaml_data, f,default_flow_style=False)
00332    print '\nModified kinematics.yaml at ' + kin_yaml_file_name
00333 
00334    # Create a script for easily updating the plugin in the future in case the plugin needs to be updated
00335    easy_script_file_name = "update_ikfast_plugin.sh"
00336    easy_script_file_path = plugin_pkg_dir + "/" + easy_script_file_name
00337    with open(easy_script_file_path,'w') as f:
00338       f.write("rosrun moveit_kinematics create_ikfast_moveit_plugin.py"
00339               + " " + robot_name
00340               + " " + planning_group_name
00341               + " " + plugin_pkg
00342               + " " + solver_file_name )
00343 
00344    print '\nCreated update plugin script at '+easy_script_file_path


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Wed Jan 17 2018 03:32:16