Split View: 순기구학 — 관절 각도에서 손끝 위치로, 동차변환행렬과 DH 파라미터

Forward Kinematics: From Joint Angles to End-Effector Position with Homogeneous Transforms and DH Parameters

• Introduction — A Function That Tells You Where the End Effector Is, Given the Angles
• Coordinate Frames, Rotation Matrices, and Translation
• Why the Homogeneous Transformation Matrix Is 4x4
• Why Chaining Frames Becomes Multiplication
• Hand-Deriving Forward Kinematics for a 2-Link Arm
• DH Parameters — Writing a Link With Four Numbers
• The Tradeoffs of Rotation Representations — Euler Angles, Rotation Matrices, Quaternions
• What Actually Happens When One Sign Is Wrong
• Conclusion — Writing Down the Coordinate Frames Precisely Is Half the Job

Introduction — A Function That Tells You Where the End Effector Is, Given the Angles

Say you've settled on the robot arm's structure and mounted the servos. Now you can read each joint's angle: shoulder 30 degrees, elbow 45 degrees.

So where is the end effector?

Answering that question is forward kinematics, and it's the starting point of every calculation done on a robot arm. Inverse kinematics, trajectory planning, collision checking, translating an object a camera sees into the arm's coordinates — all of it is built on top of this one calculation.

Fortunately, forward kinematics is easy. Feed in angles, and exactly one position comes out, every single time, and it's done in a handful of multiplications. The principle is the same whether there are six joints or ten.

The problem is that as the number of links grows, the trigonometric expressions become unmanageably long. A 2-link arm you can write by hand, and a 3-link arm is still just about manageable, but a 6-link arm with axes pointing in different directions in 3D space is not something you can write by hand. So you need a notation a human can actually use, and that's the homogeneous transformation matrix and DH parameters.

This post explains, with real numbers, why these two tools take the shape they do.

Coordinate Frames, Rotation Matrices, and Translation

In a robot arm, a coordinate frame gets attached to each link: one at the shoulder, one at the upper arm, one at the forearm, one at the end effector. Each frame is fixed to its own link and moves along with it.

The reason for doing this is that everything inside a single link is constant. Where the end effector sits relative to the wrist's frame never changes no matter how the joints move. What changes is only the relationship between the frames. So the problem shrinks from "a complicated shape" down to "a handful of transforms between frames."

The relationship between two frames comes down to two things: how much rotation, and how much offset.

Rotation is a matrix. In 2D, a counterclockwise rotation by θ is:

R(θ) = [ cos θ   -sin θ ]
       [ sin θ    cos θ ]

In 3D, rotation about the z-axis is:

Rz(θ) = [ cos θ   -sin θ   0 ]
        [ sin θ    cos θ   0 ]
        [   0        0     1 ]

Reading this matrix one column at a time makes its meaning clear. The first column is the direction the rotated frame's x-axis points in, expressed in the original frame; the second column is the y-axis; the third is the z-axis. A rotation matrix is simply the three axis directions written out side by side.

Translation is vector addition. If a frame has shifted by t, you add t to a point's coordinates.

Here's an inconvenience: rotation is multiplication, and translation is addition. Write them together and things get messy once you chain multiple steps:

p_world = R · p_local + t

This form gets ugly once you chain several stages. Chain three:

p = R1·(R2·(R3·p + t3) + t2) + t1
  = R1·R2·R3·p + R1·R2·t3 + R1·t2 + t1

With six joints, the number of terms grows to six, and each term carries a product of every preceding rotation. Hard to work with by hand and awkward to translate into code.

Why the Homogeneous Transformation Matrix Is 4x4

The fix turns out to be surprisingly simple. Append a single 1 to the coordinate.

Write the 3D point (x, y, z) as (x, y, z, 1). Then build a 4x4 matrix like this:

T = [ R11  R12  R13  tx ]
    [ R21  R22  R23  ty ]
    [ R31  R32  R33  tz ]
    [  0    0    0    1 ]

The upper-left 3x3 is the rotation matrix, the upper-right 3x1 is the translation, and the bottom row is always 0 0 0 1.

Let's compute just the first row of what happens when you multiply this matrix by (x, y, z, 1).

R11·x + R12·y + R13·z + tx·1

The first row of the rotation, plus the first component of the translation. Exactly the first component of R·p + t. And the last row works out to 0·x + 0·y + 0·z + 1·1 = 1, so the 1 you appended stays intact.

