Forward kinematics finds the hand position
Forward kinematics is the calculation that turns the joint angles of a robot arm into the position of its hand. A controller needs it all the time, because it can read the angles of the joints but cannot see where the hand is.
In this lesson you work it out for a simulated arm with two joints, using only sine, cosine and addition.
By the end of this lesson
Note
I can compute where the hand of a two-joint arm is from its link lengths and joint angles.
Key words
| Word | What it means |
|---|---|
| Link | a rigid bar of the arm between two joints |
| Joint | the place where two links turn about each other |
| End effector | the hand, the tip of the last link |
| Forward kinematics | finding the hand from the joint angles |
| Workspace | every point the hand can reach |
The cells students fill in are left blank in this preview.
A two-joint arm has two links
The simulated arm has a fixed base at the origin. Link 1 has length l1 and turns by the angle a1, measured from the x axis. At its end is the elbow joint.
Link 2 has length l2 and turns by a2 relative to link 1. So link 2 points at a1 plus a2 from the x axis. The hand is the far tip of link 2.
Each link is a step across and up
Each link is a step. Link 1 goes across by l1 times cos a1 and up by l1 times sin a1. Link 2 does the same with the angle a1 plus a2.
Add the steps. The hand x is l1 cos a1 plus l2 cos(a1 + a2). The hand y is l1 sin a1 plus l2 sin(a1 + a2). That is forward kinematics in two lines.
Worked example: links 3 and 2
| a1 | a2 | Elbow (link 1) | Hand (add link 2) |
|---|---|---|---|
| 0 | 0 | (3, 0) | (5, 0), the arm is straight |
| 0 | 90 | (3, 0) | (3, 2) |
| 90 | 0 | (0, 3) | (0, 5) |
| 90 | 90 | (0, 3) | (-2, 3), link 2 points at 180 degrees |
| 30 | 60 | (2.598, 1.5) | (2.598, 3.5), link 2 points at 90 degrees |
The cells students fill in are left blank in this preview.
Hand position for 30 and 60 degrees
docker exec jupyter python3 -c "import numpy as np; a = np.radians([30, 60]); print(np.round([3 * np.cos(a[0]) + 2 * np.cos(a[0] + a[1]), 3 * np.sin(a[0]) + 2 * np.sin(a[0] + a[1])], 3))"Shown here, not run. Create a free account to press Run and type it into your own lab’s terminal.
Read the answer against the worked table
The Run above prints the hand position as a list of two numbers, 2.598 and 3.5. That matches the last row of the table.
The first angle is 30 and link 2 points at 90 degrees, straight up. So link 2 adds nothing across and its whole length of 2 upward: 1.5 plus 2 is 3.5.
A hand function in plain Python
import math
def hand(l1, l2, a1, a2):
t1 = math.radians(a1)
t2 = math.radians(a1 + a2)
x = l1 * math.cos(t1) + l2 * math.cos(t2)
y = l1 * math.sin(t1) + l2 * math.sin(t2)
return [round(x, 3), round(y, 3)]
print(hand(3, 2, 0, 0))
print(hand(3, 2, 30, 60))
# Change the second angle to 180 and press Run.
# The arm folds back on itself. Where is the hand?
A common mistake
Warning
Many people use a2 on its own for the direction of link 2. The elbow angle is measured from link 1, not from the x axis. Link 2 points at a1 plus a2, so add the two angles before you take the cosine.
The workspace is a ring around the base
With links 3 and 2 the hand is furthest from the base when the arm is straight: 3 plus 2 is 5. It is closest when the arm is folded: 3 minus 2 is 1.
So the workspace is a ring from 1 to 5 units around the base. Nothing outside the ring, or inside its hole, can be reached.
Hand distance for each elbow angle
import math
l1 = 3
l2 = 2
for elbow in [0, 45, 90, 135, 180]:
t = math.radians(elbow)
far = math.sqrt(l1 * l1 + l2 * l2 + 2 * l1 * l2 * math.cos(t))
print("elbow", elbow, "hand is", round(far, 3), "from the base")
# Which elbow angle puts the hand closest to the base?
Going further
In start.ipynb, write a cell that loops over a1 from 0 to 90 in steps of 15 degrees with a2 fixed at 60. Print the hand position each time.
Does the hand move in a straight line, or along a curve? What shape is it?