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
Contents
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:
- Create new directories:
$ roscd my_robot_arm_navigation $ mkdir include src
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.
Copy the IKFast header file (if available) to the new include directory:
$ cp <openravepy>/ikfast.h include/ikfast.h
- 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.
- 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.