create_ikfast_moveit_plugin.py
Go to the documentation of this file.
1 #! /usr/bin/env python
2 from __future__ import print_function
3 '''
4 IKFast Plugin Generator for MoveIt!
5 
6 Creates a kinematics plugin using the output of IKFast from OpenRAVE.
7 This plugin and the move_group node can be used as a general
8 kinematics service, from within the moveit planning environment, or in
9 your own ROS node.
10 
11 Author: Dave Coleman, PickNik Robotics
12  Michael Lautman, PickNik Robotics
13  Based heavily on the arm_kinematic_tools package by Jeremy Zoss, SwRI
14  and the arm_navigation plugin generator by David Butterworth, KAIST
15 
16 Date: March 2013
17 
18 '''
19 '''
20 Copyright (c) 2013, Jeremy Zoss, SwRI
21 All rights reserved.
22 
23 Redistribution and use in source and binary forms, with or without
24 modification, are permitted provided that the following conditions are met:
25 
26 * Redistributions of source code must retain the above copyright
27 notice, this list of conditions and the following disclaimer.
28 * Redistributions in binary form must reproduce the above copyright
29 notice, this list of conditions and the following disclaimer in the
30 documentation and/or other materials provided with the distribution.
31 * Neither the name of the Willow Garage, Inc. nor the names of its
32 contributors may be used to endorse or promote products derived from
33 this software without specific prior written permission.
34 
35 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
36 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
37 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
38 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
39 IABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
40 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
41 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
42 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
43 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
44 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
45 POSSIBILITY OF SUCH DAMAGE.
46 '''
47 
48 import re
49 import os
50 import yaml
51 from lxml import etree
52 from getpass import getuser
53 import shutil
54 import argparse
55 
56 try:
57  from roslib.packages import get_pkg_dir, InvalidROSPkgException
58 except ImportError:
59  print("Failed to import roslib. No ROS environment available? Trying without.")
60  # define stubs
61  class InvalidROSPkgException(Exception):
62  pass
63  def get_pkg_dir(pkg_name):
64  raise InvalidROSPkgException
65 
66 # Package containing this file
67 plugin_gen_pkg = 'moveit_kinematics'
68 # Allowed search modes, see SEARCH_MODE enum in template file
69 search_modes = ['OPTIMIZE_MAX_JOINT', 'OPTIMIZE_FREE_JOINT']
70 
71 
73  parser = argparse.ArgumentParser(
74  description="Generate an IKFast MoveIt! kinematic plugin")
75  parser.add_argument("robot_name",
76  help="The name of your robot")
77  parser.add_argument("planning_group_name",
78  help="The name of the planning group for which your IKFast solution was generated")
79  parser.add_argument("ikfast_plugin_pkg",
80  help="The name of the MoveIt! IKFast Kinematics Plugin to be created/updated")
81  parser.add_argument("base_link_name",
82  help="The name of the base link that was used when generating your IKFast solution")
83  parser.add_argument("eef_link_name",
84  help="The name of the end effector link that was used when generating your IKFast solution")
85  parser.add_argument("ikfast_output_path",
86  help="The full path to the analytic IK solution output by IKFast")
87  parser.add_argument("--search_mode",
88  default=search_modes[0],
89  help="The search mode used to solve IK for robots with more than 6DOF")
90  parser.add_argument("--srdf_filename",
91  help="The name of your robot. Defaults to <robot_name>.srdf")
92  parser.add_argument("--robot_name_in_srdf",
93  help="The name of your robot as defined in the srdf. Defaults to <robot_name>")
94  parser.add_argument("--moveit_config_pkg",
95  help="The robot moveit_config package. Defaults to <robot_name>_moveit_config")
96  return parser
97 
98 
100  if args.srdf_filename is None:
101  args.srdf_filename = args.robot_name + ".srdf"
102  if args.robot_name_in_srdf is None:
103  args.robot_name_in_srdf = args.robot_name
104  if args.moveit_config_pkg is None:
105  args.moveit_config_pkg = args.robot_name + "_moveit_config"
106 
107 
108 def print_args(args):
109  print("Creating IKFastKinematicsPlugin with parameters: ")
110  print(" robot_name: %s" % args.robot_name)
111  print(" base_link_name: %s" % args.base_link_name)
112  print(" eef_link_name: %s" % args.eef_link_name)
113  print(" planning_group_name: %s" % args.planning_group_name)
114  print(" ikfast_plugin_pkg: %s" % args.ikfast_plugin_pkg)
115  print(" ikfast_output_path: %s" % args.ikfast_output_path)
116  print(" search_mode: %s" % args.search_mode)
117  print(" srdf_filename: %s" % args.srdf_filename)
118  print(" robot_name_in_srdf: %s" % args.robot_name_in_srdf)
119  print(" moveit_config_pkg: %s" % args.moveit_config_pkg)
120  print("")
121 
122 
123 def update_deps(reqd_deps, req_type, e_parent):
124  curr_deps = [e.text for e in e_parent.findall(req_type)]
125  missing_deps = set(reqd_deps) - set(curr_deps)
126  for dep in missing_deps:
127  etree.SubElement(e_parent, req_type).text = dep
128 
129 
131  if not os.path.exists(args.ikfast_output_path):
132  raise Exception("Can't find IKFast source code at " + args.ikfast_output_path)
133 
134  # Detect version of IKFast used to generate solver code
135  solver_version = 0
136  with open(args.ikfast_output_path, 'r') as src:
137  for line in src:
138  if line.startswith('/// ikfast version'):
139  line_search = re.search('ikfast version (.*) generated', line)
140  if line_search:
141  solver_version = int(line_search.group(1), 0) & ~0x10000000
142  break
143  print('Found source code generated by IKFast version %s' % str(solver_version))
144 
145  # Chose template depending on IKFast version
146  if solver_version >= 56:
147  setattr(args, 'template_version', 61)
148  else:
149  raise Exception('This converter requires IKFast 0.5.6 or newer.')
150 
151 def xmlElement(name, text=None, **attributes):
152  e = etree.Element(name, **attributes)
153  e.text = text
154  return e
155 
157  try:
158  setattr(args, 'ikfast_plugin_pkg_path', get_pkg_dir(args.ikfast_plugin_pkg))
159  except InvalidROSPkgException:
160  args.ikfast_plugin_pkg_path = os.path.abspath(args.ikfast_plugin_pkg)
161  print("Failed to find package: %s. Will create it in %s." %
162  (args.ikfast_plugin_pkg, args.ikfast_plugin_pkg_path))
163  # update pkg name to basename of path
164  args.ikfast_plugin_pkg=os.path.basename(args.ikfast_plugin_pkg_path)
165 
166  src_path = args.ikfast_plugin_pkg_path + '/src/'
167  if not os.path.exists(src_path):
168  os.makedirs(src_path)
169 
170  include_path = args.ikfast_plugin_pkg_path + '/include/'
171  if not os.path.exists(include_path):
172  os.makedirs(include_path)
173 
174  # Create package.xml
175  pkg_xml_path = args.ikfast_plugin_pkg_path + '/package.xml'
176  if not os.path.exists(pkg_xml_path):
177  root = xmlElement("package", format="2")
178  root.append(xmlElement("name", text=args.ikfast_plugin_pkg))
179  root.append(xmlElement("version", text="0.0.0"))
180  root.append(xmlElement("description", text="IKFast plugin for " + args.robot_name))
181  root.append(xmlElement("license", text="BSD"))
182  user_name = getuser()
183  root.append(xmlElement("maintainer", email="%s@todo.todo" % user_name, text=user_name))
184  root.append(xmlElement("buildtool_depend", text="catkin"))
185  with open(pkg_xml_path, 'w') as f:
186  etree.ElementTree(root).write(f, xml_declaration=True, pretty_print=True, encoding="UTF-8")
187  print("Created package.xml at: '%s'" % pkg_xml_path)
188 
189 
191  for candidate in [os.path.dirname(__file__) + '/../templates']:
192  if os.path.exists(candidate) and os.path.exists(candidate + '/ikfast.h'):
193  return os.path.realpath(candidate)
194  try:
195  return os.path.join(get_pkg_dir(plugin_gen_pkg), 'ikfast_kinematics_plugin/templates')
196  except InvalidROSPkgException:
197  raise Exception("Can't find package %s" % plugin_gen_pkg)
198 
199 
201  # Copy the source code generated by IKFast into our src folder
202  src_path = args.ikfast_plugin_pkg_path + '/src/'
203  solver_file_path = src_path + args.robot_name + '_' + args.planning_group_name + '_ikfast_solver.cpp'
204  if not os.path.exists(solver_file_path) or not os.path.samefile(args.ikfast_output_path, solver_file_path):
205  shutil.copy2(args.ikfast_output_path, solver_file_path)
206 
207  if not os.path.exists(solver_file_path):
208  raise Exception("Failed to copy IKFast source code from '%s' to '%s'\n"
209  "Manually copy the source file generated by IKFast to this location and re-run" %
210  (args.ikfast_output_path, solver_file_path))
211  # Remember ikfast solver file for update of MoveIt package
212  args.ikfast_output_path = solver_file_path
213 
214  # Get template folder location
215  template_dir = find_template_dir()
216 
217  # namespace for the plugin
218  setattr(args, 'namespace', args.robot_name + "_" + args.planning_group_name)
219  replacements = dict(_ROBOT_NAME_= args.robot_name,
220  _GROUP_NAME_ = args.planning_group_name,
221  _SEARCH_MODE_ = args.search_mode,
222  _EEF_LINK_ = args.eef_link_name,
223  _BASE_LINK_ = args.base_link_name,
224  _PACKAGE_NAME_ = args.ikfast_plugin_pkg,
225  _NAMESPACE_ = args.namespace
226  )
227 
228  # Copy ikfast header file
229  copy_file(template_dir + '/ikfast.h', args.ikfast_plugin_pkg_path + "/include/ikfast.h",
230  "ikfast header file")
231  # Create ikfast plugin template
232  copy_file(template_dir + '/ikfast' + str(args.template_version) + '_moveit_plugin_template.cpp',
233  args.ikfast_plugin_pkg_path + "/src/" + args.robot_name + '_' + args.planning_group_name + "_ikfast_moveit_plugin.cpp",
234  "ikfast plugin file", replacements)
235 
236  # Create plugin definition .xml file
237  ik_library_name = args.namespace + "_moveit_ikfast_plugin"
238  plugin_def = etree.Element("library", path="lib/lib" + ik_library_name)
239  setattr(args, 'plugin_name', args.namespace + '/IKFastKinematicsPlugin')
240  cl = etree.SubElement(plugin_def, "class", name=args.plugin_name,
241  type=args.namespace + "::IKFastKinematicsPlugin",
242  base_class_type="kinematics::KinematicsBase")
243  desc = etree.SubElement(cl, "description")
244  desc.text = 'IKFast{template} plugin for closed-form kinematics of {robot} {group}' \
245  .format(template=args.template_version, robot=args.robot_name, group=args.planning_group_name)
246 
247  # Write plugin definition to file
248  plugin_file_name = ik_library_name + "_description.xml"
249  plugin_file_path = args.ikfast_plugin_pkg_path + "/" + plugin_file_name
250  with open(plugin_file_path, 'w') as f:
251  etree.ElementTree(plugin_def).write(f, xml_declaration=True, pretty_print=True, encoding="UTF-8")
252  print("Created plugin definition at '%s'" % plugin_file_path)
253 
254  # Create CMakeLists file
255  replacements.update(dict(_LIBRARY_NAME_ = ik_library_name))
256  copy_file(template_dir + "/CMakeLists.txt", args.ikfast_plugin_pkg_path + '/CMakeLists.txt',
257  "cmake file", replacements)
258 
259  # Add plugin export to package manifest
260  parser = etree.XMLParser(remove_blank_text=True)
261  package_file_name = args.ikfast_plugin_pkg_path + "/package.xml"
262  package_xml = etree.parse(package_file_name, parser).getroot()
263 
264  # Make sure at least all required dependencies are in the depends lists
265  build_deps = ["liblapack-dev", "moveit_core", "pluginlib", "roscpp", "tf2_kdl", "tf2_eigen", "eigen_conversions"]
266  run_deps = ["liblapack-dev", "moveit_core", "pluginlib", "roscpp", "eigen_conversions"]
267 
268  update_deps(build_deps, "build_depend", package_xml)
269  update_deps(run_deps, "exec_depend", package_xml)
270 
271  # Check that plugin definition file is in the export list
272  new_export = etree.Element("moveit_core", plugin="${prefix}/" + plugin_file_name)
273 
274  export_element = package_xml.find("export")
275  if export_element is None:
276  export_element = etree.SubElement(package_xml, "export")
277 
278  found = False
279  for el in export_element.findall("moveit_core"):
280  found = (etree.tostring(new_export) == etree.tostring(el))
281  if found:
282  break
283 
284  if not found:
285  export_element.append(new_export)
286 
287  # Always write the package xml file, even if there are no changes, to ensure
288  # proper encodings are used in the future (UTF-8)
289  with open(package_file_name, "w") as f:
290  etree.ElementTree(package_xml).write(f, xml_declaration=True, pretty_print=True, encoding="UTF-8")
291  print("Wrote package.xml at '%s'" % package_file_name)
292 
293  # Create a script for easily updating the plugin in the future in case the plugin needs to be updated
294  easy_script_file_path = args.ikfast_plugin_pkg_path + "/update_ikfast_plugin.sh"
295  with open(easy_script_file_path, 'w') as f:
296  f.write("rosrun moveit_kinematics create_ikfast_moveit_plugin.py" +
297  " --search_mode=" + args.search_mode +
298  " --srdf_filename=" + args.srdf_filename +
299  " --robot_name_in_srdf=" + args.robot_name_in_srdf +
300  " --moveit_config_pkg=" + args.moveit_config_pkg +
301  " " + args.robot_name +
302  " " + args.planning_group_name +
303  " " + args.ikfast_plugin_pkg +
304  " " + args.base_link_name +
305  " " + args.eef_link_name +
306  " " + args.ikfast_output_path)
307 
308  print("Created update plugin script at '%s'" % easy_script_file_path)
309 
310 
312  try:
313  moveit_config_pkg_path = get_pkg_dir(args.moveit_config_pkg)
314  except InvalidROSPkgException:
315  raise Exception("Failed to find package: " + args.moveit_config_pkg)
316 
317  try:
318  srdf_file_name = moveit_config_pkg_path + '/config/' + args.srdf_filename
319  srdf = etree.parse(srdf_file_name).getroot()
320  except IOError:
321  raise Exception("Failed to find SRDF file: " + srdf_file_name)
322  except etree.XMLSyntaxError as err:
323  raise Exception("Failed to parse xml in file: %s\n%s" % (srdf_file_name, err.msg))
324 
325  if args.robot_name_in_srdf != srdf.get('name'):
326  raise Exception("Robot name in srdf ('%s') doesn't match expected name ('%s')" %
327  (srdf.get('name'), args.robot_name_in_srdf))
328 
329  groups = srdf.findall('group')
330  if len(groups) < 1:
331  raise Exception("No planning groups are defined in the SRDF")
332 
333  planning_group = None
334  for group in groups:
335  if group.get('name').lower() == args.planning_group_name.lower():
336  planning_group = group
337 
338  if planning_group is None:
339  raise Exception("Planning group '%s' not defined in the SRDF. Available groups: \n%s" % (
340  args.planning_group_name, ", ".join([group_name.get('name') for group_name in groups])))
341 
342  # Modify kinematics.yaml file
343  kin_yaml_file_name = moveit_config_pkg_path + "/config/kinematics.yaml"
344  with open(kin_yaml_file_name, 'r') as f:
345  kin_yaml_data = yaml.safe_load(f)
346 
347  kin_yaml_data[args.planning_group_name]["kinematics_solver"] = args.plugin_name
348  with open(kin_yaml_file_name, 'w') as f:
349  yaml.dump(kin_yaml_data, f, default_flow_style=False)
350 
351  print("Modified kinematics.yaml at '%s'" % kin_yaml_file_name)
352 
353 
354 def copy_file(src_path, dest_path, description, replacements=None):
355  if not os.path.exists(src_path):
356  raise Exception("Can't find %s at '%s'" % (description, src_path))
357 
358  if replacements is None:
359  replacements = dict()
360 
361  with open(src_path, 'r') as f:
362  content = f.read()
363 
364  # replace templates
365  for key, value in replacements.items():
366  content = re.sub(key, value, content)
367 
368  with open(dest_path, 'w') as f:
369  f.write(content)
370  print("Created %s at '%s'" % (description, dest_path))
371 
372 
373 def main():
374  parser = create_parser()
375  args = parser.parse_args()
376 
377  populate_optional(args)
378  print_args(args)
382  try:
384  except Exception as e:
385  print("Failed to update MoveIt package:\n" + e.message)
386 
387 
388 if __name__ == '__main__':
389  main()
def copy_file(src_path, dest_path, description, replacements=None)
def xmlElement(name, text=None, attributes)
def update_deps(reqd_deps, req_type, e_parent)


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Wed Nov 20 2019 03:56:30