fix(llm-plan-executor): use the known empty outcome after a missing-tool grasp

This commit is contained in:
Даниил Грабарь
2026-11-11 11:52:21 +10:00
parent 48c190d75b
commit 454a2014f3
7 changed files with 832 additions and 17 deletions
+50 -1
View File
@@ -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.
@@ -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
@@ -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', {
@@ -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)
@@ -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"
}
]
}
]
}
@@ -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
@@ -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(