fix(llm-plan-executor): report missing tools as runtime grasp failures
This commit is contained in:
@@ -16,7 +16,7 @@
|
|||||||
- Создать ровно два дополнительных ROS 2 пакета: `src/iiwa_scene_context` и `src/iiwa_llm_plan_executor`. Изменения `iiwa_experement` и `iiwa_web` ограничить интеграцией. Не делать систему оценки Qwen3, её обучение или вызов модели: внешний клиент передаёт **готовый** граф.
|
- Создать ровно два дополнительных ROS 2 пакета: `src/iiwa_scene_context` и `src/iiwa_llm_plan_executor`. Изменения `iiwa_experement` и `iiwa_web` ограничить интеграцией. Не делать систему оценки Qwen3, её обучение или вызов модели: внешний клиент передаёт **готовый** граф.
|
||||||
- Номер эксперимента выбирает исходную сцену, но не задаёт робототехническое задание. Исполнять только переданный граф. Не вставлять операции взятия инструмента, герметизации, установки люка или передачи винтов на основании номера эксперимента.
|
- Номер эксперимента выбирает исходную сцену, но не задаёт робототехническое задание. Исполнять только переданный граф. Не вставлять операции взятия инструмента, герметизации, установки люка или передачи винтов на основании номера эксперимента.
|
||||||
- Внутри *одного* навыка допустимы обязательные геометрические фазы: безопасный подход сверху, осевое опускание, действие, осевой подъём к сохранённой высоте. Явные узлы этих фаз не должны вызывать повторное действие. Неизвестный объект, навык, инструмент, цель, положение или неоднозначная зависимость дают отказ до движения либо `replan_required`.
|
- Внутри *одного* навыка допустимы обязательные геометрические фазы: безопасный подход сверху, осевое опускание, действие, осевой подъём к сохранённой высоте. Явные узлы этих фаз не должны вызывать повторное действие. Неизвестный объект, навык, инструмент, цель, положение или неоднозначная зависимость дают отказ до движения либо `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 используется для контроля, а не для вычисления позы.
|
- `detect`: перейти в `work`, дождаться свежего кадра верхней камеры, получить точную позу из supervisor. При настроенном близком осмотре получить также новый кадр камеры на роботе; RGB/D используется для контроля, а не для вычисления позы.
|
||||||
- Рабочий кончик направлен вниз; при рабочих движениях наклон не более 10° и J7 в жёстком коридоре ±90° относительно `work` (сейчас 0 рад). Применять ограничения к *траектории*, не только к конечной позе. Если траектория недоступна, остановиться; не переходить на ветвь с разворотом J7 около −180°. Поворот кисти и переход в `work` только после осевого отвода от контакта.
|
- Рабочий кончик направлен вниз; при рабочих движениях наклон не более 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` означает выполненный граф и проверенные постусловия, а не оценку соответствия произвольному текстовому запросу.
|
- Один активный граф на робота. Внешний клиент передаёт ответ 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] Написать тесты: показанный пользователем формат `status/entry/plan/transitions` разбирается; `status=blocked` не допускается к исполнению; дубликаты ID, неизвестные навыки/объекты/цели/события, пропущенный `entry`, несуществующие `next`, недостижимые узлы, пустые обязательные переходы и превышение лимитов отвергаются.
|
||||||
- [x] Написать тесты ветвей отказа: `collision` не ведёт к продолжению операции; `grasp_failed` не превращается в `success`; цикл с допустимым восстановлением имеет ограничение 256 узлов и отдельный предел выполненных шагов; неверный `scene_revision` отклоняется.
|
- [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] Прочитать несовпадение имён в приложенных наборах. Ввести версию контракта и только явно проверенные алиасы `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` сначала до, затем после реализации; сохранить контракт и отдельный коммит.
|
- [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`.
|
**Интерфейсы:** `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] Написать тесты фаз на фиктивном 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] Написать тесты объектов: `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] Написать тесты `detect`: сначала осевой отвод и `work`, затем свежий верхний кадр + поза из supervisor; близкий кадр по настройке; просроченный кадр возвращает `failed`. `inspect_state` различает `empty`, `holding`, `placed`, `unknown`.
|
||||||
- [x] Реализовать навыки реестра из спецификации, используя общие геометрические примитивы. Каждый результат отображать в одно событие контракта; неизвестный исход не подменять успехом. Прогнать `pytest -q src/iiwa_llm_plan_executor/test/test_skills.py` и зафиксировать коммит.
|
- [x] Реализовать навыки реестра из спецификации, используя общие геометрические примитивы. Каждый результат отображать в одно событие контракта; неизвестный исход не подменять успехом. Прогнать `pytest -q src/iiwa_llm_plan_executor/test/test_skills.py` и зафиксировать коммит.
|
||||||
|
|||||||
@@ -18,7 +18,7 @@
|
|||||||
|
|
||||||
Выбран **граф + реестр навыков**. Движения, неизбежные внутри одного навыка, задаются его проверяемым контрактом: например, `grab` может сам подойти сверху, опуститься, захватить и подняться до сохранённой высоты, если явных узлов `approach`/`lower`/`lift` не было. Если они уже выполнялись, повторного движения нет. Это геометрические фазы **того же** действия, а не добавление нового технологического задания.
|
Выбран **граф + реестр навыков**. Движения, неизбежные внутри одного навыка, задаются его проверяемым контрактом: например, `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 навыка в фиксированную траекторию без проверки текущей позы и состояния; он не учитывает смещения объектов и ветви событий.
|
Рассмотренный, но отвергнутый подход — переписывать граф модели и автоматически вставлять недостающие технологические действия. Это могло бы выполнить лишние операции при произвольных запросах. Второй отвергнутый подход — напрямую переводить ID навыка в фиксированную траекторию без проверки текущей позы и состояния; он не учитывает смещения объектов и ветви событий.
|
||||||
|
|
||||||
|
|||||||
@@ -17,7 +17,7 @@ colcon test --base-paths src --packages-select \
|
|||||||
colcon test-result --verbose
|
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`.
|
0 skipped`.
|
||||||
|
|
||||||
`git diff --check` is clean.
|
`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`. |
|
| `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. |
|
| `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. |
|
| `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`. |
|
| 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) →
|
The screw handoff graph (`grab(magnetic_tool) → grab(screw_1) →
|
||||||
|
|||||||
@@ -67,15 +67,27 @@ returns `ValidationResult(ok, code, reason, details)` with codes:
|
|||||||
| --- | --- |
|
| --- | --- |
|
||||||
| `ok` | admitted |
|
| `ok` | admitted |
|
||||||
| `blocked` | the model returned `status=blocked`; no motion |
|
| `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 |
|
| `rejected` | structural or safety violation |
|
||||||
| `stale_scene` | the graph was built for another `scene_revision` |
|
| `stale_scene` | the graph was built for another `scene_revision` |
|
||||||
|
|
||||||
Important pre-movement rules:
|
Important pre-movement rules:
|
||||||
|
|
||||||
* `grab(cap)` requires `vacuum` in `patron`. If the scene starts with the cup
|
* A missing required tool is **not** an admission error. `pick(cap)` /
|
||||||
docked, the graph must explicitly `grab(vacuum)`; otherwise admission is
|
`grab(cap)` is admitted even when `vacuum` is docked. At execution time the
|
||||||
`replan_required` and nothing moves.
|
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
|
* `release(vacuum)` without a target, or anywhere except its named socket and
|
||||||
after an explicit `transfer(vacuum, vacuum_socket)`, is `replan_required`.
|
after an explicit `transfer(vacuum, vacuum_socket)`, is `replan_required`.
|
||||||
The cup stays mounted. `release(vacuum, vacuum_socket)` and
|
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/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. |
|
| 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`,
|
Admission conditions: fresh `scene_json`, matching `scene_revision`,
|
||||||
`scene_ready`, a ready MoveIt/controller, fresh `joint_states`, and at least
|
`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
|
3.0 s of simulation time since the first ready scene. Only one run may be
|
||||||
|
|||||||
@@ -382,16 +382,24 @@ class GraphEngine:
|
|||||||
next_id: str,
|
next_id: str,
|
||||||
outcome: Any,
|
outcome: Any,
|
||||||
) -> None:
|
) -> 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 = {
|
entry = {
|
||||||
'run_id': run.run_id,
|
'run_id': run.run_id,
|
||||||
'node_id': node.id,
|
'node_id': node.id,
|
||||||
'skill': node.skill,
|
'skill': node.skill,
|
||||||
'event': event,
|
'event': event,
|
||||||
|
'reason': details.get('reason', ''),
|
||||||
'next': next_id,
|
'next': next_id,
|
||||||
'scene_revision': run.scene_revision,
|
'scene_revision': run.scene_revision,
|
||||||
'mounted_tool': run.mounted_tool,
|
'mounted_tool': run.mounted_tool,
|
||||||
'held_object': run.held_object,
|
'held_object': run.held_object,
|
||||||
'details': getattr(outcome, 'details', {}) or {},
|
'details': details,
|
||||||
'timestamp': self._clock(),
|
'timestamp': self._clock(),
|
||||||
}
|
}
|
||||||
run.events.append(entry)
|
run.events.append(entry)
|
||||||
|
|||||||
@@ -143,6 +143,23 @@ class SkillRegistry:
|
|||||||
return {}
|
return {}
|
||||||
return state if isinstance(state, dict) else {}
|
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(
|
def _move(
|
||||||
self,
|
self,
|
||||||
context: SkillContext,
|
context: SkillContext,
|
||||||
@@ -430,18 +447,35 @@ class SkillRegistry:
|
|||||||
# ------------------------------------------------------------------ grab
|
# ------------------------------------------------------------------ grab
|
||||||
def _grab(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
|
def _grab(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
|
||||||
obj = node.object
|
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:
|
if obj in context.spec.tool_names:
|
||||||
return self._grab_tool(node, context)
|
return self._grab_tool(node, context)
|
||||||
if obj == 'cap':
|
if obj == 'cap':
|
||||||
if context.mounted_tool != 'vacuum':
|
if context.mounted_tool != 'vacuum':
|
||||||
return SkillOutcome('failed', {
|
return SkillOutcome('grasp_failed', {
|
||||||
'reason': 'grab(cap) requires the vacuum tool in patron',
|
'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')
|
return self._grab_part(node, context, 'grab_cap', 'vacuum')
|
||||||
if obj.startswith('screw_'):
|
if obj.startswith('screw_'):
|
||||||
if context.mounted_tool != 'magnetic_tool':
|
if context.mounted_tool != 'magnetic_tool':
|
||||||
return SkillOutcome('failed', {
|
return SkillOutcome('grasp_failed', {
|
||||||
'reason': 'grab(screw) requires magnetic_tool in patron',
|
'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(
|
return self._grab_part(
|
||||||
node, context, 'grab_screw', 'magnetic_tool',
|
node, context, 'grab_screw', 'magnetic_tool',
|
||||||
@@ -548,6 +582,9 @@ class SkillRegistry:
|
|||||||
def _transfer(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
|
def _transfer(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
|
||||||
obj = node.object
|
obj = node.object
|
||||||
target = node.target
|
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 obj in context.spec.tool_names:
|
||||||
if context.mounted_tool != obj:
|
if context.mounted_tool != obj:
|
||||||
return SkillOutcome('failed', {
|
return SkillOutcome('failed', {
|
||||||
@@ -601,6 +638,9 @@ class SkillRegistry:
|
|||||||
def _release(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
|
def _release(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
|
||||||
obj = node.object
|
obj = node.object
|
||||||
target = node.target
|
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 obj in context.spec.tool_names:
|
||||||
return self._release_tool(node, context)
|
return self._release_tool(node, context)
|
||||||
if obj == 'cap':
|
if obj == 'cap':
|
||||||
@@ -614,7 +654,9 @@ class SkillRegistry:
|
|||||||
socket = f'{tool}_socket'
|
socket = f'{tool}_socket'
|
||||||
if context.mounted_tool != tool:
|
if context.mounted_tool != tool:
|
||||||
return SkillOutcome('failed', {
|
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,
|
'moved': False,
|
||||||
})
|
})
|
||||||
if node.target != socket:
|
if node.target != socket:
|
||||||
@@ -664,7 +706,10 @@ class SkillRegistry:
|
|||||||
|
|
||||||
def _release_cap(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
|
def _release_cap(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
|
||||||
if context.held_object != 'cap':
|
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'
|
target = node.target or 'hatch_seat'
|
||||||
self._ensure_approach(context, node)
|
self._ensure_approach(context, node)
|
||||||
adapter = self._adapter(context)
|
adapter = self._adapter(context)
|
||||||
@@ -692,6 +737,9 @@ class SkillRegistry:
|
|||||||
return SkillOutcome('success', {'object': 'cap', 'target': target})
|
return SkillOutcome('success', {'object': 'cap', 'target': target})
|
||||||
|
|
||||||
def _dock_tool(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
|
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:
|
if node.object not in context.spec.tool_names:
|
||||||
return SkillOutcome('failed', {'reason': f'{node.object!r} is not a tool'})
|
return SkillOutcome('failed', {'reason': f'{node.object!r} is not a tool'})
|
||||||
socket = f'{node.object}_socket'
|
socket = f'{node.object}_socket'
|
||||||
@@ -704,12 +752,20 @@ class SkillRegistry:
|
|||||||
def _handoff_screw(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
|
def _handoff_screw(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
|
||||||
screw = node.object
|
screw = node.object
|
||||||
hole = node.target
|
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:
|
if context.held_object != screw:
|
||||||
return SkillOutcome('grasp_failed', {
|
return SkillOutcome('grasp_failed', {
|
||||||
'reason': f'{screw!r} is not held',
|
'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)
|
self._ensure_approach(context, node)
|
||||||
adapter = self._adapter(context)
|
adapter = self._adapter(context)
|
||||||
prepare = getattr(adapter, 'prepare_handoff_screw', None)
|
prepare = getattr(adapter, 'prepare_handoff_screw', None)
|
||||||
@@ -752,8 +808,15 @@ class SkillRegistry:
|
|||||||
return SkillOutcome('success', {'tcp': list(work.xyz)})
|
return SkillOutcome('success', {'tcp': list(work.xyz)})
|
||||||
|
|
||||||
def _dispense(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
|
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':
|
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
|
side = node.object or node.target
|
||||||
start_fraction = float(node.params.get('start_fraction', 0.0))
|
start_fraction = float(node.params.get('start_fraction', 0.0))
|
||||||
end_fraction = float(node.params.get('end_fraction', 1.0))
|
end_fraction = float(node.params.get('end_fraction', 1.0))
|
||||||
|
|||||||
@@ -323,11 +323,8 @@ def _precondition(
|
|||||||
if obj == 'cap':
|
if obj == 'cap':
|
||||||
if 'cap' not in facts.components:
|
if 'cap' not in facts.components:
|
||||||
return _reject(f'{node.id!r}: hatch is absent from this scene')
|
return _reject(f'{node.id!r}: hatch is absent from this scene')
|
||||||
if state.mounted != 'vacuum':
|
# A missing vacuum tool is a runtime grasp_failed condition, not
|
||||||
return _replan(
|
# a reason to reject the graph before execution.
|
||||||
f'{node.id!r} requires the vacuum tool in patron; '
|
|
||||||
f'mounted tool is {state.mounted!r}'
|
|
||||||
)
|
|
||||||
if state.held is not None:
|
if state.held is not None:
|
||||||
return _replan(
|
return _replan(
|
||||||
f'{node.id!r} cannot grab the hatch while holding '
|
f'{node.id!r} cannot grab the hatch while holding '
|
||||||
@@ -335,11 +332,7 @@ def _precondition(
|
|||||||
)
|
)
|
||||||
return None
|
return None
|
||||||
if obj in facts.components and facts.components[obj] == 'screw':
|
if obj in facts.components and facts.components[obj] == 'screw':
|
||||||
if state.mounted != 'magnetic_tool':
|
# A missing magnetic tool is a runtime grasp_failed condition.
|
||||||
return _replan(
|
|
||||||
f'{node.id!r} requires magnetic_tool in patron; '
|
|
||||||
f'mounted tool is {state.mounted!r}'
|
|
||||||
)
|
|
||||||
if state.held is not None:
|
if state.held is not None:
|
||||||
return _replan(
|
return _replan(
|
||||||
f'{node.id!r} cannot grab a screw while holding '
|
f'{node.id!r} cannot grab a screw while holding '
|
||||||
@@ -351,11 +344,6 @@ def _precondition(
|
|||||||
if skill == 'release':
|
if skill == 'release':
|
||||||
if obj in facts.tools:
|
if obj in facts.tools:
|
||||||
socket = facts.tools[obj]
|
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:
|
if target != socket:
|
||||||
return _replan(
|
return _replan(
|
||||||
f'{node.id!r}: release({obj}) must name its socket '
|
f'{node.id!r}: release({obj}) must name its socket '
|
||||||
@@ -388,8 +376,6 @@ def _precondition(
|
|||||||
|
|
||||||
if skill in ('transfer', 'move_to_storage'):
|
if skill in ('transfer', 'move_to_storage'):
|
||||||
if obj in facts.tools:
|
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]):
|
if target not in ('work', facts.tools[obj]):
|
||||||
return _replan(
|
return _replan(
|
||||||
f'{node.id!r}: tool transfer must target work or its socket'
|
f'{node.id!r}: tool transfer must target work or its socket'
|
||||||
@@ -419,10 +405,6 @@ def _precondition(
|
|||||||
return None
|
return None
|
||||||
|
|
||||||
if skill == 'dispense':
|
if skill == 'dispense':
|
||||||
if state.mounted != 'sealant_tool':
|
|
||||||
return _replan(
|
|
||||||
f'{node.id!r}: dispense requires sealant_tool in patron'
|
|
||||||
)
|
|
||||||
side = obj or target
|
side = obj or target
|
||||||
if facts.destinations.get(side) != 'contour':
|
if facts.destinations.get(side) != 'contour':
|
||||||
return _reject(
|
return _reject(
|
||||||
@@ -434,11 +416,6 @@ def _precondition(
|
|||||||
socket = facts.tools.get(obj)
|
socket = facts.tools.get(obj)
|
||||||
if socket is None:
|
if socket is None:
|
||||||
return _reject(f'{node.id!r}: {obj!r} is not a present tool')
|
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:
|
if target != socket:
|
||||||
return _replan(
|
return _replan(
|
||||||
f'{node.id!r}: dock_tool must target socket {socket!r}'
|
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')
|
return _reject(f'{node.id!r}: {obj!r} is not a present screw')
|
||||||
if state.held != obj:
|
if state.held != obj:
|
||||||
return _replan(f'{node.id!r}: {obj!r} is not held')
|
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':
|
if facts.destinations.get(target) != 'hole':
|
||||||
return _replan(
|
return _replan(
|
||||||
f'{node.id!r}: handoff_screw target must be a hole'
|
f'{node.id!r}: handoff_screw target must be a hole'
|
||||||
@@ -471,6 +444,58 @@ def _precondition(
|
|||||||
return None
|
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(
|
def _apply_event(
|
||||||
node: PlanNode,
|
node: PlanNode,
|
||||||
event: str,
|
event: str,
|
||||||
@@ -540,6 +565,14 @@ def _ownership_checks(graph: PlanGraph, facts: _Facts) -> ValidationResult | Non
|
|||||||
for transition in node.transitions:
|
for transition in node.transitions:
|
||||||
if transition.next not in graph.ids:
|
if transition.next not in graph.ids:
|
||||||
continue
|
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)
|
next_state = _apply_event(node, transition.event, state, facts)
|
||||||
if (transition.next, next_state) not in seen:
|
if (transition.next, next_state) not in seen:
|
||||||
queue.append((transition.next, next_state))
|
queue.append((transition.next, next_state))
|
||||||
|
|||||||
@@ -42,6 +42,7 @@ class FakeSkills:
|
|||||||
def __init__(self):
|
def __init__(self):
|
||||||
self.calls = []
|
self.calls = []
|
||||||
self.events = {}
|
self.events = {}
|
||||||
|
self.details = {}
|
||||||
self.on_execute = None
|
self.on_execute = None
|
||||||
self.block = False
|
self.block = False
|
||||||
self.started = threading.Event()
|
self.started = threading.Event()
|
||||||
@@ -54,7 +55,10 @@ class FakeSkills:
|
|||||||
self.started.set()
|
self.started.set()
|
||||||
self.release.wait(timeout=5.0)
|
self.release.wait(timeout=5.0)
|
||||||
self.calls.append(node.id)
|
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):
|
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()
|
engine, skills, events, _, _ = make_engine()
|
||||||
blocked = engine.submit('r1', {'status': 'blocked', 'entry': '', 'plan': []}, 1)
|
blocked = engine.submit('r1', {'status': 'blocked', 'entry': '', 'plan': []}, 1)
|
||||||
assert blocked.code == 'blocked'
|
assert blocked.code == 'blocked'
|
||||||
cap_without_vacuum = graph(
|
bad_next = graph(
|
||||||
'g', node('g', 'grab', 'cap', next_on={'success': 'completed'}),
|
'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.accepted is False
|
||||||
assert rejected.code == 'replan_required'
|
assert rejected.code == 'rejected'
|
||||||
assert events['plans'] == []
|
assert events['plans'] == []
|
||||||
assert skills.calls == []
|
assert skills.calls == []
|
||||||
stale = engine.submit('r3', STOP_GRAPH().as_dict(), 2)
|
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'
|
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():
|
def test_accepted_graph_is_published_once_before_the_first_motion():
|
||||||
engine, skills, events, _, _ = make_engine()
|
engine, skills, events, _, _ = make_engine()
|
||||||
order = []
|
order = []
|
||||||
|
|||||||
@@ -225,10 +225,11 @@ def test_grab_keeps_target_in_collision_scene_during_approach():
|
|||||||
self.events.append(('prepare_grab_cap', object_id))
|
self.events.append(('prepare_grab_cap', object_id))
|
||||||
|
|
||||||
adapter = RecordingScene()
|
adapter = RecordingScene()
|
||||||
registry, context, motion, _ = make_registry(
|
registry, context, motion, supervisor = make_registry(
|
||||||
motion=FakeMotion(success=False), adapter=adapter,
|
motion=FakeMotion(success=False), adapter=adapter,
|
||||||
)
|
)
|
||||||
context.mounted_tool = 'vacuum'
|
context.mounted_tool = 'vacuum'
|
||||||
|
supervisor.state['mounted_tool'] = 'vacuum'
|
||||||
result = registry.execute(node('g', 'grab', 'cap'), context)
|
result = registry.execute(node('g', 'grab', 'cap'), context)
|
||||||
assert result.event == 'failed'
|
assert result.event == 'failed'
|
||||||
assert phases(motion) == ['approach']
|
assert phases(motion) == ['approach']
|
||||||
@@ -253,10 +254,11 @@ def test_grab_restores_target_when_lowering_fails():
|
|||||||
)
|
)
|
||||||
|
|
||||||
adapter = RecordingScene()
|
adapter = RecordingScene()
|
||||||
registry, context, motion, _ = make_registry(
|
registry, context, motion, supervisor = make_registry(
|
||||||
motion=FailOnLower(), adapter=adapter,
|
motion=FailOnLower(), adapter=adapter,
|
||||||
)
|
)
|
||||||
context.mounted_tool = 'vacuum'
|
context.mounted_tool = 'vacuum'
|
||||||
|
supervisor.state['mounted_tool'] = 'vacuum'
|
||||||
result = registry.execute(node('g', 'grab', 'cap'), context)
|
result = registry.execute(node('g', 'grab', 'cap'), context)
|
||||||
assert result.event == 'failed'
|
assert result.event == 'failed'
|
||||||
assert phases(motion) == ['approach', 'lower']
|
assert phases(motion) == ['approach', 'lower']
|
||||||
@@ -292,11 +294,46 @@ def test_explicit_phases_are_not_repeated_by_grab():
|
|||||||
assert phases(motion) == ['approach', 'lower', 'retreat']
|
assert phases(motion) == ['approach', 'lower', 'retreat']
|
||||||
|
|
||||||
|
|
||||||
def test_grab_cap_without_vacuum_does_not_move():
|
def test_grab_cap_without_vacuum_reports_grasp_failed_without_motion_or_command():
|
||||||
registry, context, motion, _ = make_registry()
|
registry, context, motion, supervisor = make_registry()
|
||||||
|
before = copy.deepcopy(supervisor.state)
|
||||||
result = registry.execute(node('g', 'grab', 'cap'), context)
|
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 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():
|
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():
|
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'
|
context.mounted_tool = 'sealant_tool'
|
||||||
|
supervisor.state['mounted_tool'] = 'sealant_tool'
|
||||||
|
supervisor.state['tools']['sealant_tool']['state'] = 'mounted'
|
||||||
result = registry.execute(
|
result = registry.execute(
|
||||||
node('d', 'dispense', 'side_1', params={
|
node('d', 'dispense', 'side_1', params={
|
||||||
'start_fraction': 0.0, 'end_fraction': 1.0,
|
'start_fraction': 0.0, 'end_fraction': 1.0,
|
||||||
|
|||||||
@@ -111,15 +111,64 @@ def test_blocked_status_never_reaches_execution():
|
|||||||
assert result.code == 'blocked'
|
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(
|
value = graph(
|
||||||
'grab_cap',
|
'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))
|
result = expect(value, scene_for(1))
|
||||||
assert result.ok is False
|
assert result.ok is False
|
||||||
assert result.code == 'replan_required'
|
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():
|
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
|
assert result.ok is True
|
||||||
|
|
||||||
|
|
||||||
def test_dispense_requires_the_sealant_tool():
|
def test_dispense_requires_the_sealant_tool_at_runtime():
|
||||||
without = graph(
|
without = graph(
|
||||||
'dispense', node('dispense', 'dispense', 'side_1',
|
'dispense', node('dispense', 'dispense', 'side_1',
|
||||||
next_on={'success': 'completed'}),
|
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(
|
with_tool = graph(
|
||||||
'grab_sealant',
|
'grab_sealant',
|
||||||
node('grab_sealant', 'grab', 'sealant_tool',
|
node('grab_sealant', 'grab', 'sealant_tool',
|
||||||
|
|||||||
Reference in New Issue
Block a user