Lab: The AR4 from Its URDF

The previous section built the AR4 analytically from its Denavit-Hartenberg table. This lab takes the same arm from the other end: its URDF, the as-built description that drives the live Gazebo simulation of the AR4 physical-AI project. You read the kinematic chain straight out of the file, compute forward kinematics from it, reconcile the result against the DH model, map the reachable workspace, and solve position inverse kinematics. The same forward-kinematics code was used on the project to diagnose why a vision-language-action policy’s grasps were missing: forward-mapping what the policy commanded into Cartesian space and asking where the hand actually went.

import xml.etree.ElementTree as ET
import numpy as np

np.set_printoptions(precision=4, suppress=True)

URDF = "ar4.urdf"
ARM_JOINTS = [f"joint_{i}" for i in range(1, 7)]   # the six actuated revolute joints

root = ET.parse(URDF).getroot()
joints = {}
for j in root.findall("joint"):
    o = j.find("origin")
    xyz = [float(v) for v in (o.get("xyz", "0 0 0") if o is not None else "0 0 0").split()]
    rpy = [float(v) for v in (o.get("rpy", "0 0 0") if o is not None else "0 0 0").split()]
    a = j.find("axis")
    axis = [float(v) for v in a.get("xyz").split()] if a is not None else [0, 0, 1]
    lim = j.find("limit")
    limits = (float(lim.get("lower", "nan")), float(lim.get("upper", "nan"))) if lim is not None else None
    joints[j.get("name")] = dict(
        type=j.get("type"), parent=j.find("parent").get("link"), child=j.find("child").get("link"),
        xyz=np.array(xyz), rpy=rpy, axis=np.array(axis, float), limits=limits)

print(f"{len(joints)} joints total; actuated arm joints:")
for name in ARM_JOINTS:
    j = joints[name]
    lo, hi = j["limits"]
    print(f"  {name}: axis={j['axis']}, limits=[{lo:+.3f}, {hi:+.3f}] rad")

30 joints total; actuated arm joints:
  joint_1: axis=[0. 0. 1.], limits=[-2.793, +2.793] rad
  joint_2: axis=[ 0.  0. -1.], limits=[-0.733, +1.571] rad
  joint_3: axis=[ 0.  0. -1.], limits=[-1.553, +0.908] rad
  joint_4: axis=[ 0.  0. -1.], limits=[-3.142, +3.142] rad
  joint_5: axis=[1. 0. 0.], limits=[-1.833, +1.833] rad
  joint_6: axis=[0. 0. 1.], limits=[-3.142, +3.142] rad

Only 8 of the 38 joints move: six revolute arm joints and two prismatic gripper jaws. The rest are fixed attachments (covers, motors, the wrist camera). A URDF is a tree; the chain from the base to any link is recovered by walking child→parent:

child2joint = {v["child"]: k for k, v in joints.items()}

def chain_to(link):
    "Ordered joint names from the tree root down to `link`."
    ch = []
    while link in child2joint:
        jn = child2joint[link]
        ch.append(jn)
        link = joints[jn]["parent"]
    return ch[::-1]

JAWS = [k for k in joints if "jaw" in k.lower()]
print("chain to gripper jaw 1:")
print("  " + " -> ".join(chain_to(joints[JAWS[0]]["child"])))

chain to gripper jaw 1:
  base_joint -> joint_1 -> joint_2 -> joint_3 -> joint_4 -> joint_5 -> joint_6 -> gripper_base_joint -> gripper_jaw1_joint

Forward kinematics

Each URDF joint contributes two transforms: a fixed origin (translation

p

, then a roll-pitch-yaw rotation) and, for actuated joints, a motion about/along its axis

\hat a

by the joint variable

q

T_i(q_i) \;=\; \underbrace{\begin{bmatrix} R_{rpy} & p \\ 0 & 1 \end{bmatrix}}_{\text{origin (constant)}} \;\cdot\; \underbrace{\begin{bmatrix} R(\hat a_i,\, q_i) & 0 \\ 0 & 1 \end{bmatrix}}_{\text{revolute motion}} \qquad\text{or}\qquad \begin{bmatrix} I & \hat a_i q_i \\ 0 & 1 \end{bmatrix} \;\;\text{(prismatic)}

with

R(\hat a, q)

given by the axis-angle (Rodrigues) formula. FK is the ordered product along the chain:

