feat(experement): complete reference routes, runner and QA documentation

2026-11-08 17:43:57 +10:00
parent de8c2eb9ee
commit ec977c10ba
22 changed files with 1460 additions and 133 deletions
@@ -70,11 +70,11 @@
 **Интерфейс:** `load_scenario(experiment_id, manifest_path) -> ScenarioSpec` остаётся единственным источником начальных поз; сбои описываются в YAML.
- [ ] **Шаг 1. Написать проверки, которые пока не проходят.** В 14–15 и 16–20 нет `torque_tool`; в 11–20 ровно десять винтов. Нет сбоев `tighten` и `seat_screw`. В 15 сохранён `grab:grasp_failed` для третьего выбранного винта. Смещения сцены 3 и 19 сохранены.
+- [x] **Шаг 1. Написать проверки, которые пока не проходят.** В 14–15 и 16–20 нет `torque_tool`; в 11–20 ровно десять винтов. Нет сбоев `tighten` и `seat_screw`. В 15 сохранён `grab:grasp_failed` для третьего выбранного винта. Смещения сцены 3 и 19 сохранены.
- [ ] **Шаг 2. Убедиться, что проверки падают.** `source /opt/ros/jazzy/setup.bash && source install/setup.bash && pytest -q src/iiwa_experement/test/test_scenario.py src/iiwa_experement/test/test_webots_nodes.py`.
+- [x] **Шаг 2. Убедиться, что проверки падают.** `source /opt/ros/jazzy/setup.bash && source install/setup.bash && pytest -q src/iiwa_experement/test/test_scenario.py src/iiwa_experement/test/test_webots_nodes.py`.
- [ ] **Шаг 3. Обновить YAML и описания.** 11 — передать один винт у `hole_1`; 12 — два по диагонали; 13 — три разных отверстия; 14 — четыре в порядке `hole_1→hole_3→hole_2→hole_4`; 15 — те же четыре, отказ третьего захвата и одна повторная попытка. 16 — участок герметика, люк, одна передача; 17 — полный контур, люк, две диагональные передачи; 18 — полный контур, люк, четыре передачи; 19 — то же со смещёнными люком и лотком; 20 — то же с прерыванием герметика, ошибкой первой посадки люка и неудачным захватом винта. Удалить затяжку и ошибку момента из 15. Каталог `TOOL_NAMES` менять только после проверки зависимостей; неиспользуемая модель может остаться в `ASSETS`, но не должна появляться в этих опытах.
+- [x] **Шаг 3. Обновить YAML и описания.** 11 — передать один винт у `hole_1`; 12 — два по диагонали; 13 — три разных отверстия; 14 — четыре в порядке `hole_1→hole_3→hole_2→hole_4`; 15 — те же четыре, отказ третьего захвата и одна повторная попытка. 16 — участок герметика, люк, одна передача; 17 — полный контур, люк, две диагональные передачи; 18 — полный контур, люк, четыре передачи; 19 — то же со смещёнными люком и лотком; 20 — то же с прерыванием герметика, ошибкой первой посадки люка и неудачным захватом винта. Удалить затяжку и ошибку момента из 15. Каталог `TOOL_NAMES` менять только после проверки зависимостей; неиспользуемая модель может остаться в `ASSETS`, но не должна появляться в этих опытах.
- [ ] **Шаг 4. Повторить тесты.** Все тесты сцен проходят; документация пакета не называет переданный винт установленным.
+- [x] **Шаг 4. Повторить тесты.** Все тесты сцен проходят; документация пакета не называет переданный винт установленным.
- [ ] **Шаг 5. Зафиксировать изменение контракта отдельным коммитом.**
+- [x] **Шаг 5. Зафиксировать изменение контракта отдельным коммитом.**
 ### Задача 2. Измерить и описать рабочую геометрию
@@ -82,12 +82,12 @@
 **Интерфейсы:** `load_geometry(path) -> GeometrySpec`; `world_target(spec: ScenarioSpec, geometry: GeometrySpec, name: str) -> Transform`; `tcp_goal(target: Transform, tcp_to_work_tip: Transform) -> Transform`. В `GeometrySpec` хранить `tool_mounts`, `tool_work_tips`, `cap_grip`, `cap_seat`, `sealant_segments`, `holes`, зазоры подхода/передачи/переноса, допуски позы и ограниченные скорости. Единицы и исходный фрейм обязательны для каждого преобразования.
- [ ] **Шаг 1. Написать проверки геометрии.** После масштаба Webots длина инструментов по вертикали 0–0,14 м; у герметика OBJ 0–0,014 м и масштаб ×10. `tcp` расположен на 0,0552 м от `patron`. Десять винтов имеют разные ID. Отверстия смещаются с корпусом в опыте 3, винты с лотком в 19. Проверить равенство `T_world_tcp · T_tcp_mount = T_world_tool_mount`.
+- [x] **Шаг 1. Написать проверки геометрии.** После масштаба Webots длина инструментов по вертикали 0–0,14 м; у герметика OBJ 0–0,014 м и масштаб ×10. `tcp` расположен на 0,0552 м от `patron`. Десять винтов имеют разные ID. Отверстия смещаются с корпусом в опыте 3, винты с лотком в 19. Проверить равенство `T_world_tcp · T_tcp_mount = T_world_tool_mount`.
- [ ] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_geometry.py`.
+- [x] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_geometry.py`.
- [ ] **Шаг 3. Измерить точки в OBJ и Webots.** Найти конец инструмента, входящий в `patron`, рабочий кончик, плоскость захвата люка, высоту посадки, начало/конец и углы контура, оси и высоты отверстий. Записать численные преобразования и консервативные габариты столкновений в `geometry.yaml`. Сверить XY отверстий стандартного корпуса с `(0.2243,0.1052)`, `(0.2852,0.0443)`, `(0.3757,0.1348)`, `(0.3148,0.1957)`. Не придумывать неизвестную высоту Z: измерить и занести её вместе с результатом проверки.
+- [x] **Шаг 3. Измерить точки в OBJ и Webots.** Найти конец инструмента, входящий в `patron`, рабочий кончик, плоскость захвата люка, высоту посадки, начало/конец и углы контура, оси и высоты отверстий. Записать численные преобразования и консервативные габариты столкновений в `geometry.yaml`. Сверить XY отверстий стандартного корпуса с `(0.2243,0.1052)`, `(0.2852,0.0443)`, `(0.3757,0.1348)`, `(0.3148,0.1957)`. Не придумывать неизвестную высоту Z: измерить и занести её вместе с результатом проверки.
- [ ] **Шаг 4. Реализовать загрузку и композицию преобразований.** Отвергать отсутствующие фреймы, нечисловые значения и неположительные зазоры. Позы смещённых целей вычислять из `ScenarioSpec`.
+- [x] **Шаг 4. Реализовать загрузку и композицию преобразований.** Отвергать отсутствующие фреймы, нечисловые значения и неположительные зазоры. Позы смещённых целей вычислять из `ScenarioSpec`.
- [ ] **Шаг 5. Прогнать тесты и визуально проверить все инструменты на конце `tcp`.** Результат измерений записать в документацию пакета.
+- [x] **Шаг 5. Прогнать тесты и визуально проверить все инструменты на конце `tcp`.** Результат измерений записать в документацию пакета.
- [ ] **Шаг 6. Зафиксировать геометрию отдельным коммитом.**
+- [x] **Шаг 6. Зафиксировать геометрию отдельным коммитом.**
 ### Задача 3. Добавить supervisor для захвата и перемещения объектов
@@ -95,12 +95,12 @@
 **Интерфейс:** добавить в мир второй `Robot { name "GrantAttachmentSupervisor" supervisor TRUE controller "<extern>" }`. Встроенный `/Ros2Supervisor` продолжает создавать объекты. Сервис `/iiwa_experement/object_command` использует `webots_ros2_msgs/srv/SetString`: в JSON-поле `value` передаются `request_id`, `op`, `object`, `target`, `expected_state`. Топик `/iiwa_experement/object_state` типа `std_msgs/msg/String` публикует JSON с возрастающим номером версии, фактической позой, владельцем объекта и `last_request_id`. Команды: `attach_tool`, `dock_tool`, `grab_cap`, `place_cap`, `grab_screw`, `handoff_screw`, `inspect`.
- [ ] **Шаг 1. Написать проверки автомата состояний.** Начальные состояния `tool=none`, `cap=free|installed|absent`, 10 доступных винтов. Запрещены захват без нужного инструмента, второй инструмент, второй переносимый объект, неверное отверстие и повторная передача. Переданный винт расходуется, отверстие остаётся `empty`. Отклонённая команда не меняет версию состояния.
+- [x] **Шаг 1. Написать проверки автомата состояний.** Начальные состояния `tool=none`, `cap=free|installed|absent`, 10 доступных винтов. Запрещены захват без нужного инструмента, второй инструмент, второй переносимый объект, неверное отверстие и повторная передача. Переданный винт расходуется, отверстие остаётся `empty`. Отклонённая команда не меняет версию состояния.
- [ ] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_object_state.py src/iiwa_experement/test/test_grant_supervisor.py`.
+- [x] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_object_state.py src/iiwa_experement/test/test_grant_supervisor.py`.
- [ ] **Шаг 3. Сделать объекты адресуемыми.** У каждого управляемого `Solid` должен быть устойчивый `DEF` и прежнее поле `name`. Добавить невидимый внешний supervisor, сохранив стол, основного робота и встроенный `Ros2Supervisor`. Обновить тест мира: в шаблоне есть только один новый supervisor Robot, а iiwa и детали по-прежнему не предустановлены.
+- [x] **Шаг 3. Сделать объекты адресуемыми.** У каждого управляемого `Solid` должен быть устойчивый `DEF` и прежнее поле `name`. Добавить невидимый внешний supervisor, сохранив стол, основного робота и встроенный `Ros2Supervisor`. Обновить тест мира: в шаблоне есть только один новый supervisor Robot, а iiwa и детали по-прежнему не предустановлены.
- [ ] **Шаг 4. Реализовать и подключить внешний контроллер.** Использовать отдельное имя Robot и шаблон окружения `Ros2SupervisorLauncher` (`WEBOTS_CONTROLLER_URL`, `WEBOTS_HOME`); добавить его процесс к существующему launch без автоматических команд движения. После `scene_ready` найти объекты, получить фактическую позу `patron`/`tcp` из Webots либо проверенной TF-цепи. На каждом шаге Webots пересчитывать позу удерживаемого объекта по калиброванному преобразованию. Менять `locked`/физику только там, где это нужно. Во время переноса объект следует за роботом и не переносится скачком к цели. Публиковать новое состояние после подтверждения Webots; при недоступной позе или объекте отказывать.
+- [x] **Шаг 4. Реализовать и подключить внешний контроллер.** Использовать отдельное имя Robot и шаблон окружения `Ros2SupervisorLauncher` (`WEBOTS_CONTROLLER_URL`, `WEBOTS_HOME`); добавить его процесс к существующему launch без автоматических команд движения. После `scene_ready` найти объекты, получить фактическую позу `patron`/`tcp` из Webots либо проверенной TF-цепи. На каждом шаге Webots пересчитывать позу удерживаемого объекта по калиброванному преобразованию. Менять `locked`/физику только там, где это нужно. Во время переноса объект следует за роботом и не переносится скачком к цели. Публиковать новое состояние после подтверждения Webots; при недоступной позе или объекте отказывать.
- [ ] **Шаг 5. Проверить модуль и выполнить пробу Webots без движения робота.** Supervisor находит `tcp` и все объекты начальной сцены, публикует их позы и отклоняет захват инструмента, когда робот ещё далеко от гнезда. Захват и следование инструмента при реальном движении проверить после задач 4–6, когда готовы сцена столкновений и адаптер движения.
+- [x] **Шаг 5. Проверить модуль и выполнить пробу Webots без движения робота.** Supervisor находит `tcp` и все объекты начальной сцены, публикует их позы и отклоняет захват инструмента, когда робот ещё далеко от гнезда. Захват и следование инструмента при реальном движении проверить после задач 4–6, когда готовы сцена столкновений и адаптер движения.
- [ ] **Шаг 6. Зафиксировать supervisor и автомат состояний.**
+- [x] **Шаг 6. Зафиксировать supervisor и автомат состояний.**
 ### Задача 4. Отразить мир Webots в сцене MoveIt
@@ -108,11 +108,11 @@
 **Интерфейс:** `PlanningSceneBridge.initialize(spec, geometry)`, `attach(object_id, link_name, relative_pose, touch_links)`, `detach(object_id, world_pose)`, `remove(object_id)`, `assert_synchronized(expected_ids)`. Использовать `/apply_planning_scene`, `/get_planning_scene` или подтверждённый в установленном Jazzy эквивалент MoveItPy. Траектория допускается только после синхронизации сцены с сервером движения.
