feat(experement): implement reference operations and stable scene sync

src/iiwa_experement/test/test_grant_supervisor.py

@@ -78,7 +78,8 @@ def make_node(experiment_id=1):
     node.spec = spec
     node.geometry = geometry
     node.state = ObjectState(spec, geometry)
-    node.attached = None
+    node.tool_held = None
+    node.payload = None
     node.rack_poses = {
         name: entry['pose'] for name, entry in node.state.tools.items()
     }
@@ -144,7 +145,7 @@ def test_attach_requires_tcp_at_the_socket_and_rejects_far_pose():
     accepted, error = node._apply('attach_tool', {'object': 'vacuum'}, 'r1')
     assert accepted is True
     assert error is None
-    assert node.attached == ('tool', 'vacuum')
+    assert node.tool_held == 'vacuum'
 
     other = make_node(1)
     set_tcp(other, good.compose_relaxed(
@@ -155,7 +156,7 @@ def test_attach_requires_tcp_at_the_socket_and_rejects_far_pose():
     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.tool_held is None
     assert other.state.version == 0
 
 
@@ -170,7 +171,7 @@ def test_follow_moves_the_attached_tool_and_dock_returns_it_to_the_rack():
     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.tool_held is None
     assert node.state.mounted_tool is None
     assert node.nodes['grant_vacuum'].getPosition() == pytest.approx(
         rack_pose.position, abs=1e-9,
@@ -213,8 +214,29 @@ def test_grab_and_handoff_remove_only_the_selected_screw():
 def test_follow_refuses_when_tcp_is_unknown():
     node = make_node(1)
     node.patron_node = None
-    node.attached = ('tool', 'vacuum')
+    node.tool_held = 'vacuum'
     node.follow()
     assert node.nodes['grant_vacuum'].getPosition() == pytest.approx(
         node.state.tools['vacuum']['pose'].position, abs=1e-9,
     )
+
+
+def test_mounted_tool_and_payload_follow_together():
+    node = make_node(1)
+    tool_pose = node.state.tools['vacuum']['pose']
+    tcp = tcp_goal(tool_pose, node.geometry.tool_tcp_to_nodes['vacuum'])
+    set_tcp(node, tcp)
+    assert node._apply('attach_tool', {'object': 'vacuum'}, 't')[0]
+    cap_pose = node.state.cap_pose
+    cap_tcp = tcp_goal(cap_pose, node.geometry.cap_tcp_to_node)
+    set_tcp(node, cap_tcp)
+    assert node._apply('grab_cap', {'object': 'cap'}, 'c')[0]
+    node.follow()
+    expected_tool = cap_tcp.compose_relaxed(node.geometry.tool_tcp_to_nodes['vacuum'])
+    expected_cap = cap_tcp.compose_relaxed(node.geometry.cap_tcp_to_node)
+    assert node.nodes['grant_vacuum'].getPosition() == pytest.approx(
+        expected_tool.position, abs=1e-9,
+    )
+    assert node.nodes['grant_cap'].getPosition() == pytest.approx(
+        expected_cap.position, abs=1e-9,
+    )

src/iiwa_experement/test/test_operations.py

@@ -0,0 +1,219 @@
+"""Operation sequences are visible, ordered and fail closed."""
+from pathlib import Path
+
+import pytest
+
+from iiwa_experement.geometry import load_geometry, world_target
+from iiwa_experement.motion import MotionResult
+from iiwa_experement.operations import OperationError, ReferenceOperations
+from iiwa_experement.scenario import load_scenario
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+SCENES = PACKAGE / 'config' / 'scenes.yaml'
+GEOMETRY = PACKAGE / 'config' / 'geometry.yaml'
+
+
+class FakeSupervisor:
+    def __init__(self, spec, geometry):
+        self.spec = spec
+        self.geometry = geometry
+        self.commands = []
+        self.latest = {
+            'cap': {'state': spec.cap_state},
+            'screws': {},
+        }
+        self.fail_on = None
+
+    def object_pose(self, name):
+        mapping = {
+            'grant_cap': 'cap',
+            **{f'grant_{tool}': f'tool:{tool}' for tool in self.spec.tool_names},
+            **{
+                f'grant_screw_{index:02d}': f'screw:screw_{index}'
+                for index in range(1, 11)
+            },
+        }
+        target = mapping[name]
+        return world_target(self.spec, self.geometry, target)
+
+    def command(self, op, object_id='', **kwargs):
+        if self.fail_on == op:
+            raise OperationError(f'{op} rejected')
+        self.commands.append((op, object_id, kwargs))
+        return self.latest
+
+
+class FakeBridge:
+    def __init__(self):
+        self.attached = []
+        self.detached = []
+        self.removed = []
+
+    def attach(self, spec):
+        self.attached.append(spec.object_id)
+
+    def detach(self, object_id, pose):
+        self.detached.append((object_id, pose))
+
+    def remove(self, object_id):
+        self.removed.append(object_id)
+
+    def add_world(self, object_id, box, pose):
+        self.added_world = getattr(self, 'added_world', [])
+        self.added_world.append((object_id, pose))
+
+    def expected_sets(self):
+        return set(), set()
+
+    def assert_synchronized(self, *args, **kwargs):
+        return None
+
+
+class FakeMotion:
+    def __init__(self):
+        self.moves = []
+        self.named = []
+        self.fail_labels = set()
+
+    def move_pose(self, goal, planner='ptp', speed=0.2, label=''):
+        self.moves.append((label, planner, speed, goal))
+        if label in self.fail_labels:
+            return MotionResult(False, 'injected failure', label)
+        return MotionResult(True, 'ok', label, True, goal, goal)
+
+    def move_named(self, name, speed=0.2):
+        self.named.append((name, speed))
+        return MotionResult(True, 'ok', f'move_named({name})')
+
+
+def make_operations(experiment_id):
+    spec = load_scenario(experiment_id, SCENES)
+    geometry = load_geometry(GEOMETRY)
+    supervisor = FakeSupervisor(spec, geometry)
+    bridge = FakeBridge()
+    motion = FakeMotion()
+    operations = ReferenceOperations(
+        node=None,
+        spec=spec,
+        geometry=geometry,
+        supervisor=supervisor,
+        bridge=bridge,
+        motion=motion,
+    )
+    return operations, supervisor, bridge, motion, geometry
+
+
+def labels(motion):
+    return [label for label, _, _, _ in motion.moves]
+
+
+def test_take_tool_follows_the_visible_axial_route():
+    operations, supervisor, bridge, motion, geometry = make_operations(1)
+    tip = world_target(operations.spec, geometry, 'tool:vacuum')
+    expected = tip.compose_relaxed(geometry.tool_tcp_to_nodes['vacuum'].inverse())
+    result = operations.take_tool('vacuum')
+    assert result.success
+    assert labels(motion) == [
+        'take_tool(vacuum):transfer',
+        'take_tool(vacuum):approach',
+        'take_tool(vacuum):contact',
+        'take_tool(vacuum):retreat',
+        'take_tool(vacuum):clear',
+    ]
+    assert supervisor.commands[0][:2] == ('attach_tool', 'vacuum')
+    assert bridge.attached == []
+    contact_goal = motion.moves[2][3]
+    assert contact_goal.xyz == pytest.approx(expected.xyz, abs=1e-9)
+    assert motion.moves[0][1] == 'ompl'
+    assert motion.moves[1][1] == 'ompl'
+    assert motion.moves[2][1] == 'lin'
+
+
+def test_dock_tool_returns_the_tool_to_its_socket():
+    operations, supervisor, bridge, motion, geometry = make_operations(1)
+    result = operations.dock_tool('vacuum')
+    assert result.success
+    assert supervisor.commands[0][:2] == ('dock_tool', 'vacuum')
+    assert bridge.added_world[0][0] == 'grant_vacuum'
+    assert labels(motion)[-1] == 'dock_tool(vacuum):clear'
+
+
+def test_cap_pick_and_place_use_the_hatch_node_transform():
+    operations, supervisor, bridge, motion, geometry = make_operations(1)
+    assert operations.pick_cap().success
+    assert bridge.attached == []
+    assert supervisor.commands[0][:2] == ('grab_cap', 'cap')
+    assert operations.place_cap().success
+    assert bridge.added_world[0][0] == 'grant_cap'
+    assert supervisor.commands[1][0] == 'place_cap'
+
+
+def test_single_sealant_side_lowers_and_lifts():
+    operations, supervisor, bridge, motion, geometry = make_operations(6)
+    assert operations.dispense('side_1').success
+    assert labels(motion) == [
+        'dispense(side_1):transfer',
+        'dispense(side_1):approach',
+        'dispense(side_1):contact',
+        'dispense(side_1):path',
+        'dispense(side_1):retreat',
+        'dispense(side_1):clear',
+    ]
+    assert operations.completed_sealant == {'side_1'}
+    assert operations.inspect_sealant()['ok'] is True
+
+
+def test_contour_does_not_lift_between_adjacent_sides():
+    operations, supervisor, bridge, motion, geometry = make_operations(8)
+    assert operations.dispense_contour(
+        ['side_1', 'side_2', 'side_3', 'side_4']
+    ).success
+    names = labels(motion)
+    assert names[:3] == [
+        'dispense_contour:start:transfer',
+        'dispense_contour:start:approach',
+        'dispense_contour:start:contact',
+    ]
+    assert names[3:7] == [
+        'dispense_contour:side_1',
+        'dispense_contour:side_2',
+        'dispense_contour:side_3',
+        'dispense_contour:side_4',
+    ]
+    assert 'dispense_contour:side_1' in names
+    assert names[-2:] == [
+        'dispense_contour:retreat', 'dispense_contour:clear',
+    ]
+    assert operations.completed_sealant == {
+        'side_1', 'side_2', 'side_3', 'side_4',
+    }
+
+
+def test_screw_pick_and_handoff_are_one_motion_and_one_removal():
+    operations, supervisor, bridge, motion, geometry = make_operations(11)
+    assert operations.pick_screw('screw_1').success
+    assert bridge.attached == []
+    assert supervisor.commands[0][:2] == ('grab_screw', 'screw_1')
+    assert operations.handoff_screw('screw_1', 'hole_1').success
+    assert supervisor.commands[1][0] == 'handoff_screw'
+    assert supervisor.commands[1][2]['target_name'] == 'hole_1'
+    assert bridge.removed == ['grant_screw_01', 'grant_screw_01']
+    assert bridge.attached == []
+
+
+def test_failed_motion_stops_the_operation_and_emits_failure():
+    operations, supervisor, bridge, motion, geometry = make_operations(1)
+    motion.fail_labels.add('take_tool(vacuum):contact')
+    with pytest.raises(OperationError):
+        operations.take_tool('vacuum')
+    assert operations.events[-1]['phase'] == 'failed'
+    assert bridge.attached == []
+
+
+def test_failed_supervisor_command_is_not_ignored():
+    operations, supervisor, bridge, motion, geometry = make_operations(1)
+    supervisor.fail_on = 'attach_tool'
+    with pytest.raises(OperationError):
+        operations.take_tool('vacuum')
+    assert bridge.attached == []

src/iiwa_experement/test/test_planning_scene.py

@@ -10,6 +10,8 @@ from iiwa_experement.planning_scene import (
     PlanningSceneBridge,
     SceneObject,
     attached_cap,
+    build_collision_matrix,
+    load_srdf_disabled_pairs,
     attached_screw,
     attached_tool,
     build_world_objects,
@@ -40,6 +42,11 @@ class FakeBridge(PlanningSceneBridge):
             obj.object_id: obj for obj in build_world_objects(scenario, geometry)
         }
         self._attached = {}
+        self._disabled_pairs = []
+        self._acm_names = []
+        self._acm_values = []
+        self._latest_joint_state = None
+        self.node = None
         self.last_scene = PlanningScene()
         self.published = []
 
@@ -53,13 +60,14 @@ class FakeBridge(PlanningSceneBridge):
 def test_initial_scene_contains_every_experiment_object():
     objects = build_world_objects(spec(11), load_geometry(GEOMETRY))
     ids = {obj.object_id for obj in objects}
-    assert {'grant_table', 'grant_fixture', 'grant_body', 'grant_rack',
+    assert {'grant_table', 'grant_fixture', 'grant_fixture_rail_a',
+            'grant_fixture_rail_b', 'grant_body', 'grant_rack',
             'grant_tray', 'grant_cap', 'grant_magnetic_tool'} <= ids
     assert {f'grant_screw_{index:02d}' for index in range(1, 11)} <= ids
     assert 'grant_vacuum' not in ids
-    # table, fixture, body, rack, tray base, four tray walls, ten screws,
-    # installed hatch and one magnetic tool.
-    assert len(ids) == 4 + 1 + 4 + 10 + 1 + 1
+    # table, fixture base, two fixture rails, body, rack, tray base, four
+    # tray walls, ten screws, installed hatch and one magnetic tool.
+    assert len(ids) == 23
 
 
 def test_experiment_nineteen_moves_tray_and_screws():
@@ -99,11 +107,11 @@ def test_attach_removes_the_world_object_and_adds_one_attachment():
     assert tool.object_id not in bridge._world_objects
     assert bridge._attached[tool.object_id] is tool
     scene = bridge.published[-1]
-    assert scene.is_diff is True
-    assert [obj.id for obj in scene.world.collision_objects] == [tool.object_id]
-    assert scene.world.collision_objects[0].operation == CollisionObject.REMOVE
+    assert scene.is_diff is False
+    assert tool.object_id not in [obj.id for obj in scene.world.collision_objects]
     assert len(scene.robot_state.attached_collision_objects) == 1
-    assert scene.robot_state.attached_collision_objects[0].link_name == 'tcp'
+    assert scene.robot_state.attached_collision_objects[0].object.id == tool.object_id
+    assert scene.robot_state.attached_collision_objects[0].link_name == 'patron'
     assert 'tcp' in scene.robot_state.attached_collision_objects[0].touch_links
 
 
@@ -117,10 +125,8 @@ def test_detach_reverses_the_attachment():
     assert tool.object_id in bridge._world_objects
     assert tool.object_id not in bridge._attached
     scene = bridge.published[-1]
-    assert scene.robot_state.attached_collision_objects[0].object.operation == (
-        CollisionObject.REMOVE
-    )
-    assert scene.world.collision_objects[0].operation == CollisionObject.ADD
+    assert not scene.robot_state.attached_collision_objects
+    assert tool.object_id in [obj.id for obj in scene.world.collision_objects]
 
 
 def test_handoff_removes_only_the_delivered_screw():
@@ -138,9 +144,9 @@ def test_cap_and_screw_attachment_geometry_uses_tcp_offsets():
     geometry = load_geometry(GEOMETRY)
     cap = attached_cap(geometry)
     screw = attached_screw('screw_3', geometry)
-    assert cap.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.141835))
+    assert cap.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.141835 + 0.0552))
     assert cap.box.size == pytest.approx((0.1384, 0.0984, 0.00367))
