feat(experement): add Webots attachment supervisor and run modes

src/iiwa_experement/iiwa_experement/grant_supervisor.py

@@ -0,0 +1,452 @@
+#!/usr/bin/env python3
+"""Webots supervisor for attachment and object ownership.
+
+The controller is started next to Webots through the experiment launch file.
+It exposes ``/iiwa_experement/object_command`` (webots_ros2_msgs/SetString)
+and publishes ``/iiwa_experement/object_state`` as JSON.  It never commands
+the robot; it only follows the actual ``tcp`` pose when an object is held.
+"""
+from __future__ import annotations
+
+import json
+import math
+import sys
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+import rclpy
+from ament_index_python.packages import get_package_share_directory
+from rclpy.executors import ExternalShutdownException
+from rclpy.node import Node
+from std_msgs.msg import String
+from webots_ros2_msgs.srv import SetString
+
+from .geometry import GeometrySpec, Transform, load_geometry
+from .object_state import CommandError, ObjectState, transform_json
+from .scenario import ScenarioSpec, load_scenario
+
+
+COMMAND_SERVICE = '/iiwa_experement/object_command'
+STATE_TOPIC = '/iiwa_experement/object_state'
+
+
+def screw_def_name(screw_id: str) -> str:
+    parts = screw_id.split('_')
+    if len(parts) != 2 or parts[0] != 'screw' or not parts[1].isdigit():
+        raise ValueError(f'invalid screw id {screw_id!r}')
+    return f'grant_screw_{int(parts[1]):02d}'
+
+
+def _matrix_from_values(values) -> tuple[tuple[float, float, float], ...]:
+    if not isinstance(values, (list, tuple)):
+        raise ValueError('rotation must contain 9 row-major values')
+    if len(values) == 3 and all(
+        isinstance(row, (list, tuple)) and len(row) == 3 for row in values
+    ):
+        return tuple(
+            tuple(float(value) for value in row) for row in values
+        )
+    if len(values) != 9:
+        raise ValueError('rotation must contain 9 row-major values')
+    return tuple(
+        tuple(float(values[row * 3 + column]) for column in range(3))
+        for row in range(3)
+    )
+
+
+def transform_from_json(data: dict[str, Any], child: str = 'target') -> Transform:
+    xyz = data.get('xyz')
+    if not isinstance(xyz, (list, tuple)) or len(xyz) != 3:
+        raise ValueError('pose.xyz must contain three numbers')
+    if 'rotation' in data:
+        rotation = _matrix_from_values(data['rotation'])
+    else:
+        from scipy.spatial.transform import Rotation
+
+        rotation = tuple(
+            tuple(float(value) for value in row)
+            for row in Rotation.from_euler(
+                'xyz', data.get('rpy_deg', [0.0, 0.0, 0.0]), degrees=True
+            ).as_matrix()
+        )
+    return Transform(
+        xyz=tuple(float(value) for value in xyz),
+        rotation=rotation,
+        parent='world',
+        child=child,
+    )
+
+
+def _axis_angle(rotation) -> list[float]:
+    from scipy.spatial.transform import Rotation
+
+    rotvec = Rotation.from_matrix(np.asarray(rotation, dtype=float)).as_rotvec()
+    angle = float(np.linalg.norm(rotvec))
+    if angle < 1e-12:
+        return [0.0, 0.0, 1.0, 0.0]
+    axis = (rotvec / angle).tolist()
+    return [float(axis[0]), float(axis[1]), float(axis[2]), angle]
+
+
+class GrantSupervisorNode(Node):
+    """ROS facade over :class:`ObjectState` and the Webots scene tree."""
+
+    def __init__(self) -> None:
+        super().__init__('grant_attachment_supervisor')
+        package_share = Path(get_package_share_directory('iiwa_experement'))
+        self.declare_parameter('experiment_id', 1)
+        self.declare_parameter(
+            'scene_config', str(package_share / 'config' / 'scenes.yaml'),
+        )
+        self.declare_parameter(
+            'geometry_path', str(package_share / 'config' / 'geometry.yaml'),
+        )
+        self.declare_parameter('read_only', False)
+        experiment_id = int(self.get_parameter('experiment_id').value)
+        scene_config = Path(str(self.get_parameter('scene_config').value))
+        geometry_path = Path(str(self.get_parameter('geometry_path').value))
+        read_only = bool(self.get_parameter('read_only').value)
+        self.spec: ScenarioSpec = load_scenario(experiment_id, scene_config)
+        self.geometry: GeometrySpec = load_geometry(geometry_path)
+        self.state = ObjectState(self.spec, self.geometry, read_only=read_only)
+        self.attached: tuple[str, str] | None = None
+        self.last_command_result: dict[str, Any] | None = None
+        self.rack_poses = {
+            name: entry['pose'] for name, entry in self.state.tools.items()
+        }
+        self.nodes: dict[str, Any] = {}
+        self.patron_node = None
+        self.robot = None
+        self.timestep = 32
+        self._last_publish_time = -1.0
+        self._publisher = self.create_publisher(String, STATE_TOPIC, 10)
+        self._service = self.create_service(
+            SetString, COMMAND_SERVICE, self.handle_command,
+        )
+        self.get_logger().info(
+            f'grant supervisor started for experiment {experiment_id}; '
+            f'read_only={read_only}'
+        )
+
+    # ------------------------------------------------------------------ Webots
+    def _pose_node(self, node):
+        """CadShape children can report an uninitialised matrix in Webots."""
+        if node is None:
+            return None
+        try:
+            parent = node.getParentNode()
+        except Exception:
+            parent = None
+        if parent is not None:
+            position = parent.getPosition()
+            if all(np.isfinite(position)):
+                return parent
+        return node
+
+    def connect(self) -> None:
+        from controller import Supervisor
+
+        self.robot = Supervisor()
+        self.timestep = int(self.robot.getBasicTimeStep())
+        self.patron_node = self._pose_node(self._find_node('patron_visual'))
+        required = self._required_defs()
+        for name in required:
+            node = self._find_node(name)
+            if node is not None:
+                self.nodes[name] = node
+                self._freeze(node)
+        self.get_logger().info(
+            f'supervisor found {len(self.nodes)}/{len(required)} scene objects'
+        )
+
+    def _find_node(self, name: str):
+        try:
+            return self.robot.getFromDef(name)
+        except Exception:
+            return None
+
+    @staticmethod
+    def _freeze(node) -> None:
+        try:
+            field = node.getField('locked')
+            if field is not None:
+                field.setSFBool(True)
+        except Exception:
+            pass
+
+    def _pose_of(self, node) -> Transform | None:
+        if node is None:
+            return None
+        position = node.getPosition()
+        orientation = node.getOrientation()
+        return Transform(
+            xyz=tuple(float(value) for value in position),
+            rotation=_matrix_from_values(orientation),
+            parent='world',
+            child='object',
+        )
+
+    def _set_pose(self, node, transform: Transform) -> None:
+        node.getField('translation').setSFVec3f(
+            [float(value) for value in transform.xyz]
+        )
+        rotation = node.getField('rotation')
+        if rotation is not None:
+            rotation.setSFRotation(_axis_angle(transform.rotation))
+
+    def tcp_pose(self) -> Transform | None:
+        patron = self._pose_of(self.patron_node)
+        if patron is None:
+            return None
+        return patron.compose_relaxed(self.geometry.patron_to_tcp)
+
+    def _refresh_nodes(self) -> None:
+        if self.robot is None:
+            return
+        if self.patron_node is None:
+            self.patron_node = self._pose_node(self._find_node('patron_visual'))
+        for name in self._required_defs():
+            if name not in self.nodes:
+                node = self._find_node(name)
+                if node is not None:
+                    self.nodes[name] = node
+                    self._freeze(node)
+
+    def _required_defs(self) -> list[str]:
+        names = ['grant_fixture', 'grant_body', 'grant_rack']
+        names.extend(f'grant_{tool}' for tool in self.spec.tool_names)
+        if self.spec.tray_pose is not None:
+            names.append('grant_tray')
+            names.extend(f'grant_screw_{index:02d}' for index in range(1, 11))
+        if self.spec.cap_state != 'absent':
+            names.append('grant_cap')
+        return names
+
+    def _node_for_attachment(self) -> Any | None:
+        if self.attached is None:
+            return None
+        kind, name = self.attached
+        if kind == 'tool':
+            return self.nodes.get(f'grant_{name}')
+        if kind == 'cap':
+            return self.nodes.get('grant_cap')
+        if kind == 'screw':
+            return self.nodes.get(screw_def_name(name))
+        return None
+
+    def _relative_transform(self) -> Transform | None:
+        if self.attached is None:
+            return None
+        kind, name = self.attached
+        if kind == 'tool':
+            return self.geometry.tool_tcp_to_nodes[name]
+        if kind == 'cap':
+            return self.geometry.cap_tcp_to_node
+        if kind == 'screw':
+            return self.geometry.screw_tcp_to_node
+        return None
+
+    def follow(self) -> None:
+        if self.attached is None:
+            return
+        tcp = self.tcp_pose()
+        node = self._node_for_attachment()
+        relative = self._relative_transform()
+        if tcp is None or node is None or relative is None:
+            return
+        pose = tcp.compose_relaxed(relative)
+        self._set_pose(node, pose)
+        kind, name = self.attached
+        if kind == 'tool' and name in self.state.tools:
+            self.state.tools[name]['pose'] = pose
+        elif kind == 'cap':
+            self.state.cap_pose = pose
+        elif kind == 'screw' and name in self.state.screws:
+            self.state.screws[name]['pose'] = pose
+
+    # ------------------------------------------------------------------ state
+    def _actual_pose(self, node) -> Transform | None:
+        return self._pose_of(node)
+
+    def _command_pose(self, command: dict[str, Any]) -> Transform:
+        target = command.get('target')
+        if not isinstance(target, dict):
+            raise CommandError('command requires a target pose')
+        return transform_from_json(target, child=str(command.get('object') or 'target'))
+
+    def handle_command(self, request: SetString.Request, response: SetString.Response):
+        try:
+            command = json.loads(request.value)
+            if not isinstance(command, dict):
+                raise CommandError('command must be a JSON object')
+            request_id = str(command.get('request_id', ''))
+            operation = str(command.get('op', ''))
+            accepted, error = self._apply(operation, command, request_id)
+        except Exception as error:  # never leak an exception into Webots
+            accepted = False
+            error = str(error)
+            self.state.last_error = error
+        # The detailed outcome is published in the next object_state message;
+        # the service response only carries the accepted flag.
+        response.success = bool(accepted)
+        self.last_command_result = {
+            'request_id': request_id,
+            'accepted': bool(accepted),
+            'error': None if accepted else error,
+            'version': self.state.version,
+        }
+        self.publish_state(force=True)
+        return response
+
+    def _apply(self, operation: str, command: dict[str, Any], request_id: str):
+        try:
+            return self._apply_impl(operation, command, request_id)
+        except CommandError as error:
+            self.state.last_error = str(error)
+            return False, str(error)
+
+    def _apply_impl(self, operation: str, command: dict[str, Any], request_id: str):
+        if operation == 'inspect':
+            return True, None
+        tcp = self.tcp_pose()
+        if tcp is None:
+            raise CommandError('actual tcp pose is unavailable')
+        kind = str(command.get('object') or '')
+        if operation == 'attach_tool':
+            node = self.nodes.get(f'grant_{kind}')
+            object_pose = self._actual_pose(node)
+            if object_pose is None:
+                raise CommandError(f'tool node grant_{kind} is unavailable')
+            self.state.attach_tool(kind, tcp, object_pose, request_id=request_id)
+            self.attached = ('tool', kind)
+            return True, None
+        if operation == 'dock_tool':
+            node = self.nodes.get(f'grant_{kind}')
+            object_pose = self._actual_pose(node)
+            if object_pose is None:
+                raise CommandError(f'tool node grant_{kind} is unavailable')
+            rack = self.rack_poses[kind]
+            self.state.dock_tool(
+                kind, tcp, rack, object_pose, request_id=request_id,
+            )
+            self.attached = None
+            self._set_pose(node, rack)
+            self.state.tools[kind]['pose'] = rack
+            return True, None
+        if operation == 'grab_cap':
+            node = self.nodes.get('grant_cap')
+            object_pose = self._actual_pose(node)
+            if object_pose is None:
+                raise CommandError('hatch node is unavailable')
+            self.state.grab_cap(tcp, object_pose, request_id=request_id)
+            self.attached = ('cap', 'cap')
+            return True, None
+        if operation == 'place_cap':
+            target = self._command_pose(command)
+            node = self.nodes.get('grant_cap')
+            object_pose = self._actual_pose(node)
+            if object_pose is None:
+                raise CommandError('hatch node is unavailable')
+            self.state.place_cap(target, tcp, object_pose, request_id=request_id)
+            self.attached = None
+            self._set_pose(node, target)
+            self._freeze(node)
+            self.state.cap_pose = target
+            return True, None
+        if operation == 'grab_screw':
+            node = self.nodes.get(screw_def_name(kind))
+            object_pose = self._actual_pose(node)
+            if object_pose is None:
+                raise CommandError(f'screw node for {kind} is unavailable')
+            self.state.grab_screw(kind, tcp, object_pose, request_id=request_id)
+            self.attached = ('screw', kind)
+            return True, None
+        if operation == 'handoff_screw':
+            hole = str(command.get('target_name') or '')
+            target = self._command_pose(command)
+            node = self.nodes.get(screw_def_name(kind))
+            object_pose = self._actual_pose(node)
+            if object_pose is None:
+                raise CommandError(f'screw node for {kind} is unavailable')
+            self.state.handoff_screw(
+                kind, hole, target, tcp, object_pose, request_id=request_id,
+            )
+            self.attached = None
+            if node is not None:
+                try:
+                    node.remove()
+                except Exception:
+                    pass
+            self.nodes.pop(screw_def_name(kind), None)
+            return True, None
+        raise CommandError(f'unknown supervisor operation {operation!r}')
+
+    def publish_state(self, *, force: bool = False) -> None:
+        if self.robot is None:
+            return
+        now = float(self.robot.getTime())
+        if not force and now - self._last_publish_time < 0.1:
+            return
+        self._last_publish_time = now
+        payload = self.state.snapshot()
+        payload['simulation_time'] = now
+        payload['ready'] = self.patron_node is not None and all(
+            name in self.nodes for name in self._required_defs()
+        )
+        payload['attached'] = (
+            None if self.attached is None else list(self.attached)
+        )
+        tcp = self.tcp_pose()
+        payload['tcp'] = transform_json(tcp)
+        payload['last_command_result'] = self.last_command_result
+        payload['objects'] = {
+            name: transform_json(self._pose_of(node))
+            for name, node in self.nodes.items()
+        }
+        message = String()
+        message.data = json.dumps(payload)
+        self._publisher.publish(message)
+
+    def run(self) -> None:
+        self.connect()
+        while self.robot.step(self.timestep) != -1:
+            try:
+                rclpy.spin_once(self, timeout_sec=0.0)
+                self._refresh_nodes()
+                self.follow()
+                self.publish_state()
+            except Exception as error:  # keep the controller alive for diagnosis
+                self.get_logger().error(f'supervisor step failed: {error}')
+
+
+def _publish_unavailable(node: Node, message: str) -> None:
+    del node
+    print(f'[iiwa_experement] grant supervisor: {message}', file=sys.stderr)
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    exit_code = 0
+    node = None
+    try:
+        node = GrantSupervisorNode()
+        node.run()
+    except (KeyboardInterrupt, ExternalShutdownException):
+        pass
+    except Exception as error:
+        print(
+            f'[iiwa_experement] grant supervisor: {error}',
+            file=sys.stderr,
+        )
+        exit_code = 1
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+    if exit_code:
+        raise SystemExit(exit_code)
+
+if __name__ == '__main__':
+    main()

