Inverse Kinematics

To be honest, we normally don't want our robot to tell us where it is - we want to tell where it should be. How does we achieve this? Imagine you have a GPS device and want to move to 22°13'27.8"N 22°06'55.7"E. The GPS device only tells you your position, just the coordinates, no map. You are for sure able to get to the goal position by moving in a random direction and comparing the coordinates on your device with the goal coordinates, but this would be quite inefficient. If we tell our robot to move to the goal pose (0.5, 0.1) with a rotation of 0.75, it could start moving its motors in a random direction and compare how much the current pose of the end-effector differs from the goal pose.

Try it out yourself! Can you move the end-effector to (0,0) with a rotation of 0? The current pose is displayed at the bottom of the simulation.

However, it would be much better if we could just compute the configuration - the joint angles - the robot will have at the goal pose and move the joints straight to this configuration.

In other words, we try to find a formula which takes a pose as an input and outputs the configuration q of the robot. In forwards kinematics we developed a formula which takes the configuration as an input and outputs the pose. We therefore call this problem - you might have guessed it - inverse kinematics or IK for short.

Unfortunately there is no one size fits all solution. Depending on the robot, the complexity of this problem varies significantly and there are different strategies to solve it. Two major approaches are algebraic and geometric inverse kinematics.

Algebraic Inverse Kinematics

For algebraic inverse kinematics, an equation system is set up and solved. For our planar robot we could use the three equations for the forwards kinematics and solve them for q0q_0, q1q_1 and q2q_2. These equation are

ϕO,ee=q0+q1+q2\phi_{O,ee} = q_0+q_1+q_2

xO,ee=l0cos(q0)+l1cos(q0+q1)+l2cos(q0+q1+q2)x_{O,ee} = l_0 \cdot cos(q_0) + l_1 \cdot cos(q_0+q_1) + l_2 \cdot cos(q_0+q_1+q_2)

yO,ee=l0sin(q0)+l1sin(q0+q1)+l2sin(q0+q1+q2)y_{O,ee} = l_0 \cdot sin(q_0) + l_1 \cdot sin(q_0+q_1) + l_2 \cdot sin(q_0+q_1+q_2)

where l0l_0, l1l_1 and l2l_2 are the lengths of the links. With a good mathematical software solving these equations would be a matter of plugging in the formulas and waiting for a solution. However, in this article I want to show you a goemetric approach.

Geometric Inverse Kinematics

As the name of this approach suggests, we are going to use a lot of trigonometry. TO be prepared for the next sections, it is helpful to have the definitions of sine, cosine and tangent in mind. We also use the Pythagorean theorem and the law of cosines.

In the following image you see our robot with an arbitrary pose. Given x, y and ϕ\phi, we want to compute the three joint angles q0q_0, q1q_1 and q2q_2. I marked four triangles in the image, a green, red, yellow, and blue one. Let's look what we know about these triangles.


Green triangle

We already know a lot about the green triangle. It has a right angle at the bottom right, the bottom left is equal to ϕ\phi, its hypotenuse is equal to the length of the end-effector l2l_2 and the corner at the top has the position (x,y). With this information we can compute the position of p2p_2, the center of the joint connected to the end-effector. With the definition of sine and cosine, we can compute the length of the bottom side of the rectangle with l2sin(ϕ)l_2 \cdot sin(\phi) and the right side of the triangle with l2cos(ϕ)l_2 \cdot cos(\phi). The point p2p_2 is therefore defined as

p2=[xy]=[xl2cos(ϕ)yl2sin(ϕ)]p_2 = \begin{bmatrix} x' \\ y' \end{bmatrix} = \begin{bmatrix} x - l_2 \cdot cos(\phi) \\ y - l_2 \cdot sin(\phi) \end{bmatrix}

Red triangle

The red triangle has a right angle at the bottom right corner. We can therefore use the Pythagorean theorem to compute the hypotenuse r by using the x- and y- coordinate of p2p_2.

r2=x2+y2r^2 = x'^2 + y'^2

Furthermore, we can compute the the angle at the bottom left corner with atan(xy)atan\Big(\frac{x'}{y'}\Big)

Blue triangle

Now things get more complicated! The blue triangle has no right angle. We use thefore the Law of cosines a2=b2+c22bccosαa^2=b^2+c^2-2bc\cos\alpha where a, b and c are the sides of the triangle and α\alpha is the angle between the sides b and c. In our triangle, a=ra = r, b=l0b = l_0, c=l1c = l_1 and α=πq1\alpha = \pi - q_1. By rearranging the formula and plugging in the equation for r2r^2 from the red triangle, we are able to compute the angle of the second joint q1q_1.

q1=acos(x2+y2l02l122l0l1)q_1 = acos\Big(\frac{x'^2 + y'^2 - l_0^2 - l_1^2}{2l_0l_1}\Big)

Yellow triangle

The yellow triangle has a right angle at the top. We therefore know, that the side right to the top angle has the length l1cos(q1)l_1 \cdot cos(q_1) and the side left to the top angle has the length l1sin(q1)l_1 \cdot sin(q_1)

Blue-yellow triangle

The angle of the first joint q0q_0 is the sum of the bottom left angles of the blue and the red triangle. We already computed this angle for the red triangle. The angle for the blue triangle can be computed by looking at the triangle consisting of the blue and the yellow triangle. It has a right angle at the top. The angle at the bottom left is therefore atan(l1sin(q1)l0+l1cos(q1))-atan\Big(\frac{l_1 \cdot sin(q_1)}{l_0 + l_1 \cdot cos(q_1)}\Big). We can compute q0q_0 with

q0=atan(xy)atan(l1sin(q1)l0+l1cos(q1))q_0 = atan\Big(\frac{x'}{y'}\Big) - atan\Big(\frac{l_1 \cdot sin(q_1)}{l_0 + l_1 \cdot cos(q_1)}\Big)

The only angle missing is now q2q_2. We can compute it with q2=ϕq1q0q_2 = \phi - q_1 - q_0.

Seeing it in action

In the interactive demo below, you can see the inverse kinematics in action. With the sliders on top you can modify x, y and ϕ\phi. At the bottom you see the values for the three joint angles. Whenever the values for x, y or ϕ\phi are changed, the configuration is computed with the formulas we just derived. The configuration is then used to draw the robot.