3 IKFast Plugin Generator for MoveIt! 5 Creates a kinematics plugin using the output of IKFast from OpenRAVE. 6 This plugin and the move_group node can be used as a general 7 kinematics service, from within the moveit planning environment, or in 10 Author: Dave Coleman, CU Boulder 11 Based heavily on the arm_kinematic_tools package by Jeremy Zoss, SwRI 12 and the arm_navigation plugin generator by David Butterworth, KAIST 17 Copyright (c) 2013, Jeremy Zoss, SwRI 20 Redistribution and use in source and binary forms, with or without 21 modification, are permitted provided that the following conditions are met: 23 * Redistributions of source code must retain the above copyright 24 notice, this list of conditions and the following disclaimer. 25 * Redistributions in binary form must reproduce the above copyright 26 notice, this list of conditions and the following disclaimer in the 27 documentation and/or other materials provided with the distribution. 28 * Neither the name of the Willow Garage, Inc. nor the names of its 29 contributors may be used to endorse or promote products derived from 30 this software without specific prior written permission. 32 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 33 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 34 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 35 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 36 IABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 37 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 38 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 39 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 40 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 41 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 42 POSSIBILITY OF SUCH DAMAGE. 51 from lxml
import etree
54 plugin_gen_pkg =
'moveit_kinematics' 55 plugin_sub_dir =
'ikfast_kinematics_plugin' 57 search_modes = [
'OPTIMIZE_MAX_JOINT',
'OPTIMIZE_FREE_JOINT' ]
59 if __name__ ==
'__main__':
62 robot_name = sys.argv[1]
63 planning_group_name = sys.argv[2]
64 moveit_plugin_pkg = sys.argv[3]
65 if len(sys.argv) == 6:
66 ikfast_output_file = sys.argv[5]
67 search_mode = sys.argv[4]
68 if search_mode
not in search_modes:
69 print 'Invalid search mode. Allowed values: ', search_modes
71 elif len(sys.argv) == 5:
72 search_mode = search_modes[0];
73 print "Warning: The default search has changed from OPTIMIZE_FREE_JOINT to now %s!" % (search_mode)
74 ikfast_output_file = sys.argv[4]
78 print(
"\nUsage: create_ikfast_plugin.py <yourrobot_name> <planning_group_name> <moveit_plugin_pkg> [<search_mode>] <ikfast_output_path>\n")
80 print '\nIKFast Plugin Generator' 84 plan_pkg = robot_name +
'_moveit_config' 85 plan_pkg_dir = roslib.packages.get_pkg_dir(plan_pkg)
86 print 'Loading robot from \''+plan_pkg+
'\' package ... ' 88 print '\nERROR: can\'t find package '+plan_pkg+
'\n' 91 plugin_pkg = moveit_plugin_pkg
92 plugin_pkg_dir = roslib.packages.get_pkg_dir(plugin_pkg)
93 print 'Creating plugin in \''+plugin_pkg+
'\' package ... ' 95 print '\nERROR: can\'t find package '+plugin_pkg+
'\n' 100 srdf_files = glob.glob(plan_pkg_dir+
'/config/*.srdf')
101 if (len(srdf_files) == 1):
102 srdf_file_name = srdf_files[0]
104 srdf_file_name = plan_pkg_dir +
'/config/' + robot_name +
'.srdf' 105 srdf = etree.parse(srdf_file_name).getroot()
107 print(
"\nERROR: unable to parse robot configuration file\n" + srdf_file_name +
"\n")
110 if (robot_name != srdf.get(
'name')):
111 print '\nERROR: non-matching robot name found in ' + srdf_file_name +
'.' \
112 +
' Expected \'' + robot_name +
'\',' +
' found \''+srdf.get(
'name')+
'\'' 115 groups = srdf.findall(
'group')
116 if(len(groups) < 1) :
118 if groups[0].get(
'name') ==
None:
121 print(
"\nERROR: need at least 1 planning group in robot planning description ")
122 print srdf_file_name +
'\n' 124 print ' found ' + str(len(groups)) +
' planning groups: ' \
125 +
", ".join([g.get(
'name')
for g
in groups])
128 planning_group =
None 130 foundName = (g.get(
'name').lower() == planning_group_name.lower())
135 if planning_group
is None:
136 print '\nERROR: could not find planning group ' + planning_group_name +
' in SRDF.\n' 138 print ' found group \'' + planning_group_name +
'\'' 141 plugin_pkg_src_dir = plugin_pkg_dir+
'/src/' 142 plugin_pkg_include_dir = plugin_pkg_dir+
'/include/' 144 if not os.path.exists(plugin_pkg_src_dir):
145 os.makedirs(plugin_pkg_src_dir)
146 if not os.path.exists(plugin_pkg_include_dir):
147 os.makedirs(plugin_pkg_include_dir)
150 if not os.path.exists(ikfast_output_file):
151 print '\nERROR: can\'t find IKFast source code at \'' + \
152 ikfast_output_file +
'\'\n' 153 print 'Make sure this input argument is correct \n' 157 solver_file_name = plugin_pkg_dir+
'/src/'+robot_name+
'_'+planning_group_name+
'_ikfast_solver.cpp' 160 if os.path.exists(ikfast_output_file) & os.path.exists(solver_file_name):
161 if os.path.samefile(ikfast_output_file, solver_file_name):
162 print 'Skipping copying ' + solver_file_name +
' since it is already in the correct location' 165 shutil.copy2(ikfast_output_file,solver_file_name)
167 if not os.path.exists(solver_file_name):
168 print '\nERROR: Unable to copy IKFast source code from \'' + ikfast_output_file +
'\'' +
' to \'' + solver_file_name +
'\'' 169 print 'Manually copy the source file generated by IKFast to this location \n' 174 with open(solver_file_name,
'r') as src: 176 if line.startswith(
'/// ikfast version'):
177 line_search = re.search(
'ikfast version (.*) generated', line)
179 solver_version = int(line_search.group(1), 0)
181 print ' found source code generated by IKFast version ' + str(solver_version)
185 plugin_gen_dir = os.path.join(roslib.packages.get_pkg_dir(plugin_gen_pkg), plugin_sub_dir)
187 print '\nERROR: can\'t find package '+plugin_gen_pkg+
' \n' 191 if solver_version >= 56:
192 template_version = 61
194 print '\nERROR this converter is not made for IKFast 54 or anything but 61' 198 template_header_file = plugin_gen_dir +
'/templates/ikfast.h' 199 if not os.path.exists(template_header_file):
200 print '\nERROR: can\'t find ikfast header file at \'' + template_header_file +
'\'\n' 204 header_file_name = plugin_pkg_dir+
'/include/ikfast.h' 205 shutil.copy2(template_header_file,header_file_name)
206 if not os.path.exists(header_file_name):
207 print '\nERROR: Unable to copy IKFast header file from \'' + \
208 template_header_file +
'\'' +
' to \'' + header_file_name +
'\' \n' 209 print 'Manually copy ikfast.h to this location \n' 213 template_file_name = plugin_gen_dir +
'/templates/ikfast' + str(template_version) +
'_moveit_plugin_template.cpp' 215 if not os.path.exists(template_file_name):
216 print '\nERROR: can\'t find template file at \'' + template_file_name +
'\'\n' 220 template_file_data = open(template_file_name,
'r') 221 template_text = template_file_data.read() 222 template_text = re.sub('_ROBOT_NAME_', robot_name, template_text)
223 template_text = re.sub(
'_GROUP_NAME_', planning_group_name, template_text)
224 template_text = re.sub(
'_SEARCH_MODE_', search_mode, template_text)
225 plugin_file_base = robot_name +
'_' + planning_group_name +
'_ikfast_moveit_plugin.cpp' 227 plugin_file_name = plugin_pkg_dir +
'/src/' + plugin_file_base
228 with open(plugin_file_name,
'w')
as f:
229 f.write(template_text)
230 print '\nCreated plugin file at \'' + plugin_file_name +
'\'' 233 ik_library_name = robot_name +
"_" + planning_group_name +
"_moveit_ikfast_plugin" 234 plugin_name = robot_name +
'_' + planning_group_name + \
235 '_kinematics/IKFastKinematicsPlugin' 236 plugin_def = etree.Element(
"library", path=
"lib/lib"+ik_library_name)
237 cl = etree.SubElement(plugin_def,
"class")
238 cl.set(
"name", plugin_name)
239 cl.set(
"type",
'ikfast_kinematics_plugin::IKFastKinematicsPlugin')
240 cl.set(
"base_class_type",
"kinematics::KinematicsBase")
241 desc = etree.SubElement(cl,
"description")
242 desc.text =
'IKFast'+str(template_version)+
' plugin for closed-form kinematics' 245 def_file_base = ik_library_name +
"_description.xml" 246 def_file_name = plugin_pkg_dir +
"/" + def_file_base
247 with open(def_file_name,
'w')
as f:
248 etree.ElementTree(plugin_def).write(f, xml_declaration=
True, pretty_print=
True, encoding=
"UTF-8")
249 print '\nCreated plugin definition at: \''+def_file_name+
'\'' 255 cmake_template_file = plugin_gen_dir+
"/templates/CMakeLists.txt" 256 if not os.path.exists(cmake_template_file):
257 print '\nERROR: can\'t find CMakeLists template file at \'' + cmake_template_file +
'\'\n' 261 cmake_file = plugin_pkg_dir+
'/CMakeLists.txt' 264 template_file_data = open(cmake_template_file,
'r') 265 template_text = template_file_data.read() 266 template_text = re.sub('_ROBOT_NAME_', robot_name, template_text)
267 template_text = re.sub(
'_GROUP_NAME_', planning_group_name, template_text)
268 template_text = re.sub(
'_PACKAGE_NAME_', moveit_plugin_pkg, template_text)
269 template_text = re.sub(
'_LIBRARY_NAME_', ik_library_name, template_text)
271 with open(cmake_file,
'w')
as f:
272 f.write(template_text)
273 print '\nOverwrote CMakeLists file at \'' + cmake_file +
'\'' 276 parser = etree.XMLParser(remove_blank_text=
True)
277 package_file_name = plugin_pkg_dir+
"/package.xml" 278 package_xml = etree.parse(package_file_name, parser)
281 build_deps = [
"liblapack-dev",
"moveit_core",
"pluginlib",
"roscpp",
"tf_conversions"]
282 run_deps = [
"liblapack-dev",
"moveit_core",
"pluginlib",
"roscpp",
"tf_conversions"]
285 curr_deps = [e.text
for e
in e_parent.findall(req_type)]
286 missing_deps = set(reqd_deps) - set(curr_deps)
287 for d
in missing_deps:
288 etree.SubElement(e_parent, req_type).text = d
291 update_deps(build_deps,
"build_depend", package_xml.getroot())
292 update_deps(run_deps,
"exec_depend", package_xml.getroot())
295 new_export = etree.Element(
"moveit_core", \
296 plugin=
"${prefix}/"+def_file_base)
297 export_element = package_xml.getroot().find(
"export")
298 if export_element ==
None:
299 export_element = etree.SubElement(package_xml.getroot(),
"export")
301 for el
in export_element.findall(
"moveit_core"):
302 found = (etree.tostring(new_export) == etree.tostring(el))
306 export_element.append(new_export)
310 with open(package_file_name,
"w")
as f:
311 package_xml.write(f, xml_declaration=
True, pretty_print=
True, encoding=
"UTF-8")
312 print '\nModified package.xml at \''+package_file_name+
'\'' 315 kin_yaml_file_name = plan_pkg_dir+
"/config/kinematics.yaml" 316 with open(kin_yaml_file_name,
'r') as f: 317 kin_yaml_data = yaml.safe_load(f) 318 kin_yaml_data[planning_group_name]["kinematics_solver"] = plugin_name
319 with open(kin_yaml_file_name,
'w')
as f:
320 yaml.dump(kin_yaml_data, f,default_flow_style=
False)
321 print '\nModified kinematics.yaml at ' + kin_yaml_file_name
324 easy_script_file_name =
"update_ikfast_plugin.sh" 325 easy_script_file_path = plugin_pkg_dir +
"/" + easy_script_file_name
326 with open(easy_script_file_path,
'w')
as f:
327 f.write(
"rosrun moveit_kinematics create_ikfast_moveit_plugin.py" 329 +
" " + planning_group_name
331 +
" " + solver_file_name )
333 print '\nCreated update plugin script at '+easy_script_file_path
def update_deps(reqd_deps, req_type, e_parent)