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

src/iiwa_experement/config/geometry.yaml

@@ -128,18 +128,28 @@ scene_collisions:
     type: box
     size: [1.28, 1.08, 0.045]
     offset_z: 0.7665
-  fixture:
+  # The fixture is a frame, not a solid block: a low base plate and two
+  # side rails.  A full box would cover the hatch seat and make placement
+  # impossible.
+  fixture_base:
     type: box
-    size: [0.350, 0.220, 0.041]
+    size: [0.350, 0.220, 0.010]
-    offset_z: 0.0105
+    offset_z: -0.005
+  fixture_rail:
+    type: box
+    size: [0.014, 0.156, 0.016]
+    offset_z: 0.008
+    offset_x: 0.104
   body:
     type: box
     size: [0.300, 0.170, 0.00915]
     offset_z: 0.000695
   rack:
+    # The magazine is represented by its base plate only.  Keep it just
+    # below the tool sockets so the vertical tool bodies do not overlap it.
     type: box
     size: [0.420, 0.140, 0.010]
-    offset_z: 0.005
+    offset_z: -0.015
   tray_base:
     type: box
     size: [0.120, 0.070, 0.0024]

src/iiwa_experement/config/moveit_cpp.yaml

@@ -0,0 +1,61 @@
+# Package-local MoveItCpp settings for the reference executor.
+#
+# The stock iiwa_config copy publishes the MoveItPy maintained scene on
+# /monitored_planning_scene (the same topic move_group publishes) but listens
+# to /planning_scene.  The executor publishes its diffs to move_group, so the
+# MoveItPy monitor must instead listen to move_group's maintained scene.
+#
+# publish_planning_scene_topic is the topic this monitor subscribes to;
+# monitored_planning_scene_topic is where it republishes its own scene.
+planning_scene_monitor_options:
+  name: "planning_scene_monitor"
+  robot_description: "robot_description"
+  joint_state_topic: "/joint_states"
+  attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
+  publish_planning_scene_topic: "/monitored_planning_scene"
+  monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
+  wait_for_initial_state_timeout: 10.0
+
+planning_pipelines:
+  pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp"]
+
+plan_request_params:
+  planning_attempts: 1
+  planning_pipeline: ompl
+  max_velocity_scaling_factor: 0.5
+  max_acceleration_scaling_factor: 1.0
+
+ompl_rrtc:
+  plan_request_params:
+    planning_attempts: 1
+    planning_pipeline: ompl
+    planner_id: "RRTConnectkConfigDefault"
+    max_velocity_scaling_factor: 1.0
+    max_acceleration_scaling_factor: 1.0
+    planning_time: 1.0
+
+pilz_ptp:
+  plan_request_params:
+    planning_attempts: 1
+    planning_pipeline: pilz_industrial_motion_planner
+    planner_id: "PTP"
+    max_velocity_scaling_factor: 1.0
+    max_acceleration_scaling_factor: 1.0
+    planning_time: 0.8
+
+pilz_lin:
+  plan_request_params:
+    planning_attempts: 1
+    planning_pipeline: pilz_industrial_motion_planner
+    planner_id: "LIN"
+    max_velocity_scaling_factor: 1.0
+    max_acceleration_scaling_factor: 1.0
+    planning_time: 0.8
+
+chomp_planner:
+  plan_request_params:
+    planning_attempts: 1
+    planning_pipeline: chomp
+    max_velocity_scaling_factor: 1.0
+    max_acceleration_scaling_factor: 1.0
+    planning_time: 1.5

src/iiwa_experement/iiwa_experement/grant_supervisor.py

@@ -110,7 +110,8 @@ class GrantSupervisorNode(Node):
         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.tool_held: str | None = None
+        self.payload: 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()
@@ -174,6 +175,15 @@ class GrantSupervisorNode(Node):
                 field.setSFBool(True)
         except Exception:
             pass
+        # All managed objects are teleported by the supervisor.  Leaving a
+        # physics node attached lets Webots integrate huge velocities while
+        # the pose is overwritten every step.
+        try:
+            physics = node.getField('physics')
+            if physics is not None:
+                physics.remove()
+        except Exception:
+            pass
 
     def _pose_of(self, node) -> Transform | None:
         if node is None:
@@ -223,24 +233,25 @@ class GrantSupervisorNode(Node):
             names.append('grant_cap')
         return names
 
-    def _node_for_attachment(self) -> Any | None:
+    def _tool_node(self):
-        if self.attached is None:
+        if self.tool_held is None:
             return None
-        kind, name = self.attached
+        return self.nodes.get(f'grant_{self.tool_held}')
-        if kind == 'tool':
+
-            return self.nodes.get(f'grant_{name}')
+    def _payload_node(self):
+        if self.payload is None:
+            return None
+        kind, name = self.payload
         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:
+    def _payload_relative(self) -> Transform | None:
-        if self.attached is None:
+        if self.payload is None:
             return None
-        kind, name = self.attached
+        kind, _ = self.payload
-        if kind == 'tool':
-            return self.geometry.tool_tcp_to_nodes[name]
         if kind == 'cap':
             return self.geometry.cap_tcp_to_node
         if kind == 'screw':
@@ -248,19 +259,24 @@ class GrantSupervisorNode(Node):
         return None
 
     def follow(self) -> None:
-        if self.attached is None:
-            return
         tcp = self.tcp_pose()
-        node = self._node_for_attachment()
+        if tcp is None:
-        relative = self._relative_transform()
-        if tcp is None or node is None or relative is None:
             return
+        if self.tool_held is not None:
+            node = self._tool_node()
+            relative = self.geometry.tool_tcp_to_nodes[self.tool_held]
+            if node is not None:
                 pose = tcp.compose_relaxed(relative)
                 self._set_pose(node, pose)
-        kind, name = self.attached
+                self.state.tools[self.tool_held]['pose'] = pose
-        if kind == 'tool' and name in self.state.tools:
+        if self.payload is not None:
-            self.state.tools[name]['pose'] = pose
+            node = self._payload_node()
-        elif kind == 'cap':
+            relative = self._payload_relative()
+            if node is not None and relative is not None:
+                pose = tcp.compose_relaxed(relative)
+                self._set_pose(node, pose)
+                kind, name = self.payload
+                if kind == 'cap':
                     self.state.cap_pose = pose
                 elif kind == 'screw' and name in self.state.screws:
                     self.state.screws[name]['pose'] = pose
@@ -309,6 +325,10 @@ class GrantSupervisorNode(Node):
     def _apply_impl(self, operation: str, command: dict[str, Any], request_id: str):
         if operation == 'inspect':
             return True, None
+        # A command can arrive before the per-step follow update.  Apply the
+        # current tcp to the held object first, otherwise the attachment
+        # appears one control step behind (about 11 mm at transfer speed).
+        self.follow()
         tcp = self.tcp_pose()
         if tcp is None:
             raise CommandError('actual tcp pose is unavailable')
@@ -319,7 +339,7 @@ class GrantSupervisorNode(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)
+            self.tool_held = kind
             return True, None
         if operation == 'dock_tool':
             node = self.nodes.get(f'grant_{kind}')
@@ -330,7 +350,7 @@ class GrantSupervisorNode(Node):
             self.state.dock_tool(
                 kind, tcp, rack, object_pose, request_id=request_id,
             )