Because the thing being multiplied has a constant 1 built into it, that column's contribution behaves like addition. That's the whole trick behind the 4x4. The 0 0 0 1 in the last row is just the device that keeps the result a valid point.

Let's confirm with numbers. Frame 2 is rotated 45 degrees about the z-axis relative to frame 1, and offset 0.20 meters along x. In frame 2, the end effector sits 0.15 meters along its own x-axis.

import numpy as np
np.set_printoptions(precision=6, suppress=True)


def transform_z(theta, tx):
    """Homogeneous 4x4 transform: rotate by theta about z, then translate by tx along x."""
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, 0, tx],
                     [s,  c, 0, 0],
                     [0,  0, 1, 0],
                     [0,  0, 0, 1]])


T = transform_z(np.radians(45), 0.20)
print("T (frame 2 -> frame 1):")
print(T)

p_local = np.array([0.15, 0.0, 0.0, 1.0])     # end effector as seen from frame 2
p_world = T @ p_local
print("End effector in frame 2:", p_local[:3])
print("End effector in frame 1:", np.round(p_world[:3], 6))
print("Hand-computed value    :", np.round([0.20 + 0.15*np.cos(np.radians(45)),
                                             0.15*np.sin(np.radians(45)), 0.0], 6))

Here's the output.

T (frame 2 -> frame 1):
[[ 0.707107 -0.707107  0.        0.2     ]
 [ 0.707107  0.707107  0.        0.      ]
 [ 0.        0.        1.        0.      ]
 [ 0.        0.        0.        1.      ]]
End effector in frame 2: [0.15 0.   0.  ]
End effector in frame 1: [0.306066 0.106066 0.      ]
Hand-computed value    : [0.306066 0.106066 0.      ]

By hand:

x = 0.20 + 0.15 × cos(45°) = 0.20 + 0.106066 = 0.306066
y =  0   + 0.15 × sin(45°) =  0   + 0.106066 = 0.106066

Matches what the matrix gave us.

A homogeneous transformation matrix has one more convenient property: its inverse is cheap. A general 4x4 matrix's inverse is expensive to compute, but a homogeneous transform's structure lets you write it as:

T⁻¹ = [ Rᵀ   -Rᵀ·t ]
      [ 0      1   ]

Since a rotation matrix's inverse is its transpose (because it's orthogonal), this only takes a handful of multiplications. That means going from the end-effector frame's view of an object back to the base frame — and back again — are both cheap, and that's exactly the calculation a wrist-mounted camera does every single frame.

Why Chaining Frames Becomes Multiplication

Now it's clear why this representation fits a robot arm so well.

Call the transform from base to link 1 T01, and from link 1 to link 2 T12. To move a point p in link 2's frame into the base frame:

p_1 = T12 · p
p_0 = T01 · p_1 = T01 · T12 · p

Chaining transforms together is exactly the same as multiplying matrices. The messy expansion from earlier disappears.

With six joints:

T06 = T01 · T12 · T23 · T34 · T45 · T56

That's the whole of forward kinematics. Each transform carries one joint angle, and multiplying them all out gives the end effector's position and orientation. Position is the resulting matrix's upper-right 3x1, and orientation is the upper-left 3x3.

You have to watch the order of multiplication. Matrix multiplication doesn't commute, so T01·T12 and T12·T01 are different. The trick for keeping the order straight is to look at the subscripts: the inner ones have to match up. In T01·T12, the two 1s meet in the middle, leaving 0 and 2, so the result is T02.

Hand-Deriving Forward Kinematics for a 2-Link Arm

Before reaching for matrices, working out the simplest case with trigonometry gives you something to check against later.

Two links lie in a plane. Upper arm L1 = 0.20 m, forearm L2 = 0.15 m. Shoulder angle θ1 is an absolute angle measured from the x-axis, and elbow angle θ2 is a relative angle measured against the upper arm.

Start with the elbow's position. Go L1 from the shoulder in direction θ1:

elbow = (L1·cos θ1, L1·sin θ1)

The end effector goes another L2 from the elbow. The forearm's absolute direction here is θ1 + θ2. This one line, converting a relative angle into an absolute angle, is the key step.

x = L1·cos(θ1) + L2·cos(θ1 + θ2)
y = L1·sin(θ1) + L2·sin(θ1 + θ2)

Plug in θ1 = 30°, θ2 = 45°:

x = 0.20 × cos(30°) + 0.15 × cos(75°)
  = 0.20 × 0.8660254 + 0.15 × 0.2588190
  = 0.1732051 + 0.0388229
  = 0.2120279

y = 0.20 × sin(30°) + 0.15 × sin(75°)
  = 0.20 × 0.5000000 + 0.15 × 0.9659258
  = 0.1000000 + 0.1448889
  = 0.2448889

The end effector is at (0.212028, 0.244889).

Add one more link and the same pattern repeats. With L3 = 0.10 m, θ3 = -60°, the third link's absolute direction is 30 + 45 - 60 = 15°, so:

x = 0.2120279 + 0.10 × cos(15°) = 0.2120279 + 0.0965926 = 0.3086205
y = 0.2448889 + 0.10 × sin(15°) = 0.2448889 + 0.0258819 = 0.2707708

A pattern shows up. Each link advances by its own length, in the direction that's the sum of all the angles up to it. For a planar arm, that's all you need, and this exact same formula holds no matter how many joints there are.

But this approach breaks down on a 3D arm whose joint axes point in different directions. The shoulder's first axis might be vertical while the second is horizontal, and the wrist's three axes are mutually orthogonal. You can't simply add angles anymore. This is exactly where matrices become necessary.

DH Parameters — Writing a Link With Four Numbers

The transform connecting one link normally needs six degrees of freedom — three rotations plus three translations. But a robot's links aren't attached arbitrarily. There are joint axes, and links connect those axes. Use this structure to place the coordinate frames according to a rule, and six degrees of freedom shrinks down to four.

The Denavit-Hartenberg convention is that rule, and its four numbers are these:

For a revolute joint, θ is the variable and the other three are constants. For a prismatic joint, d is the variable and the other three are constants. One variable per joint — that correspondence stays clean throughout.

The transform built from these four numbers is four elementary transforms chained together:

A_i = Rot_z(θ_i) · Trans_z(d_i) · Trans_x(a_i) · Rot_x(α_i)

Expanded out, it looks like this:

A = [ cos θ   -sin θ·cos α    sin θ·sin α    a·cos θ ]
    [ sin θ    cos θ·cos α   -cos θ·sin α    a·sin θ ]
    [   0          sin α          cos α          d   ]
    [   0            0              0            1   ]

There's a trap you absolutely need to know about here. There isn't just one DH convention.

The one above is standard (classic) DH, and the modified DH used by Craig's textbook has a different multiplication order.

Standard DH:  A_i = Rot_z(θ_i) · Trans_z(d_i) · Trans_x(a_i) · Rot_x(α_i)
Modified DH:  A_i = Rot_x(α_{i-1}) · Trans_x(a_{i-1}) · Rot_z(θ_i) · Trans_z(d_i)

The two conventions place the coordinate frames at different spots, so the parameter table for the same robot comes out different between them. In modified DH, a and α carry an i-1 subscript, and that's the fastest clue for telling which convention a table follows. Drop a table copied from a paper straight into standard-DH code, and the arm ends up in a bizarre shape, and it can take days to find why.

For reference, you'll often see standard DH's multiplication order written as Trans_z(d)·Rot_z(θ)·Trans_x(a)·Rot_x(α). That's not wrong — it's the same thing. Rot_z and Trans_z share the same axis and so commute, and the same goes for Trans_x and Rot_x.

Let's write out the DH table for a 3-link planar arm. All the joint axes are parallel and perpendicular to the plane, so twist and offset are all zero.

Let's confirm in code.

import numpy as np
np.set_printoptions(precision=6, suppress=True)


def dh_transform(a, alpha, d, theta):
    """Link transform matrix for the standard (classic) DH convention.
    This is the expanded form of Rot_z(theta) · Trans_z(d) · Trans_x(a) · Rot_x(alpha)."""
    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(dh_rows, q):
    """Each row of the DH table is (a, alpha, d, theta_offset)."""
    T = np.eye(4)
    for (a, alpha, d, offset), theta in zip(dh_rows, q):
        T = T @ dh_transform(a, alpha, d, offset + theta)
    return T


PLANAR_3R = [(0.20, 0.0, 0.0, 0.0),
             (0.15, 0.0, 0.0, 0.0),
             (0.10, 0.0, 0.0, 0.0)]

q = np.radians([30, 45, -60])
T = forward_kinematics(PLANAR_3R, q)
print("Transform matrix from the DH chain:")
print(T)