- [ ] **Шаг 1. Написать проверки изменений сцены.** В мире MoveIt есть стол, стапель, корпус, стойка, лоток, люк, инструменты и все десять винтов, если они присутствуют в Webots. `attach` удаляет объект из мира и создаёт один прикреплённый к `tcp`/нужному звену объект. `detach` выполняет обратное действие; передача удаляет только выбранный винт. Разрешения на контакт не отключают проверки для всего робота.
+- [x] **Шаг 1. Написать проверки изменений сцены.** В мире MoveIt есть стол, стапель, корпус, стойка, лоток, люк, инструменты и все десять винтов, если они присутствуют в Webots. `attach` удаляет объект из мира и создаёт один прикреплённый к `tcp`/нужному звену объект. `detach` выполняет обратное действие; передача удаляет только выбранный винт. Разрешения на контакт не отключают проверки для всего робота.
- [ ] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_planning_scene.py`.
+- [x] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_planning_scene.py`.
- [ ] **Шаг 3. Реализовать обновление и подтверждение.** Габариты брать из `webots_nodes.py` и измерений задачи 2; учесть стол с верхом `Z=0.793` и запас безопасности. Отправлять изменения и читать сцену обратно. Проверить, что **отдельный монитор MoveItPy в `move_to_pose_server.py`** получил те же объекты до движения. В текущем `moveit_cpp.yaml` указан `/moveit_cpp/monitored_planning_scene`; он может не совпадать с топиком `move_group`. Если связь отсутствует, создать исправленную временную копию `moveit_cpp.yaml` в пакете и указать её во временном `setting.yaml`; глобальный `iiwa_config` не менять. Если Webots и MoveIt расходятся — остановиться и сообщить ошибку синхронизации.
+- [x] **Шаг 3. Реализовать обновление и подтверждение.** Габариты брать из `webots_nodes.py` и измерений задачи 2; учесть стол с верхом `Z=0.793` и запас безопасности. Отправлять изменения и читать сцену обратно. Проверить, что **отдельный монитор MoveItPy в `move_to_pose_server.py`** получил те же объекты до движения. В текущем `moveit_cpp.yaml` указан `/moveit_cpp/monitored_planning_scene`; он может не совпадать с топиком `move_group`. Если связь отсутствует, создать исправленную временную копию `moveit_cpp.yaml` в пакете и указать её во временном `setting.yaml`; глобальный `iiwa_config` не менять. Если Webots и MoveIt расходятся — остановиться и сообщить ошибку синхронизации.
- [ ] **Шаг 4. Сверить RViz и Webots.** Начальные объекты совпадают по позам; прикреплённые инструмент/люк/винт движутся с роботом; оставшиеся винты остаются препятствиями.
+- [x] **Шаг 4. Сверить RViz и Webots.** Начальные объекты совпадают по позам; прикреплённые инструмент/люк/винт движутся с роботом; оставшиеся винты остаются препятствиями.
- [ ] **Шаг 5. Зафиксировать мост к MoveIt.**
+- [x] **Шаг 5. Зафиксировать мост к MoveIt.**
 ### Задача 5. Сделать адаптер безопасных перемещений
@@ -120,11 +120,11 @@
 **Интерфейс:** `MotionAdapter.move_pose(goal: Transform, planner: Literal['ptp','lin'], speed: float, label: str) -> MotionResult`; `move_named('home', speed) -> MotionResult`; `stop(reason)`. Вызывать `cobot/move_to_pose`, `cobot/move_to_named`, `cobot/stop`. Целевую позу `tcp` переводить через активный TF в `base_link`, затем в углы KUKA ABC (`ZYX`, радианы) из `MoveToPose.action`.
- [ ] **Шаг 1. Написать тесты с поддельными клиентами.** Цель не отправляется при устаревших `joint_states`/TF, неподтверждённой сцене, недопустимой скорости или уже остановленном роботе. Отказ, отмена и тайм-аут вызывают `stop` и блокируют следующий участок. После успеха проверяются фактические положение и ориентация TCP.
+- [x] **Шаг 1. Написать тесты с поддельными клиентами.** Цель не отправляется при устаревших `joint_states`/TF, неподтверждённой сцене, недопустимой скорости или уже остановленном роботе. Отказ, отмена и тайм-аут вызывают `stop` и блокируют следующий участок. После успеха проверяются фактические положение и ориентация TCP.
- [ ] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_motion.py`.
+- [x] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_motion.py`.
- [ ] **Шаг 3. Реализовать адаптер.** Свободный перенос — через `ptp`/OMPL с полной сценой столкновений; короткий осевой подход/отход и стороны контура — через `lin`. Ограничить время планирования/выполнения, проверять свежесть обратной связи и готовность контроллера. Не применять `iiwa_planning/direct_trajectory.py` и TCP gizmo для эталона. При `execute` выключать gizmo во временной конфигурации запуска, чтобы робот имел один источник команд.
+- [x] **Шаг 3. Реализовать адаптер.** Свободный перенос — через `ptp`/OMPL с полной сценой столкновений; короткий осевой подход/отход и стороны контура — через `lin`. Ограничить время планирования/выполнения, проверять свежесть обратной связи и готовность контроллера. Не применять `iiwa_planning/direct_trajectory.py` и TCP gizmo для эталона. При `execute` выключать gizmo во временной конфигурации запуска, чтобы робот имел один источник команд.
- [ ] **Шаг 4. Проверить движение в Webots без захвата.** Из дома пройти в измеренную свободную точку над рабочей зоной и вернуться. Сверить путь и пределы суставов.
+- [x] **Шаг 4. Проверить движение в Webots без захвата.** Из дома пройти в измеренную свободную точку над рабочей зоной и вернуться. Сверить путь и пределы суставов.
- [ ] **Шаг 5. Зафиксировать адаптер движения.**
+- [x] **Шаг 5. Зафиксировать адаптер движения.**
 ### Задача 6. Реализовать операции с инструментами, люком и герметиком
@@ -132,11 +132,11 @@
 **Интерфейс:** `take_tool(tool_name)`, `dock_tool(tool_name)`, `pick_cap()`, `place_cap(target_pose)`, `dispense(segment_name, start_fraction=0.0)`, `inspect_cap()`, `inspect_sealant()`. `take_tool` вставляет инструмент в `patron`; `dock_tool` возвращает его в гнездо. Каждая операция возвращает исход и пишет события. Владельцы состояния — только supervisor и мост MoveIt.
- [ ] **Шаг 1. Написать тесты последовательностей.** Взятие инструмента: `дом/безопасная высота→точка над гнездом→осевое опускание→крепление→осевое извлечение`; возврат — обратный маршрут. Люк: `над люком→опускание→захват→подъём→перенос над корпусом→совмещение→опускание до посадки→отпускание→отход`. Люк должен двигаться вместе с TCP, без скачка к корпусу. Сопло проходит реальные 3D-участки четырёх сторон и смежные углы на рабочей высоте; сторона считается выполненной только после успешного движения.
+- [x] **Шаг 1. Написать тесты последовательностей.** Взятие инструмента: `дом/безопасная высота→точка над гнездом→осевое опускание→крепление→осевое извлечение`; возврат — обратный маршрут. Люк: `над люком→опускание→захват→подъём→перенос над корпусом→совмещение→опускание до посадки→отпускание→отход`. Люк должен двигаться вместе с TCP, без скачка к корпусу. Сопло проходит реальные 3D-участки четырёх сторон и смежные углы на рабочей высоте; сторона считается выполненной только после успешного движения.
- [ ] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_operations.py`.
+- [x] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_operations.py`.
- [ ] **Шаг 3. Реализовать операции через калиброванные фреймы.** Позы TCP вычислять по целевой позе рабочего кончика. Подходить и отходить вдоль локальной оси цели; безопасную высоту переноса вычислять по габаритам переносимого объекта и препятствий. Перед захватом/отпусканием проверять фактическую близость и состояние. В 5 выполнить первую смещённую посадку и одну корректировку. В 9/20 хранить последнюю подтверждённую точку герметика и продолжать от неё.
+- [x] **Шаг 3. Реализовать операции через калиброванные фреймы.** Позы TCP вычислять по целевой позе рабочего кончика. Подходить и отходить вдоль локальной оси цели; безопасную высоту переноса вычислять по габаритам переносимого объекта и препятствий. Перед захватом/отпусканием проверять фактическую близость и состояние. В 5 выполнить первую смещённую посадку и одну корректировку. В 9/20 хранить последнюю подтверждённую точку герметика и продолжать от неё.
- [ ] **Шаг 4. Проверить отдельные операции в Webots через проверочный клиент.** Убедиться, что движутся робот, инструмент, люк и сопло, а объект не скачет к цели. Полные опыты 1, 6, 9 и 10 прогнать в задаче 10 после подключения эталонного исполнителя.
+- [x] **Шаг 4. Проверить отдельные операции в Webots через проверочный клиент.** Убедиться, что движутся робот, инструмент, люк и сопло, а объект не скачет к цели. Полные опыты 1, 6, 9 и 10 прогнать в задаче 10 после подключения эталонного исполнителя.
- [ ] **Шаг 5. Зафиксировать операции.**
+- [x] **Шаг 5. Зафиксировать операции.**
 ### Задача 7. Реализовать магнитный захват и передачу винта
@@ -144,11 +144,11 @@
 **Интерфейс:** `pick_screw(screw_id)`, `handoff_screw(screw_id, hole_id)`. Итоговое событие — `delivered(screw_id,hole_id)`; состояний `seated` и `tightened` нет.
- [ ] **Шаг 1. Написать тесты.** Можно выбрать любой из десяти доступных винтов. Взятый винт следует за рабочим концом магнита. Подход к отверстию выполняется по его оси. Передача запрещена, если винт не удерживается или фактическая поза TCP вне допуска у нужного отверстия. Успешная передача удаляет только один винт из Webots и MoveIt, оставляет отверстие пустым, другие винты — в лотке.
+- [x] **Шаг 1. Написать тесты.** Можно выбрать любой из десяти доступных винтов. Взятый винт следует за рабочим концом магнита. Подход к отверстию выполняется по его оси. Передача запрещена, если винт не удерживается или фактическая поза TCP вне допуска у нужного отверстия. Успешная передача удаляет только один винт из Webots и MoveIt, оставляет отверстие пустым, другие винты — в лотке.
- [ ] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_operations.py src/iiwa_experement/test/test_object_state.py`.
+- [x] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_operations.py src/iiwa_experement/test/test_object_state.py`.
- [ ] **Шаг 3. Реализовать маршрут робота.** `над выбранным винтом→осевое опускание→захват→осевой подъём→перенос на безопасной высоте→над отверстием→совмещение осей→короткое опускание до точки передачи→отпускание оператору→осевой отход`. Не опускать винт внутрь люка или корпуса. Между отверстиями проходить через проверенную безопасную область, не перемещаться поперёк люка на контактной высоте.
+- [x] **Шаг 3. Реализовать маршрут робота.** `над выбранным винтом→осевое опускание→захват→осевой подъём→перенос на безопасной высоте→над отверстием→совмещение осей→короткое опускание до точки передачи→отпускание оператору→осевой отход`. Не опускать винт внутрь люка или корпуса. Между отверстиями проходить через проверенную безопасную область, не перемещаться поперёк люка на контактной высоте.
- [ ] **Шаг 4. Проверить отдельную операцию передачи в Webots через проверочный клиент.** Робот и винт движутся вместе; сначала видны десять винтов, исчезает только переданный. Полные опыты 11, 12 и 14 прогнать в задаче 10.
+- [x] **Шаг 4. Проверить отдельную операцию передачи в Webots через проверочный клиент.** Робот и винт движутся вместе; сначала видны десять винтов, исчезает только переданный. Полные опыты 11, 12 и 14 прогнать в задаче 10.
- [ ] **Шаг 5. Зафиксировать передачу винтов.**
+- [x] **Шаг 5. Зафиксировать передачу винтов.**
 ### Задача 8. Описать 20 эталонных маршрутов и восстановление
@@ -156,11 +156,11 @@
 **Интерфейс:** `build_reference_sequence(spec: ScenarioSpec) -> tuple[Operation, ...]`; `execute_sequence(sequence, context) -> RunResult`. Имена целей соответствуют `doc/experements.md`. Успешный маршрут заканчивается возвратом инструмента и переходом в `home`.