-            self.attached = None
+            self.tool_held = None
             self._set_pose(node, rack)
             self.state.tools[kind]['pose'] = rack
             return True, None
@@ -340,7 +360,7 @@ class GrantSupervisorNode(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')
+            self.payload = ('cap', 'cap')
             return True, None
         if operation == 'place_cap':
             target = self._command_pose(command)
@@ -349,7 +369,7 @@ class GrantSupervisorNode(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.payload = None
             self._set_pose(node, target)
             self._freeze(node)
             self.state.cap_pose = target
@@ -360,7 +380,7 @@ class GrantSupervisorNode(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)
+            self.payload = ('screw', kind)
             return True, None
         if operation == 'handoff_screw':
             hole = str(command.get('target_name') or '')
@@ -372,7 +392,7 @@ class GrantSupervisorNode(Node):
             self.state.handoff_screw(
                 kind, hole, target, tcp, object_pose, request_id=request_id,
             )
-            self.attached = None
+            self.payload = None
             if node is not None:
                 try:
                     node.remove()
@@ -394,9 +414,10 @@ class GrantSupervisorNode(Node):
         payload['ready'] = self.patron_node is not None and all(
             name in self.nodes for name in self._required_defs()
         )
-        payload['attached'] = (
+        payload['attached'] = {
-            None if self.attached is None else list(self.attached)
+            'tool': self.tool_held,
-        )
+            'payload': None if self.payload is None else list(self.payload),
+        }
         tcp = self.tcp_pose()
         payload['tcp'] = transform_json(tcp)
         payload['last_command_result'] = self.last_command_result

src/iiwa_experement/iiwa_experement/motion.py

@@ -107,7 +107,10 @@ class MotionAdapter:
         position_tolerance: float | None = None,
         orientation_tolerance: float | None = None,
         command_timeout_sec: float = 90.0,
-        settle_time_sec: float = 0.5,
+        settle_time_sec: float = 1.0,
+        stability_timeout_sec: float = 5.0,
+        stability_epsilon: float = 0.001,
+        stability_angle: float = 0.01,
     ) -> None:
         self.node = node
         self.geometry = geometry
@@ -139,6 +142,9 @@ class MotionAdapter:
         )
         self.command_timeout_sec = float(command_timeout_sec)
         self.settle_time_sec = max(0.0, float(settle_time_sec))
+        self.stability_timeout_sec = max(0.0, float(stability_timeout_sec))
+        self.stability_epsilon = float(stability_epsilon)
+        self.stability_angle = float(stability_angle)
         self.stopped = False
         self.stop_reason: str | None = None
         self._last_joint_time = None
@@ -283,8 +289,7 @@ class MotionAdapter:
                 base_goal, started,
             )
 
-        self._settle()
+        actual = self._settle_and_measure()
-        actual = self.actual_tcp()
         if actual is None:
             return self._fail(label, 'actual tcp pose is unavailable after motion', base_goal, started)
         distance, angle = _pose_distance(actual, base_goal)
@@ -343,6 +348,37 @@ class MotionAdapter:
                 timeout_sec=min(0.05, max(0.0, end - time.monotonic())),
             )
 
