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

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]
-    assert node.node_package == 'iiwa_experement'
-    assert node.node_executable == 'scene_spawner'
-    assert vars(node)['_Node__node_name'] == 'scene_spawner'
+    spawner = nodes[0]
+    assert spawner.node_package == 'iiwa_experement'
+    assert spawner.node_executable == '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 'scenes.yaml' in repr(arguments['scene_config'])
-    assert 'setting.yaml' in repr(arguments['base_setting'])
+    assert [substitution.text for substitution in arguments['mode']] == ['view']
+    assert [substitution.text for substitution in arguments['start_delay_sec']] == [
+        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 == [
-        '../../../iiwa_description/resource/protos/CollaborativeRobotTable.proto',
-        '../../../iiwa_description/resource/protos/WorkspaceLimiter.proto',
-    ]
+    assert len(local) == 2
+    assert local[0].endswith('CollaborativeRobotTable.proto')
+    assert local[1].endswith('WorkspaceLimiter.proto')
     for url in externprotos:
         if url.startswith(('http://', 'https://')):
             assert '/webots/R2025a/' in url