feat(experement): add calibrated grant working geometry

src/iiwa_experement/config/geometry.yaml

@@ -0,0 +1,153 @@
+# Calibrated working geometry for the grant reference executor.
+#
+# All lengths are metres, all angles are degrees unless a key says rad.
+# Every transform below names its parent and child frame.  The numeric values
+# were measured from the meshes in iiwa_description/objects/grant, from
+# patron.xacro and from the actual Webots scene.  They are deliberately kept
+# in one file so the executor never hard-codes a working pose.
+schema_version: 1
+units: m
+angle_units: deg
+
+frames:
+  world: {parent: null}
+  base_link: {parent: world}
+  patron: {parent: base_link}
+  tcp: {parent: patron}
+
+# Webots spawns the robot at this fixed world pose (setting.yaml transform).
+world_to_base:
+  xyz: [-0.25, 0.0, 0.79]
+  rpy_deg: [0.0, 0.0, 0.0]
+  parent: world
+  child: base_link
+
+# patron.xacro: patron_tcp origin is 55.2 mm along patron Z, rotated -90 deg.
+patron_to_tcp:
+  xyz: [0.0, 0.0, 0.0552]
+  rpy_deg: [0.0, 0.0, -90.0]
+  parent: patron
+  child: tcp
+
+# The mount (top of a working tool) sits exactly at tcp.  The working tip is
+# 0.140 m further along tcp +Z, which points towards the work piece when the
+# arm is in a working pose.
+tool_mounts:
+  vacuum: {xyz: [0.0, 0.0, 0.0], rpy_deg: [0.0, 0.0, 0.0]}
+  magnetic_tool: {xyz: [0.0, 0.0, 0.0], rpy_deg: [0.0, 0.0, 0.0]}
+  sealant_tool: {xyz: [0.0, 0.0, 0.0], rpy_deg: [0.0, 0.0, 0.0]}
+
+tool_work_tips:
+  vacuum: {xyz: [0.0, 0.0, 0.140], rpy_deg: [0.0, 0.0, 0.0]}
+  magnetic_tool: {xyz: [0.0, 0.0, 0.140], rpy_deg: [0.0, 0.0, 0.0]}
+  sealant_tool: {xyz: [0.0, 0.0, 0.140], rpy_deg: [0.0, 0.0, 0.0]}
+
+# Transform from tcp to the Webots node origin (bottom of the vertical OBJ).
+# The OBJ Y axis is imported as Webots Z, so the node is Rx(-90) relative to a
+# tcp that points down.
+tool_tcp_to_nodes:
+  vacuum: {xyz: [0.0, 0.0, 0.140], rpy_deg: [-90.0, 0.0, 0.0]}
+  magnetic_tool: {xyz: [0.0, 0.0, 0.140], rpy_deg: [-90.0, 0.0, 0.0]}
+  sealant_tool: {xyz: [0.0, 0.0, 0.140], rpy_deg: [-90.0, 0.0, 0.0]}
+
+# Boxes expressed in the tcp frame, used for the attached collision object.
+tool_collisions:
+  vacuum: {size: [0.0821, 0.081, 0.140], offset: [0.0, 0.0, 0.070]}
+  magnetic_tool: {size: [0.036, 0.036, 0.140], offset: [0.0, 0.0, 0.070]}
+  sealant_tool: {size: [0.0561, 0.0561, 0.140], offset: [0.0, 0.0, 0.070]}
+
+# Hatch (cap.obj) is 0.1384 x 0.0984 x 0.00367.  Its node origin is the lower
+# face; the suction cup touches the highest point (local Y = 0.006).
+cap_tcp_to_node:
+  xyz: [0.0, 0.0, 0.146]
+  rpy_deg: [-90.0, 0.0, 0.0]
+cap_collision:
+  # Expressed in tcp: the plate is horizontal, 3.67 mm thick.
+  size: [0.1384, 0.0984, 0.00367]
+  offset: [0.0, 0.0, 0.141835]
+# Installed hatch pose relative to the body node (body local Y is world Z).
+body_to_installed_cap:
+  xyz: [0.0, 0.00294, 0.0]
+  rpy_deg: [0.0, 0.0, 0.0]
+
+# Screws stand with their head up.  The magnet touches the head, 9 mm above
+# the node origin; the screw node is therefore 149 mm below tcp.
+screw_tcp_to_node:
+  xyz: [0.0, 0.0, 0.149]
+  rpy_deg: [-90.0, 0.0, 0.0]
+screw_collision:
+  size: [0.0055, 0.0055, 0.009]
+  offset: [0.0, 0.0, 0.1445]
+
+# Hole centres are measured in the world frame of the standard, unshifted
+# body at 45 deg.  They rotate with the body.  The values agree with
+# doc/experiment-scenes.md.
+hole_reference_yaw_deg: 45.0
+hole_z_above_body_origin: 0.00894
+handoff_gap: 0.002
+holes:
+  hole_1: {dx: -0.0757, dy: -0.0148}
+  hole_2: {dx: -0.0148, dy: -0.0757}
+  hole_3: {dx: 0.0757, dy: 0.0148}
+  hole_4: {dx: 0.0148, dy: 0.0757}
+
+# Sealant ring: a closed rectangle just inside the hatch rim.  Half sizes are
+# in the hatch body plane; the ring is traversed counter-clockwise starting at
+# the hole_1 side.  Segment endpoints are derived from these measured values.
+sealant_reference_yaw_deg: 45.0
+sealant_rect:
+  half_x: 0.058
+  half_z: 0.037
+sealant_z_above_body_top: 0.0015
+sealant_sides:
+  side_1: {start: [-0.06718, -0.01485], end: [-0.01485, -0.06718]}
+  side_2: {start: [-0.01485, -0.06718], end: [0.06718, 0.01485]}
+  side_3: {start: [0.06718, 0.01485], end: [0.01485, 0.06718]}
+  side_4: {start: [0.01485, 0.06718], end: [-0.06718, -0.01485]}
+
+# Approach / transfer clearances and limits.
+approach_gap: 0.060
+contact_gap: 0.003
+transfer_gap: 0.080
+seat_gap: 0.002
+position_tolerance: 0.006
+orientation_tolerance_rad: 0.08
+speeds:
+  transfer: 0.35
+  approach: 0.18
+  contact: 0.06
+  contour: 0.09
+
+# Conservative collision models for the static scene.  They mirror the
+# Webots bounding objects so MoveIt and the simulator agree.
+scene_collisions:
+  table:
+    type: box
+    size: [1.28, 1.08, 0.045]
+    center_z: 0.7705
+  fixture:
+    type: box
+    size: [0.350, 0.220, 0.041]
+    offset_z: 0.0105
+  body:
+    type: box
+    size: [0.300, 0.170, 0.00915]
+    offset_z: 0.000695
+  rack:
+    type: box
+    size: [0.420, 0.140, 0.010]
+    offset_z: 0.005
+  tray_base:
+    type: box
+    size: [0.120, 0.070, 0.0024]
+    offset_z: 0.0012
+  tray_wall_x:
+    type: box
+    size: [0.002, 0.070, 0.010]
+    offset_z: 0.005
+    offset_x: 0.059
+  tray_wall_y:
+    type: box
+    size: [0.120, 0.002, 0.010]
+    offset_z: 0.005
+    offset_y: 0.034

