Forward kinematics from scratch

Forward kinematics (FK) is the forward pass of a model with zero learned parameters: a deterministic, differentiable function mapping six joint angles to the gripper's pose in space. It is also the only function in your stack whose output you should trust completely — cameras lie, pose estimators drift, learned policies hallucinate, but FK is exact geometry evaluated in float64. That is why Phase 02 starts here: the classical oracle exists to separate perception failures from planning failures from execution failures, and FK is the measuring stick those comparisons rest on. When a π₀-class policy later emits joint-space actions at 50 Hz, FK is how you turn them into checkable statements about the world.

Foundations: Frames, Conventions, and the Geometry of Motion

Before deriving the forward kinematics (FK) equations, we must rigorously define the geometric objects and conventions that make the algebra unambiguous. The core abstraction is the homogeneous transformation matrix, a 4×4 matrix that encodes both the position and orientation of a coordinate frame. We adopt the standard convention that a transform $T_{A,B}$ maps coordinates of a point expressed in frame $B$ to coordinates in frame $A$. This is a passive transformation: the physical point does not move; only its numerical description changes as we switch reference frames. If you are instead rotating the frame itself relative to a fixed point, you apply the inverse transform. This distinction is critical: confusing active and passive rotations is the most common source of sign errors in robotics.

The rotation matrix $R$ is an element of the special orthogonal group $SO(3)$. It must satisfy two conditions: orthonormality ($R^T R = I$) and proper rotation ($\det(R) = 1$). The translation vector $p$ has units of length (meters). The bottom row $[0, 0, 0, 1]$ ensures that the matrix acts as an identity on the homogeneous coordinate, allowing us to treat rotation and translation as a single linear operation. This structure is why we call it a 'homogeneous' transform: it unifies affine transformations into a single matrix multiplication.

A common pitfall is assuming that the fixed offset in a URDF is always a pure translation. In reality, the rpy (roll-pitch-yaw) parameters define a rotation between the parent frame and the joint axis. If you ignore this rotation and assume the joint axis is aligned with the parent frame's axes, your FK will be wrong for any non-zero joint angle. The general form of the per-joint transform is $T_{i-1,i} = T_{\text{fixed}} \cdot T_{\text{motion}}$, where $T_{\text{fixed}}$ includes both the translation $p_i$ and the rotation $R_{\text{rpy},i}$, and $T_{\text{motion}}$ is the rotation about the joint axis by angle $q_i$.

Worked Example: Two-Link Planar Arm

Let us derive the FK for a two-link planar arm with link lengths $\ell_1 = 0.3$ m and $\ell_2 = 0.3$ m. The joint angles are $\theta_1 = 30^\circ$ ($\pi/6$ rad) and $\theta_2 = -60^\circ$ ($-\pi/3$ rad). We will compute the end-effector position $(x, y)$ and orientation $\phi$ step-by-step, showing the intermediate matrix multiplication to clarify the geometric composition.

First, we compute the individual transforms. For joint 1: $\theta_1 = \pi/6$, $\ell_1 = 0.3$. $\cos(\pi/6) = \sqrt{3}/2 \approx 0.8660$, $\sin(\pi/6) = 0.5$. For joint 2: $\theta_2 = -\pi/3$, $\ell_2 = 0.3$. $\cos(-\pi/3) = 0.5$, $\sin(-\pi/3) = -\sqrt{3}/2 \approx -0.8660$. The cumulative angle for the end-effector orientation is $\phi = \theta_1 + \theta_2 = \pi/6 - \pi/3 = -\pi/6$.

Performing the multiplication, the position vector $p$ is the sum of the first link's position and the second link's position rotated into the base frame: $p = \begin{bmatrix} \ell_1 c_1 \\ \ell_1 s_1 \end{bmatrix} + R(\theta_1) \begin{bmatrix} \ell_2 c_2 \\ \ell_2 s_2 \end{bmatrix}$. Substituting the values: $x = 0.3(0.8660) + 0.3(0.8660) = 0.2598 + 0.2598 = 0.5196$ m. $y = 0.3(0.5) + 0.3(-0.5) = 0.15 - 0.15 = 0.0$ m. The result $(0.5196, 0.0)$ m makes physical sense: the arm is symmetric about the x-axis, so the y-components cancel.

