Forward kinematics

Forward kinematics (FK) answers the easy question: I know every joint angle, so where is the hand? It is easy because the answer is unique. Each set of joint angles gives exactly one tip position, and you get it by chaining simple geometry from the base outward. FK is also the foundation for everything else in this course. Inverse kinematics inverts it, and the Jacobian is its derivative.

Deriving the planar two-link arm

Put the base at the origin. Link 1 has length L1 and makes angle t1 with the x axis. Its tip, the elbow, is at:

xe = L1*cos(t1) ye = L1*sin(t1)

Link 2 is attached at the elbow. The elbow joint rotates by t2 relative to link 1, and link 1 already points along t1. So link 2 points along the sum t1 + t2. This is the key idea: every joint rotates everything beyond it, so angles add up along the chain. Link 2 contributes L2*cos(t1 + t2) in x and L2*sin(t1 + t2) in y, and the tip is:

x = L1*cos(t1) + L2*cos(t1 + t2) y = L1*sin(t1) + L2*sin(t1 + t2) phi = t1 + t2 (the direction the tool points)

Three numbers describe the tool pose (x, y, phi), but only two joint angles feed them. That is the 2-DOF limit from lesson 1.

Rotation matrices

Writing sines and cosines by hand works for two links but breaks down fast. A rotation matrix packages the same idea. To rotate a 2D vector by an angle t counter-clockwise:

R(t) = [[cos t, -sin t], [sin t, cos t]]

Check it on the vector (1, 0), which should land at (cos t, sin t): multiplying gives (cos t*1 - sin t*0, sin t*1 + cos t*0) = (cos t, sin t). Correct. Rotations compose by multiplication, and R(t1)*R(t2) = R(t1 + t2), which is the matrix form of "angles add up".

Homogeneous transforms

A rotation alone cannot express "rotate, then move 100 mm along the link". The trick is to add one dimension. In 2D we use 3 by 3 matrices acting on (x, y, 1):

T = [[cos t, -sin t, px], [sin t, cos t, py], [0, 0, 1]]

The upper-left 2 by 2 block is the rotation, and (px, py) is a translation. Now rotation and translation live in one matrix, and a chain of joints is a chain of matrix products. For our arm, joint i rotates by ti and then the link extends Li along its own x axis:

Ai = [[cos ti, -sin ti, Li*cos ti], [sin ti, cos ti, Li*sin ti], [0, 0, 1]]

and the whole arm is T = A1 * A2. The tip position is the right-hand column of T. In 3D the same idea uses 4 by 4 matrices, and the standard recipe for building the Ai of any robot is called Denavit-Hartenberg parameters. The two-link case is the 3D recipe with one dimension removed.

A worked example

Take L1 = 100 mm, L2 = 80 mm, t1 = 30 degrees, t2 = 45 degrees, so t1 + t2 = 75 degrees.

1. Look up the trigonometry: cos 30 = 0.8660, sin 30 = 0.5000, cos 75 = 0.2588, sin 75 = 0.9659.
2. Elbow: xe = 100*0.8660 = 86.60 mm, ye = 100*0.5000 = 50.00 mm.
3. Tip: x = 86.60 + 80*0.2588 = 86.60 + 20.71 = 107.31 mm.
4. y = 50.00 + 80*0.9659 = 50.00 + 77.27 = 127.27 mm.

Now the same thing with matrices. With cos 45 = sin 45 = 0.7071:

A1 = [[0.866, -0.5, 86.60], [0.5, 0.866, 50.00], [0, 0, 1]] A2 = [[0.7071, -0.7071, 56.57], [0.7071, 0.7071, 56.57], [0, 0, 1]]

The position column of A1*A2 is R1 * (56.57, 56.57) + (86.60, 50.00). The rotation gives (0.866*56.57 - 0.5*56.57, 0.5*56.57 + 0.866*56.57) = (20.71, 77.27), and adding the offset gives (107.31, 127.27) mm. Same answer, as it must be.

A sanity check you can always do: the tip distance from the base follows the law of cosines, r^2 = L1^2 + L2^2 + 2*L1*L2*cos(t2) = 10000 + 6400 + 16000*0.7071 = 27713, so r = 166.5 mm. And sqrt(107.31^2 + 127.27^2) is also 166.5 mm. Note that r depends only on t2: the elbow sets how far you reach and the shoulder only swings the arm around.

Try the geometry

The widget below is a two-link arm. You will use it properly in the next lesson, where it solves the reverse problem, but you can already use it for intuition. Move the target and watch how the solved joint angles change, then plug those angles into the FK formulas and confirm that the tip lands on the target. The link lengths in the widget may differ from the 100 and 80 mm used here, so use its own values.

x110

y70

elbow: up

theta1 = 70.3°

theta2 = -87.9°

L1 = 100, L2 = 80

Python code

import numpy as np

L1, L2 = 100.0, 80.0     # link lengths, mm

def fk(t1, t2):
    """Closed-form forward kinematics, angles in radians."""
    x = L1 * np.cos(t1) + L2 * np.cos(t1 + t2)
    y = L1 * np.sin(t1) + L2 * np.sin(t1 + t2)
    return x, y

def link(t, L):
    """Homogeneous transform of one joint plus its link."""
    c, s = np.cos(t), np.sin(t)
    return np.array([[c, -s, L * c],
                     [s,  c, L * s],
                     [0,  0, 1.0]])

t1, t2 = np.radians(30), np.radians(45)
T = link(t1, L1) @ link(t2, L2)

print(fk(t1, t2))                 # (107.31, 127.27)
print(T[:2, 2])                   # same, from the matrix
print(np.degrees(np.arctan2(T[1, 0], T[0, 0])))   # 75.0, the tool angle

The matrix version scales: add a third link by multiplying one more link(t3, L3), with no new algebra. The closed form is faster and is what you put on a microcontroller.

Adding the base joint

For a MeArm-style arm the shoulder and elbow work in a vertical plane that the base joint t0 rotates about the vertical axis. Treat the planar result as a horizontal reach r and a height z, then rotate: X = r*cos(t0), Y = r*sin(t0), Z = z.