src/iiwa_experement/iiwa_experement/geometry.py

@@ -0,0 +1,521 @@
+"""Calibrated transforms, working tips and scene collision models.
+
+This module is pure Python (numpy/scipy only) so the same numbers are used by
+the unit tests, the MoveIt bridge, the motion adapter and the Webots
+supervisor.  Nothing here talks to ROS or Webots.
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any, Mapping
+
+import numpy as np
+import yaml
+from scipy.spatial.transform import Rotation
+
+from .scenario import SCREW_HOLES, SEALANT_SIDES, ScenarioSpec, Tool
+
+
+TOOL_GEOMETRY_KEYS = ('vacuum', 'magnetic_tool', 'sealant_tool')
+_SEALANT_SIDES = SEALANT_SIDES
+
+
+class GeometryError(ValueError):
+    """Raised when the geometry file is missing or inconsistent."""
+
+
+def _number(value: Any, label: str) -> float:
+    if isinstance(value, bool) or not isinstance(value, (int, float)):
+        raise GeometryError(f'{label} must be a finite number')
+    result = float(value)
+    if not np.isfinite(result):
+        raise GeometryError(f'{label} must be a finite number')
+    return result
+
+
+def _vector(value: Any, label: str, length: int) -> tuple[float, ...]:
+    if not isinstance(value, (list, tuple)) or len(value) != length:
+        raise GeometryError(f'{label} must be a list of {length} numbers')
+    return tuple(_number(item, f'{label}[{index}]') for index, item in enumerate(value))
+
+
+def _mapping(value: Any, label: str) -> Mapping[str, Any]:
+    if not isinstance(value, Mapping):
+        raise GeometryError(f'{label} must be a mapping')
+    return value
+
+
+@dataclass(frozen=True)
+class Box:
+    """Axis-aligned box in a named frame."""
+
+    size: tuple[float, float, float]
+    offset: tuple[float, float, float] = (0.0, 0.0, 0.0)
+
+    @property
+    def half_extents(self) -> tuple[float, float, float]:
+        return tuple(value / 2.0 for value in self.size)
+
+
+@dataclass(frozen=True)
+class Transform:
+    """Rigid transform ``parent -> child``.
+
+    ``rotation`` is a 3x3 row-major matrix.  Composition follows the usual
+    convention: ``parent_to_child.compose(child_to_grandchild)``.
+    """
+
+    xyz: tuple[float, float, float]
+    rotation: tuple[tuple[float, float, float], ...]
+    parent: str
+    child: str
+
+    @classmethod
+    def from_xyz_rpy(
+        cls,
+        xyz,
+        rpy_deg,
+        *,
+        parent: str,
+        child: str,
+    ) -> 'Transform':
+        values = _vector(xyz, f'{parent}->{child}.xyz', 3)
+        angles = _vector(rpy_deg, f'{parent}->{child}.rpy_deg', 3)
+        matrix = Rotation.from_euler('xyz', angles, degrees=True).as_matrix()
+        return cls(
+            xyz=values,
+            rotation=tuple(tuple(float(v) for v in row) for row in matrix),
+            parent=parent,
+            child=child,
+        )
+
+    @property
+    def matrix(self) -> np.ndarray:
+        return np.asarray(self.rotation, dtype=float)
+
+    @property
+    def position(self) -> np.ndarray:
+        return np.asarray(self.xyz, dtype=float)
+
+    def compose_relaxed(self, other: 'Transform') -> 'Transform':
+        """Compose two transforms whose frame labels are documentation only."""
+        rotation = self.matrix @ other.matrix
+        translation = self.position + self.matrix @ other.position
+        return Transform(
+            xyz=tuple(float(v) for v in translation),
+            rotation=tuple(tuple(float(v) for v in row) for row in rotation),
+            parent=self.parent,
+            child=other.child,
+        )
+
+    def compose(self, other: 'Transform') -> 'Transform':
+        if self.child != other.parent:
+            raise GeometryError(
+                f'cannot compose {self.parent}->{self.child} with '
+                f'{other.parent}->{other.child}'
+            )
+        rotation = self.matrix @ other.matrix
+        translation = self.position + self.matrix @ other.position
+        return Transform(
+            xyz=tuple(float(v) for v in translation),
+            rotation=tuple(tuple(float(v) for v in row) for row in rotation),
+            parent=self.parent,
+            child=other.child,
+        )
+
+    def inverse(self) -> 'Transform':
+        rotation = self.matrix.T
+        translation = -rotation @ self.position
+        return Transform(
+            xyz=tuple(float(v) for v in translation),
+            rotation=tuple(tuple(float(v) for v in row) for row in rotation),
+            parent=self.child,
+            child=self.parent,
+        )
+
+    def apply(self, point) -> tuple[float, float, float]:
+        result = self.matrix @ np.asarray(point, dtype=float) + self.position
+        return tuple(float(v) for v in result)
+
+    def distance_to(self, other: 'Transform') -> float:
+        if self.parent != other.parent:
+            raise GeometryError('cannot compare transforms in different parents')
+        return float(np.linalg.norm(self.position - other.position))
+
+    def angle_to(self, other: 'Transform', *, parent: str | None = None) -> float:
+        matrix = self.matrix
+        other_matrix = other.matrix
+        if parent is not None:
+            matrix = _frame_matrix(self, parent) @ matrix
+            other_matrix = _frame_matrix(other, parent) @ other_matrix
+        cosine = (np.trace(matrix.T @ other_matrix) - 1.0) / 2.0
+        return float(np.arccos(max(-1.0, min(1.0, cosine))))
+
+
+def _frame_matrix(transform: Transform, frame: str) -> np.ndarray:
+    # Kept small and explicit: only world/base_link are ever needed here.
+    del transform
+    if frame in ('base_link', 'world'):
+        return np.eye(3)
+    raise GeometryError(f'unsupported comparison frame {frame!r}')
+
+
+@dataclass(frozen=True)
+class HoleSpec:
+    name: str
+    dx: float
+    dy: float
+
+
+@dataclass(frozen=True)
+class SegmentSpec:
+    name: str
+    start: tuple[float, float]
+    end: tuple[float, float]
+
+
+@dataclass(frozen=True)
+class GeometrySpec:
+    world_to_base: Transform
+    patron_to_tcp: Transform
+    tool_mounts: dict[str, Transform]
+    tool_work_tips: dict[str, Transform]
+    tool_tcp_to_nodes: dict[str, Transform]
+    tool_collisions: dict[str, Box]
+    cap_tcp_to_node: Transform
+    cap_collision: Box
+    body_to_installed_cap: Transform
+    screw_tcp_to_node: Transform
+    screw_collision: Box
+    holes: dict[str, HoleSpec]
+    hole_reference_yaw_deg: float
+    hole_z_above_body_origin: float
+    handoff_gap: float
+    sealant_reference_yaw_deg: float
+    sealant_sides: dict[str, SegmentSpec]
+    sealant_z_above_body_top: float
+    approach_gap: float
+    contact_gap: float
+    transfer_gap: float
+    seat_gap: float
+    position_tolerance: float
+    orientation_tolerance_rad: float
+    speeds: dict[str, float]
+    scene_collisions: dict[str, Box]
+
+    @property
+    def body_top_local(self) -> float:
+        body = self.scene_collisions['body']
+        return body.offset[2] + body.size[2] / 2.0
+
+
+def _load_transform(raw: Any, label: str, parent: str, child: str) -> Transform:
+    data = _mapping(raw, label)
+    return Transform.from_xyz_rpy(
+        data.get('xyz', [0.0, 0.0, 0.0]),
+        data.get('rpy_deg', [0.0, 0.0, 0.0]),
+        parent=parent,
+        child=child,
+    )
+
+
+def _load_box(raw: Any, label: str) -> Box:
+    data = _mapping(raw, label)
+    size = _vector(data.get('size'), f'{label}.size', 3)
+    offset = _vector(data.get('offset', [0.0, 0.0, 0.0]), f'{label}.offset', 3)
+    if any(value <= 0.0 for value in size):
+        raise GeometryError(f'{label}.size must be positive')
+    return Box(size=size, offset=offset)
+
+
+def load_geometry(path: Path | str) -> GeometrySpec:
+    """Load and validate ``config/geometry.yaml``."""
+    source = Path(path)
+    if not source.is_file():
+        raise FileNotFoundError(f'geometry file does not exist: {source}')
+    try:
+        raw = yaml.safe_load(source.read_text(encoding='utf-8'))
+    except yaml.YAMLError as error:
+        raise GeometryError(f'invalid YAML in {source}: {error}') from error
+    data = _mapping(raw, 'geometry')
+    if data.get('schema_version') != 1:
+        raise GeometryError('geometry schema_version must be 1')
+
+    world_to_base = _load_transform(
+        data.get('world_to_base'), 'world_to_base', 'world', 'base_link',
+    )
+    patron_to_tcp = _load_transform(
+        data.get('patron_to_tcp'), 'patron_to_tcp', 'patron', 'tcp',
+    )
+
+    tool_mounts_raw = _mapping(data.get('tool_mounts'), 'tool_mounts')
+    tool_tips_raw = _mapping(data.get('tool_work_tips'), 'tool_work_tips')
+    tool_nodes_raw = _mapping(data.get('tool_tcp_to_nodes'), 'tool_tcp_to_nodes')
+    tool_collisions_raw = _mapping(data.get('tool_collisions'), 'tool_collisions')
+    missing = [name for name in TOOL_GEOMETRY_KEYS if name not in tool_mounts_raw]
+    if missing:
+        raise GeometryError(f'tool_mounts is missing {missing}')
+    tool_mounts = {
+        name: _load_transform(
+            tool_mounts_raw[name], f'tool_mounts.{name}', 'tcp', f'{name}_mount',
+        )
+        for name in TOOL_GEOMETRY_KEYS
+    }
+    tool_work_tips = {
+        name: _load_transform(
+            tool_tips_raw[name], f'tool_work_tips.{name}', 'tcp', f'{name}_tip',
+        )
+        for name in TOOL_GEOMETRY_KEYS
+    }
+    tool_tcp_to_nodes = {
+        name: _load_transform(
+            tool_nodes_raw[name], f'tool_tcp_to_nodes.{name}', 'tcp', f'tool_{name}',
+        )
+        for name in TOOL_GEOMETRY_KEYS
+    }
+    tool_collisions = {
+        name: _load_box(tool_collisions_raw[name], f'tool_collisions.{name}')
+        for name in TOOL_GEOMETRY_KEYS
+    }
+
+    holes_raw = _mapping(data.get('holes'), 'holes')
+    holes = {}
+    for name in SCREW_HOLES:
+        if name not in holes_raw:
+            raise GeometryError(f'holes is missing {name}')
+        entry = _mapping(holes_raw[name], f'holes.{name}')
+        holes[name] = HoleSpec(
+            name=name,
+            dx=_number(entry.get('dx'), f'holes.{name}.dx'),
+            dy=_number(entry.get('dy'), f'holes.{name}.dy'),
+        )
+
+    sides_raw = _mapping(data.get('sealant_sides'), 'sealant_sides')
+    sealant_sides = {}
+    for name in _SEALANT_SIDES:
+        entry = _mapping(sides_raw.get(name), f'sealant_sides.{name}')
+        sealant_sides[name] = SegmentSpec(
+            name=name,
+            start=_vector(entry.get('start'), f'sealant_sides.{name}.start', 2),
+            end=_vector(entry.get('end'), f'sealant_sides.{name}.end', 2),
+        )
+
+    speeds_raw = _mapping(data.get('speeds'), 'speeds')
+    speeds = {
+        key: _number(speeds_raw.get(key), f'speeds.{key}')
+        for key in ('transfer', 'approach', 'contact', 'contour')
+    }
+    for key, value in speeds.items():
+        if not 0.0 < value <= 1.0:
+            raise GeometryError(f'speeds.{key} must be within (0, 1]')
+
+    scene_raw = _mapping(data.get('scene_collisions'), 'scene_collisions')
+    scene_collisions = {}
+    for name, entry in scene_raw.items():
+        box_data = _mapping(entry, f'scene_collisions.{name}')
+        size = _vector(box_data.get('size'), f'scene_collisions.{name}.size', 3)
+        offset = (
+            _number(box_data.get('offset_x', 0.0), f'scene_collisions.{name}.offset_x'),
+            _number(box_data.get('offset_y', 0.0), f'scene_collisions.{name}.offset_y'),
+            _number(box_data.get('offset_z', 0.0), f'scene_collisions.{name}.offset_z'),
+        )
+        scene_collisions[name] = Box(size=size, offset=offset)
+
+    positive = {
+        'approach_gap': data.get('approach_gap'),
+        'contact_gap': data.get('contact_gap'),
+        'transfer_gap': data.get('transfer_gap'),
+        'seat_gap': data.get('seat_gap'),
+        'position_tolerance': data.get('position_tolerance'),
+    }
+    values = {
+        key: _number(value, key) for key, value in positive.items()
+    }
+    for key, value in values.items():
+        if value <= 0.0:
+            raise GeometryError(f'{key} must be positive')
+    orientation_tolerance = _number(
+        data.get('orientation_tolerance_rad'), 'orientation_tolerance_rad'
+    )
+    if orientation_tolerance <= 0.0:
+        raise GeometryError('orientation_tolerance_rad must be positive')
+
+    cap_tcp_to_node = _load_transform(
+        data.get('cap_tcp_to_node'), 'cap_tcp_to_node', 'tcp', 'cap_node',
+    )
+    body_to_cap = _load_transform(
+        data.get('body_to_installed_cap'), 'body_to_installed_cap', 'body', 'cap_seat',
+    )
+    screw_tcp_to_node = _load_transform(
+        data.get('screw_tcp_to_node'), 'screw_tcp_to_node', 'tcp', 'screw_node',
+    )
+
+    return GeometrySpec(
+        world_to_base=world_to_base,
+        patron_to_tcp=patron_to_tcp,
+        tool_mounts=tool_mounts,
+        tool_work_tips=tool_work_tips,
+        tool_tcp_to_nodes=tool_tcp_to_nodes,
+        tool_collisions=tool_collisions,
+        cap_tcp_to_node=cap_tcp_to_node,
+        cap_collision=_load_box(data.get('cap_collision'), 'cap_collision'),
+        body_to_installed_cap=body_to_cap,
+        screw_tcp_to_node=screw_tcp_to_node,
+        screw_collision=_load_box(data.get('screw_collision'), 'screw_collision'),
+        holes=holes,
+        hole_reference_yaw_deg=_number(
+            data.get('hole_reference_yaw_deg'), 'hole_reference_yaw_deg'
+        ),
+        hole_z_above_body_origin=_number(
+            data.get('hole_z_above_body_origin'), 'hole_z_above_body_origin'
+        ),
+        handoff_gap=_number(data.get('handoff_gap'), 'handoff_gap'),
+        sealant_reference_yaw_deg=_number(
+            data.get('sealant_reference_yaw_deg'), 'sealant_reference_yaw_deg'
+        ),
+        sealant_sides=sealant_sides,
+        sealant_z_above_body_top=_number(
+            data.get('sealant_z_above_body_top'), 'sealant_z_above_body_top'
+        ),
+        approach_gap=values['approach_gap'],
+        contact_gap=values['contact_gap'],
+        transfer_gap=values['transfer_gap'],
+        seat_gap=values['seat_gap'],
+        position_tolerance=values['position_tolerance'],
+        orientation_tolerance_rad=orientation_tolerance,
+        speeds=speeds,
+        scene_collisions=scene_collisions,
+    )
+
+
+def object_pose_transform(
+    x: float,
+    y: float,
+    z: float,
+    yaw_deg: float,
+    *,
+    parent: str = 'world',
+    child: str,
+) -> Transform:
+    """Webots pose of an OBJ whose local Y axis becomes world Z."""
+    return Transform.from_xyz_rpy(
+        [x, y, z], [90.0, 0.0, yaw_deg], parent=parent, child=child,
+    )
+
+
+def _tool_transform(tool: Tool, parent: str = 'world') -> Transform:
+    return object_pose_transform(
+        tool.pose.x,
+        tool.pose.y,
+        tool.pose.z,
+        tool.pose.yaw_deg + 180.0,
+        parent=parent,
+        child=f'tool_{tool.name}',
+    )
+
+
+def _body_transform(spec: ScenarioSpec) -> Transform:
+    return object_pose_transform(
+        spec.body.x, spec.body.y, spec.body.z, spec.body.yaw_deg,
+        parent='world', child='body',
+    )
+
+
+def _down_transform(x: float, y: float, z: float, yaw_deg: float, child: str) -> Transform:
+    """Work-point pose whose local Z points down (tool working direction)."""
+    return Transform.from_xyz_rpy(
+        [x, y, z], [180.0, 0.0, yaw_deg], parent='world', child=child,
+    )
+
+
+def _rotate_xy(dx: float, dy: float, degrees: float) -> tuple[float, float]:
+    angle = np.radians(degrees)
+    cosine, sine = np.cos(angle), np.sin(angle)
+    return dx * cosine - dy * sine, dx * sine + dy * cosine
+
+
+def world_target(spec: ScenarioSpec, geometry: GeometrySpec, name: str) -> Transform:
+    """Return the named world pose from an already validated scenario.
+
+    Names:
+      ``base_link``, ``tool:<name>``, ``cap``, ``cap_seat``,
+      ``screw:<screw_i>``, ``hole:<hole_i>`` and ``sealant:<side_i>``.
+    """
+    if name == 'base_link':
+        return geometry.world_to_base
+    if name.startswith('tool:'):
+        tool_name = name.split(':', 1)[1]
+        tools = {tool.name: tool for tool in spec.tools}
+        if tool_name not in tools:
+            raise GeometryError(f'experiment {spec.experiment_id} has no tool {tool_name!r}')
+        return _tool_transform(tools[tool_name])
+    if name == 'cap':
+        if spec.cap_pose is None:
+            raise GeometryError(f'experiment {spec.experiment_id} has no hatch')
+        pose = spec.cap_pose
+        return object_pose_transform(
+            pose.x, pose.y, pose.z, pose.yaw_deg, child='cap',
+        )
+    if name == 'cap_seat':
+        return _body_transform(spec).compose(geometry.body_to_installed_cap)
+    if name.startswith('screw:'):
+        screw_name = name.split(':', 1)[1]
+        screws = {screw.name: screw for screw in spec.screws}
+        if screw_name not in screws:
+            raise GeometryError(
+                f'experiment {spec.experiment_id} has no screw {screw_name!r}'
+            )
+        screw = screws[screw_name]
+        return object_pose_transform(
+            screw.pose.x, screw.pose.y, screw.pose.z,
+            screw.pose.yaw_deg, child=screw.name,
+        )
+    if name.startswith('hole:'):
+        hole_name = name.split(':', 1)[1]
+        if hole_name not in geometry.holes:
+            raise GeometryError(f'unknown hole {hole_name!r}')
+        hole = geometry.holes[hole_name]
+        dx, dy = _rotate_xy(
+            hole.dx, hole.dy, spec.body.yaw_deg - geometry.hole_reference_yaw_deg,
+        )
+        return _down_transform(
+            spec.body.x + dx,
+            spec.body.y + dy,
+            spec.body.z + geometry.hole_z_above_body_origin + geometry.handoff_gap,
+            spec.body.yaw_deg,
+            child=hole_name,
+        )
+    if name.startswith('sealant:'):
+        side_name = name.split(':', 1)[1]
+        return sealant_point(geometry, spec, side_name, 0.0)
+    raise GeometryError(f'unknown world target {name!r}')
+
+
+def sealant_point(
+    geometry: GeometrySpec,
+    spec: ScenarioSpec,
+    side_name: str,
+    fraction: float,
+) -> Transform:
+    """Return a work-point transform along one named sealant segment."""
+    if side_name not in geometry.sealant_sides:
+        raise GeometryError(f'unknown sealant side {side_name!r}')
+    if not 0.0 <= fraction <= 1.0:
+        raise GeometryError('sealant fraction must be within 0..1')
+    segment = geometry.sealant_sides[side_name]
+    dx = segment.start[0] + (segment.end[0] - segment.start[0]) * fraction
+    dy = segment.start[1] + (segment.end[1] - segment.start[1]) * fraction
+    dx, dy = _rotate_xy(
+        dx, dy, spec.body.yaw_deg - geometry.sealant_reference_yaw_deg,
+    )
+    z = spec.body.z + geometry.body_top_local + geometry.sealant_z_above_body_top
+    return _down_transform(
+        spec.body.x + dx, spec.body.y + dy, z, spec.body.yaw_deg,
+        child=f'sealant_{side_name}',
+    )
+
+
+def tcp_goal(target: Transform, tcp_to_work_tip: Transform) -> Transform:
+    """Convert a work-point target into the required tcp pose."""
+    return target.compose_relaxed(tcp_to_work_tip.inverse())

src/iiwa_experement/test/test_geometry.py

@@ -0,0 +1,225 @@
+"""Calibrated geometry: tool mounting, hatch, contour and holes."""
+from pathlib import Path
+
+import numpy as np
+import pytest
+import yaml
+from ament_index_python.packages import get_package_share_directory
+
+from iiwa_experement.geometry import (
+    GeometryError,
+    Transform,
+    load_geometry,
+    object_pose_transform,
+    sealant_point,
+    tcp_goal,
+    world_target,
+)
+from iiwa_experement.scenario import load_scenario
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+SCENES = PACKAGE / 'config' / 'scenes.yaml'
+GEOMETRY = PACKAGE / 'config' / 'geometry.yaml'
+ASSET_ROOT = (
+    Path(get_package_share_directory('iiwa_description')) / 'objects' / 'grant'
+)
+
+
+def geometry():
+    return load_geometry(GEOMETRY)
+
+
+def scenario(experiment_id):
+    return load_scenario(experiment_id, SCENES)
+
+
+def obj_y_bounds(asset):
+    low, high = np.inf, -np.inf
+    for line in (ASSET_ROOT / asset).read_text(encoding='utf-8', errors='replace').splitlines():
+        if line.startswith('v '):
+            y = float(line.split()[2])
+            low, high = min(low, y), max(high, y)
+    return low, high
+
+
+def test_tool_lengths_and_sealant_scale():
+    for asset in ('vacum-tool.obj', 'magnit-tool.obj', 'wranch-tool.obj'):
+        low, high = obj_y_bounds(asset)
+        assert low == pytest.approx(0.0, abs=1e-12)
+        assert high == pytest.approx(0.140, abs=1e-12)
+    low, high = obj_y_bounds('germetic-tool.obj')
+    assert low == pytest.approx(0.0, abs=1e-12)
+    assert high == pytest.approx(0.014, abs=1e-12)
+    assert high * 10.0 == pytest.approx(0.140)
+
+
+def test_patron_tcp_offset_is_55_2_mm():
+    spec = geometry()
+    assert spec.patron_to_tcp.parent == 'patron'
+    assert spec.patron_to_tcp.child == 'tcp'
+    assert spec.patron_to_tcp.position == pytest.approx([0.0, 0.0, 0.0552])
+
+
+def test_all_tools_mount_at_tcp_and_end_fourteen_centimetres_away():
+    spec = geometry()
+    for name in ('vacuum', 'magnetic_tool', 'sealant_tool'):
+        mount = spec.tool_mounts[name]
+        tip = spec.tool_work_tips[name]
+        assert mount.parent == tip.parent == 'tcp'
+        assert mount.position == pytest.approx([0.0, 0.0, 0.0])
+        assert tip.position == pytest.approx([0.0, 0.0, 0.140])
+        node = spec.tool_tcp_to_nodes[name]
+        assert node.position == pytest.approx([0.0, 0.0, 0.140])
+        # The node transform must reproduce an upright OBJ.
+        world_tcp = Transform.from_xyz_rpy(
+            [0.0, 0.0, 0.93], [180.0, 0.0, 45.0], parent='world', child='tcp',
+        )
+        tool_mount = world_tcp.compose(mount)
+        assert tool_mount.parent == 'world'
+        assert tool_mount.child == f'{name}_mount'
+        node_world = world_tcp.compose(node)
+        # Node origin is the working tip, 0.14 below the mount.
+        assert node_world.position == pytest.approx([0.0, 0.0, 0.79], abs=1e-12)
+        assert np.linalg.det(node_world.matrix) == pytest.approx(1.0)
+
+
+def test_ten_screws_have_distinct_ids_and_poses():
+    for experiment_id in range(11, 21):
+        spec = scenario(experiment_id)
+        assert len(spec.screws) == 10
+        assert len({screw.name for screw in spec.screws}) == 10
+        assert len({(screw.pose.x, screw.pose.y) for screw in spec.screws}) == 10
+
+
+def test_experiment_three_moves_holes_with_the_body():
+    standard = scenario(1)
+    shifted = scenario(3)
+    g = geometry()
+    for hole in ('hole_1', 'hole_2', 'hole_3', 'hole_4'):
+        ref = world_target(standard, g, f'hole:{hole}')
+        moved = world_target(shifted, g, f'hole:{hole}')
+        assert moved.position[:2] == pytest.approx(
+            ref.position[:2] + np.array([-0.01, 0.02]), abs=1e-9,
+        )
+        assert moved.position[2] == pytest.approx(ref.position[2], abs=1e-12)
+
+
+def test_standard_holes_match_the_documented_coordinates():
+    g = geometry()
+    spec = scenario(11)
+    expected = {
+        'hole_1': (0.2243, 0.1052),
+        'hole_2': (0.2852, 0.0443),
+        'hole_3': (0.3757, 0.1348),
+        'hole_4': (0.3148, 0.1957),
+    }
+    for name, xy in expected.items():
+        target = world_target(spec, g, f'hole:{name}')
+        assert target.position[:2] == pytest.approx(xy, abs=1e-9)
+        assert target.position[2] == pytest.approx(0.823 + 0.00894 + 0.002, abs=1e-12)
+
+
+def test_experiment_nineteen_moves_screws_with_the_tray():
+    g = geometry()
+    standard = scenario(11)
+    shifted = scenario(19)
+    delta = np.array([-0.02, 0.02, 0.0])
+    for index in range(1, 11):
+        name = f'screw_{index}'
+        ref = world_target(standard, g, f'screw:{name}')
+        moved = world_target(shifted, g, f'screw:{name}')
+        assert moved.position == pytest.approx(ref.position + delta, abs=1e-9)
+
+
+def test_cap_seat_follows_the_body():
+    g = geometry()
+    for experiment_id in (1, 3, 11):
+        spec = scenario(experiment_id)
+        body = object_pose_transform(
+            spec.body.x, spec.body.y, spec.body.z, spec.body.yaw_deg, child='body',
+        )
+        seat = world_target(spec, g, 'cap_seat')
+        expected = body.compose(g.body_to_installed_cap)
+        assert seat.position == pytest.approx(expected.position, abs=1e-12)
+        assert seat.matrix == pytest.approx(expected.matrix, abs=1e-12)
+
+
+def test_cap_collision_center_is_below_tcp():
+    g = geometry()
+    assert g.cap_collision.offset[2] == pytest.approx(0.146 - 0.004165, abs=1e-9)
+    assert g.cap_collision.size[2] == pytest.approx(0.00367, abs=1e-12)
+
+
+def test_sealant_contour_is_closed_and_ordered():
+    g = geometry()
+    spec = scenario(18)
+    sides = ('side_1', 'side_2', 'side_3', 'side_4')
+    starts = [world_target(spec, g, f'sealant:{side}') for side in sides]
+    ends = [sealant_point(g, spec, side, 1.0) for side in sides]
+    for index in range(4):
+        assert starts[index].position[2] == pytest.approx(ends[index].position[2])
+        assert starts[(index + 1) % 4].position[:2] == pytest.approx(
+            ends[index].position[:2], abs=1e-9,
+        )
+
+
+def test_tcp_goal_places_tip_on_the_target_and_points_down():
+    g = geometry()
+    spec = scenario(18)
+    target = sealant_point(g, spec, 'side_1', 0.5)
+    tcp = tcp_goal(target, g.tool_work_tips['sealant_tool'])
+    assert tcp.position == pytest.approx(
+        target.position + np.array([0.0, 0.0, 0.140]), abs=1e-12,
+    )
+    assert tcp.matrix[2, 2] == pytest.approx(-1.0, abs=1e-12)
+
+
+def test_tool_and_cap_node_targets_produce_downward_tcp():
+    g = geometry()
+    spec = scenario(1)
+    for target, relative in (
+        (world_target(spec, g, 'tool:vacuum'), g.tool_tcp_to_nodes['vacuum']),
+        (world_target(spec, g, 'cap'), g.cap_tcp_to_node),
+    ):
+        tcp = tcp_goal(target, relative)
+        assert tcp.matrix[2, 2] == pytest.approx(-1.0, abs=1e-9)
+
+
+def test_missing_frames_are_rejected(tmp_path):
+    data = yaml.safe_load(GEOMETRY.read_text(encoding='utf-8'))
+    data['patron_to_tcp']['parent'] = 'missing'
+    data['patron_to_tcp'].pop('parent')
+    path = tmp_path / 'geometry.yaml'
+    path.write_text(yaml.safe_dump(data, sort_keys=False), encoding='utf-8')
+    # The loader supplies parent/child itself, so a missing key is not an
+    # error; a missing transform block is.
+    data.pop('patron_to_tcp')
+    path.write_text(yaml.safe_dump(data, sort_keys=False), encoding='utf-8')
+    with pytest.raises(GeometryError, match='patron_to_tcp'):
+        load_geometry(path)
+
+
+def test_nonnumeric_and_nonpositive_values_are_rejected(tmp_path):
+    data = yaml.safe_load(GEOMETRY.read_text(encoding='utf-8'))
+    data['approach_gap'] = 'far'
+    path = tmp_path / 'geometry.yaml'
+    path.write_text(yaml.safe_dump(data, sort_keys=False), encoding='utf-8')
+    with pytest.raises(GeometryError, match='approach_gap'):
+        load_geometry(path)
+
+    data = yaml.safe_load(GEOMETRY.read_text(encoding='utf-8'))
+    data['contact_gap'] = 0.0
+    path.write_text(yaml.safe_dump(data, sort_keys=False), encoding='utf-8')
+    with pytest.raises(GeometryError, match='contact_gap'):
+        load_geometry(path)
+
+
+def test_unknown_targets_are_rejected():
+    g = geometry()
+    with pytest.raises(GeometryError, match='unknown world target'):
+        world_target(scenario(1), g, 'nothing')
+    with pytest.raises(GeometryError, match='has no tool'):
+        world_target(scenario(1), g, 'tool:magnetic_tool')
+    with pytest.raises(GeometryError, match='has no screw'):
+        world_target(scenario(1), g, 'screw:screw_1')