Skip to content
LumenaryAcademy
Motion 70 min

Forward kinematics of robot arms

From joint angles to where the gripper is. Kinematic chains, Denavit–Hartenberg parameters, the product of exponentials, workspaces, and the URDF files that describe real robots.

Code tested · 6AI-draftedAwaiting review by a roboticist

Your guides: Chloe · The Builder, Margaret · The Experimenter, Gideon · The Safety Engineer

MotivationH

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.

DefinitionsH

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 nn one-degree-of-freedom joints has nn degrees of freedom; its joint variables q=(q1,…,qn)\mathbf{q} = (q_1, \dots, q_n) 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 q↦0Tn(q)\mathbf{q} \mapsto {}^{0}T_{n}(\mathbf{q}) from joint variables to the pose of the end-effector frame nn in the base frame 00. If i−1Ti(qi){}^{i-1}T_{i}(q_i) is the pose of link ii's frame in link i−1i-1's frame, then

0Tn(q)=0T1(q1) 1T2(q2)⋯n−1Tn(qn).{}^{0}T_{n}(\mathbf{q}) = {}^{0}T_{1}(q_1)\,{}^{1}T_{2}(q_2)\cdots{}^{n-1}T_{n}(q_n).

Everything else in this lesson is a matter of how to choose the frames and write each i−1Ti{}^{i-1}T_{i} so that the product is easy to set up and hard to get wrong.

Worked examplesH

Planar arms: 2R and 3R

Start in the plane. A 2R arm has a shoulder joint at the origin, a first link of length l1l_1, an elbow joint, and a second link of length l2l_2. Measure q1q_1 from the base's xx axis and q2q_2 relative to the first link. Then the elbow is at l1(cos⁡q1,sin⁡q1)l_1(\cos q_1, \sin q_1), the second link points in direction q1+q2q_1 + q_2, and

x=l1cos⁡q1+l2cos⁡(q1+q2),y=l1sin⁡q1+l2sin⁡(q1+q2).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).
q1q2l1l2(x, y)base
A planar 2R arm. The shoulder angle q1 is measured from the base x axis; the elbow angle q2 is measured from the extension of the first link (dashed). The tip's direction is q1 + q2.

The same answer comes from multiplying planar transforms: rotate by q1q_1, translate by l1l_1 along the new xx, rotate by q2q_2, translate by l2l_2. Writing RR for a rotation and Trx\mathrm{Tr}_x for a translation along xx,

0Ttip=R(q1) Trx(l1) R(q2) Trx(l2),{}^{0}T_{\text{tip}} = R(q_1)\,\mathrm{Tr}_x(l_1)\,R(q_2)\,\mathrm{Tr}_x(l_2),

and multiplying out gives the formulas above, with the tip's direction ϕ=q1+q2\phi = q_1 + q_2 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 ϕ=q1+q2+q3\phi = q_1 + q_2 + q_3, 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.

python
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")
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

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 15°=30°+45°−60°15° = 30° + 45° - 60°.

DefinitionsH

Denavit–Hartenberg parameters

In 3-D, choosing a frame for each link by hand and writing i−1Ti{}^{i-1}T_{i} as a general 4×44\times4 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 ii is attached to the far end of link ii, on the axis of joint i+1i+1.

Definition— Standard Denavit–Hartenberg frames and parameters

Number the joints 1,…,n1, \dots, n. Frame ii is placed as follows.

  1. Its ziz_i axis lies along the axis of joint i+1i+1. (So z0z_0, in the base, lies along joint 1.)
  2. Its xix_i axis lies along the common normal from zi−1z_{i-1} to ziz_i, the line perpendicular to both, pointing away from zi−1z_{i-1}. If the two axes intersect, xix_i is perpendicular to both; if they are parallel, any common normal will do, usually the one that makes di=0d_i = 0.
  3. Its origin is where xix_i meets ziz_i; yiy_i completes a right-handed frame.