# Compare against the closed form derived by hand.
a = np.cumsum(q)
x = 0.20 * np.cos(a[0]) + 0.15 * np.cos(a[1]) + 0.10 * np.cos(a[2])
y = 0.20 * np.sin(a[0]) + 0.15 * np.sin(a[1]) + 0.10 * np.sin(a[2])
print(f"Closed form: x={x:.6f}  y={y:.6f}")
print(f"DH result  : x={T[0,3]:.6f}  y={T[1,3]:.6f}")
print("Are the two results equal:", np.allclose([x, y], T[:2, 3]))
print(f"End-effector direction (sum of three angles) = {np.degrees(a[2]):.4f} degrees, "
      f"value read from the matrix = {np.degrees(np.arctan2(T[1,0], T[0,0])):.4f} degrees")

Here's the output.

Transform matrix from the DH chain:
[[ 0.965926 -0.258819  0.        0.308621]
 [ 0.258819  0.965926  0.        0.270771]
 [ 0.        0.        1.        0.      ]
 [ 0.        0.        0.        1.      ]]
Closed form: x=0.308621  y=0.270771
DH result  : x=0.308621  y=0.270771
Are the two results equal: True
End-effector direction (sum of three angles) = 15.0000 degrees, value read from the matrix = 15.0000 degrees

The hand-derived (0.308621, 0.270771) matches what the matrix chain gave. And the end-effector direction read from the upper-left 3x3 of the result matrix matches the sum of the three angles, 15 degrees. Getting orientation along with position in a single pass is the advantage of the matrix approach.

Modern tools let you carry this table over directly. Peter Corke's Robotics Toolbox for Python (PyPI roboticstoolbox-python, version 1.3.1 as of July 2026, requires Python 3.10+) distinguishes standard-DH links as RevoluteDH and modified-DH links as RevoluteMDH, and you can't mix the two on the same robot. Splitting the convention by class rather than a flag actually acts as a safeguard here. Wrap them in a DHRobot and call .fkine(q), and the code above collapses to a single line.

The ROS ecosystem uses URDF instead of DH. Links and joints are written in XML, and each joint states its position and axis relative to its parent link.

<joint name="elbow" type="revolute">
  <parent link="upper_arm"/>
  <child link="forearm"/>
  <origin xyz="0.20 0 0" rpy="0 0 0"/>
  <axis xyz="0 0 1"/>
  <limit lower="-2.6" upper="2.6" effort="1.5" velocity="3.0"/>
</joint>

There are exactly six valid strings for a joint type in URDF: revolute, continuous, prismatic, fixed, floating, planar. And within the limit element, effort and velocity are required, while lower and upper are optional. For a joint with unlimited rotation range, use continuous and omit the bounds.

What URDF has over DH is that you don't need to place coordinate frames according to a rule. Put the origin wherever's convenient and just get the axis directions right. In exchange, the parameter count grows to six per joint. URDF is much more convenient for a person transcribing dimensions from CAD, while DH is more convenient for deriving equations by hand.

The Tradeoffs of Rotation Representations — Euler Angles, Rotation Matrices, Quaternions

Position wraps up in three numbers, but orientation doesn't work that way. A 3D rotation has three degrees of freedom, but there are several choices for how to write those three, and each comes with a different cost.

Let's look at each one's cost individually.

A rotation matrix is the most convenient for computation. Rotating a vector is a single multiplication, and composing rotations is a single multiplication too. In exchange, it uses nine numbers to represent three degrees of freedom, which brings six constraints along with it: the columns need to be mutually orthogonal, and each needs unit length. As floating-point error accumulates, this constraint drifts slightly, and left unchecked, an object subtly stretches or distorts. So it needs periodic re-orthonormalization.

Euler angles are the friendliest to a human. Say roll 30 degrees, pitch 0 degrees, yaw 90 degrees, and anyone can picture it. URDF's rpy attribute is exactly this, and the convention is fixed-axis order x, y, z, which as a matrix is Rz(yaw)·Ry(pitch)·Rx(roll).

The problem is gimbal lock. When the second angle hits 90 degrees, the first and third axes end up pointing in the same direction, so different combinations of angles produce the same rotation.

import numpy as np
np.set_printoptions(precision=6, suppress=True)


