feat(llm-plan-executor): execute Grant skills with verified phases

src/iiwa_llm_plan_executor/docs/interfaces.md

@@ -89,6 +89,26 @@ Important pre-movement rules:
   and no insertion/tightening occurs.
 * No task is inferred from `experiment_id`; only the initial scene is selected.
 
+## Skill phases
+
+A skill may contain the mandatory geometric phases of one action. `approach`
+moves above the target and saves that height; `lower` moves along the tool
+axis to contact; `grab` performs only the missing phases, confirms the grasp
+through the supervisor, then lifts back to the saved approach pose. Explicit
+`approach`/`lower`/`lift` nodes do not repeat motion already performed.
+
+`release` always lifts axially before any lateral motion or wrist rotation.
+`transfer` stops at the safe height over the destination. `release(cap,
+hatch_seat)` lowers, places, detaches and withdraws. `handoff_screw` lowers at
+the hole, hands the screw to the virtual operator and withdraws; it never
+inserts or tightens and the hole remains empty.
+
+`detect` first withdraws axially from a contact, moves to `work`, waits for a
+fresh top frame and reads the exact pose from the supervisor. When the object
+is configured for close inspection, a second fresh frame from the hand camera
+is required; RGB/D is used for liveness only.
+
+
 ## Constrained motion adapter
 
 `MotionPolicy` (`config/motion_limits.yaml`) checks every point before the

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skill_context.py

@@ -0,0 +1,107 @@
+"""Execution context shared by the skill registry and the graph engine."""
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from typing import Any, Callable
+
+from iiwa_experement.geometry import GeometrySpec, Transform
+from iiwa_experement.scenario import ScenarioSpec
+
+
+@dataclass
+class SkillContext:
+    spec: ScenarioSpec
+    geometry: GeometrySpec
+    scene_revision: int = 0
+    mounted_tool: str | None = None
+    held_object: str | None = None
+    ownership_unknown: bool = False
+    approach_pose: Transform | None = None
+    approach_signature: tuple[str, str] | None = None
+    contact_pose: Transform | None = None
+    phase: str = 'free'  # free | approach | contact | lifted
+    transfer_target: str | None = None
+    last_verified_pose: Transform | None = None
+    cap_relative: Transform | None = None
+    supervisor_state: dict[str, Any] = field(default_factory=dict)
+    accepted_revisions: set[int] = field(default_factory=set)
+    close_inspection_objects: frozenset[str] = frozenset()
+
+    # Injected sources, replaced in tests.
+    frame_provider: Callable[[str], float | None] | None = None
+    scene_clear_provider: Callable[[], bool] | None = None
+    notifier: Callable[[str, dict[str, Any]], bool] | None = None
+    scene_adapter: Any = None
+
+    # ------------------------------------------------------------------ phases
+    def mark_approach(self, pose: Transform, signature: tuple[str, str]) -> None:
+        self.approach_pose = pose
+        self.approach_signature = signature
+        self.contact_pose = None
+        self.phase = 'approach'
+
+    def mark_contact(self, pose: Transform) -> None:
+        self.contact_pose = pose
+        self.phase = 'contact'
+
+    def mark_lifted(self) -> None:
+        self.contact_pose = None
+        self.phase = 'lifted'
+
+    def reset_phase(self) -> None:
+        self.approach_pose = None
+        self.approach_signature = None
+        self.contact_pose = None
+        self.phase = 'free'
+        self.transfer_target = None
+
+    def approach_matches(self, signature: tuple[str, str]) -> bool:
+        return (
+            self.approach_pose is not None
+            and self.approach_signature == signature
+            and self.phase in ('approach', 'contact', 'lifted')
+        )
+
+    # ------------------------------------------------------------------ state
+    def update_from_state(self, state: dict[str, Any]) -> None:
+        if not isinstance(state, dict):
+            return
+        self.supervisor_state = state
+        mounted = state.get('mounted_tool')
+        held = state.get('held_object')
+        self.mounted_tool = mounted if isinstance(mounted, str) else None
+        self.held_object = held if isinstance(held, str) else None
+        version = state.get('version')
+        if isinstance(version, int):
+            self.accepted_revisions.add(self.scene_revision)
+        self.ownership_unknown = False
+
+    def capture_frame(self, name: str) -> float | None:
+        if self.frame_provider is None:
+            return None
+        try:
+            stamp = self.frame_provider(name)
+        except Exception:
+            return None
+        if stamp is None:
+            return None
+        try:
+            return float(stamp)
+        except (TypeError, ValueError):
+            return None
+
+    def scene_is_clear(self) -> bool:
+        if self.scene_clear_provider is None:
+            return True
+        try:
+            return bool(self.scene_clear_provider())
+        except Exception:
+            return False
+
+    def notify(self, reason: str, details: dict[str, Any]) -> bool:
+        if self.notifier is None:
+            return True
+        try:
+            return bool(self.notifier(reason, details))
+        except Exception:
+            return False

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py

