Limit of KDL Solver

The ROS MoveIt! KDL solver is only suitable for use with 6 or more DoF kinematic chains. This is an issue that normally would make IK on the PaBiLegs impossible, as this model only has 2 DoF.

To overcome these limitations, MoveIt! allows to use another solver, the so called IKFast solver, which is capable of solving IK equations analytically of any kinematic chain. It even needs less time for the same problem as KDL. IKFast is provided by Rosen Diankov's OpenRAVE (Open Robotics Automation Virtual Environment).

Install IKFast

OpenRAVE Workstation

OpenRave for Ubuntu 16.04 Xenial can be downloaded as a robotic workstation by following this tutorial. Unfortunately many compiling errors occured during all of my attempts. Using the workstation with a graphical visualization by OpenSceneGraph, as it is described in the tutorial, may however not be necessary for this project, as ROS MoveIt! offers an own solution.

MoveIt! IKFast

MoveIt! IKFast is the name of a MoveIt! tool that helps to generate a IKFast Plugin, that can replace solvers like the KDL solver. There are two ways to install this tool for ROS kinetic.

Binary Install:

sudo apt-get install ros-kinetic-moveit-kinematics

Source (inside your catkin workspace):

git clone https://github.com/ros-planning/moveit.git

Create IKFast Plugin

Is MoveIt! IKFast successfully installed you can start generating your own IKFast solver plugin. In general there are some steps, that have to be done every time you do this for your robot model. First of all you need to clarify about these descriptions of your robot:

  • robot name (<robotname>): This is the name of your robot, as it is clarified in your URDF.
  • planning group name (<pl>): The planning group
  • IKFast output path (<ikpath>): The path to the C++ file, that will be generated by IKFast and contain the analytical calculations to solve inverse kinematics.
  • MoveIt! IKFast plugin package (<ikpkg>): The name of your final IKFast Plugin-
  • MoveIt! robot config (<robot_config>): The name of the config package you created for your robot

IKFast Plugin Creation Step by Step:

1.) Create a working robot URDF file, as described in the "Robot Models" chapter

2.) Load this URDF in the MoveIt! setup_assisant by running roslaunch moveit_setup_assistant setup_assistant.launch

3.) (OPTIONAL - if you haven't done it before) Create a new working MoveiIt! Config plugin. Remember the name of planning group and in the last step "Configuration files" chose the folder of your workspace as output path. Now edit this path by adding /<robotname>_moveit_config. This will be the name of your config package. From now on, we will reference <robotname>_moveit_config as <robot_config>. Execute catkin_make to build the package.

4.) Navigate into the folder, where the URDF of your robot is saved. Open your terminal here and execute:

rosrun collada_urdf urdf_to_collada <robotname>.urdf <robotname>.dae

This will generate an .dae file out of the URDF

5.) (OPTIONAL - if there any floating point issues) Execute:

rosrun moveit_kinematics round_collada_numbers.py <robotname>.dae <robotname>_rounded.dae 5

6.) Execute

openrave-robot.py <robotname>.dae --info links

This will list you the links of your robot model. We need the index of the links, that start and end your planning group. Remember them for the next step.

7.) Edit the command

python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=<robotname>.dae --iktype=transform6d --baselink=1 --eelink=8 --savefile=<ikpath>

so that it fits your robot model.

  • iktype is the type of kinematic you can chose. A list with all possible tpyes is here
  • baselink is the index of the starting link for your planning group
  • eelink is the end effector link of the planning group
  • savefile is the path to your IKFast C++ file. In general chose any path. Go there first and create an empty .cpp file. IMPORTANT: Give it the name of your planning group <pl>. Now your <ikpath> could look like this: /home/offi/catkin_ws/src/roboy_ik/src/<pl>.cpp It is important to also include the file into the path.

7.) Execute the command from bulletpoint 7.)

8.) Create the package, where the IKfast solver will be put in.

cd ~/catkin_ws/src
catkin_create_pkg <ikpkg>

has to be named like this: ikfast_plugin. Continue with:

cd ~/catkin_ws
catkin_make

9.) Execute

rosrun moveit_kinematics create_ikfast_moveit_plugin.py <robotname> <pl> <ikpkg> <ikpath>

10.) Add the line set(CMAKE_CXX_STANDARD 11) to your CMakeLists.txt of the <ikpkg> package and finally build your workspace again to create the ik plugin:

cd ~/catkin_ws
catkin_make

Use IKFast

If everything was successfully built, you can test the plugin. First, ensure that the plugin has been set as the default IK solver for your robot. Do this by running rosed <robotname>_moveit_config/config/kinematics.yaml or navigate manually to this file.

Edit these parts, if not already done:

<planning_group_name>:
  kinematics_solver: <robotname>_<pl>_kinematics/IKFastKinematicsPlugin
-INSTEAD OF-
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin

Now you can test your IK Plugin by running the Demo.launch of your <robot_config> package. Do this by executing roslaunch <robot_config> demo_launch. Move the endeffector with the markers or sample a random valid position. Check wheather the planning is successful.

Sources:

Facing any issues / compiling errors I can recommend reading the official MoveIt! IKFast tutorial, which includes more detailed information