Easy analytical Inverse Kinematics in ROS

One of the most common ways to calculate the inverse kinematics in ROS is through the KDL package. The ‘problem’ with this kind of solvers is that they are numeric, which means they will perform numerical optimization iteratively until a solution (or none) is found. An alternative approach would be to calculate the analytical expression of the Inverse Kinematics. Having a closed form solution means that you’ll be certain to always obtain the exact solutions if they exist. The downside is that the process of obtaining the analytical expression for large kinematic links is complex. IKFast is a script that takes in a URDF and directly outputs library agnostic C++ code that computes the Inverse Kinematics of a specified link.

IKFast is part of the OpenRAVE project, so to use it you’ll have to install it first. OpenRAVE is a robotics framework for motion planning, but it seems to have been abandoned. The ubuntu repository is not working at the time of writing, so the only choice is to install from source. To do so, you must follow these steps (for Ubuntu 16.04):

Download the source code:

$ git clone --branch latest_stable https://github.com/rdiankov/openrave.git

Install required dependencies:

$ sudo apt install libboost-python-dev python-dev python-sympy ipython \
                   python-numpy python-scipy python-pip \
                   libassimp-dev assimp-utils python-pyassimp \
                   libsoqt4-dev

Collada is also required, but it is not on the official repositories, so you’ll have to build it from source:

$ git clone https://github.com/rdiankov/collada-dom.git
$ cd collada-dom && mkdir build && cd build
$ cmake .. && make -j && sudo make install

Then move to the OpenRAVE source’s folder and edit the CMakeLists.txt to disable OPT_MATLAB and OPT_OCTAVE:

option(OPT_OCTAVE "Build octave bindings" OFF)
option(OPT_MATLAB "Build matlab bindings" OFF)

Then, as usual:

$ mkdir build && cd build
$ cmake .. && make && sudo make install

If everything went well, you should be able to open a 3D viewer by keying openrave in a new shell session.

The next step is to transform our existing urdf robot representation to the COLLADA format, since this is the way OpenRAVE works internally. To do so, we will use the convenient collada_urdf ROS nose:

$ roscore && rosun collada_urdf urdf_to_collada robot.urdf robot.dae

In principle, you should be able to visualize your COLLADA robot by opening it with:

$ openrave robot.dae

In my case, no robot was shown but a mesh of triangles. So I opened the file in Blender and it loaded fine.

To check the links or joints of your robot, you can run the following commands:

$ openrave-robot.py robot.dae --info joints
$ openrave-robot.py robot.dae --info links

This is useful to check for the IDs of each link/joint, since you will need to know them to specify the kinematic chain on which to compute the inverse kinematics.

Once you know the IDs of the links that form your kinematic chain, execute:

$ python $(openrave-config --python-dir)/openravepy/_openravepy_/ikfast.py \
    --robot=robot.dae --iktype=transform6d --baselink=0 --eelink=7 --savefile=ik.cc

This will generate a pretty ugly source file with all the analytical inverse kinematics solutions. But it needs ikfast.h in order to compile. You can copy it to your current folder with this command:

$ cp /usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.h .

Finally, to compile:

$ g++ ik.cc -o ik

This will generate an executable since ik.cc comes with a main function by default. You can play with some of the defines in the ikfast.h to get what you want (standalone program, static or dynamic library, …).

For my use case, I needed to have two simultaneous and different kinematic chains (for the two legs of a biped robot). Since having the same ikfast.h results in name collisions for more than two kinematic chains, I decided to duplicate the file in ikfast_left.h and ikfast_right.h and put everything within a unique namespace. Also, I enabled these defines: IKFAST_NO_MAIN and IKFAST_HAS_LIBRARY. I also removed the default namespaces. There may have been a better way for solving this but I needed to get things done.

In my case, there were 7 solutions for my 6-DoF legs. I chose the one that made most sense (which turned out to be the third solution for both legs) and after some boilerplate code later, here is the result (github here):

My poor robot is lacking some limbs, but that is an issue for another time.