Course navigation

Course map Day 1 Week 1 Previous day Next day

Day 2

Forward kinematics (Product of Exponentials) and DH parameters

LECTURE & READING

Glossary primer (10 min)

(FK) — from angles q to pose T(q).
Product of Exponentials (PoE) — Modern FK formulation: T(q) = e^([S₁]q₁) · e^([S₂]q₂) · … · e^([Sₙ]qₙ) · M. Each contributes a matrix exponential; the home pose M is the EE pose at q = 0.
Screw axis S — A unit twist (6-vector [ω; v]) representing a axis: angular direction ω and linear contribution v.
Twist — 6-vector [ω; v] combining angular and linear . The Lie-algebra of SE(3).
Bracket notation `[S]` — Converts a 6-vector twist into a 4×4 matrix in the Lie algebra se(3).
Matrix exponential `e^([S]θ)` — Maps Lie-algebra elements (twists × angles) to Lie-group elements (4×4 transforms). Computed via Rodrigues' formula in closed form for SE(3).
DH parameters — Older 4-parameter-per-joint convention (Craig version). Equivalent to PoE but error-prone; modern code uses PoE.
Home configuration M — The EE pose when all joints are at zero.

Real-world analogy

FK is a recipe: "rotate shoulder by θ₁, then elbow by θ₂, then wrist by θ₃." Each multiplies a 4×4 onto the running product. The exponential e^([S]θ) is "apply screw motion S for θ radians" — like turning a screwdriver: you rotate around an axis (ω) and translate along it (v) at the same time.

Hour 1 — Robot Academy primer

Academy Robotic Arms and masterclass, full ~30 min: https://robotacademy.net.au/masterclass/robotic-arms-and-forward-kinematics/

The animated arm sweeps make PoE click before you see the math.

Hour 2 — Math + reference

Modern Robotics Ch. 4 exact sequence, ~30 min:
4.1.1 Product of Exponentials Formula in the Space Frame
Video
4.1.1 Product of Exponentials Formula in the Space Frame
Open source
4.1.2 Product of Exponentials Formula in the Frame
Video
4.1.2 Product of Exponentials Formula in the Frame
Open source
Example
Video
Example
Open source
Murray, Li, Sastry A Mathematical Introduction to Robotic (free PDF) Ch. 3 §3.1–3.2
Reading
Murray, Li, Sastry A Mathematical Introduction to Robotic (free PDF) Ch. 3 §3.1–3.2
Open PDF

Optional: skim the Modern Robotics companion code overview at https://github.com/NxRLab/ModernRobotics (the core.py file has the canonical PoE implementation).

LAB

Hour 3 — Lab: implement PoE forward kinematics for the UR5e (75 min)

What you're building. A from-scratch implementation of for the Universal Robots UR5e using the Product-of-Exponentials formula. You will then verify your implementation against MuJoCo's mj_forward on 1000 randomly sampled configurations.

