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
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
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
$ 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
$ 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_right.h and put
everything within a unique
namespace. Also, I enabled these defines:
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.