From 454a2014f33785b039b6ffd1628167346e36c518 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 11:52:21 +1000 Subject: [PATCH] fix(llm-plan-executor): use the known empty outcome after a missing-tool grasp --- src/iiwa_llm_plan_executor/doc/webots-qa.md | 51 ++- src/iiwa_llm_plan_executor/docs/interfaces.md | 7 + .../iiwa_llm_plan_executor/skills.py | 10 +- .../iiwa_llm_plan_executor/validator.py | 132 +++++- .../test/data/qwen_cap_full_graph.json | 430 ++++++++++++++++++ .../test/test_full_qwen_graph.py | 212 +++++++++ .../test/test_skills.py | 7 +- 7 files changed, 832 insertions(+), 17 deletions(-) create mode 100644 src/iiwa_llm_plan_executor/test/data/qwen_cap_full_graph.json create mode 100644 src/iiwa_llm_plan_executor/test/test_full_qwen_graph.py diff --git a/src/iiwa_llm_plan_executor/doc/webots-qa.md b/src/iiwa_llm_plan_executor/doc/webots-qa.md index dcac4d8..e9cef77 100644 --- a/src/iiwa_llm_plan_executor/doc/webots-qa.md +++ b/src/iiwa_llm_plan_executor/doc/webots-qa.md @@ -17,7 +17,7 @@ colcon test --base-paths src --packages-select \ colcon test-result --verbose ``` -Result: build of 10 packages succeeded; `359 tests, 0 errors, 0 failures, +Result: build of 10 packages succeeded; `361 tests, 0 errors, 0 failures, 0 skipped`. `git diff --check` is clean. @@ -117,3 +117,52 @@ ros2 service call /iiwa_scene_context/get_scene std_srvs/srv/Trigger {} ros2 topic echo /iiwa_llm_plan_executor/plan --qos-durability transient_local ros2 topic echo /iiwa_llm_plan_executor/status ``` + +## Full Qwen cap graph (`qwen_cap_full_graph.json`) + +The complete graph from the external Qwen run was added as a test fixture and +checked in scene 20. + +### Empty patron + +The integration test uses the real `GraphEngine`, real `SkillRegistry` and the +full 20-node graph, with fake motion/supervisor adapters that forbid supervisor +commands. The executed node sequence is: + +``` +a_find_part → a_find_target → a_approach → a_pick(grasp_failed) +→ a_stop_recover → a_state(empty) → a_retry_find_part → a_retry_approach +→ a_retry_pick(grasp_failed) → stop_operator → notify → operator_required +``` + +The halt reasons are `required tool vacuum is not mounted`; no supervisor +command is issued, no `lower`/`contact` phase is planned, the hatch stays +`free`, and `mounted_tool`/`held_object` stay `null`. + +In Webots the full graph reached the requested actual branch: + +``` +a_pick(grasp_failed) → a_stop_recover → a_state(empty) +→ a_retry_find_part → a_retry_approach → a_retry_pick(grasp_failed) +→ stop_operator → notify → operator_required +``` + +The halt reasons were `required tool vacuum is not mounted`; the supervisor +stayed at version 0, `mounted_tool`/`held_object` were `null`, the hatch stayed +`free`, and no `grab_cap`/`attach_tool`/`place_cap` command appeared in the log. + +### Vacuum mounted + +With `vacuum` mounted, the graph's nominal success route is: + +``` +a_inspect(ok) → a_state(placed) → a_inspect(ok) → ... +``` + +It is a cycle and the graph contains no `completed` transition at all, so a +nominal successful run cannot finish. The full graph also has a conservative +recovery issue: `grasp_failed` leads through `inspect_state: holding` to +`a_retry_pick`, which would pick the hatch while already holding it. The +validator therefore rejects that recovery branch with its own ownership reason +(`a_retry_pick ... while holding 'cap'`); this does not repair the model graph. +Completion must not be claimed for this graph. diff --git a/src/iiwa_llm_plan_executor/docs/interfaces.md b/src/iiwa_llm_plan_executor/docs/interfaces.md index af785ed..2fa11a1 100644 --- a/src/iiwa_llm_plan_executor/docs/interfaces.md +++ b/src/iiwa_llm_plan_executor/docs/interfaces.md @@ -83,6 +83,13 @@ Important pre-movement rules: * The same rule applies to screws without `magnetic_tool` (`required tool magnetic_tool is not mounted`) and to other tool-dependent skills, which return their contract's failure event. +* After a `grasp_failed` caused specifically by the missing tool, no contact + and no object change occurred. For the **same** `scene_revision` the + following `inspect_state` for that part may therefore only produce `empty`; + `holding`/`placed` are not simulated. Unknown ownership (a generic grasp + failure) keeps the conservative check of all `inspect_state` outcomes. + A known empty hatch that is already installed makes `placed` possible after + a release, while a free hatch makes `empty` possible. * The validator still simulates the reachable ownership branches. It skips a nominal success edge only when that success is impossible from the current tool/held state; a recovery branch that then assumes the part is held is diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py index ca63d2c..671e57c 100644 --- a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py +++ b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py @@ -362,7 +362,8 @@ class SkillRegistry: context.mark_lifted() work = context.geometry.safe_poses['work'] if not (context.at_work and context.phase == 'free'): - self._move(context, work, 'transfer') + phase = 'transfer_lin' if context.phase != 'free' else 'transfer' + self._move(context, work, phase) context.reset_phase() context.at_work = True if context.capture_frame('top') is None: @@ -406,7 +407,7 @@ class SkillRegistry: ) return state != 'delivered' try: - context.catalog.destination_kind(effective, context.spec) + self.catalog.destination_kind(effective, context.spec) return True except Exception: return False @@ -850,6 +851,11 @@ class SkillRegistry: del node try: self.motion.stop('graph stop node') + # A stop node is a graph decision, not a terminal safety latch: + # a verified inspect_state may deliberately retry afterwards. + reset = getattr(self.motion, 'reset_stop', None) + if reset is not None: + reset() except Exception as error: return SkillOutcome('failed', {'reason': str(error)}) return SkillOutcome('success', { 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 index be008fc..4c76677 100644 --- a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/validator.py +++ b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/validator.py @@ -49,6 +49,8 @@ class _State: held: str | None = None unknown: bool = False transfer_target: str | None = None + empty_failure: bool = False + cap_installed: bool = False @dataclass(frozen=True) @@ -61,6 +63,7 @@ class _Facts: initial: _State available_skills: set[str] envelope: Mapping[str, Any] + cap_installed: bool def _scene_payload(scene: Mapping[str, Any]) -> tuple[Mapping[str, Any], Mapping[str, Any]]: @@ -91,10 +94,19 @@ def _facts(scene: Mapping[str, Any]) -> _Facts: } cameras = set(payload.get('cameras', {})) service_objects = set(payload.get('service_objects', {})) + cap_entry = next( + ( + entry for entry in payload.get('components', []) + if isinstance(entry, Mapping) and entry.get('id') == 'cap' + ), + {}, + ) + cap_installed = str(cap_entry.get('state', '')) == 'installed' 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, + cap_installed=cap_installed, ) available = set(payload.get('available_skills', [])) return _Facts( @@ -106,6 +118,7 @@ def _facts(scene: Mapping[str, Any]) -> _Facts: initial=initial, available_skills=available, envelope=envelope, + cap_installed=cap_installed, ) @@ -496,6 +509,45 @@ def _success_feasible( return True +def _missing_tool_for_grasp( + node: PlanNode, + state: _State, + facts: _Facts, +) -> bool: + """True when a grasp fails specifically because its tool is absent.""" + if node.skill not in ('grab', 'pick'): + return False + obj = node.object + if obj == 'cap': + return state.mounted != 'vacuum' + if obj and facts.components.get(obj) == 'screw': + return state.mounted != 'magnetic_tool' + return False + + +def _inspect_event_feasible( + node: PlanNode, + state: _State, + event: str, + facts: _Facts, +) -> bool: + if node.skill != 'inspect_state': + return True + if state.unknown: + return True + if state.empty_failure: + return event == 'empty' + if node.object != 'cap': + return True + if event == 'empty': + return state.held is None and not state.cap_installed + if event == 'holding': + return state.held == 'cap' + if event == 'placed': + return state.held is None and state.cap_installed + return True + + def _apply_event( node: PlanNode, event: str, @@ -506,37 +558,85 @@ def _apply_event( 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) + return _State( + state.mounted, None, False, state.transfer_target, False, False, + ) if event == 'holding': - return _State(state.mounted, obj or None, False, state.transfer_target) + return _State( + state.mounted, obj or None, False, state.transfer_target, False, + False, + ) if event == 'placed': - return _State(state.mounted, None, False, state.transfer_target) - return _State(state.mounted, state.held, True, state.transfer_target) + return _State( + state.mounted, None, False, state.transfer_target, False, + state.cap_installed or obj == 'cap', + ) + return _State( + state.mounted, state.held, True, state.transfer_target, False, + state.cap_installed, + ) if event == 'grasp_failed': - return _State(state.mounted, state.held, True, state.transfer_target) + missing_tool = _missing_tool_for_grasp(node, state, facts) + return _State( + state.mounted, + state.held, + not missing_tool, + state.transfer_target, + missing_tool, + state.cap_installed, + ) 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) + return _State( + obj, state.held, False, state.transfer_target, False, + state.cap_installed, + ) if obj == 'cap': - return _State(state.mounted, 'cap', False, state.transfer_target) + return _State( + state.mounted, 'cap', False, state.transfer_target, False, False, + ) if obj and facts.components.get(obj) == 'screw': - return _State(state.mounted, obj, False, state.transfer_target) + return _State( + state.mounted, obj, False, state.transfer_target, False, + state.cap_installed, + ) return state if skill == 'release': if obj in facts.tools: - return _State(None, state.held, False, state.transfer_target) + return _State( + None, state.held, False, state.transfer_target, False, + state.cap_installed, + ) + if obj == 'cap': + return _State( + state.mounted, None, False, state.transfer_target, False, True, + ) if obj: - return _State(state.mounted, None, False, state.transfer_target) + return _State( + state.mounted, None, False, state.transfer_target, False, + state.cap_installed, + ) return state if skill == 'dock_tool': - return _State(None, state.held, False, state.transfer_target) + return _State( + None, state.held, False, state.transfer_target, False, + state.cap_installed, + ) if skill == 'handoff_screw': - return _State(state.mounted, None, False, state.transfer_target) + return _State( + state.mounted, None, False, state.transfer_target, False, + state.cap_installed, + ) if skill == 'transfer': return _State( - state.mounted, state.held, False, node.target or state.transfer_target, + state.mounted, + state.held, + False, + node.target or state.transfer_target, + False, + state.cap_installed, ) return state @@ -565,6 +665,12 @@ def _ownership_checks(graph: PlanGraph, facts: _Facts) -> ValidationResult | Non for transition in node.transitions: if transition.next not in graph.ids: continue + if ( + not _inspect_event_feasible(node, state, transition.event, facts) + ): + # Known empty state: only "empty" is physically possible. + # Unknown state remains conservative and allows every branch. + continue if ( transition.event in PROGRESS_EVENTS and not _success_feasible(node, state, facts) diff --git a/src/iiwa_llm_plan_executor/test/data/qwen_cap_full_graph.json b/src/iiwa_llm_plan_executor/test/data/qwen_cap_full_graph.json new file mode 100644 index 0000000..7631c81 --- /dev/null +++ b/src/iiwa_llm_plan_executor/test/data/qwen_cap_full_graph.json @@ -0,0 +1,430 @@ +{ + "status": "ready", + "entry": "a_find_part", + "plan": [ + { + "id": "a_find_part", + "skill": "detect", + "object": "cap", + "target": "", + "transitions": [ + { + "event": "found", + "next": "a_find_target" + }, + { + "event": "not_found", + "next": "replan_required" + }, + { + "event": "failed", + "next": "replan_required" + } + ] + }, + { + "id": "a_find_target", + "skill": "detect", + "object": "hatch_seat", + "target": "", + "transitions": [ + { + "event": "found", + "next": "a_approach" + }, + { + "event": "not_found", + "next": "replan_required" + }, + { + "event": "failed", + "next": "replan_required" + } + ] + }, + { + "id": "a_approach", + "skill": "approach", + "object": "cap", + "target": "", + "transitions": [ + { + "event": "success", + "next": "a_pick" + }, + { + "event": "human_in_scene", + "next": "a_stop_human" + }, + { + "event": "collision", + "next": "stop_collision" + }, + { + "event": "failed", + "next": "stop_operator" + } + ] + }, + { + "id": "a_pick", + "skill": "pick", + "object": "cap", + "target": "", + "transitions": [ + { + "event": "success", + "next": "a_transfer" + }, + { + "event": "grasp_failed", + "next": "a_stop_recover" + }, + { + "event": "human_in_scene", + "next": "a_stop_human" + }, + { + "event": "collision", + "next": "stop_collision" + }, + { + "event": "failed", + "next": "stop_operator" + } + ] + }, + { + "id": "a_transfer", + "skill": "move_to_storage", + "object": "cap", + "target": "hatch_seat", + "transitions": [ + { + "event": "success", + "next": "a_release" + }, + { + "event": "human_in_scene", + "next": "a_stop_human" + }, + { + "event": "collision", + "next": "stop_collision" + }, + { + "event": "failed", + "next": "stop_operator" + } + ] + }, + { + "id": "a_release", + "skill": "release", + "object": "cap", + "target": "hatch_seat", + "transitions": [ + { + "event": "success", + "next": "a_inspect" + }, + { + "event": "human_in_scene", + "next": "a_stop_human" + }, + { + "event": "collision", + "next": "stop_collision" + }, + { + "event": "failed", + "next": "stop_operator" + } + ] + }, + { + "id": "a_inspect", + "skill": "inspect", + "object": "cap", + "target": "hatch_seat", + "transitions": [ + { + "event": "ok", + "next": "a_state" + }, + { + "event": "not_ok", + "next": "stop_operator" + }, + { + "event": "failed", + "next": "stop_operator" + } + ] + }, + { + "id": "a_stop_human", + "skill": "stop", + "object": "robot", + "target": "", + "transitions": [ + { + "event": "success", + "next": "a_wait_clear" + }, + { + "event": "failed", + "next": "notify" + } + ] + }, + { + "id": "a_wait_clear", + "skill": "wait_scene_clear", + "object": "workspace", + "target": "", + "transitions": [ + { + "event": "clear", + "next": "a_state" + }, + { + "event": "timeout", + "next": "notify" + }, + { + "event": "failed", + "next": "notify" + } + ] + }, + { + "id": "a_stop_recover", + "skill": "stop", + "object": "robot", + "target": "", + "transitions": [ + { + "event": "success", + "next": "a_state" + }, + { + "event": "failed", + "next": "notify" + } + ] + }, + { + "id": "a_state", + "skill": "inspect_state", + "object": "cap", + "target": "hatch_seat", + "transitions": [ + { + "event": "empty", + "next": "a_retry_find_part" + }, + { + "event": "holding", + "next": "a_retry_find_part" + }, + { + "event": "placed", + "next": "a_inspect" + }, + { + "event": "unknown", + "next": "notify" + } + ] + }, + { + "id": "a_retry_find_part", + "skill": "detect", + "object": "cap", + "target": "", + "transitions": [ + { + "event": "found", + "next": "a_retry_approach" + }, + { + "event": "not_found", + "next": "replan_required" + }, + { + "event": "failed", + "next": "replan_required" + } + ] + }, + { + "id": "a_retry_approach", + "skill": "approach", + "object": "cap", + "target": "", + "transitions": [ + { + "event": "success", + "next": "a_retry_pick" + }, + { + "event": "human_in_scene", + "next": "stop_operator" + }, + { + "event": "collision", + "next": "stop_collision" + }, + { + "event": "failed", + "next": "stop_operator" + } + ] + }, + { + "id": "a_retry_pick", + "skill": "pick", + "object": "cap", + "target": "", + "transitions": [ + { + "event": "success", + "next": "a_retry_find_target" + }, + { + "event": "grasp_failed", + "next": "stop_operator" + }, + { + "event": "human_in_scene", + "next": "stop_operator" + }, + { + "event": "collision", + "next": "stop_collision" + }, + { + "event": "failed", + "next": "stop_operator" + } + ] + }, + { + "id": "a_retry_find_target", + "skill": "detect", + "object": "hatch_seat", + "target": "", + "transitions": [ + { + "event": "found", + "next": "a_retry_transfer" + }, + { + "event": "not_found", + "next": "replan_required" + }, + { + "event": "failed", + "next": "replan_required" + } + ] + }, + { + "id": "a_retry_transfer", + "skill": "move_to_storage", + "object": "cap", + "target": "hatch_seat", + "transitions": [ + { + "event": "success", + "next": "a_retry_release" + }, + { + "event": "human_in_scene", + "next": "stop_operator" + }, + { + "event": "collision", + "next": "stop_collision" + }, + { + "event": "failed", + "next": "stop_operator" + } + ] + }, + { + "id": "a_retry_release", + "skill": "release", + "object": "cap", + "target": "hatch_seat", + "transitions": [ + { + "event": "success", + "next": "a_inspect" + }, + { + "event": "human_in_scene", + "next": "stop_operator" + }, + { + "event": "collision", + "next": "stop_collision" + }, + { + "event": "failed", + "next": "stop_operator" + } + ] + }, + { + "id": "stop_collision", + "skill": "stop", + "object": "robot", + "target": "", + "transitions": [ + { + "event": "success", + "next": "notify" + }, + { + "event": "failed", + "next": "notify" + } + ] + }, + { + "id": "stop_operator", + "skill": "stop", + "object": "robot", + "target": "", + "transitions": [ + { + "event": "success", + "next": "notify" + }, + { + "event": "failed", + "next": "notify" + } + ] + }, + { + "id": "notify", + "skill": "notify_operator", + "object": "robot", + "target": "", + "transitions": [ + { + "event": "success", + "next": "operator_required" + }, + { + "event": "failed", + "next": "operator_required" + } + ] + } + ] +} diff --git a/src/iiwa_llm_plan_executor/test/test_full_qwen_graph.py b/src/iiwa_llm_plan_executor/test/test_full_qwen_graph.py new file mode 100644 index 0000000..5621f1d --- /dev/null +++ b/src/iiwa_llm_plan_executor/test/test_full_qwen_graph.py @@ -0,0 +1,212 @@ +"""Integration tests with the complete Qwen graph submitted by the user.""" +from __future__ import annotations + +import copy +import json +from pathlib import Path + +from iiwa_experement.geometry import load_geometry, world_target +from iiwa_experement.object_state import transform_json +from iiwa_experement.scenario import load_scenario +from iiwa_llm_plan_executor.engine import GraphEngine +from iiwa_llm_plan_executor.moveit_backend import MotionOutcome +from iiwa_llm_plan_executor.schema import parse_response +from iiwa_llm_plan_executor.skill_context import SkillContext +from iiwa_llm_plan_executor.skills import SkillRegistry +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] +ASSETS = PACKAGE.parent / 'iiwa_experement' / 'config' +SCENES = ASSETS / 'scenes.yaml' +GRAPH_PATH = Path(__file__).resolve().parent / 'data' / 'qwen_cap_full_graph.json' +GEOMETRY = load_geometry(ASSETS / 'geometry.yaml') +SPEC = load_scenario(20, SCENES) +FULL_GRAPH = parse_response(json.loads(GRAPH_PATH.read_text(encoding='utf-8'))) + + +class FakeMotion: + def __init__(self): + self.calls = [] + self.stop_calls = [] + + def plan_and_execute(self, goal, phase, held_object): + self.calls.append((goal, phase, held_object)) + return MotionOutcome(True, 'ok', 'ok') + + def stop(self, reason='stop'): + self.stop_calls.append(reason) + + +class FakeSupervisor: + def __init__(self): + self.commands = [] + self.state = { + 'version': 1, + 'mounted_tool': None, + 'held_object': None, + 'cap': { + 'state': 'free', + 'pose': transform_json(world_target(SPEC, GEOMETRY, 'cap')), + }, + 'tools': { + tool.name: {'state': 'docked'} + for tool in SPEC.tools + }, + 'screws': {}, + 'holes': {}, + 'objects': {}, + } + + def latest(self): + return copy.deepcopy(self.state) + + def command(self, *args, **kwargs): + self.commands.append((args, kwargs)) + raise AssertionError( + 'no supervisor command is allowed on the missing-tool path' + ) + + +def envelope(*, mounted: str | None = None): + tools = {tool.name: {'state': 'docked'} for tool in SPEC.tools} + if mounted is not None: + tools[mounted] = {'state': 'mounted'} + scene = build_scene( + SPEC, + { + 'version': 1, + 'mounted_tool': mounted, + 'held_object': None, + 'ready': True, + 'cap': {'state': 'free'}, + 'tools': tools, + 'screws': {}, + 'holes': {}, + }, + DEFAULT_CATALOG, + ) + return build_envelope( + scene, + scene_revision=1, + state_version=1, + stamp=5.0, + experiment_id=20, + ) + + +def make_engine(*, mounted: str | None = None): + scene_envelope = envelope(mounted=mounted) + motion = FakeMotion() + supervisor = FakeSupervisor() + registry = SkillRegistry(motion, supervisor) + + def context_factory(env, scene_json): + del scene_json + context = SkillContext( + spec=SPEC, + geometry=GEOMETRY, + scene_revision=int(env['scene_revision']), + ) + context.frame_provider = lambda name: 5.0 + context.notifier = lambda reason, details: True + return context + + engine = GraphEngine( + lambda: scene_envelope, + context_factory, + registry, + clock=lambda: 5.0, + threaded=False, + min_start_delay_sec=3.0, + ) + engine.update_readiness(scene_ready=True, ready_at=0.0, motion_ready=True) + return engine, motion, supervisor, scene_envelope + + +def test_full_qwen_graph_empty_patron_follows_actual_failure_path(): + engine, motion, supervisor, scene_envelope = make_engine(mounted=None) + result = validate_graph( + FULL_GRAPH, + scene_envelope, + DEFAULT_CATALOG, + expected_scene_revision=1, + ) + assert result.ok is True, result.reason + + accepted = engine.submit('full-empty', FULL_GRAPH.as_dict(), 1) + assert accepted.accepted is True + engine.run_accepted('full-empty') + status = engine.get_status('full-empty') + assert status.state == 'operator_required' + assert [event['node_id'] for event in status.events] == [ + 'a_find_part', + 'a_find_target', + 'a_approach', + 'a_pick', + 'a_stop_recover', + 'a_state', + 'a_retry_find_part', + 'a_retry_approach', + 'a_retry_pick', + 'stop_operator', + 'notify', + ] + pick_events = [ + event for event in status.events if event['node_id'] == 'a_pick' + ] + state_events = [ + event for event in status.events if event['node_id'] == 'a_state' + ] + retry_pick = [ + event for event in status.events if event['node_id'] == 'a_retry_pick' + ] + assert pick_events[0]['event'] == 'grasp_failed' + assert pick_events[0]['reason'] == 'required tool vacuum is not mounted' + assert pick_events[0]['next'] == 'a_stop_recover' + assert state_events[0]['event'] == 'empty' + assert state_events[0]['next'] == 'a_retry_find_part' + assert retry_pick[0]['event'] == 'grasp_failed' + assert retry_pick[0]['next'] == 'stop_operator' + assert status.mounted_tool is None + assert status.held_object is None + assert supervisor.state['cap']['state'] == 'free' + assert supervisor.state['held_object'] is None + assert supervisor.commands == [] + assert not any(phase in ('lower', 'contact') for _, phase, _ in motion.calls) + + +def test_full_qwen_graph_with_vacuum_has_a_success_cycle_without_completed(): + # The nominal successful branch is inspect ok -> state -> placed -> inspect. + a_inspect = FULL_GRAPH.node('a_inspect') + a_state = FULL_GRAPH.node('a_state') + assert { + transition.event: transition.next + for transition in a_inspect.transitions + }['ok'] == 'a_state' + assert { + transition.event: transition.next + for transition in a_state.transitions + }['placed'] == 'a_inspect' + + all_nexts = { + transition.next + for node in FULL_GRAPH.nodes + for transition in node.transitions + } + assert 'completed' not in all_nexts + + # With the vacuum mounted the graph is still not executable as a whole: + # the generic grasp_failed branch leads through inspect_state holding to a + # retry pick while the hatch is held, which the ownership check rejects. + result = validate_graph( + FULL_GRAPH, + envelope(mounted='vacuum'), + DEFAULT_CATALOG, + expected_scene_revision=1, + ) + assert result.ok is False + assert 'a_retry_pick' in result.reason + assert 'holding' in result.reason diff --git a/src/iiwa_llm_plan_executor/test/test_skills.py b/src/iiwa_llm_plan_executor/test/test_skills.py index b45f2b2..c61cbfc 100644 --- a/src/iiwa_llm_plan_executor/test/test_skills.py +++ b/src/iiwa_llm_plan_executor/test/test_skills.py @@ -25,6 +25,7 @@ class FakeMotion: def __init__(self, *, success=True): self.calls = [] self.stop_calls = [] + self.reset_calls = 0 self.success = success def plan_and_execute(self, goal, phase, held_object): @@ -37,6 +38,9 @@ class FakeMotion: def stop(self, reason='stop'): self.stop_calls.append(reason) + def reset_stop(self): + self.reset_calls += 1 + class FakeSupervisor: def __init__(self, spec, geometry): @@ -486,7 +490,7 @@ def test_detect_withdraws_axially_then_uses_fresh_frames(): result = registry.execute(node('d', 'detect', 'cap'), context) assert result.event == 'found' assert phases(motion)[0] == 'retreat' - assert phases(motion)[1] == 'transfer' + assert phases(motion)[1] == 'transfer_lin' assert context.phase == 'free' context.frame_provider = lambda name: None @@ -562,6 +566,7 @@ def test_stop_preserves_ownership_and_wait_scene_clear_reports_timeout(): context.held_object = 'cap' assert registry.execute(node('s', 'stop', 'robot'), context).event == 'success' assert motion.stop_calls == ['graph stop node'] + assert motion.reset_calls == 1 assert context.held_object == 'cap' context.scene_clear_provider = lambda: False assert registry.execute(