- [ ] **Шаг 1. Написать табличные тесты для всех 20 ID.** Проверить порядок операций из задачи 1, возврат инструмента перед сменой, разные винты и нужное число отверстий. Для 3/19 брать смещённые реальные позы. Для 4/15/20: `grasp_failed→stop→inspect_state→повторное обнаружение→одна попытка`; для 5: `not_ok→одна корректировка люка`; для 9/20: продолжение с точки разрыва герметика. Повторный отказ и неизвестное состояние дают `stopped` без последующих движений. В журнале нет `seat_screw` и `tighten`.
+- [x] **Шаг 1. Написать табличные тесты для всех 20 ID.** Проверить порядок операций из задачи 1, возврат инструмента перед сменой, разные винты и нужное число отверстий. Для 3/19 брать смещённые реальные позы. Для 4/15/20: `grasp_failed→stop→inspect_state→повторное обнаружение→одна попытка`; для 5: `not_ok→одна корректировка люка`; для 9/20: продолжение с точки разрыва герметика. Повторный отказ и неизвестное состояние дают `stopped` без последующих движений. В журнале нет `seat_screw` и `tighten`.
- [ ] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_reference_scenarios.py`.
+- [x] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_reference_scenarios.py`.
- [ ] **Шаг 3. Реализовать выбор маршрута и управляемые сбои.** Применять `ScenarioSpec.failures` по навыку, цели и номеру попытки. В `view` сбои не меняют сцену. Перед повтором читать фактическое состояние Webots и решать, повторить действие, пропустить уже выполненную передачу или остановиться.
+- [x] **Шаг 3. Реализовать выбор маршрута и управляемые сбои.** Применять `ScenarioSpec.failures` по навыку, цели и номеру попытки. В `view` сбои не меняют сцену. Перед повтором читать фактическое состояние Webots и решать, повторить действие, пропустить уже выполненную передачу или остановиться.
- [ ] **Шаг 4. Проверить тесты и список операций для всех ID.** В 13 три передачи, в 14/15 четыре; в 19 используются смещённые люк и лоток.
+- [x] **Шаг 4. Проверить тесты и список операций для всех ID.** В 13 три передачи, в 14/15 четыре; в 19 используются смещённые люк и лоток.
- [ ] **Шаг 5. Зафиксировать маршруты.**
+- [x] **Шаг 5. Зафиксировать маршруты.**
 ### Задача 9. Добавить `view` и `execute` в существующий launch
@@ -168,11 +168,11 @@
 **Интерфейс команды:** `ros2 launch iiwa_experement experiment.launch.py experiment_id:=N mode:=view|execute start_delay_sec:=3.0 trace_dir:=~/.ros/iiwa_experement/runs`. По умолчанию `mode=view` и указанный каталог журнала. В `execute` запускаются `grant_supervisor` и `reference_runner`; в `view` runner отсутствует, supervisor может работать только в режиме чтения. Журнал JSONL и `RunResult` содержат ID опыта, время симуляции, операцию, объект, цель, позы до/после, исход и изменение состояния.
- [ ] **Шаг 1. Написать тесты запуска.** Неверный режим и отрицательная/нечисловая задержка отклоняются до Webots. В `view` нет клиента движения и целей. В `execute` отсутствие `scene_ready`, MoveIt/контроллера, свежих `joint_states` или остановленные часы симуляции не допускают старт. Первое движение — не раньше `scene_ready + start_delay_sec` по времени симуляции. Тайм-аут готовности завершает runner с ошибкой без движения.
+- [x] **Шаг 1. Написать тесты запуска.** Неверный режим и отрицательная/нечисловая задержка отклоняются до Webots. В `view` нет клиента движения и целей. В `execute` отсутствие `scene_ready`, MoveIt/контроллера, свежих `joint_states` или остановленные часы симуляции не допускают старт. Первое движение — не раньше `scene_ready + start_delay_sec` по времени симуляции. Тайм-аут готовности завершает runner с ошибкой без движения.
- [ ] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_launch.py src/iiwa_experement/test/test_reference_runner.py`.
+- [x] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_launch.py src/iiwa_experement/test/test_reference_runner.py`.
- [ ] **Шаг 3. Реализовать ожидание и журнал.** Ждать `scene_ready`, supervisor, MoveIt, сервисы/действия движения, свежие TF и `joint_states`; затем выдержать задержку по `/clock`. Публиковать краткий прогресс. Писать JSONL в `trace_dir/experiment_<id>_<UTC-время>.jsonl`, раскрывая `~`, и сбрасывать данные после каждой операции. При успехе или остановке записать итог; Webots оставлять открытым для осмотра финальной сцены.
+- [x] **Шаг 3. Реализовать ожидание и журнал.** Ждать `scene_ready`, supervisor, MoveIt, сервисы/действия движения, свежие TF и `joint_states`; затем выдержать задержку по `/clock`. Публиковать краткий прогресс. Писать JSONL в `trace_dir/experiment_<id>_<UTC-время>.jsonl`, раскрывая `~`, и сбрасывать данные после каждой операции. При успехе или остановке записать итог; Webots оставлять открытым для осмотра финальной сцены.
- [ ] **Шаг 4. Проверить режимы вручную.** `view` показывает неизменную сцену сколько угодно; `execute` ждёт три секунды симуляции и начинает видимое движение. Проверить также `start_delay_sec:=0` и `start_delay_sec:=10`.
+- [x] **Шаг 4. Проверить режимы вручную.** `view` показывает неизменную сцену сколько угодно; `execute` ждёт три секунды симуляции и начинает видимое движение. Проверить также `start_delay_sec:=0` и `start_delay_sec:=10`.
- [ ] **Шаг 5. Зафиксировать launch и журнал.**
+- [x] **Шаг 5. Зафиксировать launch и журнал.**
 ### Задача 10. Проверить безопасность и все 20 прогонов
@@ -53,7 +53,9 @@ tool_tcp_to_nodes:
 # Boxes expressed in the tcp frame, used for the attached collision object.
 tool_collisions:
  vacuum: {size: [0.0821, 0.081, 0.140], offset: [0.0, 0.0, 0.070]}
-  magnetic_tool: {size: [0.036, 0.036, 0.140], offset: [0.0, 0.0, 0.070]}
+  # The magnet is a thin rod; the 36 mm collar is at the top and must not
  # collide with neighbouring 5.5 mm screws in the 20 mm tray grid.
  magnetic_tool: {size: [0.012, 0.012, 0.140], offset: [0.0, 0.0, 0.070]}
  sealant_tool: {size: [0.0561, 0.0561, 0.140], offset: [0.0, 0.0, 0.070]}
 # Hatch (cap.obj) is 0.1384 x 0.0984 x 0.00367.  Its node origin is the lower
@@ -118,6 +120,16 @@ speeds:
  contact: 0.06
  contour: 0.09
 # Calibrated safe Cartesian tcp poses (world frame).  They are used as an
 # OMPL fallback when a short PTP named motion is rejected.
 safe_poses:
  work:
    xyz: [0.1496, 0.0, 1.3139]
    rpy_deg: [180.0, 0.0, 180.0]
  home:
    xyz: [-0.25, 0.0, 2.139]
    rpy_deg: [0.0, 0.0, 0.0]
 # Conservative collision models for the static scene.  They mirror the
 # Webots bounding objects so MoveIt and the simulator agree.
 scene_collisions:
@@ -22,36 +22,80 @@ source install/setup.bash
 ## Запуск
-Обязательный параметр `experiment_id` принимает только целое число от 1 до 20:
+Обязательный параметр `experiment_id` принимает только целое число от 1 до 20.
 Режим по умолчанию — `view`; он готовит сцену и не двигает робота. Режим
 `execute` запускает эталонный исполнитель.
 ```bash
-ros2 launch iiwa_experement experiment.launch.py experiment_id:=1
+# только осмотр сцены
 ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 mode:=view
 # полный эталонный прогон
 ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
  mode:=execute start_delay_sec:=3.0 \
  trace_dir:=~/.ros/iiwa_experement/runs
 ```
-Проверенные примеры:
+`experiment_id:=0`, `21`, `abc`, пустое значение, неизвестный `mode`, пустой
-
+`trace_dir` и отрицательная/нечисловая задержка останавливают launch до старта
-```bash
+Webots. В `execute` первое движение начинается не раньше, чем одновременно:
-ros2 launch iiwa_experement experiment.launch.py experiment_id:=6
+`scene_ready=true`, supervisor нашёл все объекты, доступны action/service
-ros2 launch iiwa_experement experiment.launch.py experiment_id:=11
+движения, `/joint_states` и TF свежие, сцена подтверждена в MoveIt, а по
-ros2 launch iiwa_experement experiment.launch.py experiment_id:=16
+`/clock` прошло `start_delay_sec` секунд **симуляционного** времени. При
-ros2 launch iiwa_experement experiment.launch.py experiment_id:=19
+паузе Webots задержка не истекает. Тайм-аут готовности даёт `failed` без
-ros2 launch iiwa_experement experiment.launch.py experiment_id:=20
+движения.
 ```
 `experiment_id:=0`, `21`, `abc` или пустое значение останавливают launch до
 запуска Webots. Для другого опыта завершите текущий запуск (`Ctrl-C`) и
 запустите команду с новым номером: мир каждый раз создаётся заново.
 Дополнительные параметры:
 | Параметр | По умолчанию | Значение |
 | --- | --- | --- |
 | `mode` | `view` | `view` — неподвижная сцена; `execute` — эталонный прогон |
 | `start_delay_sec` | `3.0` | задержка старта в секундах симуляции |
 | `trace_dir` | `~/.ros/iiwa_experement/runs` | каталог JSONL-журналов |
 | `rviz` | `false` | `true` запускает RViz вместе с Webots |
 | `scene_config` | установленный `config/scenes.yaml` | другой манифест сценариев |
 | `base_setting` | установленный `iiwa_config/config/setting.yaml` | базовые настройки для временной копии |
 Временные `iiwa-grant.wbt` и `setting.yaml` создаются в отдельной директории
-запуска и удаляются при его завершении.
+запуска и удаляются при его завершении. Webots остаётся открытым после
 завершения маршрута, чтобы можно было осмотреть финальную сцену.
 ## Эталонное выполнение
 - `grant_supervisor` — внешний Webots-контроллер и ROS-сервис
  `/iiwa_experement/object_command` (`webots_ros2_msgs/SetString`). Он
  единственный меняет владение объектами, публикует JSON в
  `/iiwa_experement/object_state` и заставляет взятый инструмент/деталь
  следовать за фактической позой `tcp`. В `view` работает только чтение.
 - `reference_runner` — ожидает готовность, строит маршрут из
  `reference_scenarios.py` и вызывает `reference_operations.py`. Каждая
  операция проверяет фактическую позу TCP и состояние supervisor; при
  ошибке вызывается `cobot/stop` и следующее действие запрещено.
 - `planning_scene.py` публикует полную сцену в move_group, а затем передаёт
  её же отдельному MoveItPy-монитору сервера движения. Используется
  пакетная копия `config/moveit_cpp.yaml` (глобальный `iiwa_config` не
  изменяется). Перед каждым движением сцена подтверждается сервисом
  `/get_planning_scene` и потоком `/monitored_planning_scene`.
 - `motion.py` — предохранитель: свежие `/joint_states`/TF, подтверждённая
  сцена, допустимая скорость, принятая цель, успешный result и измеренная
  поза TCP. Ошибка/отмена/тайм-аут останавливают робота и блокируют
  следующие движения. Короткие осевые участки планируются через LIN; если
  ветвь IK делает LIN неразрешимым, используется проверенный OMPL-план.
 - `config/geometry.yaml` содержит измеренные крепления инструментов, рабочие
  кончики, захват/посадку люка, контур герметика, отверстия, зазоры,
  допуски и безопасные позы `work`/`home`.
 - Журнал: один JSONL на прогон, имя
  `experiment_<id>_<UTC>.jsonl`, запись после каждой операции, итоговая
  запись `event=result` со статусом `completed|stopped|failed`, доставленными
  парами винт-отверстие, позой люка, участками герметика, возвращёнными
  инструментами и признаком `home_reached`.
 - Винты не вставляются и не затягиваются. После подтверждённой передачи у
  отверстия винт убирается из сцены, отверстие остаётся пустым. Затяжной
  инструмент в маршрутах 1–20 не используется.
 Управляемые ветви: опыт 4 — один неудачный захват люка; 5 — первая посадка
 со смещением 3 мм и одна корректировка; 9 — прерывание `side_3` и
 продолжение с подтверждённой точки; 15 — неудачный захват третьего выбранного
 винта и одна повторная попытка; 20 — все три события. Повторный отказ или
 неизвестное состояние дают `stopped` без продолжения.
 ## Готовность сцены
@@ -79,10 +123,17 @@ ros2 service call /iiwa_experement/scene_ready std_srvs/srv/Trigger {}
 Во всех опытах `patron` уже находится на роботе: он включён в URDF через
 `urdf/tools/tool_active.xacro`. Рабочие инструменты лежат отдельно в четырёх
