Inverse Kinematics for 2DOF Arm

When I first came across the problem of inverse kinematics I thought - quite naively - that it would be a simple matter to find a solution to the problem because the forward kinematics problem was so simple to solve. I decided to start out with a 2 link arm and see if I could work out the solution on my own from scratch without having any initial knowledge about inverse kinematics. After filling pages and pages with calculations that didn't quite seem to be going anywhere I realized why it was such a difficult problem. 

Of course a 2 link arm had a complete close form solution. After a bit of online research I found the solution. However, the solution for arms with more than 2 degrees of freedom turns out to be not so simple. It requires using numerical methods to slowly converge on a solution. This approach doesn't solve everything either. There's still the problem of choosing the right set of angles because there are often multiple solutions for a particular location of the end of the arm and some of these solutions are invalid because they are outside the range of motion of the motors at the joints of the arm.

I've decided to slowly familiarize myself with all the different ways of finding inverse kinematics solutions. I think I'll make blogposts each with a higher degree of freedom and eventually I'll talk about the solution for the 6DOF arm. Why stop at 6? Because it turns out that in 3D space, you don't need more than 6DOF to reach a particular location/orientation. Any more than that and your arm becomes redundant. 3DOF for location in space plus 3DOF for orientation in space are all we need from an arm.

So in this post I'll talk about the solution to 2 link robotic arm. And this arm will have hinge type joints only. I think that the technical term used is "revolute joint".

A 2 link planar robotic arm looks like this:

The forward kinematics equations for this arrangement are simple enough.
$x = l_1 cos(\theta_1) + l_2 cos(\theta_1 + \theta_2)$
$y = l_1 sin(\theta_1) + l_2 sin(\theta_1 + \theta_2)$

The inverse kinematics equations are quite monstrous. The full derivation is given in this fantastic pdf. I'll just write down the final equations here.

Given a value of x and y, the inverse kinematics equations are:
Let
$k_1 = l_1 + l_2 cos(\theta_2)$
$k_2 = l_2 sin(\theta_2)$
$\gamma = atan2(k_2, k_1)$

$\theta_2 = atan2(\sqrt{1 - (\frac{x^2 + y^2 - l_1^2 - l_2^2}{2l_1 l_2})^2}, \frac{x^2 + y^2 - l_1^2 - l_2^2}{2l_1 l_2})$
$\theta_1 = atan2(y, x) - \gamma$

I wrote a simple python script to calculate the values of theta. You can get the code here. The invkin2() function calculates the values of $\theta_1$ and $\theta_2$ for an arm with 2 links.

It was a bit difficult to test the code this way though. Can't see if I'm right or wrong in most cases. So I wrote this processing sketch to visualize the calculations so I can easily verify their correctness.

You can view the processing simulation here: http://rationalash.github.io/invkin/