+    def _settle_and_measure(self) -> Transform | None:
+        """Wait until the controller stops moving before verifying tcp.
+
+        The action result is delivered when the trajectory finishes, but the
+        joint trajectory controller can still be settling.  Treating the
+        first sample as final caused false out-of-tolerance stops.
+        """
+        self._settle()
+        import rclpy
+
+        deadline = time.monotonic() + self.stability_timeout_sec
+        last = self.actual_tcp()
+        stable_since = None
+        while time.monotonic() < deadline and self.node is not None:
+            rclpy.spin_once(self.node, timeout_sec=0.05)
+            current = self.actual_tcp()
+            if current is None:
+                last = None
+                stable_since = None
+                continue
+            if last is not None:
+                distance, angle = _pose_distance(current, last)
+                if distance <= self.stability_epsilon and angle <= self.stability_angle:
+                    stable_since = stable_since or time.monotonic()
+                    if time.monotonic() - stable_since >= 0.25:
+                        return current
+                else:
+                    stable_since = None
+            last = current
+        return last
+
     def _fail(
         self,
         label: str,

src/iiwa_experement/iiwa_experement/operations.py

@@ -0,0 +1,573 @@
+"""Reference operations: tools, hatch, sealant contour and screw handoff."""
+from __future__ import annotations
+
+import json
+import math
+import uuid
+from dataclasses import dataclass, field
+from typing import Any, Callable, Iterable
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+from webots_ros2_msgs.srv import SetString
+
+from .geometry import (
+    GeometrySpec,
+    Transform,
+    sealant_point,
+    tcp_goal,
+    world_target,
+)
+from .motion import MotionAdapter, MotionResult
+from .planning_scene import (
+    PlanningSceneBridge,
+    attached_cap,
+    attached_screw,
+    attached_tool,
+    screw_object_id,
+)
+from .scenario import ScenarioSpec
+
+
+COMMAND_SERVICE = '/iiwa_experement/object_command'
+STATE_TOPIC = '/iiwa_experement/object_state'
+
+
+class OperationError(RuntimeError):
+    """An operation could not be completed safely."""
+
+
+@dataclass(frozen=True)
+class OperationResult:
+    success: bool
+    operation: str
+    message: str
+    events: tuple[dict[str, Any], ...] = field(default_factory=tuple)
+
+
+def _translation(xyz) -> Transform:
+    return Transform.from_xyz_rpy(
+        [float(value) for value in xyz], [0.0, 0.0, 0.0],
+        parent='tcp', child='offset',
+    )
+
+
+class SupervisorClient:
+    """Synchronous facade over the Webots attachment supervisor."""
+
+    def __init__(self, node: Node, *, timeout_sec: float = 15.0) -> None:
+        self.node = node
+        self.timeout_sec = float(timeout_sec)
+        self.latest: dict[str, Any] | None = None
+        self._client = node.create_client(SetString, COMMAND_SERVICE)
+        self._subscription = node.create_subscription(
+            String, STATE_TOPIC, self._on_state, 10,
+        )
+
+    def _on_state(self, message: String) -> None:
+        try:
+            self.latest = json.loads(message.data)
+        except Exception:
+            pass
+
+    def _spin(self, seconds: float = 0.05) -> None:
+        import time
+
+        finish = time.monotonic() + seconds
+        while time.monotonic() < finish:
+            rclpy.spin_once(self.node, timeout_sec=0.02)
+
+    def wait_ready(self, timeout_sec: float = 60.0) -> dict[str, Any]:
+        import time
+
+        end = time.monotonic() + timeout_sec
+        while time.monotonic() < end:
+            rclpy.spin_once(self.node, timeout_sec=0.05)
+            if self.latest and self.latest.get('ready'):
+                return self.latest
+        raise OperationError('supervisor did not become ready')
+
+    def wait_version(self, minimum: int, timeout_sec: float | None = None) -> dict[str, Any]:
+        import time
+
+        end = time.monotonic() + (self.timeout_sec if timeout_sec is None else timeout_sec)
+        while time.monotonic() < end:
+            rclpy.spin_once(self.node, timeout_sec=0.05)
+            if self.latest and self.latest.get('version', -1) >= minimum:
+                return self.latest
+        raise OperationError(f'supervisor state version {minimum} was not published')
+
+    def command(
+        self,
+        op: str,
+        object_id: str = '',
+        *,
+        target: Transform | None = None,
+        target_name: str = '',
+        expected_state: dict[str, Any] | None = None,
+    ) -> dict[str, Any]:
+        if not self._client.wait_for_service(timeout_sec=5.0):
+            raise OperationError(f'{COMMAND_SERVICE} is unavailable')
+        request_id = uuid.uuid4().hex
+        payload: dict[str, Any] = {
+            'request_id': request_id,
+            'op': op,
+            'object': object_id,
+            'target_name': target_name,
+        }
+        if target is not None:
+            payload['target'] = _transform_payload(target)
+        if expected_state is not None:
+            payload['expected_state'] = expected_state
+        request = SetString.Request()
+        request.value = json.dumps(payload)
+        future = self._client.call_async(request)
+        self._spin(0.0)
+        import time
+
+        end = time.monotonic() + self.timeout_sec
+        while not future.done() and time.monotonic() < end:
+            rclpy.spin_once(self.node, timeout_sec=0.05)
+        if not future.done():
+            raise OperationError(f'supervisor command {op} timed out')
+        accepted = bool(future.result().success)
+        # The detailed result is published atomically after the transition.
+        state = self.latest
+        if state is None or state.get('last_command_result', {}).get('request_id') != request_id:
+            end = time.monotonic() + 2.0
+            while time.monotonic() < end:
+                rclpy.spin_once(self.node, timeout_sec=0.02)
+                state = self.latest
+                if state and state.get('last_command_result', {}).get('request_id') == request_id:
+                    break
+        result = (state or {}).get('last_command_result', {})
+        if not accepted or not result.get('accepted', False):
+            reason = result.get('error') or 'supervisor rejected the command'
+            raise OperationError(f'{op}({object_id or "-"}) rejected: {reason}')
+        return state or {}
+
+    @property
+    def objects(self) -> dict[str, Any]:
+        return dict((self.latest or {}).get('objects') or {})
+
+    def object_pose(self, name: str) -> Transform:
+        entry = self.objects.get(name)
+        if not isinstance(entry, dict):
+            raise OperationError(f'object {name!r} is not present in the supervisor state')
+        return _transform_from_payload(entry, child=name)
+
+    def logical_cap(self) -> dict[str, Any]:
+        return dict((self.latest or {}).get('cap') or {})
+
+    def logical_screws(self) -> dict[str, Any]:
+        return dict((self.latest or {}).get('screws') or {})
+
+
+def _transform_payload(transform: Transform) -> dict[str, Any]:
+    return {
+        'xyz': [float(value) for value in transform.xyz],
+        'rotation': [
+            [float(value) for value in row] for row in transform.rotation
+        ],
+    }
+
+
+def _transform_from_payload(data: dict[str, Any], child: str = 'object') -> Transform:
+    xyz = data.get('xyz')
+    rotation = data.get('rotation')
+    if not isinstance(xyz, (list, tuple)) or len(xyz) != 3:
+        raise OperationError('object pose has no valid xyz')
+    if not isinstance(rotation, (list, tuple)) or len(rotation) != 3:
+        raise OperationError('object pose has no valid rotation')
+    if any(not isinstance(row, (list, tuple)) or len(row) != 3 for row in rotation):
+        raise OperationError('object pose rotation must be a 3x3 matrix')
+    return Transform(
+        xyz=tuple(float(value) for value in xyz),
+        rotation=tuple(tuple(float(value) for value in row) for row in rotation),
+        parent='world',
+        child=child,
+    )
+
+
+class ReferenceOperations:
+    """Sequence of visible robot operations for one experiment."""
+
+    def __init__(
+        self,
+        node: Node,
+        spec: ScenarioSpec,
+        geometry: GeometrySpec,
+        supervisor: SupervisorClient,
+        bridge: PlanningSceneBridge,
+        motion: MotionAdapter,
+        *,
+        event_sink: Callable[[dict[str, Any]], None] | None = None,
+    ) -> None:
+        self.node = node
+        self.spec = spec
+        self.geometry = geometry
+        self.supervisor = supervisor
+        self.bridge = bridge
+        self.motion = motion
+        self._event_sink = event_sink
+        self.events: list[dict[str, Any]] = []
+        self.completed_sealant: set[str] = set()
+        self.last_sealant_point: tuple[str, float] | None = None
+
+    # ------------------------------------------------------------------ helpers
+    def _emit(self, operation: str, **payload) -> None:
+        event = {'operation': operation, **payload}
+        self.events.append(event)
+        if self._event_sink is not None:
+            self._event_sink(event)
+
+    def _require(self, condition: bool, message: str) -> None:
+        if not condition:
+            raise OperationError(message)
+
+    def _move(
+        self,
+        goal: Transform,
+        planner: str,
+        speed: float,
+        label: str,
+        *,
+        allow_fallback: bool = True,
+    ) -> MotionResult:
+        result = self.motion.move_pose(goal, planner=planner, speed=speed, label=label)
+        if result.success:
+            return result
+        planning_failed = (
+            'Планирование' in result.message
+            or 'planning' in result.message.lower()
+            or 'недостижима' in result.message
+        )
+        if allow_fallback and planner != 'ompl' and planning_failed:
+            # A short LIN segment can be rejected because the arm chose a
+            # different IK branch.  A collision-checked OMPL retry is safe;
+            # execution or pose-verification failures are never retried.
+            self.motion.reset_stop()
+            fallback = self.motion.move_pose(
+                goal, planner='ompl', speed=speed, label=f'{label}:ompl',
+            )
+            if fallback.success:
+                self._emit(
+                    'motion_fallback', label=label, original=planner,
+                    reason=result.message,
+                )
+                return fallback
+            result = fallback
+        raise OperationError(f'{label}: {result.message}')
+
+    def _above(self, goal: Transform, gap: float) -> Transform:
+        return goal.compose_relaxed(_translation([0.0, 0.0, -gap]))
+
+    def _contact(
+        self,
+        goal: Transform,
+        label: str,
+        *,
+        approach_gap: float | None = None,
+        transfer_gap: float | None = None,
+        remove_before_contact: str | None = None,
+        remove_phase: str = 'transfer',
+    ) -> None:
+        approach_gap = (
+            self.geometry.approach_gap if approach_gap is None else approach_gap
+        )
+        transfer_gap = (
+            self.geometry.transfer_gap if transfer_gap is None else transfer_gap
+        )
+        if remove_before_contact is not None and remove_phase == 'transfer':
+            # The object is the intended contact target.  Remove it from the
+            # world scene for the whole approach so the attached copy cannot
+            # duplicate it and the short axial moves are not rejected by a
+            # false contact with the target itself.
+            self.bridge.remove(remove_before_contact)
+            self._require_synchronized()
+        transfer = self._above(goal, transfer_gap)
+        approach = self._above(goal, approach_gap)
+        self._move(transfer, 'ompl', self.geometry.speeds['transfer'], f'{label}:transfer')
+        try:
+            self._move(
+                approach, 'ompl', self.geometry.speeds['approach'],
+                f'{label}:approach',
+            )
+        except OperationError as error:
+            message = str(error)
+            if 'Планирование' not in message and 'недостижима' not in message:
+                raise
+            # A pre-contact point can sit on a different IK branch.  The
+            # collision-checked direct path to the contact is safer than
+            # forcing an infeasible axial segment.
+            self._emit('approach_skipped', label=f'{label}:approach',
+                       reason=message)
+            self.motion.reset_stop()
+        if remove_before_contact is not None and remove_phase == 'contact':
+            self.bridge.remove(remove_before_contact)
+            self._require_synchronized()
+        self._move(goal, 'lin', self.geometry.speeds['contact'], f'{label}:contact')
+
+    def _retreat(self, goal: Transform, label: str) -> None:
+        approach = self._above(goal, self.geometry.approach_gap)
+        transfer = self._above(goal, self.geometry.transfer_gap)
+        self._move(approach, 'lin', self.geometry.speeds['approach'], f'{label}:retreat')
+        self._move(transfer, 'ompl', self.geometry.speeds['transfer'], f'{label}:clear')
+
+    def _require_synchronized(self) -> None:
+        try:
+            self.bridge.assert_synchronized(*self.bridge.expected_sets())
+        except Exception as error:
+            raise OperationError(f'planning scene is not synchronized: {error}') from error
+
+    def _expect_latest(self, timeout: float = 2.0) -> dict[str, Any]:
+        import time
+
+        end = time.monotonic() + timeout
+        while time.monotonic() < end:
+            rclpy.spin_once(self.node, timeout_sec=0.02)
+            if self.supervisor.latest is not None:
+                return self.supervisor.latest
+        raise OperationError('supervisor state is unavailable')
+
+    # ------------------------------------------------------------------ tools
+    def take_tool(self, tool_name: str) -> OperationResult:
+        self._emit('take_tool', object=tool_name, phase='start')
+        try:
+            self._require_synchronized()
+            tool_pose = self.supervisor.object_pose(f'grant_{tool_name}')
+            goal = tcp_goal(tool_pose, self.geometry.tool_tcp_to_nodes[tool_name])
+            self._contact(
+                goal, f'take_tool({tool_name})',
+                remove_before_contact=f'grant_{tool_name}',
+            )
+            self.supervisor.command('attach_tool', tool_name)
+            self._retreat(goal, f'take_tool({tool_name})')
+            self._emit('take_tool', object=tool_name, phase='done',
+                       tool=tool_name, tcp=goal.xyz)
+            return OperationResult(True, 'take_tool', f'{tool_name} mounted', tuple(self.events))
+        except Exception as error:
+            self._emit('take_tool', object=tool_name, phase='failed', error=str(error))
+            raise
+
+    def dock_tool(self, tool_name: str) -> OperationResult:
+        self._emit('dock_tool', object=tool_name, phase='start')
+        try:
+            self._require_synchronized()
+            rack_pose = world_target(self.spec, self.geometry, f'tool:{tool_name}')
+            goal = tcp_goal(rack_pose, self.geometry.tool_tcp_to_nodes[tool_name])
+            # Remove the held tool from MoveIt before entering the socket
+            # region.  The supervisor keeps following tcp, and the contact
+            # pose is verified by both the motion adapter and supervisor.
+            self.bridge.remove(f'grant_{tool_name}')
+            self._require_synchronized()
+            self._contact(goal, f'dock_tool({tool_name})')
+            self.supervisor.command('dock_tool', tool_name)
+            self.bridge.add_world(
+                f'grant_{tool_name}',
+                self.geometry.tool_collisions[tool_name],
+                rack_pose,
+            )
+            self._require_synchronized()
+            self._retreat(goal, f'dock_tool({tool_name})')
+            self._emit('dock_tool', object=tool_name, phase='done')
+            return OperationResult(True, 'dock_tool', f'{tool_name} docked', tuple(self.events))
+        except Exception as error:
+            self._emit('dock_tool', object=tool_name, phase='failed', error=str(error))
+            raise
+
+    # ------------------------------------------------------------------ hatch
+    def pick_cap(self) -> OperationResult:
+        self._emit('pick_cap', object='cap', phase='start')
+        try:
+            self._require_synchronized()
+            cap_pose = self.supervisor.object_pose('grant_cap')
+            goal = tcp_goal(cap_pose, self.geometry.cap_tcp_to_node)
+            self._contact(
+                goal, 'pick_cap', remove_before_contact='grant_cap',
+            )
+            self.supervisor.command('grab_cap', 'cap')
+            self._retreat(goal, 'pick_cap')
+            self._emit('pick_cap', object='cap', phase='done', tcp=goal.xyz)
+            return OperationResult(True, 'pick_cap', 'hatch grasped', tuple(self.events))
+        except Exception as error:
+            self._emit('pick_cap', object='cap', phase='failed', error=str(error))
+            raise
+
+    def place_cap(self, target_pose: Transform | None = None) -> OperationResult:
+        self._emit('place_cap', object='cap', phase='start')
+        try:
+            self._require_synchronized()
+            target = target_pose or world_target(self.spec, self.geometry, 'cap_seat')
+            goal = tcp_goal(target, self.geometry.cap_tcp_to_node)
+            self._contact(
+                goal, 'place_cap',
+                remove_before_contact='grant_cap',
+                remove_phase='contact',
+            )
+            self.supervisor.command('place_cap', 'cap', target=target)
+            self.bridge.add_world(
+                'grant_cap', self.geometry.cap_collision, target,
+            )
+            self._require_synchronized()
+            self._retreat(goal, 'place_cap')
+            self._emit('place_cap', object='cap', phase='done', target=target.xyz)
+            return OperationResult(True, 'place_cap', 'hatch placed', tuple(self.events))
+        except Exception as error:
+            self._emit('place_cap', object='cap', phase='failed', error=str(error))
+            raise
+
+    # ------------------------------------------------------------------ sealant
+    def dispense(
+        self,
+        segment_name: str,
+        start_fraction: float = 0.0,
+        end_fraction: float = 1.0,
+        *,
+        lift: bool = True,
+    ) -> OperationResult:
+        self._emit('dispense', object=segment_name, phase='start')
+        try:
+            self._require_synchronized()
+            start = sealant_point(
+                self.geometry, self.spec, segment_name, start_fraction,
+            )
+            end = sealant_point(
+                self.geometry, self.spec, segment_name, end_fraction,
+            )
+            tip = self.geometry.tool_work_tips['sealant_tool']
+            start_goal = tcp_goal(start, tip)
+            end_goal = tcp_goal(end, tip)
+            if start_fraction <= 0.0 and end_fraction >= 1.0:
+                self._contact(start_goal, f'dispense({segment_name})')
+            else:
+                self._contact(start_goal, f'dispense({segment_name}):resume')
+            self._move(
+                end_goal, 'lin', self.geometry.speeds['contour'],
+                f'dispense({segment_name}):path', allow_fallback=False,
+            )
+            self.last_sealant_point = (segment_name, end_fraction)
+            self.completed_sealant.add(segment_name)
+            if lift:
+                self._retreat(end_goal, f'dispense({segment_name})')
+            self._emit('dispense', object=segment_name, phase='done',
+                       start_fraction=start_fraction, end_fraction=end_fraction)
+            return OperationResult(True, 'dispense', f'{segment_name} completed', tuple(self.events))
+        except Exception as error:
+            self._emit('dispense', object=segment_name, phase='failed', error=str(error))
+            raise
+
+    def dispense_contour(self, segments: Iterable[str]) -> OperationResult:
+        """Traverse adjacent sides continuously without lifting the nozzle."""
+        segments = list(segments)
+        self._emit('dispense_contour', segments=list(segments), phase='start')
+        try:
+            self._require_synchronized()
+            tip = self.geometry.tool_work_tips['sealant_tool']
+            first = sealant_point(self.geometry, self.spec, segments[0], 0.0)
+            first_goal = tcp_goal(first, tip)
+            self._contact(first_goal, 'dispense_contour:start')
+            for index, segment in enumerate(segments):
+                end_fraction = 1.0 if index < len(segments) - 1 else 1.0
+                end = sealant_point(self.geometry, self.spec, segment, end_fraction)
+                end_goal = tcp_goal(end, tip)
+                self._move(
+                    end_goal, 'lin', self.geometry.speeds['contour'],
+                    f'dispense_contour:{segment}', allow_fallback=False,
+                )
+                self.completed_sealant.add(segment)
+                self.last_sealant_point = (segment, 1.0)
+                self._emit('dispense', object=segment, phase='done')
+            self._retreat(end_goal, 'dispense_contour')
+            self._emit('dispense_contour', segments=list(segments), phase='done')
+            return OperationResult(True, 'dispense_contour', 'contour completed', tuple(self.events))
+        except Exception as error:
+            self._emit('dispense_contour', phases='failed', error=str(error))
+            raise
+
+    # ------------------------------------------------------------------ screws
+    def pick_screw(self, screw_id: str) -> OperationResult:
+        def_name = screw_object_id(screw_id)
+        self._emit('pick_screw', object=screw_id, phase='start')
+        try:
+            self._require_synchronized()
+            screw_pose = self.supervisor.object_pose(def_name)
+            goal = tcp_goal(screw_pose, self.geometry.screw_tcp_to_node)
+            self._contact(
+                goal, f'pick_screw({screw_id})',
+                remove_before_contact=def_name,
+            )
+            self.supervisor.command('grab_screw', screw_id)
+            self._retreat(goal, f'pick_screw({screw_id})')
+            self._emit('pick_screw', object=screw_id, phase='done')
+            return OperationResult(True, 'pick_screw', f'{screw_id} grasped', tuple(self.events))
+        except Exception as error:
+            self._emit('pick_screw', object=screw_id, phase='failed', error=str(error))
+            raise
+
+    def handoff_screw(self, screw_id: str, hole_id: str) -> OperationResult:
+        def_name = screw_object_id(screw_id)
+        self._emit('handoff_screw', object=screw_id, target=hole_id, phase='start')
+        try:
+            self._require_synchronized()
+            hole = world_target(self.spec, self.geometry, f'hole:{hole_id}')
+            goal = tcp_goal(hole, self.geometry.screw_tcp_to_node)
+            self._contact(
+                goal, f'handoff_screw({screw_id},{hole_id})',
+                remove_before_contact=def_name,
+                remove_phase='contact',
+            )
+            self.supervisor.command(
+                'handoff_screw', screw_id, target=hole, target_name=hole_id,
+            )
+            self._require_synchronized()
+            # A Cartesian retreat from the hole can leave the wrist in a
+            # singular branch.  The caller moves to 'work' in joint space
+            # immediately afterwards; that is the verified clearance move.
+            self._emit('delivered', object=screw_id, target=hole_id,
+                       phase='done', hole=hole_id)
+            return OperationResult(
+                True, 'handoff_screw', f'{screw_id} delivered to {hole_id}',
+                tuple(self.events),
+            )
+        except Exception as error:
+            self._emit('handoff_screw', object=screw_id, target=hole_id,
+                       phase='failed', error=str(error))
+            raise
+
+    # ------------------------------------------------------------------ verifies
+    def inspect_cap(self, target_pose: Transform | None = None) -> dict[str, Any]:
+        self.supervisor.command('inspect')
+        target = target_pose or world_target(self.spec, self.geometry, 'cap_seat')
+        state = self._expect_latest()
+        cap = state.get('cap') or {}
+        if cap.get('state') != 'installed':
+            return {'ok': False, 'reason': f'hatch state is {cap.get("state")!r}'}
+        pose_entry = cap.get('pose')
+        if not pose_entry:
+            return {'ok': False, 'reason': 'hatch pose is unknown'}
+        pose = _transform_from_payload(pose_entry, child='cap')
+        distance = float(
+            (pose.position - target.position) @ (pose.position - target.position)
+        ) ** 0.5
+        ok = distance <= self.geometry.position_tolerance
+        return {
+            'ok': ok,
+            'distance': distance,
+            'target': target.xyz,
+            'actual': pose.xyz,
+        }
+
+    def inspect_sealant(self) -> dict[str, Any]:
+        expected = list(self.spec.sealant_sides)
+        missing = [side for side in expected if side not in self.completed_sealant]
+        return {
+            'ok': not missing,
+            'completed': sorted(self.completed_sealant),
+            'missing': missing,
+        }
+
+    def inspect_state(self) -> dict[str, Any]:
+        self.supervisor.command('inspect')
+        return self._expect_latest()

src/iiwa_experement/iiwa_experement/planning_scene.py

@@ -10,19 +10,24 @@ from __future__ import annotations
 import copy
 import math
 import time
+import xml.etree.ElementTree as ET
 from dataclasses import dataclass
+from pathlib import Path
 from typing import Iterable, Sequence
 
 import numpy as np
 import rclpy
+from ament_index_python.packages import get_package_share_directory
 from geometry_msgs.msg import Pose
 from moveit_msgs.msg import (
+    AllowedCollisionEntry,
     AttachedCollisionObject,
     CollisionObject,
     PlanningScene,
 )
 from moveit_msgs.srv import GetPlanningScene
 from rclpy.node import Node
+from sensor_msgs.msg import JointState
 from rclpy.parameter import Parameter
 from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
 from shape_msgs.msg import SolidPrimitive
@@ -35,7 +40,55 @@ SCENE_TOPIC = '/planning_scene'
 MONITORED_TOPIC = '/monitored_planning_scene'
 GET_SCENE_SERVICE = '/get_planning_scene'
 SCENE_FRAME = 'base_link'
-TOUCH_LINKS = ('tcp', 'patron', 'camera_holder')
+TOUCH_LINKS = (
+    'tcp', 'patron', 'camera_holder', 'camera_corner',
+    'camera_hand', 'camera_optical_frame',
+)
+ROBOT_LINKS = (
+    'base_link', 'link1', 'link2', 'link3', 'link4', 'link5', 'link6',
+    'link7', 'patron', 'camera_holder', 'camera_corner', 'camera_hand',
+    'camera_optical_frame', 'tcp',
+)
+DEFAULT_SRDF = 'iiwa_config/config/moveit/iiwa7.srdf'
+
+
+def load_srdf_disabled_pairs(path) -> list[tuple[str, str]]:
+    root = ET.parse(str(path)).getroot()
+    pairs = []
+    for element in root.findall('disable_collisions'):
+        first = element.get('link1')
+        second = element.get('link2')
+        if first and second:
+            pairs.append((first, second))
+    return pairs
+
+
+def build_collision_matrix(
+    object_ids: Iterable[str],
+    *,
+    disabled_pairs: Iterable[tuple[str, str]] = (),
+    extra_allowed: Iterable[tuple[str, str]] = (),
+) -> tuple[list[str], list[list[bool]]]:
+    """Build a complete ACM: only explicitly disabled pairs are allowed.
+
+    This mirrors the SRDF and keeps every other robot/object and object/object
+    pair checked.  It replaces an empty ACM that would otherwise make the
+    planning scene treat adjacent robot links as collisions.
+    """
+    names = list(ROBOT_LINKS)
+    for name in object_ids:
+        if name not in names:
+            names.append(name)
+    matrix = [[False] * len(names) for _ in names]
+    for index in range(len(names)):
+        matrix[index][index] = True
+    for first, second in list(disabled_pairs) + list(extra_allowed):
+        if first in names and second in names:
+            row = names.index(first)
+            column = names.index(second)
+            matrix[row][column] = True
+            matrix[column][row] = True
+    return names, matrix
 
 
 class SceneSynchronizationError(RuntimeError):
@@ -59,6 +112,7 @@ class AttachedSpec:
     box: Box
     relative_pose: Transform
     touch_links: tuple[str, ...]
+    frame_id: str | None = None
 
 
 def _box_at(pose: Transform, box: Box) -> Box:
@@ -90,7 +144,7 @@ def build_world_objects(spec: ScenarioSpec, geometry: GeometrySpec) -> list[Scen
         'grant_table', table, _rotated_pose(0.0, 0.0, table.offset[2], 0.0),
     ))
 