The four parameters of link ii are then:

  • θi\theta_i: the angle from xi−1x_{i-1} to xix_i, measured about zi−1z_{i-1} (the joint variable for a revolute joint);
  • did_i: the distance from the origin of frame i−1i-1 to the common normal, measured along zi−1z_{i-1} (the joint variable for a prismatic joint);
  • aia_i: the length of the common normal, the distance from zi−1z_{i-1} to ziz_i measured along xix_i;
  • αi\alpha_i: the angle from zi−1z_{i-1} to ziz_i, measured about xix_i (the twist).

The link transform is

i−1Ti=Rz(θi) Trz(di) Trx(ai) Rx(αi)=(cos⁡θi−sin⁡θicos⁡αisin⁡θisin⁡αiaicos⁡θisin⁡θicos⁡θicos⁡αi−cos⁡θisin⁡αiaisin⁡θi0sin⁡αicos⁡αidi0001).{}^{i-1}T_{i} = R_z(\theta_i)\,\mathrm{Tr}_z(d_i)\,\mathrm{Tr}_x(a_i)\,R_x(\alpha_i) = \begin{pmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i \\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{pmatrix}.
z(i−1)z(i)x(i−1)x(i)a(i)d(i)θ(i)α(i)frame i−1frame i
The four standard DH parameters between joint axes z(i−1) and z(i), drawn schematically. d is measured along z(i−1) from frame i−1 up to the common normal; a is the length of the common normal; θ is the turn from x(i−1) to x(i) about z(i−1); α is the twist from z(i−1) to z(i) about x(i).

Example— The planar 2R arm in DH form

Both joint axes point out of the page, so they are parallel: every αi=0\alpha_i = 0, and choosing the common normals along the links makes every di=0d_i = 0. The table is (θ1,d1,a1,α1)=(q1,0,l1,0)(\theta_1, d_1, a_1, \alpha_1) = (q_1, 0, l_1, 0) and (θ2,d2,a2,α2)=(q2,0,l2,0)(\theta_2, d_2, a_2, \alpha_2) = (q_2, 0, l_2, 0). With α=d=0\alpha = d = 0 the DH matrix reduces to "rotate by θ\theta, then move aa along the new xx", 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 ii sits on the axis of joint ii itself and the link transform is i−1Ti=Rx(αi−1) Trx(ai−1) Rz(θi) Trz(di){}^{i-1}T_{i} = R_x(\alpha_{i-1})\,\mathrm{Tr}_x(a_{i-1})\,R_z(\theta_i)\,\mathrm{Tr}_z(d_i), with the indices of aa and α\alpha 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.

ComputationC

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.

1 waist2 shoulder3 elbow4, 5, 6 wristflange0.4000.4550.4200.0800.035
Lumen-6 at its home configuration (all joint variables zero), seen from the side. The shoulder sits 0.400 m up and 0.025 m forward of the base axis; the upper arm (0.455 m) points straight up; the forearm (0.420 m, raised 0.035 m by the elbow offset) points forward to the wrist centre; the flange is 0.080 m beyond it.

Its standard DH table, with qiq_i the joint variables and θi=qi+offseti\theta_i = q_i + \text{offset}_i, is

Jointdid_i (m)aia_i (m)αi\alpha_iθi\theta_i
1 waist0.4000.025−90°-90°q1q_1
2 shoulder00.45500q2−90°q_2 - 90°
3 elbow00.035−90°-90°q3q_3
4 forearm roll0.420090°90°q4q_4
5 wrist bend00−90°-90°q5q_5
6 flange roll0.080000q6q_6

The −90°-90° offset on joint 2 is there so that q=0q = 0 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 S=(ω,v)\mathcal{S} = (\boldsymbol{\omega}, \mathbf{v}), where ω\boldsymbol{\omega} is the unit direction of the axis and v=−ω×p\mathbf{v} = -\boldsymbol{\omega} \times \mathbf{p} for any point p\mathbf{p} on it. Turning the joint by qq moves everything beyond it by the matrix exponential e[S]qe^{[\mathcal{S}]q}, the SE(3)SE(3) version of Rodrigues' formula.

The product of exponentials (PoE) formula, introduced by Roger Brockett in 1984, says: write down the end-effector pose MM at the home configuration, and the screw axis Si\mathcal{S}_i of each joint at the home configuration, all in the base frame. Then

0Tn(q)=e[S1]q1 e[S2]q2⋯e[Sn]qn M.{}^{0}T_{n}(\mathbf{q}) = e^{[\mathcal{S}_1]q_1}\,e^{[\mathcal{S}_2]q_2}\cdots e^{[\mathcal{S}_n]q_n}\,M.

Why this order? Read it right to left: move joint nn first, while every other joint is still at home, so its axis is still where you wrote it down; then joint n−1n-1, which carries joint nn'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.

python
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}")
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