T_{0\to E}(q) = \prod_i T_i(q_i)

def rpy_mat(r, p, y):
    cr, sr, cp, sp, cy, sy = np.cos(r), np.sin(r), np.cos(p), np.sin(p), np.cos(y), np.sin(y)
    return np.array([[cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],
                     [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],
                     [-sp,   cp*sr,            cp*cr]])

def axis_mat(a, q):
    "Rodrigues rotation about unit axis a by angle q."
    a = a / np.linalg.norm(a)
    x, y, z = a
    c, s = np.cos(q), np.sin(q)
    C = 1 - c
    return np.array([[x*x*C + c,   x*y*C - z*s, x*z*C + y*s],
                     [y*x*C + z*s, y*y*C + c,   y*z*C - x*s],
                     [z*x*C - y*s, z*y*C + x*s, z*z*C + c]])

def T_of(R, p):
    T = np.eye(4); T[:3, :3] = R; T[:3, 3] = p
    return T

def fk(q_arm, target_link, jaw=0.0):
    "Pose (4x4) of `target_link` in the base frame for arm joint vector q_arm (rad)."
    T = np.eye(4)
    for jn in chain_to(target_link):
        j = joints[jn]
        T = T @ T_of(rpy_mat(*j["rpy"]), j["xyz"])
        if j["type"] == "revolute" and jn in ARM_JOINTS:
            T = T @ T_of(axis_mat(j["axis"], q_arm[ARM_JOINTS.index(jn)]), [0, 0, 0])
        elif j["type"] == "prismatic":
            T = T @ T_of(np.eye(3), j["axis"] * jaw)
    return T

def grasp_center(q_arm, jaw=0.014):
    "Midpoint between the two gripper jaws, the natural 'tool center point'."
    pts = [fk(q_arm, joints[jj]["child"], jaw)[:3, 3] for jj in JAWS]
    return np.mean(pts, axis=0)

HOME = np.array([0.0, -0.3, 0.5, 0.0, 1.2, 0.0])   # the project's home pose
print("link_6 position at home:      ", fk(HOME, "link_6")[:3, 3])
print("grasp center (jaw midpoint):  ", grasp_center(HOME))

link_6 position at home:       [ 0.     -0.1995  0.3765]
grasp center (jaw midpoint):   [ 0.     -0.2056  0.341 ]

Frame caveat from practice. These coordinates are in the robot’s base_link frame. In the project’s Gazebo world the base is yawed 90°, so $x_{world} = -y_{base}$ , $y_{world} = x_{base}$ : forgetting this cost us an afternoon when comparing FK output against ground-truth object poses.

Reconciling the URDF with the DH model

The two descriptions are of one physical arm, so the parts that encode geometry must agree, and the parts that encode a coordinate convention are free to differ. Separating the two is the practical skill. Converting the URDF’s metres to millimetres and reading the link lengths off the joint origins lines them up against the DH table: the upper arm and the tool offset match exactly, the shoulder offset to a twentieth of a millimetre. One number does not: the forearm reads 222.63 mm in the DH table and 222.94 mm in the URDF, a genuine 0.31 mm gap between the nominal calibration and the CAD-derived model that no change of frame removes. The tool positions at the zero pose, by contrast, differ completely, because the two models define the zero configuration through different conventions (the URDF through each joint-origin roll-pitch-yaw, the DH table through its angle-offset column). Making the positions coincide takes an explicit base transform and a per-joint sign and offset map, which is bookkeeping, not physics.

# DH link constants (mm) from the previous section's standard-DH table.
DH_LENGTHS = {"upper arm a2": 305.0, "shoulder a1": 64.2, "forearm d4": 222.63, "tool d6": 41.0}

# The same constants as they appear in the URDF joint origins (metres -> mm).
def urdf_mm(joint, axis):
    return float(joints[joint]["xyz"]["xyz".index(axis)]) * 1000.0

urdf_lengths = {
    "upper arm a2": abs(urdf_mm("joint_3", "y")),
    "shoulder a1":  abs(urdf_mm("joint_2", "y")),
    "forearm d4":   abs(urdf_mm("joint_5", "z")),
    "tool d6":      abs(urdf_mm("joint_6", "z")),
}