-    fixture = geometry.scene_collisions['fixture']
+    fixture = geometry.scene_collisions['fixture_base']
     objects.append(SceneObject(
         'grant_fixture',
         fixture,
@@ -101,6 +155,20 @@ def build_world_objects(spec: ScenarioSpec, geometry: GeometrySpec) -> list[Scen
             spec.fixture.yaw_deg,
         ),
     ))
+    rail = geometry.scene_collisions['fixture_rail']
+    sine = math.sin(math.radians(spec.fixture.yaw_deg))
+    cosine = math.cos(math.radians(spec.fixture.yaw_deg))
+    for sign, suffix in ((-1.0, 'a'), (1.0, 'b')):
+        objects.append(SceneObject(
+            f'grant_fixture_rail_{suffix}',
+            rail,
+            _rotated_pose(
+                spec.fixture.x + sign * rail.offset[0] * cosine,
+                spec.fixture.y + sign * rail.offset[0] * sine,
+                spec.fixture.z + rail.offset[2],
+                spec.fixture.yaw_deg,
+            ),
+        ))
 
     body = geometry.scene_collisions['body']
     objects.append(SceneObject(
@@ -211,7 +279,10 @@ def attached_tool(name: str, geometry: GeometrySpec) -> AttachedSpec:
             offset=collision.offset,
         ),
         relative_pose=Transform.from_xyz_rpy(
-            collision.offset, [0.0, 0.0, 0.0], parent='tcp', child=f'grant_{name}',
+            [0.0, 0.0, collision.offset[2] + 0.0552],
+            [0.0, 0.0, 0.0],
+            parent='patron',
+            child=f'grant_{name}',
         ),
         touch_links=TOUCH_LINKS,
     )