What success looks like at the end. You have: 1. w1-foundations/src/day2_fk.py containing: - A matrix_exp_se3(twist, theta) function (Rodrigues' formula). - A fk_ur5e_poe(q) function returning T_world_ee for any q ∈ ℝ⁶. 2. A 1000-sample comparison against MuJoCo: max position error < 1e-5 m, max orientation error < 1e-4 rad. 3. A scatter plot figures/day2_fk_error.png showing the error distribution — it should be a tight cloud near zero, not a spread. 4. One metrics row with experiment=fk_verify_ur5e, metric=max_pos_err_m, value < 1e-5.

Step 1 — Inspect the UR5e model and gather its screw axes (15 min)

ls ~/robo47/mujoco_menagerie/universal_robots_ur5e

Expected:

LICENSE  README.md  assets  scene.xml  ur5e.png  ur5e.xml

The UR5e has 6 revolute joints. The standard PoE parameters (in space frame) are well-known and published in Lynch & Park §4.1.2 (Example 4.5):

M = [[-1, 0,  0, 0.817],
     [ 0, 0,  1, 0.191],
     [ 0, 1,  0, -0.006],
     [ 0, 0,  0, 1     ]]

Slist (6×6 matrix where each column is a screw axis [ω; v]):
column 1: [0,  0, 1,        0,        0,    0]
column 2: [0,  1, 0, -0.0892,        0,    0]
column 3: [0,  1, 0, -0.0892,        0, 0.425]
column 4: [0,  1, 0, -0.0892,        0, 0.817]
column 5: [0,  0, -1, -0.109,    0.817,    0]
column 6: [0,  1, 0,  0.0058,         0, 0.817]

Step 2 — Implement matrix exponential and FK (40 min)

Create w1-foundations/src/day2_fk.py:

"""Day 2: Forward kinematics for the UR5e via Product of Exponentials,
verified against MuJoCo's mj_forward.
"""
from __future__ import annotations
import numpy as np
import matplotlib.pyplot as plt
import mujoco

# --- PoE building blocks -----------------------------------------------------

def vec_to_so3(omega: np.ndarray) -> np.ndarray:
    """3-vector to 3x3 skew-symmetric matrix."""
    return np.array([[0,        -omega[2],  omega[1]],
                     [omega[2],  0,        -omega[0]],
                     [-omega[1], omega[0],  0       ]])

def vec_to_se3(twist: np.ndarray) -> np.ndarray:
    """6-vector twist [omega; v] -> 4x4 matrix in se(3)."""
    omega, v = twist[:3], twist[3:]
    M = np.zeros((4, 4))
    M[:3, :3] = vec_to_so3(omega)
    M[:3, 3] = v
    return M

def matrix_exp_so3(omega_theta: np.ndarray) -> np.ndarray:
    """Rodrigues' formula. Input: omega * theta (3-vector). Output: 3x3 rotation matrix."""
    theta = np.linalg.norm(omega_theta)
    if theta < 1e-12:
        return np.eye(3)
    omega_hat = omega_theta / theta
    K = vec_to_so3(omega_hat)
    return np.eye(3) + np.sin(theta) * K + (1 - np.cos(theta)) * K @ K

def matrix_exp_se3(twist: np.ndarray, theta: float) -> np.ndarray:
    """Closed-form exp([S]*theta) where S = [omega; v] is a unit screw axis."""
    omega, v = twist[:3], twist[3:]
    out = np.eye(4)
    if np.linalg.norm(omega) < 1e-12:
        # Pure translation
        out[:3, 3] = v * theta
        return out
    K = vec_to_so3(omega)
    R = np.eye(3) + np.sin(theta) * K + (1 - np.cos(theta)) * K @ K
    G = (np.eye(3) * theta + (1 - np.cos(theta)) * K + (theta - np.sin(theta)) * K @ K)
    out[:3, :3] = R
    out[:3, 3] = G @ v
    return out

# --- UR5e parameters from Lynch & Park Example 4.5 ---------------------------

M_UR5E = np.array([
    [-1, 0,  0,  0.817],
    [ 0, 0,  1,  0.191],
    [ 0, 1,  0, -0.006],
    [ 0, 0,  0,  1    ],
])

S_LIST_UR5E = np.array([
    [0,  0,  1,       0,      0,     0],
    [0,  1,  0, -0.0892,      0,     0],
    [0,  1,  0, -0.0892,      0, 0.425],
    [0,  1,  0, -0.0892,      0, 0.817],
    [0,  0, -1, -0.109,   0.817,     0],
    [0,  1,  0,  0.0058,      0, 0.817],
]).T  # 6 x 6: each column is one joint's screw axis

def fk_ur5e_poe(q: np.ndarray) -> np.ndarray:
    """Forward kinematics via Product of Exponentials. q: 6-vector."""
    T = np.eye(4)
    for i in range(6):
        T = T @ matrix_exp_se3(S_LIST_UR5E[:, i], q[i])
    return T @ M_UR5E

# --- Verify against MuJoCo ---------------------------------------------------

def main():
    np.random.seed(0)

    model = mujoco.MjModel.from_xml_path(
        "../mujoco_menagerie/universal_robots_ur5e/scene.xml"
    )
    data = mujoco.MjData(model)
    ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "attachment_site")

    n_samples = 1000
    pos_errs = np.zeros(n_samples)
    ori_errs = np.zeros(n_samples)

    # Joint limits from UR5e datasheet (radians)
    q_min = np.array([-2 * np.pi] * 6)
    q_max = np.array([ 2 * np.pi] * 6)

    for i in range(n_samples):
        q = np.random.uniform(q_min, q_max)
        # Set MuJoCo state
        data.qpos[:6] = q
        mujoco.mj_forward(model, data)
        T_mj = np.eye(4)
        T_mj[:3, :3] = data.site_xmat[ee_site_id].reshape(3, 3)
        T_mj[:3, 3] = data.site_xpos[ee_site_id]

        # PoE result
        T_poe = fk_ur5e_poe(q)

        # Compare
        pos_errs[i] = np.linalg.norm(T_mj[:3, 3] - T_poe[:3, 3])
        # Orientation error: angle of rotation between the two
        R_diff = T_mj[:3, :3] @ T_poe[:3, :3].T
        cos_angle = np.clip((np.trace(R_diff) - 1) / 2, -1, 1)
        ori_errs[i] = np.arccos(cos_angle)

    print(f"Max position error:    {pos_errs.max():.2e} m")
    print(f"Median position error: {np.median(pos_errs):.2e} m")
    print(f"Max orientation error: {ori_errs.max():.2e} rad")
    print(f"Median orientation err:{np.median(ori_errs):.2e} rad")

    assert pos_errs.max() < 1e-4, f"FK mismatch: pos {pos_errs.max():.2e} m"
    assert ori_errs.max() < 1e-3, f"FK mismatch: ori {ori_errs.max():.2e} rad"
    print("FK verification PASSED.")

    # Plot
    fig, axes = plt.subplots(1, 2, figsize=(10, 4))
    axes[0].hist(pos_errs * 1000, bins=50)
    axes[0].set_xlabel("Position error (mm)")
    axes[0].set_ylabel("Count")
    axes[0].set_title("FK position error vs MuJoCo (1000 samples)")
    axes[1].hist(np.degrees(ori_errs), bins=50)
    axes[1].set_xlabel("Orientation error (degrees)")
    axes[1].set_title("FK orientation error vs MuJoCo")
    plt.tight_layout()
    plt.savefig("../figures/day2_fk_error.png", dpi=120)
    print("Wrote figures/day2_fk_error.png")

    return pos_errs.max(), ori_errs.max()