-    assert screw.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.1445))
+    assert screw.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.1445 + 0.0552))
 
 
 def test_contact_permission_changes_exactly_one_pair():
@@ -153,6 +159,24 @@ def test_contact_permission_changes_exactly_one_pair():
     assert matrix[0][1] is False
 
 
+def test_srdf_collision_matrix_allows_only_disabled_pairs():
+    root = Path(__file__).resolve().parents[2]
+    srdf = root / 'iiwa_config' / 'config' / 'moveit' / 'iiwa7.srdf'
+    pairs = load_srdf_disabled_pairs(srdf)
+    assert ('link1', 'link2') in pairs
+    names, matrix = build_collision_matrix(
+        ['grant_body', 'grant_tool'], disabled_pairs=pairs,
+    )
+    assert 'base_link' in names and 'grant_body' in names
+    def allowed(first, second):
+        return matrix[names.index(first)][names.index(second)]
+    assert allowed('link1', 'link2') is True
+    assert allowed('base_link', 'link1') is True
+    assert allowed('link1', 'grant_body') is False
+    assert allowed('grant_body', 'grant_tool') is False
+    assert allowed('grant_body', 'grant_body') is True
+
+
 def test_scene_ids_reports_world_and_attached_sets():
     scene = PlanningScene()
     scene.world.collision_objects.append(CollisionObject(id='world_a'))

src/iiwa_experement/test/test_webots_nodes.py

@@ -232,13 +232,11 @@ def test_loose_cap_touches_the_table_and_installed_cap_touches_the_body():
             assert cap_z + cap_min == pytest.approx(body_z + body_max, abs=1e-9)
 
 
-def test_free_cap_has_physics_and_installed_cap_is_locked():
-    free_cap = by_name(nodes(1))['grant_cap']
-    assert 'physics Physics {' in free_cap
-    assert 'locked TRUE' not in free_cap
-    installed_cap = by_name(nodes(11))['grant_cap']
-    assert 'locked TRUE' in installed_cap
-    assert 'physics Physics {' not in installed_cap
+def test_all_caps_are_locked_and_supervisor_controlled():
+    for experiment_id in (1, 11):
+        cap = by_name(nodes(experiment_id))['grant_cap']
+        assert 'locked TRUE' in cap
+        assert 'physics Physics {' not in cap
 
 
 def test_ten_screws_are_distinct_and_inside_the_tray():