Calculating the current coordinates of a robot’s hand is easy.
We just need to look at each segment of a robot’s arm–the coordinates of the segment’s base, the direction of the joint’s axis, the angle between this segment and the next one, and the length of the segment–in order to calculate where the end of this segment is. Repeat this with each segment, until we arrive at the robot’s hand. Et voilà: we determined the hand’s position.
This is called forward kinematics.
Now the robot’s arm must adjust each joint’s angle in order to move its hand over the cup. This is quite the opposite of the previous calculation – here, we start with a given position and want to know how to rotate each segment of the arm.
It turns out that this is much harder than the forward case. And whenever something is hard to solve, there are usually several different approaches available for solving that problem. For inverse kinematics, there are three of them:
- The algebraic approach: This basically works by solving (frankly, rather complex) matrix equations.
- The geometric approach: The idea is to combine knowledge about the robotic arm’s geometry with suitable trigonometric formulas.
- The numeric approach: Take a guess and look how far we are off. Move one or more segments to locally minimize the error. Repeat.
Which one to pick? After all, each of them has its raison d’être.
For the sake of brevity, let’s drop the first one. It involves a lot of matrix calculations, and frankly, I haven’t done any since the last millennium or so.
The second one, the geometric approach, can become quite complex when the robot’s arm consists of many segments and joints. Luckily, all complexity vanishes in the case of a simple robotic arm with only two segments, so let’s go with this approach. (In a future article, I’ll give the numeric approach a try.)
At this point, I must admit that when I started working on this article, I expected that the formulas for the simple two-segment arm could easily be generalized to multi-segment, multi-joint robotic arms, but I found that this is not the case. So for this article, we’ll stick with what is probably the most simple robotic arm with rotary joints.
The SCARA robot
Our robotic arm shall meet the following requirements.
- The arm has only two segments of fixed length.
- The segments can only rotate around their base joint; there is no sliding movement.
- The axes of both joints have the same direction.
- There is no hand attached to the end of the arm. (Robot hands would have additional degrees of freedom, and remember that we want to keep things simple. Now you know why our robot just serves tea.)
A robot of this kind is called a SCARA robot .
Here is a schematic diagram of our robot:
Applying the geometric approach to the SCARA robot
Let me just tweak the diagram a little by replacing some of the labels and adding one line and two angles:
This diagram tells us a couple of things:
- The segments have the length len1 and len2 , respectively.
- The root joint describes an angle A1 measured from the x axis.
- The second joint describes an angle A2 measured from the first segment (counterclockwise in both cases).
- The tip of segment 2 points to (x,y) , and we want to calculate back from that point to the yet unknown values of A1 and A2 .
In the diagram you also see a new dotted line named
dist . It points from (0,0) to (x,y) , and as you can easily see, the three lines
dist , len1 , and len2 define a triangle. Furthermore, dist divides angle A1 into two angles D1 and D2 .
Now is a good moment to dig out an old trig formula you may remember from school: The law of cosines.
The law of cosines (see the first formula in the figure above) is a generalization of the Pythagorean theorem (c 2 = a 2 + b 2 for right(-angled) triangles) to arbitrary triangles. We do not need the basic form, but rather the transformed version that you can see below the original formula. With this version, we can calculate angle C from the triangle’s sides a , b , and c . This comes handy in two places, as we’ll see shortly.
So how do we calculate A1 and A2 ?
From the robotic arm diagram above (the one with D1 , D2 , dist , etc), we can directly derive the first formula:
A1 = D1 + D2
D1 is fairly easy to calculate. This is good ol’ Pythagoras:
- D2 requires the law of cosines. Basically, we just map our “robot triangle” to the “law of cosines” triangle by using dist as a , len1 as b , and len2 as c . The resulting angle C is our D2 .
- Now only A2 is left. Luckily, we can reuse the law of cosines for this. We only need to map our triangle to the one from the law of cosines with different parameter mappings than for D2 : len1 as a , len2 as b , and dist as c .
And that’s it. Let’s pour this into code now.