The kinematic chain: what an arm actually is

Strip the housing off a robot arm and you find a serial kinematic chain: rigid links connected by joints, each joint permitting exactly one degree of freedom of relative motion. A revolute joint rotates about a fixed axis; a prismatic joint translates along one. Arms are almost entirely revolute — motors like to spin — while prismatic joints show up in gantries, lifts, and gripper fingers. The 6-DOF WidowX AI is six revolute joints in series — Trossen numbers them 0 through 5 — plus a parallel-jaw gripper whose fingers translate without changing the arm's pose:

• Joint 0 — base yaw about the vertical axis, ±180°. Sweeps the whole arm around the table.
• Joint 1 — shoulder-level pitch, 0–180°. Together with the elbow, does most of the positional work.
• Joint 2 — elbow-level pitch, 0–135°, axis parallel to joint 1's. Shoulder plus elbow form the planar two-link arm we derive by hand below.
• Joint 3 — first wrist axis, ±90°.
• Joint 4 — second wrist axis, ±90°, for aiming the gripper.
• Joint 5 — roll about the tool axis, ±180°. The last three joints mostly steer orientation rather than position.

A configuration is the vector $q \in \mathbb{R}^6$ of joint angles, restricted to a box by the joint limits. This is exactly the action space your future learned policy emits — the Trossen OpenPI integration for this arm outputs joint positions plus a gripper command per timestep — so every claim you make about a VLA's task-space behavior passes through the function built here. Numbers to keep in your head for the WidowX AI: 0.769 m maximum reach, 1.4 m span, 1.5 kg rated payload, ~1 mm repeatability per Trossen's spec sheet.

Run that repeatability backwards through the geometry. At the full 0.769 m lever arm, 1 mm of fingertip displacement corresponds to $0.001 / 0.769 \approx 1.3$ mrad of equivalent joint error. Modern integrated actuators sense far finer than that — the AK60-6 bench actuator from the anatomy lesson carries a 21-bit magnetic encoder, about 3 μrad per count, which is microns at the fingertip — so the millimeter floor is not encoder quantization; it is gear-train backlash, structural flex, and controller deadband. FK turns exactly this kind of joint-space arithmetic into task-space statements, and its formal job description is:

One frame per link, one transform per joint

The construction is the frame tree from Lab 0 with parameters added. Attach a frame rigidly to every link. Joint $i$ then contributes a transform $T_{i-1,i}(q_i)$ mapping link-$i$ coordinates into link-$(i{-}1)$ coordinates, factored into two pieces: a fixed offset — where the joint sits on its parent link, read from the URDF's origin xyz and rpy — and a motion term, a rotation by $q_i$ about the joint's unit axis $\omega_i$:

The full chain is a product, read left to right from base to tip, closed with a fixed tool transform from the last joint's frame to the point you actually care about — usually the midpoint between the fingertips:

You will meet two other encodings of the same idea. Denavit–Hartenberg (DH) parameters compress each link into four scalars $(a, \alpha, d, \theta)$ by imposing strict rules on where frames may sit — the textbook standard, worth recognizing on sight, but unintuitive, and its two incompatible conventions (standard and modified) produce silent off-by-one-link errors when mixed. The product of exponentials (PoE) from Lynch and Park's Modern Robotics goes the other way: no intermediate frames at all. Each joint is a screw axis $S_i$ in the base frame, and the chain is $T(q) = e^{[S_1]q_1} \cdots e^{[S_6]q_6} M$, with $M$ the zero-configuration pose. We implement the URDF-style frame chain because that is the artifact your arm ships with — but keep PoE as the mental model.

Derive it by hand: the planar two-link arm

