Inverse Kinematics for a 2-Joint Robot Arm Using Geometry

We saw this simple two-link robot in the previous lecture about forward kinematics. The tooltip pose of this robot is described simply by two numbers, the coordinates x and y with respect to the world coordinate frame. So, the problem here is that given x and y, we want to determine the joined angles, Q1 and Q2. The solution that we’re going to follow in this particular section is a geometric one. We’re going to start with a simple piece of construction. We’re going to overlay the red triangle on top of our robot. We know that the end point coordinate is x, y, so the vertical height of the triangle is y, the horizontal width is x. And, using Pythagoras theorem, we can write r squared equals x squared plus y squared. So far, so easy.

Now, we’re going to look at this triangle highlighted here in red and we want to determine the angle alpha. In order to do that, we need to use the cosine rule. And, if you’re a little rusty on the cosine rule, here is a bit of a refresher. We have an arbitrary triangle. We don’t have to have any right angles in it and we’re going to label the length of this edge as A and the angle opposite that edge, we’re going to label as little a. And, we do the same for this edge and this angle, and this edge and this angle. So, all together, the sides are labelled capitals A, B and C, and the angles are labelled little a, little b, and little c. So, the cosine rule is simply this relationship here. It’s a bit like Pythagoras’ theorem except for this extra term on the end with the cos a in it.

Now, let’s apply the cosine rule to the particular triangle we looked at a moment ago. It’s pretty straightforward to write down this particular relationship. We can isolate the term cos alpha which gives us the angle alpha that we’re interested in. And, it’s defined in terms of the constant link lengths, A1 and A2 and the position of the end effector, x and y. We can write this simple relationship between the angles alpha and Q2. And, we know from the shape of the cosine function that cos of Q2 must be equal to negative of cos alpha. This time, let’s just write an expression for the cosine of the joined angle Q2. Now, we’re going to draw yet another red triangle and we’re going apply some simple trigonometry here. If we know Q2, then we know this length and this length of the red triangle. We can write this relationship for the sine of the joined angle Q2.

Now, we can consider this bigger triangle whose angle is beta and this side length of the triangle is given here in blue. And, the length of the other side of the triangle is this. So, now we can write an expression for the angle beta in terms of these parameters here. Going back to the red triangle that we drew earlier, we can establish a relationship between Q1 and the angle beta. Introduce yet another angle, this one gamma and we can write a relationship between the angle gamma and the tooltip coordinates x and y. Now, we can write a simple relationship between the angles that we’ve constructed, gamma and beta and the joined angle we’re interested in which is Q1. And, the total relationship looks something like this.

Quite a complex relationship, it gives us the angle of joined one, that’s Q1 in terms of the end effector coordinates y and x, and a bunch of constants, a1 and a2, and it’s also a function of the second joint angle, Q2. So, let’s summarize what it is that we have derived here. We have an expression for the cosine of Q2 and we have an expression for Q1. Now, the cosine function is symmetrical about 0. So, if we know the value of the cosine of Q2, then there are two possible solutions, a positive angle and a negative angle. We’re going to explicitly choose the positive angle, which means that I can write this expression here. And now, we have what we call the inverse kinematic solution for this two-link robot. We have an expression for the two joined angles, Q1 and Q2 in terms of the end effector pose x and y, and a bunch of constants.

You notice that the two equations are not independent. The equation for Q1, in fact, depends on the solution for Q2. In this case, Q2 is negative and we’re going to write the solution for Q2 with a negative sign in front of the inverse cosine. Now, we need to solve for Q1, so we’re going to introduce this particular red triangle, the angle beta that we solved previously, and the angle gamma which is defined in terms of y and x. Now, we write a slightly different relationship between Q1, gamma and beta, different to what we had before. There’s a change of sign involved. Then, we can substitute all that previous equation and come up with this expression for Q1. Again, there is a change of sign here. Previously, this was a negative sign.

And, here in summary form is the solution for the inverse kinematics of our two-link robot when it is in this particular configuration, where Q2 is negative. Let’s compare the two solutions, the case where Q2 is positive and the case where Q2 is negative.