create_ikfast_moveit_plugin.py
Go to the documentation of this file.
1 #! /usr/bin/env python
2 '''
3 IKFast Plugin Generator for MoveIt!
4 
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
8 your own ROS node.
9 
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
13 Date: March 2013
14 
15 '''
16 '''
17 Copyright (c) 2013, Jeremy Zoss, SwRI
18 All rights reserved.
19 
20 Redistribution and use in source and binary forms, with or without
21 modification, are permitted provided that the following conditions are met:
22 
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.
31 
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.
43 '''
44 
45 import glob
46 import sys
47 import roslib
48 import re
49 import os
50 import yaml
51 from lxml import etree
52 import shutil
53 
54 plugin_gen_pkg = 'moveit_kinematics' # package containing this file
55 plugin_sub_dir = 'ikfast_kinematics_plugin' # sub directory which contains the template directory
56 # Allowed search modes, see SEARCH_MODE enum in template file
57 search_modes = ['OPTIMIZE_MAX_JOINT', 'OPTIMIZE_FREE_JOINT' ]
58 
59 if __name__ == '__main__':
60  # Check input arguments
61  try:
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
70  raise Exception()
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]
75  else:
76  raise Exception()
77  except:
78  print("\nUsage: create_ikfast_plugin.py <yourrobot_name> <planning_group_name> <moveit_plugin_pkg> [<search_mode>] <ikfast_output_path>\n")
79  sys.exit(-1)
80  print '\nIKFast Plugin Generator'
81 
82  # Setup key package directories
83  try:
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 ... '
87  except:
88  print '\nERROR: can\'t find package '+plan_pkg+'\n'
89  sys.exit(-1)
90  try:
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 ... '
94  except:
95  print '\nERROR: can\'t find package '+plugin_pkg+'\n'
96  sys.exit(-1)
97 
98  # Check for at least 1 planning group
99  try:
100  srdf_files = glob.glob(plan_pkg_dir+'/config/*.srdf')
101  if (len(srdf_files) == 1):
102  srdf_file_name = srdf_files[0]
103  else:
104  srdf_file_name = plan_pkg_dir + '/config/' + robot_name + '.srdf'
105  srdf = etree.parse(srdf_file_name).getroot()
106  except:
107  print("\nERROR: unable to parse robot configuration file\n" + srdf_file_name + "\n")
108  sys.exit(-1)
109  try:
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')+'\''
113  raise
114 
115  groups = srdf.findall('group')
116  if(len(groups) < 1) : # No groups
117  raise
118  if groups[0].get('name') == None: # Group name is blank
119  raise
120  except:
121  print("\nERROR: need at least 1 planning group in robot planning description ")
122  print srdf_file_name + '\n'
123  sys.exit(-1)
124  print ' found ' + str(len(groups)) + ' planning groups: ' \
125  + ", ".join([g.get('name') for g in groups])
126 
127  # Select manipulator arm group
128  planning_group = None
129  for g in groups:
130  foundName = (g.get('name').lower() == planning_group_name.lower())
131 
132  if (foundName):
133  planning_group = g
134  break
135  if planning_group is None:
136  print '\nERROR: could not find planning group ' + planning_group_name + ' in SRDF.\n'
137  sys.exit(-1)
138  print ' found group \'' + planning_group_name + '\''
139 
140  # Create src and include folders in target package
141  plugin_pkg_src_dir = plugin_pkg_dir+'/src/'
142  plugin_pkg_include_dir = plugin_pkg_dir+'/include/'
143 
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)
148 
149  # Check for source code generated by IKFast
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'
154  sys.exit(-1)
155 
156  # Copy the source code generated by IKFast into our src folder
157  solver_file_name = plugin_pkg_dir+'/src/'+robot_name+'_'+planning_group_name+'_ikfast_solver.cpp'
158  # Check if they are the same file - if so, skip
159  skip = False
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'
163  skip = True
164  if not skip:
165  shutil.copy2(ikfast_output_file,solver_file_name)
166  # Error check
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'
170  sys.exit(-1)
171 
172  # Detect version of IKFast used to generate solver code
173  solver_version = 0
174  with open(solver_file_name,'r') as src:
175  for line in src:
176  if line.startswith('/// ikfast version'):
177  line_search = re.search('ikfast version (.*) generated', line)
178  if line_search:
179  solver_version = int(line_search.group(1), 0)
180  break
181  print ' found source code generated by IKFast version ' + str(solver_version)
182 
183  # Get template folder location
184  try:
185  plugin_gen_dir = os.path.join(roslib.packages.get_pkg_dir(plugin_gen_pkg), plugin_sub_dir)
186  except:
187  print '\nERROR: can\'t find package '+plugin_gen_pkg+' \n'
188  sys.exit(-1)
189 
190  # Chose template depending on IKFast version
191  if solver_version >= 56:
192  template_version = 61
193  else:
194  print '\nERROR this converter is not made for IKFast 54 or anything but 61'
195  sys.exit(-1)
196 
197  # Check if IKFast header file exists
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'
201  sys.exit(-1)
202 
203  # Copy the IKFast header file into the include directory
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'
210  sys.exit(-1)
211 
212  # Check if template exists
213  template_file_name = plugin_gen_dir + '/templates/ikfast' + str(template_version) + '_moveit_plugin_template.cpp'
214 
215  if not os.path.exists(template_file_name):
216  print '\nERROR: can\'t find template file at \'' + template_file_name + '\'\n'
217  sys.exit(-1)
218 
219  # Create plugin source from template
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'
226 
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 + '\''
231 
232  # Create plugin definition .xml file
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'
243 
244  # Write plugin definition to file
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+'\''
250 
251 
252 
253 
254  # Check if CMakeLists file exists
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'
258  sys.exit(-1)
259 
260  # Create new CMakeLists file
261  cmake_file = plugin_pkg_dir+'/CMakeLists.txt'
262 
263  # Create plugin source from template
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)
270 
271  with open(cmake_file,'w') as f:
272  f.write(template_text)
273  print '\nOverwrote CMakeLists file at \'' + cmake_file + '\''
274 
275  # Add plugin export to package manifest
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)
279 
280  # Make sure at least all required dependencies are in the depends lists
281  build_deps = ["liblapack-dev", "moveit_core", "pluginlib", "roscpp", "tf_conversions"]
282  run_deps = ["liblapack-dev", "moveit_core", "pluginlib", "roscpp", "tf_conversions"]
283 
284  def update_deps(reqd_deps, req_type, e_parent):
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
289  return missing_deps
290 
291  update_deps(build_deps, "build_depend", package_xml.getroot())
292  update_deps(run_deps, "exec_depend", package_xml.getroot())
293 
294  # Check that plugin definition file is in the export list
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")
300  found = False
301  for el in export_element.findall("moveit_core"):
302  found = (etree.tostring(new_export) == etree.tostring(el))
303  if found: break
304 
305  if not found:
306  export_element.append(new_export)
307 
308  # Always write the package xml file, even if there are no changes, to ensure
309  # proper encodings are used in the future (UTF-8)
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+'\''
313 
314  # Modify kinematics.yaml file
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
322 
323  # Create a script for easily updating the plugin in the future in case the plugin needs to be updated
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"
328  + " " + robot_name
329  + " " + planning_group_name
330  + " " + plugin_pkg
331  + " " + solver_file_name )
332 
333  print '\nCreated update plugin script at '+easy_script_file_path
def update_deps(reqd_deps, req_type, e_parent)


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 10 2019 04:03:41