fix(llm-plan-executor): report missing tools as runtime grasp failures

This commit is contained in:
Даниил Грабарь
2026-11-11 11:28:12 +10:00
parent 112673f1f4
commit 48c190d75b
10 changed files with 304 additions and 66 deletions
@@ -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` и зафиксировать коммит.
@@ -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 навыка в фиксированную траекторию без проверки текущей позы и состояния; он не учитывает смещения объектов и ветви событий.