From 9ca08c01c226e1e7d279d47df9f90f87d35fb164 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Wed, 11 Nov 2026 00:14:33 +1000 Subject: [PATCH] feat(llm-plan-executor): parse and validate externally supplied graphs --- src/iiwa_llm_plan_executor/docs/interfaces.md | 90 +++ .../iiwa_llm_plan_executor/__init__.py | 23 + .../iiwa_llm_plan_executor/schema.py | 251 ++++++++ .../iiwa_llm_plan_executor/validator.py | 591 ++++++++++++++++++ src/iiwa_llm_plan_executor/package.xml | 36 ++ .../resource/iiwa_llm_plan_executor | 1 + src/iiwa_llm_plan_executor/setup.cfg | 4 + src/iiwa_llm_plan_executor/setup.py | 30 + .../test/test_schema.py | 153 +++++ .../test/test_validator.py | 412 ++++++++++++ 10 files changed, 1591 insertions(+) create mode 100644 src/iiwa_llm_plan_executor/docs/interfaces.md create mode 100644 src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/__init__.py create mode 100644 src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/schema.py create mode 100644 src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/validator.py create mode 100644 src/iiwa_llm_plan_executor/package.xml create mode 100644 src/iiwa_llm_plan_executor/resource/iiwa_llm_plan_executor create mode 100644 src/iiwa_llm_plan_executor/setup.cfg create mode 100644 src/iiwa_llm_plan_executor/setup.py create mode 100644 src/iiwa_llm_plan_executor/test/test_schema.py create mode 100644 src/iiwa_llm_plan_executor/test/test_validator.py diff --git a/src/iiwa_llm_plan_executor/docs/interfaces.md b/src/iiwa_llm_plan_executor/docs/interfaces.md new file mode 100644 index 0000000..01aab13 --- /dev/null +++ b/src/iiwa_llm_plan_executor/docs/interfaces.md @@ -0,0 +1,90 @@ +# `iiwa_llm_plan_executor` interfaces + +Graph contract version: **1** (it matches `iiwa_scene_context` contract 1). +The executor never calls Qwen3 and never rewrites the model graph. + +## Graph response + +The accepted response is exactly: + +```json +{ + "status": "ready", + "entry": "p1_find", + "plan": [ + { + "id": "p1_find", + "skill": "detect", + "object": "cap", + "target": "", + "params": {}, + "transitions": [ + {"event": "found", "next": "p1_grab"}, + {"event": "not_found", "next": "replan_required"}, + {"event": "failed", "next": "replan_required"} + ] + } + ] +} +``` + +* `status` is `ready` or `blocked`. `blocked` is never executed. +* `plan` is a list of at most 256 nodes. Unknown keys, duplicate ids, + reserved terminal ids, missing `entry`, missing required events, unknown + events, unknown `next`, unreachable nodes and nodes that cannot reach a + terminal are rejected. +* Transitions may also be sent once at top level as + `{"from": "...", "event": "...", "to": "..."}`; they are merged into the + node by `parse_response`. Mixing both forms for the same event is rejected. +* Reserved terminals: `completed`, `replan_required`, `operator_required`. + `completed` is the verified end of the submitted graph; it is not a score of + the model against a Russian instruction. +* `params` is optional and currently only `dispense` accepts + `start_fraction`/`end_fraction` in `0..1`. + +## Skills and aliases + +Native: `detect`, `approach`, `lower`, `grab`, `lift`, `transfer`, `align`, +`release`, `retreat`, `capture`, `verify`, `stop`, `wait_scene_clear`, +`inspect_state`, `notify_operator`, `dispense`, `dock_tool`, +`handoff_screw`. + +Compatibility aliases: `pick` (= `grab`), `move_to_storage` +(= `transfer` + axial `lower`, keeping the grasp), `inspect` (= `verify`). +`tighten` is not in this profile and is rejected before any motion. + +The contract, outcomes and available ids come from +`iiwa_scene_context/catalog.py` and the live `scene_json`; a model cannot add +a skill or a contract. + +## Admission rules + +`parse_response(raw) -> PlanGraph` performs the strict JSON shape check. +`validate_graph(graph, scene_envelope, catalog, expected_scene_revision=...)` +returns `ValidationResult(ok, code, reason, details)` with codes: + +| code | meaning | +| --- | --- | +| `ok` | admitted | +| `blocked` | the model returned `status=blocked`; no motion | +| `replan_required` | a fresh scene/plan is required before motion (missing tool, missing explicit transfer, unconfirmed ownership) | +| `rejected` | structural or safety violation | +| `stale_scene` | the graph was built for another `scene_revision` | + +Important pre-movement rules: + +* `grab(cap)` requires `vacuum` in `patron`. If the scene starts with the cup + docked, the graph must explicitly `grab(vacuum)`; otherwise admission is + `replan_required` and nothing moves. +* `release(vacuum)` without a target, or anywhere except its named socket and + after an explicit `transfer(vacuum, vacuum_socket)`, is `replan_required`. + The cup stays mounted. `release(vacuum, vacuum_socket)` and + `dock_tool(vacuum, vacuum_socket)` are accepted only at the reached socket. +* A graph ending in `work` with `vacuum` mounted is accepted; the tool stays in + `patron`. +* `grasp_failed` must go to `inspect_state` or `stop`; it can never become + `completed`. `collision` must reach `stop`/`notify_operator` or + `replan_required`/`operator_required`. +* Screws are handed to the virtual operator at a hole; the hole stays empty + and no insertion/tightening occurs. +* No task is inferred from `experiment_id`; only the initial scene is selected. diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/__init__.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/__init__.py new file mode 100644 index 0000000..ccf74c3 --- /dev/null +++ b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/__init__.py @@ -0,0 +1,23 @@ +"""Graph validation and execution for externally supplied Qwen3 plans.""" +from .schema import ( + MAX_NODES, + TERMINALS, + PlanGraph, + PlanNode, + PlanSchemaError, + Transition, + parse_response, +) +from .validator import ValidationResult, validate_graph + +__all__ = [ + 'MAX_NODES', + 'TERMINALS', + 'PlanGraph', + 'PlanNode', + 'PlanSchemaError', + 'Transition', + 'ValidationResult', + 'parse_response', + 'validate_graph', +] diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/schema.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/schema.py new file mode 100644 index 0000000..0e6c061 --- /dev/null +++ b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/schema.py @@ -0,0 +1,251 @@ +"""Strict parsing of the externally supplied Qwen3 graph response.""" +from __future__ import annotations + +import json +from dataclasses import dataclass, field +from typing import Any, Mapping + + +MAX_NODES = 256 +MAX_RESPONSE_BYTES = 200_000 +ALLOWED_STATUSES = ('ready', 'blocked') +TERMINALS = ('completed', 'replan_required', 'operator_required') +TOP_LEVEL_KEYS = ('status', 'entry', 'plan', 'transitions') +NODE_KEYS = ('id', 'skill', 'object', 'target', 'transitions', 'params') + + +class PlanSchemaError(ValueError): + """The response is not a well-formed plan graph.""" + + +@dataclass(frozen=True) +class Transition: + event: str + next: str + + def as_dict(self) -> dict[str, str]: + return {'event': self.event, 'next': self.next} + + +@dataclass(frozen=True) +class PlanNode: + id: str + skill: str + object: str = '' + target: str = '' + params: dict[str, Any] = field(default_factory=dict) + transitions: tuple[Transition, ...] = () + + def as_dict(self) -> dict[str, Any]: + payload: dict[str, Any] = { + 'id': self.id, + 'skill': self.skill, + 'object': self.object, + 'target': self.target, + 'transitions': [entry.as_dict() for entry in self.transitions], + } + if self.params: + payload['params'] = dict(self.params) + return payload + + +@dataclass(frozen=True) +class PlanGraph: + status: str + entry: str + nodes: tuple[PlanNode, ...] + top_level_transitions: tuple[tuple[str, Transition], ...] = () + + def node(self, node_id: str) -> PlanNode: + for item in self.nodes: + if item.id == node_id: + return item + raise KeyError(node_id) + + @property + def ids(self) -> tuple[str, ...]: + return tuple(item.id for item in self.nodes) + + def as_dict(self) -> dict[str, Any]: + return { + 'status': self.status, + 'entry': self.entry, + 'plan': [item.as_dict() for item in self.nodes], + } + + +def _string(value: Any, label: str, *, allow_empty: bool = True) -> str: + if not isinstance(value, str): + raise PlanSchemaError(f'{label} must be a string') + text = value.strip() + if not allow_empty and not text: + raise PlanSchemaError(f'{label} must not be empty') + return text + + +def _params(raw: Any, label: str) -> dict[str, Any]: + if raw is None: + return {} + if not isinstance(raw, Mapping): + raise PlanSchemaError(f'{label}.params must be an object') + result: dict[str, Any] = {} + for key, value in raw.items(): + if not isinstance(key, str) or not key: + raise PlanSchemaError(f'{label}.params keys must be non-empty strings') + if isinstance(value, bool) or not isinstance(value, (int, float, str)): + raise PlanSchemaError( + f'{label}.params.{key} must be a number or string' + ) + result[key] = value + return result + + +def _transition(raw: Any, label: str) -> Transition: + if not isinstance(raw, Mapping): + raise PlanSchemaError(f'{label} must be an object') + extra = set(raw) - {'event', 'next'} + if extra: + raise PlanSchemaError(f'{label} has unknown keys {sorted(extra)}') + event = _string(raw.get('event'), f'{label}.event', allow_empty=False) + next_id = _string(raw.get('next'), f'{label}.next', allow_empty=False) + return Transition(event, next_id) + + +def _node(raw: Any, index: int) -> PlanNode: + label = f'plan[{index}]' + if not isinstance(raw, Mapping): + raise PlanSchemaError(f'{label} must be an object') + extra = set(raw) - set(NODE_KEYS) + if extra: + raise PlanSchemaError(f'{label} has unknown keys {sorted(extra)}') + node_id = _string(raw.get('id'), f'{label}.id', allow_empty=False) + skill = _string(raw.get('skill'), f'{label}.skill', allow_empty=False) + obj = _string(raw.get('object', ''), f'{label}.object') + target = _string(raw.get('target', ''), f'{label}.target') + transitions_raw = raw.get('transitions', []) + if not isinstance(transitions_raw, list): + raise PlanSchemaError(f'{label}.transitions must be a list') + transitions: list[Transition] = [] + seen: set[str] = set() + for t_index, entry in enumerate(transitions_raw): + transition = _transition(entry, f'{label}.transitions[{t_index}]') + if transition.event in seen: + raise PlanSchemaError( + f'{label} has duplicate event {transition.event!r}' + ) + seen.add(transition.event) + transitions.append(transition) + return PlanNode( + id=node_id, + skill=skill, + object=obj, + target=target, + params=_params(raw.get('params'), label), + transitions=tuple(transitions), + ) + + +def _merge_top_level( + nodes: list[PlanNode], + raw: Any, +) -> tuple[list[PlanNode], tuple[tuple[str, Transition], ...]]: + if raw is None: + return nodes, () + if not isinstance(raw, list): + raise PlanSchemaError('transitions must be a list') + by_id = {node.id: node for node in nodes} + merged: dict[str, list[Transition]] = { + node.id: list(node.transitions) for node in nodes + } + top_level: list[tuple[str, Transition]] = [] + for index, entry in enumerate(raw): + label = f'transitions[{index}]' + if not isinstance(entry, Mapping): + raise PlanSchemaError(f'{label} must be an object') + extra = set(entry) - {'from', 'event', 'to', 'next'} + if extra: + raise PlanSchemaError(f'{label} has unknown keys {sorted(extra)}') + source = _string(entry.get('from'), f'{label}.from', allow_empty=False) + next_id = entry.get('to', entry.get('next')) + transition = Transition( + _string(entry.get('event'), f'{label}.event', allow_empty=False), + _string(next_id, f'{label}.to', allow_empty=False), + ) + if source not in by_id: + raise PlanSchemaError(f'{label}.from references unknown node {source!r}') + existing = {item.event for item in merged[source]} + if transition.event in existing: + raise PlanSchemaError( + f'{label}: node {source!r} already has event ' + f'{transition.event!r}' + ) + merged[source].append(transition) + top_level.append((source, transition)) + result = [ + PlanNode( + id=node.id, + skill=node.skill, + object=node.object, + target=node.target, + params=node.params, + transitions=tuple(merged[node.id]), + ) + for node in nodes + ] + return result, tuple(top_level) + + +def parse_response(raw: str | Mapping[str, Any]) -> PlanGraph: + """Parse a model response or raise :class:`PlanSchemaError`.""" + if isinstance(raw, str): + if len(raw.encode('utf-8')) > MAX_RESPONSE_BYTES: + raise PlanSchemaError( + f'response exceeds {MAX_RESPONSE_BYTES} bytes' + ) + try: + payload = json.loads(raw) + except json.JSONDecodeError as error: + raise PlanSchemaError(f'response is not valid JSON: {error}') from error + elif isinstance(raw, Mapping): + payload = dict(raw) + else: + raise PlanSchemaError('response must be a JSON string or object') + if not isinstance(payload, Mapping): + raise PlanSchemaError('response must be a JSON object') + extra = set(payload) - set(TOP_LEVEL_KEYS) + if extra: + raise PlanSchemaError(f'response has unknown keys {sorted(extra)}') + status = _string(payload.get('status'), 'status', allow_empty=False) + if status not in ALLOWED_STATUSES: + raise PlanSchemaError( + f'status must be one of {list(ALLOWED_STATUSES)}, got {status!r}' + ) + entry = _string(payload.get('entry', ''), 'entry') + plan_raw = payload.get('plan') + if not isinstance(plan_raw, list): + raise PlanSchemaError('plan must be a list') + if len(plan_raw) > MAX_NODES: + raise PlanSchemaError( + f'plan contains {len(plan_raw)} nodes; limit is {MAX_NODES}' + ) + nodes: list[PlanNode] = [] + seen_ids: set[str] = set() + for index, raw_node in enumerate(plan_raw): + node = _node(raw_node, index) + if node.id in seen_ids: + raise PlanSchemaError(f'duplicate node id {node.id!r}') + if node.id in TERMINALS: + raise PlanSchemaError( + f'node id {node.id!r} is a reserved terminal' + ) + seen_ids.add(node.id) + nodes.append(node) + nodes, top_level = _merge_top_level(nodes, payload.get('transitions')) + if status == 'ready': + if not nodes: + raise PlanSchemaError('a ready graph must contain at least one node') + if not entry: + raise PlanSchemaError('a ready graph must have a non-empty entry') + elif nodes or entry: + raise PlanSchemaError('a blocked response must not contain an executable plan') + return PlanGraph(status, entry, tuple(nodes), top_level) diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/validator.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/validator.py new file mode 100644 index 0000000..e8d61de --- /dev/null +++ b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/validator.py @@ -0,0 +1,591 @@ +"""Validate a parsed graph against the live scene and the server catalog.""" +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Any, Iterable, Mapping + +from iiwa_scene_context.catalog import ALLOWED_PARAMS, SkillCatalog + +from .schema import TERMINALS, PlanGraph, PlanNode + + +SAFE_COLLISION_SKILLS = {'stop', 'notify_operator'} +SAFE_GRASP_FAILURE_SKILLS = {'inspect_state', 'stop'} +SAFE_FAILURE_TERMINALS = {'replan_required', 'operator_required'} +PROGRESS_EVENTS = { + 'success', 'ok', 'found', 'clear', 'empty', 'holding', 'placed', +} +REQUIRES_OBJECT = { + 'detect', 'approach', 'lower', 'grab', 'lift', 'transfer', 'align', + 'release', 'retreat', 'verify', 'dispense', 'dock_tool', + 'handoff_screw', 'pick', 'move_to_storage', 'inspect', 'inspect_state', + 'capture', +} + + +@dataclass(frozen=True) +class ValidationResult: + ok: bool + code: str + reason: str + details: dict[str, Any] = field(default_factory=dict) + + @property + def accepted(self) -> bool: + return self.ok + + def as_dict(self) -> dict[str, Any]: + return { + 'ok': self.ok, + 'code': self.code, + 'reason': self.reason, + 'details': dict(self.details), + } + + +@dataclass(frozen=True) +class _State: + mounted: str | None = None + held: str | None = None + unknown: bool = False + transfer_target: str | None = None + + +@dataclass(frozen=True) +class _Facts: + components: dict[str, str] + tools: dict[str, str] + destinations: dict[str, str] + cameras: set[str] + service_objects: set[str] + initial: _State + available_skills: set[str] + envelope: Mapping[str, Any] + + +def _scene_payload(scene: Mapping[str, Any]) -> tuple[Mapping[str, Any], Mapping[str, Any]]: + if 'scene_json' in scene: + payload = scene['scene_json'] + if not isinstance(payload, Mapping): + raise ValueError('scene_json must be an object') + return scene, payload + return {}, scene + + +def _facts(scene: Mapping[str, Any]) -> _Facts: + envelope, payload = _scene_payload(scene) + components = { + entry['id']: str((entry.get('attributes') or {}).get('kind', 'component')) + for entry in payload.get('components', []) + if isinstance(entry, Mapping) and isinstance(entry.get('id'), str) + } + tools = { + entry['id']: str(entry.get('socket', f"{entry['id']}_socket")) + for entry in payload.get('tools', []) + if isinstance(entry, Mapping) and isinstance(entry.get('id'), str) + } + destinations = { + entry['id']: str(entry.get('kind', 'destination')) + for entry in payload.get('destinations', []) + if isinstance(entry, Mapping) and isinstance(entry.get('id'), str) + } + cameras = set(payload.get('cameras', {})) + service_objects = set(payload.get('service_objects', {})) + initial_raw = payload.get('initial_state') or {} + initial = _State( + mounted=initial_raw.get('tool_mounted') or None, + held=initial_raw.get('held_object') or None, + ) + available = set(payload.get('available_skills', [])) + return _Facts( + components=components, + tools=tools, + destinations=destinations, + cameras=cameras, + service_objects=service_objects, + initial=initial, + available_skills=available, + envelope=envelope, + ) + + +def _reject(reason: str, *, code: str = 'rejected', **details) -> ValidationResult: + return ValidationResult(False, code, reason, details) + + +def _replan(reason: str, **details) -> ValidationResult: + return ValidationResult(False, 'replan_required', reason, details) + + +def _node_or_terminal(graph: PlanGraph, node_id: str) -> PlanNode | None: + if node_id in TERMINALS: + return None + return graph.node(node_id) + + +def _structural_checks(graph: PlanGraph, facts: _Facts) -> ValidationResult | None: + ids = set(graph.ids) + for node in graph.nodes: + if not node.transitions: + return _reject(f'node {node.id!r} has no transitions') + if node.skill not in facts.available_skills: + return _reject( + f'node {node.id!r} uses unknown or unavailable skill ' + f'{node.skill!r}' + ) + if node.object: + known = ( + node.object in facts.components + or node.object in facts.tools + or node.object in facts.cameras + or node.object in facts.service_objects + or node.object in facts.destinations + ) + if not known: + return _reject( + f'node {node.id!r} references unknown object ' + f'{node.object!r}' + ) + if node.target: + known = ( + node.target in facts.destinations + or node.target in facts.components + or node.target in facts.tools + ) + if not known: + return _reject( + f'node {node.id!r} references unknown target ' + f'{node.target!r}' + ) + if node.params: + allowed_params = set(ALLOWED_PARAMS.get(node.skill, ())) + unknown_params = set(node.params) - allowed_params + if unknown_params: + return _reject( + f'node {node.id!r} has unsupported params ' + f'{sorted(unknown_params)}' + ) + if node.skill == 'dispense': + for key in ('start_fraction', 'end_fraction'): + if key in node.params: + value = node.params[key] + if ( + isinstance(value, bool) + or not isinstance(value, (int, float)) + or not 0.0 <= float(value) <= 1.0 + ): + return _reject( + f'node {node.id!r} param {key} must be ' + 'within 0..1' + ) + start = float(node.params.get('start_fraction', 0.0)) + end = float(node.params.get('end_fraction', 1.0)) + if start > end: + return _reject( + f'node {node.id!r} dispense start_fraction exceeds ' + 'end_fraction' + ) + allowed_events = set(facts.envelope.get('skill_contracts', {}).get( + node.skill, {}, + ).get('outcomes', {})) + if not allowed_events: + # The server catalog remains authoritative even if the envelope + # was produced by an older snapshot. + from iiwa_scene_context.catalog import SKILL_OUTCOMES + allowed_events = set(SKILL_OUTCOMES.get(node.skill, {})) + events = {transition.event for transition in node.transitions} + missing = allowed_events - events + extra = events - allowed_events + if missing: + return _reject( + f'node {node.id!r} misses required transitions {sorted(missing)}' + ) + if extra: + return _reject( + f'node {node.id!r} has unknown events {sorted(extra)}' + ) + for transition in node.transitions: + if transition.next not in ids and transition.next not in TERMINALS: + return _reject( + f'node {node.id!r} points to unknown node ' + f'{transition.next!r}' + ) + if transition.next == node.id: + # A self-loop that does not inspect state would spin forever. + if node.skill not in ('inspect_state', 'wait_scene_clear'): + return _reject( + f'node {node.id!r} has a direct self-loop' + ) + if graph.entry not in ids: + return _reject(f'entry {graph.entry!r} is not a plan node') + + reachable: set[str] = set() + stack = [graph.entry] + while stack: + current = stack.pop() + if current in reachable or current not in ids: + continue + reachable.add(current) + stack.extend(item.next for item in graph.node(current).transitions) + unreachable = ids - reachable + if unreachable: + return _reject(f'unreachable nodes {sorted(unreachable)}') + + reverse: dict[str, set[str]] = {node_id: set() for node_id in ids} + for node in graph.nodes: + for transition in node.transitions: + if transition.next in ids: + reverse[transition.next].add(node.id) + can_finish = set(TERMINALS) + changed = True + while changed: + changed = False + for node_id in ids - can_finish: + if any(item in can_finish for item in _targets(graph.node(node_id))): + can_finish.add(node_id) + changed = True + dead = ids - can_finish + if dead: + return _reject( + f'nodes cannot reach a terminal: {sorted(dead)}' + ) + + for node in graph.nodes: + for transition in node.transitions: + target = _node_or_terminal(graph, transition.next) + if transition.event == 'collision': + if target is None and transition.next not in SAFE_FAILURE_TERMINALS: + return _reject( + f'collision in {node.id!r} must stop the operation' + ) + if target is not None and target.skill not in SAFE_COLLISION_SKILLS: + return _reject( + f'collision in {node.id!r} must reach stop/notify, ' + f'got {target.skill!r}' + ) + if transition.event == 'grasp_failed': + if target is None and transition.next not in SAFE_FAILURE_TERMINALS: + return _reject( + f'grasp_failed in {node.id!r} must not continue blindly' + ) + if target is not None and target.skill not in SAFE_GRASP_FAILURE_SKILLS: + return _reject( + f'grasp_failed in {node.id!r} must reach inspect_state ' + f'or stop, got {target.skill!r}' + ) + if transition.event in ( + 'not_found', 'failed', 'not_ok', 'timeout', 'unknown', + ) and transition.next == 'completed': + return _reject( + f'{transition.event} in {node.id!r} cannot map to completed' + ) + return None + + +def _targets(node: PlanNode) -> list[str]: + return [item.next for item in node.transitions] + + +def _precondition( + node: PlanNode, + state: _State, + facts: _Facts, +) -> ValidationResult | None: + skill = node.skill + obj = node.object + target = node.target + if node.id in ('',) or skill not in facts.available_skills: + return _reject(f'unknown skill {skill!r}') + + if skill in REQUIRES_OBJECT and not obj: + return _reject(f'{skill} in {node.id!r} requires an object') + + if state.unknown and skill not in ( + 'inspect_state', 'stop', 'notify_operator', 'wait_scene_clear', + ): + return _replan( + f'{node.id!r} follows an unconfirmed grasp; inspect_state is required', + ) + + if skill in ('grab', 'pick'): + if obj in facts.tools: + if state.mounted is not None: + return _replan( + f'{node.id!r} cannot grab {obj!r}: tool ' + f'{state.mounted!r} is already mounted' + ) + if state.held is not None: + return _replan( + f'{node.id!r} cannot grab {obj!r} while holding ' + f'{state.held!r}' + ) + return None + if obj == 'cap': + if 'cap' not in facts.components: + return _reject(f'{node.id!r}: hatch is absent from this scene') + if state.mounted != 'vacuum': + return _replan( + f'{node.id!r} requires the vacuum tool in patron; ' + f'mounted tool is {state.mounted!r}' + ) + if state.held is not None: + return _replan( + f'{node.id!r} cannot grab the hatch while holding ' + f'{state.held!r}' + ) + return None + if obj in facts.components and facts.components[obj] == 'screw': + if state.mounted != 'magnetic_tool': + return _replan( + f'{node.id!r} requires magnetic_tool in patron; ' + f'mounted tool is {state.mounted!r}' + ) + if state.held is not None: + return _replan( + f'{node.id!r} cannot grab a screw while holding ' + f'{state.held!r}' + ) + return None + return _reject(f'{node.id!r}: object {obj!r} cannot be grasped') + + if skill == 'release': + if obj in facts.tools: + socket = facts.tools[obj] + if state.mounted != obj: + return _replan( + f'{node.id!r}: {obj!r} is not mounted ' + f'(mounted={state.mounted!r})' + ) + if target != socket: + return _replan( + f'{node.id!r}: release({obj}) must name its socket ' + f'{socket!r}' + ) + if state.transfer_target != socket: + return _replan( + f'{node.id!r}: release({obj},{socket}) requires an explicit ' + 'preceding transfer to that socket' + ) + return None + if obj == 'cap': + if state.held != 'cap': + return _replan(f'{node.id!r}: hatch is not held') + if facts.destinations.get(target) != 'seat': + return _replan( + f'{node.id!r}: hatch release requires hatch_seat, ' + f'got {target!r}' + ) + return None + if obj in facts.components and facts.components[obj] == 'screw': + if state.held != obj: + return _replan(f'{node.id!r}: {obj!r} is not held') + if facts.destinations.get(target) != 'hole': + return _replan( + f'{node.id!r}: screw handoff requires a hole target' + ) + return None + return _reject(f'{node.id!r}: object {obj!r} cannot be released') + + if skill in ('transfer', 'move_to_storage'): + if obj in facts.tools: + if state.mounted != obj: + return _replan(f'{node.id!r}: {obj!r} is not mounted') + if target not in ('work', facts.tools[obj]): + return _replan( + f'{node.id!r}: tool transfer must target work or its socket' + ) + return None + if obj == 'cap': + if state.held != 'cap': + return _replan(f'{node.id!r}: hatch is not held') + if target != 'hatch_seat' and target != 'work': + return _replan( + f'{node.id!r}: hatch transfer must target hatch_seat or work' + ) + return None + if obj in facts.components and facts.components[obj] == 'screw': + if state.held != obj: + return _replan(f'{node.id!r}: {obj!r} is not held') + if facts.destinations.get(target) != 'hole' and target != 'work': + return _replan( + f'{node.id!r}: screw transfer must target a hole or work' + ) + return None + return _reject(f'{node.id!r}: object {obj!r} cannot be transferred') + + if skill == 'align': + if state.held is None and state.mounted is None: + return _replan(f'{node.id!r}: align requires a held part or tool') + return None + + if skill == 'dispense': + if state.mounted != 'sealant_tool': + return _replan( + f'{node.id!r}: dispense requires sealant_tool in patron' + ) + side = obj or target + if facts.destinations.get(side) != 'contour': + return _reject( + f'{node.id!r}: dispense target {side!r} is not a sealant side' + ) + return None + + if skill == 'dock_tool': + socket = facts.tools.get(obj) + if socket is None: + return _reject(f'{node.id!r}: {obj!r} is not a present tool') + if state.mounted != obj: + return _replan( + f'{node.id!r}: {obj!r} is not mounted ' + f'(mounted={state.mounted!r})' + ) + if target != socket: + return _replan( + f'{node.id!r}: dock_tool must target socket {socket!r}' + ) + if state.transfer_target != socket: + return _replan( + f'{node.id!r}: dock_tool requires an explicit preceding ' + 'transfer to that socket' + ) + return None + + if skill == 'handoff_screw': + if obj not in facts.components or facts.components[obj] != 'screw': + return _reject(f'{node.id!r}: {obj!r} is not a present screw') + if state.held != obj: + return _replan(f'{node.id!r}: {obj!r} is not held') + if state.mounted != 'magnetic_tool': + return _replan( + f'{node.id!r}: handoff_screw requires magnetic_tool' + ) + if facts.destinations.get(target) != 'hole': + return _replan( + f'{node.id!r}: handoff_screw target must be a hole' + ) + return None + + if skill in ('capture',): + if obj not in facts.cameras: + return _reject(f'{node.id!r}: unknown camera {obj!r}') + return None + + +def _apply_event( + node: PlanNode, + event: str, + state: _State, + facts: _Facts, +) -> _State: + skill = node.skill + obj = node.object + if skill == 'inspect_state' and event in ('empty', 'holding', 'placed', 'unknown'): + if event == 'empty': + return _State(state.mounted, None, False, state.transfer_target) + if event == 'holding': + return _State(state.mounted, obj or None, False, state.transfer_target) + if event == 'placed': + return _State(state.mounted, None, False, state.transfer_target) + return _State(state.mounted, state.held, True, state.transfer_target) + if event == 'grasp_failed': + return _State(state.mounted, state.held, True, state.transfer_target) + if event not in PROGRESS_EVENTS: + return state + if skill in ('grab', 'pick'): + if obj in facts.tools: + return _State(obj, state.held, False, state.transfer_target) + if obj == 'cap': + return _State(state.mounted, 'cap', False, state.transfer_target) + if obj and facts.components.get(obj) == 'screw': + return _State(state.mounted, obj, False, state.transfer_target) + return state + if skill == 'release': + if obj in facts.tools: + return _State(None, state.held, False, state.transfer_target) + if obj: + return _State(state.mounted, None, False, state.transfer_target) + return state + if skill == 'dock_tool': + return _State(None, state.held, False, state.transfer_target) + if skill == 'handoff_screw': + return _State(state.mounted, None, False, state.transfer_target) + if skill == 'transfer': + return _State( + state.mounted, state.held, False, node.target or state.transfer_target, + ) + return state + + +def _ownership_checks(graph: PlanGraph, facts: _Facts) -> ValidationResult | None: + initial = facts.initial + seen: set[tuple[str, _State]] = set() + queue: list[tuple[str, _State]] = [(graph.entry, initial)] + visited_states: dict[str, set[_State]] = {node_id: set() for node_id in graph.ids} + while queue: + node_id, state = queue.pop() + if (node_id, state) in seen: + continue + seen.add((node_id, state)) + node = graph.node(node_id) + failure = _precondition(node, state, facts) + if failure is not None: + failure.details.update({ + 'node_id': node.id, + 'skill': node.skill, + 'mounted_tool': state.mounted, + 'held_object': state.held, + }) + return failure + visited_states[node.id].add(state) + for transition in node.transitions: + if transition.next not in graph.ids: + continue + next_state = _apply_event(node, transition.event, state, facts) + if (transition.next, next_state) not in seen: + queue.append((transition.next, next_state)) + return None + + +def validate_graph( + graph: PlanGraph, + scene: Mapping[str, Any], + catalog: SkillCatalog, + *, + expected_scene_revision: int | None = None, +) -> ValidationResult: + """Return an admission decision; never mutates the graph or the scene.""" + if graph.status == 'blocked': + return ValidationResult( + False, 'blocked', 'the model reported status=blocked; no motion', + {'status': graph.status}, + ) + if graph.status != 'ready': + return _reject(f'unsupported graph status {graph.status!r}') + if len(graph.nodes) > 256: + return _reject('graph exceeds the 256-node limit') + facts = _facts(scene) + if expected_scene_revision is not None: + revision = facts.envelope.get('scene_revision') + if revision is not None and int(revision) != int(expected_scene_revision): + return _reject( + f'graph was built for scene_revision {expected_scene_revision}, ' + f'current revision is {revision}', + code='stale_scene', + ) + structural = _structural_checks(graph, facts) + if structural is not None: + return structural + ownership = _ownership_checks(graph, facts) + if ownership is not None: + return ownership + return ValidationResult( + True, + 'ok', + 'graph accepted', + { + 'nodes': len(graph.nodes), + 'entry': graph.entry, + 'scene_revision': facts.envelope.get('scene_revision'), + 'available_skills': len(facts.available_skills), + }, + ) diff --git a/src/iiwa_llm_plan_executor/package.xml b/src/iiwa_llm_plan_executor/package.xml new file mode 100644 index 0000000..746d5b3 --- /dev/null +++ b/src/iiwa_llm_plan_executor/package.xml @@ -0,0 +1,36 @@ + + + + iiwa_llm_plan_executor + 2026.11.10 + Validate and execute externally supplied Qwen3 plan graphs in Webots + daniel + Apache-2.0 + + ament_python + + ament_index_python + geometry_msgs + iiwa_experement + iiwa_msgs + iiwa_scene_context + launch + launch_ros + moveit_msgs + moveit_py + python3-numpy + python3-scipy + python3-yaml + rclpy + sensor_msgs + std_msgs + std_srvs + tf2_ros + webots_ros2_msgs + + python3-pytest + + + ament_python + + diff --git a/src/iiwa_llm_plan_executor/resource/iiwa_llm_plan_executor b/src/iiwa_llm_plan_executor/resource/iiwa_llm_plan_executor new file mode 100644 index 0000000..e4aa5c2 --- /dev/null +++ b/src/iiwa_llm_plan_executor/resource/iiwa_llm_plan_executor @@ -0,0 +1 @@ +iiwa_llm_plan_executor diff --git a/src/iiwa_llm_plan_executor/setup.cfg b/src/iiwa_llm_plan_executor/setup.cfg new file mode 100644 index 0000000..6899e20 --- /dev/null +++ b/src/iiwa_llm_plan_executor/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/iiwa_llm_plan_executor +[install] +install_scripts=$base/lib/iiwa_llm_plan_executor diff --git a/src/iiwa_llm_plan_executor/setup.py b/src/iiwa_llm_plan_executor/setup.py new file mode 100644 index 0000000..d0a2f42 --- /dev/null +++ b/src/iiwa_llm_plan_executor/setup.py @@ -0,0 +1,30 @@ +from glob import glob + +from setuptools import find_packages, setup + +package_name = 'iiwa_llm_plan_executor' + +setup( + name=package_name, + version='2026.11.10', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/config', glob('config/*.yaml')), + ('share/' + package_name + '/launch', glob('launch/*.py')), + ('share/' + package_name + '/doc', glob('doc/*.md') + glob('docs/*.md')), + ], + install_requires=['setuptools', 'PyYAML'], + tests_require=['pytest'], + zip_safe=True, + maintainer='daniel', + maintainer_email='grabardm@ml-dev.ru', + description='Validate and execute externally supplied Qwen3 plan graphs in Webots', + license='Apache-2.0', + entry_points={ + 'console_scripts': [ + 'llm_plan_executor = iiwa_llm_plan_executor.node:main', + ], + }, +) diff --git a/src/iiwa_llm_plan_executor/test/test_schema.py b/src/iiwa_llm_plan_executor/test/test_schema.py new file mode 100644 index 0000000..f404a3e --- /dev/null +++ b/src/iiwa_llm_plan_executor/test/test_schema.py @@ -0,0 +1,153 @@ +"""Parsing tests for the externally supplied plan response.""" +from __future__ import annotations + +import json + +import pytest + +from iiwa_llm_plan_executor.schema import ( + MAX_NODES, + PlanSchemaError, + parse_response, +) + + +def sample_graph(): + return { + 'status': 'ready', + 'entry': 'p1_find', + 'plan': [ + { + 'id': 'p1_find', + 'skill': 'detect', + 'object': 'cap', + 'target': '', + 'transitions': [ + {'event': 'found', 'next': 'p1_grab'}, + {'event': 'not_found', 'next': 'replan_required'}, + {'event': 'failed', 'next': 'replan_required'}, + ], + }, + { + 'id': 'p1_grab', + 'skill': 'grab', + 'object': 'vacuum', + 'target': '', + 'transitions': [ + {'event': 'success', 'next': 'completed'}, + {'event': 'grasp_failed', 'next': 'operator_required'}, + {'event': 'human_in_scene', 'next': 'operator_required'}, + {'event': 'collision', 'next': 'operator_required'}, + {'event': 'failed', 'next': 'replan_required'}, + ], + }, + ], + } + + +def test_shown_status_entry_plan_transitions_format_is_parsed(): + graph = parse_response(json.dumps(sample_graph())) + assert graph.status == 'ready' + assert graph.entry == 'p1_find' + assert graph.ids == ('p1_find', 'p1_grab') + assert graph.node('p1_find').transitions[0].event == 'found' + assert graph.node('p1_grab').object == 'vacuum' + + +def test_top_level_transitions_are_merged_into_nodes(): + payload = sample_graph() + payload['plan'][0].pop('transitions') + payload['transitions'] = [ + {'from': 'p1_find', 'event': 'found', 'to': 'p1_grab'}, + {'from': 'p1_find', 'event': 'not_found', 'to': 'replan_required'}, + {'from': 'p1_find', 'event': 'failed', 'to': 'replan_required'}, + ] + graph = parse_response(payload) + events = {item.event for item in graph.node('p1_find').transitions} + assert events == {'found', 'not_found', 'failed'} + + +def test_blocked_response_has_no_plan(): + graph = parse_response({'status': 'blocked', 'entry': '', 'plan': []}) + assert graph.status == 'blocked' + assert graph.nodes == () + + +def test_blocked_response_with_nodes_is_rejected(): + with pytest.raises(PlanSchemaError): + parse_response({ + 'status': 'blocked', + 'entry': 'a', + 'plan': sample_graph()['plan'], + }) + + +@pytest.mark.parametrize('payload', [ + {'status': 'ready', 'entry': 'a', 'plan': []}, + {'status': 'ready', 'entry': '', 'plan': sample_graph()['plan']}, + {'status': 'unknown', 'entry': 'a', 'plan': sample_graph()['plan']}, + {'status': 'ready', 'entry': 'a', 'plan': sample_graph()['plan'], + 'extra': 1}, +]) +def test_invalid_top_level_shapes_are_rejected(payload): + with pytest.raises(PlanSchemaError): + parse_response(payload) + + +def test_duplicate_ids_are_rejected(): + payload = sample_graph() + payload['plan'][1]['id'] = 'p1_find' + with pytest.raises(PlanSchemaError, match='duplicate node id'): + parse_response(payload) + + +def test_unknown_node_keys_and_transition_events_are_rejected(): + payload = sample_graph() + payload['plan'][0]['pose'] = {'x': 1} + with pytest.raises(PlanSchemaError, match='unknown keys'): + parse_response(payload) + + payload = sample_graph() + payload['plan'][0]['transitions'].append( + {'event': 'found', 'next': 'p1_grab'}, + ) + with pytest.raises(PlanSchemaError, match='duplicate event'): + parse_response(payload) + + +def test_reserved_terminal_ids_and_oversized_plans_are_rejected(): + payload = sample_graph() + payload['plan'][0]['id'] = 'completed' + with pytest.raises(PlanSchemaError, match='reserved terminal'): + parse_response(payload) + + nodes = [] + for index in range(MAX_NODES + 1): + nodes.append({ + 'id': f'n{index}', + 'skill': 'stop', + 'object': 'robot', + 'target': '', + 'transitions': [ + {'event': 'success', 'next': 'completed'}, + {'event': 'failed', 'next': 'replan_required'}, + ], + }) + with pytest.raises(PlanSchemaError, match='limit'): + parse_response({'status': 'ready', 'entry': 'n0', 'plan': nodes}) + + +def test_params_are_strict_scalars(): + payload = sample_graph() + payload['plan'][0]['params'] = {'start_fraction': 0.2, 'end_fraction': 1.0} + graph = parse_response(payload) + assert graph.node('p1_find').params['start_fraction'] == 0.2 + + payload['plan'][0]['params'] = {'bad': {'nested': 1}} + with pytest.raises(PlanSchemaError): + parse_response(payload) + + +def test_invalid_json_is_reported(): + with pytest.raises(PlanSchemaError, match='not valid JSON'): + parse_response('{not json') diff --git a/src/iiwa_llm_plan_executor/test/test_validator.py b/src/iiwa_llm_plan_executor/test/test_validator.py new file mode 100644 index 0000000..30f3287 --- /dev/null +++ b/src/iiwa_llm_plan_executor/test/test_validator.py @@ -0,0 +1,412 @@ +"""Graph admission: structure, safe branches, ownership and scene revision.""" +from __future__ import annotations + +from pathlib import Path + +import pytest + +from iiwa_experement.scenario import load_scenario +from iiwa_llm_plan_executor.schema import PlanSchemaError, parse_response +from iiwa_llm_plan_executor.validator import validate_graph +from iiwa_scene_context.catalog import DEFAULT_CATALOG +from iiwa_scene_context.snapshot import build_envelope, build_scene + + +PACKAGE = Path(__file__).resolve().parents[1] +SCENES = PACKAGE.parent / 'iiwa_experement' / 'config' / 'scenes.yaml' +SAFE_DEFAULTS = { + 'collision': 'operator_required', + 'grasp_failed': 'operator_required', + 'human_in_scene': 'operator_required', + 'not_found': 'replan_required', + 'failed': 'replan_required', + 'not_ok': 'operator_required', + 'timeout': 'operator_required', + 'unknown': 'operator_required', +} +PROGRESS = {'success', 'ok', 'found', 'clear', 'empty', 'holding', 'placed'} + + +def spec_for(experiment_id: int = 1): + return load_scenario(experiment_id, SCENES) + + +def scene_for(experiment_id: int = 1, *, mounted=None, held=None, + cap_state=None, revision=1): + spec = spec_for(experiment_id) + tools = {tool.name: {'state': 'docked'} for tool in spec.tools} + if mounted: + tools[mounted]['state'] = 'mounted' + scene = build_scene( + spec, + { + 'version': 1, + 'mounted_tool': mounted, + 'held_object': held, + 'ready': True, + 'cap': {'state': cap_state or spec.cap_state}, + 'tools': tools, + 'screws': {}, + 'holes': {}, + }, + DEFAULT_CATALOG, + ) + return build_envelope( + scene, + scene_revision=revision, + state_version=1, + stamp=10.0, + experiment_id=experiment_id, + ) + + +def node(node_id, skill, obj='', target='', *, next_on=None, params=None, + outcomes=None): + events = outcomes or DEFAULT_CATALOG.outcomes(skill) + next_on = next_on or {} + transitions = [] + for event in events: + if event in next_on: + destination = next_on[event] + elif event in PROGRESS: + destination = 'completed' + else: + destination = SAFE_DEFAULTS.get(event, 'operator_required') + transitions.append({'event': event, 'next': destination}) + entry = { + 'id': node_id, + 'skill': skill, + 'object': obj, + 'target': target, + 'transitions': transitions, + } + if params is not None: + entry['params'] = params + return entry + + +def graph(entry_id, *nodes): + return parse_response({ + 'status': 'ready', + 'entry': entry_id, + 'plan': list(nodes), + }) + + +def expect(graph_value, scene, *, expected_revision=None): + return validate_graph( + graph_value, + scene, + DEFAULT_CATALOG, + expected_scene_revision=expected_revision, + ) + + +def test_blocked_status_never_reaches_execution(): + result = expect( + parse_response({'status': 'blocked', 'entry': '', 'plan': []}), + scene_for(), + ) + assert result.ok is False + assert result.code == 'blocked' + + +def test_cap_grab_without_explicit_vacuum_is_replan_required_before_motion(): + value = graph( + 'grab_cap', + node('grab_cap', 'grab', 'cap', next_on={'success': 'completed'}), + ) + result = expect(value, scene_for(1)) + assert result.ok is False + assert result.code == 'replan_required' + assert 'vacuum' in result.reason + + +def test_take_vacuum_and_leave_it_in_patron_is_accepted(): + value = graph( + 'grab_vacuum', + node('grab_vacuum', 'grab', 'vacuum', + next_on={'success': 'transfer'}), + node('transfer', 'transfer', 'vacuum', 'work', + next_on={'success': 'completed'}), + ) + result = expect(value, scene_for(1)) + assert result.ok is True + + +def test_release_from_work_without_a_socket_is_rejected(): + value = graph( + 'grab_vacuum', + node('grab_vacuum', 'grab', 'vacuum', + next_on={'success': 'transfer'}), + node('transfer', 'transfer', 'vacuum', 'work', + next_on={'success': 'release'}), + node('release', 'release', 'vacuum', '', + next_on={'success': 'completed'}), + ) + result = expect(value, scene_for(1)) + assert result.ok is False + assert result.code == 'replan_required' + + +def test_release_at_socket_requires_a_preceding_explicit_transfer(): + value = graph( + 'grab_vacuum', + node('grab_vacuum', 'grab', 'vacuum', + next_on={'success': 'transfer'}), + node('transfer', 'transfer', 'vacuum', 'work', + next_on={'success': 'release'}), + node('release', 'release', 'vacuum', 'vacuum_socket', + next_on={'success': 'completed'}), + ) + result = expect(value, scene_for(1)) + assert result.code == 'replan_required' + assert 'transfer' in result.reason + + +def test_explicit_transfer_then_dock_or_release_returns_the_tool(): + dock = graph( + 'grab_vacuum', + node('grab_vacuum', 'grab', 'vacuum', + next_on={'success': 'transfer'}), + node('transfer', 'transfer', 'vacuum', 'vacuum_socket', + next_on={'success': 'dock'}), + node('dock', 'dock_tool', 'vacuum', 'vacuum_socket', + next_on={'success': 'completed'}), + ) + assert expect(dock, scene_for(1)).ok is True + + release = graph( + 'grab_vacuum', + node('grab_vacuum', 'grab', 'vacuum', + next_on={'success': 'transfer'}), + node('transfer', 'transfer', 'vacuum', 'vacuum_socket', + next_on={'success': 'release'}), + node('release', 'release', 'vacuum', 'vacuum_socket', + next_on={'success': 'completed'}), + ) + assert expect(release, scene_for(1)).ok is True + + +def test_dock_tool_without_the_explicit_transfer_is_rejected(): + value = graph( + 'grab_vacuum', + node('grab_vacuum', 'grab', 'vacuum', + next_on={'success': 'dock'}), + node('dock', 'dock_tool', 'vacuum', 'vacuum_socket', + next_on={'success': 'completed'}), + ) + assert expect(value, scene_for(1)).code == 'replan_required' + + +def test_collision_must_not_continue_the_operation(): + value = graph( + 'approach', + node('approach', 'approach', 'cap', + next_on={'success': 'completed', 'collision': 'completed'}), + ) + result = expect(value, scene_for(1)) + assert result.ok is False + assert 'collision' in result.reason + + +def test_grasp_failed_cannot_map_to_success(): + value = graph( + 'grab_vacuum', + node('grab_vacuum', 'grab', 'vacuum', + next_on={'success': 'completed', 'grasp_failed': 'completed'}), + ) + result = expect(value, scene_for(1)) + assert result.ok is False + assert 'grasp_failed' in result.reason + + +def test_structural_errors_are_rejected(): + scene = scene_for(1) + unknown_skill = { + 'status': 'ready', 'entry': 'a', + 'plan': [{ + 'id': 'a', 'skill': 'approval', 'object': 'cap', 'target': '', + 'transitions': [ + {'event': 'success', 'next': 'completed'}, + {'event': 'failed', 'next': 'replan_required'}, + ], + }], + } + assert expect(parse_response(unknown_skill), scene).ok is False + assert expect( + graph('a', node('a', 'detect', 'ghost')), scene, + ).ok is False + assert expect( + graph('a', node('a', 'detect', 'cap', 'ghost')), scene, + ).ok is False + assert expect( + graph('missing', node('a', 'detect', 'cap')), scene, + ).ok is False + unreachable = graph( + 'a', + node('a', 'detect', 'cap'), + node('b', 'detect', 'cap'), + ) + assert expect(unreachable, scene).ok is False + no_events = { + 'status': 'ready', 'entry': 'a', + 'plan': [{ + 'id': 'a', 'skill': 'detect', 'object': 'cap', 'target': '', + 'transitions': [], + }], + } + assert expect(parse_response(no_events), scene).ok is False + missing_event = { + 'status': 'ready', 'entry': 'a', + 'plan': [{ + 'id': 'a', 'skill': 'detect', 'object': 'cap', 'target': '', + 'transitions': [{'event': 'found', 'next': 'completed'}], + }], + } + assert expect(parse_response(missing_event), scene).ok is False + bad_next = { + 'status': 'ready', 'entry': 'a', + 'plan': [node('a', 'detect', 'cap', + next_on={'found': 'ghost'})], + } + assert expect(parse_response(bad_next), scene).ok is False + + +def test_unknown_tighten_is_rejected_before_motion(): + payload = { + 'status': 'ready', 'entry': 'a', + 'plan': [{ + 'id': 'a', 'skill': 'tighten', 'object': 'hole_1', 'target': '', + 'transitions': [ + {'event': 'success', 'next': 'completed'}, + {'event': 'failed', 'next': 'replan_required'}, + ], + }], + } + result = expect(parse_response(payload), scene_for(1)) + assert result.ok is False + assert 'unavailable skill' in result.reason + + +def test_cycles_are_bounded_and_must_keep_a_recovery_branch(): + dead_cycle = graph( + 'a', + node('a', 'wait_scene_clear', 'workspace', + next_on={'clear': 'b', 'timeout': 'b', 'failed': 'b'}), + node('b', 'wait_scene_clear', 'workspace', + next_on={'clear': 'a', 'timeout': 'a', 'failed': 'a'}), + ) + assert expect(dead_cycle, scene_for(1)).ok is False + + recovering_cycle = graph( + 'a', + node('a', 'wait_scene_clear', 'workspace', + next_on={'clear': 'b'}), + node('b', 'wait_scene_clear', 'workspace', + next_on={'clear': 'a'}), + ) + assert expect(recovering_cycle, scene_for(1)).ok is True + + +def test_wrong_scene_revision_is_rejected(): + value = graph('a', node('a', 'detect', 'cap')) + result = expect(value, scene_for(1, revision=3), expected_revision=2) + assert result.ok is False + assert result.code == 'stale_scene' + + +def test_aliases_for_pick_move_to_storage_and_inspect_are_supported(): + value = graph( + 'grab_vacuum', + node('grab_vacuum', 'grab', 'vacuum', + next_on={'success': 'pick_cap'}), + node('pick_cap', 'pick', 'cap', next_on={'success': 'transfer'}), + node('transfer', 'transfer', 'cap', 'hatch_seat', + next_on={'success': 'release'}), + node('release', 'release', 'cap', 'hatch_seat', + next_on={'success': 'inspect'}), + node('inspect', 'inspect', 'cap', 'hatch_seat', + next_on={'ok': 'completed'}), + ) + assert expect(value, scene_for(1)).ok is True + + bad = graph('move', node( + 'move', 'move_to_storage', 'body', 'work', + next_on={'success': 'completed'}, + )) + assert expect(bad, scene_for(1)).ok is False + + +def test_screw_handoff_is_to_the_operator_and_leaves_the_hole_empty(): + value = graph( + 'grab_magnet', + node('grab_magnet', 'grab', 'magnetic_tool', + next_on={'success': 'pick_screw'}), + node('pick_screw', 'pick', 'screw_1', + next_on={'success': 'transfer'}), + node('transfer', 'transfer', 'screw_1', 'hole_1', + next_on={'success': 'handoff'}), + node('handoff', 'handoff_screw', 'screw_1', 'hole_1', + next_on={'success': 'completed'}), + ) + result = expect(value, scene_for(20)) + assert result.ok is True + + +def test_dispense_requires_the_sealant_tool(): + without = graph( + 'dispense', node('dispense', 'dispense', 'side_1', + next_on={'success': 'completed'}), + ) + assert expect(without, scene_for(20)).code == 'replan_required' + with_tool = graph( + 'grab_sealant', + node('grab_sealant', 'grab', 'sealant_tool', + next_on={'success': 'dispense'}), + node('dispense', 'dispense', 'side_1', + next_on={'success': 'completed'}), + ) + assert expect(with_tool, scene_for(20)).ok is True + + +def test_params_limit_dispense_fractions(): + value = graph( + 'grab_sealant', + node('grab_sealant', 'grab', 'sealant_tool', + next_on={'success': 'dispense'}), + node('dispense', 'dispense', 'side_1', + next_on={'success': 'completed'}, + params={'start_fraction': 0.0, 'end_fraction': 1.0}), + ) + assert expect(value, scene_for(20)).ok is True + + payload = { + 'status': 'ready', 'entry': 'dispense', + 'plan': [node('dispense', 'dispense', 'side_1', + next_on={'success': 'completed'}, + params={'unknown': 1.0})], + } + result = expect(parse_response(payload), scene_for(20)) + assert result.ok is False + assert 'params' in result.reason + + +def test_unconfirmed_grasp_requires_inspect_state(): + value = graph( + 'grab_vacuum', + node('grab_vacuum', 'grab', 'vacuum', + next_on={'success': 'transfer', 'grasp_failed': 'stop'}), + node('stop', 'stop', 'robot', + next_on={'success': 'transfer'}), + node('transfer', 'transfer', 'vacuum', 'work', + next_on={'success': 'completed'}), + ) + result = expect(value, scene_for(1)) + assert result.ok is False + assert 'inspect_state' in result.reason + + +def test_parse_errors_are_raised_not_smuggled_into_a_graph(): + with pytest.raises(PlanSchemaError): + parse_response('[]')