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 answers.ros.org. 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 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.

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