print(f"{'link':14s}{'DH (mm)':>10s}{'URDF (mm)':>12s}{'diff (mm)':>12s}")
for k in DH_LENGTHS:
    d, u = DH_LENGTHS[k], urdf_lengths[k]
    print(f"{k:14s}{d:10.2f}{u:12.2f}{u - d:12.2f}")

link             DH (mm)   URDF (mm)   diff (mm)
upper arm a2      305.00      305.00        0.00
shoulder a1        64.20       64.15       -0.05
forearm d4        222.63      222.94        0.31
tool d6            41.00       41.00        0.00

The reachable workspace

Sampling joint vectors uniformly inside the URDF limits and plotting the grasp center for each gives an honest picture of where the hand can actually go. The red marker is the home pose.

# Sample joint vectors inside the URDF limits and forward-map each to a grasp center.
rng = np.random.default_rng(0)
lims = np.array([joints[j]["limits"] for j in ARM_JOINTS])
Q = rng.uniform(lims[:, 0], lims[:, 1], size=(3000, 6))
P = np.array([grasp_center(q) for q in Q])

Inverse kinematics: position by damped least squares

Position inverse kinematics asks: find

q

such that

f(q) = p^{\ast}

, where

f

is the forward kinematics above. Linearize with the Jacobian

J = \partial f / \partial q \in \mathbb{R}^{3\times 6}

(estimated here by finite differences) and iterate the damped least-squares update, which stays well behaved near singularities where the plain pseudo-inverse blows up:

\Delta q = J^{\top}\left(J J^{\top} + \lambda^2 I\right)^{-1} \left(p^{\ast} - f(q)\right)

This solves for position only. The previous section solved the full six-dimensional pose, position and orientation together, by stacking an orientation error under the position error and growing

J

6\times 6

def jacobian(q, eps=1e-5):
    J = np.zeros((3, 6))
    f0 = grasp_center(q)
    for i in range(6):
        dq = q.copy(); dq[i] += eps
        J[:, i] = (grasp_center(dq) - f0) / eps
    return J

def ik(p_target, q0=HOME, lam=0.05, iters=200, tol=1e-4):
    q = np.array(q0, float)
    errs = []
    for _ in range(iters):
        e = np.asarray(p_target) - grasp_center(q)
        errs.append(np.linalg.norm(e))
        if errs[-1] < tol:
            break
        J = jacobian(q)
        q = q + J.T @ np.linalg.solve(J @ J.T + lam**2 * np.eye(3), e)
        q = np.clip(q, lims[:, 0], lims[:, 1])       # respect URDF limits
    return q, errs

# a pick-zone target from the project (base frame; world (0.39, 0.03, 0.05))
target = np.array([0.03, -0.39, 0.05])
q_sol, errs = ik(target)

print("target:        ", target)
print("FK(q_solution):", grasp_center(q_sol))
print(f"final error:    {errs[-1]*1000:.2f} mm in {len(errs)} iterations")
print("q_solution:    ", q_sol)

target:         [ 0.03 -0.39  0.05]
FK(q_solution): [ 0.03 -0.39  0.05]
final error:    0.01 mm in 7 iterations
q_solution:     [-0.0727  1.0449  0.0915  0.0278  0.8436 -0.    ]

Where to go from here. This is position-only IK; a full pose solve stacks an orientation error (e.g. the axis-angle of

R^{\ast} R^{\top}

) under the position error and grows

J

6 \times 6

. Redundancy, joint-limit avoidance, and collision-aware IK are then the practical concerns, which is exactly what MoveIt’s IK (used by this project’s scripted expert) layers on top of the same URDF you just parsed. And the closing thought that connects this lecture to modern robot learning: a vision-language-action policy trained by behavior cloning never sees this section’s math, yet its failures are often best diagnosed with it, by forward-mapping what the policy commanded into Cartesian space and asking where it actually went.

Found an error on this page? Report an issue.

Robotics Systems

Motion Algebras

Kinematics

Dynamics

Systems Integration

Lab: The AR4 from Its URDF

Forward kinematics

Reconciling the URDF with the DH model

The reachable workspace

Inverse kinematics: position by damped least squares

​Forward kinematics

​Reconciling the URDF with the DH model

​The reachable workspace

​Inverse kinematics: position by damped least squares

Forward kinematics

Reconciling the URDF with the DH model

The reachable workspace

Inverse kinematics: position by damped least squares