@@ -222,9 +293,9 @@ def attached_cap(geometry: GeometrySpec) -> AttachedSpec:
         object_id='grant_cap',
         box=geometry.cap_collision,
         relative_pose=Transform.from_xyz_rpy(
-            geometry.cap_collision.offset,
+            [0.0, 0.0, geometry.cap_collision.offset[2] + 0.0552],
             [0.0, 0.0, 0.0],
-            parent='tcp',
+            parent='patron',
             child='grant_cap',
         ),
         touch_links=TOUCH_LINKS,
@@ -237,9 +308,9 @@ def attached_screw(screw_id: str, geometry: GeometrySpec) -> AttachedSpec:
         object_id=object_id,
         box=geometry.screw_collision,
         relative_pose=Transform.from_xyz_rpy(
-            geometry.screw_collision.offset,
+            [0.0, 0.0, geometry.screw_collision.offset[2] + 0.0552],
             [0.0, 0.0, 0.0],
-            parent='tcp',
+            parent='patron',
             child=object_id,
         ),
         touch_links=TOUCH_LINKS,
@@ -274,25 +345,32 @@ def collision_object(
     message.header.frame_id = SCENE_FRAME
     message.id = object_id
     message.operation = operation
+    message.pose = _pose_message(pose)
     primitive = SolidPrimitive()
     primitive.type = SolidPrimitive.BOX
     primitive.dimensions = [float(value) for value in box.size]
     message.primitives.append(primitive)
-    message.primitive_poses.append(_pose_message(pose))
+    identity = Pose()
+    identity.orientation.w = 1.0
+    message.primitive_poses.append(identity)
     return message
 
 
 def attached_object_message(
     spec: AttachedSpec,
     *,
-    link_name: str = 'tcp',
+    link_name: str | None = None,
     operation: int = CollisionObject.ADD,
 ) -> AttachedCollisionObject:
     message = AttachedCollisionObject()
-    message.link_name = link_name
+    message.link_name = link_name or spec.relative_pose.parent
     message.object = collision_object(
         spec.object_id, spec.box, spec.relative_pose, operation=operation,
     )
+    # For attached objects the object pose is relative to the link.
+    message.object.header.frame_id = (
+        spec.frame_id if spec.frame_id is not None else message.link_name
+    )
     message.touch_links = list(spec.touch_links)
     return message
 
@@ -334,6 +412,7 @@ class PlanningSceneBridge:
         *,
         node: Node | None = None,
         use_sim_time: bool = True,
+        srdf_path: str | None = None,
     ) -> None:
         self._owns_node = node is None
         if node is None:
@@ -366,8 +445,29 @@ class PlanningSceneBridge:
         self._attached: dict[str, AttachedSpec] = {}
         self._seen_world: set[str] = set()
         self._seen_attached: set[str] = set()
+        self._latest_joint_state = None
+        self._last_full_scene: PlanningScene | None = None
+        self._acm_names: list[str] = []
+        self._acm_values: list[list[bool]] = []
+        self._disabled_pairs = []
+        srdf = srdf_path or str(
+            Path(get_package_share_directory('iiwa_config'))
+            / 'config' / 'moveit' / 'iiwa7.srdf'
+        )
+        try:
+            self._disabled_pairs = load_srdf_disabled_pairs(srdf)
+        except Exception:
+            self._disabled_pairs = []
+        self._joint_subscription = self.node.create_subscription(
+            JointState, '/joint_states', self._on_joint_state, 10,
+        )
+
+    def _on_joint_state(self, message: JointState) -> None:
+        self._latest_joint_state = message
 
     def _on_scene(self, message: PlanningScene) -> None:
+        if not message.is_diff:
+            self._last_full_scene = copy.deepcopy(message)
         self._seen_world.update(
             obj.id for obj in message.world.collision_objects
         )
@@ -411,6 +511,137 @@ class PlanningSceneBridge:
         self._publisher.publish(scene)
         self._spin()
 