@@ -0,0 +1,852 @@
+"""Skill registry: the only place where a graph node becomes robot motion."""
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from typing import Any
+
+import numpy as np
+
+from iiwa_experement.geometry import (
+    GeometrySpec,
+    Transform,
+    sealant_point,
+    tcp_goal,
+    world_target,
+)
+from iiwa_experement.scenario import ScenarioSpec
+from iiwa_scene_context.catalog import DEFAULT_CATALOG, SkillCatalog
+
+from .schema import PlanNode
+from .skill_context import SkillContext
+
+
+class SkillError(RuntimeError):
+    def __init__(self, event: str, message: str) -> None:
+        super().__init__(message)
+        self.event = event
+
+
+@dataclass(frozen=True)
+class SkillOutcome:
+    event: str
+    details: dict[str, Any] = field(default_factory=dict)
+
+    def as_dict(self) -> dict[str, Any]:
+        return {'event': self.event, 'details': dict(self.details)}
+
+
+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',
+    )
+
+
+def _above(goal: Transform, gap: float) -> Transform:
+    return goal.compose_relaxed(_translation([0.0, 0.0, -gap]))
+
+
+def _distance(first: Transform, second: Transform) -> float:
+    return float(np.linalg.norm(first.position - second.position))
+
+
+def _angle(first: Transform, second: Transform) -> float:
+    cosine = (np.trace(first.matrix.T @ second.matrix) - 1.0) / 2.0
+    return float(np.arccos(max(-1.0, min(1.0, cosine))))
+
+
+def _state_pose(entry: Any, child: str) -> Transform | None:
+    if not isinstance(entry, dict):
+        return None
+    xyz = entry.get('xyz')
+    rotation = entry.get('rotation')
+    if not isinstance(xyz, (list, tuple)) or len(xyz) != 3:
+        return None
+    if not isinstance(rotation, (list, tuple)) or len(rotation) != 3:
+        return None
+    try:
+        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,
+        )
+    except (TypeError, ValueError):
+        return None
+
+
+def object_definition_name(object_id: str) -> str | None:
+    if object_id == 'cap':
+        return 'grant_cap'
+    if object_id.startswith('screw_') and object_id.split('_')[-1].isdigit():
+        return f'grant_screw_{int(object_id.split("_")[-1]):02d}'
+    for name in ('vacuum', 'magnetic_tool', 'sealant_tool', 'torque_tool'):
+        if object_id == name:
+            return f'grant_{name}'
+    return None
+
+
+class SkillRegistry:
+    def __init__(
+        self,
+        motion: Any,
+        supervisor: Any,
+        *,
+        catalog: SkillCatalog = DEFAULT_CATALOG,
+        scene_adapter: Any = None,
+    ) -> None:
+        self.motion = motion
+        self.supervisor = supervisor
+        self.catalog = catalog
+        self.scene_adapter = scene_adapter
+        self._handlers = {
+            'detect': self._detect,
+            'approach': self._approach_skill,
+            'lower': self._lower_skill,
+            'grab': self._grab,
+            'pick': self._grab,
+            'lift': self._lift,
+            'transfer': self._transfer,
+            'move_to_storage': self._move_to_storage,
+            'align': self._align,
+            'release': self._release,
+            'retreat': self._retreat,
+            'capture': self._capture,
+            'verify': self._verify,
+            'inspect': self._verify,
+            'stop': self._stop,
+            'wait_scene_clear': self._wait_scene_clear,
+            'inspect_state': self._inspect_state,
+            'notify_operator': self._notify_operator,
+            'dispense': self._dispense,
+            'dock_tool': self._dock_tool,
+            'handoff_screw': self._handoff_screw,
+        }
+
+    # ------------------------------------------------------------------ dispatch
+    def execute(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        handler = self._handlers.get(node.skill)
+        if handler is None:
+            return SkillOutcome('failed', {'reason': f'unknown skill {node.skill!r}'})
+        try:
+            return handler(node, context)
+        except SkillError as error:
+            return SkillOutcome(error.event, {'reason': str(error)})
+        except Exception as error:  # never turn an unexpected exception into success
+            return SkillOutcome('failed', {'reason': str(error)})
+
+    # ------------------------------------------------------------------ motion
+    def _state(self) -> dict[str, Any]:
+        try:
+            state = self.supervisor.latest()
+        except Exception:
+            return {}
+        return state if isinstance(state, dict) else {}
+
+    def _move(
+        self,
+        context: SkillContext,
+        goal: Transform,
+        phase: str,
+    ) -> Any:
+        outcome = self.motion.plan_and_execute(goal, phase, context.held_object)
+        if not getattr(outcome, 'success', False):
+            code = getattr(outcome, 'code', 'failed')
+            event = 'collision' if code == 'collision' else 'failed'
+            raise SkillError(
+                event, getattr(outcome, 'message', 'motion failed'),
+            )
+        context.last_verified_pose = goal
+        return outcome
+
+    def _command(
+        self,
+        context: SkillContext,
+        op: str,
+        object_id: str = '',
+        *,
+        target: Transform | None = None,
+        target_name: str = '',
+        event: str = 'failed',
+    ) -> dict[str, Any]:
+        try:
+            state = self.supervisor.command(
+                op, object_id, target=target, target_name=target_name,
+            )
+        except Exception as error:
+            raise SkillError(event, str(error)) from error
+        if isinstance(state, dict):
+            context.update_from_state(state)
+        return state
+
+    # ------------------------------------------------------------------ geometry
+    def _live_pose(self, context: SkillContext, object_id: str) -> Transform:
+        state = self._state()
+        if object_id == 'cap':
+            pose = _state_pose((state.get('cap') or {}).get('pose'), 'cap')
+            if pose is not None:
+                return pose
+        if object_id.startswith('screw_'):
+            pose = _state_pose(
+                (state.get('screws') or {}).get(object_id, {}).get('pose'),
+                object_id,
+            )
+            if pose is not None:
+                return pose
+        if object_id in context.spec.tool_names:
+            pose = _state_pose(
+                (state.get('objects') or {}).get(f'grant_{object_id}'),
+                object_id,
+            )
+            if pose is not None:
+                return pose
+        if object_id == 'cap':
+            return world_target(context.spec, context.geometry, 'cap')
+        if object_id.startswith('screw_'):
+            return world_target(
+                context.spec, context.geometry, f'screw:{object_id}',
+            )
+        if object_id in context.spec.tool_names:
+            return world_target(
+                context.spec, context.geometry, f'tool:{object_id}',
+            )
+        raise SkillError('failed', f'no live pose for {object_id!r}')
+
+    def _object_contact_goal(
+        self, context: SkillContext, object_id: str,
+    ) -> Transform:
+        if object_id in context.spec.tool_names:
+            return tcp_goal(
+                self._live_pose(context, object_id),
+                context.geometry.tool_tcp_to_nodes[object_id],
+            )
+        if object_id == 'cap':
+            return tcp_goal(
+                self._live_pose(context, object_id),
+                context.geometry.cap_tcp_to_node,
+            )
+        if object_id.startswith('screw_'):
+            return tcp_goal(
+                self._live_pose(context, object_id),
+                context.geometry.screw_tcp_to_node,
+            )
+        raise SkillError('failed', f'object {object_id!r} cannot be approached')
+
+    def _destination_goal(
+        self,
+        context: SkillContext,
+        object_id: str,
+        target: str,
+    ) -> Transform:
+        if target in ('', 'work'):
+            return context.geometry.safe_poses['work']
+        if target == 'hatch_seat':
+            seat = world_target(context.spec, context.geometry, 'cap_seat')
+            relative = context.cap_relative or context.geometry.cap_tcp_to_node
+            return tcp_goal(seat, relative)
+        if target.startswith('hole_'):
+            return tcp_goal(
+                world_target(context.spec, context.geometry, f'hole:{target}'),
+                context.geometry.screw_tcp_to_node,
+            )
+        if target.endswith('_socket'):
+            tool = target[: -len('_socket')]
+            return tcp_goal(
+                world_target(context.spec, context.geometry, f'tool:{tool}'),
+                context.geometry.tool_tcp_to_nodes[tool],
+            )
+        if target in context.spec.sealant_sides:
+            point = sealant_point(
+                context.geometry, context.spec, target, 0.0,
+            )
+            return tcp_goal(
+                point, context.geometry.tool_work_tips['sealant_tool'],
+            )
+        raise SkillError('failed', f'unknown target {target!r}')
+
+    def _contact_goal(
+        self,
+        context: SkillContext,
+        object_id: str,
+        target: str,
+    ) -> Transform:
+        if target in context.spec.sealant_sides:
+            return self._destination_goal(context, object_id, target)
+        if target and target != 'work':
+            return self._destination_goal(context, object_id, target)
+        return self._object_contact_goal(context, object_id)
+
+    def _signature(self, node: PlanNode) -> tuple[str, str]:
+        return (node.object, node.target)
+
+    def _ensure_approach(
+        self,
+        context: SkillContext,
+        node: PlanNode,
+        *,
+        gap: float | None = None,
+    ) -> Transform:
+        signature = self._signature(node)
+        if context.approach_matches(signature):
+            return context.approach_pose  # type: ignore[return-value]
+        goal = self._contact_goal(context, node.object, node.target)
+        safe = _above(
+            goal,
+            context.geometry.approach_gap if gap is None else gap,
+        )
+        self._move(context, safe, 'approach')
+        context.mark_approach(safe, signature)
+        return safe
+
+    def _ensure_contact(
+        self,
+        context: SkillContext,
+        node: PlanNode,
+    ) -> Transform:
+        self._ensure_approach(context, node)
+        if context.phase == 'contact' and context.contact_pose is not None:
+            return context.contact_pose
+        goal = self._contact_goal(context, node.object, node.target)
+        self._move(context, goal, 'lower')
+        context.mark_contact(goal)
+        return goal
+
+    def _lift_to_approach(
+        self,
+        context: SkillContext,
+        *,
+        label: str = 'lift',
+    ) -> None:
+        if context.phase == 'lifted':
+            return
+        if context.approach_pose is None:
+            raise SkillError('failed', f'{label}: saved approach pose is unknown')
+        self._move(context, context.approach_pose, 'retreat')
+        context.mark_lifted()
+
+    def _adapter(self, context: SkillContext) -> Any:
+        return context.scene_adapter or self.scene_adapter
+
+    # ------------------------------------------------------------------ watch
+    def _detect(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        # An axial withdrawal is mandatory before leaving a contact.
+        if context.phase == 'contact' and context.approach_pose is not None:
+            self._move(context, context.approach_pose, 'retreat')
+            context.mark_lifted()
+        work = context.geometry.safe_poses['work']
+        self._move(context, work, 'transfer')
+        context.reset_phase()
+        if context.capture_frame('top') is None:
+            return SkillOutcome('failed', {'reason': 'no fresh top camera frame'})
+        if node.object in context.close_inspection_objects:
+            if context.capture_frame('hand') is None:
+                return SkillOutcome(
+                    'failed', {'reason': 'no fresh close-inspection frame'},
+                )
+        found = self._object_present(context, node.object)
+        if not found:
+            return SkillOutcome('not_found', {'object': node.object})
+        pose = None
+        try:
+            pose = self._live_pose(context, node.object)
+            context.last_verified_pose = pose
+        except Exception:
+            pose = None
+        return SkillOutcome('found', {
+            'object': node.object,
+            'tcp': list(pose.xyz) if pose is not None else None,
+            'close_inspection': node.object in context.close_inspection_objects,
+        })
+
+    def _object_present(self, context: SkillContext, object_id: str) -> bool:
+        effective = object_id or ''
+        if effective in context.spec.tool_names:
+            return True
+        if effective == 'cap':
+            if context.spec.cap_state == 'absent':
+                return False
+            state = (self._state().get('cap') or {}).get('state', 'free')
+            return state != 'absent'
+        if effective.startswith('screw_'):
+            if not any(screw.name == effective for screw in context.spec.screws):
+                return False
+            state = (
+                (self._state().get('screws') or {}).get(effective, {}).get(
+                    'state', 'tray',
+                )
+            )
+            return state != 'delivered'
+        try:
+            context.catalog.destination_kind(effective, context.spec)
+            return True
+        except Exception:
+            return False
+
+    def _capture(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        stamp = context.capture_frame(node.object)
+        if stamp is None:
+            return SkillOutcome('failed', {'camera': node.object})
+        return SkillOutcome('success', {'camera': node.object, 'stamp': stamp})
+
+    # ------------------------------------------------------------------ phases
+    def _approach_skill(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        safe = self._ensure_approach(context, node)
+        return SkillOutcome('success', {
+            'object': node.object, 'target': node.target,
+            'approach_height': context.geometry.approach_gap,
+            'tcp': list(safe.xyz),
+        })
+
+    def _lower_skill(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        goal = self._ensure_contact(context, node)
+        return SkillOutcome('success', {
+            'object': node.object, 'target': node.target,
+            'tcp': list(goal.xyz),
+        })
+
+    def _lift(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        if context.phase == 'lifted':
+            return SkillOutcome('success', {'reused': True})
+        self._ensure_approach(context, node)
+        goal = context.approach_pose
+        self._lift_to_approach(context)
+        return SkillOutcome('success', {
+            'reused': False,
+            'tcp': list(goal.xyz) if goal is not None else None,
+        })
+
+    # ------------------------------------------------------------------ grab
+    def _grab(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        obj = node.object
+        if obj in context.spec.tool_names:
+            return self._grab_tool(node, context)
+        if obj == 'cap':
+            if context.mounted_tool != 'vacuum':
+                return SkillOutcome('failed', {
+                    'reason': 'grab(cap) requires the vacuum tool in patron',
+                })
+            return self._grab_part(node, context, 'grab_cap', 'vacuum')
+        if obj.startswith('screw_'):
+            if context.mounted_tool != 'magnetic_tool':
+                return SkillOutcome('failed', {
+                    'reason': 'grab(screw) requires magnetic_tool in patron',
+                })
+            return self._grab_part(
+                node, context, 'grab_screw', 'magnetic_tool',
+            )
+        return SkillOutcome('failed', {'reason': f'{obj!r} cannot be grasped'})
+
+    def _grab_tool(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        tool = node.object
+        if context.mounted_tool == tool:
+            return SkillOutcome('success', {'reused': True, 'tool': tool})
+        self._ensure_contact(context, node)
+        try:
+            self._command(
+                context, 'attach_tool', tool, event='grasp_failed',
+            )
+        except SkillError as error:
+            context.ownership_unknown = True
+            return SkillOutcome('grasp_failed', {'reason': str(error)})
+        context.mounted_tool = tool
+        context.held_object = None
+        context.ownership_unknown = False
+        adapter = self._adapter(context)
+        if adapter is not None:
+            try:
+                adapter.attach_tool(tool)
+            except Exception as error:
+                return SkillOutcome('failed', {'reason': f'planning scene: {error}'})
+        self._lift_to_approach(context)
+        pose = context.approach_pose
+        return SkillOutcome('success', {
+            'tool': tool,
+            'tcp': list(pose.xyz) if pose is not None else None,
+        })
+
+    def _grab_part(
+        self,
+        node: PlanNode,
+        context: SkillContext,
+        command: str,
+        tool: str,
+    ) -> SkillOutcome:
+        contact = self._ensure_contact(context, node)
+        try:
+            self._command(context, command, node.object, event='grasp_failed')
+        except SkillError as error:
+            context.ownership_unknown = True
+            return SkillOutcome('grasp_failed', {'reason': str(error)})
+        context.held_object = node.object
+        context.mounted_tool = tool
+        context.ownership_unknown = False
+        if node.object == 'cap':
+            cap_pose = self._live_pose(context, node.object)
+            context.cap_relative = contact.inverse().compose_relaxed(cap_pose)
+        adapter = self._adapter(context)
+        if adapter is not None:
+            try:
+                if node.object == 'cap':
+                    adapter.attach_cap()
+                else:
+                    adapter.attach_screw(node.object)
+            except Exception as error:
+                return SkillOutcome('failed', {'reason': f'planning scene: {error}'})
+        self._lift_to_approach(context)
+        return SkillOutcome('success', {
+            'object': node.object, 'tcp': list(contact.xyz),
+        })
+
+    # ------------------------------------------------------------------ transfer
+    def _transfer(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        obj = node.object
+        target = node.target
+        if obj in context.spec.tool_names:
+            if context.mounted_tool != obj:
+                return SkillOutcome('failed', {
+                    'reason': f'{obj!r} is not mounted',
+                })
+        elif obj == 'cap':
+            if context.held_object != 'cap':
+                return SkillOutcome('failed', {'reason': 'hatch is not held'})
+        elif obj.startswith('screw_'):
+            if context.held_object != obj:
+                return SkillOutcome('failed', {'reason': f'{obj!r} is not held'})
+        else:
+            return SkillOutcome('failed', {'reason': f'{obj!r} cannot be transferred'})
+        goal = self._destination_goal(context, obj, target)
+        safe = _above(goal, context.geometry.transfer_gap)
+        self._move(context, safe, 'transfer')
+        context.transfer_target = target
+        context.mark_approach(safe, (obj, target))
+        return SkillOutcome('success', {
+            'object': obj, 'target': target,
+            'approach_height': context.geometry.transfer_gap,
+            'tcp': list(safe.xyz),
+        })
+
+    def _move_to_storage(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        if context.held_object is None:
+            return SkillOutcome('failed', {'reason': 'nothing is held'})
+        transferred = self._transfer(node, context)
+        if transferred.event != 'success':
+            return transferred
+        lowered = self._lower_skill(node, context)
+        if lowered.event != 'success':
+            return lowered
+        return SkillOutcome('success', {
+            'object': node.object, 'target': node.target,
+            'tcp': list(context.contact_pose.xyz) if context.contact_pose else None,
+        })
+
+    def _align(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        if context.held_object is None and context.mounted_tool is None:
+            return SkillOutcome('failed', {'reason': 'nothing is held or mounted'})
+        if context.approach_pose is not None:
+            self._move(context, context.approach_pose, 'align')
+            context.phase = 'approach'
+            return SkillOutcome('success', {'reused': True})
+        safe = self._ensure_approach(context, node)
+        return SkillOutcome('success', {'tcp': list(safe.xyz)})
+
+    # ------------------------------------------------------------------ release
+    def _release(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        obj = node.object
+        target = node.target
+        if obj in context.spec.tool_names:
+            return self._release_tool(node, context)
+        if obj == 'cap':
+            return self._release_cap(node, context)
+        if obj.startswith('screw_'):
+            return self._handoff_screw(node, context)
+        return SkillOutcome('failed', {'reason': f'{obj!r} cannot be released'})
+
+    def _release_tool(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        tool = node.object
+        socket = f'{tool}_socket'
+        if context.mounted_tool != tool:
+            return SkillOutcome('failed', {
+                'reason': f'{tool!r} is not mounted',
+                'moved': False,
+            })
+        if node.target != socket:
+            # A bare release(vacuum) from work must not move the robot or
+            # detach the cup.  Only an explicit socket target is valid.
+            return SkillOutcome('failed', {
+                'reason': (
+                    f'release({tool}) requires the explicit socket target '
+                    f'{socket!r}; no motion'
+                ),
+                'moved': False,
+            })
+        if context.transfer_target != socket:
+            return SkillOutcome('failed', {
+                'reason': (
+                    f'release({tool},{socket}) requires an explicit preceding '
+                    'transfer to that socket'
+                ),
+                'moved': False,
+            })
+        if not context.approach_matches((tool, socket)):
+            goal = self._destination_goal(context, tool, socket)
+            safe = _above(goal, context.geometry.approach_gap)
+            self._move(context, safe, 'transfer')
+            context.mark_approach(safe, (tool, socket))
+        if context.phase != 'contact':
+            goal = self._destination_goal(context, tool, socket)
+            self._move(context, goal, 'lower')
+            context.mark_contact(goal)
+        try:
+            self._command(context, 'dock_tool', tool)
+        except SkillError as error:
+            return SkillOutcome('failed', {'reason': str(error)})
+        context.mounted_tool = None
+        adapter = self._adapter(context)
+        if adapter is not None:
+            try:
+                adapter.dock_tool(tool)
+            except Exception as error:
+                return SkillOutcome('failed', {'reason': f'planning scene: {error}'})
+        self._lift_to_approach(context)
+        return SkillOutcome('success', {'tool': tool, 'target': socket})
+
+    def _release_cap(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        if context.held_object != 'cap':
+            return SkillOutcome('failed', {'reason': 'hatch is not held'})
+        target = node.target or 'hatch_seat'
+        self._ensure_approach(context, node)
+        if context.phase != 'contact':
+            goal = self._destination_goal(context, 'cap', target)
+            self._move(context, goal, 'lower')
+            context.mark_contact(goal)
+        seat = world_target(context.spec, context.geometry, 'cap_seat')
+        try:
+            self._command(context, 'place_cap', 'cap', target=seat)
+        except SkillError as error:
+            return SkillOutcome('failed', {'reason': str(error)})
+        context.held_object = None
+        context.cap_relative = None
+        adapter = self._adapter(context)
+        if adapter is not None:
+            try:
+                adapter.place_cap(seat)
+            except Exception as error:
+                return SkillOutcome('failed', {'reason': f'planning scene: {error}'})
+        self._lift_to_approach(context)
+        return SkillOutcome('success', {'object': 'cap', 'target': target})
+
+    def _dock_tool(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        if node.object not in context.spec.tool_names:
+            return SkillOutcome('failed', {'reason': f'{node.object!r} is not a tool'})
+        socket = f'{node.object}_socket'
+        if node.target != socket:
+            return SkillOutcome('failed', {
+                'reason': f'dock_tool requires target {socket!r}',
+            })
+        return self._release_tool(node, context)
+
+    def _handoff_screw(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        screw = node.object
+        hole = node.target
+        if context.held_object != screw:
+            return SkillOutcome('grasp_failed', {
+                'reason': f'{screw!r} is not held',
+            })
+        if context.mounted_tool != 'magnetic_tool':
+            return SkillOutcome('failed', {'reason': 'magnetic_tool is not mounted'})
+        self._ensure_approach(context, node)
+        if context.phase != 'contact':
+            goal = self._destination_goal(context, screw, hole)
+            self._move(context, goal, 'lower')
+            context.mark_contact(goal)
+        target = world_target(context.spec, context.geometry, f'hole:{hole}')
+        try:
+            self._command(
+                context, 'handoff_screw', screw,
+                target=target, target_name=hole, event='grasp_failed',
+            )
+        except SkillError as error:
+            return SkillOutcome('grasp_failed', {'reason': str(error)})
+        context.held_object = None
+        adapter = self._adapter(context)
+        if adapter is not None:
+            try:
+                adapter.handoff_screw(screw)
+            except Exception as error:
+                return SkillOutcome('failed', {'reason': f'planning scene: {error}'})
+        self._lift_to_approach(context)
+        return SkillOutcome('success', {
+            'object': screw, 'target': hole,
+            'installed': False, 'tightened': False,
+        })
+
+    # ------------------------------------------------------------------ misc
+    def _retreat(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        del node
+        if context.phase == 'contact' and context.approach_pose is not None:
+            self._move(context, context.approach_pose, 'retreat')
+            context.mark_lifted()
+        work = context.geometry.safe_poses['work']
+        self._move(context, work, 'transfer')
+        context.reset_phase()
+        return SkillOutcome('success', {'tcp': list(work.xyz)})
+
+    def _dispense(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        if context.mounted_tool != 'sealant_tool':
+            return SkillOutcome('failed', {'reason': 'sealant_tool is not mounted'})
+        side = node.object or node.target
+        start_fraction = float(node.params.get('start_fraction', 0.0))
+        end_fraction = float(node.params.get('end_fraction', 1.0))
+        start = sealant_point(
+            context.geometry, context.spec, side, start_fraction,
+        )
+        end = sealant_point(
+            context.geometry, context.spec, side, end_fraction,
+        )
+        tip = context.geometry.tool_work_tips['sealant_tool']
+        start_goal = tcp_goal(start, tip)
+        end_goal = tcp_goal(end, tip)
+        signature = (side, f'{start_fraction:.3f}:{end_fraction:.3f}')
+        if not context.approach_matches(signature):
+            safe = _above(start_goal, context.geometry.approach_gap)
+            self._move(context, safe, 'approach')
+            context.mark_approach(safe, signature)
+        if context.phase != 'contact':
+            self._move(context, start_goal, 'lower')
+            context.mark_contact(start_goal)
+        self._move(context, end_goal, 'dispense')
+        context.mark_contact(end_goal)
+        self._lift_to_approach(context)
+        return SkillOutcome('success', {
+            'side': side,
+            'start_fraction': start_fraction,
+            'end_fraction': end_fraction,
+        })
+
+    def _stop(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        del node
+        try:
+            self.motion.stop('graph stop node')
+        except Exception as error:
+            return SkillOutcome('failed', {'reason': str(error)})
+        return SkillOutcome('success', {
+            'tool': context.mounted_tool,
+            'held': context.held_object,
+        })
+
+    def _wait_scene_clear(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        del node
+        if context.scene_is_clear():
+            return SkillOutcome('clear')
+        return SkillOutcome('timeout', {'reason': 'scene did not become clear'})
+
+    def _inspect_state(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        state = self._state()
+        obj = node.object
+        if not state:
+            context.ownership_unknown = True
+            return SkillOutcome('unknown', {'reason': 'supervisor state unavailable'})
+        mounted = state.get('mounted_tool')
+        held = state.get('held_object')
+        if held == obj:
+            context.mounted_tool = mounted if isinstance(mounted, str) else context.mounted_tool
+            context.held_object = obj
+            context.ownership_unknown = False
+            return SkillOutcome('holding', {'object': obj})
+        if obj == 'cap':
+            cap_state = (state.get('cap') or {}).get('state')
+            if cap_state == 'installed':
+                context.held_object = None
+                context.ownership_unknown = False
+                return SkillOutcome('placed', {'object': obj})
+            if cap_state == 'free':
+                context.held_object = None
+                context.ownership_unknown = False
+                return SkillOutcome('empty', {'object': obj})
+            if cap_state == 'held':
+                context.held_object = 'cap'
+                context.ownership_unknown = False
+                return SkillOutcome('holding', {'object': obj})
+        if obj.startswith('screw_'):
+            screw = (state.get('screws') or {}).get(obj, {})
+            screw_state = screw.get('state')
+            if screw_state == 'delivered':
+                context.held_object = None
+                context.ownership_unknown = False
+                return SkillOutcome('placed', {'object': obj})
+            if screw_state == 'held':
+                context.held_object = obj
+                context.ownership_unknown = False
+                return SkillOutcome('holding', {'object': obj})
+            if screw_state == 'tray':
+                context.held_object = None
+                context.ownership_unknown = False
+                return SkillOutcome('empty', {'object': obj})
+        if obj in context.spec.tool_names:
+            tool_state = (state.get('tools') or {}).get(obj, {}).get('state')
+            if tool_state == 'mounted':
+                context.mounted_tool = obj
+                context.ownership_unknown = False
+                return SkillOutcome('holding', {'object': obj})
+            if tool_state == 'docked':
+                context.mounted_tool = None
+                context.ownership_unknown = False
+                return SkillOutcome('empty', {'object': obj})
+        context.ownership_unknown = True
+        return SkillOutcome('unknown', {
+            'object': obj, 'mounted_tool': mounted, 'held_object': held,
+        })
+
+    def _notify_operator(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        reason = node.target or 'operator intervention required'
+        if context.notify(reason, {
+            'node': node.id, 'tool': context.mounted_tool,
+            'held': context.held_object,
+        }):
+            return SkillOutcome('success', {'reason': reason})
+        return SkillOutcome('failed', {'reason': reason})
+
+    def _verify(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        state = self._state()
+        obj = node.object
+        target = node.target
+        if not state:
+            return SkillOutcome('failed', {'reason': 'supervisor state unavailable'})
+        if obj == 'cap':
+            cap = state.get('cap') or {}
+            if cap.get('state') != 'installed':
+                return SkillOutcome('not_ok', {
+                    'reason': f'hatch state is {cap.get("state")!r}',
+                })
+            pose = _state_pose(cap.get('pose'), 'cap')
+            if pose is None:
+                return SkillOutcome('failed', {'reason': 'hatch pose unavailable'})
+            seat = target or 'hatch_seat'
+            try:
+                expected = world_target(
+                    context.spec, context.geometry, 'cap_seat',
+                )
+            except Exception as error:
+                return SkillOutcome('failed', {'reason': str(error)})
+            distance = _distance(pose, expected)
+            if distance <= context.geometry.position_tolerance:
+                return SkillOutcome('ok', {
+                    'object': obj, 'target': seat, 'distance': distance,
+                })
+            return SkillOutcome('not_ok', {
+                'object': obj, 'target': seat, 'distance': distance,
+            })
+        if obj.startswith('screw_'):
+            screw = (state.get('screws') or {}).get(obj, {})
+            if screw.get('state') == 'delivered' and screw.get('hole') == target:
+                return SkillOutcome('ok', {'object': obj, 'target': target})
+            return SkillOutcome('not_ok', {
+                'object': obj, 'target': target,
+                'state': screw.get('state'),
+            })
+        inspected = self._inspect_state(node, context)
+        if inspected.event == 'placed':
+            return SkillOutcome('ok', {'object': obj})
+        if inspected.event in ('empty', 'holding'):
+            return SkillOutcome('not_ok', {'object': obj, 'state': inspected.event})
+        return SkillOutcome('failed', {'object': obj})

src/iiwa_llm_plan_executor/test/test_skills.py

@@ -0,0 +1,477 @@
+"""Skill phases, tool ownership and object handling on a fake backend."""
+from __future__ import annotations
+
+import copy
+from pathlib import Path
+
+import pytest
+
+from iiwa_experement.geometry import load_geometry, world_target
+from iiwa_experement.object_state import transform_json
+from iiwa_experement.scenario import load_scenario
+from iiwa_llm_plan_executor.moveit_backend import MotionOutcome
+from iiwa_llm_plan_executor.schema import PlanNode
+from iiwa_llm_plan_executor.skill_context import SkillContext
+from iiwa_llm_plan_executor.skills import SkillRegistry
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+ASSETS = PACKAGE.parent / 'iiwa_experement' / 'config'
+SCENES = ASSETS / 'scenes.yaml'
+GEOMETRY = load_geometry(ASSETS / 'geometry.yaml')
+
+
+class FakeMotion:
+    def __init__(self, *, success=True):
+        self.calls = []
+        self.stop_calls = []
+        self.success = success
+
+    def plan_and_execute(self, goal, phase, held_object):
+        self.calls.append((goal, phase, held_object))
+        return MotionOutcome(
+            self.success, 'ok' if self.success else 'motion_limit',
+            'ok' if self.success else 'rejected by policy',
+        )
+
+    def stop(self, reason='stop'):
+        self.stop_calls.append(reason)
+
+
+class FakeSupervisor:
+    def __init__(self, spec, geometry):
+        self.spec = spec
+        self.geometry = geometry
+        self.commands = []
+        self.state = {
+            'version': 1,
+            'mounted_tool': None,
+            'held_object': None,
+            'cap': {'state': spec.cap_state, 'pose': None},
+            'tools': {},
+            'screws': {},
+            'holes': {},
+            'objects': {},
+            'tcp': None,
+        }
+        if spec.cap_state != 'absent':
+            self.state['cap']['pose'] = transform_json(
+                world_target(spec, geometry, 'cap')
+            )
+        for tool in spec.tools:
+            self.state['tools'][tool.name] = {
+                'state': 'docked',
+                'pose': transform_json(world_target(spec, geometry, f'tool:{tool.name}')),
+            }
+            self.state['objects'][f'grant_{tool.name}'] = (
+                self.state['tools'][tool.name]['pose']
+            )
+        for screw in spec.screws:
+            self.state['screws'][screw.name] = {
+                'state': 'tray',
+                'pose': transform_json(
+                    world_target(spec, geometry, f'screw:{screw.name}')
+                ),
+                'hole': None,
+            }
+            self.state['objects'][f'grant_screw_{int(screw.name.split("_")[1]):02d}'] = (
+                self.state['screws'][screw.name]['pose']
+            )
+        self.state['objects']['grant_cap'] = self.state['cap']['pose']
+
+    def latest(self):
+        return copy.deepcopy(self.state)
+
+    def _advance(self):
+        self.state['version'] += 1
+        return copy.deepcopy(self.state)
+
+    def command(self, op, object_id='', target=None, target_name=''):
+        self.commands.append((op, object_id, target_name))
+        if op == 'attach_tool':
+            if self.state['mounted_tool'] is not None:
+                raise RuntimeError('tool already mounted')
+            if object_id not in self.state['tools']:
+                raise RuntimeError('unknown tool')
+            if self.state['tools'][object_id]['state'] != 'docked':
+                raise RuntimeError('tool is not docked')
+            self.state['mounted_tool'] = object_id
+            self.state['tools'][object_id]['state'] = 'mounted'
+        elif op == 'dock_tool':
+            if self.state['mounted_tool'] != object_id:
+                raise RuntimeError('tool is not mounted')
+            self.state['mounted_tool'] = None
+            self.state['tools'][object_id]['state'] = 'docked'
+            self.state['tools'][object_id]['pose'] = transform_json(
+                world_target(self.spec, self.geometry, f'tool:{object_id}')
+            )
+        elif op == 'grab_cap':
+            if self.state['mounted_tool'] != 'vacuum':
+                raise RuntimeError('vacuum is not mounted')
+            if self.state['held_object'] is not None:
+                raise RuntimeError('hand is not free')
+            self.state['held_object'] = 'cap'
+            self.state['cap']['state'] = 'held'
+        elif op == 'place_cap':
+            if self.state['held_object'] != 'cap':
+                raise RuntimeError('hatch is not held')
+            self.state['held_object'] = None
+            self.state['cap']['state'] = 'installed'
+            self.state['cap']['pose'] = transform_json(target)
+        elif op == 'grab_screw':
+            if self.state['mounted_tool'] != 'magnetic_tool':
+                raise RuntimeError('magnet is not mounted')
+            if self.state['screws'][object_id]['state'] != 'tray':
+                raise RuntimeError('screw is unavailable')
+            self.state['held_object'] = object_id
+            self.state['screws'][object_id]['state'] = 'held'
+        elif op == 'handoff_screw':
+            if self.state['held_object'] != object_id:
+                raise RuntimeError('screw is not held')
+            if self.state['mounted_tool'] != 'magnetic_tool':
+                raise RuntimeError('magnet is not mounted')
+            self.state['held_object'] = None
+            self.state['screws'][object_id]['state'] = 'delivered'
+            self.state['screws'][object_id]['hole'] = target_name
+            self.state['screws'][object_id]['pose'] = transform_json(target)
+            self.state['holes'].setdefault(target_name, {'state': 'empty', 'screw': None})
+            self.state['holes'][target_name]['screw'] = object_id
+        elif op == 'inspect':
+            pass
+        else:
+            raise RuntimeError(f'unknown op {op}')
+        return self._advance()
+
+
+class FakeScene:
+    def __init__(self):
+        self.events = []
+
+    def attach_tool(self, tool):
+        self.events.append(('attach_tool', tool))
+
+    def dock_tool(self, tool):
+        self.events.append(('dock_tool', tool))
+
+    def attach_cap(self):
+        self.events.append(('attach_cap',))
+
+    def attach_screw(self, screw):
+        self.events.append(('attach_screw', screw))
+
+    def place_cap(self, seat):
+        self.events.append(('place_cap', seat))
+
+    def handoff_screw(self, screw):
+        self.events.append(('handoff_screw', screw))
+
+
+def make_registry(experiment_id=20, *, motion=None, adapter=None):
+    spec = load_scenario(experiment_id, SCENES)
+    supervisor = FakeSupervisor(spec, GEOMETRY)
+    motion = motion or FakeMotion()
+    registry = SkillRegistry(motion, supervisor, scene_adapter=adapter or FakeScene())
+    context = SkillContext(spec=spec, geometry=GEOMETRY, scene_revision=1)
+    context.scene_adapter = registry.scene_adapter
+    context.frame_provider = lambda name: 5.0
+    return registry, context, motion, supervisor
+
+
+def node(node_id, skill, obj='', target='', params=None):
+    return PlanNode(
+        id=node_id, skill=skill, object=obj, target=target,
+        params=params or {},
+    )
+
+
+def phases(motion):
+    return [phase for _, phase, _ in motion.calls]
+
+
+def test_approach_goes_above_the_target_and_saves_the_height():
+    registry, context, motion, _ = make_registry()
+    result = registry.execute(node('a', 'approach', 'cap'), context)
+    assert result.event == 'success'
+    assert phases(motion) == ['approach']
+    safe = motion.calls[0][0]
+    assert context.approach_pose is not None
+    assert context.phase == 'approach'
+    assert safe.xyz[2] > context.contact_pose.xyz[2] if context.contact_pose else True
+
+    repeated = registry.execute(node('a', 'approach', 'cap'), context)
+    assert repeated.event == 'success'
+    assert phases(motion) == ['approach']
+
+
+def test_grab_cap_runs_missing_phases_and_lifts_to_saved_height():
+    registry, context, motion, supervisor = make_registry()
+    context.mounted_tool = 'vacuum'
+    supervisor.state['mounted_tool'] = 'vacuum'
+    supervisor.state['tools']['vacuum']['state'] = 'mounted'
+    result = registry.execute(node('g', 'grab', 'cap'), context)
+    assert result.event == 'success'
+    assert phases(motion) == ['approach', 'lower', 'retreat']
+    assert supervisor.commands == [('grab_cap', 'cap', '')]
+    assert context.held_object == 'cap'
+    assert context.phase == 'lifted'
+    assert ('attach_cap',) in registry.scene_adapter.events
+
+
+def test_explicit_phases_are_not_repeated_by_grab():
+    registry, context, motion, supervisor = make_registry()
+    context.mounted_tool = 'vacuum'
+    supervisor.state['mounted_tool'] = 'vacuum'
+    assert registry.execute(node('a', 'approach', 'cap'), context).event == 'success'
+    assert registry.execute(node('l', 'lower', 'cap'), context).event == 'success'
+    assert registry.execute(node('g', 'grab', 'cap'), context).event == 'success'
+    assert phases(motion) == ['approach', 'lower', 'retreat']
+    assert registry.execute(node('up', 'lift', 'cap'), context).event == 'success'
+    assert phases(motion) == ['approach', 'lower', 'retreat']
+
+
+def test_grab_cap_without_vacuum_does_not_move():
+    registry, context, motion, _ = make_registry()
+    result = registry.execute(node('g', 'grab', 'cap'), context)
+    assert result.event == 'failed'
+    assert motion.calls == []
+
+
+def test_tool_grab_docks_at_the_socket_and_lifts_out():
+    registry, context, motion, supervisor = make_registry()
+    result = registry.execute(node('g', 'grab', 'vacuum'), context)
+    assert result.event == 'success'
+    assert phases(motion) == ['approach', 'lower', 'retreat']
+    assert supervisor.commands == [('attach_tool', 'vacuum', '')]
+    assert context.mounted_tool == 'vacuum'
+    assert ('attach_tool', 'vacuum') in registry.scene_adapter.events
+
+
+def test_bare_release_of_the_suction_cup_does_not_move_or_detach():
+    registry, context, motion, supervisor = make_registry()
+    context.mounted_tool = 'vacuum'
+    supervisor.state['mounted_tool'] = 'vacuum'
+    result = registry.execute(node('r', 'release', 'vacuum', ''), context)
+    assert result.event == 'failed'
+    assert motion.calls == []
+    assert supervisor.commands == []
+    assert context.mounted_tool == 'vacuum'
+
+
+def test_release_at_socket_requires_the_explicit_transfer():
+    registry, context, motion, supervisor = make_registry()
+    context.mounted_tool = 'vacuum'
+    supervisor.state['mounted_tool'] = 'vacuum'
+    result = registry.execute(
+        node('r', 'release', 'vacuum', 'vacuum_socket'), context,
+    )
+    assert result.event == 'failed'
+    assert motion.calls == []
+    assert context.mounted_tool == 'vacuum'
+
+
+def test_explicit_transfer_then_dock_keeps_tool_in_patron_until_dock():
+    registry, context, motion, supervisor = make_registry()
+    assert registry.execute(node('g', 'grab', 'vacuum'), context).event == 'success'
+    transfer = registry.execute(
+        node('t', 'transfer', 'vacuum', 'work'), context,
+    )
+    assert transfer.event == 'success'
+    assert context.mounted_tool == 'vacuum'
+    assert phases(motion)[-1] == 'transfer'
+
+    to_socket = registry.execute(
+        node('t2', 'transfer', 'vacuum', 'vacuum_socket'), context,
+    )
+    assert to_socket.event == 'success'
+    dock = registry.execute(
+        node('d', 'dock_tool', 'vacuum', 'vacuum_socket'), context,
+    )
+    assert dock.event == 'success'
+    assert context.mounted_tool is None
+    assert supervisor.commands[-1][0] == 'dock_tool'
+    assert ('dock_tool', 'vacuum') in registry.scene_adapter.events
+    assert phases(motion)[-2:] == ['lower', 'retreat']
+
+
+def test_cap_transfer_align_and_release_place_the_hatch():
+    registry, context, motion, supervisor = make_registry()
+    context.mounted_tool = 'vacuum'
+    context.held_object = 'cap'
+    supervisor.state['mounted_tool'] = 'vacuum'
+    supervisor.state['held_object'] = 'cap'
+    supervisor.state['cap']['state'] = 'held'
+    assert registry.execute(
+        node('t', 'transfer', 'cap', 'hatch_seat'), context,
+    ).event == 'success'
+    assert registry.execute(node('a', 'align', 'cap', 'hatch_seat'), context).event == 'success'
+    before = len(motion.calls)
+    result = registry.execute(node('r', 'release', 'cap', 'hatch_seat'), context)
+    assert result.event == 'success'
+    assert phases(motion)[before:] == ['lower', 'retreat']
+    assert context.held_object is None
+    assert supervisor.state['cap']['state'] == 'installed'
+    assert ('place_cap',) in [tuple(event[:1]) for event in registry.scene_adapter.events]
+    assert supervisor.commands[-1][0] == 'place_cap'
+
+
+def test_release_lifts_axially_before_any_lateral_motion():
+    registry, context, motion, _ = make_registry()
+    context.mounted_tool = 'vacuum'
+    context.held_object = 'cap'
+    context.transfer_target = 'hatch_seat'
+    seat = world_target(context.spec, context.geometry, 'cap_seat')
+    from iiwa_experement.geometry import tcp_goal
+    contact = tcp_goal(seat, context.geometry.cap_tcp_to_node)
+    from iiwa_llm_plan_executor.skills import _above
+    safe = _above(contact, context.geometry.approach_gap)
+    context.mark_approach(safe, ('cap', 'hatch_seat'))
+    context.mark_contact(contact)
+    registry.supervisor.state['mounted_tool'] = 'vacuum'
+    registry.supervisor.state['held_object'] = 'cap'
+    registry.supervisor.state['cap']['state'] = 'held'
+    result = registry.execute(node('r', 'release', 'cap', 'hatch_seat'), context)
+    assert result.event == 'success'
+    assert phases(motion)[-1] == 'retreat'
+    assert motion.calls[-1][1] == 'retreat'
+    assert motion.calls[-1][0].xyz[2] > contact.xyz[2]
+
+
+def test_dispense_moves_along_the_segment_without_changing_ownership():
+    registry, context, motion, _ = make_registry()
+    context.mounted_tool = 'sealant_tool'
+    result = registry.execute(
+        node('d', 'dispense', 'side_1', params={
+            'start_fraction': 0.0, 'end_fraction': 1.0,
+        }),
+        context,
+    )
+    assert result.event == 'success'
+    assert phases(motion) == ['approach', 'lower', 'dispense', 'retreat']
+    assert context.mounted_tool == 'sealant_tool'
+    assert context.held_object is None
+
+
+def test_screw_handoff_delivers_to_the_operator_and_keeps_the_hole_empty():
+    registry, context, motion, supervisor = make_registry()
+    context.mounted_tool = 'magnetic_tool'
+    context.held_object = 'screw_1'
+    supervisor.state['mounted_tool'] = 'magnetic_tool'
+    supervisor.state['held_object'] = 'screw_1'
+    supervisor.state['screws']['screw_1']['state'] = 'held'
+    assert registry.execute(
+        node('t', 'transfer', 'screw_1', 'hole_1'), context,
+    ).event == 'success'
+    before = len(motion.calls)
+    result = registry.execute(
+        node('h', 'handoff_screw', 'screw_1', 'hole_1'), context,
+    )
+    assert result.event == 'success'
+    assert phases(motion)[before:] == ['lower', 'retreat']
+    assert supervisor.state['screws']['screw_1']['state'] == 'delivered'
+    assert supervisor.state['screws']['screw_1']['hole'] == 'hole_1'
+    assert supervisor.state['holes']['hole_1']['state'] == 'empty'
+    assert ('handoff_screw', 'screw_1') in registry.scene_adapter.events
+    assert result.details['installed'] is False
+    assert result.details['tightened'] is False
+
+
+def test_detect_withdraws_axially_then_uses_fresh_frames():
+    registry, context, motion, _ = make_registry()
+    context.mark_approach(
+        GEOMETRY.safe_poses['work'], ('cap', ''),
+    )
+    context.mark_contact(GEOMETRY.safe_poses['work'])
+    result = registry.execute(node('d', 'detect', 'cap'), context)
+    assert result.event == 'found'
+    assert phases(motion)[0] == 'retreat'
+    assert phases(motion)[1] == 'transfer'
+    assert context.phase == 'free'
+
+    context.frame_provider = lambda name: None
+    assert registry.execute(node('d', 'detect', 'cap'), context).event == 'failed'
+
+    context.frame_provider = lambda name: 5.0
+    context.close_inspection_objects = frozenset({'cap'})
+    context.frame_provider = lambda name: 5.0 if name == 'top' else None
+    assert registry.execute(node('d', 'detect', 'cap'), context).event == 'failed'
+
+
+def test_capture_requires_a_fresh_frame():
+    registry, context, _, _ = make_registry()
+    assert registry.execute(node('c', 'capture', 'top'), context).event == 'success'
+    context.frame_provider = lambda name: None
+    assert registry.execute(node('c', 'capture', 'top'), context).event == 'failed'
+
+
+def test_inspect_state_distinguishes_empty_holding_placed_unknown():
+    registry, context, _, supervisor = make_registry()
+
+    supervisor.state['cap']['state'] = 'free'
+    assert registry.execute(node('i', 'inspect_state', 'cap'), context).event == 'empty'
+
+    supervisor.state['held_object'] = 'cap'
+    supervisor.state['cap']['state'] = 'held'
+    assert registry.execute(node('i', 'inspect_state', 'cap'), context).event == 'holding'
+
+    supervisor.state['held_object'] = None
+    supervisor.state['cap']['state'] = 'installed'
+    assert registry.execute(node('i', 'inspect_state', 'cap'), context).event == 'placed'
+
+    supervisor.state['cap']['state'] = 'bogus'
+    assert registry.execute(node('i', 'inspect_state', 'cap'), context).event == 'unknown'
+
+
+def test_inspect_state_recovers_ownership_after_a_failed_grasp():
+    registry, context, _, supervisor = make_registry()
+    supervisor.state['held_object'] = None
+    supervisor.state['cap']['state'] = 'free'
+    context.ownership_unknown = True
+    result = registry.execute(node('i', 'inspect_state', 'cap'), context)
+    assert result.event == 'empty'
+    assert context.ownership_unknown is False
+
+
+def test_verify_measures_the_placed_hatch():
+    registry, context, _, supervisor = make_registry()
+    supervisor.state['cap']['state'] = 'installed'
+    supervisor.state['cap']['pose'] = transform_json(
+        world_target(context.spec, context.geometry, 'cap_seat')
+    )
+    result = registry.execute(
+        node('v', 'verify', 'cap', 'hatch_seat'), context,
+    )
+    assert result.event == 'ok'
+
+    shifted = world_target(context.spec, context.geometry, 'cap_seat')
+    shifted = type(shifted)(
+        xyz=(shifted.xyz[0] + 0.02, shifted.xyz[1], shifted.xyz[2]),
+        rotation=shifted.rotation,
+        parent=shifted.parent,
+        child=shifted.child,
+    )
+    supervisor.state['cap']['pose'] = transform_json(shifted)
+    assert registry.execute(
+        node('v', 'verify', 'cap', 'hatch_seat'), context,
+    ).event == 'not_ok'
+
+
+def test_stop_preserves_ownership_and_wait_scene_clear_reports_timeout():
+    registry, context, motion, _ = make_registry()
+    context.held_object = 'cap'
+    assert registry.execute(node('s', 'stop', 'robot'), context).event == 'success'
+    assert motion.stop_calls == ['graph stop node']
+    assert context.held_object == 'cap'
+    context.scene_clear_provider = lambda: False
+    assert registry.execute(
+        node('w', 'wait_scene_clear', 'workspace'), context,
+    ).event == 'timeout'
+    context.scene_clear_provider = lambda: True
+    assert registry.execute(
+        node('w', 'wait_scene_clear', 'workspace'), context,
+    ).event == 'clear'
+
+
+def test_unknown_skill_and_backend_failure_are_not_successes():
+    registry, context, motion, _ = make_registry(motion=FakeMotion(success=False))
+    assert registry.execute(node('x', 'tighten', 'hole_1'), context).event == 'failed'
+    assert registry.execute(node('a', 'approach', 'cap'), context).event == 'failed'