Forward kinematics of a two-link arm

Forward kinematics answers one question: given the joint angles, where is the tool? It is the first calculation in almost every robot program. A simulator needs it to draw the arm, a collision checker needs it to know where the links are, and every inverse kinematics solver calls it in its inner loop. This lesson works it out for the simplest useful arm, a planar arm with two revolute joints, first with trigonometry and then with the homogeneous transforms from lesson 02. Then it maps the arm’s workspace. Lesson 04 runs the problem backwards.

The arm

The arm moves in the x-y plane of its base frame. Joint 1 sits at the base origin and rotates the first link, of length l1. Joint 2 sits at the far end of link 1 (the elbow) and rotates the second link, of length l2. The tip of link 2 is the end-effector, the point we care about. Both joint axes point along z, out of the page.

We use the conventions from lesson 02 and REP 103. Angles are in radians in code. A positive angle turns anticlockwise when viewed from +z (the right-hand rule). theta1 is measured from the base x axis. theta2 is measured from the line of link 1, not from the base x axis, so theta2 of zero means the arm is straight. This relative definition matches what a joint encoder on the elbow actually measures.

The arm has 2 degrees of freedom, and its tip has 2 coordinates, so it can control the position of the tip but not, independently, the direction the last link points. That direction is theta1 + theta2 and comes along for free.

Forward kinematics by geometry

Follow the arm from the base. The elbow is l1 along a line at angle theta1, so it sits at (l1 cos theta1, l1 sin theta1). Link 2 points at angle theta1 + theta2 measured from the base x axis, because the elbow angle adds to the shoulder angle. Its tip is l2 further along that direction. Adding the two vectors:

x = l1 cos(theta1) + l2 cos(theta1 + theta2)
y = l1 sin(theta1) + l2 sin(theta1 + theta2)

That is the whole of forward kinematics for this arm. In numpy, with links of 0.4 m and 0.3 m:

import numpy as np

L1, L2 = 0.4, 0.3   # link lengths, m

def fk_geometric(theta1: float, theta2: float) -> tuple[np.ndarray, np.ndarray]:
    """Elbow and end-effector positions in the base frame, by trigonometry."""
    elbow = np.array([L1 * np.cos(theta1), L1 * np.sin(theta1)])
    tip = elbow + np.array([L2 * np.cos(theta1 + theta2), L2 * np.sin(theta1 + theta2)])
    return elbow, tip

for deg1, deg2 in [(0, 0), (90, 0), (0, 90), (30, 60), (45, -90)]:
    elbow, tip = fk_geometric(np.deg2rad(deg1), np.deg2rad(deg2))
    print(f"theta1 {deg1:4d} deg, theta2 {deg2:4d} deg -> "
          f"elbow ({elbow[0]:6.3f}, {elbow[1]:6.3f}) m, tip ({tip[0]:6.3f}, {tip[1]:6.3f}) m")

theta1    0 deg, theta2    0 deg -> elbow ( 0.400,  0.000) m, tip ( 0.700,  0.000) m
theta1   90 deg, theta2    0 deg -> elbow ( 0.000,  0.400) m, tip ( 0.000,  0.700) m
theta1    0 deg, theta2   90 deg -> elbow ( 0.400,  0.000) m, tip ( 0.400,  0.300) m
theta1   30 deg, theta2   60 deg -> elbow ( 0.346,  0.200) m, tip ( 0.346,  0.500) m
theta1   45 deg, theta2  -90 deg -> elbow ( 0.283,  0.283) m, tip ( 0.495,  0.071) m

Check each line against a sketch before trusting the code. With both joints at zero the arm lies along x and the tip is at 0.7 m. With the elbow at 90° the second link points straight up from (0.4, 0). At (30°, 60°) link 2 points at 90° from the base x axis, so the tip sits directly above the elbow, 0.3 m higher. The last case bends the elbow clockwise and brings the tip back down towards the x axis.

Forward kinematics by chaining transforms

The trigonometry works for two links but becomes hard to manage for six links in 3D. The method that scales is the one from lesson 02: give every link its own frame and multiply the transforms along the chain.

Walk along the arm and write down each step as a 4x4 homogeneous transformation:

1. Rotate about z by theta1 (joint 1).
2. Translate along the new x axis by l1 (link 1). You are now at the elbow, still pointing along link 1.
3. Rotate about z by theta2 (joint 2).
4. Translate along the new x axis by l2 (link 2). You are now at the tip, with x pointing along link 2.

The product of the four matrices is T_base_tool, the pose of a tool frame at the tip. Its last column holds the tip position, and its top-left 2x2 block holds the tool’s orientation.

