> ## Documentation Index
> Fetch the complete documentation index at: https://aegean.ai/llms.txt
> Use this file to discover all available pages before exploring further.
# Lab: The AR4 from Its URDF
> Forward and inverse kinematics of the AR4 built from its as-built URDF, reconciled against the Denavit-Hartenberg model.
The [previous section](/aiml-common/lectures/kinematics/ar4-forward-inverse-kinematics/index)
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](https://github.com/aegean-ai/ar4-physical-ai). 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.
```python theme={null}
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")
```
```output theme={null}
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:
```python theme={null}
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"])))
```
```output theme={null}
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)$.
```python theme={null}
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))
```
```output theme={null}
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.
```python theme={null}
# 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}")
```
```output theme={null}
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.
```python theme={null}
# 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](/aiml-common/lectures/kinematics/ar4-forward-inverse-kinematics/index)
solved the full six-dimensional pose, position and orientation together, by stacking an
orientation error under the position error and growing $J$ to $6\times 6$.
```python theme={null}
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)
```
```output theme={null}
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$ to
$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.
***
[Edit this page on GitHub](https://github.com/aegean-ai/eaia/edit/main/src/aiml-common/lectures/kinematics/ar4-kinematics-lab/index.mdx) or [file an issue](https://github.com/aegean-ai/eaia/issues/new/choose).