Writing an Inverse Kinematics Solver for ROS

Roboticists generally baulk at the name of an Inverse Kinematics Solver. And I have never understood that.

Inverse kinematics is the transformation from the coordinate space to the joint space. Let us break that down.

You want your arm to reach a particular point in space at a certain angle or orientation. This is Coordinate Space. So you need to know what all angles should all your joints be at such that your end-effector is at the desired point and orientation. This is Joint Space. This mapping between coordinate points and joint angles constitutes Inverse Kinematics.

ROS provides some different IK solvers for setting up with your arm. But as with ros_control, not much info is provided on this front. Orocos-KDL is the default solver used when setting up your arm with MoveIt! But be warned: it does not work if your arm has <6 DOFs.

Orocos KDL Components

In fact I wasn’t aware of this until I had set up my entire hardware interface, configured my controllers and was about to start upon motion planning.

The other alternatives that ROS provides for <6 DOF arms is IKFast and Trac-IK. Documentation and support for Trac-IK was too sparse for me to have made an attempt to use it. Though there were a couple of good IKFast tutorials available on ROS-Wiki, I couldn’t get it to work. IKFast was more of a black box where you had to follow the steps to generate the solver configuration for your arm rather than understand it. I had no clue what went behind the scenes. And as has my experience with CFD and FEA taught me, black boxes do not work with me.

An interesting point to note here is that Orocos-KDL and Trak-IK are numerical solvers while IKFast is an analytical one. What it means that former ones iterate and arrive at a solution while the latter has a clearly defined set of equations which give the solution. Naturally analytical solvers are much faster than numerical ones.

So it fell upon to me write my own IK Solver. And as with all things, ROS provides the flexibility to do that. This is how you do it:

1. The motion planning framework MoveIt! provides a moveit_core::KinematicsBase class to build your solver around.
2. Your solver will be a MoveIt! plugin and so you will have to create a plugin.xml file in your catkin package.
3. All the heavy lifting of the solver is done by a single function InverseKinematics::ik which outputs the KDL::JntArray solution and takes a KDL::Frame for an input. Take a look at my GitHub repository to get a better idea.
4. To calculate the analytical solution, consult robotic manipulator texts and books. You should be able to find one similar to your arm. If you don’t, do it yourself. Try working backwards from the most distal link inwards. Trust me it is plain old geometry.
5. Now create a MoveIt! configuration where you should be able to choose the plugin you just created as your IK solver. Else just modify the kinematics.yaml in your MoveIt! package.
6. Create a gtests config file where you can test your solutions. Alternatively MoveIt! provides a compute_fk and compute_ik service. Use compute_fk to generate a moveit_msgs::Pose and feed it back through compute_ik to see if you get the same joint angles.

Trust me, writing your custom IK solver is much easier than what people tell you to be.

If you’re stuck somewhere shoot me a mail at rohin@nymble.in

Cheers!

Rohin