feat(experement): mirror Webots scene into MoveIt with attachments

src/iiwa_experement/config/geometry.yaml

@@ -122,9 +122,12 @@ speeds:
 # Webots bounding objects so MoveIt and the simulator agree.
 scene_collisions:
   table:
+    # Top at Z = 0.789, deliberately 1 mm below the robot base (Z = 0.790)
+    # and 4 mm below the work surface (Z = 0.793), so the base is not in
+    # collision while downward motion is still blocked.
     type: box
     size: [1.28, 1.08, 0.045]
-    center_z: 0.7705
+    offset_z: 0.7665
   fixture:
     type: box
     size: [0.350, 0.220, 0.041]

src/iiwa_experement/iiwa_experement/geometry.py

@@ -479,7 +479,10 @@ def world_target(spec: ScenarioSpec, geometry: GeometrySpec, name: str) -> Trans
         dx, dy = _rotate_xy(
             hole.dx, hole.dy, spec.body.yaw_deg - geometry.hole_reference_yaw_deg,
         )
-        return _down_transform(
+        # The target is the screw node at the handoff point: upright, its
+        # lower point just above the hole.  tcp_goal() turns it into a
+        # downward-facing tcp with the magnet above the screw head.
+        return object_pose_transform(
             spec.body.x + dx,
             spec.body.y + dy,
             spec.body.z + geometry.hole_z_above_body_origin + geometry.handoff_gap,

src/iiwa_experement/iiwa_experement/planning_scene.py

@@ -0,0 +1,601 @@
+"""Mirror the Webots scene into MoveIt and keep attached objects in sync.
+
+The pure builders in this module are used by the unit tests; the
+``PlanningSceneBridge`` class is the thin ROS adapter used by the reference
+runner.  A trajectory is allowed only after ``assert_synchronized`` has seen
+the expected object ids in the monitored planning scene.
+"""
+from __future__ import annotations
+
+import copy
+import math
+import time
+from dataclasses import dataclass
+from typing import Iterable, Sequence
+
+import numpy as np
+import rclpy
+from geometry_msgs.msg import Pose
+from moveit_msgs.msg import (
+    AttachedCollisionObject,
+    CollisionObject,
+    PlanningScene,
+)
+from moveit_msgs.srv import GetPlanningScene
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
+from shape_msgs.msg import SolidPrimitive
+
+from .geometry import Box, GeometrySpec, Transform, object_pose_transform
+from .scenario import ScenarioSpec
+
+
+SCENE_TOPIC = '/planning_scene'
+MONITORED_TOPIC = '/monitored_planning_scene'
+GET_SCENE_SERVICE = '/get_planning_scene'
+SCENE_FRAME = 'base_link'
+TOUCH_LINKS = ('tcp', 'patron', 'camera_holder')
+
+
+class SceneSynchronizationError(RuntimeError):
+    """The MoveIt scene does not match the requested object set."""
+
+
+def screw_object_id(screw_id: str) -> str:
+    return f'grant_screw_{int(screw_id.split("_")[1]):02d}'
+
+
+@dataclass(frozen=True)
+class SceneObject:
+    object_id: str
+    box: Box
+    pose: Transform
+
+
+@dataclass(frozen=True)
+class AttachedSpec:
+    object_id: str
+    box: Box
+    relative_pose: Transform
+    touch_links: tuple[str, ...]
+
+
+def _box_at(pose: Transform, box: Box) -> Box:
+    del pose
+    return box
+
+
+def _pose_with_offset(pose: Transform, offset) -> Transform:
+    translation = pose.position + pose.matrix @ np.asarray(offset, dtype=float)
+    return Transform(
+        xyz=tuple(float(value) for value in translation),
+        rotation=pose.rotation,
+        parent=pose.parent,
+        child=pose.child,
+    )
+
+
+def _rotated_pose(x: float, y: float, z: float, yaw_deg: float) -> Transform:
+    return Transform.from_xyz_rpy(
+        [x, y, z], [0.0, 0.0, yaw_deg], parent='world', child='scene_object',
+    )
+
+
+def build_world_objects(spec: ScenarioSpec, geometry: GeometrySpec) -> list[SceneObject]:
+    """Return the collision objects present in the initial Webots scene."""
+    objects: list[SceneObject] = []
+    table = geometry.scene_collisions['table']
+    objects.append(SceneObject(
+        'grant_table', table, _rotated_pose(0.0, 0.0, table.offset[2], 0.0),
+    ))
+
+    fixture = geometry.scene_collisions['fixture']
+    objects.append(SceneObject(
+        'grant_fixture',
+        fixture,
+        _rotated_pose(
+            spec.fixture.x,
+            spec.fixture.y,
+            spec.fixture.z + fixture.offset[2],
+            spec.fixture.yaw_deg,
+        ),
+    ))
+
+    body = geometry.scene_collisions['body']
+    objects.append(SceneObject(
+        'grant_body',
+        body,
+        _rotated_pose(
+            spec.body.x,
+            spec.body.y,
+            spec.body.z + body.offset[2],
+            spec.body.yaw_deg,
+        ),
+    ))
+
+    rack = geometry.scene_collisions['rack']
+    objects.append(SceneObject(
+        'grant_rack',
+        rack,
+        _rotated_pose(
+            spec.rack.x,
+            spec.rack.y,
+            spec.rack.z + rack.offset[2],
+            spec.rack.yaw_deg,
+        ),
+    ))
+
+    if spec.tray_pose is not None:
+        tray = spec.tray_pose
+        base = geometry.scene_collisions['tray_base']
+        objects.append(SceneObject(
+            'grant_tray',
+            base,
+            _rotated_pose(
+                tray.x, tray.y, tray.z + base.offset[2], tray.yaw_deg,
+            ),
+        ))
+        wall_x = geometry.scene_collisions['tray_wall_x']
+        for sign, suffix in ((-1.0, 'a'), (1.0, 'b')):
+            dx, dy = math.cos(math.radians(tray.yaw_deg)) * sign * wall_x.offset[0], \
+                math.sin(math.radians(tray.yaw_deg)) * sign * wall_x.offset[0]
+            objects.append(SceneObject(
+                f'grant_tray_wall_x_{suffix}',
+                wall_x,
+                _rotated_pose(
+                    tray.x + dx,
+                    tray.y + dy,
+                    tray.z + wall_x.offset[2],
+                    tray.yaw_deg,
+                ),
+            ))
+        wall_y = geometry.scene_collisions['tray_wall_y']
+        for sign, suffix in ((-1.0, 'a'), (1.0, 'b')):
+            dx = -math.sin(math.radians(tray.yaw_deg)) * sign * wall_y.offset[1]
+            dy = math.cos(math.radians(tray.yaw_deg)) * sign * wall_y.offset[1]
+            objects.append(SceneObject(
+                f'grant_tray_wall_y_{suffix}',
+                wall_y,
+                _rotated_pose(
+                    tray.x + dx,
+                    tray.y + dy,
+                    tray.z + wall_y.offset[2],
+                    tray.yaw_deg,
+                ),
+            ))
+        for screw in spec.screws:
+            objects.append(SceneObject(
+                screw_object_id(screw.name),
+                geometry.screw_collision,
+                _rotated_pose(
+                    screw.pose.x,
+                    screw.pose.y,
+                    screw.pose.z + 0.0045,
+                    screw.pose.yaw_deg,
+                ),
+            ))
+
+    for tool in spec.tools:
+        collision = geometry.tool_collisions[tool.name]
+        size = (collision.size[0], collision.size[1], 0.140)
+        objects.append(SceneObject(
+            f'grant_{tool.name}',
+            Box(size=size, offset=(0.0, 0.0, 0.070)),
+            _rotated_pose(
+                tool.pose.x, tool.pose.y, tool.pose.z + 0.070,
+                tool.pose.yaw_deg + 180.0,
+            ),
+        ))
+
+    if spec.cap_state != 'absent':
+        cap_pose = spec.cap_pose
+        assert cap_pose is not None
+        objects.append(SceneObject(
+            'grant_cap',
+            geometry.cap_collision,
+            _rotated_pose(
+                cap_pose.x, cap_pose.y, cap_pose.z + 0.004165,
+                cap_pose.yaw_deg,
+            ),
+        ))
+    return objects
+
+
+def attached_tool(name: str, geometry: GeometrySpec) -> AttachedSpec:
+    collision = geometry.tool_collisions[name]
+    return AttachedSpec(
+        object_id=f'grant_{name}',
+        box=Box(
+            size=(collision.size[0], collision.size[1], 0.140),
+            offset=collision.offset,
+        ),
+        relative_pose=Transform.from_xyz_rpy(
+            collision.offset, [0.0, 0.0, 0.0], parent='tcp', child=f'grant_{name}',
+        ),
+        touch_links=TOUCH_LINKS,
+    )
+
+
+def attached_cap(geometry: GeometrySpec) -> AttachedSpec:
+    return AttachedSpec(
+        object_id='grant_cap',
+        box=geometry.cap_collision,
+        relative_pose=Transform.from_xyz_rpy(
+            geometry.cap_collision.offset,
+            [0.0, 0.0, 0.0],
+            parent='tcp',
+            child='grant_cap',
+        ),
+        touch_links=TOUCH_LINKS,
+    )
+
+
+def attached_screw(screw_id: str, geometry: GeometrySpec) -> AttachedSpec:
+    object_id = screw_object_id(screw_id)
+    return AttachedSpec(
+        object_id=object_id,
+        box=geometry.screw_collision,
+        relative_pose=Transform.from_xyz_rpy(
+            geometry.screw_collision.offset,
+            [0.0, 0.0, 0.0],
+            parent='tcp',
+            child=object_id,
+        ),
+        touch_links=TOUCH_LINKS,
+    )
+
+
+def _pose_message(transform: Transform) -> Pose:
+    from scipy.spatial.transform import Rotation
+
+    quaternion = Rotation.from_matrix(transform.matrix).as_quat()
+    pose = Pose()
+    pose.position.x, pose.position.y, pose.position.z = (
+        float(transform.xyz[0]),
+        float(transform.xyz[1]),
+        float(transform.xyz[2]),
+    )
+    pose.orientation.x = float(quaternion[0])
+    pose.orientation.y = float(quaternion[1])
+    pose.orientation.z = float(quaternion[2])
+    pose.orientation.w = float(quaternion[3])
+    return pose
+
+
+def collision_object(
+    object_id: str,
+    box: Box,
+    pose: Transform,
+    *,
+    operation: int = CollisionObject.ADD,
+) -> CollisionObject:
+    message = CollisionObject()
+    message.header.frame_id = SCENE_FRAME
+    message.id = object_id
+    message.operation = operation
+    primitive = SolidPrimitive()
+    primitive.type = SolidPrimitive.BOX
+    primitive.dimensions = [float(value) for value in box.size]
+    message.primitives.append(primitive)
+    message.primitive_poses.append(_pose_message(pose))
+    return message
+
+
+def attached_object_message(
+    spec: AttachedSpec,
+    *,
+    link_name: str = 'tcp',
+    operation: int = CollisionObject.ADD,
+) -> AttachedCollisionObject:
+    message = AttachedCollisionObject()
+    message.link_name = link_name
+    message.object = collision_object(
+        spec.object_id, spec.box, spec.relative_pose, operation=operation,
+    )
+    message.touch_links = list(spec.touch_links)
+    return message
+
+
+def update_contact_entry(
+    names: Sequence[str],
+    values,
+    first: str,
+    second: str,
+    allowed: bool,
+) -> tuple[list[str], list[list[bool]]]:
+    """Return a copy of an ACM with exactly one pair changed."""
+    names = list(names)
+    matrix = [list(row) for row in values]
+    for name in (first, second):
+        if name not in names:
+            names.append(name)
+            for row in matrix:
+                row.append(allowed)
+            matrix.append([allowed] * len(names))
+    first_index = names.index(first)
+    second_index = names.index(second)
+    matrix[first_index][second_index] = allowed
+    matrix[second_index][first_index] = allowed
+    return names, matrix
+
+
+def scene_ids(scene: PlanningScene) -> tuple[set[str], set[str]]:
+    world = {obj.id for obj in scene.world.collision_objects}
+    attached = {obj.object.id for obj in scene.robot_state.attached_collision_objects}
+    return world, attached
+
+
+class PlanningSceneBridge:
+    """Thin ROS adapter around a ``moveit_msgs/PlanningScene`` diff stream."""
+
+    def __init__(
+        self,
+        *,
+        node: Node | None = None,
+        use_sim_time: bool = True,
+    ) -> None:
+        self._owns_node = node is None
+        if node is None:
+            node = Node('planning_scene_bridge')
+            node.set_parameters([Parameter(
+                'use_sim_time', value=use_sim_time,
+            )])
+        self.node = node
+        self.geometry: GeometrySpec | None = None
+        self.spec: ScenarioSpec | None = None
+        self.base_to_world: Transform | None = None
+        self.last_scene: PlanningScene | None = None
+        self._publisher = self.node.create_publisher(
+            PlanningScene, SCENE_TOPIC, 10,
+        )
+        self._subscription = self.node.create_subscription(
+            PlanningScene,
+            MONITORED_TOPIC,
+            self._on_scene,
+            QoSProfile(
+                depth=10,
+                reliability=ReliabilityPolicy.RELIABLE,
+                durability=DurabilityPolicy.VOLATILE,
+            ),
+        )
+        self._get_scene = self.node.create_client(
+            GetPlanningScene, GET_SCENE_SERVICE,
+        )
+        self._world_objects: dict[str, SceneObject] = {}
+        self._attached: dict[str, AttachedSpec] = {}
+        self._seen_world: set[str] = set()
+        self._seen_attached: set[str] = set()
+
+    def _on_scene(self, message: PlanningScene) -> None:
+        self._seen_world.update(
+            obj.id for obj in message.world.collision_objects
+        )
+        self._seen_attached.update(
+            entry.object.id
+            for entry in message.robot_state.attached_collision_objects
+        )
+        if not message.is_diff:
+            self.last_scene = copy.deepcopy(message)
+            return
+        if self.last_scene is None:
+            self.last_scene = PlanningScene()
+            self.last_scene.is_diff = False
+        scene = self.last_scene
+        world = {obj.id: obj for obj in scene.world.collision_objects}
+        for obj in message.world.collision_objects:
+            if obj.operation == CollisionObject.REMOVE:
+                world.pop(obj.id, None)
+            else:
+                world[obj.id] = copy.deepcopy(obj)
+        scene.world.collision_objects = list(world.values())
+        attached = {
+            entry.object.id: entry
+            for entry in scene.robot_state.attached_collision_objects
+        }
+        for entry in message.robot_state.attached_collision_objects:
+            if entry.object.operation == CollisionObject.REMOVE:
+                attached.pop(entry.object.id, None)
+            else:
+                attached[entry.object.id] = copy.deepcopy(entry)
+        scene.robot_state.attached_collision_objects = list(attached.values())
+
+    def _spin(self, seconds: float = 0.05) -> None:
+        end = time.monotonic() + seconds
+        while time.monotonic() < end:
+            rclpy.spin_once(self.node, timeout_sec=0.01)
+
+    def _publish(self, scene: PlanningScene) -> None:
+        self._seen_world.clear()
+        self._seen_attached.clear()
+        self._publisher.publish(scene)
+        self._spin()
+
+    @staticmethod
+    def _diff() -> PlanningScene:
+        scene = PlanningScene()
+        scene.is_diff = True
+        scene.robot_state.is_diff = True
+        return scene
+
+    def update_world_to_base(self, world_to_base: Transform) -> None:
+        self.base_to_world = world_to_base.inverse()
+
+    def _base_pose(self, pose: Transform) -> Transform:
+        if self.base_to_world is None:
+            raise SceneSynchronizationError('base transform is not initialised')
+        return self.base_to_world.compose_relaxed(pose)
+
+    def initialize(
+        self,
+        spec: ScenarioSpec,
+        geometry: GeometrySpec,
+        *,
+        world_to_base: Transform | None = None,
+    ) -> None:
+        self.spec = spec
+        self.geometry = geometry
+        self.base_to_world = (world_to_base or geometry.world_to_base).inverse()
+        self._world_objects = {
+            obj.object_id: obj for obj in build_world_objects(spec, geometry)
+        }
+        self._attached.clear()
+        scene = self._diff()
+        for obj in self._world_objects.values():
+            scene.world.collision_objects.append(collision_object(
+                obj.object_id, obj.box, self._base_pose(obj.pose),
+            ))
+        self._publish(scene)
+        self.assert_synchronized(
+            self._world_objects, (), seen_world=set(self._world_objects),
+        )
+
+    def attach(self, spec: AttachedSpec) -> None:
+        if spec.object_id not in self._world_objects:
+            raise SceneSynchronizationError(
+                f'cannot attach {spec.object_id!r}: not a known world object'
+            )
+        scene = self._diff()
+        scene.world.collision_objects.append(collision_object(
+            spec.object_id, spec.box, Transform.from_xyz_rpy(
+                [0, 0, 0], [0, 0, 0], parent='world', child='removed',
+            ), operation=CollisionObject.REMOVE,
+        ))
+        scene.robot_state.attached_collision_objects.append(
+            attached_object_message(spec)
+        )
+        self._world_objects.pop(spec.object_id, None)
+        self._attached[spec.object_id] = spec
+        self._publish(scene)
+
+    def detach(self, object_id: str, world_pose: Transform) -> None:
+        if object_id not in self._attached:
+            raise SceneSynchronizationError(
+                f'cannot detach {object_id!r}: it is not attached'
+            )
+        spec = self._attached.pop(object_id)
+        object_spec = SceneObject(object_id, spec.box, world_pose)
+        self._world_objects[object_id] = object_spec
+        scene = self._diff()
+        scene.robot_state.attached_collision_objects.append(
+            attached_object_message(
+                spec, operation=CollisionObject.REMOVE,
+            )
+        )
+        scene.world.collision_objects.append(collision_object(
+            object_id, spec.box, self._base_pose(world_pose),
+        ))
+        self._publish(scene)
+
+    def remove(self, object_id: str) -> None:
+        scene = self._diff()
+        if object_id in self._attached:
+            spec = self._attached.pop(object_id)
+            scene.robot_state.attached_collision_objects.append(
+                attached_object_message(
+                    spec, operation=CollisionObject.REMOVE,
+                )
+            )
+        if object_id in self._world_objects:
+            self._world_objects.pop(object_id)
+            scene.world.collision_objects.append(collision_object(
+                object_id, Box(size=(0.001, 0.001, 0.001)), Transform.from_xyz_rpy(
+                    [0, 0, 0], [0, 0, 0], parent='world', child='removed',
+                ), operation=CollisionObject.REMOVE,
+            ))
+        self._publish(scene)
+
+    def allow_contact(self, first: str, second: str, allowed: bool = True) -> None:
+        """Change exactly one ACM pair; all other checks stay active."""
+        if self.last_scene is None or not self.last_scene.allowed_collision_matrix.entry_names:
+            raise SceneSynchronizationError(
+                'cannot update contact permissions without a monitored scene'
+            )
+        scene = self._diff()
+        names = list(self.last_scene.allowed_collision_matrix.entry_names)
+        values = [
+            list(row) for row in self.last_scene.allowed_collision_matrix.entry_values
+        ]
+        names, values = update_contact_entry(
+            names, values, first, second, allowed,
+        )
+        scene.allowed_collision_matrix.entry_names = names
+        scene.allowed_collision_matrix.entry_values = values
+        self._publish(scene)
+
+    def expected_sets(self) -> tuple[set[str], set[str]]:
+        return set(self._world_objects), set(self._attached)
+
+    def _query_scene(self, timeout_sec: float) -> tuple[set[str], set[str]] | None:
+        if not self._get_scene.service_is_ready():
+            if not self._get_scene.wait_for_service(timeout_sec=1.0):
+                return None
+        request = GetPlanningScene.Request()
+        request.components.components = 8 | 16 | 4 | 128
+        future = self._get_scene.call_async(request)
+        end = time.monotonic() + timeout_sec
+        while time.monotonic() < end and not future.done():
+            rclpy.spin_once(self.node, timeout_sec=0.05)
+        if not future.done():
+            return None
+        try:
+            response = future.result()
+        except Exception:
+            return None
+        return scene_ids(response.scene)
+
+    def assert_synchronized(
+        self,
+        expected_world: Iterable[str] | None = None,
+        expected_attached: Iterable[str] | None = None,
+        *,
+        seen_world: Iterable[str] | None = None,
+        seen_attached: Iterable[str] | None = None,
+        timeout_sec: float = 8.0,
+    ) -> None:
+        world_expected = set(expected_world) if expected_world is not None else set()
+        attached_expected = (
+            set(expected_attached) if expected_attached is not None else set()
+        )
+        world_seen_expected = set(seen_world) if seen_world is not None else set()
+        attached_seen_expected = (
+            set(seen_attached)
+            if seen_attached is not None
+            else set(attached_expected)
+        )
+        end = time.monotonic() + timeout_sec
+        last_world: set[str] = set()
+        last_attached: set[str] = set()
+        while time.monotonic() < end:
+            rclpy.spin_once(self.node, timeout_sec=0.05)
+            # 1. Every expected id must have appeared in a monitored scene
+            #    (published by the MoveItPy or move_group monitor).
+            served = (
+                world_seen_expected <= self._seen_world
+                and attached_seen_expected <= self._seen_attached
+            )
+            # 2. The move_group service is the authoritative full snapshot.
+            snapshot = self._query_scene(timeout_sec=1.0)
+            if snapshot is not None:
+                last_world, last_attached = snapshot
+                if world_expected <= last_world and attached_expected <= last_attached:
+                    if not served:
+                        # The service proves move_group; still let the next
+                        # monitored message confirm the second monitor.
+                        continue
+                    return
+            elif served:
+                if self.last_scene is not None:
+                    last_world, last_attached = scene_ids(self.last_scene)
+                return
+        raise SceneSynchronizationError(
+            'MoveIt scene is not synchronized: '
+            f'missing world={sorted(world_expected - last_world)}, '
+            f'missing attached={sorted(attached_expected - last_attached)}; '
+            f'seen world={sorted(self._seen_world)}, '
+            f'seen attached={sorted(self._seen_attached)}'
+        )
+
+    def destroy(self) -> None:
+        if self._owns_node:
+            self.node.destroy_node()

src/iiwa_experement/test/test_planning_scene.py

@@ -0,0 +1,166 @@
+"""The MoveIt scene must mirror Webots and keep attachments exclusive."""
+from pathlib import Path
+
+import numpy as np
+import pytest
+from moveit_msgs.msg import CollisionObject, PlanningScene
+
+from iiwa_experement.geometry import Box, Transform, load_geometry
+from iiwa_experement.planning_scene import (
+    PlanningSceneBridge,
+    SceneObject,
+    attached_cap,
+    attached_screw,
+    attached_tool,
+    build_world_objects,
+    collision_object,
+    scene_ids,
+    update_contact_entry,
+)
+from iiwa_experement.scenario import load_scenario
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+SCENES = PACKAGE / 'config' / 'scenes.yaml'
+GEOMETRY = PACKAGE / 'config' / 'geometry.yaml'
+
+
+def spec(experiment_id):
+    return load_scenario(experiment_id, SCENES)
+
+
+class FakeBridge(PlanningSceneBridge):
+    def __init__(self, experiment_id=11):
+        geometry = load_geometry(GEOMETRY)
+        scenario = spec(experiment_id)
+        self.geometry = geometry
+        self.spec = scenario
+        self.base_to_world = geometry.world_to_base.inverse()
+        self._world_objects = {
+            obj.object_id: obj for obj in build_world_objects(scenario, geometry)
+        }
+        self._attached = {}
+        self.last_scene = PlanningScene()
+        self.published = []
+
+    def _publish(self, scene):
+        self.published.append(scene)
+
+    def assert_synchronized(self, *args, **kwargs):
+        return None
+
+
+def test_initial_scene_contains_every_experiment_object():
+    objects = build_world_objects(spec(11), load_geometry(GEOMETRY))
+    ids = {obj.object_id for obj in objects}
+    assert {'grant_table', 'grant_fixture', 'grant_body', 'grant_rack',
+            'grant_tray', 'grant_cap', 'grant_magnetic_tool'} <= ids
+    assert {f'grant_screw_{index:02d}' for index in range(1, 11)} <= ids
+    assert 'grant_vacuum' not in ids
+    # table, fixture, body, rack, tray base, four tray walls, ten screws,
+    # installed hatch and one magnetic tool.
+    assert len(ids) == 4 + 1 + 4 + 10 + 1 + 1
+
+
+def test_experiment_nineteen_moves_tray_and_screws():
+    standard = {obj.object_id: obj for obj in build_world_objects(
+        spec(11), load_geometry(GEOMETRY))}
+    shifted = {obj.object_id: obj for obj in build_world_objects(
+        spec(19), load_geometry(GEOMETRY))}
+    for index in range(1, 11):
+        name = f'grant_screw_{index:02d}'
+        assert shifted[name].pose.position == pytest.approx(
+            standard[name].pose.position + np.array([-0.02, 0.02, 0.0]),
+            abs=1e-9,
+        )
+
+
+def test_collision_boxes_match_measured_meshes():
+    objects = {obj.object_id: obj for obj in build_world_objects(
+        spec(16), load_geometry(GEOMETRY))}
+    assert objects['grant_body'].box.size == pytest.approx((0.30, 0.17, 0.00915))
+    assert objects['grant_cap'].box.size == pytest.approx((0.1384, 0.0984, 0.00367))
+    assert objects['grant_screw_01'].box.size == pytest.approx((0.0055, 0.0055, 0.009))
+    assert objects['grant_vacuum'].box.size[2] == pytest.approx(0.140)
+
+
+def test_collision_object_is_in_the_robot_model_frame():
+    pose = Transform.from_xyz_rpy([0.0, 0.0, 0.0], [0, 0, 0], parent='world', child='x')
+    message = collision_object('obj', Box((0.1, 0.2, 0.3)), pose)
+    assert message.header.frame_id == 'base_link'
+    assert message.id == 'obj'
+    assert list(message.primitives[0].dimensions) == pytest.approx([0.1, 0.2, 0.3])
+
+
+def test_attach_removes_the_world_object_and_adds_one_attachment():
+    bridge = FakeBridge(16)
+    tool = attached_tool('vacuum', bridge.geometry)
+    bridge.attach(tool)
+    assert tool.object_id not in bridge._world_objects
+    assert bridge._attached[tool.object_id] is tool
+    scene = bridge.published[-1]
+    assert scene.is_diff is True
+    assert [obj.id for obj in scene.world.collision_objects] == [tool.object_id]
+    assert scene.world.collision_objects[0].operation == CollisionObject.REMOVE
+    assert len(scene.robot_state.attached_collision_objects) == 1
+    assert scene.robot_state.attached_collision_objects[0].link_name == 'tcp'
+    assert 'tcp' in scene.robot_state.attached_collision_objects[0].touch_links
+
+
+def test_detach_reverses_the_attachment():
+    bridge = FakeBridge(16)
+    tool = attached_tool('vacuum', bridge.geometry)
+    bridge.attach(tool)
+    bridge.detach(tool.object_id, bridge.geometry.world_to_base.compose_relaxed(
+        Transform.from_xyz_rpy([0, 0, 0], [0, 0, 0], parent='world', child='x')
+    ))
+    assert tool.object_id in bridge._world_objects
+    assert tool.object_id not in bridge._attached
+    scene = bridge.published[-1]
+    assert scene.robot_state.attached_collision_objects[0].object.operation == (
+        CollisionObject.REMOVE
+    )
+    assert scene.world.collision_objects[0].operation == CollisionObject.ADD
+
+
+def test_handoff_removes_only_the_delivered_screw():
+    bridge = FakeBridge(11)
+    screw = attached_screw('screw_1', bridge.geometry)
+    bridge.attach(screw)
+    assert 'grant_screw_02' in bridge._world_objects
+    bridge.remove(screw.object_id)
+    assert screw.object_id not in bridge._attached
+    assert screw.object_id not in bridge._world_objects
+    assert 'grant_screw_02' in bridge._world_objects
+
+
+def test_cap_and_screw_attachment_geometry_uses_tcp_offsets():
+    geometry = load_geometry(GEOMETRY)
+    cap = attached_cap(geometry)
+    screw = attached_screw('screw_3', geometry)
+    assert cap.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.141835))
+    assert cap.box.size == pytest.approx((0.1384, 0.0984, 0.00367))
+    assert screw.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.1445))
+
+
+def test_contact_permission_changes_exactly_one_pair():
+    names = ['link1', 'link2', 'tool']
+    values = [[True, False, False], [False, True, False], [False, False, True]]
+    new_names, matrix = update_contact_entry(names, values, 'tool', 'link2', True)
+    assert new_names == names
+    assert matrix[2][1] is True and matrix[1][2] is True
+    assert matrix[2][0] is False and matrix[0][2] is False
+    assert matrix[0][1] is False
+
+
+def test_scene_ids_reports_world_and_attached_sets():
+    scene = PlanningScene()
+    scene.world.collision_objects.append(CollisionObject(id='world_a'))
+    from moveit_msgs.msg import AttachedCollisionObject
+
+    attached = AttachedCollisionObject()
+    attached.object.id = 'attached_a'
+    scene.robot_state.attached_collision_objects.append(attached)
+    world, held = scene_ids(scene)
+    assert world == {'world_a'}
+    assert held == {'attached_a'}