About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

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 create_ikfast_plugin.py <robot_name>
     - OR -
   $ python /path/to/create_ikfast_plugin.py <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>_kinematics_lib.so.

Usage

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/create_launch_files.py <your_robot_name>

Launch the warehouse viewer:

$ roslaunch <your_robot_name>_arm_navigation
planning_scene_warehouse_viewer_<your_robot_name>.launch

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