feat(llm-plan-executor): parse and validate externally supplied graphs
This commit is contained in:
Даниил Грабарь
@@ -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.
|
||||
@@ -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',
|
||||
]
|
||||
@@ -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)
|
||||
@@ -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),
|
||||
},
|
||||
)
|
||||
@@ -0,0 +1,36 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>iiwa_llm_plan_executor</name>
|
||||
<version>2026.11.10</version>
|
||||
<description>Validate and execute externally supplied Qwen3 plan graphs in Webots</description>
|
||||
<maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<exec_depend>ament_index_python</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>iiwa_experement</exec_depend>
|
||||
<exec_depend>iiwa_msgs</exec_depend>
|
||||
<exec_depend>iiwa_scene_context</exec_depend>
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
<exec_depend>moveit_msgs</exec_depend>
|
||||
<exec_depend>moveit_py</exec_depend>
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
<exec_depend>python3-scipy</exec_depend>
|
||||
<exec_depend>python3-yaml</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>std_srvs</exec_depend>
|
||||
<exec_depend>tf2_ros</exec_depend>
|
||||
<exec_depend>webots_ros2_msgs</exec_depend>
|
||||
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1 @@
|
||||
iiwa_llm_plan_executor
|
||||
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/iiwa_llm_plan_executor
|
||||
[install]
|
||||
install_scripts=$base/lib/iiwa_llm_plan_executor
|
||||
@@ -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',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -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')
|
||||
@@ -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('[]')
|
||||
Reference in New Issue
Block a user