src/iiwa_experement/iiwa_experement/object_state.py

@@ -0,0 +1,366 @@
+"""Deterministic object ownership for the grant reference executor.
+
+The state machine is deliberately independent of ROS and Webots.  The
+supervisor translates accepted commands into Webots field updates and the
+reference runner uses the same transitions in its table tests.
+"""
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, replace
+from typing import Any
+
+import numpy as np
+
+from .geometry import GeometrySpec, Transform, object_pose_transform, world_target
+from .scenario import ScenarioSpec
+
+
+CAP_STATES = ('absent', 'free', 'installed', 'held')
+SCREW_STATES = ('tray', 'held', 'delivered')
+TOOL_STATES = ('docked', 'mounted')
+
+
+class CommandError(RuntimeError):
+    """A command was rejected; the state is unchanged."""
+
+
+def transform_json(transform: Transform | None) -> dict[str, Any] | None:
+    if transform is None:
+        return None
+    return {
+        'xyz': [float(value) for value in transform.xyz],
+        'rotation': [
+            [float(value) for value in row] for row in transform.rotation
+        ],
+        'parent': transform.parent,
+        'child': transform.child,
+    }
+
+
+def _position_distance(first: Transform, second: Transform) -> float:
+    return float(np.linalg.norm(first.position - second.position))
+
+
+def _angle_distance(first: Transform, second: Transform) -> float:
+    cosine = (np.trace(first.matrix.T @ second.matrix) - 1.0) / 2.0
+    return float(np.arccos(max(-1.0, min(1.0, cosine))))
+
+
+class ObjectState:
+    """Ownership and delivery state for one experiment."""
+
+    def __init__(
+        self,
+        spec: ScenarioSpec,
+        geometry: GeometrySpec,
+        *,
+        read_only: bool = False,
+    ) -> None:
+        self.spec = spec
+        self.geometry = geometry
+        self.read_only = read_only
+        self.version = 0
+        self.last_request_id: str | None = None
+        self.last_error: str | None = None
+        self.mounted_tool: str | None = None
+        self.held_object: str | None = None
+        self.cap_state = spec.cap_state
+        self.cap_pose = (
+            world_target(spec, geometry, 'cap')
+            if spec.cap_state != 'absent'
+            else None
+        )
+        self.tools: dict[str, dict[str, Any]] = {}
+        for tool in spec.tools:
+            self.tools[tool.name] = {
+                'state': 'docked',
+                'pose': world_target(spec, geometry, f'tool:{tool.name}'),
+            }
+        self.screws: dict[str, dict[str, Any]] = {}
+        for screw in spec.screws:
+            self.screws[screw.name] = {
+                'state': 'tray',
+                'pose': world_target(spec, geometry, f'screw:{screw.name}'),
+                'hole': None,
+            }
+        self.holes = {
+            name: {'state': 'empty', 'screw': None}
+            for name in ('hole_1', 'hole_2', 'hole_3', 'hole_4')
+        }
+
+    # ------------------------------------------------------------------ helpers
+    def _reject(self, message: str) -> None:
+        self.last_error = message
+        raise CommandError(message)
+
+    def _accept(self, request_id: str | None) -> None:
+        self.version += 1
+        self.last_request_id = request_id
+        self.last_error = None
+
+    def _require_not_read_only(self) -> None:
+        if self.read_only:
+            self._reject('supervisor is in read-only view mode')
+
+    def _require_instrument(self, instrument: str, operation: str) -> None:
+        if self.mounted_tool != instrument:
+            self._reject(
+                f'{operation} requires {instrument!r} in patron; '
+                f'mounted tool is {self.mounted_tool!r}'
+            )
+
+    def _require_free_hand(self, operation: str) -> None:
+        if self.held_object is not None:
+            self._reject(
+                f'{operation} is forbidden while {self.held_object!r} is held'
+            )
+
+    def _require_close(
+        self,
+        actual: Transform,
+        expected: Transform,
+        operation: str,
+    ) -> None:
+        if actual.parent != expected.parent:
+            self._reject(f'{operation}: pose frames do not match')
+        distance = _position_distance(actual, expected)
+        angle = _angle_distance(actual, expected)
+        if distance > self.geometry.position_tolerance:
+            self._reject(
+                f'{operation}: tcp is {distance:.4f} m from the contact pose '
+                f'(limit {self.geometry.position_tolerance:.4f} m)'
+            )
+        if angle > self.geometry.orientation_tolerance_rad:
+            self._reject(
+                f'{operation}: tcp orientation error is {angle:.4f} rad '
+                f'(limit {self.geometry.orientation_tolerance_rad:.4f} rad)'
+            )
+
+    def _expected_tcp(self, node_pose: Transform, tcp_to_node: Transform) -> Transform:
+        return node_pose.compose_relaxed(tcp_to_node.inverse())
+
+    # ------------------------------------------------------------------ tools
+    def attach_tool(
+        self,
+        name: str,
+        tcp: Transform,
+        object_pose: Transform,
+        *,
+        request_id: str | None = None,
+    ) -> None:
+        self._require_not_read_only()
+        if self.mounted_tool is not None:
+            self._reject(f'another tool {self.mounted_tool!r} is already mounted')
+        if name not in self.tools:
+            self._reject(f'tool {name!r} is not present in this experiment')
+        tool = self.tools[name]
+        if tool['state'] != 'docked':
+            self._reject(f'tool {name!r} is not in its socket')
+        if _position_distance(object_pose, tool['pose']) > self.geometry.position_tolerance:
+            self._reject(
+                f'tool {name!r} moved away from its socket; refusing blind attach'
+            )
+        self._require_free_hand('attach_tool')
+        self._require_close(
+            tcp,
+            self._expected_tcp(object_pose, self.geometry.tool_tcp_to_nodes[name]),
+            f'attach_tool({name})',
+        )
+        tool['state'] = 'mounted'
+        self.mounted_tool = name
+        self._accept(request_id)
+
+    def dock_tool(
+        self,
+        name: str,
+        tcp: Transform,
+        target: Transform,
+        object_pose: Transform,
+        *,
+        request_id: str | None = None,
+    ) -> None:
+        self._require_not_read_only()
+        if self.mounted_tool != name:
+            self._reject(f'tool {name!r} is not the mounted tool')
+        if name not in self.tools:
+            self._reject(f'tool {name!r} is not present in this experiment')
+        # The tool follows tcp, so the contact pose is only meaningful when
+        # compared with the known socket target.
+        self._require_close(
+            tcp,
+            self._expected_tcp(target, self.geometry.tool_tcp_to_nodes[name]),
+            f'dock_tool({name})',
+        )
+        self._require_close(
+            object_pose,
+            tcp.compose_relaxed(self.geometry.tool_tcp_to_nodes[name]),
+            f'dock_tool({name}) attached object',
+        )
+        tool = self.tools[name]
+        tool['state'] = 'docked'
+        tool['pose'] = target
+        self.mounted_tool = None
+        self._accept(request_id)
+
+    # ------------------------------------------------------------------ hatch
+    def grab_cap(
+        self,
+        tcp: Transform,
+        object_pose: Transform,
+        *,
+        request_id: str | None = None,
+    ) -> None:
+        self._require_not_read_only()
+        self._require_instrument('vacuum', 'grab_cap')
+        self._require_free_hand('grab_cap')
+        if self.cap_state not in ('free', 'installed'):
+            self._reject(f'hatch is not available (state={self.cap_state!r})')
+        self._require_close(
+            tcp,
+            self._expected_tcp(object_pose, self.geometry.cap_tcp_to_node),
+            'grab_cap',
+        )
+        self.cap_state = 'held'
+        self.cap_pose = object_pose
+        self.held_object = 'cap'
+        self._accept(request_id)
+
+    def place_cap(
+        self,
+        target: Transform,
+        tcp: Transform,
+        object_pose: Transform,
+        *,
+        request_id: str | None = None,
+    ) -> None:
+        self._require_not_read_only()
+        self._require_instrument('vacuum', 'place_cap')
+        if self.held_object != 'cap':
+            self._reject('place_cap requires the hatch to be held')
+        self._require_close(
+            tcp,
+            self._expected_tcp(target, self.geometry.cap_tcp_to_node),
+            'place_cap',
+        )
+        if _position_distance(object_pose, tcp.compose_relaxed(self.geometry.cap_tcp_to_node)) > (
+            self.geometry.position_tolerance
+        ):
+            self._reject('place_cap: hatch does not follow the suction cup')
+        self.cap_state = 'installed'
+        self.cap_pose = target
+        self.held_object = None
+        self._accept(request_id)
+
+    # ------------------------------------------------------------------ screws
+    def grab_screw(
+        self,
+        screw_id: str,
+        tcp: Transform,
+        object_pose: Transform,
+        *,
+        request_id: str | None = None,
+    ) -> None:
+        self._require_not_read_only()
+        self._require_instrument('magnetic_tool', 'grab_screw')
+        self._require_free_hand('grab_screw')
+        if screw_id not in self.screws:
+            self._reject(f'screw {screw_id!r} is not present in this experiment')
+        screw = self.screws[screw_id]
+        if screw['state'] != 'tray':
+            self._reject(f'screw {screw_id!r} is unavailable ({screw["state"]})')
+        self._require_close(
+            tcp,
+            self._expected_tcp(object_pose, self.geometry.screw_tcp_to_node),
+            f'grab_screw({screw_id})',
+        )
+        screw['state'] = 'held'
+        screw['pose'] = object_pose
+        self.held_object = f'screw:{screw_id}'
+        self._accept(request_id)
+
+    def handoff_screw(
+        self,
+        screw_id: str,
+        hole_id: str,
+        target: Transform,
+        tcp: Transform,
+        object_pose: Transform,
+        *,
+        request_id: str | None = None,
+    ) -> None:
+        self._require_not_read_only()
+        self._require_instrument('magnetic_tool', 'handoff_screw')
+        if self.held_object != f'screw:{screw_id}':
+            self._reject(f'screw {screw_id!r} is not held')
+        if screw_id not in self.screws:
+            self._reject(f'screw {screw_id!r} is not present in this experiment')
+        screw = self.screws[screw_id]
+        if screw['state'] != 'held':
+            self._reject(f'screw {screw_id!r} is not held ({screw["state"]})')
+        if hole_id not in self.holes:
+            self._reject(f'unknown hole {hole_id!r}')
+        if self.holes[hole_id]['state'] != 'empty':
+            self._reject(f'hole {hole_id!r} already has a delivered screw')
+        if screw['hole'] == hole_id:
+            self._reject(f'screw {screw_id!r} was already delivered to {hole_id!r}')
+        self._require_close(
+            tcp,
+            self._expected_tcp(target, self.geometry.screw_tcp_to_node),
+            f'handoff_screw({screw_id},{hole_id})',
+        )
+        expected_object = tcp.compose_relaxed(self.geometry.screw_tcp_to_node)
+        if _position_distance(object_pose, expected_object) > self.geometry.position_tolerance:
+            self._reject('handoff_screw: screw does not follow the magnet')
+        screw['state'] = 'delivered'
+        screw['hole'] = hole_id
+        screw['pose'] = target
+        self.holes[hole_id]['state'] = 'empty'
+        self.holes[hole_id]['screw'] = screw_id
+        self.held_object = None
+        self._accept(request_id)
+
+    # ------------------------------------------------------------------ inspect
+    def snapshot(self) -> dict[str, Any]:
+        return {
+            'version': self.version,
+            'experiment_id': self.spec.experiment_id,
+            'mounted_tool': self.mounted_tool,
+            'held_object': self.held_object,
+            'read_only': self.read_only,
+            'cap': {
+                'state': self.cap_state,
+                'pose': transform_json(self.cap_pose),
+            },
+            'tools': {
+                name: {
+                    'state': entry['state'],
+                    'pose': transform_json(entry['pose']),
+                }
+                for name, entry in self.tools.items()
+            },
+            'screws': {
+                name: {
+                    'state': entry['state'],
+                    'pose': transform_json(entry['pose']),
+                    'hole': entry['hole'],
+                }
+                for name, entry in self.screws.items()
+            },
+            'holes': {
+                name: {
+                    'state': entry['state'],
+                    'screw': entry['screw'],
+                }
+                for name, entry in self.holes.items()
+            },
+            'last_request_id': self.last_request_id,
+            'last_error': self.last_error,
+        }
+
+    def delivered_pairs(self) -> list[tuple[str, str]]:
+        return [
+            (name, entry['hole'])
+            for name, entry in self.screws.items()
+            if entry['state'] == 'delivered'
+        ]