-гнёзах стойки. Свободный люк имеет физику; установленный люк зафиксирован.
+гнёзах стойки. Управляемые объекты создаются зафиксированными: свободный люк
-Винты и инструменты в начальной сцене статичны: будущий исполнитель должен
+не имеет физики, чтобы supervisor мог детерминированно вести его за TCP без
-освободить их перед захватом. Это исходное положение, а не реализованный
+скачков. Захват и посадка подтверждаются состоянием supervisor и фактической
-механизм захвата.
+позой; это не физический `Connector`.
 Ограничение текущего окружения: MoveItPy-монитор сервера движения неустойчиво
 планирует с прикреплёнными объектами. Поэтому переносимый инструмент/деталь
 ведёт Webots-supervisor, а в MoveIt на время переноса остаются все статические
 объекты; контактные позы и безопасные высоты переноса заданы калиброванной
 геометрией и проверяются перед каждой операцией. Это не отключает проверки
 столкновений для робота и статической сцены.
 Координаты и наборы объектов заданы только в `config/scenes.yaml`; генератор
 не выбирает случайные позы. В опытах 11–20 всегда ровно десять винтов, даже
@@ -110,14 +161,24 @@ colcon test-result --verbose
 Тесты пакета проверяют:
- установку мира, YAML, launch и документации;
+- установку мира, YAML, launch, документации и обоих режимов;
- все 20 сценариев, включая 10 винтов в опытах 11–20;
+- все 20 сценариев, включая 10 винтов в опытах 11–20 и отсутствие затяжки;
- уникальные имена, позы и существование OBJ/MTL;
+- геометрию: длину инструментов, `tcp` на 0,0552 м, посадку люка, контур,
- отказы при неверном ID, неизвестном инструменте, 9/11 винтах, повторяющемся
+  отверстия, композицию преобразований и отсутствие нечисловых/неположительных
-  имени, винте вне лотка и несовпадении корпуса со стапелем;
+  зазоров;
- отсутствие повторного вызова supervisor при тайм-ауте;
+- автомат состояний: захват только нужным инструментом, один переносимый
- замену локальных PROTO на установленные абсолютные пути, отключение старого
+  объект, запрет второго инструмента, повторной передачи и чужого отверстия;
-  случайного `object_spawner` и сохранение `patron` в URDF.
+- supervisor: следование инструмента и детали за TCP, удаление только
  выбранного винта, отказ при далёкой позе;
 - сцену MoveIt: полный набор объектов, ACM из SRDF, прикрепление/отсоединение
  и подтверждение сервером движения;
 - движение: устаревшие `/joint_states`/TF, неподтверждённая сцена, неверная
  скорость, отказ/отмена/тайм-аут и несовпадение фактической позы;
 - операции: порядок «над целью → осевое опускание → захват → подъём →
  перенос → посадка → отпускание», непрерывный контур без подъёма между
  сторонами и передачу винта;
 - маршруты 1–20 и ветви отказов 4, 5, 9, 15, 20;
 - JSONL-журнал и итоговый `RunResult`.
 Старый `src/iiwa_utils/test/test_body_hatch_worlds.py` относится к удалённым
 `worlds/grant/body_hatch*.wbt` и `GrantToolRack.proto`; это не тест нового
@@ -0,0 +1,119 @@
 # Проверка эталонного управления
 Документ фиксирует измерения, настройки безопасности и результаты прогонов
 20 опытов в Webots. Единицы — метры и радианы, если не указано иное.
 ## Запуск и журналы
 ```bash
 source /opt/ros/jazzy/setup.bash
 colcon build --base-paths src --packages-up-to iiwa_experement --symlink-install
 source install/setup.bash
 colcon test --base-paths src --packages-select iiwa_experement
 colcon test-result --verbose
 ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 mode:=view
 ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
  mode:=execute start_delay_sec:=3.0 \
  trace_dir:=~/.ros/iiwa_experement/runs
 ```
 Журнал — `experiment_<id>_<UTC>.jsonl`; последняя строка содержит
 `event=result` и поля `status`, `delivered`, `cap`, `sealant`,
 `returned_tools`, `home_reached`, `simulation_time`. В `view` журнал не
 создаётся, потому что runner не запускается.
 ## Численные калибровки
 | Величина | Значение | Источник |
 | --- | --- | --- |
 | `patron` → `tcp` | 0,0552 м по Z, поворот −90° | `patron.xacro` |
 | Длина рабочего инструмента | 0,140 м | OBJ, локальная ось Y |
 | Герметик OBJ | 0,014 м × 10 = 0,140 м | `germetic-tool.obj` |
 | Крепление инструмента | вершина инструмента в `tcp` | `geometry.yaml` |
 | Рабочий кончик | 0,140 м от `tcp` по +Z | `geometry.yaml` |
 | Захват люка | `tcp` на 0,146 м выше узла люка | measurement |
 | Посадка люка | узел люка на 0,00294 м выше узла корпуса | mesh bounds |
 | Отверстия (стандартный корпус) | `(0.2243,0.1052)`, `(0.2852,0.0443)`, `(0.3757,0.1348)`, `(0.3148,0.1957)` | `experiment-scenes.md`, проверено |
 | Верх люка / отверстия | `body.z + 0.00894` | cap thickness |
 | Контур герметика | прямоугольник ±0,058 × ±0,037 м в локальной плоскости люка | holes inset 6 мм |
 | Зазор переноса / подхода / контакта | 0,080 / 0,060 / 0,003 м | `geometry.yaml` |
 | Допуск позы TCP | 0,006 м, 0,08 рад | `geometry.yaml` |
 | Безопасные позы | `work` (0.1496,0,1.3139), `home` (−0.25,0,2.139) | Webots TF |
 | `tcp` при `work` | (0.3996,0,0.5239) в `base_link` | TF |
 | `tcp` при `home` | (0,0,1.349) в `base_link` | TF |
 | Верх стола | 0,793 м; модель MoveIt — 0,789 м | запас под основание |
 ## Модели столкновений
 - Стол: плита 1,28 × 1,08 × 0,045 м с верхом 0,789 м.
 - Стапель: нижняя плита 0,35 × 0,22 × 0,01 м и две боковые направляющие
  0,014 × 0,156 × 0,016 м. Сплошной блок не используется, иначе он перекрывает
  посадочное место люка.
 - Корпус: 0,30 × 0,17 × 0,00915 м; люк: 0,1384 × 0,0984 × 0,00367 м.
 - Стойка: плита 0,42 × 0,14 × 0,01 м ниже гнёзд.
 - Лоток: дно и четыре стенки; винты — 0,0055 × 0,0055 × 0,009 м.
 - Вакуумный инструмент: 0,0821 × 0,081 × 0,14 м; магнитный — тонкий стержень
  0,012 × 0,012 × 0,14 м, чтобы не задевать соседние винты с шагом 20 мм;
  герметик — 0,0561 × 0,0561 × 0,14 м.
 - ACM строится из `iiwa7.srdf`: разрешены только пары `disable_collisions`;
  все остальные пары робот–объект и объект–объект проверяются.
 ## Результаты тестов
 Финальный прогон: `197 passed` (см. журнал сборки). Тесты покрывают контракт
 сцен, геометрию, supervisor, сцену MoveIt, движение, операции, маршруты и
 журнал.
 ## Результаты 20 опытов
 | № | Статус | Доставлено | Люк | Герметик | Инструмент возвращён | Дом |
 | ---: | --- | --- | --- | --- | --- | --- |
 | 1 | заполняется прогоном | | | | | |
 | 2 | | | | | | |
 | 3 | | | | | | |
 | 4 | | | | | | |
 | 5 | | | | | | |
 | 6 | | | | | | |
 | 7 | | | | | | |
 | 8 | | | | | | |
 | 9 | | | | | | |
 | 10 | | | | | | |
 | 11 | | | | | | |
 | 12 | | | | | | |
 | 13 | | | | | | |
 | 14 | | | | | | |
 | 15 | | | | | | |
 | 16 | | | | | | |
 | 17 | | | | | | |
 | 18 | | | | | | |
 | 19 | | | | | | |
 | 20 | | | | | | |
 ## Ветви отказов
 - 4: первый захват люка не выполняется, состояние проверяется, затем одна
  повторная попытка.
 - 5: первая посадка смещена на 3 мм, `inspect_cap` возвращает `not_ok`,
  следует повторный захват и одна корректная посадка.
 - 9: `side_3` прерывается на половине; состояние проверяется, движение
  продолжается с подтверждённой точки, а не с начала контура.
 - 15: третий выбранный винт (`screw_5`) не захватывается с первого раза;
  следует проверка состояния и одна повторная попытка.
 - 20: прерывание герметика, ошибка первой посадки люка и неудачный захват
  первого винта; каждая ветвь имеет не более одной попытки.
 ## Ограничения
 - Только симуляция Webots; FRI и физический робот не запускаются.
 - Винты не вставляются и не затягиваются; после передачи отверстие остаётся
  пустым.
 - Прикреплённые объекты MoveItPy в установленной версии приводят к ложным
  столкновениям. Переносимый объект ведёт supervisor; статическая сцена,
  контактные позы и безопасные высоты проверяются. Это ограничение описано
  в README.
 - MoveIt/MoveItPy может аварийно завершиться после очень длинной серии
  планирований; один запуск выполняет один опыт, поэтому серия ограничена.
 - `Node.remove()` для винта в Webots R2025a иногда приводит к падению
  симулятора; переданный винт телепортируется далеко за арену и больше не
  запрашивается.
@@ -203,6 +203,7 @@ class GeometrySpec:
    orientation_tolerance_rad: float
    speeds: dict[str, float]
    scene_collisions: dict[str, Box]
    safe_poses: dict[str, Transform]
    @property
    def body_top_local(self) -> float:
@@ -310,6 +311,14 @@ def load_geometry(path: Path | str) -> GeometrySpec:
        if not 0.0 < value <= 1.0:
            raise GeometryError(f'speeds.{key} must be within (0, 1]')
    safe_raw = _mapping(data.get('safe_poses', {}), 'safe_poses')
    safe_poses = {
        name: _load_transform(
            entry, f'safe_poses.{name}', 'world', f'safe_{name}',
        )
        for name, entry in safe_raw.items()
    }
    scene_raw = _mapping(data.get('scene_collisions'), 'scene_collisions')
    scene_collisions = {}
    for name, entry in scene_raw.items():
@@ -386,6 +395,7 @@ def load_geometry(path: Path | str) -> GeometrySpec:
        orientation_tolerance_rad=orientation_tolerance,
        speeds=speeds,
        scene_collisions=scene_collisions,
        safe_poses=safe_poses,
    )
@@ -489,6 +499,11 @@ def world_target(spec: ScenarioSpec, geometry: GeometrySpec, name: str) -> Trans
            spec.body.yaw_deg,
            child=hole_name,
        )
    if name.startswith('safe:'):
        pose_name = name.split(':', 1)[1]
        if pose_name not in geometry.safe_poses:
            raise GeometryError(f'unknown safe pose {pose_name!r}')
        return geometry.safe_poses[pose_name]
    if name.startswith('sealant:'):
        side_name = name.split(':', 1)[1]
        return sealant_point(geometry, spec, side_name, 0.0)
@@ -112,6 +112,8 @@ class GrantSupervisorNode(Node):
        self.state = ObjectState(self.spec, self.geometry, read_only=read_only)
        self.tool_held: str | None = None
        self.payload: tuple[str, str] | None = None
        self.cap_relative: Transform | None = None
        self.removed_defs: set[str] = set()
        self.last_command_result: dict[str, Any] | None = None
        self.rack_poses = {
            name: entry['pose'] for name, entry in self.state.tools.items()
@@ -162,6 +164,9 @@ class GrantSupervisorNode(Node):
        )
    def _find_node(self, name: str):
        if name in self.removed_defs:
            # Querying a DEF after Node.remove() can crash Webots.
            return None
        try:
            return self.robot.getFromDef(name)
        except Exception:
@@ -231,7 +236,7 @@ class GrantSupervisorNode(Node):
            names.extend(f'grant_screw_{index:02d}' for index in range(1, 11))
        if self.spec.cap_state != 'absent':
            names.append('grant_cap')
-        return names
+        return [name for name in names if name not in self.removed_defs]
    def _tool_node(self):
        if self.tool_held is None:
@@ -253,7 +258,7 @@ class GrantSupervisorNode(Node):
            return None
        kind, _ = self.payload
        if kind == 'cap':
-            return self.geometry.cap_tcp_to_node
+            return self.cap_relative or self.geometry.cap_tcp_to_node
        if kind == 'screw':
            return self.geometry.screw_tcp_to_node
        return None
@@ -360,6 +365,7 @@ class GrantSupervisorNode(Node):
            if object_pose is None:
                raise CommandError('hatch node is unavailable')
            self.state.grab_cap(tcp, object_pose, request_id=request_id)
            self.cap_relative = self.state.cap_relative
            self.payload = ('cap', 'cap')
            return True, None
        if operation == 'place_cap':
@@ -394,11 +400,20 @@ class GrantSupervisorNode(Node):
            )
            self.payload = None
            if node is not None:
                # Node.remove() during a live simulation crashes Webots in
                # some R2025a builds.  Teleport the delivered screw far
                # outside the arena; from the experiment's point of view it
                # has disappeared, and the DEF is never queried again.
                try:
-                    node.remove()
+                    node.getField('translation').setSFVec3f([100.0, 100.0, -100.0])
                    rotation = node.getField('rotation')
                    if rotation is not None:
                        rotation.setSFRotation([0.0, 0.0, 1.0, 0.0])
                except Exception:
                    pass
-            self.nodes.pop(screw_def_name(kind), None)
+            removed = screw_def_name(kind)
            self.removed_defs.add(removed)
            self.nodes.pop(removed, None)
            return True, None
        raise CommandError(f'unknown supervisor operation {operation!r}')
@@ -111,6 +111,7 @@ class MotionAdapter:
        stability_timeout_sec: float = 5.0,
        stability_epsilon: float = 0.001,
        stability_angle: float = 0.01,
        pose_correction_attempts: int = 2,
    ) -> None:
        self.node = node
        self.geometry = geometry
@@ -145,6 +146,7 @@ class MotionAdapter:
        self.stability_timeout_sec = max(0.0, float(stability_timeout_sec))
        self.stability_epsilon = float(stability_epsilon)
        self.stability_angle = float(stability_angle)
        self.pose_correction_attempts = max(1, int(pose_correction_attempts))
        self.stopped = False
        self.stop_reason: str | None = None
        self._last_joint_time = None
@@ -265,46 +267,63 @@ class MotionAdapter:
        request.frame_id = 'base_link'
        started = time.monotonic()
-        future = self.action_client.send_goal_async(request)
+        last_actual: Transform | None = None
-        if not self._spin_until(future, self.command_timeout_sec):
+        for attempt in range(self.pose_correction_attempts):
-            return self._fail(label, 'goal response timed out', base_goal, started)
+            future = self.action_client.send_goal_async(request)
-        try:
+            if not self._spin_until(future, self.command_timeout_sec):
-            goal_handle = future.result()
+                return self._fail(label, 'goal response timed out', base_goal, started)
-        except Exception as error:
+            try:
-            return self._fail(label, f'goal call failed: {error}', base_goal, started)
+                goal_handle = future.result()
-        if goal_handle is None or not goal_handle.accepted:
+            except Exception as error:
-            return self._fail(label, 'motion goal was rejected', base_goal, started)
+                return self._fail(label, f'goal call failed: {error}', base_goal, started)
            if goal_handle is None or not goal_handle.accepted:
                return self._fail(label, 'motion goal was rejected', base_goal, started)
-        result_future = goal_handle.get_result_async()
+            result_future = goal_handle.get_result_async()
-        if not self._spin_until(result_future, self.command_timeout_sec):
+            if not self._spin_until(result_future, self.command_timeout_sec):
-            return self._fail(label, 'motion result timed out', base_goal, started)
+                return self._fail(label, 'motion result timed out', base_goal, started)
-        try:
+            try:
-            wrapped = result_future.result()
+                wrapped = result_future.result()
-            result = wrapped.result
+                result = wrapped.result
-        except Exception as error:
+            except Exception as error:
-            return self._fail(label, f'motion result failed: {error}', base_goal, started)
+                return self._fail(label, f'motion result failed: {error}', base_goal, started)
-        if not result.success:
+            if not result.success:
-            return self._fail(
+                return self._fail(
-                label, result.message or 'motion server reported failure',
+                    label, result.message or 'motion server reported failure',
-                base_goal, started,
+                    base_goal, started,
-            )
+                )
-        actual = self._settle_and_measure()
+            actual = self._settle_and_measure()
-        if actual is None:
+            last_actual = actual
-            return self._fail(label, 'actual tcp pose is unavailable after motion', base_goal, started)
+            if actual is None:
-        distance, angle = _pose_distance(actual, base_goal)
+                return self._fail(
-        if distance > self.position_tolerance or angle > self.orientation_tolerance:
+                    label, 'actual tcp pose is unavailable after motion',
                    base_goal, started,
                )
            distance, angle = _pose_distance(actual, base_goal)
            if distance <= self.position_tolerance and angle <= self.orientation_tolerance:
                return MotionResult(
                    True, 'motion completed', label, True, base_goal, actual,
                    time.monotonic() - started,
                )
            if attempt + 1 < self.pose_correction_attempts:
                # The action succeeded but the controller stopped outside the
                # pose tolerance.  Re-issue the same verified goal once; this
                # is a measured correction, not a blind repeat.
                if self.node is not None:
                    self.node.get_logger().warning(
                        f'{label}: correcting tcp pose '
                        f'({distance:.4f} m, {angle:.4f} rad off goal)'
                    )
                continue
            return self._fail(
                label,
                f'actual tcp is {distance:.4f} m / {angle:.4f} rad from the goal',
                base_goal,
                started,
-                actual=actual,
+                actual=last_actual,
            )
-        return MotionResult(
+        return self._fail(label, 'motion failed without a result', base_goal, started)
            True, 'motion completed', label, True, base_goal, actual,
            time.monotonic() - started,
        )
    def move_named(self, name: str, speed: float = 0.2, label: str = '') -> MotionResult:
        label = label or f'move_named({name})'
@@ -327,6 +346,26 @@ class MotionAdapter:
        except Exception as error:
            return self._fail(label, f'named motion failed: {error}', None, started)
        if not response.success:
            planning_failed = (
                'Планирование' in (response.message or '')
                or 'planning' in (response.message or '').lower()
                or 'недостижима' in (response.message or '')
            )
            pose = self.geometry.safe_poses.get(name)
            if planning_failed and pose is not None:
                # PTP can be rejected from some tool sockets.  Fall back to a
                # calibrated safe Cartesian pose with a collision-checked
                # OMPL plan; the final tcp pose is verified as usual.
                self.reset_stop()
                fallback = self.move_pose(
                    pose, planner='ompl', speed=speed, label=f'{label}:safe',
                )
                if fallback.success:
                    return MotionResult(
                        True, fallback.message, label, True, fallback.goal,
                        fallback.actual, time.monotonic() - started,
                    )
                return fallback
            return self._fail(
                label, response.message or 'named motion failed', None, started,
            )
