Denavit–Hartenberg and the forward kinematics of the PAROL6

10.1 The problem DH solves

You could attach frames to a robot's links any way you like, and then write out each transform by hand. That works but it is error-prone and it means every robot needs bespoke code.

Denavit and Hartenberg observed that if you place the frames according to a specific set of rules, the transform between consecutive frames always has the same form and always requires exactly four parameters instead of six. That is the idea: a systematic frame placement that reduces each joint to four numbers.

The rules are: put the z-axis of frame $i$ along the axis of joint $i+1$ (so z is always the axis a joint rotates about, or slides along); put the x-axis of frame $i$ along the common perpendicular between $z_{i-1}$ and $z_i$; and y follows from the right-hand rule.

Given that placement, the four parameters are:

• $\theta$ (theta) — the rotation about z that takes $x_{i-1}$ to $x_i$. For a revolute joint, this is the joint variable.
• $d$ — the translation along z from $x_{i-1}$ to $x_i$. The link offset.
• $a$ — the translation along x. The link length, or more precisely the distance between the two z-axes along their common perpendicular.
• $\alpha$ (alpha) — the rotation about x that takes $z_{i-1}$ to $z_i$. The link twist.

And the transform is always:

$$T_i = R_z(\theta) \cdot T_z(d) \cdot T_x(a) \cdot R_x(\alpha)$$

which multiplies out to:

$$T = \begin{bmatrix} \cos\theta & -\sin\theta\cos\alpha & \sin\theta\sin\alpha & a\cos\theta \\ \sin\theta & \cos\theta\cos\alpha & -\cos\theta\sin\alpha & a\sin\theta \\ 0 & \sin\alpha & \cos\alpha & d \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

It is worth being able to recognise this matrix on sight; every DH-based kinematics implementation contains it.

A caution about conventions. There are two DH variants in circulation: "standard" or "distal" DH, which is what I have written above, and "modified" or "proximal" DH, popularised by Craig, which orders the four elementary transforms differently and attaches frames to the proximal rather than distal end of each link. They are not interchangeable, and a table for one convention plugged into code for the other gives wrong answers that look plausible. Establish which convention a table uses before you use it. The one below is standard DH.

10.2 The PAROL6 DH table

The published link dimensions are:

Symbol	Value (mm)	What it is
$a_1$	110.50	Base height to shoulder axis
$a_2$	23.42	Lateral offset at the shoulder
$a_3$	180.00	Upper arm length
$a_4$	43.50	Elbow offset
$a_5$	176.35	Forearm length
$a_6$	62.80	Wrist to flange along the tool axis
$a_7$	45.25	Flange lateral offset

And the DH table, in standard convention:

i	Link	$\theta$	$\alpha$	$d$ (mm)	$a$ (mm)
1	Base	$\theta_1$	$-\pi/2$	110.50	23.42
2	Shoulder	$\theta_2 - \pi/2$	$\pi$	0	180.00
3	Elbow	$\theta_3 + \pi$	$\pi/2$	0	−43.50
4	Wrist 1	$\theta_4$	$-\pi/2$	−176.35	0
5	Wrist 2	$\theta_5$	$\pi/2$	0	0
6	Wrist 3	$\theta_6 + \pi$	$\pi$	−62.80	−45.25

The $\theta$ column contains the joint variable plus a constant offset. Those offsets exist because the DH frame placement rules do not generally put the frames where a human would want "zero" to be. They are a bookkeeping device that lets the robot's zero position be something sensible while the maths stays in DH form.

10.3 Forward kinematics

Forward kinematics is now trivial to state: given the six joint angles, build the six transforms and multiply them.

$$T_0^6 = T_1(\theta_1) \cdot T_2(\theta_2) \cdot T_3(\theta_3) \cdot T_4(\theta_4) \cdot T_5(\theta_5) \cdot T_6(\theta_6)$$

The result is a 4×4 matrix whose top-right 3×1 block is the flange position in base coordinates and whose top-left 3×3 block is the flange orientation.

In code this is about twenty lines:

import numpy as np

a1, a2, a3, a4, a5, a6, a7 = 110.50, 23.42, 180.00, 43.50, 176.35, 62.80, 45.25
pi = np.pi

# (theta_offset, alpha, d, a)
DH = [(0.0,   -pi/2,  a1,  a2),
      (-pi/2,  pi,    0.0, a3),
      (pi,     pi/2,  0.0, -a4),
      (0.0,   -pi/2, -a5,  0.0),
      (0.0,    pi/2,  0.0, 0.0),
      (pi,     pi,   -a6, -a7)]

def dh_transform(theta, alpha, d, a):
    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 forward_kinematics(q):
    """q: six joint angles in radians. Returns 4x4 base-to-flange transform."""
    T = np.eye(4)
    for i, (off, alpha, d, a) in enumerate(DH):
        T = T @ dh_transform(q[i] + off, alpha, d, a)
    return T

Running it with all joints at zero places the flange at approximately (262.6, 0.0, 288.7) mm, and with the arm extended it reaches about 396 mm from the base axis, which agrees with the published 400 mm figure and suggests the table is transcribed correctly.

10.4 Verifying your implementation

Three checks catch almost every error in kinematics code.

Check the reach. Extend the arm as far as it will go and confirm the flange distance from the base matches the specification. If your link lengths or your α signs are wrong, the reach will be visibly wrong.

Check the wrist intersection. The PAROL6 has a spherical wrist, meaning the axes of joints 4, 5, and 6 all pass through a common point. If you compute the origins of frames 4 and 5 for arbitrary joint angles, they should be identical. I have verified this holds for the table above. If it does not hold in your implementation, you have a transcription error, and the closed-form IK in the next chapter will not work.

Check the round trip. Pick random joint angles, run forward kinematics to get a pose, run inverse kinematics on that pose, and run forward kinematics again on the result. The two poses must match to within numerical noise. The joint angles need not match, since there are multiple solutions, but the poses must. Do this for a few thousand random configurations; it is the test worth writing first.

10.5 The tool frame

Everything above computes the pose of the flange, the mounting face at the end of the arm. What you usually want is the tool centre point, somewhere out in front of the gripper.

Handle this with one more transform. Define $T_{\text{flange}}^{\text{tool}}$ describing where the TCP sits relative to the flange, and then:

$$T_{\text{base}}^{\text{tool}} = T_{\text{base}}^{\text{flange}} \cdot T_{\text{flange}}^{\text{tool}}$$

Keep this separate from the DH chain, so that changing grippers changes one matrix and nothing else. Baking the tool offset into the DH table costs you that the first time you swap end effectors.

Similarly, if the robot is bolted to a table and you want to work in table coordinates, define $T_{\text{world}}^{\text{base}}$ and pre-multiply. The full chain is then world → base → flange → tool, and each piece has a clear physical meaning and a clear owner.