How to use IKFast for new robot type?

[fishbotics]

Hi all,

I'm just getting started using Tesseract and I'd like to be able to use it for the Franka Panda. In the past, I've used IKFast (because, well, it's fast), but I had to generate a new C++ file using OpenRave. In the ikfast directory, I don't see any robot-specific info. Can someone point me to how I might be able to integrate a new robot type into the IK solver?

Thanks!

[Levi-Armstrong]

Take a look here at the following files.

• abb_irb2400_ikfast_kinematics.h
• abb_irb2400_ikfast_kinematics.cpp
• abb_irb2400_ikfast_solver.hpp.

Also there is iiwa7 example in the same directory.

[Levi-Armstrong]

After you create the kinematics object you will need to create the factory which is the plugin to tesseract.

[fishbotics]

Thanks @Levi-Armstrong. I might be misunderstanding here, but that factory you linked is for KDL. I see in here that you list all the plugins. The iiwa seems to point to KDL--not IKFast and abb points to OPW, which I think is defined here.

Is the OPW IK library your preferred standard? I'm used to using IKFast, but I'd be happy to set up something else if it's better. Or, do you have an example of an IKFast factory?

[gavanderhoorn]

If possible, try to use OPW.

The 'only' thing you'd need is the parameters. The plugin code is already written, and it's both faster and much more compact than IKFast.

Whether the Panda can be modelled as an OPW-type robot I don't know.

[fishbotics]

It looks pretty similar to the OPW model, but I don't know how to get all the parameters. Maybe I can load it in sim and figure out how to take the measurements.

[Levi-Armstrong]

Thanks @Levi-Armstrong. I might be misunderstanding here, but that factory you linked is for KDL. I see in here that you list all the plugins. The iiwa seems to point to KDL--not IKFast and abb points to OPW, which I think is defined here.

Is the OPW IK library your preferred standard? I'm used to using IKFast, but I'd be happy to set up something else if it's better. Or, do you have an example of an IKFast factory?

@fishbotics Here are the ikfast examples for the iiwa

https://github.com/tesseract-robotics/tesseract/blob/master/tesseract_kinematics/test/iiwa7_ikfast_kinematics.h
https://github.com/tesseract-robotics/tesseract/blob/master/tesseract_kinematics/test/iiwa7_ikfast_kinematics.cpp

Generated from ikfast
https://github.com/tesseract-robotics/tesseract/blob/master/tesseract_kinematics/test/iiwa7_ikfast_solver.hpp

[marip8]

OPW kinematics can only be used with a very specific robot configuration, and I don't think this particular robot meets the criteria. A few general requirements are that the robot must be 6 DOF and have a spherical wrist (i.e. the last three axes all intersect at a single point). Here is a picture of a valid OPW configuration with the dimensions denoted. If your robot doesn't have an exact correlation to this diagram, then OPW can't be used

[marip8]

I'm coincidentally also trying to implement a custom IKFast plugin currently, and it seems like the functionality for loading a custom plugin is fairly limited in one case and broken in another.

Case 1: YAML config file (limited capability)

You can specify the names and locations of plugins in the plugin config file, such as this example. However, I believe these names have to be hard-coded, so they wouldn't transfer between machines unless the plugins were installed to a universal directory (which is not the case for most ROS users building in a workspace in their home directory).

Case 2: Environment variables (broken)

You can also specify the names and locations of plugins using two (fairly hidden) environment variables. A user could use a script or ROS launch file to populate these environment variables in a generic way that is machine independent.

However, the plugin loader does not attempt to load plugins that it finds from environment variables. Instead it only loads libraries from its internal cache (see below that plugins_local never really gets used). I think the fix for this is simple: iterate over plugins_local rather than this->search_libraries.

tesseract/tesseract_common/include/tesseract_common/plugin_loader.hpp

Lines 133 to 162 in eebe3fd

	// Check for environment variable for plugin definitions
	std::set<std::string> plugins_local = getAllSearchLibraries(search_libraries_env, search_libraries);
	if (plugins_local.empty())
	{
	CONSOLE_BRIDGE_logError("No plugin libraries were provided!");
	return false;
	}
	// Check for environment variable to override default library
	std::set<std::string> search_paths_local = getAllSearchPaths(search_paths_env, search_paths);
	for (const auto& path : search_paths_local)
	{
	for (const auto& library : search_libraries)
	{
	if (ClassLoader::isClassAvailable(plugin_name, library, path))
	return true;
	}
	}
	// If not found in any of the provided search paths then search system folders if allowed
	if (search_system_folders)
	{
	for (const auto& library : search_libraries)
	{
	if (ClassLoader::isClassAvailable(plugin_name, library))
	return true;
	}
	}
	return false;

Are there other ways of loading custom plugins, or do we need to fix these methods?

Also the kinematic plugin unit tests don't test this loading capability because the plugin loader already knows about the plugins built by tesseract through the TESSERACT_KINEMATICS_PLUGINS compiler definition.

[marip8]

I guess there is a third case which is to use some combination of both other methods, specifically setting the search path in the environment variable and adding the plugin library name to the YAML config file. We still should address the shortcomings of the other approaches too though

[Levi-Armstrong]

Case 1: YAML config file (limited capability)