At home the flange is at (0.525,0,0.89)(0.525, 0, 0.89) m: forward 0.025+0.420+0.0800.025 + 0.420 + 0.080 and up 0.400+0.455+0.0350.400 + 0.455 + 0.035, as the figure says, and its rotation block shows the tool's zz axis (the third column) pointing forward along the base's xx. For the configuration (30°,−20°,45°,10°,60°,−30°)(30°, -20°, 45°, 10°, 60°, -30°) the flange is at (0.2298,0.1465,0.6030)(0.2298, 0.1465, 0.6030) m. The two formulations agree on a thousand random configurations to better than 10−1510^{-15}, rounding error: they are two descriptions of the same function.

ComputationC

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 l1+l2l_1 + l_2 when the arm is straight, the inner radius ∣l1−l2∣|l_1 - l_2| 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.

python
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))
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

The first picture is the annulus from 0.250.25 m to 0.750.75 m, with the hole in the middle that no configuration reaches. Limiting the elbow to ±150°\pm150° means the arm can never fold back fully, so the hole grows to 0.310.31 m; limiting the shoulder to 0°0°–180°180° (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 0.455+0.4202+0.0352≈0.8760.455 + \sqrt{0.420^2 + 0.035^2} \approx 0.876 m; manufacturers publish exactly such side and top views of the workspace on their datasheets.

ExplanationH

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.

python
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))
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

At zero, the tool sits at (0.25,0,0.40)(0.25, 0, 0.40), at the end of the horizontal forearm, and its zz axis points forward, because the fixed tool_mount joint pitches it by 90°90°. With the base turned 90°90° and the shoulder lifted 45°45°, the tool rises to (0,0.1768,0.5768)(0, 0.1768, 0.5768), 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 misconceptionsH

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 θ\theta and α\alpha 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 θ\theta and α\alpha 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

Exercises

1. Forward kinematics of robot arms

Warm-upHuman

A planar 2R arm has l1=0.3l_1 = 0.3 m and l2=0.2l_2 = 0.2 m. With q1=90°q_1 = 90° and q2=−90°q_2 = -90°, where is the tip, in metres? Answer as x, y.

2. Forward kinematics of robot arms

CoreHuman

A planar 2R arm has l1=0.40l_1 = 0.40 m and l2=0.25l_2 = 0.25 m. Its elbow joint is limited to −120°≤q2≤120°-120° \le q_2 \le 120°; 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

CoreHuman

Which product is the link transform i−1Ti{}^{i-1}T_{i} in the standard (distal) Denavit–Hartenberg convention used in this lesson?

Choose one

4. Forward kinematics of robot arms

Warm-upHuman

A 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

CoreHuman

Which of these statements about forward kinematics are true? Select all that apply.

Choose all that apply

6. Forward kinematics of robot arms

CoreComputational

Write planar_tip(lengths, angles) for a planar serial arm with any number of revolute joints. lengths[i] is link ii's length and angles[i] is joint ii'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.

Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

7. Forward kinematics of robot arms

StretchComputational

Write dh_fk(table, q): forward kinematics from a standard DH table. Each row of table is (d, a, alpha, offset), and joint ii's angle is θi=qi+offseti\theta_i = q_i + \text{offset}_i (all joints revolute). Return the 4×44\times4 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.

Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

Further reading and resources