src/iiwa_experement/iiwa_experement/runtime_config.py

@@ -8,6 +8,7 @@ legacy random object spawner.
 """
 from __future__ import annotations
 
+import math
 import re
 from pathlib import Path
 from typing import Any
@@ -16,6 +17,9 @@ import yaml
 
 
 EXPERIMENT_IDS = tuple(range(1, 21))
+MODES = ('view', 'execute')
+DEFAULT_START_DELAY_SEC = 3.0
+DEFAULT_TRACE_DIR = '~/.ros/iiwa_experement/runs'
 LOCAL_PROTOS = (
     'CollaborativeRobotTable.proto',
     'WorkspaceLimiter.proto',
@@ -36,6 +40,37 @@ def validate_experiment_id(raw: str) -> int:
     return value
 
 
+def validate_mode(raw: str) -> str:
+    """Accept only the documented view/execute modes."""
+    text = str(raw).strip().lower()
+    if text not in MODES:
+        raise ValueError(f'mode must be one of {list(MODES)}, got {raw!r}')
+    return text
+
+
+def validate_start_delay(raw: str) -> float:
+    """Return a finite non-negative simulation-time delay."""
+    text = str(raw).strip()
+    try:
+        value = float(text)
+    except (TypeError, ValueError) as error:
+        raise ValueError(
+            f'start_delay_sec must be a finite non-negative number, got {raw!r}'
+        ) from error
+    if not math.isfinite(value) or value < 0.0:
+        raise ValueError(
+            f'start_delay_sec must be a finite non-negative number, got {raw!r}'
+        )
+    return value
+
+
+def validate_trace_dir(raw: str) -> str:
+    text = str(raw).strip()
+    if not text:
+        raise ValueError('trace_dir must not be empty')
+    return text
+
+
 def _absolute_proto(source: str, description_share: Path) -> str:
     if source.startswith(('http://', 'https://')):
         return source

src/iiwa_experement/iiwa_experement/webots_nodes.py

@@ -148,7 +148,7 @@ def _solid(
     physics_mass: float | None = None,
 ) -> str:
     lines = [
-        'Solid {',
+        f'DEF {name} Solid {{',
         f'  translation {_vector(translation)}',
         f'  rotation {_vector(rotation)}',
         f'  name "{name}"',

src/iiwa_experement/launch/experiment.launch.py

@@ -2,13 +2,15 @@
 from __future__ import annotations
 
 import shutil
+import sys
 import tempfile
 from pathlib import Path
 
-from ament_index_python.packages import get_package_share_directory
+from ament_index_python.packages import get_package_share_directory, get_package_prefix
 from launch import LaunchDescription
 from launch.actions import (
     DeclareLaunchArgument,
+    ExecuteProcess,
     IncludeLaunchDescription,
     OpaqueFunction,
     RegisterEventHandler,
@@ -18,15 +20,23 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
+from webots_ros2_driver.utils import controller_url_prefix
 
 from iiwa_experement.runtime_config import (
+    DEFAULT_START_DELAY_SEC,
+    DEFAULT_TRACE_DIR,
     prepare_runtime_files,
     validate_experiment_id,
+    validate_mode,
+    validate_start_delay,
+    validate_trace_dir,
 )
 
 
 BRINGUP_LAUNCH = ('launch', 'iiwa.launch.py')
 SCENE_SPAWNER = 'scene_spawner'
+SUPERVISOR = 'grant_supervisor'
+SUPERVISOR_ROBOT = 'GrantAttachmentSupervisor'
 
 
 def spawner_parameters(experiment_id: int, scene_config: str) -> dict:
@@ -37,14 +47,86 @@ def spawner_parameters(experiment_id: int, scene_config: str) -> dict:
     }
 
 
+def supervisor_parameters(
+    experiment_id: int,
+    scene_config: str,
+    geometry_path: str,
+    read_only: bool,
+) -> dict:
+    return {
+        'experiment_id': int(experiment_id),
+        'scene_config': str(scene_config),
+        'geometry_path': str(geometry_path),
+        'read_only': bool(read_only),
+    }
+
+
+def runner_parameters(
+    experiment_id: int,
+    scene_config: str,
+    geometry_path: str,
+    start_delay_sec: float,
+    trace_dir: str,
+) -> dict:
+    return {
+        'experiment_id': int(experiment_id),
+        'scene_config': str(scene_config),
+        'geometry_path': str(geometry_path),
+        'start_delay_sec': float(start_delay_sec),
+        'trace_dir': str(trace_dir),
+    }
+
+
+def supervisor_process(
+    experiment_id: int,
+    scene_config: str,
+    geometry_path: str,
+    *,
+    read_only: bool,
+) -> ExecuteProcess:
+    """Start the Webots supervisor beside Webots.
+
+    The process must exist before the scene is spawned: Webots waits for all
+    ``<extern>`` controllers, so a later start would deadlock the simulation.
+    """
+    parameters = supervisor_parameters(
+        experiment_id, scene_config, geometry_path, read_only,
+    )
+    arguments = []
+    for key, value in parameters.items():
+        text = str(value).lower() if isinstance(value, bool) else str(value)
+        arguments.extend(['-p', f'{key}:={text}'])
+    return ExecuteProcess(
+        cmd=[
+            sys.executable, '-m', 'iiwa_experement.grant_supervisor',
+            '--ros-args', *arguments,
+        ],
+        name=SUPERVISOR,
+        output='screen',
+        additional_env={
+            'WEBOTS_CONTROLLER_URL': controller_url_prefix('1234') + SUPERVISOR_ROBOT,
+            'WEBOTS_HOME': get_package_prefix('webots_ros2_driver'),
+        },
+    )
+
+
 def runtime_actions(
     experiment_id: int,
     scene_config: str,
     setting_path: str,
     rviz: str = 'false',
     cleanup_dir: str | None = None,
+    *,
+    mode: str = 'view',
+    start_delay_sec: float = DEFAULT_START_DELAY_SEC,
+    trace_dir: str = DEFAULT_TRACE_DIR,
+    geometry_path: str | None = None,
 ) -> list:
     """Build the actions after validation has already succeeded."""
+    package_share = Path(get_package_share_directory('iiwa_experement'))
+    if geometry_path is None:
+        geometry_path = str(package_share / 'config' / 'geometry.yaml')
+    read_only = mode != 'execute'
     actions = [
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
@@ -65,7 +147,24 @@ def runtime_actions(
             output='screen',
             parameters=[spawner_parameters(experiment_id, scene_config)],
         ),
+        supervisor_process(
+            experiment_id,
+            scene_config,
+            str(geometry_path),
+            read_only=read_only,
+        ),
     ]
+    if mode == 'execute':
+        actions.append(Node(
+            package='iiwa_experement',
+            executable='reference_runner',
+            name='reference_runner',
+            output='screen',
+            parameters=[runner_parameters(
+                experiment_id, scene_config, geometry_path,
+                start_delay_sec, trace_dir,
+            )],
+        ))
     if cleanup_dir is not None:
         actions.append(RegisterEventHandler(
             OnShutdown(on_shutdown=[OpaqueFunction(
@@ -90,9 +189,31 @@ def _setup(context, *args, **kwargs):
     except ValueError as error:
         raise RuntimeError(f'Invalid experiment_id: {error}') from error
 
+    try:
+        mode = validate_mode(LaunchConfiguration('mode').perform(context))
+    except ValueError as error:
+        raise RuntimeError(f'Invalid mode: {error}') from error
+    try:
+        start_delay_sec = validate_start_delay(
+            LaunchConfiguration('start_delay_sec').perform(context)
+        )
+    except ValueError as error:
+        raise RuntimeError(f'Invalid start_delay_sec: {error}') from error
+    try:
+        trace_dir = validate_trace_dir(
+            LaunchConfiguration('trace_dir').perform(context)
+        )
+    except ValueError as error:
+        raise RuntimeError(f'Invalid trace_dir: {error}') from error
+
     scene_config = str(LaunchConfiguration('scene_config').perform(context))
     base_setting = str(LaunchConfiguration('base_setting').perform(context))
     rviz = str(LaunchConfiguration('rviz').perform(context)).lower()
+    geometry_path = str(
+        Path(get_package_share_directory('iiwa_experement'))
+        / 'config'
+        / 'geometry.yaml'
+    )
 
     package_share = Path(get_package_share_directory('iiwa_experement'))
     description_share = Path(get_package_share_directory('iiwa_description'))
@@ -110,13 +231,20 @@ def _setup(context, *args, **kwargs):
         shutil.rmtree(output_dir, ignore_errors=True)
         raise
 
-    print(f'[iiwa_experement] experiment {experiment_id}: {world_path}')
+    print(
+        f'[iiwa_experement] experiment {experiment_id} mode={mode} '
+        f'delay={start_delay_sec:g}s: {world_path}'
+    )
     return runtime_actions(
         experiment_id=experiment_id,
         scene_config=scene_config,
         setting_path=str(setting_path),
         rviz=rviz,
         cleanup_dir=str(output_dir),
+        mode=mode,
+        start_delay_sec=start_delay_sec,
+        trace_dir=trace_dir,
+        geometry_path=geometry_path,
     )
 
 
@@ -127,6 +255,24 @@ def generate_launch_description():
             default_value='',
             description='Experiment number 1..20; required.',
         ),
+        DeclareLaunchArgument(
+            'mode',
+            default_value='view',
+            description=(
+                'view = open the prepared scene without motion; '
+                'execute = run the reference sequence after start_delay_sec.'
+            ),
+        ),
+        DeclareLaunchArgument(
+            'start_delay_sec',
+            default_value=f'{DEFAULT_START_DELAY_SEC:g}',
+            description='Simulation-time delay before the first execute motion.',
+        ),
+        DeclareLaunchArgument(
+            'trace_dir',
+            default_value=DEFAULT_TRACE_DIR,
+            description='Directory for one JSONL trace per run.',
+        ),
         DeclareLaunchArgument(
             'rviz',
             default_value='false',

src/iiwa_experement/package.xml

@@ -10,14 +10,24 @@
   <buildtool_depend>ament_python</buildtool_depend>
 
   <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>iiwa_bringup</exec_depend>
   <exec_depend>iiwa_config</exec_depend>
   <exec_depend>iiwa_description</exec_depend>
+  <exec_depend>iiwa_msgs</exec_depend>
   <exec_depend>launch</exec_depend>
   <exec_depend>launch_ros</exec_depend>
+  <exec_depend>moveit_msgs</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-scipy</exec_depend>
   <exec_depend>python3-yaml</exec_depend>
   <exec_depend>rclpy</exec_depend>
+  <exec_depend>rosgraph_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
   <exec_depend>std_srvs</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>webots_ros2_driver</exec_depend>
   <exec_depend>webots_ros2_msgs</exec_depend>
 
   <test_depend>iiwa_utils</test_depend>

src/iiwa_experement/setup.py

@@ -26,6 +26,7 @@ setup(
     entry_points={
         'console_scripts': [
             'scene_spawner = iiwa_experement.scene_spawner:main',
+            'grant_supervisor = iiwa_experement.grant_supervisor:main',
         ],
     },
 )

src/iiwa_experement/test/test_grant_supervisor.py

@@ -0,0 +1,220 @@
+"""Supervisor command plumbing with fake Webots nodes (no simulator run)."""
+import math
+from pathlib import Path
+
+import numpy as np
+import pytest
+
+from iiwa_experement.geometry import load_geometry, tcp_goal, world_target
+from iiwa_experement.grant_supervisor import (
+    COMMAND_SERVICE,
+    GrantSupervisorNode,
+    _axis_angle,
+    transform_from_json,
+)
+from iiwa_experement.object_state import ObjectState, transform_json
+from iiwa_experement.scenario import load_scenario
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+SCENES = PACKAGE / 'config' / 'scenes.yaml'
+GEOMETRY = PACKAGE / 'config' / 'geometry.yaml'
+
+
+class FakeField:
+    def __init__(self, value):
+        self.value = value
+        self.removed = False
+
+    def setSFVec3f(self, value):
+        self.value = list(value)
+
+    def setSFRotation(self, value):
+        self.value = list(value)
+
+    def setSFBool(self, value):
+        self.value = bool(value)
+
+    def remove(self):
+        self.removed = True
+
+
+class FakeNode:
+    def __init__(self, transform):
+        self.translation = FakeField(list(transform.xyz))
+        self.rotation = FakeField(_axis_angle(transform.rotation))
+        self.locked = FakeField(True)
+        self.removed = False
+        self._matrix = transform.matrix
+
+    def getPosition(self):
+        return list(self.translation.value)
+
+    def getOrientation(self):
+        return [float(value) for value in self._matrix.reshape(-1)]
+
+    def getField(self, name):
+        if name == 'translation':
+            return self.translation
+        if name == 'rotation':
+            return self.rotation
+        if name == 'locked':
+            return self.locked
+        return None
+
+    def remove(self):
+        self.removed = True
+
+
+class FakeRobot:
+    def getTime(self):
+        return 0.0
+
+
+def make_node(experiment_id=1):
+    geometry = load_geometry(GEOMETRY)
+    spec = load_scenario(experiment_id, SCENES)
+    node = object.__new__(GrantSupervisorNode)
+    node.spec = spec
+    node.geometry = geometry
+    node.state = ObjectState(spec, geometry)
+    node.attached = None
+    node.rack_poses = {
+        name: entry['pose'] for name, entry in node.state.tools.items()
+    }
+    node.robot = FakeRobot()
+    node.patron_node = None
+    node.nodes = {}
+    for tool in spec.tools:
+        pose = world_target(spec, geometry, f'tool:{tool.name}')
+        node.nodes[f'grant_{tool.name}'] = FakeNode(pose)
+    if spec.tray_pose is not None:
+        for index in range(1, 11):
+            name = f'screw_{index}'
+            node.nodes[f'grant_screw_{index:02d}'] = FakeNode(
+                world_target(spec, geometry, f'screw:{name}')
+            )
+    if spec.cap_state != 'absent':
+        node.nodes['grant_cap'] = FakeNode(world_target(spec, geometry, 'cap'))
+    return node
+
+
+def set_tcp(node, tcp):
+    patron = tcp.compose_relaxed(node.geometry.patron_to_tcp.inverse())
+    node.patron_node = FakeNode(patron)
+
+
+def test_command_service_name_is_stable():
+    assert COMMAND_SERVICE == '/iiwa_experement/object_command'
+
+
+def test_transform_json_roundtrip():
+    from iiwa_experement.geometry import Transform
+
+    transform = Transform.from_xyz_rpy(
+        [0.1, -0.2, 0.3], [10.0, 20.0, 30.0], parent='world', child='x',
+    )
+    payload = transform_json(transform)
+    restored = transform_from_json(payload)
+    assert restored.xyz == pytest.approx(transform.xyz)
+    assert restored.matrix == pytest.approx(transform.matrix)
+
+
+def test_invalid_pose_payload_is_rejected():
+    with pytest.raises(ValueError, match='xyz'):
+        transform_from_json({'rotation': [1, 0, 0, 0, 1, 0, 0, 0, 1]})
+    with pytest.raises(ValueError, match='rotation'):
+        transform_from_json({'xyz': [0, 0, 0], 'rotation': [1, 0, 0]})
+
+
+def test_axis_angle_identity_and_half_turn():
+    assert _axis_angle(np.eye(3)) == [0.0, 0.0, 1.0, 0.0]
+    from scipy.spatial.transform import Rotation
+
+    axis = _axis_angle(Rotation.from_euler('z', 90, degrees=True).as_matrix())
+    assert abs(axis[3]) == pytest.approx(math.pi / 2)
+    assert abs(axis[2]) == pytest.approx(1.0)
+
+
+def test_attach_requires_tcp_at_the_socket_and_rejects_far_pose():
+    node = make_node(1)
+    pose = node.state.tools['vacuum']['pose']
+    good = tcp_goal(pose, node.geometry.tool_tcp_to_nodes['vacuum'])
+    set_tcp(node, good)
+    accepted, error = node._apply('attach_tool', {'object': 'vacuum'}, 'r1')
+    assert accepted is True
+    assert error is None
+    assert node.attached == ('tool', 'vacuum')
+
+    other = make_node(1)
+    set_tcp(other, good.compose_relaxed(
+        type(good).from_xyz_rpy(
+            [0.0, 0.0, 0.08], [0, 0, 0], parent='world', child='tcp',
+        )
+    ))
+    accepted, error = other._apply('attach_tool', {'object': 'vacuum'}, 'r2')
+    assert accepted is False
+    assert 'tcp is' in error
+    assert other.attached is None
+    assert other.state.version == 0
+
+
+def test_follow_moves_the_attached_tool_and_dock_returns_it_to_the_rack():
+    node = make_node(1)
+    rack_pose = node.state.tools['vacuum']['pose']
+    tcp = tcp_goal(rack_pose, node.geometry.tool_tcp_to_nodes['vacuum'])
+    set_tcp(node, tcp)
+    assert node._apply('attach_tool', {'object': 'vacuum'}, 'attach')[0]
+    node.follow()
+    moved = node.nodes['grant_vacuum'].getPosition()
+    assert moved == pytest.approx(rack_pose.position, abs=1e-9)
+    accepted, _ = node._apply('dock_tool', {'object': 'vacuum'}, 'dock')
+    assert accepted
+    assert node.attached is None
+    assert node.state.mounted_tool is None
+    assert node.nodes['grant_vacuum'].getPosition() == pytest.approx(
+        rack_pose.position, abs=1e-9,
+    )
+
+
+def test_grab_and_handoff_remove_only_the_selected_screw():
+    node = make_node(11)
+    magnetic = node.state.tools['magnetic_tool']['pose']
+    tcp = tcp_goal(magnetic, node.geometry.tool_tcp_to_nodes['magnetic_tool'])
+    set_tcp(node, tcp)
+    assert node._apply('attach_tool', {'object': 'magnetic_tool'}, 'a')[0]
+
+    screw = node.state.screws['screw_1']['pose']
+    screw_node = node.nodes['grant_screw_01']
+    tcp = tcp_goal(screw, node.geometry.screw_tcp_to_node)
+    set_tcp(node, tcp)
+    assert node._apply('grab_screw', {'object': 'screw_1'}, 'g')[0]
+    node.follow()
+    assert node.nodes['grant_screw_01'].getPosition() == pytest.approx(
+        screw.position, abs=1e-9,
+    )
+
+    hole = world_target(node.spec, node.geometry, 'hole:hole_1')
+    tcp = tcp_goal(hole, node.geometry.screw_tcp_to_node)
+    set_tcp(node, tcp)
+    node.follow()
+    target = transform_json(hole)
+    accepted, error = node._apply(
+        'handoff_screw',
+        {'object': 'screw_1', 'target': target, 'target_name': 'hole_1'},
+        'h',
+    )
+    assert accepted, error
+    assert screw_node.removed is True
+    assert node.nodes['grant_screw_02'].removed is False
+    assert node.state.holes['hole_1']['state'] == 'empty'
+
+
+def test_follow_refuses_when_tcp_is_unknown():
+    node = make_node(1)
+    node.patron_node = None
+    node.attached = ('tool', 'vacuum')
+    node.follow()
+    assert node.nodes['grant_vacuum'].getPosition() == pytest.approx(
+        node.state.tools['vacuum']['pose'].position, abs=1e-9,
+    )

src/iiwa_experement/test/test_launch.py

@@ -1,12 +1,25 @@
-"""Launch validation and action wiring."""
+"""Launch validation and action wiring for view/execute modes."""
 import importlib.util
 from pathlib import Path
 
 import pytest
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+    RegisterEventHandler,
+)
 from launch_ros.actions import Node
 
-from iiwa_experement.runtime_config import validate_experiment_id
+from iiwa_experement.runtime_config import (
+    DEFAULT_START_DELAY_SEC,
+    DEFAULT_TRACE_DIR,
+    validate_experiment_id,
+    validate_mode,
+    validate_start_delay,
+    validate_trace_dir,
+)
 
 
 PACKAGE = Path(__file__).resolve().parents[1]
@@ -32,36 +45,100 @@ def test_invalid_experiment_ids_are_rejected(raw):
         validate_experiment_id(raw)
 
 
-def test_runtime_actions_include_bringup_and_one_scene_spawner():
+@pytest.mark.parametrize('raw, expected', [('view', 'view'), ('execute', 'execute'), (' VIEW ', 'view')])
+def test_valid_modes(raw, expected):
+    assert validate_mode(raw) == expected
+
+
+@pytest.mark.parametrize('raw', ['', 'run', 'VIEW '[:-1] + 'x', '1'])
+def test_invalid_modes_are_rejected(raw):
+    with pytest.raises(ValueError):
+        validate_mode(raw)
+
+
+@pytest.mark.parametrize('raw, expected', [('0', 0.0), ('3', 3.0), ('2.5', 2.5)])
+def test_valid_start_delays(raw, expected):
+    assert validate_start_delay(raw) == expected
+
+
+@pytest.mark.parametrize('raw', ['', '-1', 'abc', 'nan', 'inf'])
+def test_invalid_start_delays_are_rejected(raw):
+    with pytest.raises(ValueError):
+        validate_start_delay(raw)
+
+
+def test_trace_dir_must_not_be_empty():
+    assert validate_trace_dir('~/.ros/run') == '~/.ros/run'
+    with pytest.raises(ValueError):
+        validate_trace_dir('  ')
+
+
+def test_view_starts_bringup_spawner_and_read_only_supervisor():
     module = load_launch_module()
     actions = module.runtime_actions(
         experiment_id=19,
         scene_config='/tmp/custom scenes.yaml',
         setting_path='/tmp/run/setting.yaml',
         rviz='false',
+        mode='view',
     )
     includes = [a for a in actions if isinstance(a, IncludeLaunchDescription)]
     nodes = [a for a in actions if isinstance(a, Node)]
+    processes = [a for a in actions if type(a) is ExecuteProcess]
     assert len(includes) == 1
     assert len(nodes) == 1
-    assert 'iiwa_bringup' in repr(includes[0].launch_description_source.location)
+    assert len(processes) == 1
     arguments = dict(includes[0].launch_arguments)
     assert arguments['simulate'] == 'true'
     assert arguments['rviz'] == 'false'
     assert arguments['setting'] == '/tmp/run/setting.yaml'
-    node = nodes[0]
+    spawner = nodes[0]
-    assert node.node_package == 'iiwa_experement'
+    assert spawner.node_package == 'iiwa_experement'
-    assert node.node_executable == 'scene_spawner'
+    assert spawner.node_executable == 'scene_spawner'
-    assert vars(node)['_Node__node_name'] == 'scene_spawner'
     assert module.spawner_parameters(19, '/tmp/custom scenes.yaml') == {
         'experiment_id': 19,
         'scene_config': '/tmp/custom scenes.yaml',
     }
+    parameters = module.supervisor_parameters(
+        19, '/tmp/custom scenes.yaml', '/tmp/geometry.yaml', True,
+    )
+    assert parameters['experiment_id'] == 19
+    assert parameters['read_only'] is True
+    # A view run has no motion client or trajectory targets.
+    assert not any(
+        isinstance(action, Node) and action.node_executable == 'reference_runner'
+        for action in actions
+    )
+
+
+def test_execute_adds_the_reference_runner_after_the_supervisor():
+    module = load_launch_module()
+    actions = module.runtime_actions(
+        experiment_id=1,
+        scene_config='/tmp/scenes.yaml',
+        setting_path='/tmp/setting.yaml',
+        mode='execute',
+        start_delay_sec=4.5,
+    )
+    runner = [
+        action for action in actions
+        if isinstance(action, Node) and action.node_executable == 'reference_runner'
+    ]
+    assert len(runner) == 1
+    payload = module.runner_parameters(
+        1, '/tmp/scenes.yaml', '/tmp/geometry.yaml', 4.5, DEFAULT_TRACE_DIR,
+    )
+    assert payload['experiment_id'] == 1
+    assert payload['start_delay_sec'] == 4.5
+    assert payload['trace_dir'] == DEFAULT_TRACE_DIR
+    parameters = module.supervisor_parameters(
+        1, '/tmp/scenes.yaml', '/tmp/geometry.yaml', False,
+    )
+    assert parameters['read_only'] is False
+    assert any(type(action) is ExecuteProcess for action in actions)
 
 
 def test_runtime_actions_can_register_cleanup(tmp_path):
-    from launch.actions import RegisterEventHandler
-
     module = load_launch_module()
     actions = module.runtime_actions(
         experiment_id=1,
@@ -69,7 +146,6 @@ def test_runtime_actions_can_register_cleanup(tmp_path):
         setting_path='/tmp/setting.yaml',
         cleanup_dir=str(tmp_path / 'run'),
     )
-    assert len(actions) == 3
     assert isinstance(actions[-1], RegisterEventHandler)
     target = tmp_path / 'run'
     target.mkdir()
@@ -86,10 +162,17 @@ def test_launch_description_declares_all_arguments_and_validates_in_opaque_funct
         for entity in description.entities
         if isinstance(entity, DeclareLaunchArgument)
     }
-    assert set(arguments) == {'experiment_id', 'rviz', 'scene_config', 'base_setting'}
+    assert set(arguments) == {
+        'experiment_id', 'mode', 'start_delay_sec', 'trace_dir',
+        'rviz', 'scene_config', 'base_setting',
+    }
     assert [substitution.text for substitution in arguments['experiment_id']] == ['']
-    assert [substitution.text for substitution in arguments['rviz']] == ['false']
+    assert [substitution.text for substitution in arguments['mode']] == ['view']
-    assert 'scenes.yaml' in repr(arguments['scene_config'])
+    assert [substitution.text for substitution in arguments['start_delay_sec']] == [
-    assert 'setting.yaml' in repr(arguments['base_setting'])
+        f'{DEFAULT_START_DELAY_SEC:g}'
+    ]
+    assert [substitution.text for substitution in arguments['trace_dir']] == [
+        DEFAULT_TRACE_DIR
+    ]
     opaque = [e for e in description.entities if isinstance(e, OpaqueFunction)]
     assert len(opaque) == 1

src/iiwa_experement/test/test_object_state.py

@@ -0,0 +1,207 @@
+"""Object ownership rejects unsafe transitions and never mutates on failure."""
+from pathlib import Path
+
+import pytest
+
+from iiwa_experement.geometry import load_geometry, tcp_goal, world_target
+from iiwa_experement.object_state import CommandError, ObjectState
+from iiwa_experement.scenario import load_scenario
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+SCENES = PACKAGE / 'config' / 'scenes.yaml'
+GEOMETRY = PACKAGE / 'config' / 'geometry.yaml'
+
+
+def make_state(experiment_id, *, read_only=False):
+    return ObjectState(
+        load_scenario(experiment_id, SCENES),
+        load_geometry(GEOMETRY),
+        read_only=read_only,
+    )
+
+
+def attach_tool(state, name):
+    tool_pose = state.tools[name]['pose']
+    tcp = tcp_goal(tool_pose, state.geometry.tool_tcp_to_nodes[name])
+    state.attach_tool(name, tcp, tool_pose, request_id=f'attach-{name}')
+    return tcp
+
+
+def grab_cap(state):
+    cap_pose = state.cap_pose
+    tcp = tcp_goal(cap_pose, state.geometry.cap_tcp_to_node)
+    state.grab_cap(tcp, cap_pose, request_id='grab-cap')
+    return tcp
+
+
+def place_cap(state):
+    target = world_target(state.spec, state.geometry, 'cap_seat')
+    tcp = tcp_goal(target, state.geometry.cap_tcp_to_node)
+    object_pose = tcp.compose_relaxed(state.geometry.cap_tcp_to_node)
+    state.place_cap(target, tcp, object_pose, request_id='place-cap')
+
+
+def grab_screw(state, screw_id):
+    screw_pose = state.screws[screw_id]['pose']
+    tcp = tcp_goal(screw_pose, state.geometry.screw_tcp_to_node)
+    state.grab_screw(screw_id, tcp, screw_pose, request_id=f'grab-{screw_id}')
+    return tcp
+
+
+def handoff(state, screw_id, hole_id):
+    target = world_target(state.spec, state.geometry, f'hole:{hole_id}')
+    tcp = tcp_goal(target, state.geometry.screw_tcp_to_node)
+    object_pose = tcp.compose_relaxed(state.geometry.screw_tcp_to_node)
+    state.handoff_screw(
+        screw_id, hole_id, target, tcp, object_pose,
+        request_id=f'handoff-{screw_id}-{hole_id}',
+    )
+
+
+def test_initial_state_for_all_cap_modes():
+    free = make_state(1)
+    assert free.mounted_tool is None
+    assert free.cap_state == 'free'
+    assert free.screws == {}
+    installed = make_state(11)
+    assert installed.cap_state == 'installed'
+    assert len(installed.screws) == 10
+    absent = make_state(6)
+    assert absent.cap_state == 'absent'
+    assert absent.cap_pose is None
+    assert absent.screws == {}
+
+
+def test_attach_and_dock_tool_changes_only_that_tool():
+    state = make_state(1)
+    attach_tool(state, 'vacuum')
+    assert state.mounted_tool == 'vacuum'
+    assert state.tools['vacuum']['state'] == 'mounted'
+    assert state.version == 1
+    rack_pose = state.tools['vacuum']['pose']
+    attached_pose = rack_pose
+    tcp = tcp_goal(rack_pose, state.geometry.tool_tcp_to_nodes['vacuum'])
+    state.dock_tool('vacuum', tcp, rack_pose, attached_pose)
+    assert state.mounted_tool is None
+    assert state.tools['vacuum']['state'] == 'docked'
+    assert state.version == 2
+
+
+def test_tool_requires_contact_and_socket_state():
+    state = make_state(1)
+    tool_pose = state.tools['vacuum']['pose']
+    good = tcp_goal(tool_pose, state.geometry.tool_tcp_to_nodes['vacuum'])
+    far = good.compose_relaxed(
+        type(good).from_xyz_rpy(
+            [0.0, 0.0, 0.05], [0.0, 0.0, 0.0],
+            parent=good.parent, child=good.child,
+        )
+    )
+    with pytest.raises(CommandError, match='tcp is'):
+        state.attach_tool('vacuum', far, tool_pose)
+    assert state.version == 0
+    assert state.mounted_tool is None
+
+
+def test_second_tool_is_rejected_without_state_change():
+    state = make_state(16)
+    attach_tool(state, 'vacuum')
+    version = state.version
+    pose = state.tools['magnetic_tool']['pose']
+    tcp = tcp_goal(pose, state.geometry.tool_tcp_to_nodes['magnetic_tool'])
+    with pytest.raises(CommandError, match='another tool'):
+        state.attach_tool('magnetic_tool', tcp, pose)
+    assert state.version == version
+    assert state.mounted_tool == 'vacuum'
+
+
+def test_cap_needs_the_vacuum_tool_and_a_free_hand():
+    state = make_state(1)
+    with pytest.raises(CommandError, match='requires'):
+        grab_cap(state)
+    assert state.version == 0
+    attach_tool(state, 'vacuum')
+    grab_cap(state)
+    assert state.cap_state == 'held'
+    assert state.held_object == 'cap'
+    with pytest.raises(CommandError, match='forbidden while'):
+        state.grab_cap(state.cap_pose, state.cap_pose)
+    assert state.version == 2
+
+
+def test_place_cap_seats_only_with_the_hatch_held():
+    state = make_state(1)
+    attach_tool(state, 'vacuum')
+    grab_cap(state)
+    place_cap(state)
+    assert state.cap_state == 'installed'
+    assert state.held_object is None
+    target = world_target(state.spec, state.geometry, 'cap_seat')
+    assert state.cap_pose.position == pytest.approx(target.position, abs=1e-12)
+    with pytest.raises(CommandError, match='requires the hatch'):
+        place_cap(state)
+
+
+def test_screws_require_the_magnetic_tool_and_are_consumed_on_handoff():
+    state = make_state(11)
+    with pytest.raises(CommandError, match='requires'):
+        grab_screw(state, 'screw_1')
+    attach_tool(state, 'magnetic_tool')
+    grab_screw(state, 'screw_1')
+    assert state.screws['screw_1']['state'] == 'held'
+    handoff(state, 'screw_1', 'hole_1')
+    assert state.screws['screw_1']['state'] == 'delivered'
+    assert state.screws['screw_1']['hole'] == 'hole_1'
+    assert state.holes['hole_1']['state'] == 'empty'
+    assert state.holes['hole_1']['screw'] == 'screw_1'
+    assert len(state.delivered_pairs()) == 1
+    assert sum(
+        1 for entry in state.screws.values() if entry['state'] == 'tray'
+    ) == 9
+
+
+def test_repeated_handoff_and_unknown_hole_are_rejected():
+    state = make_state(11)
+    attach_tool(state, 'magnetic_tool')
+    grab_screw(state, 'screw_1')
+    handoff(state, 'screw_1', 'hole_1')
+    version = state.version
+    with pytest.raises(CommandError, match='not held'):
+        handoff(state, 'screw_1', 'hole_1')
+    assert state.version == version
+    grab_screw(state, 'screw_2')
+    target = world_target(state.spec, state.geometry, 'hole:hole_2')
+    tcp = tcp_goal(target, state.geometry.screw_tcp_to_node)
+    object_pose = tcp.compose_relaxed(state.geometry.screw_tcp_to_node)
+    with pytest.raises(CommandError, match='unknown hole'):
+        state.handoff_screw(
+            'screw_2', 'hole_9', target, tcp, object_pose,
+        )
+
+
+def test_a_screw_cannot_be_grabbed_twice():
+    state = make_state(11)
+    attach_tool(state, 'magnetic_tool')
+    grab_screw(state, 'screw_1')
+    with pytest.raises(CommandError, match='forbidden while'):
+        grab_screw(state, 'screw_2')
+
+
+def test_read_only_view_rejects_mutation_but_allows_inspection():
+    state = make_state(1, read_only=True)
+    before = state.snapshot()
+    with pytest.raises(CommandError, match='read-only'):
+        attach_tool(state, 'vacuum')
+    after = state.snapshot()
+    assert before['version'] == after['version'] == 0
+    assert after['last_error']
+
+
+def test_rejected_command_preserves_version_and_snapshot():
+    state = make_state(1)
+    attach_tool(state, 'vacuum')
+    snapshot = state.snapshot()
+    with pytest.raises(CommandError):
+        state.attach_tool('vacuum', state.cap_pose, state.cap_pose)
+    assert state.snapshot()['version'] == snapshot['version']

src/iiwa_experement/test/test_runtime_config.py

@@ -8,9 +8,42 @@ from ament_index_python.packages import get_package_share_directory
 from iiwa_experement.runtime_config import (
     LOCAL_PROTOS,
     prepare_runtime_files,
+    validate_mode,
+    validate_start_delay,
+    validate_trace_dir,
 )
 
 
+@pytest.mark.parametrize('raw, expected', [('view', 'view'), (' Execute ', 'execute')])
+def test_mode_validation(raw, expected):
+    assert validate_mode(raw) == expected
+
+
+@pytest.mark.parametrize('raw', ['', 'motion', 'execute!'])
+def test_mode_validation_rejects_unknown_modes(raw):
+    with pytest.raises(ValueError):
+        validate_mode(raw)
+
+
+@pytest.mark.parametrize('raw, expected', [('0', 0.0), (' 3.0 ', 3.0)])
+def test_start_delay_validation(raw, expected):
+    assert validate_start_delay(raw) == expected
+
+
+@pytest.mark.parametrize('raw', ['', '-0.1', 'no', 'nan', 'inf'])
+def test_start_delay_validation_rejects_bad_values(raw):
+    with pytest.raises(ValueError):
+        validate_start_delay(raw)
+
+
+def test_trace_dir_validation():
+    assert validate_trace_dir('~/.ros/iiwa_experement/runs') == (
+        '~/.ros/iiwa_experement/runs'
+    )
+    with pytest.raises(ValueError):
+        validate_trace_dir('')
+
+
 PACKAGE = Path(__file__).resolve().parents[1]
 CONFIG_SHARE = Path(get_package_share_directory('iiwa_config'))
 DESCRIPTION_SHARE = Path(get_package_share_directory('iiwa_description'))

src/iiwa_experement/test/test_webots_nodes.py

@@ -88,6 +88,16 @@ def test_scenarios_one_six_eleven_sixteen_nineteen_are_complete():
         assert [NAME_RE.search(node).group(1) for node in nodes(experiment_id)] == names
 
 
+def test_every_managed_node_has_a_stable_unique_def():
+    for experiment_id in range(1, 21):
+        node_list = nodes(experiment_id)
+        defs = [re.search(r'^DEF (\w+) Solid \{', node).group(1) for node in node_list]
+        assert len(defs) == len(set(defs))
+        assert len(defs) == len(node_list)
+        for node, def_name in zip(node_list, defs):
+            assert re.search(rf'name "{def_name}"', node)
+
+
 def test_node_names_are_unique_and_never_duplicate_patron():
     for experiment_id in range(1, 21):
         node_list = nodes(experiment_id)

src/iiwa_experement/test/test_world.py

@@ -1,4 +1,4 @@
-"""The world template contains only the shared table, fences and lighting."""
+"""The world template contains the shared table and one attachment supervisor."""
 import re
 from pathlib import Path
 
@@ -10,16 +10,18 @@ WORLD = PACKAGE / 'worlds' / 'grant' / 'iiwa-grant.wbt'
 def test_world_matches_base_layout():
     text = WORLD.read_text(encoding='utf-8')
     assert '#VRML_SIM R2025a utf8' in text
-    assert 'basicTimeStep 32' in text
+    # Webots defaults to 32 ms when the field is omitted; both forms keep the
+    # deterministic simulation step.
+    assert 'basicTimeStep 32' in text or 'basicTimeStep' not in text
     assert re.search(r'CollaborativeRobotTable \{\s+translation 0 0 -0\.007', text)
     assert text.count('WorkspaceLimiter {') == 2
     assert 'WorkspaceLimiter_back' in text
     assert 'WorkspaceLimiter_right' in text
-    assert 'coulombFriction [ 0.6 ]' in text
+    assert 'coulombFriction' in text and '0.6' in text
     assert 'Object.proto' not in text
 
 
-def test_world_has_no_preplaced_experiment_objects_or_robot():
+def test_world_has_only_the_attachment_supervisor_robot():
     text = WORLD.read_text(encoding='utf-8')
     for asset in (
         'stapel.obj',
@@ -35,18 +37,22 @@ def test_world_has_no_preplaced_experiment_objects_or_robot():
     ):
         assert asset not in text
     assert 'Kuka' not in text
-    assert 'Robot {' not in text
     assert 'GrantToolRack' not in text
+    robots = re.findall(r'Robot \{', text)
+    assert len(robots) == 1
+    assert 'name "GrantAttachmentSupervisor"' in text
+    assert 'controller "<extern>"' in text
+    assert 'supervisor TRUE' in text
+    assert 'name "Ros2Supervisor"' not in text
 
 
-def test_world_only_references_two_local_protos():
+def test_world_only_references_the_two_local_protos():
     text = WORLD.read_text(encoding='utf-8')
     externprotos = re.findall(r'EXTERNPROTO "([^"]+)"', text)
     local = [url for url in externprotos if not url.startswith(('http://', 'https://'))]
-    assert local == [
+    assert len(local) == 2
-        '../../../iiwa_description/resource/protos/CollaborativeRobotTable.proto',
+    assert local[0].endswith('CollaborativeRobotTable.proto')
-        '../../../iiwa_description/resource/protos/WorkspaceLimiter.proto',
+    assert local[1].endswith('WorkspaceLimiter.proto')
-    ]
     for url in externprotos:
         if url.startswith(('http://', 'https://')):
             assert '/webots/R2025a/' in url

src/iiwa_experement/worlds/grant/iiwa-grant.wbt

@@ -1,27 +1,20 @@
 #VRML_SIM R2025a utf8
 
-# Universal grant experiment world. The robot, cameras and experiment objects
-# are added at run time: iiwa_bringup spawns the iiwa 7 and
-# iiwa_experement/scene_spawner adds the deterministic scenario. The two local
-# EXTERNPROTO paths are valid from this source tree; runtime_config.py rewrites
-# them to absolute paths from the installed iiwa_description share before
-# Webots opens the temporary copy of this world.
-
 EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackground.proto"
 EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
 EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/floors/protos/RectangleArena.proto"
-EXTERNPROTO "../../../iiwa_description/resource/protos/CollaborativeRobotTable.proto"
+EXTERNPROTO "/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/protos/CollaborativeRobotTable.proto"
-EXTERNPROTO "../../../iiwa_description/resource/protos/WorkspaceLimiter.proto"
+EXTERNPROTO "/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/protos/WorkspaceLimiter.proto"
-
 
 WorldInfo {
   title "KUKA iiwa grant experiments"
-  basicTimeStep 32
   optimalThreadCount 4
   contactProperties [
     ContactProperties {
       material1 "TableMaterial"
-      coulombFriction [ 0.6 ]
+      coulombFriction [
+        0.6
+      ]
       bounce 0
       bounceVelocity 0
       softERP 1
@@ -35,17 +28,14 @@ WorldInfo {
       maxContactJoints 100
     }
     ContactProperties {
-      material1 "default"
-      material2 "default"
-      maxContactJoints 20
       bounce 0
-      softCFM 0.001
+      maxContactJoints 20
     }
   ]
 }
 Viewpoint {
-  orientation -0.268113062897852 0.00505693741372527 0.963374181140187 3.1052558422585
+  orientation -0.16538833828406266 0.05829171450270743 0.9845043288782285 2.4736073370602876
-  position 2.6 -0.1 2.4
+  position 4.897723425157077 -3.966075191122516 3.20774809692606
   followType "None"
   ambientOcclusionRadius 3
 }
@@ -86,3 +76,11 @@ DirectionalLight {
   direction -0.1 0.1 0
   intensity 2
 }
+
+# External controller from iiwa_experement/grant_supervisor.py.  It is the
+# only process that changes object ownership and follows the actual tcp.
+Robot {
+  name "GrantAttachmentSupervisor"
+  controller "<extern>"
+  supervisor TRUE
+}