You can specify the names and locations of plugins in the plugin config file, such as this example. However, I believe these names have to be hard-coded, so they wouldn't transfer between machines unless the plugins were installed to a universal directory (which is not the case for most ROS users building in a workspace in their home directory).

I don't think that is the case. It first searches the directories but then reverts to system location and what is in your environment variables. I usually don't add anything here and as long as the terminal is sourced it will find them.

Update:
To clarify, you do need to add the library to the list but really do not need to add a search path.

[Levi-Armstrong]

Case 2: Environment variables (broken)

You can also specify the names and locations of plugins using two (fairly hidden) environment variables. A user could use a script or ROS launch file to populate these environment variables in a generic way that is machine independent.

However, the plugin loader does not attempt to load plugins that it finds from environment variables. Instead it only loads libraries from its internal cache (see below that plugins_local never really gets used). I think the fix for this is simple: iterate over plugins_local rather than this->search_libraries.

The environment variable was added to support containerization like using snap. Other than that it should really never need to be used because it searches the system location for the library.

[Levi-Armstrong]

However, the plugin loader does not attempt to load plugins that it finds from environment variables. Instead it only loads libraries from its internal cache (see below that plugins_local never really gets used). I think the fix for this is simple: iterate over plugins_local rather than this->search_libraries.

This piece may very well be broke and will need to look at it more closely.

[Levi-Armstrong]

@marip8 Did you run into a case where it was unable to find the plugin?

[marip8]

Case 1: YAML config file (limited capability)

You can specify the names and locations of plugins in the plugin config file, such as this example. However, I believe these names have to be hard-coded, so they wouldn't transfer between machines unless the plugins were installed to a universal directory (which is not the case for most ROS users building in a workspace in their home directory).

I don't think that is the case. It first searches the directories but then reverts to system location and what is in your environment variables. I usually don't add anything here and as long as the terminal is sourced it will find them.

Update: To clarify, you do need to add the library to the list but really do not need to add a search path.

For some reason this doesn't seem to work on a ROS2 system after I have sourced my workspace and added the name of the library to the config file. It shows a search path in my tools workspace where tesseract was built (which I think actually comes from the target compile definition from tesseract_kinematics?), but the current workspace where the custom plugin is built does not have a known search path

snp_motion_planning_node-6] Warning: Failed to load symbol 'MotomanHC10InvKinFactory'
[snp_motion_planning_node-6]          at line 310 in /home/mripperger/ros/snpd/snpd_tools_ws/src/tesseract/tesseract_kinematics/core/src/kinematics_plugin_factory.cpp
[snp_motion_planning_node-6] Error:   Failed to instantiate plugin 'MotomanHC10InvKinFactory', Details: 
[snp_motion_planning_node-6] Search Paths (Search System Folders: True):
[snp_motion_planning_node-6]     - /home/mripperger/ros/snpd/snpd_tools_ws/install/tesseract_kinematics/lib
[snp_motion_planning_node-6] Search Libraries:
[snp_motion_planning_node-6]     - libsnp_motion_planning_plugins.so
[snp_motion_planning_node-6]     - libtesseract_kinematics_core_factories.so
[snp_motion_planning_node-6]     - libtesseract_kinematics_kdl_factories.so
[snp_motion_planning_node-6]     - libtesseract_kinematics_opw_factory.so
[snp_motion_planning_node-6]     - libtesseract_kinematics_ur_factory.so

[Levi-Armstrong]

Is libsnp_motion_planning_plugins.so location in your LD_LIBRARY_PATH environment variable?

[Levi-Armstrong]

I also think it would be good to update the parsing of the search path to use the resource locator if possible so you may use a package url.

[Levi-Armstrong]

Here is the documentation for what dlopen searches.

[Levi-Armstrong]

@marip8 Is this library by chance in a pure cmake package?

[marip8]

Is this library by chance in a pure cmake package?

No, it's in a ROS2 package.

Is libsnp_motion_planning_plugins.so location in your LD_LIBRARY_PATH environment variable?

Actually no. Turns out it was a few ROS2 errors. I forgot to add an ament_export_targets command, so the package location wasn't added to the LD_LIBRARY_PATH. I also didn't realize that ROS2 libraries need to be installed in lib not lib/<package_name>. Once I fixed those, it works as expected

[Levi-Armstrong]

@fishbotics Were you able to integrate your ikfast?

[fishbotics]

@Levi-Armstrong , sorry it's taken me so long to get back to you on this. I was crunching for an unrelated deadline and had to put everything else aside for a bit.

I'm just getting back to this now and will be working on this for the next few days. I'll update here as I work on getting it figured out.

[marip8]

@Levi-Armstrong would you be interested in adding this IKFast factory as a boilerplate file in tesseract_planning? It should be generically applicable to all IKFast plugins (minus the HC10 include and plugin export). Users could then make their own tesseract IKFast kinematics plugins in 3 lines in a .cpp file.

#include <tesseract_kinematics/ikfast/ikfast_factory_boilerplate.hpp>  // proposed addition to tesseract_kinematics
#include "custom_ikfast_generated_code.hpp"
TESSERACT_ADD_PLUGIN(tesseract_kinematics::IKFastInvKinFactory, MyCustomIKFastInvKinFactory)

[Levi-Armstrong]

Sounds great to me.

[rjoomen]

Implemented and submitted as #916.