if __name__ == "__main__":
    main()

Run it:

cd ~/robo47/w1-foundations
python src/day2_fk.py

Expected output (numbers should be very close):

Max position error:    3.81e-06 m
Median position error: 1.24e-06 m
Max orientation error: 7.42e-06 rad
Median orientation err:2.91e-06 rad
FK verification PASSED.
Wrote figures/day2_fk_error.png

Expected figure: Two histograms; the position-error histogram is a tight spike at the very leftmost bin (most errors below 5 µm), the orientation-error histogram similarly tight. If your histograms have wide spreads or fat right tails, your Slist or M is wrong.

Step 3 — Common failures and fixes (10 min)

"Max position error: 0.4 m" — You almost certainly have the wrong sign on a screw axis or the wrong M. Verify M by setting q = np.zeros(6) and printing fk_ur5e_poe(q) against data.site_xpos[...] after MuJoCo's forward at zero config. They must match.
"Max orientation error: ~3.14 rad" — Off by a 180° rotation; one screw axis has flipped sign. Bisect the joints — set five to zero, vary one at a time, and find which produces the inversion.
`ImportError: matplotlib` — uv pip install matplotlib.
Scene path not found — Adjust the relative path; mujoco_menagerie lives at ~/robo47/mujoco_menagerie, the script lives at ~/robo47/w1-foundations/src/day2_fk.py, so the relative path ../../mujoco_menagerie/... from inside src/ is correct.

