fix(llm-plan-executor): use the known empty outcome after a missing-tool grasp
This commit is contained in:
@@ -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(
|
||||
|
||||
Reference in New Issue
Block a user