Where is the gripper?
A robot arm in a parcel depot in Rotterdam must pick a box from a moving belt. Its controller knows, from an encoder on every motor, exactly how far each of its six joints has turned. What it needs to know is where its gripper is, and which way it is pointing, in the frame of the conveyor. Turning joint angles into the pose of the end of the arm is forward kinematics (FK).
It is the most basic computation an arm performs, run thousands of times a second. The controller uses it to check where it is going; the safety system uses it to keep the arm inside its allowed zone; the motion planner uses it, millions of times per plan, to check whether a candidate motion would hit anything; and the vision system uses it to know where the wrist camera is when it takes a picture. Forward kinematics is also the easy direction: for every set of joint angles there is exactly one pose. The reverse question, which joint angles put the gripper at a desired pose, may have no answer or several, and is the subject of Inverse kinematics.
This lesson builds FK from the transforms of Frames and rigid transforms and the rotations of Rotations in 3-D and quaternions: an arm is a chain of frames, and FK is the product of the transforms along it.
Links, joints and kinematic chains
Definition— Link, joint, kinematic chain
A link is a rigid body. A joint connects two links and allows a particular relative motion: a revolute joint (R) allows rotation about an axis, and a prismatic joint (P) allows sliding along an axis; each has one degree of freedom, described by one joint variable (an angle or a distance). A kinematic chain is a set of links connected by joints. In a serial (open) chain each link is connected to the next, from a fixed base to a free end, the end effector.
Most industrial arms are serial chains of six revolute joints, written 6R. A SCARA arm, the fast arm that places components on circuit boards, is RRPR. Other joints, such as a ball joint (spherical, three degrees of freedom) or a universal joint (two), can always be modelled as several one-degree-of-freedom joints with coincident axes, so the theory needs only R and P. Chains can also branch (a humanoid's torso carries two arms, two legs and a head: a tree, which is why tf2 keeps a tree of frames) or close into loops (a parallel robot, such as the delta robots that pick chocolates from conveyors at high speed, has several chains joining the base to one moving platform; its kinematics behave very differently, and we leave them aside).
A serial arm with one-degree-of-freedom joints has degrees of freedom; its joint variables form its configuration, and the set of all configurations is its configuration space, studied in Configuration space. Since a pose in space has six degrees of freedom, six joints is the least that can reach a general position and orientation; arms with seven, such as many collaborative and research arms, have one to spare and can move their elbow without moving the gripper.
Definition— Forward kinematics
The forward kinematics of a serial arm is the map from joint variables to the pose of the end-effector frame in the base frame . If is the pose of link 's frame in link 's frame, then
Everything else in this lesson is a matter of how to choose the frames and write each so that the product is easy to set up and hard to get wrong.
Planar arms: 2R and 3R
Start in the plane. A 2R arm has a shoulder joint at the origin, a first link of length , an elbow joint, and a second link of length . Measure from the base's axis and relative to the first link. Then the elbow is at , the second link points in direction , and
The same answer comes from multiplying planar transforms: rotate by , translate by along the new , rotate by , translate by . Writing for a rotation and for a translation along ,
and multiplying out gives the formulas above, with the tip's direction in the rotation block. A 3R arm adds a wrist joint and a short third link; now the tip has a position and an orientation , three numbers for three joints, so a planar 3R arm can place a tool at any position in its workspace with any chosen orientation (within limits). The program computes both arms both ways.
import numpy as np
def se2(x, y, theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c, -s, x], [s, c, y], [0, 0, 1.0]])
def planar_fk(lengths, angles):
"""Pose of every joint and of the tip of a planar serial arm, as 3x3 transforms in the base frame.
Joint i turns by angles[i] relative to the previous link; link i has length lengths[i]."""
T = np.eye(3)
frames = [T]
for l, q in zip(lengths, angles):
T = T @ se2(0, 0, q) @ se2(l, 0, 0) # rotate at the joint, then move along the link
frames.append(T)
return frames
l1, l2 = 0.5, 0.4
q1, q2 = np.radians(30), np.radians(45)
tip = planar_fk([l1, l2], [q1, q2])[-1]
closed = (l1 * np.cos(q1) + l2 * np.cos(q1 + q2), l1 * np.sin(q1) + l2 * np.sin(q1 + q2))
print("2R arm, q = (30, 45) deg:")
print(" tip from the chain of transforms:", tip[:2, 2].round(4))
print(" tip from the closed form: ", np.round(closed, 4))
# A 3R arm: the third joint is a wrist, and the tip now has an orientation phi = q1 + q2 + q3.
lengths = [0.5, 0.4, 0.1]
angles = np.radians([30, 45, -60])
frames = planar_fk(lengths, angles)
print("3R arm, q = (30, 45, -60) deg:")
for name, T in zip(["base", "elbow", "wrist", "tip"], frames):
print(f" {name:6} at ({T[0, 2]:7.4f}, {T[1, 2]:7.4f}) pointing {np.degrees(np.arctan2(T[1, 0], T[0, 0])):6.1f} deg")The closed form and the chain agree. For the 3R arm the program prints every intermediate frame, which is what a real controller also computes, because the elbow's and wrist's positions matter for collision checking, not only the tip's. The tip points at .
Denavit–Hartenberg parameters
In 3-D, choosing a frame for each link by hand and writing as a general matrix would need six numbers per link. In 1955 Jacques Denavit and Richard Hartenberg showed that by placing the frames cleverly, four numbers per link suffice, one of which is the joint variable. Their convention became the lingua franca of arm kinematics, and every robotics engineer still meets it on datasheets and in old code.
This lesson uses the standard (or distal) Denavit–Hartenberg (DH) convention, in which frame is attached to the far end of link , on the axis of joint .
Definition— Standard Denavit–Hartenberg frames and parameters
Number the joints . Frame is placed as follows.
- Its axis lies along the axis of joint . (So , in the base, lies along joint 1.)
- Its axis lies along the common normal from to , the line perpendicular to both, pointing away from . If the two axes intersect, is perpendicular to both; if they are parallel, any common normal will do, usually the one that makes .
- Its origin is where meets ; completes a right-handed frame.
The four parameters of link are then:
- : the angle from to , measured about (the joint variable for a revolute joint);
- : the distance from the origin of frame to the common normal, measured along (the joint variable for a prismatic joint);
- : the length of the common normal, the distance from to measured along ;
- : the angle from to , measured about (the twist).
The link transform is
Example— The planar 2R arm in DH form
Both joint axes point out of the page, so they are parallel: every , and choosing the common normals along the links makes every . The table is and . With the DH matrix reduces to "rotate by , then move along the new ", which is exactly the chain of the previous section.
Note
Standard or modified? John Craig's widely used textbook (1986) introduced a modified (proximal) DH convention, in which frame sits on the axis of joint itself and the link transform is , with the indices of and shifted by one. Both conventions are correct and both are in daily use; a table written for one gives nonsense in the other. A DH table you did not write yourself should always say which convention it uses. If it does not, test it against a configuration whose pose you know, such as the home position.
A 6-axis arm, two ways
Most industrial arms share one layout: a waist joint about the vertical axis, a shoulder and an elbow with parallel horizontal axes (together placing the wrist), and a spherical wrist of three joints whose axes meet at one point (together orienting the tool). We will use a made-up arm, Lumen-6, sized like a medium industrial arm with a reach of about 0.9 m.
Its standard DH table, with the joint variables and , is
| Joint | (m) | (m) | ||
|---|---|---|---|---|
| 1 waist | 0.400 | 0.025 | ||
| 2 shoulder | 0 | 0.455 | ||
| 3 elbow | 0 | 0.035 | ||
| 4 forearm roll | 0.420 | 0 | ||
| 5 wrist bend | 0 | 0 | ||
| 6 flange roll | 0.080 | 0 |
The offset on joint 2 is there so that is the comfortable "L" pose in the figure; manufacturers choose such offsets so that the zero position is easy to recognise and to calibrate.
The product of exponentials
There is a second way to write FK that needs no link frames at all. By Euler's and Chasles's theorems, every rigid motion is a screw motion: a rotation about some line combined with a translation along it. A revolute joint's motion is a pure rotation about its axis, described by a screw axis , where is the unit direction of the axis and for any point on it. Turning the joint by moves everything beyond it by the matrix exponential , the version of Rodrigues' formula.
The product of exponentials (PoE) formula, introduced by Roger Brockett in 1984, says: write down the end-effector pose at the home configuration, and the screw axis of each joint at the home configuration, all in the base frame. Then
Why this order? Read it right to left: move joint first, while every other joint is still at home, so its axis is still where you wrote it down; then joint , which carries joint 's already-moved links with it; and so on down to the base. Each axis is used only while it is still in its home position.
PoE needs just a home pose and one axis per joint, read straight off a drawing, with no rules about where frames go and no singular cases for parallel axes. It is the formulation used by Modern Robotics and much recent research, and it connects FK to the Lie-group tools of the previous lesson. The program builds Lumen-6 from its DH table, reads the screw axes off the DH frames at home, and checks that the two formulations agree.
import numpy as np
def dh(theta, d, a, alpha):
"""Standard (distal) Denavit-Hartenberg link transform: Rot_z(theta) Trans_z(d) Trans_x(a) Rot_x(alpha)."""
ct, st, ca, sa = np.cos(theta), np.sin(theta), 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.0]])
# "Lumen-6": a made-up 6-axis arm with a spherical wrist, sized like a mid-sized industrial arm.
# d (m) a (m) alpha (rad) theta offset (rad)
DH_TABLE = [(0.400, 0.025, -np.pi / 2, 0.0), # 1 waist
(0.0, 0.455, 0.0, -np.pi / 2), # 2 shoulder (offset: upper arm points up at q = 0)
(0.0, 0.035, -np.pi / 2, 0.0), # 3 elbow
(0.420, 0.0, np.pi / 2, 0.0), # 4 forearm roll
(0.0, 0.0, -np.pi / 2, 0.0), # 5 wrist bend
(0.080, 0.0, 0.0, 0.0)] # 6 flange roll
def fk_dh(q):
T = np.eye(4)
for (d, a, alpha, offset), qi in zip(DH_TABLE, q):
T = T @ dh(qi + offset, d, a, alpha)
return T
print("home pose, q = 0:\n", fk_dh(np.zeros(6)).round(4) + 0.0) # + 0.0 tidies -0.0
q = np.radians([30, -20, 45, 10, 60, -30])
T = fk_dh(q)
print("q = (30, -20, 45, 10, 60, -30) deg: flange at", T[:3, 3].round(4), "m")
# The same arm as a product of exponentials: home pose M and one screw axis per joint.
def skew(w):
return np.array([[0, -w[2], w[1]], [w[2], 0, -w[0]], [-w[1], w[0], 0]])
def exp_twist(w, v, theta):
"""exp([S] theta) for a revolute screw axis S = (w, v) with |w| = 1."""
W = skew(w)
R = np.eye(3) + np.sin(theta) * W + (1 - np.cos(theta)) * W @ W
p = (np.eye(3) * theta + (1 - np.cos(theta)) * W + (theta - np.sin(theta)) * W @ W) @ v
T = np.eye(4)
T[:3, :3], T[:3, 3] = R, p
return T
# Read the screw axes off the home configuration: joint i turns about the z axis of DH frame i-1.
frames, T = [np.eye(4)], np.eye(4)
for (d, a, alpha, offset) in DH_TABLE:
T = T @ dh(offset, d, a, alpha)
frames.append(T)
M = frames[-1]
screws = [(F[:3, 2], -np.cross(F[:3, 2], F[:3, 3])) for F in frames[:6]] # (w, v = -w x point on axis)
def fk_poe(q):
T = np.eye(4)
for (w, v), qi in zip(screws, q):
T = T @ exp_twist(w, v, qi)
return T @ M
rng = np.random.default_rng(0)
worst = max(np.abs(fk_poe(qq) - fk_dh(qq)).max() for qq in rng.uniform(-np.pi, np.pi, (1000, 6)))
print("DH and product of exponentials agree on 1000 random configurations; worst difference", f"{worst:.1e}")At home the flange is at m: forward and up , as the figure says, and its rotation block shows the tool's axis (the third column) pointing forward along the base's . For the configuration the flange is at m. The two formulations agree on a thousand random configurations to better than , rounding error: they are two descriptions of the same function.
Workspace
The workspace of an arm is the set of positions its end effector can reach. (The dexterous workspace is the smaller set it can reach with every orientation.) For a 2R arm without joint limits it is an annulus: the outer radius when the arm is straight, the inner radius when it is folded back. Joint limits, which every real arm has to keep cables and links from colliding, carve it further. Workspaces rarely have tidy formulas, so engineers sample them: draw many random configurations within the limits, compute FK for each, and look at where the points land.
import numpy as np
rng = np.random.default_rng(1)
l1, l2 = 0.5, 0.25
def ascii_plot(xs, ys, x_range, y_range, cols=49, rows=25):
grid = [[" "] * cols for _ in range(rows)]
for x, y in zip(xs, ys):
c = int((x - x_range[0]) / (x_range[1] - x_range[0]) * (cols - 1) + 0.5)
r = int((y_range[1] - y) / (y_range[1] - y_range[0]) * (rows - 1) + 0.5)
if 0 <= r < rows and 0 <= c < cols:
grid[r][c] = "#"
mid_r = int(y_range[1] / (y_range[1] - y_range[0]) * (rows - 1) + 0.5)
mid_c = int(-x_range[0] / (x_range[1] - x_range[0]) * (cols - 1) + 0.5)
grid[mid_r][mid_c] = "O" # the base joint
print("\n".join("".join(row).rstrip() for row in grid))
for title, lim1, lim2 in [("no joint limits", (-180, 180), (-180, 180)),
("shoulder 0..180 deg, elbow -150..150 deg", (0, 180), (-150, 150))]:
q1 = np.radians(rng.uniform(*lim1, 20000))
q2 = np.radians(rng.uniform(*lim2, 20000))
x = l1 * np.cos(q1) + l2 * np.cos(q1 + q2)
y = l1 * np.sin(q1) + l2 * np.sin(q1 + q2)
reach = np.hypot(x, y)
print(f"\n2R arm, l1 = {l1}, l2 = {l2}, {title}: reach from {reach.min():.3f} to {reach.max():.3f} m")
ascii_plot(x, y, (-0.8, 0.8), (-0.8, 0.8))The first picture is the annulus from m to m, with the hole in the middle that no configuration reaches. Limiting the elbow to means the arm can never fold back fully, so the hole grows to m; limiting the shoulder to – (an arm mounted on a table, never pointing down through it) removes most of the lower half, leaving only two lobes that the forearm reaches by folding down past the horizontal. The same sampling, done in 3-D for Lumen-6, gives a thick spherical shell around the shoulder whose outer radius for the wrist centre is m; manufacturers publish exactly such side and top views of the workspace on their datasheets.
URDF: how real robots are described
DH tables are compact but awkward for real robots, which have cameras, grippers, wheels and branches, and whose links need shapes and masses for simulation and collision checking. The ROS ecosystem describes robots with URDF, the Unified Robot Description Format: an XML file listing <link> elements (each with optional visual geometry, collision geometry and inertia) and <joint> elements. Each joint names a parent and a child link, a type (revolute, continuous, prismatic, fixed, and others), an <origin> giving the child's frame relative to the parent's when the joint is at zero, as a translation xyz and fixed-axis roll–pitch–yaw rpy, and an <axis> in the child's frame for the motion, plus limits on position, velocity and effort.
FK from a URDF is therefore a PoE-like product with a twist of its own: for each joint, multiply by the fixed origin transform and then by the joint's motion about its axis. The program parses a small URDF with Python's standard XML library and computes the pose of every link.
import xml.etree.ElementTree as ET
import numpy as np
URDF = """
<robot name="tiny_arm">
<link name="base_link"/>
<link name="upper_arm"/>
<link name="forearm"/>
<link name="tool"/>
<joint name="shoulder_pan" type="revolute">
<parent link="base_link"/> <child link="upper_arm"/>
<origin xyz="0 0 0.10" rpy="0 0 0"/> <axis xyz="0 0 1"/>
<limit lower="-3.14" upper="3.14" effort="5" velocity="1.0"/>
</joint>
<joint name="shoulder_lift" type="revolute">
<parent link="upper_arm"/> <child link="forearm"/>
<origin xyz="0 0 0.30" rpy="0 0 0"/> <axis xyz="0 1 0"/>
<limit lower="-1.57" upper="1.57" effort="5" velocity="1.0"/>
</joint>
<joint name="tool_mount" type="fixed">
<parent link="forearm"/> <child link="tool"/>
<origin xyz="0.25 0 0" rpy="0 1.5708 0"/>
</joint>
</robot>
"""
def rpy_matrix(r, p, y):
"""URDF's rpy: roll about x, then pitch about y, then yaw about z, all fixed axes."""
cr, sr, cp, sp, cy, sy = np.cos(r), np.sin(r), np.cos(p), np.sin(p), np.cos(y), np.sin(y)
Rx = np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]])
Ry = np.array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]])
Rz = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]])
return Rz @ Ry @ Rx
def axis_angle(axis, angle):
k = np.asarray(axis, float) / np.linalg.norm(axis)
K = np.array([[0, -k[2], k[1]], [k[2], 0, -k[0]], [-k[1], k[0], 0]])
return np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * K @ K
def transform(R=np.eye(3), p=(0, 0, 0)):
T = np.eye(4)
T[:3, :3], T[:3, 3] = R, p
return T
def floats(text):
return [float(v) for v in text.split()]
def fk_urdf(urdf, joint_values):
"""Pose of every link in the root link's frame: walk the tree from the root."""
joints = ET.fromstring(urdf).findall("joint")
children = {j.find("child").get("link") for j in joints}
root = next(l.get("name") for l in ET.fromstring(urdf).findall("link") if l.get("name") not in children)
poses, frontier = {root: np.eye(4)}, [root]
while frontier:
parent = frontier.pop()
for j in joints:
if j.find("parent").get("link") != parent:
continue
o = j.find("origin")
T_origin = transform(rpy_matrix(*floats(o.get("rpy"))), floats(o.get("xyz")))
q = joint_values.get(j.get("name"), 0.0)
if j.get("type") in ("revolute", "continuous"):
T_motion = transform(axis_angle(floats(j.find("axis").get("xyz")), q))
elif j.get("type") == "prismatic":
T_motion = transform(p=q * np.asarray(floats(j.find("axis").get("xyz"))))
else:
T_motion = np.eye(4) # fixed joint
child = j.find("child").get("link")
poses[child] = poses[parent] @ T_origin @ T_motion
frontier.append(child)
return poses
for q in [{"shoulder_pan": 0.0, "shoulder_lift": 0.0},
{"shoulder_pan": np.pi / 2, "shoulder_lift": -np.pi / 4}]:
poses = fk_urdf(URDF, q)
tool = poses["tool"]
print({k: round(float(v), 4) for k, v in q.items()})
print(" tool at", (tool[:3, 3].round(4) + 0.0), " tool z axis points along", (tool[:3, 2].round(4) + 0.0))At zero, the tool sits at , at the end of the horizontal forearm, and its axis points forward, because the fixed tool_mount joint pitches it by . With the base turned and the shoulder lifted , the tool rises to , pointing up and to the left. This walk over the tree is what ROS's robot_state_publisher does continuously: it reads the URDF once, receives joint angles on the /joint_states topic, and broadcasts every link's transform to tf2, so that any node can ask where the gripper or the wrist camera is.
In practice URDFs are generated from xacro macros (so that a left and a right arm share one definition) or exported from CAD, and they are usually published by the manufacturer. Other formats describe the same information for other tools: SDF for the Gazebo simulator, MJCF for MuJoCo, and USD for NVIDIA's Isaac tools. Libraries such as Pinocchio, KDL, Drake and MoveIt load these files and provide FK, Jacobians and dynamics; the Robotics Toolbox for Python is a convenient way to check your own implementations against a reference.
Note
Real arms are not their drawings. An industrial arm's links and joints differ from the nominal model by fractions of a millimetre and hundredths of a degree, and those small errors are multiplied by the arm's length. Typical arms repeat a taught position to a few hundredths of a millimetre, but place the tool at a computed coordinate far less accurately unless their kinematic parameters are calibrated by measuring the flange at many configurations, for example with a laser tracker. And industrial arms are fast and strong enough to injure people: they work behind fences or with certified safety sensors, under standards such as ISO 10218, and only collaborative arms with force and speed limits work beside people. To experiment at home or in class, use simulation or a small, low-power educational arm.
Common misconceptions
Misconception
"A DH table is a description of the robot." It is a description of the robot together with a choice of frames. The same arm has many valid DH tables (different frame choices, standard or modified convention, different zero offsets), and two correct tables can look completely different. Compare tables by computing FK at a few configurations, never by comparing entries.
Misconception
"The workspace is a sphere of radius equal to the sum of the link lengths." That is only its outer boundary, without limits. Inner holes, joint limits, self-collision and the floor all remove regions, and the dexterous workspace, where every orientation is possible, is smaller still.
Misconception
"Forward and inverse kinematics are equally hard." FK is a product of matrices with exactly one answer. IK, solving that product for the joint angles, can have no solution (out of reach), a finite number (a typical 6-axis arm has up to eight), or infinitely many (a 7-axis arm). That asymmetry is why the next lesson exists.
Misconception
"URDF's rpy and DH's and are the same kind of angle." URDF's rpy sets a fixed offset between frames using roll, pitch and yaw about fixed axes; DH's and are rotations about specific axes chosen by the DH rules. A URDF joint can sit at any position and orientation relative to its parent; a DH frame cannot.
Exercises
1. Forward kinematics of robot arms
Warm-upHumanA planar 2R arm has m and m. With and , where is the tip, in metres? Answer as x, y.
2. Forward kinematics of robot arms
CoreHumanA planar 2R arm has m and m. Its elbow joint is limited to ; the shoulder is unlimited. What is the smallest distance, in millimetres, from the shoulder at which the tip can be?
3. Forward kinematics of robot arms
CoreHumanWhich product is the link transform in the standard (distal) Denavit–Hartenberg convention used in this lesson?
4. Forward kinematics of robot arms
Warm-upHumanA 6R arm is described by a standard DH table. Not counting the six joint variables, how many constant numbers does the table contain (including zeros, and ignoring any joint offsets)?
5. Forward kinematics of robot arms
CoreHumanWhich of these statements about forward kinematics are true? Select all that apply.
6. Forward kinematics of robot arms
CoreComputationalWrite planar_tip(lengths, angles) for a planar serial arm with any number of revolute joints. lengths[i] is link 's length and angles[i] is joint 's angle relative to the previous link (radians). Return the tip's pose (x, y, phi), where phi is the direction the last link points. Do it without building matrices: keep a running direction.
7. Forward kinematics of robot arms
StretchComputationalWrite dh_fk(table, q): forward kinematics from a standard DH table. Each row of table is (d, a, alpha, offset), and joint 's angle is (all joints revolute). Return the pose of the last frame in the base frame. The tests check a planar 2R arm, Lumen-6's home pose and the lesson's example configuration, and random tables against the four elementary transforms multiplied in the standard order.
Further reading and resources
- A kinematic notation for lower-pair mechanisms based on matrices · Jacques Denavit and Richard S. Hartenberg · 1955The original paper of the Denavit–Hartenberg convention.
- Modern Robotics: Mechanics, Planning, and Control · Kevin M. Lynch and Frank C. Park · 2017Chapter 4 develops forward kinematics with the product of exponentials, and its appendix compares it with Denavit–Hartenberg.
- Robotic manipulators and the product of exponentials formula · Roger W. Brockett · 1984Published in the proceedings of the 1984 Mathematical Theory of Networks and Systems symposium; the origin of the product-of-exponentials formulation.
- URDF specification: joint elementThe fields of a URDF joint (origin, axis, limits), as defined by the ROS project.
- ROS 2 documentation: URDF tutorials
- PinocchioA fast open-source C++ and Python library for the kinematics and dynamics of robots described by URDF.
- Robotics Toolbox for Python · Peter Corke and contributorsOpen-source Python tools for DH and URDF robots, good for checking your own code.