From 48c190d75b1a36f72d06e25f3f356ddbcd512b41 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:28:12 +1000 Subject: [PATCH] fix(llm-plan-executor): report missing tools as runtime grasp failures --- .../plans/2026-11-10-llm-graph-execution.md | 6 +- .../2026-11-10-llm-graph-execution-design.md | 2 +- src/iiwa_llm_plan_executor/doc/webots-qa.md | 4 +- src/iiwa_llm_plan_executor/docs/interfaces.md | 25 ++++- .../iiwa_llm_plan_executor/engine.py | 10 +- .../iiwa_llm_plan_executor/skills.py | 81 ++++++++++++++-- .../iiwa_llm_plan_executor/validator.py | 93 +++++++++++++------ .../test/test_engine.py | 39 +++++++- .../test/test_skills.py | 51 ++++++++-- .../test/test_validator.py | 59 +++++++++++- 10 files changed, 304 insertions(+), 66 deletions(-) diff --git a/docs/superpowers/plans/2026-11-10-llm-graph-execution.md b/docs/superpowers/plans/2026-11-10-llm-graph-execution.md index af4eb6b..e451175 100644 --- a/docs/superpowers/plans/2026-11-10-llm-graph-execution.md +++ b/docs/superpowers/plans/2026-11-10-llm-graph-execution.md @@ -16,7 +16,7 @@ - Создать ровно два дополнительных ROS 2 пакета: `src/iiwa_scene_context` и `src/iiwa_llm_plan_executor`. Изменения `iiwa_experement` и `iiwa_web` ограничить интеграцией. Не делать систему оценки Qwen3, её обучение или вызов модели: внешний клиент передаёт **готовый** граф. - Номер эксперимента выбирает исходную сцену, но не задаёт робототехническое задание. Исполнять только переданный граф. Не вставлять операции взятия инструмента, герметизации, установки люка или передачи винтов на основании номера эксперимента. - Внутри *одного* навыка допустимы обязательные геометрические фазы: безопасный подход сверху, осевое опускание, действие, осевой подъём к сохранённой высоте. Явные узлы этих фаз не должны вызывать повторное действие. Неизвестный объект, навык, инструмент, цель, положение или неоднозначная зависимость дают отказ до движения либо `replan_required`. -- `release(vacuum)` не перемещает инструмент к стойке. Чтобы вернуть присоску, граф должен явно содержать `transfer(vacuum,vacuum_socket)` и `release(vacuum,vacuum_socket)` либо `dock_tool(vacuum,vacuum_socket)` в достигнутом гнезде. `release(vacuum)` из `work` без безопасной цели отклоняется; присоска остаётся в `patron`. Для `grab(cap)` присоска в `patron` обязательна; если из исходного состояния она находится в стойке, граф должен явно предписать её взятие. Винт подносится к отверстию и передаётся виртуальному человеку: без вставки и затяжки. Навык `tighten` в профиле Grant первого этапа не поддерживается. +- `release(vacuum)` не перемещает инструмент к стойке. Чтобы вернуть присоску, граф должен явно содержать `transfer(vacuum,vacuum_socket)` и `release(vacuum,vacuum_socket)` либо `dock_tool(vacuum,vacuum_socket)` в достигнутом гнезде. `release(vacuum)` из `work` без безопасной цели отклоняется; присоска остаётся в `patron`. `grab(cap)` без присоски не отклоняется на входе: при исполнении проверяется живой supervisor, контакт не выполняется, возвращается `grasp_failed` с причиной `required tool vacuum is not mounted` и выбирается переход графа для этого события; автоматического взятия инструмента нет. Винт подносится к отверстию и передаётся виртуальному человеку: без вставки и затяжки. Навык `tighten` в профиле Grant первого этапа не поддерживается. - `detect`: перейти в `work`, дождаться свежего кадра верхней камеры, получить точную позу из supervisor. При настроенном близком осмотре получить также новый кадр камеры на роботе; RGB/D используется для контроля, а не для вычисления позы. - Рабочий кончик направлен вниз; при рабочих движениях наклон не более 10° и J7 в жёстком коридоре ±90° относительно `work` (сейчас 0 рад). Применять ограничения к *траектории*, не только к конечной позе. Если траектория недоступна, остановиться; не переходить на ветвь с разворотом J7 около −180°. Поворот кисти и переход в `work` только после осевого отвода от контакта. - Один активный граф на робота. Внешний клиент передаёт ответ Qwen3 через `/iiwa_llm_plan_executor/submit`; принятый граф можно прочитать из ROS 2 топика `/iiwa_llm_plan_executor/plan`, а события и итог — из отдельного `/iiwa_llm_plan_executor/status`. `status=blocked` не запускает движение. `replan_required` выдаёт новый снимок сцены, но не вызывает Qwen3. `completed` означает выполненный граф и проверенные постусловия, а не оценку соответствия произвольному текстовому запросу. @@ -99,7 +99,7 @@ - [x] Написать тесты: показанный пользователем формат `status/entry/plan/transitions` разбирается; `status=blocked` не допускается к исполнению; дубликаты ID, неизвестные навыки/объекты/цели/события, пропущенный `entry`, несуществующие `next`, недостижимые узлы, пустые обязательные переходы и превышение лимитов отвергаются. - [x] Написать тесты ветвей отказа: `collision` не ведёт к продолжению операции; `grasp_failed` не превращается в `success`; цикл с допустимым восстановлением имеет ограничение 256 узлов и отдельный предел выполненных шагов; неверный `scene_revision` отклоняется. -- [x] Написать тест на исходную присоску в стойке: граф `grab(cap)` без явного `grab(vacuum)` получает `replan_required` **до первого движения**. Граф `grab(vacuum) → transfer(vacuum,work) → completed` принимается и оставляет инструмент в `patron`; граф `grab(vacuum) → transfer(vacuum,work) → release(vacuum)` отклоняется. Граф с явным `transfer(vacuum,vacuum_socket) → dock_tool(vacuum,vacuum_socket)` допускает возврат. Не выводить задачу из `experiment_id`. +- [x] Написать тест на исходную присоску в стойке: граф `grab(cap)` без явного `grab(vacuum)` принимается, при исполнении даёт `grasp_failed` до контакта и идёт по ветке `grasp_failed`; граф `grab(vacuum) → transfer(vacuum,work) → completed` принимается и оставляет инструмент в `patron`; граф `grab(vacuum) → transfer(vacuum,work) → release(vacuum)` отклоняется. Граф с явным `transfer(vacuum,vacuum_socket) → dock_tool(vacuum,vacuum_socket)` допускает возврат. Не выводить задачу из `experiment_id`. - [x] Прочитать несовпадение имён в приложенных наборах. Ввести версию контракта и только явно проверенные алиасы `pick`, `move_to_storage`, `inspect`; `align` и Grant-расширения — самостоятельные записи каталога. Неизвестный `tighten` отклоняется до движения. Проверить тестами и зафиксировать. - [x] Запустить `pytest -q src/iiwa_llm_plan_executor/test/test_schema.py src/iiwa_llm_plan_executor/test/test_validator.py` сначала до, затем после реализации; сохранить контракт и отдельный коммит. @@ -121,7 +121,7 @@ **Интерфейсы:** `SkillContext` хранит текущий `scene_revision`, удерживаемый объект/инструмент, сохранённую безопасную позу подхода, фазу контакта и последнюю проверенную позу. `SkillRegistry.execute(node: PlanNode, context: SkillContext) -> SkillOutcome(event, details)`. Команды supervisor отправлять с `request_id` и подтверждать по следующему `object_state`. - [x] Написать тесты фаз на фиктивном backend: `approach` идёт над целью и сохраняет высоту; `grab` без предварительного подхода выполняет подход, осевое опускание, захват и подъём; явные `approach/lower/lift` не дублируют движение. После `release` сначала осевой подъём, только затем горизонтальный уход или поворот. -- [x] Написать тесты инструментов: `grab(vacuum)` стыкуется кончиком `patron` к нужной точке; `release(vacuum)` из `work` **не двигает робот и не отделяет инструмент**; только после явного `transfer(vacuum,vacuum_socket)` навыки `release(vacuum,vacuum_socket)` и `dock_tool(vacuum,vacuum_socket)` опускаются в гнездо, отделяют и поднимаются. Отсутствие/занятость гнезда даёт отказ. `grab(cap)` без присоски не начинается. +- [x] Написать тесты инструментов: `grab(vacuum)` стыкуется кончиком `patron` к нужной точке; `release(vacuum)` из `work` **не двигает робот и не отделяет инструмент**; только после явного `transfer(vacuum,vacuum_socket)` навыки `release(vacuum,vacuum_socket)` и `dock_tool(vacuum,vacuum_socket)` опускаются в гнездо, отделяют и поднимаются. Отсутствие/занятость гнезда даёт отказ. `grab(cap)` без присоски не начинает захват: проверяется supervisor, контакт не выполняется, возвращается `grasp_failed`. - [x] Написать тесты объектов: `transfer/align/release(cap,hatch_seat)` реально переносит и сажает люк; `dispense` движется вдоль заданного участка; `handoff_screw(screw_1,hole_1)` подносит винт и отпускает виртуальному человеку, не занимает отверстие установленным винтом. Неподдерживаемый тип цели отвергается. - [x] Написать тесты `detect`: сначала осевой отвод и `work`, затем свежий верхний кадр + поза из supervisor; близкий кадр по настройке; просроченный кадр возвращает `failed`. `inspect_state` различает `empty`, `holding`, `placed`, `unknown`. - [x] Реализовать навыки реестра из спецификации, используя общие геометрические примитивы. Каждый результат отображать в одно событие контракта; неизвестный исход не подменять успехом. Прогнать `pytest -q src/iiwa_llm_plan_executor/test/test_skills.py` и зафиксировать коммит. diff --git a/docs/superpowers/specs/2026-11-10-llm-graph-execution-design.md b/docs/superpowers/specs/2026-11-10-llm-graph-execution-design.md index 349ec9c..07f8ad6 100644 --- a/docs/superpowers/specs/2026-11-10-llm-graph-execution-design.md +++ b/docs/superpowers/specs/2026-11-10-llm-graph-execution-design.md @@ -18,7 +18,7 @@ Выбран **граф + реестр навыков**. Движения, неизбежные внутри одного навыка, задаются его проверяемым контрактом: например, `grab` может сам подойти сверху, опуститься, захватить и подняться до сохранённой высоты, если явных узлов `approach`/`lower`/`lift` не было. Если они уже выполнялись, повторного движения нет. Это геометрические фазы **того же** действия, а не добавление нового технологического задания. -Исполнитель не придумывает следующую деталь, нужный инструмент или цель из номера эксперимента. Если `grab(cap)` требует вакуумный инструмент, а в `patron` его нет, граф должен явно содержать взятие инструмента; иначе предзапусковая проверка возвращает `replan_required` без движения. `release` инструмента **не возвращает его автоматически**. Для возврата граф должен явно содержать перемещение к гнезду `vacuum_socket` и затем `release(vacuum, vacuum_socket)` либо `dock_tool(vacuum, vacuum_socket)`; оба последних навыка только выполняют стыковку и отделение в уже достигнутом гнезде. `release(vacuum)` из `work` без подходящего места для инструмента отклоняется, присоска остаётся в `patron`. Если граф заканчивается в `work` с присоской в `patron`, она остаётся там до следующего явного задания. Новый процесс может подключить другой адаптер и свои контракты; неизвестный навык или объект не исполняется. +Исполнитель не придумывает следующую деталь, нужный инструмент или цель из номера эксперимента. Если `grab(cap)` требует вакуумный инструмент, а в `patron` его нет, граф не отклоняется на входе: при исполнении узла проверяется живое состояние supervisor, захват не начинается, контакт не выполняется, и узел возвращает `grasp_failed` с причиной `required tool vacuum is not mounted`; далее исполняется ровно тот переход `grasp_failed`, который указала модель. Если в графе нет явного узла `grab(vacuum)`, инструмент автоматически не берётся. `release` инструмента **не возвращает его автоматически**. Для возврата граф должен явно содержать перемещение к гнезду `vacuum_socket` и затем `release(vacuum, vacuum_socket)` либо `dock_tool(vacuum, vacuum_socket)`; оба последних навыка только выполняют стыковку и отделение в уже достигнутом гнезде. `release(vacuum)` из `work` без подходящего места для инструмента отклоняется, присоска остаётся в `patron`. Если граф заканчивается в `work` с присоской в `patron`, она остаётся там до следующего явного задания. Новый процесс может подключить другой адаптер и свои контракты; неизвестный навык или объект не исполняется. Рассмотренный, но отвергнутый подход — переписывать граф модели и автоматически вставлять недостающие технологические действия. Это могло бы выполнить лишние операции при произвольных запросах. Второй отвергнутый подход — напрямую переводить ID навыка в фиксированную траекторию без проверки текущей позы и состояния; он не учитывает смещения объектов и ветви событий. diff --git a/src/iiwa_llm_plan_executor/doc/webots-qa.md b/src/iiwa_llm_plan_executor/doc/webots-qa.md index 25fa742..dcac4d8 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; `348 tests, 0 errors, 0 failures, +Result: build of 10 packages succeeded; `359 tests, 0 errors, 0 failures, 0 skipped`. `git diff --check` is clean. @@ -50,7 +50,7 @@ client-supplied `run_id`; the accepted graph appeared in | `grab(vacuum) → transfer(vacuum,work) → completed` | `completed`; status ended with `mounted_tool: "vacuum"`, `held_object: null`. The cup stayed in `patron`. | | `transfer(vacuum,vacuum_socket) → dock_tool(vacuum,vacuum_socket) → completed` | `completed`; status ended with `mounted_tool: null`. This was a separate graph after the previous one. | | `grab(cap) → transfer(cap,hatch_seat) → align → release(cap,hatch_seat)` (cup already mounted) | Cap grasp, transfer, align and release succeeded; the supervisor reported the hatch installed. The final `verify` first failed because of a web-wrapper attribute shadowing bug (`latest`), which was fixed afterwards; a clean repeat of the whole cap graph was not obtained in this session. | -| cap graph without explicit `grab(vacuum)` | Rejected before motion: `replan_required: 'gc' requires the vacuum tool in patron; mounted tool is None`. | +| cap graph without explicit `grab(vacuum)`, with a `grasp_failed` edge | Admitted; execution returned `grasp_failed` with `reason: "required tool vacuum is not mounted"`, `next: "operator_required"`, `mounted_tool: null`, `held_object: null`. The supervisor state stayed at version 0 with the cap `free` and `vacuum` `docked`; the log contains no planner call for that run. `/plan` held the original graph, `/status`, `GET /planner/runs/...` and the JSONL log held the actual event/reason/ownership/next edge. | | stale `scene_revision=999` | Rejected: `stale_scene: submitted revision 999 does not match current revision 1`. | The screw handoff graph (`grab(magnetic_tool) → grab(screw_1) → diff --git a/src/iiwa_llm_plan_executor/docs/interfaces.md b/src/iiwa_llm_plan_executor/docs/interfaces.md index dfc0754..af785ed 100644 --- a/src/iiwa_llm_plan_executor/docs/interfaces.md +++ b/src/iiwa_llm_plan_executor/docs/interfaces.md @@ -67,15 +67,27 @@ returns `ValidationResult(ok, code, reason, details)` with codes: | --- | --- | | `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) | +| `replan_required` | a fresh scene/plan is required before motion (missing explicit transfer, unconfirmed ownership, or a graph branch that contradicts the live ownership state) | | `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. +* A missing required tool is **not** an admission error. `pick(cap)` / + `grab(cap)` is admitted even when `vacuum` is docked. At execution time the + skill re-reads the live supervisor state. If `vacuum` is not mounted it + returns `grasp_failed` with the reason + `required tool vacuum is not mounted`, performs no contact motion, sends no + capture command, changes no object state, and the engine follows exactly the + graph's `grasp_failed` edge. The `success` edge is not assumed. +* 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. +* 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 + rejected with its own ownership reason (for example `hatch is not held`), + not with a missing-tool reason. * `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 @@ -99,6 +111,11 @@ Important pre-movement rules: | service | `/iiwa_llm_plan_executor/get_status` | `iiwa_msgs/GetRunStatus` | request `string run_id`; response `bool found`, `string status_json`. | | service | `/iiwa_llm_plan_executor/cancel` | `webots_ros2_msgs/SetString` | `{"run_id": "..."}` or a plain run id; stops execution and preserves the grasp. | +Every event is logged with the actual skill result, the failure reason when +present, `mounted_tool` and `held_object` read from the supervisor, and the +`next` node selected from the published graph. The same event is written to +the JSONL log and returned by `GET /planner/runs/{run_id}`. + Admission conditions: fresh `scene_json`, matching `scene_revision`, `scene_ready`, a ready MoveIt/controller, fresh `joint_states`, and at least 3.0 s of simulation time since the first ready scene. Only one run may be diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/engine.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/engine.py index 82a14e6..cf95473 100644 --- a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/engine.py +++ b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/engine.py @@ -382,16 +382,24 @@ class GraphEngine: next_id: str, outcome: Any, ) -> None: + context = run.context + if context is not None: + if hasattr(context, 'mounted_tool'): + run.mounted_tool = context.mounted_tool + if hasattr(context, 'held_object'): + run.held_object = context.held_object + details = getattr(outcome, 'details', {}) or {} entry = { 'run_id': run.run_id, 'node_id': node.id, 'skill': node.skill, 'event': event, + 'reason': details.get('reason', ''), 'next': next_id, 'scene_revision': run.scene_revision, 'mounted_tool': run.mounted_tool, 'held_object': run.held_object, - 'details': getattr(outcome, 'details', {}) or {}, + 'details': details, 'timestamp': self._clock(), } run.events.append(entry) 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 2785c0f..ca63d2c 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 @@ -143,6 +143,23 @@ class SkillRegistry: return {} return state if isinstance(state, dict) else {} + def _refresh_live_ownership(self, context: SkillContext) -> str | None: + """Read the tool and held object from the live supervisor. + + Context memory is not authoritative between commands or runs: the + supervisor may have been changed by another accepted graph or by a + previous failed attempt. + """ + state = self._state() + if not state: + return 'supervisor state is unavailable' + context.supervisor_state = state + mounted = state.get('mounted_tool') + held = state.get('held_object') + context.mounted_tool = mounted if isinstance(mounted, str) else None + context.held_object = held if isinstance(held, str) else None + return None + def _move( self, context: SkillContext, @@ -430,18 +447,35 @@ class SkillRegistry: # ------------------------------------------------------------------ grab def _grab(self, node: PlanNode, context: SkillContext) -> SkillOutcome: obj = node.object + unavailable = self._refresh_live_ownership(context) + if unavailable is not None: + return SkillOutcome('grasp_failed', {'reason': unavailable}) if obj in context.spec.tool_names: return self._grab_tool(node, context) if obj == 'cap': if context.mounted_tool != 'vacuum': - return SkillOutcome('failed', { - 'reason': 'grab(cap) requires the vacuum tool in patron', + return SkillOutcome('grasp_failed', { + 'reason': 'required tool vacuum is not mounted', + 'required_tool': 'vacuum', + 'mounted_tool': context.mounted_tool, + }) + if context.held_object is not None: + return SkillOutcome('grasp_failed', { + 'reason': f'hand is not free: {context.held_object!r} is held', + 'held_object': context.held_object, }) return self._grab_part(node, context, 'grab_cap', 'vacuum') if obj.startswith('screw_'): if context.mounted_tool != 'magnetic_tool': - return SkillOutcome('failed', { - 'reason': 'grab(screw) requires magnetic_tool in patron', + return SkillOutcome('grasp_failed', { + 'reason': 'required tool magnetic_tool is not mounted', + 'required_tool': 'magnetic_tool', + 'mounted_tool': context.mounted_tool, + }) + if context.held_object is not None: + return SkillOutcome('grasp_failed', { + 'reason': f'hand is not free: {context.held_object!r} is held', + 'held_object': context.held_object, }) return self._grab_part( node, context, 'grab_screw', 'magnetic_tool', @@ -548,6 +582,9 @@ class SkillRegistry: def _transfer(self, node: PlanNode, context: SkillContext) -> SkillOutcome: obj = node.object target = node.target + unavailable = self._refresh_live_ownership(context) + if unavailable is not None: + return SkillOutcome('failed', {'reason': unavailable}) if obj in context.spec.tool_names: if context.mounted_tool != obj: return SkillOutcome('failed', { @@ -601,6 +638,9 @@ class SkillRegistry: def _release(self, node: PlanNode, context: SkillContext) -> SkillOutcome: obj = node.object target = node.target + unavailable = self._refresh_live_ownership(context) + if unavailable is not None: + return SkillOutcome('failed', {'reason': unavailable}) if obj in context.spec.tool_names: return self._release_tool(node, context) if obj == 'cap': @@ -614,7 +654,9 @@ class SkillRegistry: socket = f'{tool}_socket' if context.mounted_tool != tool: return SkillOutcome('failed', { - 'reason': f'{tool!r} is not mounted', + 'reason': f'required tool {tool} is not mounted', + 'required_tool': tool, + 'mounted_tool': context.mounted_tool, 'moved': False, }) if node.target != socket: @@ -664,7 +706,10 @@ class SkillRegistry: def _release_cap(self, node: PlanNode, context: SkillContext) -> SkillOutcome: if context.held_object != 'cap': - return SkillOutcome('failed', {'reason': 'hatch is not held'}) + return SkillOutcome('failed', { + 'reason': 'hatch is not held', + 'held_object': context.held_object, + }) target = node.target or 'hatch_seat' self._ensure_approach(context, node) adapter = self._adapter(context) @@ -692,6 +737,9 @@ class SkillRegistry: return SkillOutcome('success', {'object': 'cap', 'target': target}) def _dock_tool(self, node: PlanNode, context: SkillContext) -> SkillOutcome: + unavailable = self._refresh_live_ownership(context) + if unavailable is not None: + return SkillOutcome('failed', {'reason': unavailable}) if node.object not in context.spec.tool_names: return SkillOutcome('failed', {'reason': f'{node.object!r} is not a tool'}) socket = f'{node.object}_socket' @@ -704,12 +752,20 @@ class SkillRegistry: def _handoff_screw(self, node: PlanNode, context: SkillContext) -> SkillOutcome: screw = node.object hole = node.target + unavailable = self._refresh_live_ownership(context) + if unavailable is not None: + return SkillOutcome('grasp_failed', {'reason': unavailable}) + if context.mounted_tool != 'magnetic_tool': + return SkillOutcome('grasp_failed', { + 'reason': 'required tool magnetic_tool is not mounted', + 'required_tool': 'magnetic_tool', + 'mounted_tool': context.mounted_tool, + }) if context.held_object != screw: return SkillOutcome('grasp_failed', { 'reason': f'{screw!r} is not held', + 'held_object': context.held_object, }) - if context.mounted_tool != 'magnetic_tool': - return SkillOutcome('failed', {'reason': 'magnetic_tool is not mounted'}) self._ensure_approach(context, node) adapter = self._adapter(context) prepare = getattr(adapter, 'prepare_handoff_screw', None) @@ -752,8 +808,15 @@ class SkillRegistry: return SkillOutcome('success', {'tcp': list(work.xyz)}) def _dispense(self, node: PlanNode, context: SkillContext) -> SkillOutcome: + unavailable = self._refresh_live_ownership(context) + if unavailable is not None: + return SkillOutcome('failed', {'reason': unavailable}) if context.mounted_tool != 'sealant_tool': - return SkillOutcome('failed', {'reason': 'sealant_tool is not mounted'}) + return SkillOutcome('failed', { + 'reason': 'required tool sealant_tool is not mounted', + 'required_tool': 'sealant_tool', + 'mounted_tool': context.mounted_tool, + }) side = node.object or node.target start_fraction = float(node.params.get('start_fraction', 0.0)) end_fraction = float(node.params.get('end_fraction', 1.0)) 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 e8d61de..be008fc 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 @@ -323,11 +323,8 @@ def _precondition( 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}' - ) + # A missing vacuum tool is a runtime grasp_failed condition, not + # a reason to reject the graph before execution. if state.held is not None: return _replan( f'{node.id!r} cannot grab the hatch while holding ' @@ -335,11 +332,7 @@ def _precondition( ) 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}' - ) + # A missing magnetic tool is a runtime grasp_failed condition. if state.held is not None: return _replan( f'{node.id!r} cannot grab a screw while holding ' @@ -351,11 +344,6 @@ def _precondition( 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 ' @@ -388,8 +376,6 @@ def _precondition( 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' @@ -419,10 +405,6 @@ def _precondition( 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( @@ -434,11 +416,6 @@ def _precondition( 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}' @@ -455,10 +432,6 @@ def _precondition( 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' @@ -471,6 +444,58 @@ def _precondition( return None +def _success_feasible( + node: PlanNode, + state: _State, + facts: _Facts, +) -> bool: + """Whether this node's nominal success can happen from ``state``. + + A missing required tool must not reject the graph, but its success branch + must not be treated as already achieved either. The validator therefore + skips only the success edge for that state and continues through the + failure events the graph actually declared. + """ + skill = node.skill + obj = node.object + if skill in ('grab', 'pick'): + if obj in facts.tools: + return ( + state.mounted in (None, obj) + and state.held is None + ) + if obj == 'cap': + return state.mounted == 'vacuum' and state.held is None + if obj and facts.components.get(obj) == 'screw': + return state.mounted == 'magnetic_tool' and state.held is None + return False + if skill in ('transfer', 'move_to_storage'): + if obj in facts.tools: + return state.mounted == obj + if obj == 'cap': + return state.held == 'cap' + if obj and facts.components.get(obj) == 'screw': + return state.held == obj + return False + if skill == 'release': + if obj in facts.tools: + return state.mounted == obj + if obj == 'cap': + return state.held == 'cap' + if obj and facts.components.get(obj) == 'screw': + return state.held == obj and state.mounted == 'magnetic_tool' + return False + if skill == 'align': + return state.held is not None or state.mounted is not None + if skill == 'dispense': + return state.mounted == 'sealant_tool' + if skill == 'dock_tool': + return state.mounted == obj + if skill == 'handoff_screw': + return state.held == obj and state.mounted == 'magnetic_tool' + return True + + def _apply_event( node: PlanNode, event: str, @@ -540,6 +565,14 @@ def _ownership_checks(graph: PlanGraph, facts: _Facts) -> ValidationResult | Non for transition in node.transitions: if transition.next not in graph.ids: continue + if ( + transition.event in PROGRESS_EVENTS + and not _success_feasible(node, state, facts) + ): + # Do not simulate a success that cannot happen (for example + # pick(cap) without the vacuum tool). The grasp_failed edge + # from the same state is still traversed and checked. + continue next_state = _apply_event(node, transition.event, state, facts) if (transition.next, next_state) not in seen: queue.append((transition.next, next_state)) diff --git a/src/iiwa_llm_plan_executor/test/test_engine.py b/src/iiwa_llm_plan_executor/test/test_engine.py index 12584c5..d230105 100644 --- a/src/iiwa_llm_plan_executor/test/test_engine.py +++ b/src/iiwa_llm_plan_executor/test/test_engine.py @@ -42,6 +42,7 @@ class FakeSkills: def __init__(self): self.calls = [] self.events = {} + self.details = {} self.on_execute = None self.block = False self.started = threading.Event() @@ -54,7 +55,10 @@ class FakeSkills: self.started.set() self.release.wait(timeout=5.0) self.calls.append(node.id) - return SimpleNamespace(event=self.events.get(node.id, 'success'), details={}) + return SimpleNamespace( + event=self.events.get(node.id, 'success'), + details=self.details.get(node.id, {}), + ) def envelope(revision=1, stamp=5.0, experiment_id=1): @@ -180,12 +184,13 @@ def test_rejected_graphs_are_never_published_or_executed(): engine, skills, events, _, _ = make_engine() blocked = engine.submit('r1', {'status': 'blocked', 'entry': '', 'plan': []}, 1) assert blocked.code == 'blocked' - cap_without_vacuum = graph( - 'g', node('g', 'grab', 'cap', next_on={'success': 'completed'}), + bad_next = graph( + 'g', + node('g', 'grab', 'vacuum', next_on={'success': 'ghost'}), ) - rejected = engine.submit('r2', cap_without_vacuum.as_dict(), 1) + rejected = engine.submit('r2', bad_next.as_dict(), 1) assert rejected.accepted is False - assert rejected.code == 'replan_required' + assert rejected.code == 'rejected' assert events['plans'] == [] assert skills.calls == [] stale = engine.submit('r3', STOP_GRAPH().as_dict(), 2) @@ -193,6 +198,30 @@ def test_rejected_graphs_are_never_published_or_executed(): assert engine.get_status('r2').state == 'rejected' +def test_cap_grab_without_vacuum_is_admitted_and_follows_grasp_failed(): + engine, skills, events, _, _ = make_engine() + value = graph( + 'g', + node('g', 'grab', 'cap', next_on={ + 'success': 'completed', + 'grasp_failed': 'operator_required', + }), + ) + accepted = engine.submit('r1', value.as_dict(), 1) + assert accepted.accepted is True + assert len(events['plans']) == 1 + skills.events = {'g': 'grasp_failed'} + skills.details = {'g': {'reason': 'required tool vacuum is not mounted'}} + engine.run_accepted('r1') + status = engine.get_status('r1') + assert status.state == 'operator_required' + assert status.events[0]['event'] == 'grasp_failed' + assert status.events[0]['next'] == 'operator_required' + assert status.events[0]['reason'] + assert status.mounted_tool is None + assert status.held_object is None + + def test_accepted_graph_is_published_once_before_the_first_motion(): engine, skills, events, _, _ = make_engine() order = [] diff --git a/src/iiwa_llm_plan_executor/test/test_skills.py b/src/iiwa_llm_plan_executor/test/test_skills.py index c100640..b45f2b2 100644 --- a/src/iiwa_llm_plan_executor/test/test_skills.py +++ b/src/iiwa_llm_plan_executor/test/test_skills.py @@ -225,10 +225,11 @@ def test_grab_keeps_target_in_collision_scene_during_approach(): self.events.append(('prepare_grab_cap', object_id)) adapter = RecordingScene() - registry, context, motion, _ = make_registry( + registry, context, motion, supervisor = make_registry( motion=FakeMotion(success=False), adapter=adapter, ) context.mounted_tool = 'vacuum' + supervisor.state['mounted_tool'] = 'vacuum' result = registry.execute(node('g', 'grab', 'cap'), context) assert result.event == 'failed' assert phases(motion) == ['approach'] @@ -253,10 +254,11 @@ def test_grab_restores_target_when_lowering_fails(): ) adapter = RecordingScene() - registry, context, motion, _ = make_registry( + registry, context, motion, supervisor = make_registry( motion=FailOnLower(), adapter=adapter, ) context.mounted_tool = 'vacuum' + supervisor.state['mounted_tool'] = 'vacuum' result = registry.execute(node('g', 'grab', 'cap'), context) assert result.event == 'failed' assert phases(motion) == ['approach', 'lower'] @@ -292,11 +294,46 @@ def test_explicit_phases_are_not_repeated_by_grab(): assert phases(motion) == ['approach', 'lower', 'retreat'] -def test_grab_cap_without_vacuum_does_not_move(): - registry, context, motion, _ = make_registry() +def test_grab_cap_without_vacuum_reports_grasp_failed_without_motion_or_command(): + registry, context, motion, supervisor = make_registry() + before = copy.deepcopy(supervisor.state) result = registry.execute(node('g', 'grab', 'cap'), context) - assert result.event == 'failed' + assert result.event == 'grasp_failed' + assert result.details['reason'] == 'required tool vacuum is not mounted' + assert result.details['required_tool'] == 'vacuum' assert motion.calls == [] + assert supervisor.commands == [] + assert registry.scene_adapter.events == [] + assert supervisor.state['cap']['state'] == before['cap']['state'] + assert supervisor.state['held_object'] is None + assert context.held_object is None + assert context.mounted_tool is None + + +def test_supervisor_state_overrides_stale_context_for_tool_checks(): + registry, context, motion, supervisor = make_registry() + context.mounted_tool = 'vacuum' # stale memory; supervisor says no tool + result = registry.execute(node('g', 'grab', 'cap'), context) + assert result.event == 'grasp_failed' + assert result.details['reason'] == 'required tool vacuum is not mounted' + assert motion.calls == [] + assert supervisor.commands == [] + assert context.mounted_tool is None + + +def test_grab_screw_without_magnetic_tool_reports_grasp_failed_without_motion(): + registry, context, motion, supervisor = make_registry() + before = copy.deepcopy(supervisor.state) + result = registry.execute(node('g', 'grab', 'screw_1'), context) + assert result.event == 'grasp_failed' + assert result.details['reason'] == 'required tool magnetic_tool is not mounted' + assert result.details['required_tool'] == 'magnetic_tool' + assert motion.calls == [] + assert supervisor.commands == [] + assert registry.scene_adapter.events == [] + assert supervisor.state['screws']['screw_1']['state'] == before['screws']['screw_1']['state'] + assert supervisor.state['held_object'] is None + assert context.held_object is None def test_tool_grab_docks_at_the_socket_and_lifts_out(): @@ -400,8 +437,10 @@ def test_release_lifts_axially_before_any_lateral_motion(): def test_dispense_moves_along_the_segment_without_changing_ownership(): - registry, context, motion, _ = make_registry() + registry, context, motion, supervisor = make_registry() context.mounted_tool = 'sealant_tool' + supervisor.state['mounted_tool'] = 'sealant_tool' + supervisor.state['tools']['sealant_tool']['state'] = 'mounted' result = registry.execute( node('d', 'dispense', 'side_1', params={ 'start_fraction': 0.0, 'end_fraction': 1.0, diff --git a/src/iiwa_llm_plan_executor/test/test_validator.py b/src/iiwa_llm_plan_executor/test/test_validator.py index 30f3287..e1cebb9 100644 --- a/src/iiwa_llm_plan_executor/test/test_validator.py +++ b/src/iiwa_llm_plan_executor/test/test_validator.py @@ -111,15 +111,64 @@ def test_blocked_status_never_reaches_execution(): assert result.code == 'blocked' -def test_cap_grab_without_explicit_vacuum_is_replan_required_before_motion(): +def test_cap_grab_without_vacuum_is_admitted_for_runtime_grasp_failed(): value = graph( 'grab_cap', - node('grab_cap', 'grab', 'cap', next_on={'success': 'completed'}), + node('grab_cap', 'grab', 'cap', next_on={ + 'success': 'completed', + 'grasp_failed': 'operator_required', + }), + ) + result = expect(value, scene_for(1)) + assert result.ok is True + + # The success branch is kept in the graph but is not simulated as if the + # hatch were already held without a vacuum tool. + with_success_branch = graph( + 'grab_cap', + node('grab_cap', 'grab', 'cap', next_on={ + 'success': 'transfer', + 'grasp_failed': 'operator_required', + }), + node('transfer', 'transfer', 'cap', 'hatch_seat', + next_on={'success': 'completed'}), + ) + assert expect(with_success_branch, scene_for(1)).ok is True + + +def test_screw_grab_without_magnetic_tool_is_admitted(): + value = graph( + 'pick_screw', + node('pick_screw', 'pick', 'screw_1', next_on={ + 'success': 'completed', + 'grasp_failed': 'operator_required', + }), + ) + result = expect(value, scene_for(20)) + assert result.ok is True + + +def test_recovery_branch_that_assumes_a_held_hatch_has_its_own_reason(): + value = graph( + 'grab_cap', + node('grab_cap', 'grab', 'cap', next_on={ + 'success': 'completed', + 'grasp_failed': 'inspect', + }), + node('inspect', 'inspect_state', 'cap', next_on={ + 'empty': 'transfer', + 'holding': 'transfer', + 'placed': 'completed', + 'unknown': 'operator_required', + }), + node('transfer', 'transfer', 'cap', 'hatch_seat', + 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 + assert 'hatch is not held' in result.reason + assert 'vacuum' not in result.reason def test_take_vacuum_and_leave_it_in_patron_is_accepted(): @@ -354,12 +403,12 @@ def test_screw_handoff_is_to_the_operator_and_leaves_the_hole_empty(): assert result.ok is True -def test_dispense_requires_the_sealant_tool(): +def test_dispense_requires_the_sealant_tool_at_runtime(): without = graph( 'dispense', node('dispense', 'dispense', 'side_1', next_on={'success': 'completed'}), ) - assert expect(without, scene_for(20)).code == 'replan_required' + assert expect(without, scene_for(20)).ok is True with_tool = graph( 'grab_sealant', node('grab_sealant', 'grab', 'sealant_tool',