diff --git a/src/iiwa_llm_plan_executor/docs/interfaces.md b/src/iiwa_llm_plan_executor/docs/interfaces.md index 8f2fdc2..d4c7bc9 100644 --- a/src/iiwa_llm_plan_executor/docs/interfaces.md +++ b/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 diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skill_context.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skill_context.py new file mode 100644 index 0000000..24de901 --- /dev/null +++ b/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 diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py new file mode 100644 index 0000000..dae17bb --- /dev/null +++ b/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}) diff --git a/src/iiwa_llm_plan_executor/test/test_skills.py b/src/iiwa_llm_plan_executor/test/test_skills.py new file mode 100644 index 0000000..c91da39 --- /dev/null +++ b/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'