def rotation_rpy(roll, pitch, yaw):
    """Same convention as URDF's rpy. Rotating in fixed-axis x, y, z order is,
    as a matrix, the same as Rz(yaw) · Ry(pitch) · Rx(roll)."""
    cr, sr = np.cos(roll), np.sin(roll)
    cp, sp = np.cos(pitch), np.sin(pitch)
    cy, sy = np.cos(yaw), np.sin(yaw)
    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 rpy_from_rotation(R):
    """Recovers rpy from a rotation matrix. The key point is that the denominator is cos(pitch)."""
    pitch = np.arcsin(np.clip(-R[2, 0], -1.0, 1.0))
    roll = np.arctan2(R[2, 1], R[2, 2])
    yaw = np.arctan2(R[1, 0], R[0, 0])
    return np.degrees([roll, pitch, yaw])


A = rotation_rpy(np.radians(0),   np.radians(90), np.radians(30))
B = rotation_rpy(np.radians(-30), np.radians(90), np.radians(0))
print("roll=0, pitch=90, yaw=30 vs. roll=-30, pitch=90, yaw=0")
print("  Are the two matrices equal:", np.allclose(A, B), " Max difference:", np.abs(A - B).max())

# Just short of gimbal lock, see how much a tiny change in pose shakes up rpy.
base = rotation_rpy(np.radians(0), np.radians(89.9), np.radians(30))
print("\nRecovering (0, 89.9, 30) gives:", np.round(rpy_from_rotation(base), 4))
for eps in (0.001, 0.01, 0.1):
    perturbed = rotation_rpy(np.radians(eps), 0.0, 0.0) @ base
    print(f"  Wobble {eps:>5} degrees about x and rpy = {np.round(rpy_from_rotation(perturbed), 4)}"
          f"   (max matrix difference {np.abs(perturbed - base).max():.6f})")

Here's the output.

roll=0, pitch=90, yaw=30 vs. roll=-30, pitch=90, yaw=0
  Are the two matrices equal: True  Max difference: 3.0616169978683824e-17

Recovering (0, 89.9, 30) gives: [ 0.  89.9 30. ]
  Wobble 0.001 degrees about x and rpy = [ 0.4937 89.8995 30.4937]   (max matrix difference 0.000017)
  Wobble  0.01 degrees about x and rpy = [ 4.715  89.8946 34.715 ]   (max matrix difference 0.000175)
  Wobble   0.1 degrees about x and rpy = [30.     89.8268 60.    ]   (max matrix difference 0.001745)

The first two lines are gimbal lock itself. Two completely different angle combinations are the same rotation, and the difference, 3×10⁻¹⁷, is identical at floating-point precision.

The next three lines are where it hurts more in practice. At a pitch of 89.9 degrees, wobbling the arm by just 0.01 degrees about the x-axis moved the recovered roll from 0 degrees to 4.715 degrees, and yaw from 30 degrees to 34.715 degrees. A 0.01-degree input becomes a 4.7-degree output — a 470x amplification. Wobble by 0.1 degrees and roll becomes 30 degrees, yaw becomes 60 degrees.

The cause sits in the denominator of the recovery formula. The quantity used to divide out roll and yaw is cos(pitch), and as pitch approaches 90 degrees, that goes to zero. Dividing by a number near zero blows a small amount of input noise up into something large.

Here's what actually happens in practice. As the arm passes near this pose, encoder noise or floating-point error alone can send the roll and yaw values swinging wildly, and if you interpolate directly using those values, the wrist spins a full turn. Physically the arm barely moved, but the controller was handed a command to move 60 degrees. A large share of wrist-joint-winds-up-tight accidents come from exactly this.

Quaternions have no gimbal lock. Four numbers (w, x, y, z) with a single constraint (unit length) exactly match three degrees of freedom, and the representation is continuous. Interpolation between two poses (slerp) is smooth, and composing rotations is cheaper than with matrices.

The cost comes in two forms. One is that a person can't picture a pose just by looking at the numbers. The other is the double cover, where q and -q represent the same rotation. Interpolate between two quaternions without matching up their signs, and you take the long way around instead of the short one.

The practical answer usually looks like this: store and interpolate with quaternions, compute with rotation matrices, and only show a human Euler angles. Use all three, each in the one spot it's actually good at.

What Actually Happens When One Sign Is Wrong

The most common bug in forward kinematics code isn't something complicated. It's a single sign, a single angle convention.

Let's look at a joint angle whose direction got flipped. Assemble the gears on a servo backward and the joint spins the opposite way as the commanded angle grows. The code stays the same, but it's effectively as if θ2's sign got flipped.

