201 "ikfast_plugin_pkg_path",
202 get_package_share_directory(args.ikfast_plugin_pkg),
204 except PackageNotFoundError:
205 args.ikfast_plugin_pkg_path = os.path.abspath(args.ikfast_plugin_pkg)
207 "Failed to find package: %s. Will create it in %s."
208 % (args.ikfast_plugin_pkg, args.ikfast_plugin_pkg_path)
211 args.ikfast_plugin_pkg = os.path.basename(args.ikfast_plugin_pkg_path)
213 src_path = args.ikfast_plugin_pkg_path +
"/src/"
214 if not os.path.exists(src_path):
215 os.makedirs(src_path)
217 include_path = args.ikfast_plugin_pkg_path +
"/include/"
218 if not os.path.exists(include_path):
219 os.makedirs(include_path)
222 pkg_xml_path = args.ikfast_plugin_pkg_path +
"/package.xml"
223 if not os.path.exists(pkg_xml_path):
225 root.append(
xmlElement(
"name", text=args.ikfast_plugin_pkg))
226 root.append(
xmlElement(
"version", text=
"0.0.0"))
228 xmlElement(
"description", text=
"IKFast plugin for " + args.robot_name)
230 root.append(
xmlElement(
"license", text=
"BSD"))
231 user_name = getuser()
235 root.append(
xmlElement(
"buildtool_depend", text=
"ament_cmake"))
237 export.append(
xmlElement(
"build_type", text=
"ament_cmake"))
239 etree.ElementTree(root).write(
240 pkg_xml_path, xml_declaration=
True, pretty_print=
True, encoding=
"UTF-8"
242 print(
"Created package.xml at: '%s'" % pkg_xml_path)
245 parameters_yaml_path = src_path +
"ikfast_kinematics_parameters.yaml"
246 if not os.path.exists(parameters_yaml_path):
247 print(
"Create parameters.yaml at: '%s'" % parameters_yaml_path)
248 with open(parameters_yaml_path,
"w")
as file:
267 src_path = args.ikfast_plugin_pkg_path +
"/src/"
272 + args.planning_group_name
273 +
"_ikfast_solver.cpp"
275 if not os.path.exists(solver_file_path)
or not os.path.samefile(
276 args.ikfast_output_path, solver_file_path
278 shutil.copy2(args.ikfast_output_path, solver_file_path)
280 if not os.path.exists(solver_file_path):
282 "Failed to copy IKFast source code from '%s' to '%s'\n"
283 "Manually copy the source file generated by IKFast to this location and re-run"
284 % (args.ikfast_output_path, solver_file_path)
287 args.ikfast_output_path = solver_file_path
293 setattr(args,
"namespace", args.robot_name +
"_" + args.planning_group_name)
295 _ROBOT_NAME_=args.robot_name,
296 _GROUP_NAME_=args.planning_group_name,
297 _SEARCH_MODE_=args.search_mode,
298 _EEF_LINK_=args.eef_link_name,
299 _BASE_LINK_=args.base_link_name,
300 _PACKAGE_NAME_=args.ikfast_plugin_pkg,
301 _NAMESPACE_=args.namespace,
306 template_dir +
"/ikfast.h",
307 args.ikfast_plugin_pkg_path +
"/include/ikfast.h",
308 "ikfast header file",
314 + str(args.template_version)
315 +
"_moveit_plugin_template.cpp",
316 args.ikfast_plugin_pkg_path
320 + args.planning_group_name
321 +
"_ikfast_moveit_plugin.cpp",
322 "ikfast plugin file",
327 ik_library_name = args.namespace +
"_moveit_ikfast_plugin"
328 plugin_def = etree.Element(
"library", path=ik_library_name)
329 setattr(args,
"plugin_name", args.namespace +
"/IKFastKinematicsPlugin")
330 cl = etree.SubElement(
333 name=args.plugin_name,
334 type=args.namespace +
"::IKFastKinematicsPlugin",
335 base_class_type=
"kinematics::KinematicsBase",
337 desc = etree.SubElement(cl,
"description")
339 "IKFast{template} plugin for closed-form kinematics of {robot} {group}".format(
340 template=args.template_version,
341 robot=args.robot_name,
342 group=args.planning_group_name,
347 plugin_file_name = ik_library_name +
"_description.xml"
348 plugin_file_path = args.ikfast_plugin_pkg_path +
"/" + plugin_file_name
349 etree.ElementTree(plugin_def).write(
350 plugin_file_path, xml_declaration=
True, pretty_print=
True, encoding=
"UTF-8"
352 print(
"Created plugin definition at '%s'" % plugin_file_path)
355 replacements.update(dict(_LIBRARY_NAME_=ik_library_name))
357 template_dir +
"/CMakeLists.txt",
358 args.ikfast_plugin_pkg_path +
"/CMakeLists.txt",
364 parser = etree.XMLParser(remove_blank_text=
True)
365 package_file_name = args.ikfast_plugin_pkg_path +
"/package.xml"
366 package_xml = etree.parse(package_file_name, parser).getroot()
377 run_deps = [
"liblapack-dev",
"moveit_core",
"pluginlib",
"rclcpp"]
379 update_deps(build_deps,
"build_depend", package_xml)
383 new_export = etree.Element(
"moveit_core", plugin=
"${prefix}/" + plugin_file_name)
385 export_element = package_xml.find(
"export")
386 if export_element
is None:
387 export_element = etree.SubElement(package_xml,
"export")
390 for el
in export_element.findall(
"moveit_core"):
391 found = etree.tostring(new_export) == etree.tostring(el)
396 export_element.append(new_export)
400 etree.ElementTree(package_xml).write(
401 package_file_name, xml_declaration=
True, pretty_print=
True, encoding=
"UTF-8"
403 print(
"Wrote package.xml at '%s'" % package_file_name)
406 easy_script_file_path = args.ikfast_plugin_pkg_path +
"/update_ikfast_plugin.sh"
407 with open(easy_script_file_path,
"w")
as f:
415 +
"robot_name_in_srdf="
416 + args.robot_name_in_srdf
418 +
"moveit_config_pkg="
419 + args.moveit_config_pkg
424 +
"planning_group_name="
425 + args.planning_group_name
427 +
"ikfast_plugin_pkg="
428 + args.ikfast_plugin_pkg
431 + args.base_link_name
436 +
"ikfast_output_path="
437 + args.ikfast_output_path
439 +
"rosrun moveit_kinematics create_ikfast_moveit_plugin.py\\\n"
440 +
" --search_mode=$search_mode\\\n"
441 +
" --srdf_filename=$srdf_filename\\\n"
442 +
" --robot_name_in_srdf=$robot_name_in_srdf\\\n"
443 +
" --moveit_config_pkg=$moveit_config_pkg\\\n"
445 +
" $planning_group_name\\\n"
446 +
" $ikfast_plugin_pkg\\\n"
447 +
" $base_link_name\\\n"
448 +
" $eef_link_name\\\n"
449 +
" $ikfast_output_path\n"
452 print(
"Created update plugin script at '%s'" % easy_script_file_path)
457 moveit_config_pkg_path = get_package_share_directory(args.moveit_config_pkg)
458 except PackageNotFoundError:
459 raise Exception(
"Failed to find package: " + args.moveit_config_pkg)
462 srdf_file_name = moveit_config_pkg_path +
"/config/" + args.srdf_filename
463 srdf = etree.parse(srdf_file_name).getroot()
465 raise Exception(
"Failed to find SRDF file: " + srdf_file_name)
466 except etree.XMLSyntaxError
as err:
468 "Failed to parse xml in file: %s\n%s" % (srdf_file_name, err.msg)
471 if args.robot_name_in_srdf != srdf.get(
"name"):
473 "Robot name in srdf ('%s') doesn't match expected name ('%s')"
474 % (srdf.get(
"name"), args.robot_name_in_srdf)
477 groups = srdf.findall(
"group")
479 raise Exception(
"No planning groups are defined in the SRDF")
481 planning_group =
None
483 if group.get(
"name").lower() == args.planning_group_name.lower():
484 planning_group = group
486 if planning_group
is None:
488 "Planning group '%s' not defined in the SRDF. Available groups: \n%s"
490 args.planning_group_name,
491 ", ".join([group_name.get(
"name")
for group_name
in groups]),
496 kin_yaml_file_name = moveit_config_pkg_path +
"/config/kinematics.yaml"
497 with open(kin_yaml_file_name,
"r")
as f:
498 kin_yaml_data = yaml.safe_load(f)
500 kin_yaml_data[args.planning_group_name][
"kinematics_solver"] = args.plugin_name
501 with open(kin_yaml_file_name,
"w")
as f:
502 yaml.dump(kin_yaml_data, f, default_flow_style=
False)
504 print(
"Modified kinematics.yaml at '%s'" % kin_yaml_file_name)
507def copy_file(src_path, dest_path, description, replacements=None):
508 if not os.path.exists(src_path):
509 raise Exception(
"Can't find %s at '%s'" % (description, src_path))
511 if replacements
is None:
512 replacements = dict()
514 with open(src_path,
"r")
as f:
518 for key, value
in replacements.items():
519 content = re.sub(key, value, content)
521 with open(dest_path,
"w")
as f:
523 print(
"Created %s at '%s'" % (description, dest_path))