Project the WidowX AI onto the vertical plane through its forearm and you get the classic two-link planar arm: shoulder joint, upper-arm link $\ell_1 \approx 0.3$ m, elbow joint, forearm-plus-wrist link $\ell_2 \approx 0.3$ m to the fingertips (illustrative lengths for a 0.77 m-reach arm — the exact link geometry is not published). Every essential FK idea is visible in this 2D toy, and the closed form derived here becomes the known-answer oracle for the 6-DOF code. In the plane a pose is $(x, y, \phi)$ and a homogeneous transform is 3×3. Define one reusable block — rotate by θ, then translate $\ell$ along the new x-axis:

Chain one block per joint and multiply, $T_{0,\text{ee}} = T(\theta_1, \ell_1)\, T(\theta_2, \ell_2)$. Work the blocks separately. The rotation parts compose by the angle-addition identities — composing rotations in the plane is literally adding angles — and the second link's translation gets rotated into the base frame before being added:

Interrogate the formula where you already know the answer. At $q = (0, 0)$: $x = \ell_1 + \ell_2 = 0.60$ m, $y = 0$ — fully extended. At $q = (\pi/2, 0)$: $(0, 0.60)$ — straight up. At $q = (0, \pi)$: $x = \ell_1 - \ell_2 = 0$ — the forearm folds back and the fingertip lands on the shoulder. These three-second checks catch a remarkable fraction of real bugs, and they become the known-answer tests below. Notice also what the formula quietly promises for Lesson 3: a desired $(x, y)$ generically has two solutions — elbow-up and elbow-down — which merge into one near full extension. Hold that picture; inverse kinematics is built on it.

The general algorithm: six DOF from a URDF-like description

In 3D nothing conceptual changes — only bookkeeping. Each URDF joint element hands you the fixed offset (origin xyz, rpy) and the axis vector; the algorithm walks the chain multiplying fixed times motion. Two choices worth copying: use Rodrigues' formula for every joint rotation instead of special-casing x/y/z axes (the URDF may specify any unit vector, and sign errors hide in special cases), and return the intermediate link frames, not just the final pose — next lesson's Jacobian is assembled directly from them.

import numpy as np

def hat(w):
    wx, wy, wz = w
    return np.array([
        [0.0, -wz,  wy],
        [ wz, 0.0, -wx],
        [-wy,  wx, 0.0],
    ])

def rot_axis(axis, theta):
    w = np.asarray(axis, dtype=float)
    w = w / np.linalg.norm(w)
    K = hat(w)
    return np.eye(3) + np.sin(theta) * K + (1.0 - np.cos(theta)) * (K @ K)

def make_T(R, p):
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = p
    return T

# Offsets and axes below are illustrative approximations of a 0.77 m-reach
# 6-DOF arm (exact WidowX AI link geometry is not published). Read the real
# values from the URDF in Trossen's ROS 2 description for the WidowX AI.
CHAIN = [
    dict(name="joint0_base_yaw", xyz=[0.000, 0.0, 0.110], axis=[0, 0, 1]),
    dict(name="joint1_shoulder", xyz=[0.000, 0.0, 0.050], axis=[0, 1, 0]),
    dict(name="joint2_elbow",    xyz=[0.060, 0.0, 0.300], axis=[0, 1, 0]),
    dict(name="joint3_wrist",    xyz=[0.170, 0.0, 0.000], axis=[1, 0, 0]),
    dict(name="joint4_wrist",    xyz=[0.115, 0.0, 0.000], axis=[0, 1, 0]),
    dict(name="joint5_roll",     xyz=[0.075, 0.0, 0.000], axis=[1, 0, 0]),
]
TOOL = make_T(np.eye(3), [0.110, 0.0, 0.0])  # flange -> fingertip midpoint