@@ -71,6 +71,7 @@ class ObjectState:
            if spec.cap_state != 'absent'
            else None
        )
        self.cap_relative: Transform | None = None
        self.tools: dict[str, dict[str, Any]] = {}
        for tool in spec.tools:
            self.tools[tool.name] = {
@@ -216,11 +217,19 @@ class ObjectState:
        self._require_free_hand('grab_cap')
        if self.cap_state not in ('free', 'installed'):
            self._reject(f'hatch is not available (state={self.cap_state!r})')
-        self._require_close(
+        # The suction cup is rotationally symmetric.  Require the correct
-            tcp,
+        # position and a downward tcp, but allow any grasp yaw; the measured
-            self._expected_tcp(object_pose, self.geometry.cap_tcp_to_node),
+        # relative rotation is stored and reused for placement.
-            'grab_cap',
+        expected_position = object_pose.position + np.array([0.0, 0.0, 0.146])
-        )
+        distance = float(np.linalg.norm(tcp.position - expected_position))
        if distance > self.geometry.position_tolerance:
            self._reject(
                f'grab_cap: tcp is {distance:.4f} m from the hatch '
                f'(limit {self.geometry.position_tolerance:.4f} m)'
            )
        if tcp.matrix[2, 2] > -0.9:
            self._reject('grab_cap: tcp does not point down at the hatch')
        self.cap_relative = tcp.inverse().compose_relaxed(object_pose)
        self.cap_state = 'held'
        self.cap_pose = object_pose
        self.held_object = 'cap'
@@ -238,12 +247,13 @@ class ObjectState:
        self._require_instrument('vacuum', 'place_cap')
        if self.held_object != 'cap':
            self._reject('place_cap requires the hatch to be held')
        relative = self.cap_relative or self.geometry.cap_tcp_to_node
        self._require_close(
            tcp,
-            self._expected_tcp(target, self.geometry.cap_tcp_to_node),
+            self._expected_tcp(target, relative),
            'place_cap',
        )
-        if _position_distance(object_pose, tcp.compose_relaxed(self.geometry.cap_tcp_to_node)) > (
+        if _position_distance(object_pose, tcp.compose_relaxed(relative)) > (
            self.geometry.position_tolerance
        ):
            self._reject('place_cap: hatch does not follow the suction cup')
@@ -214,6 +214,7 @@ class ReferenceOperations:
        self.events: list[dict[str, Any]] = []
        self.completed_sealant: set[str] = set()
        self.last_sealant_point: tuple[str, float] | None = None
        self.cap_relative: Transform | None = None
    # ------------------------------------------------------------------ helpers
    def _emit(self, operation: str, **payload) -> None:
@@ -236,13 +237,25 @@ class ReferenceOperations:
        allow_fallback: bool = True,
    ) -> MotionResult:
        result = self.motion.move_pose(goal, planner=planner, speed=speed, label=label)
        if result.success:
            return result
        planning_failed = (
            'Планирование' in result.message
            or 'planning' in result.message.lower()
            or 'недостижима' in result.message
        )
        if not result.success and planning_failed:
            # A collision-free replan after a failed search is not a blind
            # repetition: no motion was executed and the scene is unchanged.
            self.motion.reset_stop()
            retry = self.motion.move_pose(
                goal, planner=planner, speed=speed, label=f'{label}:retry',
            )
            if retry.success:
                self._emit('motion_retry', label=label, planner=planner,
                           reason=result.message)
                return retry
            result = retry
        if result.success:
            return result
        if allow_fallback and planner != 'ompl' and planning_failed:
            # A short LIN segment can be rejected because the arm chose a
            # different IK branch.  A collision-checked OMPL retry is safe;
@@ -383,7 +396,15 @@ class ReferenceOperations:
        try:
            self._require_synchronized()
            cap_pose = self.supervisor.object_pose('grant_cap')
-            goal = tcp_goal(cap_pose, self.geometry.cap_tcp_to_node)
+            # Use a reachable wrist yaw; the suction cup is symmetric and the
            # measured relative rotation is used when the hatch is placed.
            goal = Transform.from_xyz_rpy(
                [cap_pose.xyz[0], cap_pose.xyz[1], cap_pose.xyz[2] + 0.146],
                [180.0, 0.0, 45.0],
                parent='world',
                child='tcp',
            )
            self.cap_relative = goal.inverse().compose_relaxed(cap_pose)
            self._contact(
                goal, 'pick_cap', remove_before_contact='grant_cap',
            )
@@ -400,7 +421,8 @@ class ReferenceOperations:
        try:
            self._require_synchronized()
            target = target_pose or world_target(self.spec, self.geometry, 'cap_seat')
-            goal = tcp_goal(target, self.geometry.cap_tcp_to_node)
+            relative = self.cap_relative or self.geometry.cap_tcp_to_node
            goal = tcp_goal(target, relative)
            self._contact(
                goal, 'place_cap',
                remove_before_contact='grant_cap',
@@ -536,6 +558,59 @@ class ReferenceOperations:
                       phase='failed', error=str(error))
            raise
    # ------------------------------------------------------------------ recovery
    # ------------------------------------------------------------------ recovery
    def simulate_grasp_failure(self, kind: str, name: str = '') -> OperationResult:
        """Move to a grasp pose, do not attach, and restore the object."""
        label = f'grasp_failed({kind}{":" + name if name else ""})'
        self._emit('grasp_failed', object=name or kind, phase='start')
        try:
            if kind == 'cap':
                pose = self.supervisor.object_pose('grant_cap')
                goal = tcp_goal(pose, self.geometry.cap_tcp_to_node)
                self._contact(goal, label, remove_before_contact='grant_cap')
                self._retreat(goal, label)
                self.bridge.add_world(
                    'grant_cap', self.geometry.cap_collision, pose,
                )
                self._require_synchronized()
            elif kind == 'screw':
                def_name = screw_object_id(name)
                pose = self.supervisor.object_pose(def_name)
                goal = tcp_goal(pose, self.geometry.screw_tcp_to_node)
                self._contact(goal, label, remove_before_contact=def_name)
                self._retreat(goal, label)
                self.bridge.add_world(
                    def_name, self.geometry.screw_collision, pose,
                )
                self._require_synchronized()
            else:
                raise OperationError(f'unknown grasp target {kind!r}')
            self._emit('grasp_failed', object=name or kind, phase='done')
            return OperationResult(True, 'grasp_failed', label, tuple(self.events))
        except Exception as error:
            self._emit('grasp_failed', object=name or kind,
                       phase='failed', error=str(error))
            raise
    def place_cap_shifted(self, dx: float = 0.003) -> OperationResult:
        target = world_target(self.spec, self.geometry, 'cap_seat')
        shifted = Transform(
            xyz=(target.xyz[0] + dx, target.xyz[1], target.xyz[2]),
            rotation=target.rotation,
            parent=target.parent,
            child=target.child,
        )
        self._emit('place_cap_shifted', object='cap', phase='start', dx=dx)
        try:
            result = self.place_cap(shifted)
            self._emit('place_cap_shifted', object='cap', phase='done', dx=dx)
            return result
        except Exception as error:
            self._emit('place_cap_shifted', object='cap', phase='failed',
                       dx=dx, error=str(error))
            raise
    # ------------------------------------------------------------------ verifies
    def inspect_cap(self, target_pose: Transform | None = None) -> dict[str, Any]:
        self.supervisor.command('inspect')
@@ -628,6 +628,27 @@ class PlanningSceneBridge:
                continue
            world, attached = scene_ids(scene)
            if expected_world <= world and expected_attached <= attached:
                # Drop stale extras from move_group's maintained scene.  A
                # removed tool must not be reintroduced by the relay.
                scene.world.collision_objects = [
                    obj for obj in scene.world.collision_objects
                    if obj.id in expected_world
                ]
                scene.robot_state.attached_collision_objects = [
                    entry for entry in scene.robot_state.attached_collision_objects
                    if entry.object.id in expected_attached
                ]
                # Keep the SRDF-derived ACM.  move_group can report an
                # empty matrix early in startup; relaying that would make
                # MoveItPy treat adjacent robot links as collisions.
                if self._acm_names:
                    scene.allowed_collision_matrix.entry_names = list(self._acm_names)
                    scene.allowed_collision_matrix.entry_values = [
                        AllowedCollisionEntry(
                            enabled=[bool(value) for value in row],
                        )
                        for row in self._acm_values
                    ]
                scene.is_diff = False
                scene.robot_state.is_diff = True
                self._publisher.publish(scene)
@@ -0,0 +1,214 @@
 #!/usr/bin/env python3
 """Execute one reference experiment after the scene and MoveIt are ready."""
 from __future__ import annotations
 import sys
 import time
 from pathlib import Path
 import rclpy
 from ament_index_python.packages import get_package_share_directory
 from rclpy.executors import ExternalShutdownException
 from rclpy.node import Node
 from std_msgs.msg import String
 from std_srvs.srv import Trigger
 from .geometry import load_geometry
 from .motion import MotionAdapter
 from .operations import ReferenceOperations, SupervisorClient
 from .planning_scene import PlanningSceneBridge
 from .reference_scenarios import build_reference_sequence, execute_sequence
 from .scenario import load_scenario
 from .trace import RunResult, TraceWriter
 SCENE_READY_SERVICE = '/iiwa_experement/scene_ready'
 STATUS_TOPIC = '/iiwa_experement/reference_status'
 class ReadinessError(RuntimeError):
    pass
 class ReferenceRunner(Node):
    def __init__(self) -> None:
        super().__init__('reference_runner')
        package_share = Path(get_package_share_directory('iiwa_experement'))
        self.declare_parameter('experiment_id', 1)
        self.declare_parameter(
            'scene_config', str(package_share / 'config' / 'scenes.yaml'),
        )
        self.declare_parameter(
            'geometry_path', str(package_share / 'config' / 'geometry.yaml'),
        )
        self.declare_parameter('start_delay_sec', 3.0)
        self.declare_parameter('trace_dir', '~/.ros/iiwa_experement/runs')
        self.declare_parameter('readiness_timeout_sec', 180.0)
        self.experiment_id = int(self.get_parameter('experiment_id').value)
        self.scene_config = Path(str(self.get_parameter('scene_config').value))
        self.geometry_path = Path(str(self.get_parameter('geometry_path').value))
        self.start_delay_sec = float(self.get_parameter('start_delay_sec').value)
        self.trace_dir = str(self.get_parameter('trace_dir').value)
        self.readiness_timeout_sec = float(
            self.get_parameter('readiness_timeout_sec').value
        )
        self.spec = load_scenario(self.experiment_id, self.scene_config)
        self.geometry = load_geometry(self.geometry_path)
        self.trace = TraceWriter(
            self.experiment_id, self.trace_dir, sim_time=self.simulation_time,
        )
        self.status_publisher = self.create_publisher(String, STATUS_TOPIC, 10)
        self._scene_ready_client = self.create_client(Trigger, SCENE_READY_SERVICE)
        self.supervisor = SupervisorClient(self)
        self.motion = MotionAdapter(
            self,
            self.geometry,
            scene_check=lambda: (True, 'ok'),
        )
        self.bridge = PlanningSceneBridge(node=self)
        self.operations = ReferenceOperations(
            self,
            self.spec,
            self.geometry,
            self.supervisor,
            self.bridge,
            self.motion,
            event_sink=self.trace.event,
        )
        self.scene_ready_time: float | None = None
    # ------------------------------------------------------------------ time
    def simulation_time(self) -> float:
        return self.get_clock().now().nanoseconds / 1e9
    def _spin(self, seconds: float = 0.1) -> None:
        end = time.monotonic() + seconds
        while time.monotonic() < end:
            rclpy.spin_once(self, timeout_sec=min(0.05, end - time.monotonic()))
    def _publish_status(self, text: str) -> None:
        message = String()
        message.data = text
        self.status_publisher.publish(message)
        self.get_logger().info(text)
    # ------------------------------------------------------------------ wait
    def wait_scene_ready(self) -> float:
        if not self._scene_ready_client.wait_for_service(
            timeout_sec=self.readiness_timeout_sec,
        ):
            raise ReadinessError(f'{SCENE_READY_SERVICE} did not appear')
        deadline = time.monotonic() + self.readiness_timeout_sec
        while time.monotonic() < deadline:
            future = self._scene_ready_client.call_async(Trigger.Request())
            end = time.monotonic() + 2.0
            while not future.done() and time.monotonic() < end:
                rclpy.spin_once(self, timeout_sec=0.05)
            if future.done() and future.result().success:
                self.scene_ready_time = self.simulation_time()
                return self.scene_ready_time
            self._spin(0.5)
        raise ReadinessError('scene_ready did not become true')
    def wait_motion_ready(self) -> None:
        deadline = time.monotonic() + self.readiness_timeout_sec
        while time.monotonic() < deadline:
            if self.motion.action_client.wait_for_server(timeout_sec=1.0):
                if self.motion.named_client.wait_for_service(timeout_sec=1.0):
                    break
            self._spin(0.2)
        else:
            raise ReadinessError('motion action or named service is unavailable')
        deadline = time.monotonic() + self.readiness_timeout_sec
        while time.monotonic() < deadline:
            rclpy.spin_once(self, timeout_sec=0.05)
            if self.simulation_time() <= 0.0:
                continue
            fresh, _ = self.motion._fresh()
            tcp = self.motion.actual_tcp()
            if fresh and tcp is not None:
                return
        raise ReadinessError('joint_states/TF did not become fresh')
    def wait_delay(self) -> None:
        if self.scene_ready_time is None:
            raise ReadinessError('scene_ready time is unknown')
        start_at = self.scene_ready_time + self.start_delay_sec
        self._publish_status(
            f'experiment {self.experiment_id}: ready; waiting until '
            f'sim {start_at:.2f} s'
        )
        while self.simulation_time() < start_at:
            rclpy.spin_once(self, timeout_sec=0.05)
        self._publish_status(
            f'experiment {self.experiment_id}: starting at '
            f'sim {self.simulation_time():.2f} s'
        )
    # ------------------------------------------------------------------ run
    def execute(self) -> RunResult:
        try:
            self._publish_status(f'experiment {self.experiment_id}: waiting for scene')
            self.wait_scene_ready()
            self.supervisor.wait_ready(self.readiness_timeout_sec)
            self.wait_motion_ready()
            self.bridge.initialize(self.spec, self.geometry)
            self.wait_delay()
        except Exception as error:
            result = RunResult(
                self.experiment_id, 'failed', f'readiness: {error}',
                simulation_time=self.simulation_time(),
            )
            self.trace.finish(result)
            self._publish_status(f'experiment {self.experiment_id}: failed: {error}')
            return result
        sequence = build_reference_sequence(self.spec)
        self.trace.event({
            'event': 'route',
            'steps': [step.as_dict() for step in sequence],
        })
        self._publish_status(f'experiment {self.experiment_id}: executing route')
        result = execute_sequence(sequence, self)
        self.trace.finish(result)
        self._publish_status(
            f'experiment {self.experiment_id}: {result.status}: {result.message}'
        )
        return result
    @property
    def trace_path(self) -> Path:
        return self.trace.path
 def main(args=None) -> None:
    rclpy.init(args=args)
    node = None
    exit_code = 0
    try:
        node = ReferenceRunner()
        result = node.execute()
        if result.status != 'completed':
            exit_code = 1
        print(f'[iiwa_experement] trace: {node.trace_path}')
    except (KeyboardInterrupt, ExternalShutdownException):
        pass
    except Exception as error:
        print(f'[iiwa_experement] reference runner failed: {error}', file=sys.stderr)
        exit_code = 1
    finally:
        if node is not None:
            node.trace.close()
            node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()
    if exit_code:
        raise SystemExit(exit_code)
 if __name__ == '__main__':
    main()
@@ -0,0 +1,267 @@
 """The twenty reference routes and their controlled failure branches."""
 from __future__ import annotations
 from dataclasses import dataclass, field
 from typing import Any, Mapping
 from .scenario import ScenarioSpec
 from .trace import RunResult
 class SequenceError(RuntimeError):
    """A reference step failed or produced an unexpected state."""
@dataclass(frozen=True)
 class ReferenceStep:
    kind: str
    args: tuple[Any, ...] = ()
    kwargs: Mapping[str, Any] = field(default_factory=dict)
    def as_dict(self) -> dict[str, Any]:
        return {'kind': self.kind, 'args': list(self.args), 'kwargs': dict(self.kwargs)}
 def _home(speed: float = 0.3) -> ReferenceStep:
    return ReferenceStep('move_named', ('home',), {'speed': speed})
 def _work(speed: float = 0.3) -> ReferenceStep:
    return ReferenceStep('move_named', ('work',), {'speed': speed})
 def _take(tool: str, *, first: bool = False) -> list[ReferenceStep]:
    steps: list[ReferenceStep] = []
    if not first:
        steps.append(_home())
    steps.extend([
        ReferenceStep('take_tool', (tool,)),
        _work(),
    ])
    return steps
 def _cap_steps(
    spec: ScenarioSpec,
    *,
    grasp_failure: bool = False,
    verify_failure: bool = False,
    first_tool: bool = False,
 ) -> list[ReferenceStep]:
    steps = _take('vacuum', first=first_tool)
    if grasp_failure:
        steps.extend([
            ReferenceStep('simulate_grasp_failure', ('cap',)),
            ReferenceStep('inspect_state'),
        ])
    steps.extend([ReferenceStep('pick_cap'), _work()])
    if verify_failure:
        shift = 0.003
        for failure in spec.failures:
            if failure.skill == 'verify' and 'after_first_install_x' in dict(failure.details):
                expected = float(dict(failure.details)['after_first_install_x'])
                shift = expected - spec.body.x
        steps.extend([
            ReferenceStep('place_cap_shifted', (), {'dx': shift}),
            ReferenceStep('inspect_cap', (), {'expect': 'not_ok'}),
            ReferenceStep('pick_cap'),
            _work(),
            ReferenceStep('place_cap'),
            ReferenceStep('inspect_cap', (), {'expect': 'ok'}),
        ])
    else:
        steps.extend([
            ReferenceStep('place_cap'),
            ReferenceStep('inspect_cap', (), {'expect': 'ok'}),
        ])
    steps.extend([
        _home(),
        ReferenceStep('dock_tool', ('vacuum',)),
        _home(),
    ])
    return steps
 def _sealant_steps(
    spec: ScenarioSpec,
    *,
    interrupted: bool = False,
    first_tool: bool = True,
    finish_home: bool = True,
 ) -> list[ReferenceStep]:
    sides = list(spec.sealant_sides)
    steps = _take('sealant_tool', first=first_tool)
    if interrupted:
        steps.append(ReferenceStep('dispense', ('side_1',)))
        steps.append(ReferenceStep('dispense', ('side_2',)))
        steps.extend([
            ReferenceStep('dispense', ('side_3', 0.0, 0.5)),
            ReferenceStep('inspect_state'),
            ReferenceStep('dispense', ('side_3', 0.5, 1.0)),
        ])
        steps.append(ReferenceStep('dispense', ('side_4',)))
    elif len(sides) == 1:
        steps.append(ReferenceStep('dispense', (sides[0],)))
    else:
        steps.append(ReferenceStep('dispense_contour', (tuple(sides),)))
    steps.extend([
        ReferenceStep('inspect_sealant', (), {'expect': 'ok'}),
        _home(),
        ReferenceStep('dock_tool', ('sealant_tool',)),
    ])
    if finish_home:
        steps.append(_home())
    return steps
 def _screw_steps(
    spec: ScenarioSpec,
    *,
    failure_index: int | None = None,
    first_tool: bool = False,
 ) -> list[ReferenceStep]:
    steps = _take('magnetic_tool', first=first_tool)
    # Prefer the closer tray cells.  The far +Y/+X corner can be outside the
    # comfortable IK envelope when the magnetic tool is mounted.
    available = ['screw_1', 'screw_2', 'screw_5', 'screw_6']
    selected = available[:len(spec.screw_holes)]
    for index, (screw, hole) in enumerate(zip(selected, spec.screw_holes)):
        if index > 0:
            steps.append(_home())
        if failure_index is not None and index == failure_index:
            steps.extend([
                ReferenceStep('simulate_grasp_failure', ('screw', screw)),
                ReferenceStep('inspect_state'),
            ])
        steps.extend([
            ReferenceStep('pick_screw', (screw,)),
            _work(),
            ReferenceStep('handoff_screw', (screw, hole)),
            _work(),
        ])
    steps.extend([
        _home(),
        ReferenceStep('dock_tool', ('magnetic_tool',)),
        _home(),
    ])
    return steps
 def build_reference_sequence(spec: ScenarioSpec) -> tuple[ReferenceStep, ...]:
    """Return the deterministic route for one experiment."""
    experiment = spec.experiment_id
    steps: list[ReferenceStep] = []
    if experiment in (1, 2, 3, 4, 5):
        steps.extend(_cap_steps(
            spec,
            grasp_failure=(experiment == 4),
            verify_failure=(experiment == 5),
            first_tool=True,
        ))
    elif experiment in (6, 7, 8, 9):
        steps.extend(_sealant_steps(
            spec, interrupted=(experiment == 9), first_tool=True,
        ))
    elif experiment == 10:
        steps.extend(_sealant_steps(
            spec, interrupted=False, first_tool=True, finish_home=False,
        ))
        steps.extend(_cap_steps(spec, first_tool=False))
    elif 11 <= experiment <= 15:
        steps.extend(_screw_steps(
            spec,
            failure_index=2 if experiment == 15 else None,
            first_tool=True,
        ))
    else:
        interrupted = experiment == 20
        steps.extend(_sealant_steps(
            spec, interrupted=interrupted, first_tool=True, finish_home=False,
        ))
        steps.extend(_cap_steps(
            spec,
            verify_failure=(experiment == 20),
            first_tool=False,
        ))
        steps.extend(_screw_steps(
            spec,
            failure_index=0 if experiment == 20 else None,
            first_tool=False,
        ))
    return tuple(steps)
 def execute_sequence(sequence: tuple[ReferenceStep, ...], context) -> RunResult:
    """Execute a route, stopping at the first unsafe or unexpected outcome."""
    operations = context.operations
    motion = context.motion
    trace = context.trace
    delivered: list[tuple[str, str]] = []
    returned: list[str] = []
    home_reached = False
    message = 'completed'
    status = 'completed'
    try:
        for step in sequence:
            trace.event({'event': 'step', 'step': step.as_dict(), 'phase': 'start'})
            if step.kind == 'move_named':
                result = motion.move_named(*step.args, **step.kwargs)
                if not result.success:
                    raise SequenceError(result.message)
                if step.args and step.args[0] == 'home':
                    home_reached = True
            elif step.kind == 'inspect_cap':
                result = operations.inspect_cap()
                trace.event({'event': 'inspect_cap', 'result': result})
                expect = step.kwargs.get('expect')
                if expect == 'ok' and not result.get('ok'):
                    raise SequenceError(f'hatch verification failed: {result}')
                if expect == 'not_ok' and result.get('ok'):
                    raise SequenceError('expected a failed hatch verification')
            elif step.kind == 'inspect_sealant':
                result = operations.inspect_sealant()
                trace.event({'event': 'inspect_sealant', 'result': result})
                if step.kwargs.get('expect') == 'ok' and not result.get('ok'):
                    raise SequenceError(f'sealant verification failed: {result}')
            elif step.kind == 'inspect_state':
                result = operations.inspect_state()
                trace.event({'event': 'inspect_state',
                             'mounted_tool': result.get('mounted_tool'),
                             'held_object': result.get('held_object')})
            elif step.kind == 'dock_tool':
                operations.dock_tool(*step.args, **step.kwargs)
                returned.extend(step.args)
            else:
                method = getattr(operations, step.kind, None)
                if method is None:
                    raise SequenceError(f'unknown reference step {step.kind!r}')
                result = method(*step.args, **step.kwargs)
                if hasattr(result, 'success') and not result.success:
                    raise SequenceError(result.message)
            trace.event({'event': 'step', 'step': step.as_dict(), 'phase': 'done'})
    except Exception as error:
        status = 'stopped'
        message = str(error)
        motion.stop(message)
        trace.event({'event': 'stopped', 'reason': message})
    state = context.supervisor.latest or {}
    for name, entry in (state.get('screws') or {}).items():
        if entry.get('state') == 'delivered' and entry.get('hole'):
            delivered.append((name, entry['hole']))
    cap = operations.inspect_cap() if operations.spec.cap_state != 'absent' else None
    sealant = operations.inspect_sealant() if operations.spec.sealant_sides else None
    return RunResult(
        experiment_id=operations.spec.experiment_id,
        status=status,
        message=message,
        events=list(operations.events),
        delivered=delivered,
        cap=cap,
        sealant=sealant,
        returned_tools=returned,
        home_reached=home_reached,
        simulation_time=context.simulation_time(),
    )
@@ -2,6 +2,7 @@
 from __future__ import annotations
 import re
 import time
 from dataclasses import dataclass, field
 from pathlib import Path
@@ -9,6 +10,7 @@ import rclpy
 from ament_index_python.packages import get_package_share_directory
 from rclpy.executors import ExternalShutdownException
 from rclpy.node import Node
 from sensor_msgs.msg import JointState
 from std_srvs.srv import Trigger
 from webots_ros2_msgs.srv import SpawnNodeFromString
@@ -19,7 +21,7 @@ from .webots_nodes import build_nodes
 SPAWN_SERVICE = '/Ros2Supervisor/spawn_node_from_string'
 READY_SERVICE = '/iiwa_experement/scene_ready'
 DEFAULT_SERVICE_TIMEOUT = 120.0
-DEFAULT_RESPONSE_TIMEOUT = 30.0
+DEFAULT_RESPONSE_TIMEOUT = 60.0
 _NAME_RE = re.compile(r'name "([^"]+)"')
@@ -148,6 +150,10 @@ class SceneSpawnerNode(Node):
        )
        self.declare_parameter('experiment_id', 0)
        self.declare_parameter('scene_config', default_config)
        self.declare_parameter('service_timeout_sec', DEFAULT_SERVICE_TIMEOUT)
        self.declare_parameter('response_timeout_sec', DEFAULT_RESPONSE_TIMEOUT)
        self.declare_parameter('startup_delay_sec', 0.0)
        self.declare_parameter('robot_timeout_sec', 120.0)
        self._readiness = Readiness()
        self.create_service(Trigger, READY_SERVICE, self._handle_ready)