import numpy as np

L1, L2 = 0.20, 0.15


def fk(theta1, theta2):
    return np.array([L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2),
                     L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)])


good = fk(np.radians(30), np.radians(45))
bad = fk(np.radians(30), np.radians(-45))          # Only θ2's sign is flipped
print(f"Correct end effector: ({good[0]:.6f}, {good[1]:.6f})")
print(f"Sign flipped        : ({bad[0]:.6f}, {bad[1]:.6f})")
print(f"Distance off         : {np.linalg.norm(good - bad) * 1000:.3f} mm")
print(f"Theoretical value 2·L2·sin(45°) = {2 * L2 * np.sin(np.radians(45)) * 1000:.3f} mm")

print("\nNear zero, it's barely visible:")
for d2 in (1, 5, 15, 45, 90):
    g, b = fk(np.radians(30), np.radians(d2)), fk(np.radians(30), np.radians(-d2))
    print(f"  θ2={d2:3}° -> error {np.linalg.norm(g - b) * 1000:7.3f} mm")

Here's the output.

Correct end effector: (0.212028, 0.244889)
Sign flipped        : (0.318094, 0.061177)
Distance off         : 212.132 mm
Theoretical value 2·L2·sin(45°) = 212.132 mm

Near zero, it's barely visible:
  θ2=  1° -> error   5.236 mm
  θ2=  5° -> error  26.147 mm
  θ2= 15° -> error  77.646 mm
  θ2= 45° -> error 212.132 mm
  θ2= 90° -> error 300.000 mm

At 45 degrees, it's off by 212 millimeters — and the arm's total length is only 350 millimeters. The magnitude of the error also checks out as exactly 2·L2·sin(θ2), because the end effector gets mirrored across the extension of the upper arm.

The last table tells you the character of this bug. Near the zero position, it's almost invisible. At θ2 = 1°, the error is 5 millimeters, which is easy to mistake for assembly tolerance or backlash. So testing only around the home pose passes, and moving to actual working angles goes completely off the rails.

There are a few more bugs with the same character.

Confusing radians and degrees. All of numpy's trig functions take radians. Pass 30 directly and it's interpreted as 30 radians, or 1719 degrees, computing a pose where the arm has spun more than four full turns. Fortunately this bug tends to reveal itself quickly since the result is so obviously wrong.

Confusing absolute and relative angles. In the equation above, θ2 is an angle relative to the upper arm. On an arm whose encoder reports an absolute angle, plugging that value straight in adds θ1 a second time. When θ1 = 0, the two conventions give the same value, so this bug is also invisible at the home pose.

Zero offset. The DH table's θ is an angle measured from the reference the convention defines, and the angle a servo reports is measured from its own mechanical zero point. When the two differ, they're off by a constant, and that constant needs to go into the DH table's offset term. The spot in the code above marked offset + theta is exactly that.

There's one verification method that catches all four of these at once. Move exactly one joint at a time. Hold the rest at zero, rotate one joint to a known angle, and compare the end-effector position your calculation predicts against the position you measure with a ruler. Move all six at once and try to find the cause, and you can't tell which joint is the problem.

Conclusion — Writing Down the Coordinate Frames Precisely Is Half the Job

The math of forward kinematics isn't hard. Append one row and one column to a rotation matrix to turn translation into multiplication too, and multiply everything in order. That's the whole thing.

The hard part isn't the math — it's the convention. Where is the angle measured from, which direction is positive, is it an absolute angle or a relative angle, which DH convention, where is the zero point. Fail to write these five things down and your future self, a few weeks from now, is guaranteed to get one of them wrong.

And when forward kinematics is wrong, everything built on top of it goes quietly wrong too. Inverse kinematics is the inverse of forward kinematics, so it inherits the same sign error directly; the Jacobian is the partial derivative of forward kinematics, so it inherits it too. Camera calibration, collision checking, trajectory planning — all the same. It's common to chase a strange symptom upstream for days and find it was a single sign in the DH table all along.

So once you've built forward kinematics, always check it with a ruler. One joint at a time, at a known angle, compared against the real distance. If the calculation and the ruler agree to within 5 millimeters, you can build anything on top of it.

The next step is flipping this function around. The inverse kinematics post covers why that's so much harder, and how the matrices built here connect into the Jacobian. How deep you need to go into the linear algebra and trigonometry covered in this post is laid out in order in the math you need for robotics post.