def fk(q, chain=CHAIN, tool=TOOL):
    """q: joint angles in radians. Returns (T_base_ee, per-link frames)."""
    T = np.eye(4)
    frames = []
    for joint, theta in zip(chain, q):
        fixed  = make_T(np.eye(3), joint["xyz"])  # rpy is zero on this arm
        motion = make_T(rot_axis(joint["axis"], theta), np.zeros(3))
        T = T @ fixed @ motion
        frames.append(T.copy())  # keep these: the Jacobian reads them
    return T @ tool, frames

Count the work: six iterations of two 4×4 multiplies — around a thousand FLOPs, where numpy's ~20 μs per-call overhead dominates the arithmetic; a JAX-jitted or C version runs well under a microsecond. Spend that cheapness everywhere: log the FK pose alongside raw joint states on every tick of the Lab 0 episode logger (task-space trajectories for free), have the safety watchdog run FK on every outgoing command to reject configurations outside the workspace box before dispatch, and let the planners of Lessons 3 and 7 call it in their inner loops without a second thought.

Workspace: reachable is not usable

Sweep $q$ over the joint-limit box and collect the FK positions: that is the reachable workspace — for the WidowX AI, roughly a 770 mm-radius spherical region around the shoulder, hollowed out by the base column, the table, and joint limits. The dexterous workspace is the subset where the gripper attains not just the position but the orientations your task needs — for tabletop work, usually a top-down grasp with the wrist vertical. It is dramatically smaller, because reaching far and pointing down compete: a vertical wrist folds the final ~100 mm of linkage downward instead of outward, shortening effective reach by that much.

The last few centimeters before maximum reach are unusable even for pure positioning. Reaching them forces the elbow toward fully straight, where three things degrade at once: the elbow-up and elbow-down IK branches merge (no alternative posture if one collides), the achievable-orientation set collapses toward a single wrist direction, and radial corrections become second-order — retracting the fingertip a few millimeters demands a startling amount of elbow motion. Quantify the last with the two-link model: with $\ell_1 = \ell_2 = \ell$, the fingertip radius is $r = 2\ell\cos(\theta_2/2)$, so a retraction $\delta$ from full extension costs:

The practical rule: plan inside about 85% of maximum reach — for the WidowX AI, keep pick-and-place targets within roughly 650 mm, preferably in the 250–550 mm band where the dexterous workspace is fat. Map yours empirically with a Monte Carlo sweep: sample $10^6$ configurations uniformly within joint limits, run vectorized FK (a second or two of numpy), scatter-plot positions from the top and side, then repeat keeping only samples whose gripper z-axis is within 10° of vertical to see the top-down-grasp region. Save both plots — they justify the workspace limits in Lab 0's watchdog and define the task region every later lesson inherits.

Verification: trust, but measure

Verify FK the way you would verify a hand-written CUDA kernel: known-answer tests first, an independent reference second, hardware last. Tier 1 — analytic. Configurations you can answer by inspection: the home pose (sum the offsets by hand), single-joint sweeps (joint-0-only rotation must yaw the home position about base z and leave height unchanged), the planar two-link oracle derived above, and orthonormality at random configurations. Tier 2 — reference. Compare against an implementation you did not write — any URDF-loading kinematics library fed Trossen's ROS 2 description of the WidowX AI — at 1,000 random configurations; agreement should sit at the 1e-10 level, and any real discrepancy is a convention mismatch, almost always rpy order or an axis sign. Tier 3 — physical. Command real configurations and measure the fingertip with a ruler. In code:

import numpy as np
from fk import fk, make_T, CHAIN

def test_home_pose():
    T, _ = fk(np.zeros(6))
    # All zeros: arm stretched along +x. Sum the offsets by hand:
    # x: 0.060 + 0.170 + 0.115 + 0.075 + 0.110 (tool) = 0.530
    # z: 0.110 + 0.050 + 0.300                        = 0.460
    assert np.allclose(T[:3, 3], [0.530, 0.0, 0.460], atol=1e-12)