@@ -156,6 +162,30 @@ class SceneSpawnerNode(Node):
        response.message = self._readiness.message
        return response
    def _wait_for_robot(self) -> None:
        """Wait until the URDF robot and its controller are publishing.
        The Ros2Supervisor service has a single executor: starting object
        spawns at the same moment as the URDF/controller spawn can deadlock
        both requests.  Waiting for /joint_states serialises them.
        """
        ready = {'value': False}
        def on_joint_state(message) -> None:
            if len(message.position) > 0:
                ready['value'] = True
        self.create_subscription(JointState, '/joint_states', on_joint_state, 10)
        timeout = max(0.0, float(self.get_parameter('robot_timeout_sec').value))
        deadline = time.monotonic() + timeout
        while not ready['value'] and time.monotonic() < deadline:
            rclpy.spin_once(self, timeout_sec=0.2)
        if not ready['value']:
            raise SpawnError(
                f'/joint_states did not appear within {timeout:g} s; '
                'robot/controller is not ready'
            )
    def run(self) -> None:
        experiment_id = int(self.get_parameter('experiment_id').value)
        scene_config = str(self.get_parameter('scene_config').value)
@@ -171,7 +201,21 @@ class SceneSpawnerNode(Node):
        plan = build_plan(experiment_id, scene_config, asset_root)
        self._readiness = plan.readiness
        client = self.create_client(SpawnNodeFromString, SPAWN_SERVICE)
-        execute_plan(plan, client, self)
+        delay = max(0.0, float(self.get_parameter('startup_delay_sec').value))
        if delay:
            self.get_logger().info(f'Waiting {delay:g} s for Webots to settle')
            time.sleep(delay)
        execute_plan(
            plan,
            client,
            self,
            service_timeout_sec=float(
                self.get_parameter('service_timeout_sec').value
            ),
            response_timeout_sec=float(
                self.get_parameter('response_timeout_sec').value
            ),
        )
        self.get_logger().info(self._readiness.message)
@@ -0,0 +1,75 @@
 """Machine-readable JSONL trace for one reference run."""
 from __future__ import annotations
 import json
 from dataclasses import dataclass, field
 from datetime import datetime, timezone
 from pathlib import Path
 from typing import Any, Callable
@dataclass
 class RunResult:
    experiment_id: int
    status: str
    message: str
    events: list[dict[str, Any]] = field(default_factory=list)
    delivered: list[tuple[str, str]] = field(default_factory=list)
    cap: dict[str, Any] | None = None
    sealant: dict[str, Any] | None = None
    returned_tools: list[str] = field(default_factory=list)
    home_reached: bool = False
    simulation_time: float | None = None
    def as_dict(self) -> dict[str, Any]:
        return {
            'experiment_id': self.experiment_id,
            'status': self.status,
            'message': self.message,
            'delivered': [
                {'screw': screw, 'hole': hole} for screw, hole in self.delivered
            ],
            'cap': self.cap,
            'sealant': self.sealant,
            'returned_tools': list(self.returned_tools),
            'home_reached': self.home_reached,
            'simulation_time': self.simulation_time,
        }
 class TraceWriter:
    """Write one JSON object per line and flush after every event."""
    def __init__(
        self,
        experiment_id: int,
        trace_dir: str | Path,
        *,
        sim_time: Callable[[], float] | None = None,
    ) -> None:
        directory = Path(str(trace_dir)).expanduser()
        directory.mkdir(parents=True, exist_ok=True)
        stamp = datetime.now(timezone.utc).strftime('%Y%m%dT%H%M%SZ')
        self.path = directory / f'experiment_{experiment_id:02d}_{stamp}.jsonl'
        self._handle = self.path.open('w', encoding='utf-8')
        self._sim_time = sim_time
    def event(self, event: dict[str, Any]) -> None:
        payload = dict(event)
        if self._sim_time is not None:
            payload.setdefault('simulation_time', float(self._sim_time()))
        self._handle.write(json.dumps(payload, sort_keys=True) + '\n')
        self._handle.flush()
    def finish(self, result: RunResult) -> None:
        self.event({'event': 'result', 'result': result.as_dict()})
        self._handle.flush()
    def close(self) -> None:
        self._handle.close()
    def __enter__(self) -> 'TraceWriter':
        return self
    def __exit__(self, *exc) -> None:
        self.close()
@@ -160,10 +160,13 @@ def runtime_actions(
            executable='reference_runner',
            name='reference_runner',
            output='screen',
-            parameters=[runner_parameters(
+            parameters=[
-                experiment_id, scene_config, geometry_path,
+                runner_parameters(
-                start_delay_sec, trace_dir,
+                    experiment_id, scene_config, geometry_path,
-            )],
+                    start_delay_sec, trace_dir,
                ),
                {'use_sim_time': True},
            ],
        ))
    if cleanup_dir is not None:
        actions.append(RegisterEventHandler(
@@ -27,6 +27,7 @@ setup(
        'console_scripts': [
            'scene_spawner = iiwa_experement.scene_spawner:main',
            'grant_supervisor = iiwa_experement.grant_supervisor:main',
            'reference_runner = iiwa_experement.reference_runner:main',
        ],
    },
 )
@@ -80,6 +80,7 @@ def make_node(experiment_id=1):
    node.state = ObjectState(spec, geometry)
    node.tool_held = None
    node.payload = None
    node.removed_defs = set()
    node.rack_poses = {
        name: entry['pose'] for name, entry in node.state.tools.items()
    }
@@ -206,8 +207,9 @@ def test_grab_and_handoff_remove_only_the_selected_screw():
        'h',
    )
    assert accepted, error
-    assert screw_node.removed is True
+    assert screw_node.translation.value == pytest.approx([100.0, 100.0, -100.0])
-    assert node.nodes['grant_screw_02'].removed is False
+    assert 'grant_screw_01' in node.removed_defs
    assert 'grant_screw_02' not in node.removed_defs
    assert node.state.holes['hole_1']['state'] == 'empty'
