Note: This tutorial is just one step of the larger Create_a_Fast_IK_Solution tutorial.
(!) Please ask about problems and questions regarding this tutorial on Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

IKFast Plugin for Arm Navigation

Description: Create a plugin that wraps the IKFast solver for use in Arm Navigation.

Tutorial Level: BEGINNER

You should have already created an arm navigation package for your robot, by using the Wizard and following this Tutorial. The package should have a name like 'my_robot_arm_navigation'.

Create plugin

We will create the IKFast plugin inside the arm_navigation package for your robot:

  1. Create new directories:
    $ roscd my_robot_arm_navigation
    $ mkdir include src
  2. Copy the IKFast-generated source file to the newly-created src directory:

    $ cp /path/to/ikfast61-output.cpp src/my_robot_manipulator_ikfast_solver.cpp

    NOTE: The resulting filename must be of the form: <robot_name>_<group_name>_ikfast_solver.cpp, where group_name is the name of the kinematic chain you defined while using the Wizard. For single-arm robots, this should have been named manipulator as per ROS standards.

  3. Copy the IKFast header file (if available) to the new include directory:

    $ cp <openravepy>/ikfast.h include/ikfast.h
  4. Create the plugin source code:
    $ rosrun arm_kinematics_tools <robot_name>
      - OR -
    $ python /path/to/ <robot_name>     (works without ROS)

    This will generate a new source file <robot_name>_<group_name>_ikfast_plugin.cpp in the src/ directory, and modify various configuration files.

  5. Build the plugin library:
    $ rosmake <robot_name>_arm_navigation

This will build the new plugin library lib/lib<robot_name>


The IKFast plugin should function identically to the default KDL IK Solver, but with greatly increased performance. You can switch between the KDL and IKFast solvers using the kinematics_solver parameter in the constraint_aware_kinematics.launch launch file:

<param name="kinematics_solver" type="string" value="MYROBOT_manipulator_kinematics/IKFastKinematicsPlugin"/>
<param name="OLDkinematics_solver" type="string" value="arm_kinematics_constraint_aware/KDLArmKinematicsPlugin"/>

Test the plugin

As a stand-alone kinematics service

Create a new launch file in your <robot_name>_arm_navigation stack, using this code as a template. You should edit the package names to suit your own.

This will launch a fake joint_state_publisher GUI to simulate your robot's joint angles, display the robot model in Rviz, and load an arm_kinematics_constraint_aware node with your new kinematics plugin.

To test forward kinematics, run:

$ roslaunch arm_kinematics_tools get_fk.launch

which will display the pose of the end link e.g.

[ INFO] [1354018122.441239800]: ALink    : Tool_point
[ INFO] [1354018122.441391464]: Position: -0.18908,0.146519,0.211831
[ INFO] [1354018122.441540534]: Orientation:  0.318981 i   0.631071 j   0.694215 k   0.134407

You can move the joint sliders and check the pose in Rviz.

To test inverse kinematics, run:

$ roslaunch arm_kinematics_tools get_ik.launch

which will display the joint angles of the first IK solution that is within all joint limits:

Current pose of Tool_point frame, with respect to Mounting_link: 
Translation:  -0.189080 x   0.146519 y   0.211831 z  
Quaternion:  0.318981 i   0.631071 j   0.694215 k   0.134407 w  

Joint angles:   
RSR_joint -0.659250   RSP_joint 1.492588   RSY_joint -0.000000   REB_joint 0.619524   RWY_joint -0.000000   RWP_joint 1.149826 

From within the arm planning warehouse viewer

You need to generate a launch file for the warehouse viewer, as described in this tutorial:

$ roscd move_arm_warehouse 
$ ./scripts/ <your_robot_name>

Launch the warehouse viewer:

$ roslaunch <your_robot_name>_arm_navigation

and follow the above tutorial to create new motion plans for your manipulator.

Wiki: Industrial/Tutorials/Create_a_Fast_IK_Solution/arm_navigation_plugin (last edited 2013-02-06 08:23:39 by DavidButterworth)