def test_base_yaw_is_pure_yaw():
    home, _ = fk(np.zeros(6))
    x, y, z = home[:3, 3]
    T, _ = fk(np.array([np.pi / 2, 0, 0, 0, 0, 0]))
    # +90 deg of joint 0 maps (x, y, z) -> (-y, x, z); height untouched
    assert np.allclose(T[:3, 3], [-y, x, z], atol=1e-12)

def test_rotation_stays_orthonormal():
    rng = np.random.default_rng(0)
    for _ in range(1000):
        q = rng.uniform(-np.pi, np.pi, size=6)
        R = fk(q)[0][:3, :3]
        assert np.linalg.norm(R.T @ R - np.eye(3)) < 1e-13
        assert abs(np.linalg.det(R) - 1.0) < 1e-13

def test_matches_planar_two_link_oracle():
    # Rebuild the hand-derived 2-link arm with the same machinery.
    chain = [
        dict(name="j1", xyz=[0.00, 0.0, 0.0], axis=[0, 0, 1]),
        dict(name="j2", xyz=[0.25, 0.0, 0.0], axis=[0, 0, 1]),
    ]
    tool = make_T(np.eye(3), [0.25, 0.0, 0.0])
    for t1, t2 in [(0.3, 0.5), (-0.7, 1.1), (0.0, -0.4)]:
        T, _ = fk(np.array([t1, t2]), chain=chain, tool=tool)
        x = 0.25 * np.cos(t1) + 0.25 * np.cos(t1 + t2)
        y = 0.25 * np.sin(t1) + 0.25 * np.sin(t1 + t2)
        assert np.allclose(T[:2, 3], [x, y], atol=1e-12)

For the physical tier: tape a printed millimeter grid to the table, register its origin to the robot base, command 5–8 configurations spanning the workspace, let the arm settle, and measure the fingertip with calipers or a steel rule. Expect 3–10 mm of disagreement, growing with extension as gravity sag accumulates. Resist verifying FK with your RealSense instead — its extrinsics are unverified until Lesson 4's hand-eye calibration, so you would be testing one uncalibrated system against another. The goal is not small numbers; it is knowing your FK error budget before Lessons 4 and 5 stack calibration and pose-estimation errors on top. When the numbers are wrong, the shape of the error names the bug:

• Joint effect ~57× too large or too small: degrees passed where radians were expected. The 57.3 signature is unmistakable.
• Motion mirrored relative to prediction: a joint axis sign flipped, or rpy rotations applied in the wrong order.
• Constant offset in one coordinate at every configuration: wrong base-frame origin or a missing tool transform.
• Error growing smoothly with extension: unmodeled gravity sag, or a link length off by a few millimeters.

Numerical hygiene, briefly — mostly a non-issue you should know is a non-issue. One FK evaluation in float64 leaves the rotation orthonormal to ~1e-15; six matrix multiplies cannot hurt you. Drift becomes real only when rotations are built incrementally: integrating velocity commands or IMU rates at 100 Hz to 1 kHz composes $10^5$ to $10^6$ small rotations per hour, and $\lVert R^\top R - I \rVert$ creeps into the 1e-9 range while the determinant wanders off 1. Two rules cover this course: recompute FK from $q$ from scratch every tick — statelessness is the cheapest hygiene — and where you must integrate, carry a quaternion and normalize each step, or project back to $SO(3)$ with an SVD ($R \leftarrow UV^\top$) when the departure exceeds ~1e-9.

Where this goes next: Lab 0: bring-up, safety, and the episode logger gave the robot instrumented senses and a safe way to move; this lesson gave it a geometric self-model — an exact map from joint angles to gripper pose with a measured error budget attached. The next step is to differentiate that map: The Jacobian: velocities, forces, and singularities asks what happens when each $q_i$ wiggles, and turns the per-link frames your fk() already returns into the matrix governing velocities, forces, and the singularities you just met at the workspace boundary. The rest of the classical oracle — IK in Lesson 3, calibrated cameras in Lesson 4, the full pick-and-place baseline — stands on those two functions.