import numpy as np

L1, L2 = 0.4, 0.3   # link lengths, m

def rot_z_h(theta: float) -> np.ndarray:
    """4x4 homogeneous rotation about z."""
    c, s = np.cos(theta), np.sin(theta)
    T = np.eye(4)
    T[:2, :2] = [[c, -s], [s, c]]
    return T

def trans_x_h(d: float) -> np.ndarray:
    """4x4 homogeneous translation along x."""
    T = np.eye(4)
    T[0, 3] = d
    return T

def fk_transforms(theta1: float, theta2: float) -> np.ndarray:
    """Pose of the end-effector frame in the base frame: rotate, slide along the link, repeat."""
    return rot_z_h(theta1) @ trans_x_h(L1) @ rot_z_h(theta2) @ trans_x_h(L2)

def fk_geometric(theta1: float, theta2: float) -> np.ndarray:
    return np.array([L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2),
                     L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)])

T = fk_transforms(np.deg2rad(30), np.deg2rad(60))
np.set_printoptions(precision=3, suppress=True)
print("T_base_tool at (30 deg, 60 deg):\n", np.round(T, 3) + 0.0)
print("tool x axis points at", np.round(np.rad2deg(np.arctan2(T[1, 0], T[0, 0])), 1), "deg")

rng = np.random.default_rng(0)
q = rng.uniform(-np.pi, np.pi, size=(1000, 2))
err = [np.linalg.norm(fk_transforms(a, b)[:2, 3] - fk_geometric(a, b)) for a, b in q]
print(f"max difference over 1000 random configurations: {max(err):.2e} m")

T_base_tool at (30 deg, 60 deg):
 [[ 0.    -1.     0.     0.346]
 [ 1.     0.     0.     0.5  ]
 [ 0.     0.     1.     0.   ]
 [ 0.     0.     0.     1.   ]]
tool x axis points at 90.0 deg
max difference over 1000 random configurations: 2.24e-16 m

The translation column, (0.346, 0.5), matches the geometric result for (30°, 60°). The rotation block says the tool’s x axis points along base y, which is the 90° direction of link 2. Over 1000 random configurations the two methods differ by at most 2.2e-16 m, which is the rounding error of 64-bit floats. They are the same function written two ways.

The transform version gives more for the same effort. It returns orientation as well as position. It extends to 3D by changing which axis each joint rotates about. It also produces the pose of every intermediate frame on the way, which a collision checker needs. Industrial practice formalises this walk in two ways: Denavit-Hartenberg parameters, which describe each link by four numbers, and the product of exponentials, which describes each joint by its screw axis. The DH and PoE cheat sheet compares them.

How model errors reach the tip

Forward kinematics is only as good as the numbers in it. If the real link 1 is 1 mm longer than the model, the computed tip is wrong by up to 1 mm. Angle errors are amplified by distance. An encoder zero that is off by 0.1° at the shoulder moves the tip sideways by the reach times 0.1° in radians: 0.7 × 0.00175, about 1.2 mm with the arm straight. Errors at the base matter more than errors near the tip, because they act through the longest lever. This is why arms are calibrated for joint zeros and link lengths, and why a model copied from a drawing rarely matches the arm to the millimetre.

The reachable workspace

Which points can the tip reach? The distance from the base to the tip depends only on the elbow angle. By the law of cosines, the triangle formed by the two links gives

r^2 = l1^2 + l2^2 + 2 l1 l2 cos(theta2)

The shoulder angle theta1 then swings that distance around the full circle. With the elbow straight (theta2 = 0) the reach is l1 + l2 = 0.7 m. Folded back on itself (theta2 = 180°) it is |l1 - l2| = 0.1 m. In between, every radius is reachable, so the workspace is a ring (an annulus) between those two circles. The code below sweeps both joints on a grid, measures the reach of every configuration, and compares the extremes with the formula. Then it repeats the sweep with the elbow limited to ±150°, which is a typical kind of mechanical stop.

import numpy as np

L1, L2 = 0.4, 0.3   # link lengths, m

def fk_geometric(theta1: np.ndarray, theta2: np.ndarray) -> np.ndarray:
    x = L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2)
    y = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)
    return np.stack([x, y], axis=-1)

def sweep(theta2_limit_deg: float, n: int = 361) -> np.ndarray:
    """Distance from the base to the tip over a grid of joint angles."""
    t1 = np.linspace(-np.pi, np.pi, n)
    lim = np.deg2rad(theta2_limit_deg)
    t2 = np.linspace(-lim, lim, n)
    g1, g2 = np.meshgrid(t1, t2)
    return np.linalg.norm(fk_geometric(g1, g2), axis=-1)