+    def _fetch_acm(self) -> None:
+        if getattr(self, '_get_scene', None) is None or self.node is None:
+            return
+        if not self._get_scene.service_is_ready():
+            if not self._get_scene.wait_for_service(timeout_sec=1.0):
+                return
+        request = GetPlanningScene.Request()
+        request.components.components = 128
+        future = self._get_scene.call_async(request)
+        end = time.monotonic() + 3.0
+        while time.monotonic() < end and not future.done():
+            rclpy.spin_once(self.node, timeout_sec=0.05)
+        if future.done():
+            try:
+                matrix = future.result().scene.allowed_collision_matrix
+                self._acm_names = list(matrix.entry_names)
+                self._acm_values = [
+                    list(row.enabled) for row in matrix.entry_values
+                ]
+            except Exception:
+                pass
+
+    def _rebuild_acm(self) -> None:
+        extra = []
+        for spec in self._attached.values():
+            extra.extend((spec.object_id, link) for link in spec.touch_links)
+        self._acm_names, self._acm_values = build_collision_matrix(
+            list(self._world_objects) + list(self._attached),
+            disabled_pairs=self._disabled_pairs,
+            extra_allowed=extra,
+        )
+
+    def _full_scene(self) -> PlanningScene:
+        scene = PlanningScene()
+        scene.is_diff = False
+        # Never overwrite the planner's current robot state from a planning
+        # scene update: the monitor receives /joint_states directly and a
+        # stale embedded sample makes the next trajectory start from an old
+        # pose.  ``is_diff`` leaves the state untouched.
+        scene.robot_state.is_diff = True
+        for obj in self._world_objects.values():
+            scene.world.collision_objects.append(collision_object(
+                obj.object_id, obj.box, self._base_pose(obj.pose),
+            ))
+        for spec in self._attached.values():
+            scene.robot_state.attached_collision_objects.append(
+                attached_object_message(spec)
+            )
+        if self._acm_names:
+            scene.allowed_collision_matrix.entry_names = list(self._acm_names)
+            scene.allowed_collision_matrix.entry_values = [
+                AllowedCollisionEntry(
+                    enabled=[bool(value) for value in row],
+                )
+                for row in self._acm_values
+            ]
+        return scene
+
+    def _refresh_joint_state(self) -> None:
+        """Wait for the next /joint_states sample before a full scene update.
+
+        A full PlanningScene carries a robot state.  Using a sample captured
+        before a preceding motion would make the MoveItPy planner start from
+        an old pose on the next trajectory.
+        """
+        if self.node is None:
+            return
+        previous = None
+        if self._latest_joint_state is not None:
+            previous = (
+                self._latest_joint_state.header.stamp.sec,
+                self._latest_joint_state.header.stamp.nanosec,
+            )
+        end = time.monotonic() + 0.5
+        while time.monotonic() < end:
+            rclpy.spin_once(self.node, timeout_sec=0.02)
+            current = self._latest_joint_state
+            if current is None:
+                continue
+            stamp = (
+                current.header.stamp.sec,
+                current.header.stamp.nanosec,
+            )
+            if previous is None or stamp != previous:
+                return
+
+    def _relay_full_scene(self) -> None:
+        """Give MoveItPy the exact move_group scene, including its ACM.
+
+        MoveItPy's monitor always listens on ``/planning_scene`` even when its
+        configured topics differ.  Fetching the maintained scene from
+        move_group and republishing it once keeps both planners identical
+        without a feedback loop.
+        """
+        if getattr(self, '_get_scene', None) is None or self.node is None:
+            return
+        if not self._get_scene.service_is_ready():
+            if not self._get_scene.wait_for_service(timeout_sec=1.0):
+                return
+        expected_world = set(self._world_objects)
+        expected_attached = set(self._attached)
+        deadline = time.monotonic() + 3.0
+        while time.monotonic() < deadline:
+            request = GetPlanningScene.Request()
+            request.components.components = 2 | 4 | 8 | 16 | 128
+            future = self._get_scene.call_async(request)
+            end = time.monotonic() + 1.0
+            while time.monotonic() < end and not future.done():
+                rclpy.spin_once(self.node, timeout_sec=0.05)
+            if not future.done():
+                continue
+            try:
+                scene = future.result().scene
+            except Exception:
+                continue
+            world, attached = scene_ids(scene)
+            if expected_world <= world and expected_attached <= attached:
+                scene.is_diff = False
+                scene.robot_state.is_diff = True
+                self._publisher.publish(scene)
+                self._spin()
+                return
+        # Never relay a stale scene: that would wipe the objects from the
+        # MoveItPy monitor and make the next motion unverified.
+        return
+
+    def _publish_full(self) -> None:
+        self._refresh_joint_state()
+        self._publish(self._full_scene())
+        self._relay_full_scene()
+
     @staticmethod
     def _diff() -> PlanningScene:
         scene = PlanningScene()
@@ -440,33 +671,24 @@ class PlanningSceneBridge:
             obj.object_id: obj for obj in build_world_objects(spec, geometry)
         }
         self._attached.clear()
-        scene = self._diff()
+        extra = []
-        for obj in self._world_objects.values():
+        for spec in self._attached.values():
-            scene.world.collision_objects.append(collision_object(
+            extra.extend((spec.object_id, link) for link in spec.touch_links)
-                obj.object_id, obj.box, self._base_pose(obj.pose),
+        self._acm_names, self._acm_values = build_collision_matrix(
-            ))
+            list(self._world_objects) + list(self._attached),
-        self._publish(scene)
+            disabled_pairs=self._disabled_pairs,
+            extra_allowed=extra,
+        )
+        self._publish_full()
         self.assert_synchronized(
             self._world_objects, (), seen_world=set(self._world_objects),
         )
 
     def attach(self, spec: AttachedSpec) -> None:
-        if spec.object_id not in self._world_objects:
-            raise SceneSynchronizationError(
-                f'cannot attach {spec.object_id!r}: not a known world object'
-            )
-        scene = self._diff()
-        scene.world.collision_objects.append(collision_object(
-            spec.object_id, spec.box, Transform.from_xyz_rpy(
-                [0, 0, 0], [0, 0, 0], parent='world', child='removed',
-            ), operation=CollisionObject.REMOVE,
-        ))
-        scene.robot_state.attached_collision_objects.append(
-            attached_object_message(spec)
-        )
         self._world_objects.pop(spec.object_id, None)
         self._attached[spec.object_id] = spec
-        self._publish(scene)
+        self._rebuild_acm()
+        self._publish_full()
 
     def detach(self, object_id: str, world_pose: Transform) -> None:
         if object_id not in self._attached:
@@ -476,34 +698,20 @@ class PlanningSceneBridge:
         spec = self._attached.pop(object_id)
         object_spec = SceneObject(object_id, spec.box, world_pose)
         self._world_objects[object_id] = object_spec
-        scene = self._diff()
+        self._rebuild_acm()
-        scene.robot_state.attached_collision_objects.append(
+        self._publish_full()
-            attached_object_message(
-                spec, operation=CollisionObject.REMOVE,
-            )
-        )
-        scene.world.collision_objects.append(collision_object(
-            object_id, spec.box, self._base_pose(world_pose),
-        ))
-        self._publish(scene)
 
     def remove(self, object_id: str) -> None:
