Inverse Kinematics: Part 2

On to part two of exploring inverse kinematics! This time I think I will add two additional degrees of freedom to the arm and calculate the inverse kinematics. 

$l_1$ - Length of the first link

$l_2$ - Length of the second link

$l_3$ - Length of the third link

$\theta_0$ - The angle of the base drum.

$\theta_1$ - The angle of the first link from the ground

$\theta_2$ - The angle of the second link w.r.t the first

$\theta_3$ - The angle of the third link w.r.t the second

First let's ignore the third link and consider the two links plus the rotating base. This extension is relatively simple to handle. If the value of $\theta_0$ is found then the problem can again be reduced to a planar 2D problem which already has a nice solution.

It's easy to see that:

$\theta_0 = atan2(z, x)$

Once this is found, then we can find the absolute distance in the x-z plane $d = \sqrt{x^2 + z^2}$. Then the inverse kinematics for the two links can be found by replacing $x$ with $d$ in all the equations.

Now it's time to add the third link into the equations. This third link is special. It's not really an additional degree of freedom. Two links and a rotating base are all that's needed to reach any point in 3D space. So why the third link? Imagine that there was a gripper attached to the third link. This gripper grabs a glass of water. If that third link were not there, this gripper would tilt with the angle of the second link and whatever liquid is in the glass would spill all over the place. This third link in the arm will adjust its angle to the second link so that it maintains a constant angle from the ground plane. Now if the arm is holding a glass of water and moving around, the glass will always be pointed in the right direction. It won't tilt and spill its contents all over the place. Let the constant angle between the third link and the ground be $\phi$ degrees. Applying this constraint we get: $\theta_3 = \theta_1 - \theta_2 - \phi$.

So all we need to do is subtract the vector of the third link from the target coordinates, apply the 2-link plus base inverse kinematics on this coordinates and then calculate phi using the value of $\theta_1$ and $\theta_2$ that we get from those calculations.

I've implemented all this in the invkin4() function in this python script.

I'll post a processing simulation of this soon. I'm still fiddling around with the P3D. I haven't got used to it yet.

EDIT: Go here to see a processing simulation of the inverse kinematics script. :D