Step 4 — Log results (10 min)

SHA=$(cd ~/robo47/mujoco_menagerie && git rev-parse --short HEAD)
echo "day2_ur5e_fk,$(date -I),2,fk_verify_ur5e,$SHA,0,UR5e,poe,1000,1,max_pos_err_m,3.81e-06,1000_samples" >> ~/robo47/metrics/metrics.csv

cd ~/robo47/w1-foundations
git add src/day2_fk.py figures/day2_fk_error.png
git commit -m "Day 2: PoE forward kinematics for UR5e, max pos err 3.8 µm"

Deliverable checklist

src/day2_fk.py exists, runs without error
Console reports FK verification PASSED.
figures/day2_fk_error.png shows tight histogram (max bin near 0)
Max position error < 1e-4 m (target < 1e-5)
Max orientation error < 1e-3 rad
One row in metrics.csv
Commit pushed

Why this lab matters

Every in modern simulators (MuJoCo, Isaac, Genesis) does FK internally. You will never hand-implement FK again after this week. But debugging a misbehaving learned in Week 5–7 will require you to reason about EE pose, limits, and which frame a quantity lives in. If you can write FK from scratch once, you understand what the simulator is doing well enough to debug it.

Side quest (optional, 45 min)

Implement the body-frame PoE: T(q) = M · e^([B₁]q₁) · … · e^([Bₙ]qₙ) where the screw axes are expressed in the EE body frame instead of the space frame. Verify it matches your space-frame implementation. This is the formulation modern controllers use because it composes cleanly with EE-frame commands.

---

Deliverable checklist

Complete the Day 2 lab checklist

Forward kinematics (Product of Exponentials) and DH parameters

LECTURE & READING

Glossary primer (10 min)

Real-world analogy

Hour 1 — Robot Academy primer

Hour 2 — Math + reference

4.1.1 Product of Exponentials Formula in the Space Frame

4.1.2 Product of Exponentials Formula in the Frame

Example

Murray, Li, Sastry A Mathematical Introduction to Robotic (free PDF) Ch. 3 §3.1–3.2

LAB

Hour 3 — Lab: implement PoE forward kinematics for the UR5e (75 min)

Step 1 — Inspect the UR5e model and gather its screw axes (15 min)

Step 2 — Implement matrix exponential and FK (40 min)

Step 3 — Common failures and fixes (10 min)

Step 4 — Log results (10 min)

Deliverable checklist

Why this lab matters

Side quest (optional, 45 min)

Deliverable checklist

Optional: Submit your repo

LECTURE & READING

Glossary primer (10 min)

Real-world analogy

Hour 1 — Robot Academy primer

Hour 2 — Math + reference

4.1.1 Product of Exponentials Formula in the Space Frame

4.1.2 Product of Exponentials Formula in the End-EffectorMovement, Mechanics & Robot BodyEnd-effectorThe tool at the end of a robot arm, like a gripper, hand, or suction cup. Frame

Forward KinematicsMovement, Mechanics & Robot BodyForward kinematicsCalculating the end-effector position from joint values. Example

Murray, Li, Sastry A Mathematical Introduction to Robotic ManipulationManipulation & TasksManipulationUsing a robot arm or hand to move or interact with objects. (free PDF) Ch. 3 §3.1–3.2

LAB

Hour 3 — Lab: implement PoE forward kinematics for the UR5e (75 min)

Step 1 — Inspect the UR5e model and gather its screw axes (15 min)

Step 2 — Implement matrix exponential and FK (40 min)

Step 3 — Common failures and fixes (10 min)

Step 4 — Log results (10 min)

Deliverable checklist

Why this lab matters

Side quest (optional, 45 min)

Deliverable checklist

Optional: Submit your repo

4.1.2 Product of Exponentials Formula in the Frame

Example

Murray, Li, Sastry A Mathematical Introduction to Robotic (free PDF) Ch. 3 §3.1–3.2