r = sweep(180.0)
print(f"full joint range:     r_min {r.min():.4f} m, r_max {r.max():.4f} m")
print(f"  expected:           r_min {abs(L1 - L2):.4f} m, r_max {L1 + L2:.4f} m")
print(f"  workspace area:     {np.pi * (r.max()**2 - r.min()**2):.4f} m^2")

r = sweep(150.0)
inner = np.sqrt(L1**2 + L2**2 + 2 * L1 * L2 * np.cos(np.deg2rad(150)))
print(f"theta2 within +-150:  r_min {r.min():.4f} m (expected {inner:.4f} m), r_max {r.max():.4f} m")

# Reach as a function of the elbow angle alone, for the chart
for d in range(0, 181, 10):
    rad = np.sqrt(L1**2 + L2**2 + 2 * L1 * L2 * np.cos(np.deg2rad(d)))
    print(f"[{d}, {rad:.3f}]", end=", " if d < 180 else "\n")

full joint range:     r_min 0.1000 m, r_max 0.7000 m
  expected:           r_min 0.1000 m, r_max 0.7000 m
  workspace area:     1.5080 m^2
theta2 within +-150:  r_min 0.2053 m (expected 0.2053 m), r_max 0.7000 m
[0, 0.700], [10, 0.697], [20, 0.690], [30, 0.677], [40, 0.659], [50, 0.636], [60, 0.608], [70, 0.576], [80, 0.540], [90, 0.500], [100, 0.456], [110, 0.410], [120, 0.361], [130, 0.309], [140, 0.257], [150, 0.205], [160, 0.156], [170, 0.117], [180, 0.100]

With full joint travel the sweep finds the annulus exactly: 0.1 m inside, 0.7 m outside, an area of about 1.51 m². Limiting the elbow to ±150° leaves the outer edge alone but pushes the inner edge out to 0.205 m, because the arm can no longer fold far enough to bring the tip close to the base. The chart plots the last printed line: reach against elbow angle.

The curve is flat near zero. Bending a straight elbow by 10° shortens the reach by only 3 mm. Near 180° the curve is steep again in the other direction. This flatness at the straight-arm end is the first sign of a singularity: at theta2 = 0 small joint motions barely move the tip along the arm, a fact that causes real trouble for inverse kinematics in lesson 04.

Two cautions about workspaces in practice. First, a point inside the annulus is reachable by position only. A real task usually also fixes the tool orientation, and the set of points reachable with a given orientation (the dexterous workspace) is smaller. Second, the full annulus assumes nothing is in the way. The base, the table and the arm itself remove parts of it, which is where collision checking and the motion planning hub come in.

Exercise

1. Add a third revolute joint and a link of 0.1 m to fk_transforms. Sweep all three joints and report the new inner and outer radius. Can the three-link arm reach the base origin?
2. Change the link lengths to 0.35 m and 0.35 m. What happens to the inner radius, and why might a designer choose equal links?
3. Limit theta1 to between 0 and 180° (a table-mounted arm that must not swing below the table edge) and leave theta2 free. Plot or print the points the tip can reach and describe the shape.
4. Write fk_transforms for a 3D arm whose first joint rotates about z and whose second joint rotates about y. Check it against a hand calculation for one configuration.

Summary

• Forward kinematics maps joint angles to the end-effector pose. It is unique and always defined, and it runs inside simulators, collision checkers and IK solvers.
• For a planar two-link arm, x = l1 cos θ1 + l2 cos(θ1 + θ2) and y = l1 sin θ1 + l2 sin(θ1 + θ2), with θ2 measured from link 1.
• Chaining rot_z(θ1) @ trans_x(l1) @ rot_z(θ2) @ trans_x(l2) gives the same position plus orientation, and it is the method that scales to 3D arms.
• The workspace of a two-link arm with full joint travel is an annulus from |l1 - l2| to l1 + l2. Joint limits on the elbow push the inner radius out.

Further reading

• Lynch and Park, Modern Robotics: Mechanics, Planning, and Control, Cambridge University Press, 2017: chapter 4, “Forward Kinematics”, derives the product of exponentials formula in the space frame and in the end-effector frame.
• Siciliano, Sciavicco, Villani and Oriolo, Robotics: Modelling, Planning and Control, Springer, 2009: chapter 2, “Kinematics”, goes further into the forward kinematics of serial manipulators.
• Corke, Robotics, Vision and Control: Fundamental Algorithms in Python, 3rd edition, Springer, 2023: computes arm kinematics in code with the author’s open-source Robotics Toolbox for Python.