-        scene = self._diff()
+        self._attached.pop(object_id, None)
-        if object_id in self._attached:
+        self._world_objects.pop(object_id, None)
-            spec = self._attached.pop(object_id)
+        self._rebuild_acm()
-            scene.robot_state.attached_collision_objects.append(
+        self._publish_full()
-                attached_object_message(
+
-                    spec, operation=CollisionObject.REMOVE,
+    def add_world(self, object_id: str, box: Box, world_pose: Transform) -> None:
-                )
+        self._attached.pop(object_id, None)
-            )
+        self._world_objects[object_id] = SceneObject(object_id, box, world_pose)
-        if object_id in self._world_objects:
+        self._rebuild_acm()
-            self._world_objects.pop(object_id)
+        self._publish_full()
-            scene.world.collision_objects.append(collision_object(
-                object_id, Box(size=(0.001, 0.001, 0.001)), Transform.from_xyz_rpy(
-                    [0, 0, 0], [0, 0, 0], parent='world', child='removed',
-                ), operation=CollisionObject.REMOVE,
-            ))
-        self._publish(scene)
 
     def allow_contact(self, first: str, second: str, allowed: bool = True) -> None:
         """Change exactly one ACM pair; all other checks stay active."""
@@ -511,22 +719,21 @@ class PlanningSceneBridge:
             raise SceneSynchronizationError(
                 'cannot update contact permissions without a monitored scene'
             )
-        scene = self._diff()
+        if not self._acm_names:
-        names = list(self.last_scene.allowed_collision_matrix.entry_names)
+            raise SceneSynchronizationError(
-        values = [
+                'cannot update contact permissions without an ACM'
-            list(row) for row in self.last_scene.allowed_collision_matrix.entry_values
-        ]
-        names, values = update_contact_entry(
-            names, values, first, second, allowed,
             )
-        scene.allowed_collision_matrix.entry_names = names
+        self._acm_names, self._acm_values = update_contact_entry(
-        scene.allowed_collision_matrix.entry_values = values
+            self._acm_names, self._acm_values, first, second, allowed,
-        self._publish(scene)
+        )
+        self._publish_full()
 
     def expected_sets(self) -> tuple[set[str], set[str]]:
         return set(self._world_objects), set(self._attached)
 
     def _query_scene(self, timeout_sec: float) -> tuple[set[str], set[str]] | None:
+        if getattr(self, '_get_scene', None) is None or self.node is None:
+            return
         if not self._get_scene.service_is_ready():
             if not self._get_scene.wait_for_service(timeout_sec=1.0):
                 return None

src/iiwa_experement/iiwa_experement/runtime_config.py

@@ -117,6 +117,7 @@ def _materialize_setting(
     base_setting: Path,
     world_path: Path,
     output_dir: Path,
+    moveit_cpp_path: Path | None = None,
 ) -> Path:
     if not base_setting.is_file():
         raise FileNotFoundError(f'base settings file does not exist: {base_setting}')
@@ -134,6 +135,11 @@ def _materialize_setting(
     tool = _mapping(data.setdefault('tool', {}), 'tool')
     tool['active'] = 'patron'
 
+    if moveit_cpp_path is not None:
+        controller = _mapping(data.setdefault('controller', {}), 'controller')
+        moveit = _mapping(controller.setdefault('moveit', {}), 'controller.moveit')
+        moveit['moveit_cpp'] = str(Path(moveit_cpp_path).resolve())
+
     setting_path = output_dir / 'setting.yaml'
     setting_path.write_text(
         yaml.safe_dump(data, sort_keys=False, allow_unicode=True),
@@ -147,6 +153,7 @@ def prepare_runtime_files(
     world_template: Path | str,
     description_share: Path | str,
     output_dir: Path | str,
+    moveit_cpp_path: Path | str | None = None,
 ) -> tuple[Path, Path]:
     """Write an isolated world/settings pair and return ``(world, setting)``.
 
@@ -161,5 +168,10 @@ def prepare_runtime_files(
     output_dir = Path(output_dir)
     output_dir.mkdir(parents=True, exist_ok=True)
     world_path = _materialize_world(world_template, description_share, output_dir)
-    setting_path = _materialize_setting(base_setting, world_path, output_dir)
+    setting_path = _materialize_setting(
+        base_setting,
+        world_path,
+        output_dir,
+        None if moveit_cpp_path is None else Path(moveit_cpp_path),
+    )
     return world_path, setting_path

src/iiwa_experement/iiwa_experement/webots_nodes.py

@@ -309,15 +309,16 @@ def _cap_node(spec: ScenarioSpec, asset: Path) -> str:
   translation 0 0.004165 0
   children [ Box { size 0.1384 0.00367 0.0984 } ]
 }'''
-    installed = spec.cap_state == 'installed'
+    # All managed objects are positioned by the attachment supervisor.
+    # A free cap with physics can be launched by the supervisor's per-step
+    # teleport, so it is locked just like an installed cap.
     return _solid(
         name='grant_cap',
         translation=(pose.x, pose.y, pose.z),
         rotation=rotation_axis_angle(pose.yaw_deg),
         children=_mesh_shape(asset, '0.78 0.82 0.86'),
         bounding=bounding,
-        locked=installed,
+        locked=True,
-        physics_mass=None if installed else 0.1,
     )
 
 

src/iiwa_experement/launch/experiment.launch.py

@@ -226,6 +226,7 @@ def _setup(context, *args, **kwargs):
             world_template=world_template,
             description_share=description_share,
             output_dir=output_dir,
+            moveit_cpp_path=package_share / 'config' / 'moveit_cpp.yaml',
         )
     except Exception:
         shutil.rmtree(output_dir, ignore_errors=True)

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,
+    # table, fixture base, two fixture rails, body, rack, tray base, four
-    # installed hatch and one magnetic tool.
+    # tray walls, ten screws, installed hatch and one magnetic tool.
-    assert len(ids) == 4 + 1 + 4 + 10 + 1 + 1
+    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 scene.is_diff is False
-    assert [obj.id for obj in scene.world.collision_objects] == [tool.object_id]
+    assert tool.object_id not in [obj.id for obj in scene.world.collision_objects]
-    assert scene.world.collision_objects[0].operation == CollisionObject.REMOVE
     assert len(scene.robot_state.attached_collision_objects) == 1
-    assert scene.robot_state.attached_collision_objects[0].link_name == 'tcp'
+    assert 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 == (
+    assert not scene.robot_state.attached_collision_objects
-        CollisionObject.REMOVE
+    assert tool.object_id in [obj.id for obj in scene.world.collision_objects]
-    )
-    assert scene.world.collision_objects[0].operation == CollisionObject.ADD
 
 
 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():
+def test_all_caps_are_locked_and_supervisor_controlled():
-    free_cap = by_name(nodes(1))['grant_cap']
+    for experiment_id in (1, 11):
-    assert 'physics Physics {' in free_cap
+        cap = by_name(nodes(experiment_id))['grant_cap']
-    assert 'locked TRUE' not in free_cap
+        assert 'locked TRUE' in cap
-    installed_cap = by_name(nodes(11))['grant_cap']
+        assert 'physics Physics {' not in cap
-    assert 'locked TRUE' in installed_cap
-    assert 'physics Physics {' not in installed_cap
 
 
 def test_ten_screws_are_distinct_and_inside_the_tray():