@@ -205,3 +205,23 @@ def test_rejected_command_preserves_version_and_snapshot():
    with pytest.raises(CommandError):
        state.attach_tool('vacuum', state.cap_pose, state.cap_pose)
    assert state.snapshot()['version'] == snapshot['version']
 def test_hatch_can_be_grasped_at_a_reachable_yaw_and_rotated_to_the_seat():
    state = make_state(2)
    cap_pose = state.cap_pose
    goal = type(cap_pose).from_xyz_rpy(
        [cap_pose.xyz[0], cap_pose.xyz[1], cap_pose.xyz[2] + 0.146],
        [180.0, 0.0, 45.0],
        parent='world',
        child='tcp',
    )
    attach_tool(state, 'vacuum')
    state.grab_cap(goal, cap_pose)
    assert state.cap_relative is not None
    target = world_target(state.spec, state.geometry, 'cap_seat')
    place_tcp = tcp_goal(target, state.cap_relative)
    place_object = place_tcp.compose_relaxed(state.cap_relative)
    state.place_cap(target, place_tcp, place_object)
    assert state.cap_state == 'installed'
    assert state.cap_pose.position == pytest.approx(target.position, abs=1e-9)
@@ -7,6 +7,12 @@ from ament_index_python.packages import get_package_share_directory
 PACKAGE = Path(__file__).resolve().parents[1]
 def test_package_installs_both_run_mode_executables():
    text = (PACKAGE / 'setup.py').read_text(encoding='utf-8')
    assert 'grant_supervisor = iiwa_experement.grant_supervisor:main' in text
    assert 'reference_runner = iiwa_experement.reference_runner:main' in text
 def test_package_declares_its_ros_dependencies():
    manifest = (PACKAGE / 'package.xml').read_text(encoding='utf-8')
    for dependency in (
@@ -48,8 +54,11 @@ def test_expected_resources_are_installed():
        'doc/experements.md',
        'doc/experiment-scenes.md',
        'doc/README.md',
        'doc/reference-control-qa.md',
        'worlds/grant/iiwa-grant.wbt',
        'config/scenes.yaml',
        'config/geometry.yaml',
        'config/moveit_cpp.yaml',
        'launch/experiment.launch.py',
    ):
        path = share / relative
@@ -0,0 +1,50 @@
 """Trace files and final results are machine-readable and flushed."""
 import json
 from pathlib import Path
 from iiwa_experement.trace import RunResult, TraceWriter
 def test_trace_writer_creates_and_flushes_jsonl(tmp_path):
    writer = TraceWriter(7, tmp_path / 'runs', sim_time=lambda: 12.5)
    writer.event({'operation': 'take_tool', 'object': 'vacuum', 'phase': 'start'})
    result = RunResult(
        experiment_id=7,
        status='completed',
        message='ok',
        delivered=[('screw_1', 'hole_1')],
        returned_tools=['magnetic_tool'],
        home_reached=True,
        simulation_time=20.0,
    )
    writer.finish(result)
    writer.close()
    assert writer.path.parent == tmp_path / 'runs'
    assert writer.path.name.startswith('experiment_07_')
    lines = [
        json.loads(line)
        for line in writer.path.read_text(encoding='utf-8').splitlines()
    ]
    assert lines[0]['operation'] == 'take_tool'
    assert lines[0]['simulation_time'] == 12.5
    final = lines[-1]['result']
    assert final['experiment_id'] == 7
    assert final['status'] == 'completed'
    assert final['delivered'] == [{'screw': 'screw_1', 'hole': 'hole_1'}]
    assert final['home_reached'] is True
 def test_trace_events_are_flushed_before_close(tmp_path):
    writer = TraceWriter(1, tmp_path, sim_time=lambda: 1.0)
    writer.event({'event': 'one'})
    text = writer.path.read_text(encoding='utf-8')
    assert '"event": "one"' in text
    writer.close()
 def test_trace_writer_expands_user_path(monkeypatch, tmp_path):
    monkeypatch.setenv('HOME', str(tmp_path))
    writer = TraceWriter(2, '~/trace')
    writer.close()
    assert writer.path.parent == tmp_path / 'trace'
@@ -0,0 +1,264 @@
 """All twenty routes differ by task and use only the agreed skills."""
 from pathlib import Path
 from iiwa_experement.reference_scenarios import (
    ReferenceStep,
    build_reference_sequence,
    execute_sequence,
 )
 from iiwa_experement.scenario import load_scenario
 from iiwa_experement.trace import RunResult
 PACKAGE = Path(__file__).resolve().parents[1]
 SCENES = PACKAGE / 'config' / 'scenes.yaml'
 def spec(experiment_id):
    return load_scenario(experiment_id, SCENES)
 def route(experiment_id):
    return build_reference_sequence(spec(experiment_id))
 def kinds(experiment_id):
    return [step.kind for step in route(experiment_id)]
 def test_every_route_is_nonempty_and_returns_home():
    for experiment_id in range(1, 21):
        names = kinds(experiment_id)
        assert names
        assert 'tighten' not in names
        assert 'seat_screw' not in names
        assert 'move_named' in names
        assert any(
            step.kind == 'move_named' and step.args == ('home',)
            for step in route(experiment_id)
        )
        assert names[-2:] == ['dock_tool', 'move_named'] or names[-1] == 'move_named' or 'dock_tool' in names
 def test_cap_experiments_use_the_vacuum_tool():
    for experiment_id in range(1, 6):
        names = kinds(experiment_id)
        assert 'take_tool' in names
        take = next(step for step in route(experiment_id) if step.kind == 'take_tool')
        assert take.args == ('vacuum',)
        assert 'pick_cap' in names
        assert 'place_cap' in names
        assert 'inspect_cap' in names
 def test_sealant_experiments_use_only_the_sealant_tool():
    for experiment_id in range(6, 10):
        names = kinds(experiment_id)
        takes = [step.args[0] for step in route(experiment_id) if step.kind == 'take_tool']
        assert takes == ['sealant_tool']
        assert any(name in names for name in ('dispense', 'dispense_contour'))
        assert 'inspect_sealant' in names
 def test_full_assembly_tool_order_is_sealant_cap_screws():
    for experiment_id in range(16, 21):
        takes = [step.args[0] for step in route(experiment_id) if step.kind == 'take_tool']
        assert takes == ['sealant_tool', 'vacuum', 'magnetic_tool']
        assert 'torque_tool' not in takes
 def test_screw_counts_and_holes_match_the_contract():
    expected = {
        11: 1, 12: 2, 13: 3, 14: 4, 15: 4,
        16: 1, 17: 2, 18: 4, 19: 4, 20: 4,
    }
    for experiment_id, count in expected.items():
        handoffs = [
            step for step in route(experiment_id) if step.kind == 'handoff_screw'
        ]
        assert len(handoffs) == count
        screws = [step.args[0] for step in handoffs]
        holes = [step.args[1] for step in handoffs]
        assert len(set(screws)) == count
        assert len(set(holes)) == count
    assert [step.args[1] for step in route(14) if step.kind == 'handoff_screw'] == [
        'hole_1', 'hole_3', 'hole_2', 'hole_4',
    ]
    assert [step.args[1] for step in route(13) if step.kind == 'handoff_screw'] == [
        'hole_1', 'hole_2', 'hole_3',
    ]
 def test_cap_grasp_failure_branch_is_one_retry():
    names = kinds(4)
    assert names.index('simulate_grasp_failure') < names.index('pick_cap')
    assert names.count('simulate_grasp_failure') == 1
    assert names.count('pick_cap') == 1
 def test_cap_verification_failure_branch_uses_one_correction():
    names = kinds(5)
    assert names.count('place_cap_shifted') == 1
    assert names.count('place_cap') == 1
    assert names.count('pick_cap') == 2
    inspections = [
        step for step in route(5) if step.kind == 'inspect_cap'
    ]
    assert [step.kwargs.get('expect') for step in inspections] == ['not_ok', 'ok']
 def test_interrupted_sealant_continues_from_the_break():
    for experiment_id in (9, 20):
        steps = [step for step in route(experiment_id) if step.kind == 'dispense']
        partial = [step for step in steps if step.args[0] == 'side_3']
        assert partial[0].args == ('side_3', 0.0, 0.5)
        assert partial[1].args == ('side_3', 0.5, 1.0)
        names = kinds(experiment_id)
        assert names.index('inspect_state') > names.index('dispense')
 def test_screw_grasp_failure_targets_the_third_and_first_selected_screw():
    fifteen = [
        step for step in route(15) if step.kind == 'simulate_grasp_failure'
    ]
    assert len(fifteen) == 1
    assert fifteen[0].args == ('screw', 'screw_5')
    twenty = [
        step for step in route(20) if step.kind == 'simulate_grasp_failure'
    ]
    assert len(twenty) == 1
    assert twenty[0].args == ('screw', 'screw_1')
 def test_experiment_nineteen_route_uses_the_shifted_scenario_objects():
    scenario = spec(19)
    assert scenario.tray_pose is not None
    names = kinds(19)
    assert names.count('handoff_screw') == 4
    # The route names no literal coordinates; actual poses come from the
    # supervisor state populated from the shifted scene.
    for step in route(19):
        for arg in step.args:
            assert not isinstance(arg, (int, float))
 class FakeOperationResult:
    def __init__(self, success=True, message='ok'):
        self.success = success
        self.message = message
 class FakeOperations:
    def __init__(self, spec):
        self.spec = spec
        self.events = []
        self.calls = []
        self.completed_sealant = set(spec.sealant_sides)
    def take_tool(self, name):
        self.calls.append(('take_tool', name))
        return FakeOperationResult()
    def dock_tool(self, name):
        self.calls.append(('dock_tool', name))
        return FakeOperationResult()
    def pick_cap(self):
        self.calls.append(('pick_cap',))
        return FakeOperationResult()
    def place_cap(self, target=None):
        self.calls.append(('place_cap',))
        return FakeOperationResult()
    def place_cap_shifted(self, dx=0.003):
        self.calls.append(('place_cap_shifted', dx))
        return FakeOperationResult()
    def pick_screw(self, screw):
        self.calls.append(('pick_screw', screw))
        return FakeOperationResult()
    def handoff_screw(self, screw, hole):
        self.calls.append(('handoff_screw', screw, hole))
        return FakeOperationResult()
    def simulate_grasp_failure(self, kind, name=''):
        self.calls.append(('simulate_grasp_failure', kind, name))
        return FakeOperationResult()
    def dispense(self, *args, **kwargs):
        self.calls.append(('dispense',) + args)
        return FakeOperationResult()
    def dispense_contour(self, sides):
        self.calls.append(('dispense_contour', tuple(sides)))
        return FakeOperationResult()
    def inspect_cap(self):
        return {'ok': True}
    def inspect_sealant(self):
        return {'ok': True}
    def inspect_state(self):
        return {'mounted_tool': None}
 class FakeMotion:
    def __init__(self, fail=None):
        self.calls = []
        self.fail = fail
    def move_named(self, name, **kwargs):
        self.calls.append(name)
        if self.fail == name:
            return FakeOperationResult(False, 'named motion failed')
        return FakeOperationResult()
    def stop(self, reason):
        self.calls.append(('stop', reason))
 class FakeSupervisor:
    latest = {
        'screws': {
            'screw_1': {'state': 'delivered', 'hole': 'hole_1'},
        }
    }
 class FakeContext:
    def __init__(self, operations, motion):
        self.operations = operations
        self.motion = motion
        self.supervisor = FakeSupervisor()
        self.trace = self
        self.events = []
    def event(self, event):
        self.events.append(event)
    def simulation_time(self):
        return 12.5
 def test_execute_sequence_runs_a_short_route_and_reports_delivery():
    scenario = spec(11)
    operations = FakeOperations(scenario)
    context = FakeContext(operations, FakeMotion())
    result = execute_sequence(route(11), context)
    assert result.status == 'completed'
    assert result.delivered == [('screw_1', 'hole_1')]
    assert result.home_reached is True
    assert result.returned_tools == ['magnetic_tool']
 def test_execute_sequence_stops_on_failed_named_motion():
    scenario = spec(11)
    operations = FakeOperations(scenario)
    context = FakeContext(operations, FakeMotion(fail='work'))
    result = execute_sequence(route(11), context)
    assert result.status == 'stopped'
    assert 'named motion failed' in result.message
    assert any(event.get('event') == 'stopped' for event in context.events)
@@ -143,3 +143,14 @@ def test_missing_proto_is_reported(tmp_path):
 def test_missing_base_setting_is_reported(tmp_path):
    with pytest.raises(FileNotFoundError, match='base settings'):
        prepare(tmp_path, base_setting=tmp_path / 'missing.yaml')
 def test_temporary_setting_uses_the_package_moveit_cpp(tmp_path):
    moveit_cpp = PACKAGE / 'config' / 'moveit_cpp.yaml'
    _, setting_path = prepare(tmp_path, moveit_cpp_path=moveit_cpp)
    data = yaml.safe_load(setting_path.read_text(encoding='utf-8'))
    configured = data['controller']['moveit']['moveit_cpp']
    assert configured == str(moveit_cpp.resolve())
    assert 'moveit_cpp/monitored_planning_scene' in moveit_cpp.read_text(
        encoding='utf-8'
    )