Forward Kinematics and DH Parameters

Forward kinematics (FK) answers: given the vector of joint angles (or displacements) $q = [q_1, q_2, \ldots, q_n]^\top$, where is the end-effector? Formally, FK is a function

$T_0^{EE}(q): \mathbb{R}^n \rightarrow SE(3)$

that maps joint space to Cartesian (task) space. For a 2-DOF planar arm the output collapses to $(x, y, \theta)$; a 6-DOF industrial arm produces a full $4 \times 4$ transform. FK is deterministic: one $q$ gives exactly one pose. That makes it easy to compute — the hard direction (pose $\to$ joints) is inverse kinematics, which we tackle next lesson.

A serial robot is a sequence of rigid links connected by joints. Frame $\{i\}$ is attached to link $i$. The relative transform $T_{i-1}^{i}$ depends on the joint variable $q_i$ plus the fixed geometry of link $i$. FK chains every link transform:

$T_0^n = T_0^1(q_1) \; T_1^2(q_2) \; \cdots \; T_{n-1}^n(q_n)$

For a revolute joint rotating about the $z$-axis of frame $\{i-1\}$, only $q_i$ changes; everything else is geometry. The question is: how do you parameterize that fixed geometry consistently across all links? That is exactly what Denavit-Hartenberg conventions solve.

Denavit and Hartenberg (1955) proved that any two consecutive joint axes can be related by exactly four parameters. For the transform from frame $\{i-1\}$ to frame $\{i\}$:

(*For a prismatic joint, $d_i$ is variable and $\theta_i$ is fixed.) The convention locks down axes unambiguously, keeping DH tables transferable between teams and textbooks.

The standard DH transform from frame $\{i-1\}$ to frame $\{i\}$ is a product of four elementary transforms:

$T_{i-1}^{i} = R_z(\theta_i) \; T_z(d_i) \; T_x(a_i) \; R_x(\alpha_i)$

Expanding into one matrix:

$T_{i-1}^{i} = \begin{bmatrix} c\theta_i & -s\theta_i c\alpha_i & s\theta_i s\alpha_i & a_i c\theta_i \\ s\theta_i & c\theta_i c\alpha_i & -c\theta_i s\alpha_i & a_i s\theta_i \\ 0 & s\alpha_i & c\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}$

where $c\theta_i = \cos\theta_i$, $s\theta_i = \sin\theta_i$, etc. Every robot link gets one row in a DH table, and one call to this formula.

Consider two links of lengths $l_1$ and $l_2$ in the $x$-$y$ plane, revolute joints rotating about $z$:

Because both $\alpha_i = 0$ and $d_i = 0$, each DH matrix simplifies to:

$T_{i-1}^{i} = \begin{bmatrix} c_i & -s_i & 0 & l_i c_i \\ s_i & c_i & 0 & l_i s_i \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

The FK end-effector position is:

$x = l_1 \cos q_1 + l_2 \cos(q_1+q_2), \qquad y = l_1 \sin q_1 + l_2 \sin(q_1+q_2)$

Verify with $q_1 = q_2 = 0$: both cosines are 1, so $x = l_1 + l_2$, $y = 0$ — the arm stretches along the $x$-axis. Correct.

import numpy as np

def dh_transform(theta, d, a, alpha):
    """Standard DH transform T_{i-1}^{i}."""
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)
    return np.array([
        [ct, -st*ca,  st*sa, a*ct],
        [st,  ct*ca, -ct*sa, a*st],
        [ 0,     sa,     ca,    d],
        [ 0,      0,      0,    1]
    ])

def fk_2link(q1, q2, l1=1.0, l2=0.8):
    """FK for a 2-link planar arm. Returns 4x4 T_0^EE."""
    T01 = dh_transform(theta=q1, d=0, a=l1, alpha=0)
    T12 = dh_transform(theta=q2, d=0, a=l2, alpha=0)
    return T01 @ T12

# Test: straight out along x
T = fk_2link(0, 0)
print("EE position:", T[:3, 3])  # [1.8, 0, 0]

# 90 deg + 45 deg configuration
T = fk_2link(np.pi/2, np.pi/4)
print("EE position:", np.round(T[:3, 3], 4))

The function is trivially extensible to $n$ links by adding rows to a DH table and multiplying in a loop.

From joint angles to end-effector pose:

For a 6-DOF industrial arm (e.g. PUMA-style), the DH table has six rows. FK is still the same chain multiply — just six matrices instead of two. A few engineering tips:

• Table first, code second. Write out the DH table for your robot, check it against the manufacturer's spec, then generate transforms. Bugs in the table are much easier to spot than bugs in matrix code.
• Validate at zero config. With all $q_i = 0$, compute $T_0^n$ analytically and compare to code. Any mismatch points to a sign or axis error.
• Check end-effector reachability. The maximum reach is $\sum_i |a_i| + |d_i|$; if your target is outside this sphere, FK will never put the EE there — IK will fail.

Once FK is verified, you have the foundation for everything else: Jacobians, workspace analysis, and motion planning.