From a59a3f75b2aad50437a34ce53dad9de2c1c211df 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: Sun, 8 Nov 2026 13:14:23 +1000 Subject: [PATCH] feat(experement): define reference experiment contract without tightening --- src/iiwa_experement/config/scenes.yaml | 39 ++++++--- src/iiwa_experement/doc/README.md | 12 +-- src/iiwa_experement/doc/experements.md | 69 +++++++-------- src/iiwa_experement/doc/experiment-scenes.md | 24 +++--- .../iiwa_experement/scenario.py | 67 +++++++++++++-- .../test/test_package_layout.py | 21 +++-- src/iiwa_experement/test/test_scenario.py | 83 ++++++++++++++++--- src/iiwa_experement/test/test_webots_nodes.py | 9 +- 8 files changed, 237 insertions(+), 87 deletions(-) diff --git a/src/iiwa_experement/config/scenes.yaml b/src/iiwa_experement/config/scenes.yaml index e50d280..481e72c 100644 --- a/src/iiwa_experement/config/scenes.yaml +++ b/src/iiwa_experement/config/scenes.yaml @@ -113,66 +113,85 @@ experiments: "6": cap: absent tools: [sealant_tool] + sealant: [side_1] "7": cap: absent tools: [sealant_tool] + sealant: [side_1, side_2] "8": cap: absent tools: [sealant_tool] + sealant: [side_1, side_2, side_3, side_4] "9": cap: absent tools: [sealant_tool] + sealant: [side_1, side_2, side_3, side_4] failures: - {skill: dispense, outcome: interrupted, target: side_3, attempt: 1} "10": cap: free tools: [sealant_tool, vacuum] + sealant: [side_1, side_2, side_3, side_4] "11": cap: installed tray: true tools: [magnetic_tool] + screw_holes: [hole_1] "12": cap: installed tray: true tools: [magnetic_tool] + screw_holes: [hole_1, hole_3] "13": cap: installed tray: true tools: [magnetic_tool] + screw_holes: [hole_1, hole_2, hole_3] "14": cap: installed tray: true - tools: [magnetic_tool, torque_tool] + tools: [magnetic_tool] + screw_holes: [hole_1, hole_3, hole_2, hole_4] "15": cap: installed tray: true - tools: [magnetic_tool, torque_tool] + tools: [magnetic_tool] + screw_holes: [hole_1, hole_3, hole_2, hole_4] failures: - {skill: grab, outcome: grasp_failed, target: third_selected_screw, attempt: 1} - - {skill: tighten, outcome: torque_not_reached, target: hole_4, attempt: 1} "16": cap: free tray: true - tools: [vacuum, magnetic_tool, sealant_tool, torque_tool] + tools: [vacuum, magnetic_tool, sealant_tool] + sealant: [side_1] + screw_holes: [hole_1] "17": cap: free tray: true - tools: [vacuum, magnetic_tool, sealant_tool, torque_tool] + tools: [vacuum, magnetic_tool, sealant_tool] + sealant: [side_1, side_2, side_3, side_4] + screw_holes: [hole_1, hole_3] "18": cap: free tray: true - tools: [vacuum, magnetic_tool, sealant_tool, torque_tool] + tools: [vacuum, magnetic_tool, sealant_tool] + sealant: [side_1, side_2, side_3, side_4] + screw_holes: [hole_1, hole_3, hole_2, hole_4] "19": cap: free cap_pose: {x: 0.37, y: -0.20, z: 0.790667, yaw_deg: 60} tray: true tray_pose: {x: 0.18, y: 0.48, z: 0.793, yaw_deg: 0} - tools: [vacuum, magnetic_tool, sealant_tool, torque_tool] + tools: [vacuum, magnetic_tool, sealant_tool] + sealant: [side_1, side_2, side_3, side_4] + screw_holes: [hole_1, hole_3, hole_2, hole_4] "20": cap: free tray: true - tools: [vacuum, magnetic_tool, sealant_tool, torque_tool] + tools: [vacuum, magnetic_tool, sealant_tool] + sealant: [side_1, side_2, side_3, side_4] + screw_holes: [hole_1, hole_3, hole_2, hole_4] failures: - - {skill: dispense, outcome: interrupted, attempt: 1} - - {skill: grab, outcome: grasp_failed, attempt: 1} + - {skill: dispense, outcome: interrupted, target: side_3, attempt: 1} + - {skill: grab, outcome: grasp_failed, target: first_selected_screw, attempt: 1} - {skill: verify, outcome: not_ok, target: cap, attempt: 1} diff --git a/src/iiwa_experement/doc/README.md b/src/iiwa_experement/doc/README.md index c476c4d..4679fee 100644 --- a/src/iiwa_experement/doc/README.md +++ b/src/iiwa_experement/doc/README.md @@ -6,8 +6,11 @@ `iiwa_bringup` запускает Webots, робота iiwa 7, контроллеры и камеры. После этого `scene_spawner` добавляет объекты выбранного сценария. -Реализована только начальная сцена. Захват, вставка инструмента в `patron`, -нанесение герметика, установка и затяжка винтов в пакет не входят. +В режиме `execute` эталонный исполнитель перемещает робота, вставляет рабочие +инструменты в постоянный `patron`, переносит люк, проводит сопло по контуру и +передаёт винты виртуальному оператору. Винты не вставляются и не затягиваются; +отверстия остаются пустыми. Режим `view` (по умолчанию) оставляет сцену +неподвижной. ## Сборка @@ -71,9 +74,8 @@ ros2 service call /iiwa_experement/scene_ready std_srvs/srv/Trigger {} | 1–5 | свободный | нет | присоска | | 6–9 | отсутствует | нет | герметик | | 10 | свободный | нет | герметик, присоска | -| 11–13 | установлен | 10 винтов | магнитный | -| 14–15 | установлен | 10 винтов | магнитный, затяжной | -| 16–20 | свободный | 10 винтов | присоска, магнитный, герметик, затяжной | +| 11–15 | установлен | 10 винтов | магнитный | +| 16–20 | свободный | 10 винтов | присоска, магнитный, герметик | Во всех опытах `patron` уже находится на роботе: он включён в URDF через `urdf/tools/tool_active.xacro`. Рабочие инструменты лежат отдельно в четырёх diff --git a/src/iiwa_experement/doc/experements.md b/src/iiwa_experement/doc/experements.md index 5e0fbe7..52156b1 100644 --- a/src/iiwa_experement/doc/experements.md +++ b/src/iiwa_experement/doc/experements.md @@ -2,6 +2,8 @@ Цель набора — проверить, как LLM-планировщик преобразует пользовательский запрос и `scene_json` в граф действий `skill` с переходами по событиям. В наборе 20 экспериментов: четыре категории по пять заданий. Сложность внутри каждой категории возрастает. +Эталонное исполнение этих заданий описано в [спецификации](../../../docs/superpowers/specs/2026-11-08-grant-reference-control-design.md) и [плане](../../../docs/superpowers/plans/2026-11-08-grant-reference-control.md) репозитория. Винты в эталоне не вставляются и не затягиваются: робот подносит винт к отверстию и передаёт его виртуальному оператору, отверстие остаётся пустым. Затяжной инструмент в эталонных маршрутах не используется. + Эксперименты опираются на технологическую карту «ТехнКарта вер. 1.0.xlsx» и модели из `src/iiwa_description/objects/grant`. В сцене работает один робот-манипулятор. Корпус закреплён в стапеле; оператора, мобильной платформы и вспомогательного робота нет. Отверстия для четырёх винтов считаются подготовленными, поскольку модели сверлильного шпинделя среди доступных объектов нет. Во всех экспериментах на роботе постоянно установлен `patron`: рабочие инструменты вставляются в него. После установки люк удерживается виртуальной фиксацией: это позволяет роботу отпустить присоску и сменить рабочий инструмент. Начальные положения объектов описаны отдельно в [experiment-scenes.md](experiment-scenes.md). ## Контракт для экспериментов @@ -14,8 +16,7 @@ | --- | --- | --- | | `dispense` | Пройти соплом именованный участок контура с условной подачей герметика | `completed`, `interrupted`, `failed` | | `press` | Прижать установленный люк в заданной зоне | `success`, `failed` | -| `seat_screw` | Подтвердить, что винт после осевого опускания остался в отверстии | `seated`, `not_seated`, `failed` | -| `tighten` | Затянуть конкретный винт до момента, заданного в сцене | `success`, `torque_not_reached`, `failed` | +| `handoff_screw` | Поднести винт к отверстию и передать его виртуальному оператору | `delivered`, `grasp_failed`, `pose_out_of_tolerance` | | `dock_tool` | Вернуть инструмент в его гнездо и отсоединить от робота | `success`, `failed` | Контракты `verify` и `inspect_state` нужно расширить на контур герметика и винты. Для тестов достаточно состояния симулятора; `capture` не входит в основные маршруты. Предлагаемые идентификаторы: `cap`, `hatch_seat`, `body`, `sealant_path`, `side_1`…`side_4`, `hole_1`…`hole_4`, `screw_1`…`screw_10`, `screw_tray`, `patron`, `vacuum`, `sealant_tool`, `magnetic_tool`, `torque_tool`. @@ -128,77 +129,77 @@ **Проверка плана:** присоска не берётся, пока инструмент герметика не возвращён; люк не опускается на неполный контур. -## III. Подача винтов и затяжка +## III. Подача винтов виртуальному оператору -В этих сценах люк уже установлен и зафиксирован, а четыре отверстия подготовлены. В лотке всегда находятся десять одинаковых винтов. Для задания можно выбрать любой доступный винт; проверка плана должна требовать нужное число **разных** винтов, а не заранее заданные номера. Магнитный инструмент переносит винт; `seat_screw` подтверждает, что винт остаётся в отверстии после отвода магнита. +В этих сценах люк уже установлен и зафиксирован, а четыре отверстия подготовлены. В лотке всегда находятся десять одинаковых винтов. Для задания можно выбрать любой доступный винт; проверка плана должна требовать нужное число **разных** винтов, а не заранее заданные номера. Магнитный инструмент переносит винт к конкретному отверстию. Винт не вставляется и не затягивается: после подтверждённой позы передачи робот отпускает винт виртуальному оператору, винт удаляется из сцены, а отверстие остаётся пустым. Затяжной инструмент в эталонных маршрутах не используется. ### 11. Один винт -**Запрос:** «Возьми винт из лотка и установи его в первое отверстие люка». +**Запрос:** «Возьми винт из лотка и поднеси его к первому отверстию люка для передачи оператору». -**Действия робота:** взять магнитный инструмент → найти один доступный винт в лотке → подойти и опуститься к нему → захватить и поднять → найти `hole_1` → перенести винт над отверстием → совместить оси → опустить → выполнить `seat_screw` → отпустить винт → отвести инструмент → проверить наличие винта в отверстии → вернуть инструмент → домой. +**Действия робота:** взять магнитный инструмент → найти один доступный винт в лотке → подойти и опуститься к нему → захватить и поднять → найти `hole_1` → перенести винт над отверстием → совместить оси → коротко опуститься до точки передачи → отпустить винт виртуальному оператору → отвести инструмент по оси → вернуть инструмент → домой. -**Проверка плана:** `release` невозможен до подтверждённой посадки винта. +**Проверка плана:** передача невозможна до подтверждённого захвата винта и достижения позы передачи у нужного отверстия; отверстие после передачи остаётся пустым. ### 12. Два винта по диагонали -**Запрос:** «Установи два винта в диагонально противоположные отверстия люка». +**Запрос:** «Передай два винта в диагонально противоположные отверстия люка». -**Действия робота:** взять магнитный инструмент → установить первый винт по цепочке «найти → захватить → поднять → перенести → совместить → опустить → посадить → отпустить» → подняться над люком → вернуться к лотку → повторить для диагонально противоположного отверстия → проверить обе точки → вернуть инструмент → домой. +**Действия робота:** взять магнитный инструмент → передать первый винт по цепочке «найти → захватить → поднять → перенести → совместить → опустить → передать → отвести» → подняться над люком → вернуться к лотку → повторить для диагонально противоположного отверстия → проверить обе передачи → вернуть инструмент → домой. **Проверка плана:** второму винту соответствуют другая ячейка лотка и другое отверстие. -### 13. Четыре винта крестовым порядком +### 13. Три винта в разные отверстия -**Запрос:** «Установи четыре винта в отверстия люка крестовым порядком». +**Запрос:** «Передай три разных винта в три разных отверстия люка». -**Действия робота:** взять магнитный инструмент → последовательно взять винты из четырёх ячеек и установить их в порядке `hole_1 → hole_3 → hole_2 → hole_4`; перед каждым переносом подтвердить захват, перед отпусканием — посадку → проверить наличие четырёх винтов → вернуть инструмент → домой. +**Действия робота:** взять магнитный инструмент → последовательно взять три винта из разных ячеек и передать их в отверстия `hole_1`, `hole_2`, `hole_3`; перед каждым переносом подтвердить захват, перед передачей — достигнутую позу → проверить три пары винт-отверстие → вернуть инструмент → домой. -**Проверка плана:** нет повторного использования ячейки или отверстия; после каждой установки робот возвращается к лотку с пустым магнитным захватом. +**Проверка плана:** нет повторного использования ячейки или отверстия; после каждой передачи робот возвращается к лотку с пустым магнитным захватом. -### 14. Установка и затяжка четырёх винтов +### 14. Четыре винта крестовым порядком -**Запрос:** «Установи четыре винта крестовым порядком, затем затяни каждый до заданного момента». +**Запрос:** «Передай четыре винта в отверстия люка в порядке `hole_1 → hole_3 → hole_2 → hole_4`». -**Действия робота:** выполнить четыре установки как в эксперименте 13 → вернуть магнитный инструмент → взять инструмент затяжки → для каждого из четырёх винтов подойти сверху, совместить инструмент с осью, опуститься, выполнить `tighten`, отвести инструмент → проверить четыре результата затяжки → вернуть инструмент → домой. +**Действия робота:** выполнить четыре передачи в крестовом порядке, каждый раз подтверждая захват нового винта и позу передачи у нужного отверстия → проверить четыре пары винт-отверстие → вернуть магнитный инструмент → домой. -**Проверка плана:** `tighten` не вызывается для винта без подтверждённого `seat_screw`. +**Проверка плана:** каждый винт используется ровно один раз; затяжной инструмент не берётся. -### 15. Ошибка захвата и недостигнутый момент +### 15. Ошибка захвата винта -**Запрос:** «Установи и затяни четыре винта. Если один винт не захватится или затяжка не достигнет нужного момента, повтори соответствующее действие один раз». +**Запрос:** «Передай четыре винта; если один винт не захватится с первого раза, проверь состояние и повтори захват один раз». -**Условия:** первый захват третьего выбранного винта даёт `grasp_failed`; первая затяжка в `hole_4` даёт `torque_not_reached`. +**Условия:** первый захват третьего выбранного винта даёт `grasp_failed`; разрешена одна повторная попытка. -**Действия робота:** установить первые два винта → на третьем остановиться, проверить состояние захвата и повторно обнаружить винт → повторить его захват один раз → установить третий и четвёртый → сменить магнитный инструмент на затяжной → затянуть четыре винта; для четвёртого после проверки состояния повторить затяжку один раз → проверить все четыре точки → вернуть инструмент → домой. +**Действия робота:** передать первые два винта → на третьем остановиться, проверить состояние захвата и повторно обнаружить винт → повторить его захват один раз → передать третий и четвёртый винты → проверить четыре пары → вернуть инструмент → домой. -**Проверка плана:** ошибки обрабатываются у соответствующего винта; уже установленные винты не вынимаются и не устанавливаются повторно. +**Проверка плана:** ошибка обрабатывается у соответствующего винта; уже переданные винты не возвращаются в сцену; повторный отказ ведёт к остановке. ## IV. Полная сборка и перепланирование ### 16. Герметик, люк и один винт -**Запрос:** «Нанеси герметик на заданный участок, установи люк и закрепи его одним винтом». +**Запрос:** «Нанеси герметик на заданный участок, установи люк и передай один винт к отверстию». -**Действия робота:** взять инструмент герметика → обработать участок → вернуть инструмент → взять присоску → захватить, перенести, совместить, опустить и отпустить люк → проверить его положение → вернуть присоску → взять магнитный инструмент → установить один винт → вернуть магнит → взять инструмент затяжки → затянуть винт → вернуть инструмент → домой. +**Действия робота:** взять инструмент герметика → обработать участок → вернуть инструмент → взять присоску → захватить, перенести, совместить, опустить и отпустить люк → проверить его положение → вернуть присоску → взять магнитный инструмент → перенести один винт к отверстию и передать оператору → вернуть инструмент → домой. -**Проверка плана:** зависимости «герметик → люк → винт → затяжка» соблюдены. +**Проверка плана:** зависимости «герметик → люк → винт» соблюдены; винт не устанавливается. -### 17. Полный контур и два крепления +### 17. Полный контур и две передачи -**Запрос:** «Нанеси полный контур герметика, установи люк и закрепи его двумя диагональными винтами». +**Запрос:** «Нанеси полный контур герметика, установи люк и передай два диагональных винта». -**Действия робота:** пройти четыре участка герметика и проверить контур → вернуть инструмент → присоской установить и проверить люк → вернуть присоску → магнитным инструментом установить два диагональных винта → вернуть магнит → затянуть оба винта → проверить две затяжки → вернуть инструмент → домой. +**Действия робота:** пройти четыре участка герметика и проверить контур → вернуть инструмент → присоской установить и проверить люк → вернуть присоску → магнитным инструментом передать два диагональных винта → проверить две пары → вернуть инструмент → домой. **Проверка плана:** если контур не подтверждён, план не переходит к установке люка. ### 18. Полная сборка -**Запрос:** «Нанеси герметик по контуру, установи люк, поставь четыре винта крестовым порядком и затяни их». +**Запрос:** «Нанеси герметик по контуру, установи люк и передай четыре винта крестовым порядком». -**Действия робота:** полный контур герметика → проверка → смена на присоску → захват и установка люка → проверка → смена на магнит → четыре установки винтов → смена на инструмент затяжки → четыре затяжки → итоговая проверка люка и крепежа → вернуть инструмент → домой. +**Действия робота:** полный контур герметика → проверка → смена на присоску → захват и установка люка → проверка → смена на магнит → четыре передачи винтов → итоговая проверка люка и пар винт-отверстие → вернуть инструмент → домой. -**Проверка плана:** у каждого из четырёх винтов есть собственные узлы установки и затяжки; итоговая проверка охватывает всю сборку. +**Проверка плана:** у каждого из четырёх винтов есть собственные узлы захвата и передачи; итоговая проверка охватывает всю сборку. ### 19. Полная сборка при смещённых деталях @@ -206,7 +207,7 @@ **Условия:** симулятор смещает люк и лоток в допустимой рабочей области; `detect` возвращает их актуальные позы. -**Действия робота:** нанести и проверить герметик → обнаружить фактическую позу люка, захватить и установить его → проверить → обнаружить фактическую позу лотка и по очереди найти четыре винта → установить их → затянуть → проверить сборку → вернуть инструмент → домой. +**Действия робота:** нанести и проверить герметик → обнаружить фактическую позу люка, захватить и установить его → проверить → обнаружить фактическую позу лотка и по очереди найти четыре винта → передать их в четыре отверстия → проверить сборку → вернуть инструмент → домой. **Проверка плана:** план не подставляет прежние координаты люка и винтов после нового `detect`; зависимости операций сохраняются. @@ -214,9 +215,9 @@ **Запрос:** «Выполни полную сборку. При прерывании подачи герметика, неудачном захвате винта или ошибке установки люка восстанови работу, если состояние известно и разрешена одна повторная попытка». -**Условия:** один раз прерывается `dispense`, первый захват одного винта неудачен, первая проверка положения люка возвращает `not_ok`. +**Условия:** один раз прерывается `dispense` на `side_3`, первый захват одного винта неудачен, первая проверка положения люка возвращает `not_ok`. -**Действия робота:** начать контур → определить необработанный отрезок и завершить его → проверить контур → установить люк → при `not_ok` заново обнаружить и один раз переставить его → проверить → устанавливать винты; при `grasp_failed` остановиться, проверить состояние и один раз повторить захват нужного винта → затянуть четыре винта → проверить четыре установки, четыре затяжки и положение люка → вернуть инструмент → домой. +**Действия робота:** начать контур → определить необработанный отрезок и завершить его с последней подтверждённой точки → проверить контур → установить люк → при `not_ok` заново обнаружить и один раз переставить его → проверить → передавать винты; при `grasp_failed` остановиться, проверить состояние и один раз повторить захват нужного винта → проверить четыре пары винт-отверстие и положение люка → вернуть инструмент → домой. **Проверка плана:** восстановление начинается от подтверждённого состояния; лимит попыток соблюдён; любое `unknown` или повторный отказ ведёт к остановке и уведомлению. diff --git a/src/iiwa_experement/doc/experiment-scenes.md b/src/iiwa_experement/doc/experiment-scenes.md index 0c88df2..39b361e 100644 --- a/src/iiwa_experement/doc/experiment-scenes.md +++ b/src/iiwa_experement/doc/experiment-scenes.md @@ -42,7 +42,7 @@ | `screw_4` | `(0.240, 0.475, 0.7954)` | `screw_9` | `(0.200, 0.475, 0.7954)` | | `screw_5` | `(0.180, 0.445, 0.7954)` | `screw_10` | `(0.220, 0.475, 0.7954)` | -Все десять винтов взаимозаменяемы. Для установки в четыре отверстия планировщик может выбрать **любые четыре разных доступных винта**. Оценка графа не должна требовать конкретные номера `screw_i`. В опыте 15 отказ задаётся третьей попытке захвата винта, который план выбрал третьим; отказ затяжки задаётся отверстию `hole_4`. +Все десять винтов взаимозаменяемы. Для передачи к отверстиям планировщик может выбрать **любые разные доступные винты** в нужном количестве. Оценка графа не должна требовать конкретные номера `screw_i`. В опыте 15 отказ задаётся первой попытке захвата винта, который план выбрал третьим. Винты не вставляются и не затягиваются: отверстия после передачи остаются пустыми. При стандартной позе корпуса предварительные центры отверстий, полученные из геометрии люка, таковы: @@ -69,16 +69,16 @@ | 8 | Стандартные | Нет | Нет | Герметик | Обрабатывается полный `sealant_path` | | 9 | Стандартные | Нет | Нет | Герметик | На `side_3` при выполнении прервётся подача | | 10 | Стандартные | Стандартный свободный | Нет | Герметик, присоска | Герметик, затем установка люка | -| 11 | Стандартные | Уже установлен `(0.30, 0.12, 0.82594)`, 45° | Стандартный лоток, **10 винтов** | Магнитный | Установить один из десяти винтов | -| 12 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Установить два разных винта | -| 13 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Установить четыре разных винта | -| 14 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный, затяжной | Установить и затянуть четыре винта | -| 15 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный, затяжной | Сбой третьего захвата и первой затяжки `hole_4` | -| 16 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Участок герметика, люк, один винт | -| 17 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Полный контур, люк, два винта | -| 18 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Полная сборка | -| 19 | Стандартные | Свободный `(0.37, −0.20, 0.790667)`, **60°** | Лоток `(0.18, 0.48, 0.793)`, **10 винтов** | Все четыре | Координаты всех десяти винтов смещены вместе с лотком на `(−0.02, +0.02)` | -| 20 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Исходная геометрия обычная; сбои вводятся во время выполнения | +| 11 | Стандартные | Уже установлен `(0.30, 0.12, 0.82594)`, 45° | Стандартный лоток, **10 винтов** | Магнитный | Передать один винт у `hole_1` | +| 12 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Передать два винта в диагональные отверстия | +| 13 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Передать три разных винта в `hole_1`, `hole_2`, `hole_3` | +| 14 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Передать четыре винта в порядке `hole_1 → hole_3 → hole_2 → hole_4` | +| 15 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Те же четыре передачи; сбой первого захвата третьего винта | +| 16 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик | Участок герметика, люк, одна передача винта | +| 17 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик | Полный контур, люк, две диагональные передачи | +| 18 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик | Полная сборка, четыре передачи | +| 19 | Стандартные | Свободный `(0.37, −0.20, 0.790667)`, **60°** | Лоток `(0.18, 0.48, 0.793)`, **10 винтов** | Присоска, магнитный, герметик | Полная сборка; координаты всех десяти винтов смещены вместе с лотком на `(−0.02, +0.02)` | +| 20 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик | Полная сборка; прерывание герметика, ошибка первой посадки люка, неудачный захват винта | В опыте 3 смещается вся пара «стапель + корпус» без изменения взаимной высоты и поворота. Координаты `hatch_seat` и четырёх отверстий сдвигаются вместе с ними на `(−0.01, +0.02)`. В опыте 19 сдвигается лоток **со всеми десятью винтами**; ни один винт не остаётся в старой точке. @@ -89,3 +89,5 @@ По габаритам OBJ стапель занимает в плане примерно `X = 0.0985…0.5015`, `Y = −0.0815…0.3215`; стандартный свободный люк — `X = 0.2663…0.4337`, `Y = −0.3037…−0.1363`; стойка — `X = −0.09…0.33`, `Y = −0.49…−0.35`; лоток — `X = 0.14…0.26`, `Y = 0.425…0.495`. Эти прямоугольники не пересекаются. Лоток перенесён к левому краю стола со стороны `+Y`, чтобы все винты были в достижимой зоне; зазор от лотка до стапеля по Y около 104 мм. Зазор от свободного люка до стойки по Y около 46 мм, до стапеля около 55 мм. Это проверка начальной компоновки, не проверка траекторий инструмента. Перед реализацией контактных действий нужно проверить в Webots достижимость TCP и коллизии при подъёме люка, при извлечении инструмента из гнезда и при работе между десятью винтами. В `iiwa.wbt` трение `TableMaterial` равно нулю; лоток и винты понадобится удерживать до захвата или задать подходящую контактную модель. Действующий спавнер `object_spawner.py` создаёт случайные OBJ, поэтому для этой таблицы потребуется детерминированная загрузка по номеру эксперимента. Стойка должна содержать отдельно управляемые рабочие инструменты; удалённый `GrantToolRack.proto` для нового мира не используется. + +В эталонных маршрутах затяжной инструмент `torque_tool` не участвует: модель может оставаться в `ASSETS`, но не появляется в опытах 1–20. Численные преобразования, допуски и контур герметика описаны в [геометрии](../config/geometry.yaml) и [спецификации](../../../docs/superpowers/specs/2026-11-08-grant-reference-control-design.md). diff --git a/src/iiwa_experement/iiwa_experement/scenario.py b/src/iiwa_experement/iiwa_experement/scenario.py index 360b46e..395ae43 100644 --- a/src/iiwa_experement/iiwa_experement/scenario.py +++ b/src/iiwa_experement/iiwa_experement/scenario.py @@ -17,6 +17,8 @@ import yaml CAP_STATES = ('absent', 'free', 'installed') TOOL_NAMES = ('vacuum', 'magnetic_tool', 'sealant_tool', 'torque_tool') EXPERIMENT_IDS = tuple(range(1, 21)) +SEALANT_SIDES = ('side_1', 'side_2', 'side_3', 'side_4') +SCREW_HOLES = ('hole_1', 'hole_2', 'hole_3', 'hole_4') # Bounds of lotok.obj after its local Y axis is mapped to Webots Z. They are # repeated here so validation does not need a Webots process or the mesh file. @@ -24,15 +26,13 @@ TRAY_HALF_LENGTH = 0.060 TRAY_HALF_WIDTH = 0.035 SCREW_RADIUS = 0.00275 -# Failure metadata is consumed by a future skill executor. It never changes -# the initial pose of an object. +# Failure metadata is consumed by the reference executor. It never changes +# the initial pose of an object. Screws are handed to the virtual operator: +# there is deliberately no tightening or seating failure. _ALLOWED_FAILURES = { 'grab': {'grasp_failed'}, 'verify': {'not_ok'}, 'dispense': {'interrupted'}, - 'tighten': {'torque_not_reached'}, - 'seat_screw': {'not_seated'}, - 'press': {'failed'}, } @@ -95,11 +95,18 @@ class ScenarioSpec: tray_pose: Pose | None screws: tuple[Screw, ...] failures: tuple[Failure, ...] + sealant_sides: tuple[str, ...] = () + screw_holes: tuple[str, ...] = () @property def tool_names(self) -> tuple[str, ...]: return tuple(tool.name for tool in self.tools) + @property + def delivered_screw_count(self) -> int: + """Number of screws handed to the operator in this experiment.""" + return len(self.screw_holes) + def _mapping(value: Any, label: str) -> Mapping[str, Any]: if not isinstance(value, Mapping): @@ -270,6 +277,28 @@ def _screws( return tuple(result) +def _named_list( + raw: Any, + label: str, + allowed: tuple[str, ...], + experiment_id: int, +) -> tuple[str, ...]: + if raw is None: + return () + if not isinstance(raw, list): + raise ScenarioError(f'{label} must be a list') + result: list[str] = [] + for index, name in enumerate(raw): + if not isinstance(name, str) or name not in allowed: + raise ScenarioError( + f'{label}[{index}] must be one of {list(allowed)}' + ) + if name in result: + raise ScenarioError(f'{label} contains duplicate {name!r}') + result.append(name) + return tuple(result) + + def _failures(raw: Any, experiment_id: int) -> tuple[Failure, ...]: if raw is None: return () @@ -392,6 +421,32 @@ def load_scenario(experiment_id: int, manifest_path: Path | str) -> ScenarioSpec raise ScenarioError(f'experiment {experiment_id}: tray_pose requires tray: true') tray_pose = None + sealant_sides = _named_list( + entry.get('sealant'), f'experiments.{experiment_id}.sealant', + SEALANT_SIDES, experiment_id, + ) + if sealant_sides and 'sealant_tool' not in entry.get('tools', []): + raise ScenarioError( + f'experiment {experiment_id}: sealant sides require the sealant_tool' + ) + + screw_holes = _named_list( + entry.get('screw_holes'), f'experiments.{experiment_id}.screw_holes', + SCREW_HOLES, experiment_id, + ) + if tray_pose is None and screw_holes: + raise ScenarioError( + f'experiment {experiment_id}: screw holes require a screw tray' + ) + if tray_pose is not None and not screw_holes: + raise ScenarioError( + f'experiment {experiment_id}: a screw tray requires screw_holes' + ) + if screw_holes and 'magnetic_tool' not in entry.get('tools', []): + raise ScenarioError( + f'experiment {experiment_id}: screw holes require the magnetic_tool' + ) + return ScenarioSpec( experiment_id=experiment_id, fixture=fixture, @@ -403,4 +458,6 @@ def load_scenario(experiment_id: int, manifest_path: Path | str) -> ScenarioSpec tray_pose=tray_pose, screws=_screws(entry, data, experiment_id, tray_pose), failures=_failures(entry.get('failures'), experiment_id), + sealant_sides=sealant_sides, + screw_holes=screw_holes, ) diff --git a/src/iiwa_experement/test/test_package_layout.py b/src/iiwa_experement/test/test_package_layout.py index a930548..a8d1292 100644 --- a/src/iiwa_experement/test/test_package_layout.py +++ b/src/iiwa_experement/test/test_package_layout.py @@ -24,16 +24,21 @@ def test_package_declares_its_ros_dependencies(): assert f'{dependency}' in manifest -def test_versioned_specifications_match_the_approved_copies(): +def test_package_documentation_points_at_the_reference_specification(): root = PACKAGE.parents[1] - pairs = ( - ('docs/superpowers/specs/2026-11-08-grant-experiments.md', 'doc/experements.md'), - ('docs/superpowers/specs/2026-11-08-grant-scenes.md', 'doc/experiment-scenes.md'), + reference = ( + root + / 'docs' + / 'superpowers' + / 'specs' + / '2026-11-08-grant-reference-control-design.md' ) - for source, installed in pairs: - assert (PACKAGE / installed).read_text(encoding='utf-8') == ( - root / source - ).read_text(encoding='utf-8') + assert reference.is_file() + for relative in ('doc/experements.md', 'doc/experiment-scenes.md'): + text = (PACKAGE / relative).read_text(encoding='utf-8') + assert '2026-11-08-grant-reference-control-design.md' in text + assert 'tighten' not in text.lower() or 'не затяг' in text.lower() + assert 'seat_screw' not in text def test_expected_resources_are_installed(): diff --git a/src/iiwa_experement/test/test_scenario.py b/src/iiwa_experement/test/test_scenario.py index 9f6dbfc..cca1b27 100644 --- a/src/iiwa_experement/test/test_scenario.py +++ b/src/iiwa_experement/test/test_scenario.py @@ -88,18 +88,52 @@ def test_screw_experiments_have_installed_hatch_and_ten_screws(): assert spec.cap_state == 'installed' assert spec.tray_pose is not None assert len(spec.screws) == 10 - assert spec.tool_names[0] == 'magnetic_tool' - assert set(scenario(14).tool_names) == {'magnetic_tool', 'torque_tool'} - assert set(scenario(15).tool_names) == {'magnetic_tool', 'torque_tool'} + assert spec.tool_names == ('magnetic_tool',) + assert 'torque_tool' not in spec.tool_names + assert len(spec.screw_holes) == { + 11: 1, 12: 2, 13: 3, 14: 4, 15: 4, + }[experiment_id] -def test_full_assembly_experiments_have_loose_hatch_tray_and_all_tools(): +def test_full_assembly_experiments_have_loose_hatch_tray_and_no_torque_tool(): for experiment_id in range(16, 21): spec = scenario(experiment_id) assert spec.cap_state == 'free' assert spec.tray_pose is not None assert len(spec.screws) == 10 - assert set(spec.tool_names) == STANDARD_TOOLS + assert set(spec.tool_names) == {'vacuum', 'magnetic_tool', 'sealant_tool'} + assert 'torque_tool' not in spec.tool_names + assert len(spec.screw_holes) == {16: 1, 17: 2, 18: 4, 19: 4, 20: 4}[experiment_id] + + +def test_sealant_coverage_matches_each_experiment(): + assert scenario(6).sealant_sides == ('side_1',) + assert scenario(7).sealant_sides == ('side_1', 'side_2') + for experiment_id in (8, 9, 10, 17, 18, 19, 20): + assert scenario(experiment_id).sealant_sides == ( + 'side_1', 'side_2', 'side_3', 'side_4', + ) + assert scenario(16).sealant_sides == ('side_1',) + for experiment_id in (1, 2, 3, 4, 5, 11, 12, 13, 14, 15): + assert scenario(experiment_id).sealant_sides == () + + +def test_screw_holes_are_unique_bounded_and_ordered(): + for experiment_id in range(11, 21): + holes = scenario(experiment_id).screw_holes + assert len(holes) == len(set(holes)) + assert set(holes) <= {'hole_1', 'hole_2', 'hole_3', 'hole_4'} + assert scenario(14).screw_holes == ('hole_1', 'hole_3', 'hole_2', 'hole_4') + assert scenario(15).screw_holes == ('hole_1', 'hole_3', 'hole_2', 'hole_4') + + +def test_reference_experiments_never_request_tightening(): + for experiment_id in range(1, 21): + spec = scenario(experiment_id) + assert all( + failure.skill not in ('tighten', 'seat_screw') + for failure in spec.failures + ) def test_standard_screw_layout_matches_the_specification(): @@ -162,13 +196,12 @@ def test_failure_metadata_for_experiments_four_nine_fifteen_twenty(): fifteen = [(f.skill, f.outcome, f.target) for f in scenario(15).failures] assert fifteen == [ ('grab', 'grasp_failed', 'third_selected_screw'), - ('tighten', 'torque_not_reached', 'hole_4'), ] - twenty = [(f.skill, f.outcome) for f in scenario(20).failures] + twenty = [(f.skill, f.outcome, f.target) for f in scenario(20).failures] assert twenty == [ - ('dispense', 'interrupted'), - ('grab', 'grasp_failed'), - ('verify', 'not_ok'), + ('dispense', 'interrupted', 'side_3'), + ('grab', 'grasp_failed', 'first_selected_screw'), + ('verify', 'not_ok', 'cap'), ] @@ -211,6 +244,8 @@ def test_incorrect_screw_count_is_rejected(tmp_path, count): def mutate(data): data['experiments']['1']['tray'] = True + data['experiments']['1']['tools'] = ['magnetic_tool'] + data['experiments']['1']['screw_holes'] = ['hole_1'] data['experiments']['1']['screws'] = entries with pytest.raises(ScenarioError, match='exactly 10 screws'): @@ -225,6 +260,8 @@ def test_duplicate_screw_name_is_rejected(tmp_path): def mutate(data): data['experiments']['1']['tray'] = True + data['experiments']['1']['tools'] = ['magnetic_tool'] + data['experiments']['1']['screw_holes'] = ['hole_1'] data['experiments']['1']['screws'] = entries with pytest.raises(ScenarioError, match='duplicate screw name'): @@ -240,6 +277,8 @@ def test_screw_outside_the_tray_is_rejected(tmp_path): def mutate(data): data['experiments']['1']['tray'] = True + data['experiments']['1']['tools'] = ['magnetic_tool'] + data['experiments']['1']['screw_holes'] = ['hole_1'] data['experiments']['1']['screws'] = entries with pytest.raises(ScenarioError, match='outside the tray'): @@ -279,3 +318,27 @@ def test_tray_without_screws_and_screws_without_tray_are_rejected(tmp_path): def test_missing_manifest_is_reported(tmp_path): with pytest.raises(FileNotFoundError): load_scenario(1, tmp_path / 'missing.yaml') + + +def test_invalid_sealant_side_is_rejected(tmp_path): + def mutate(data): + data['experiments']['6']['sealant'] = ['side_5'] + + with pytest.raises(ScenarioError, match='sealant'): + load_scenario(6, variant(tmp_path, mutate)) + + +def test_invalid_screw_hole_is_rejected(tmp_path): + def mutate(data): + data['experiments']['11']['screw_holes'] = ['hole_5'] + + with pytest.raises(ScenarioError, match='screw_holes'): + load_scenario(11, variant(tmp_path, mutate)) + + +def test_tray_without_screw_holes_is_rejected(tmp_path): + def mutate(data): + data['experiments']['11'].pop('screw_holes') + + with pytest.raises(ScenarioError, match='requires screw_holes'): + load_scenario(11, variant(tmp_path, mutate)) diff --git a/src/iiwa_experement/test/test_webots_nodes.py b/src/iiwa_experement/test/test_webots_nodes.py index 96f7142..0882701 100644 --- a/src/iiwa_experement/test/test_webots_nodes.py +++ b/src/iiwa_experement/test/test_webots_nodes.py @@ -74,13 +74,13 @@ def test_scenarios_one_six_eleven_sixteen_nineteen_are_complete(): 16: [ 'grant_fixture', 'grant_body', 'grant_rack', 'grant_vacuum', 'grant_magnetic_tool', 'grant_sealant_tool', - 'grant_torque_tool', 'grant_tray', + 'grant_tray', *[f'grant_screw_{i:02d}' for i in range(1, 11)], 'grant_cap', ], 19: [ 'grant_fixture', 'grant_body', 'grant_rack', 'grant_vacuum', 'grant_magnetic_tool', 'grant_sealant_tool', - 'grant_torque_tool', 'grant_tray', + 'grant_tray', *[f'grant_screw_{i:02d}' for i in range(1, 11)], 'grant_cap', ], } @@ -149,9 +149,10 @@ def test_working_tools_are_in_their_own_sockets(): (tool.pose.x, tool.pose.y, tool.pose.z) ) positions = { - translation(parsed[TOOL_NODE_NAMES[name]]) for name in TOOL_NAMES + translation(parsed[TOOL_NODE_NAMES[name]]) for name in scenario.tool_names } - assert len(positions) == 4 + assert len(positions) == len(scenario.tool_names) == 3 + assert 'grant_torque_tool' not in parsed def test_experiment_nineteen_shifts_the_tray_and_all_ten_screws():