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.
We revisit the simple 2-link planar robot and determine the inverse kinematic function using simple geometry and trigonometry.
High school mathematics
This content assumes an understanding of high school-level mathematics, e.g. trigonometry, algebra, calculus, physics (optics) and some knowledge/experience of programming (any language).
Maybe adding a video on the analitical solution for 3d robot would be helpful.
Thanks for the suggestion. You can find that solution in a lot of standard textbooks such as Siciliano et al.
1. What if I give x and y in other quadrants?
( I did it in MATLAB, theta 2 is becoming complex for negative x value)
2. Is there any solution (inverse kinematics) for 3 link planar robot?
Thanks in advance!
Q1. Other quadrants are fine. I cannot see how you can get a complex results because the equation for q1 uses x^2 which will always be positive. q1 is computed using atan2 which cannot give a complex result.
Q2. There are multiple solutions, not sure how many. You can frame the inverse kinematics problem as solving for just the end-effector position (x,y) or for end-effector pose in the plane (x,y,theta). I suspect in the first case there will be an infinite number of solutions.
I have confusion.
q2 = Pi-alpha
By definition q2 = Pi -alpha, and we know that: cos(Pi-x)=-cos(x).
Applying that rule gives cos(q2)=cos(Pi-alpha)=-cos(alpha).
Where is the Pi from though?
take cos for both sides of equation then use the difference identity for right hand side of the equation:
I think there is an error in the q1 solution in the second part (elbow up)
I found q1 = atan(y/x)-atan(a2*sin(q2)/(a1+a2*cos(q2))) (same expression as in the first part “elbow down”)
It is geometricaly logical because your β is a negative angle at this point of the calculation (because q2 is negative)
Please tell me if I’m wrong
Those diagrams look beautiful. What software did you draw them in??
Hi Prof Corke,
In Q2, the end effector pose includes the angle i.e. (x,y, theta) as well. So I think position can be placed in the place of the pose. Because if we consider it as a pose then for a given pose only unique joint values are possible.
right me if i am wrong