Kinematics/Custom Plugin

In this section, you will learn how to integrate your own custom kinematics solver with MoveIt! The process is simple:
 * Wrap your kinematics solver using the abstract KinematicsBase class that provides a common API for kinematics solvers.
 * Define a plugin file to export your kinematics solver so that MoveIt! can find it (through the ROS plugin system).

Note: If you would like to integrate IKFast as an IK plugin for your robot, see these instructions.

STEP 1: The KinematicsBase class
The [/classkinematics_1_1KinematicsBase.html KinematicsBase] class is an abstract class that every kinematics solver will need to inherit from and implement. An example implementation for the PR2 can be found in the moveit_pr2 package. You will need to implement all the pure virtual functions after inheriting from the base class. Your first step should be to create a new package, e.g. example_kinematics that depends on moveit_core (since that is where the base class is defined). Put all your code into that package.

Note that the initialize function for the class takes, among other arguments, a robot_description string argument that you can use to specify the name of the robot or the name of the parameter on the ROS param server corresponding to your robot.

STEP 2: Setup the plugin description file
In a file named example_plugins.xml in your example_kinematics package, add the following lines and modify them to match the names you have chosen for your components. This file needs to live right next to your package.xml;


 * Replace lib/libmoveit_kdl_kinematics_plugin with the name of the library object that contains your implementation of the kinematics. E.g. if you named your library example_kinematics, the first line above should read:
 * Rename the class name to be namespace/class_name. e.g. if your code is in the namespace example_kinematics and you named your class ExampleKinematics, replace the class name with example_kinematics/ExampleKinematicsPlugin.
 * Rename the type to be namespace::class_name. e.g. example_kinematics::ExampleKinematicsPlugin.

Example Plugin File
The example plugin file for the custom kinematics solver for the PR2 robot can be found in the moveit_pr2 github project.

STEP 3: Export the Plugin
Add the following line into your package.xml file. Here, example_kinematics is the name of your new package containing the kinematics implementation while example_plugins.xml is the name of the plugin description file that you just added.

STEP 4: Install the Plugin Description
In your CMakeLists.txt (the one that lives next to package.xml), you should have an install rule for your example_plugins.xml file:

Option 1: Use the Setup Assistant
How can you now get MoveIt! to use the plugin that you just generated? Recall that you created a MoveIt! configuration using the MoveIt! Setup Assistant (see this tutorial for an example with the PR2 robot). One of the steps there is to select the correct kinematics solver for a group. When you run the setup assistant, it will automatically find all available kinematics plugins. You can choose the new plugin that you just setup for your group.

Option 2: Edit the config file
The other option is to modify the corresponding file manually. The file you will need to modify is the kinematics.yaml file inside the config directory of your MoveIt! configuration package. E.g., the file for the PR2 (found here), is below:

You can modify this file manually, and specify the following:
 * kinematics_solver: The name of your kinematics solver plugin. Note that this must match the name that you specified in the plugin description file, e.g. example_kinematics/ExampleKinematicsPlugin
 * kinematics_solver_search_resolution: This specifies the resolution that a solver might use to search over the redundant space for inverse kinematics, e.g. using one of the joints for a 7 DOF arm specified as the redundant joint.
 * kinematics_solver_timeout: This is a default timeout specified (in seconds) for each internal iteration that the inverse kinematics solver may perform. A typical iteration (e.g. for a numerical solver) will consist of a random restart from a seed state followed by a solution cycle (for which this timeout is applicable). The solver may attempt multiple restarts - the default number of restarts is defined by the kinematics_solver_attempts parameter below.
 * kinematics_solver_attempts: The number of random restarts that will be performed on the solver. Each solution cycle after the restart will have a timeout defined by the kinematics_solver_timeout parameter above. In general, it is better to set this timeout low and fail quickly in an individual solution cycle.

Links

 * Back to Kinematics