change validator

- _contact transfer/approach and _retreat clear now use deterministic Pilz
  PTP joint interpolation; OMPL remains only the automatic fallback
- move_to_joints on the motion server tries PTP first, OMPL on failure
- verified in Webots execute mode: 1, 11, 14, 20 completed; no PTP failures
  in 1/11/14/20 server logs, OMPL used only as the fallback

.gitignore

@@ -17,10 +17,8 @@ venv/
 .codex/
 __pycache__/
 .pytest_cache/
-output/
+!output/
+output/reference_photo/
 
 *.egg-info
-**.FCBak
-
-CLAUDE.md
-AGENTS.md
+**.FCBak

AGENTS.md

@@ -0,0 +1,48 @@
+# Repository Guidelines
+
+## Project Structure & Module Organization
+
+Lightweight Cobot targets Ubuntu 24.04, ROS 2 Jazzy, and the KUKA LBR iiwa 7 R800.
+
+- `src/iiwa_controller/`: C++ FRI hardware interface; bundled SDK in `external/libFRI/`.
+- `src/iiwa_planning/` and `src/iiwa_msgs/`: motion planning nodes and ROS actions/services.
+- `src/iiwa_bringup/`, `src/iiwa_config/`, and `src/iiwa_description/`: launch files, configuration, URDF/Xacro, meshes, and Webots worlds.
+- `src/iiwa_utils/` and `src/iiwa_web/`: Python utilities and REST/WebSocket/MCP interfaces.
+- `src/iiwa_sunrise/`: controller-side Java applications, built separately in Sunrise Workbench.
+- `cobot/`: Python CLI; `scripts/` and `docker/`: environment setup.
+- `doc/lwc-doc/docs/`: bilingual documentation (`*.md` and `*.en.md`).
+
+## Build, Test, and Development Commands
+
+Run commands from the repository root after installing prerequisites with `cobot setup`.
+
+```bash
+source /opt/ros/jazzy/setup.bash
+colcon build --base-paths src --symlink-install
+source install/setup.bash
+colcon test --base-paths src
+colcon test-result --verbose
+```
+
+These commands build the workspace, activate its packages, and run/report registered tests. Specify `--base-paths src` because the repository root contains `COLCON_IGNORE`.
+
+- `cobot rebuild iiwa_controller --symlink-install`: rebuild one package; build dependencies first.
+- `cobot run local`: launch interactively; select **Webots simulator** for development.
+- `cobot doc-setup`: serve documentation locally using Docker.
+- `cobot doc-setup build`: generate the static documentation and PDF.
+
+## Coding Style & Naming Conventions
+
+Use four-space indentation for Python and follow adjacent code for C++ formatting (the hardware interface uses two spaces). C++ targets C++17. Use `snake_case` for Python modules/functions, `PascalCase` for classes and ROS interface files, and `*.launch.py` for launch files. Preserve existing C++ API names. No repository-wide formatter configuration is present; avoid unrelated formatting and bundled SDK edits.
+
+## Testing Guidelines
+
+Manifests declare pytest for Python packages and ament lint dependencies for several CMake packages, but no dedicated test suite or coverage threshold is established. Place new tests under the relevant package's `test/` directory, using `test_*.py` for pytest, and register them with its build system. Record Webots smoke-test steps for motion or launch changes; distinguish simulation validation from physical-hardware validation.
+
+## Commit & Pull Request Guidelines
+
+History mixes imperative summaries with `feat:`, `fix:`, `docs:`, and `refactor:` prefixes; prefer a concise prefixed summary. Keep commits focused. PRs should explain the behavior change, affected packages, validation commands/results, and related issues. Include screenshots for visual changes and update both documentation languages when applicable.
+
+## Configuration & Hardware Safety
+
+Review `cobot-setting.yaml` before launching. Validate motion changes in simulation first. Physical runs require checking robot networking, joint limits, active tool/load data, control mode, and a clear work area with operator supervision.

cobot-setting.yaml

@@ -9,14 +9,14 @@ robot:
 
 digital_twin:
   tcp_gizmo:
-    enabled: true              # TCP-гизма в Webots; только в режиме симуляции
+    enabled: false              # TCP-гизма в Webots; только в режиме симуляции
     publish_period: 0.032      # период управления [с]
     command_timeout: 0.3       # максимальный возраст цели при отпускании мыши [с]
     state_timeout: 0.5         # допустимый возраст состояния робота [с]
     max_linear_speed: 0.50     # верхний предел скорости TCP [м/с]
     max_angular_speed: 0.4     # скорость вращения TCP [рад/с]
   webots:
-    world: pkg://iiwa_description/worlds/grant/body_hatch_installed.wbt
+    world: pkg://iiwa_description/worlds/iiwa.wbt
     # world: pkg://iiwa_description/worlds/simple_world.wbt
     transform: "-0.25 0 0.79"
     rotation: "0 0 1 0"
@@ -36,8 +36,8 @@ digital_twin:
         #   rotation_deg: [1.57, 0, 0]
         # - path: pkg://iiwa_description/objects/hammer/hammer5.obj
         #   rotation_deg: [1.57, 0, 0]
-        - path: pkg://iiwa_description/objects/grant/vint.obj
-          rotation_deg: [90, 0, 0]
+        # - path: pkg://iiwa_description/objects/grant/vint.obj
+          # rotation_deg: [90, 0, 0]
       area:                       # метры в системе основания робота, до падения
         x: [0.3, 0.7]
         y: [-0.3, 0.3]

docs/superpowers/plans/2026-11-08-grant-reference-control.md

@@ -0,0 +1,192 @@
+# План реализации эталонного управления Grant
+
+> **Для агента-разработчика:** выполняйте задачи по порядку, отмечайте выполненные шаги в чекбоксах.
+
+**Цель:** обеспечить воспроизводимое эталонное движение робота во всех 20 экспериментах Grant и добавить к существующей команде запуска режимы `view` и `execute`.
+
+**Архитектура:** существующий спавнер создаёт начальную сцену. Отдельный supervisor пакета `iiwa_experement` управляет захватом и позами объектов в Webots. Эталонный исполнитель синхронизирует его с геометрией MoveIt, вызывает существующий сервер движения `cobot/move_to_pose`/`cobot/move_to_named` и записывает действия в машинно-читаемый журнал.
+
+**Стек:** Ubuntu 24.04, ROS 2 Jazzy, `rclpy`, MoveIt 2/`moveit_msgs`, Webots R2025a, Python Supervisor, `webots_ros2_driver`, PyYAML, pytest.
+
+**Спецификация:** [Описание согласованной архитектуры](../specs/2026-11-08-grant-reference-control-design.md).
+
+## Общие ограничения
+
+- Работать в существующей ветке `experement`. Перед правками проверить `git status`. В файле `src/iiwa_experement/worlds/grant/iiwa-grant.wbt` уже есть чужие незакоммиченные изменения и побочные файлы Webots: сохранить их, не сбрасывать и не перезаписывать целиком.
+- Весь новый исполняемый код, конфигурация, launch, тесты и документация пакета размещаются в `src/iiwa_experement/`. OBJ/MTL использовать из `src/iiwa_description/objects/grant/`. Постоянный `patron` остаётся на роботе.
+- `mode:=view` — режим по умолчанию; робот не движется. `mode:=execute` начинает движение только после `scene_ready`, готовности контроллера, MoveIt и supervisor, появления свежего состояния робота и задержки 3,0 секунды симуляционного времени.
+- Запуск только в Webots: в включаемом bringup остаётся `simulate:=true`. Не отправлять эталонные траектории на физический робот через FRI.
+- Винты не вставляются и не затягиваются. Робот подносит винт к конкретному отверстию, отпускает его виртуальному оператору, после чего винт удаляется из сцены, а отверстие остаётся пустым. В эталонных маршрутах робот берёт затяжной инструмент и дотягивает винты, предварительно наживлённые человеком. Считается, что человек установил винты неплотно; окончательную затяжку выполняет робот.
+- Перемещения к инструментам, извлечение инструмента, перенос люка и винтов, посадка люка, проход соплом вдоль контура и подход к отверстиям выполняются реальными траекториями робота. Без движения меняются только состояние захвата и состояние передачи винта оператору.
+- Единицы: метры и радианы. Явно задавать преобразования `world`, `base_link`, `patron`, `tcp`, точки крепления и рабочего кончика инструмента. Использовать активный TF/настройки, а не координаты основания, переписанные из текста.
+- Перед каждым движением в MoveIt должны быть все препятствия и прикреплённые объекты; обновление сцены подтверждается до планирования. Контакт допускается только для нужного инструмента и цели на нужном этапе.
+- При неопределённой геометрии, столкновении, устаревшем состоянии, неудачном захвате или ошибке движения остановиться и записать причину. Не повторять движение вслепую.
+
+## Что прочитать перед правками
+
+1. `src/iiwa_experement/config/scenes.yaml`; `src/iiwa_experement/iiwa_experement/{scenario.py,scene_spawner.py,webots_nodes.py,runtime_config.py}`; `src/iiwa_experement/launch/experiment.launch.py`; `src/iiwa_experement/doc/{experements.md,experiment-scenes.md,README.md}`.
+2. `src/iiwa_description/urdf/tools/patron.xacro`; `src/iiwa_description/worlds/iiwa.wbt`; `src/iiwa_description/resource/protos/CollaborativeRobotTable.proto`; модели `src/iiwa_description/objects/grant/` и изображения `output/reference_photo/grant/`.
+3. `src/iiwa_planning/scripts/move_to_pose_server.py`; `src/iiwa_msgs/action/MoveToPose.action`; `src/iiwa_msgs/srv/MoveToNamedPose.srv`; `src/iiwa_config/config/moveit/iiwa7.srdf`; `src/iiwa_bringup/launch/supported/{moveit_nodes.py,webots_spawn.launch.py}`.
+4. Исходные описания экспериментов: `docs/superpowers/specs/2026-11-08-grant-experiments.md` и сцен: `docs/superpowers/specs/2026-11-08-grant-scenes.md`. После задачи 1 актуальной версией новых правил про винты будут файлы `src/iiwa_experement/doc/`.
+
+## Назначение файлов
+
+| Файлы | Назначение |
+| --- | --- |
+| `config/scenes.yaml`, `iiwa_experement/scenario.py`, `doc/experements.md`, `doc/experiment-scenes.md` | Обновлённые 20 заданий, состав инструментов и сбои; без установки и затяжки винтов |
+| `config/geometry.yaml`, `iiwa_experement/geometry.py` | Крепление и рабочие кончики инструментов, захват/посадка люка, контур, отверстия, точки передачи, измеренные зазоры |
+| `worlds/grant/iiwa-grant.wbt`, `iiwa_experement/webots_nodes.py` | Отдельный supervisor и устойчивые имена управляемых объектов |
+| `iiwa_experement/grant_supervisor.py`, `iiwa_experement/object_state.py` | Следование захваченных объектов за роботом, команды и автомат состояний |
+| `iiwa_experement/planning_scene.py` | Препятствия и прикреплённые объекты MoveIt |
+| `iiwa_experement/motion.py` | Вызовы существующего сервера движения, проверка фактической позы и остановка |
+| `iiwa_experement/operations.py` | Захват, возврат инструмента, перенос, герметик, люк, передача винтов |
+| `iiwa_experement/reference_scenarios.py` | Маршруты 1–20 и ограниченное восстановление после сбоев |
+| `iiwa_experement/reference_runner.py`, `iiwa_experement/trace.py` | Ожидание готовности, выполнение, остановка, журнал JSONL и итог |
+| `iiwa_experement/runtime_config.py`, `launch/experiment.launch.py`, `setup.py`, `package.xml`, `doc/README.md` | Режимы запуска, зависимости, исполняемые файлы и инструкция |
+| `test/test_*.py` | Проверки контрактов, геометрии, запуска, состояний, сцены MoveIt и маршрутов |
+
+## Проверки по всем задачам
+
+Каждая задача ниже содержит собственные тесты и проверку результата. Таблица показывает, какой результат проверяется сразу после задачи; задача 10 повторяет проверки в полных прогонах Webots.
+
+| Задача | Что проверить |
+| --- | --- |
+| 1. Контракт опытов | Все 20 заданий согласованы с передачей винта; в 11–20 по десять винтов, нет затяжки |
+| 2. Геометрия | Инструмент крепится на конце `patron`; позы люка, контура и отверстий меняются вместе с объектами |
+| 3. Supervisor | Захват и отпускание меняют состояние одного нужного объекта; при ошибке состояние не меняется |
+| 4. MoveIt | Геометрия препятствий и прикреплённых объектов совпадает с Webots и подтверждена сервером движения |
+| 5. Движение | Устаревшие `/joint_states`/TF, неподтверждённая сцена и отказ action блокируют следующую траекторию |
+| 6. Люк и герметик | Робот действительно переносит люк и ведёт сопло по нужным участкам; объекты не скачут к цели |
+| 7. Винты | Винт переносится магнитом к заданному отверстию и передаётся оператору; отверстие остаётся пустым |
+| 8. Маршруты | Все 20 последовательностей различаются по задаче; `grasp_failed` использует текущую позу и одну повторную попытку |
+| 9. Запуск | `view` не создаёт клиента движения, `execute` ждёт готовности и заданной задержки перед первым движением |
+| 10. Итог | Все 20 прогонов проверены в Webots; сбои останавливают небезопасное продолжение |
+
+---
+
+### Задача 1. Исправить контракт 20 экспериментов
+
+**Файлы:** изменить `src/iiwa_experement/config/scenes.yaml`, `src/iiwa_experement/iiwa_experement/scenario.py`, `src/iiwa_experement/doc/experements.md`, `src/iiwa_experement/doc/experiment-scenes.md` и затронутые `test/test_scenario.py`, `test/test_webots_nodes.py`, `test/test_package_layout.py`. Исторические копии спецификаций не переписывать молча; из документации пакета дать ссылку на новую спецификацию.
+
+**Интерфейс:** `load_scenario(experiment_id, manifest_path) -> ScenarioSpec` остаётся единственным источником начальных поз; сбои описываются в YAML.
+
+- [x] **Шаг 1. Написать проверки, которые пока не проходят.** В 14–15 и 16–20 нет `torque_tool`; в 11–20 ровно десять винтов. Нет сбоев `tighten` и `seat_screw`. В 15 сохранён `grab:grasp_failed` для третьего выбранного винта. Смещения сцены 3 и 19 сохранены.
+- [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`.
+- [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`, но не должна появляться в этих опытах.
+- [x] **Шаг 4. Повторить тесты.** Все тесты сцен проходят; документация пакета не называет переданный винт установленным.
+- [x] **Шаг 5. Зафиксировать изменение контракта отдельным коммитом.**
+
+### Задача 2. Измерить и описать рабочую геометрию
+
+**Файлы:** создать `config/geometry.yaml`, `iiwa_experement/geometry.py`, `test/test_geometry.py`; при необходимости обновить `setup.py`.
+
+**Интерфейсы:** `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`, зазоры подхода/передачи/переноса, допуски позы и ограниченные скорости. Единицы и исходный фрейм обязательны для каждого преобразования.
+
+- [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`.
+- [x] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_geometry.py`.
+- [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: измерить и занести её вместе с результатом проверки.
+- [x] **Шаг 4. Реализовать загрузку и композицию преобразований.** Отвергать отсутствующие фреймы, нечисловые значения и неположительные зазоры. Позы смещённых целей вычислять из `ScenarioSpec`.
+- [x] **Шаг 5. Прогнать тесты и визуально проверить все инструменты на конце `tcp`.** Результат измерений записать в документацию пакета.
+- [x] **Шаг 6. Зафиксировать геометрию отдельным коммитом.**
+
+### Задача 3. Добавить supervisor для захвата и перемещения объектов
+
+**Файлы:** изменить `worlds/grant/iiwa-grant.wbt`, `iiwa_experement/webots_nodes.py`, `launch/experiment.launch.py`, `test/test_world.py`, `test/test_webots_nodes.py`, `test/test_launch.py`; создать `iiwa_experement/grant_supervisor.py`, `iiwa_experement/object_state.py`, `test/test_object_state.py`, `test/test_grant_supervisor.py`; обновить `setup.py` и зависимости в `package.xml`.
+
+**Интерфейс:** добавить в мир второй `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`.
+
+- [x] **Шаг 1. Написать проверки автомата состояний.** Начальные состояния `tool=none`, `cap=free|installed|absent`, 10 доступных винтов. Запрещены захват без нужного инструмента, второй инструмент, второй переносимый объект, неверное отверстие и повторная передача. Переданный винт расходуется, отверстие остаётся `empty`. Отклонённая команда не меняет версию состояния.
+- [x] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_object_state.py src/iiwa_experement/test/test_grant_supervisor.py`.
+- [x] **Шаг 3. Сделать объекты адресуемыми.** У каждого управляемого `Solid` должен быть устойчивый `DEF` и прежнее поле `name`. Добавить невидимый внешний supervisor, сохранив стол, основного робота и встроенный `Ros2Supervisor`. Обновить тест мира: в шаблоне есть только один новый supervisor Robot, а iiwa и детали по-прежнему не предустановлены.
+- [x] **Шаг 4. Реализовать и подключить внешний контроллер.** Использовать отдельное имя Robot и шаблон окружения `Ros2SupervisorLauncher` (`WEBOTS_CONTROLLER_URL`, `WEBOTS_HOME`); добавить его процесс к существующему launch без автоматических команд движения. После `scene_ready` найти объекты, получить фактическую позу `patron`/`tcp` из Webots либо проверенной TF-цепи. На каждом шаге Webots пересчитывать позу удерживаемого объекта по калиброванному преобразованию. Менять `locked`/физику только там, где это нужно. Во время переноса объект следует за роботом и не переносится скачком к цели. Публиковать новое состояние после подтверждения Webots; при недоступной позе или объекте отказывать.
+- [x] **Шаг 5. Проверить модуль и выполнить пробу Webots без движения робота.** Supervisor находит `tcp` и все объекты начальной сцены, публикует их позы и отклоняет захват инструмента, когда робот ещё далеко от гнезда. Захват и следование инструмента при реальном движении проверить после задач 4–6, когда готовы сцена столкновений и адаптер движения.
+- [x] **Шаг 6. Зафиксировать supervisor и автомат состояний.**
+
+### Задача 4. Отразить мир Webots в сцене MoveIt
+
+**Файлы:** создать `iiwa_experement/planning_scene.py`, `test/test_planning_scene.py`; добавить нужные зависимости `moveit_msgs` и геометрических сообщений в `package.xml`.
+
+**Интерфейс:** `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. Траектория допускается только после синхронизации сцены с сервером движения.
+
+- [x] **Шаг 1. Написать проверки изменений сцены.** В мире MoveIt есть стол, стапель, корпус, стойка, лоток, люк, инструменты и все десять винтов, если они присутствуют в Webots. `attach` удаляет объект из мира и создаёт один прикреплённый к `tcp`/нужному звену объект. `detach` выполняет обратное действие; передача удаляет только выбранный винт. Разрешения на контакт не отключают проверки для всего робота.
+- [x] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_planning_scene.py`.
+- [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 расходятся — остановиться и сообщить ошибку синхронизации.
+- [x] **Шаг 4. Сверить RViz и Webots.** Начальные объекты совпадают по позам; прикреплённые инструмент/люк/винт движутся с роботом; оставшиеся винты остаются препятствиями.
+- [x] **Шаг 5. Зафиксировать мост к MoveIt.**
+
+### Задача 5. Сделать адаптер безопасных перемещений
+
+**Файлы:** создать `iiwa_experement/motion.py`, `test/test_motion.py`; добавить в `package.xml` зависимости `iiwa_msgs`, `sensor_msgs`, `tf2_ros` и другие фактически используемые.
+
+**Интерфейс:** `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`.
+
+- [x] **Шаг 1. Написать тесты с поддельными клиентами.** Цель не отправляется при устаревших `joint_states`/TF, неподтверждённой сцене, недопустимой скорости или уже остановленном роботе. Отказ, отмена и тайм-аут вызывают `stop` и блокируют следующий участок. После успеха проверяются фактические положение и ориентация TCP.
+- [x] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_motion.py`.
+- [x] **Шаг 3. Реализовать адаптер.** Свободный перенос — через `ptp`/OMPL с полной сценой столкновений; короткий осевой подход/отход и стороны контура — через `lin`. Ограничить время планирования/выполнения, проверять свежесть обратной связи и готовность контроллера. Не применять `iiwa_planning/direct_trajectory.py` и TCP gizmo для эталона. При `execute` выключать gizmo во временной конфигурации запуска, чтобы робот имел один источник команд.
+- [x] **Шаг 4. Проверить движение в Webots без захвата.** Из дома пройти в измеренную свободную точку над рабочей зоной и вернуться. Сверить путь и пределы суставов.
+- [x] **Шаг 5. Зафиксировать адаптер движения.**
+
+### Задача 6. Реализовать операции с инструментами, люком и герметиком
+
+**Файлы:** создать `iiwa_experement/operations.py`, `test/test_operations.py`.
+
+**Интерфейс:** `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.
+
+- [x] **Шаг 1. Написать тесты последовательностей.** Взятие инструмента: `дом/безопасная высота→точка над гнездом→осевое опускание→крепление→осевое извлечение`; возврат — обратный маршрут. Люк: `над люком→опускание→захват→подъём→перенос над корпусом→совмещение→опускание до посадки→отпускание→отход`. Люк должен двигаться вместе с TCP, без скачка к корпусу. Сопло проходит реальные 3D-участки четырёх сторон и смежные углы на рабочей высоте; сторона считается выполненной только после успешного движения.
+- [x] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_operations.py`.
+- [x] **Шаг 3. Реализовать операции через калиброванные фреймы.** Позы TCP вычислять по целевой позе рабочего кончика. Подходить и отходить вдоль локальной оси цели; безопасную высоту переноса вычислять по габаритам переносимого объекта и препятствий. Перед захватом/отпусканием проверять фактическую близость и состояние. В 5 выполнить первую смещённую посадку и одну корректировку. В 9/20 хранить последнюю подтверждённую точку герметика и продолжать от неё.
+- [x] **Шаг 4. Проверить отдельные операции в Webots через проверочный клиент.** Убедиться, что движутся робот, инструмент, люк и сопло, а объект не скачет к цели. Полные опыты 1, 6, 9 и 10 прогнать в задаче 10 после подключения эталонного исполнителя.
+- [x] **Шаг 5. Зафиксировать операции.**
+
+### Задача 7. Реализовать магнитный захват и передачу винта
+
+**Файлы:** дополнить `iiwa_experement/operations.py`, `iiwa_experement/object_state.py`, `test/test_operations.py`, `test/test_object_state.py`.
+
+**Интерфейс:** `pick_screw(screw_id)`, `handoff_screw(screw_id, hole_id)`. Итоговое событие — `delivered(screw_id,hole_id)`; состояний `seated` и `tightened` нет.
+
+- [x] **Шаг 1. Написать тесты.** Можно выбрать любой из десяти доступных винтов. Взятый винт следует за рабочим концом магнита. Подход к отверстию выполняется по его оси. Передача запрещена, если винт не удерживается или фактическая поза TCP вне допуска у нужного отверстия. Успешная передача удаляет только один винт из Webots и MoveIt, оставляет отверстие пустым, другие винты — в лотке.
+- [x] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_operations.py src/iiwa_experement/test/test_object_state.py`.
+- [x] **Шаг 3. Реализовать маршрут робота.** `над выбранным винтом→осевое опускание→захват→осевой подъём→перенос на безопасной высоте→над отверстием→совмещение осей→короткое опускание до точки передачи→отпускание оператору→осевой отход`. Не опускать винт внутрь люка или корпуса. Между отверстиями проходить через проверенную безопасную область, не перемещаться поперёк люка на контактной высоте.
+- [x] **Шаг 4. Проверить отдельную операцию передачи в Webots через проверочный клиент.** Робот и винт движутся вместе; сначала видны десять винтов, исчезает только переданный. Полные опыты 11, 12 и 14 прогнать в задаче 10.
+- [x] **Шаг 5. Зафиксировать передачу винтов.**
+
+### Задача 8. Описать 20 эталонных маршрутов и восстановление
+
+**Файлы:** создать `iiwa_experement/reference_scenarios.py`, `test/test_reference_scenarios.py`; при необходимости дополнить `operations.py` проверкой `inspect_state`.
+
+**Интерфейс:** `build_reference_sequence(spec: ScenarioSpec) -> tuple[Operation, ...]`; `execute_sequence(sequence, context) -> RunResult`. Имена целей соответствуют `doc/experements.md`. Успешный маршрут заканчивается возвратом инструмента и переходом в `home`.
+
+- [x] **Шаг 1. Написать табличные тесты для всех 20 ID.** Проверить порядок операций из задачи 1, возврат инструмента перед сменой, разные винты и нужное число отверстий. Для 3/19 брать смещённые реальные позы. Для 4/15/20: `grasp_failed→stop→inspect_state→повторное обнаружение→одна попытка`; для 5: `not_ok→одна корректировка люка`; для 9/20: продолжение с точки разрыва герметика. Повторный отказ и неизвестное состояние дают `stopped` без последующих движений. В журнале нет `seat_screw` и `tighten`.
+- [x] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_reference_scenarios.py`.
+- [x] **Шаг 3. Реализовать выбор маршрута и управляемые сбои.** Применять `ScenarioSpec.failures` по навыку, цели и номеру попытки. В `view` сбои не меняют сцену. Перед повтором читать фактическое состояние Webots и решать, повторить действие, пропустить уже выполненную передачу или остановиться.
+- [x] **Шаг 4. Проверить тесты и список операций для всех ID.** В 13 три передачи, в 14/15 четыре; в 19 используются смещённые люк и лоток.
+- [x] **Шаг 5. Зафиксировать маршруты.**
+
+### Задача 9. Добавить `view` и `execute` в существующий launch
+
+**Файлы:** изменить `iiwa_experement/runtime_config.py`, `launch/experiment.launch.py`, `setup.py`, `package.xml`, `test/test_launch.py`, `test/test_runtime_config.py`; создать `iiwa_experement/reference_runner.py`, `iiwa_experement/trace.py`, `test/test_reference_runner.py`.
+
+**Интерфейс команды:** `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 опыта, время симуляции, операцию, объект, цель, позы до/после, исход и изменение состояния.
+
+- [x] **Шаг 1. Написать тесты запуска.** Неверный режим и отрицательная/нечисловая задержка отклоняются до Webots. В `view` нет клиента движения и целей. В `execute` отсутствие `scene_ready`, MoveIt/контроллера, свежих `joint_states` или остановленные часы симуляции не допускают старт. Первое движение — не раньше `scene_ready + start_delay_sec` по времени симуляции. Тайм-аут готовности завершает runner с ошибкой без движения.
+- [x] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_launch.py src/iiwa_experement/test/test_reference_runner.py`.
+- [x] **Шаг 3. Реализовать ожидание и журнал.** Ждать `scene_ready`, supervisor, MoveIt, сервисы/действия движения, свежие TF и `joint_states`; затем выдержать задержку по `/clock`. Публиковать краткий прогресс. Писать JSONL в `trace_dir/experiment_<id>_<UTC-время>.jsonl`, раскрывая `~`, и сбрасывать данные после каждой операции. При успехе или остановке записать итог; Webots оставлять открытым для осмотра финальной сцены.
+- [x] **Шаг 4. Проверить режимы вручную.** `view` показывает неизменную сцену сколько угодно; `execute` ждёт три секунды симуляции и начинает видимое движение. Проверить также `start_delay_sec:=0` и `start_delay_sec:=10`.
+- [x] **Шаг 5. Зафиксировать launch и журнал.**
+
+### Задача 10. Проверить безопасность и все 20 прогонов
+
+**Файлы:** обновить `src/iiwa_experement/doc/README.md`; создать `src/iiwa_experement/doc/reference-control-qa.md` с измерениями, результатами проверки столкновений и таблицей 20 опытов. Тесты дополнять по найденным конкретным ошибкам.
+
+**Интерфейс:** команда запуска и формат журнала из задачи 9 — контракт для последующего сравнения эталона с планами LLM.
+
+- [x] **Шаг 1. Собрать пакет и запустить тесты.** `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`. Исправить ошибки до заявления о готовности.
+- [x] **Шаг 2. Проверить `view` для 1, 11, 16, 19, 20.** Сверить состав объектов, десять винтов, постоянный `patron`, отсутствие движения робота и изменений объектов после 10 секунд симуляции.
+- [x] **Шаг 3. Запустить `execute` для каждого ID 1–20.** Для каждого записать: видимые движения, точность крепления инструмента к TCP, журнал, отсутствие запрещённых столкновений и проникновений, итоговую позу люка, участки герметика, пары переданных винтов, возврат инструмента и `home`. Если поза недостижима или опасна, исправить геометрию/сцену и повторить опыт и соседние затронутые варианты. Не обходить проверку столкновений.
+- [x] **Шаг 4. Проверить ветви отказов 4, 5, 9, 15, 20.** Ровно одна разрешённая попытка восстановления; повторный отказ или неизвестное состояние останавливает робот. Винт не вставляется, отверстие пустое, затяжной инструмент не движется.
+- [x] **Шаг 5. Описать команды запуска, параметры, формат журнала, численные калибровки, результаты тестов, ограничения и режим только симуляции.** Примеры: `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`.
+- [x] **Шаг 6. Зафиксировать документацию и проверенные исправления.** Просмотреть `git diff`; не включать посторонние изменения рабочего дерева.
+
+## Критерии готовности
+
+В `view` сцена остаётся неподвижной. В `execute` робот выполняет все нужные движения для всех 20 сценариев; инструмент виден на кончике `patron`, люк перемещается вместе с роботом и оказывается на корпусе, сопло идёт по заданным участкам, каждый винт реально переносится к отверстию и передаётся виртуальному оператору без установки и затяжки. Сборка, тесты и документированные прогоны Webots успешны. Небезопасный или неудачный опыт остаётся незавершённым с явной причиной.

docs/superpowers/plans/2026-11-08-iiwa-experement-world.md

@@ -0,0 +1,147 @@
+# Universal Grant Webots World Implementation Plan
+
+> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
+
+**Goal:** Создать отдельный ROS 2 пакет `iiwa_experement`, из которого одной командой `ros2 launch` запускается универсальный мир Webots с детерминированной начальной сценой любого из 20 экспериментов.
+
+**Architecture:** Новый пакет владеет шаблоном мира, конфигурацией 20 сценариев, генератором Webots `Solid`, спавнером и launch-файлом. Launch создаёт временную копию мира с абсолютными ссылками на установленные PROTO из `iiwa_description` и временный YAML с путём к этой копии. Затем он использует неизменённый `iiwa_bringup` для запуска Webots, робота iiwa 7, контроллеров и камеры; OBJ берёт из `iiwa_description`. Один запуск создаёт один эксперимент в свежем мире. Высокоуровневые навыки захвата, нанесения герметика и затяжки в этот план не входят; пакет готовит их начальные объекты и метаданные.
+
+**Tech Stack:** Ubuntu 24.04, ROS 2 Jazzy, Webots R2025a, `ament_python`, Python 3, `rclpy`, `webots_ros2_msgs/srv/SpawnNodeFromString`, PyYAML, существующий `iiwa_bringup`.
+
+**Spec:** `docs/superpowers/specs/2026-11-08-grant-experiments.md` и `docs/superpowers/specs/2026-11-08-grant-scenes.md` — версионируемые копии пользовательских `output/md/experements.md` и `output/md/experiment-scenes.md`. Задача 1 переносит их копии в новый пакет. Геометрические источники: `src/iiwa_description/worlds/iiwa.wbt`, `src/iiwa_description/objects/grant/`.
+
+## Global Constraints
+
+- Вся разработка ведётся в ветке **`experement`**, именно с таким написанием. Ветка создана при подготовке плана; перед изменениями исполнитель проверяет рабочее дерево и переключается на неё, не стирая существующие изменения.
+- Весь новый код, запуск, мир, конфигурация, тесты и документация реализации находятся под `src/iiwa_experement/`. Исключение — существующие OBJ/MTL в `src/iiwa_description/objects/grant/`; их не копировать и не менять без отдельной необходимости. Этот файл плана расположен вне пакета как рабочее задание.
+- Новый мир: `src/iiwa_experement/worlds/grant/iiwa-grant.wbt`. Прежнее предложение положить его в `iiwa_description/worlds/grant/` заменено новым требованием держать реализацию в отдельном пакете.
+- `patron` постоянно установлен на роботе через `tool.active: patron`; рабочие инструменты размещаются отдельно и впоследствии вставляются в `patron`. Не создавать вторую модель `patron` в мире и не менять конфигурацию робота на `none`.
+- Проверить также фактический `src/iiwa_description/urdf/tools/tool_active.xacro`: именно он сейчас включает `patron.xacro` в описание робота. Одного поля `tool.active` в YAML недостаточно, если этот файл изменился. Не править чужой Xacro в рамках пакета; при расхождении остановить интеграцию и сообщить о нём.
+- Для всех экспериментов с лотком при запуске появляются **10 винтов**; это эксперименты 11–20. Сценарий может использовать только 1, 2 или 4 из них. Винты взаимозаменяемы; оценка плана не зависит от выбора конкретного номера.
+- Мир и объекты используют метры и мировую систему Webots. Координаты, ориентации, наборы объектов и смещения брать из `experiment-scenes.md`; не угадывать по фотографиям. Фотографии в `output/reference_photo/grant/` служат только визуальной сверкой.
+- Не восстанавливать старые удалённые `body_hatch*.wbt` и `GrantToolRack.proto`. Стойка строится в новом пакете из `tool-magizine.obj` и отдельных объектов инструментов.
+- Не добавлять исполнителей `dispense`, `press`, `seat_screw`, `tighten`, управление захватом и динамическую фиксацию люка в рамках этого плана. В документации явно отделить готовую начальную сцену от последующего исполнения навыков.
+- В конце каждого этапа выполнять указанные проверки и делать отдельный коммит с кратким префиксом `feat:`, `test:` или `docs:`.
+
+## Review Focus
+
+1. Номер вне `1..20` либо нечисловой ввод должен остановить launch **до** запуска Webots. Проверяет задача 6.
+2. Отсутствующий OBJ/MTL или неразрешённый PROTO после изолированной установки пакета должен давать явную ошибку, а не пустой объект. Проверяют задачи 2, 4 и 6.
+3. Повторяющиеся имена объектов либо сцена с 9/11 винтами не должны проходить проверку сценария. Проверяет задача 3.
+4. Тайм-аут ответа supervisor не должен автоматически повторять spawn: первый запрос мог быть выполнен, и повтор создаст дубликат. Проверяет задача 5.
+5. Запуск не должен одновременно включить старый случайный `object_spawner` или робота без `patron` в сгенерированном URDF. Проверяет задача 6.
+
+---
+
+## Карта файлов
+
+Создать только внутри `src/iiwa_experement/`:
+
+| Файл или папка | Ответственность |
+| --- | --- |
+| `package.xml`, `setup.py`, `setup.cfg`, `resource/iiwa_experement` | ROS 2 пакет `ament_python`; установка Python, launch, YAML, WBT и MD |
+| `iiwa_experement/__init__.py` | Маркер Python пакета |
+| `iiwa_experement/scenario.py` | Чтение `scenes.yaml`, проверка ID и инвариантов, типизированный `ScenarioSpec` |
+| `iiwa_experement/webots_nodes.py` | Преобразование `ScenarioSpec` в детерминированный список Webots `Solid` со стабильными именами |
+| `iiwa_experement/scene_spawner.py` | ROS нода: вызов supervisor spawn, обработка ошибок, сигнал готовности |
+| `iiwa_experement/runtime_config.py` | Подготовка временных WBT/YAML с установленными путями PROTO, миром, `patron` и отключённым случайным спавном |
+| `config/scenes.yaml` | Единственный машинный источник координат, наборов объектов и метаданных отказов для 20 опытов |
+| `worlds/grant/iiwa-grant.wbt` | Исходный шаблон универсального мира из `iiwa.wbt`: стол, ограждения, свет; без робота и деталей |
+| `launch/experiment.launch.py` | Команда `ros2 launch iiwa_experement experiment.launch.py experiment_id:=N` |
+| `doc/experements.md`, `doc/experiment-scenes.md` | Версионируемые копии двух MD спецификаций из `output/md/` |
+| `doc/README.md` | Запуск, параметры, проверка готовности, границы реализации |
+| `test/test_package_layout.py`, `test/test_scenario.py`, `test/test_webots_nodes.py`, `test/test_scene_spawner.py`, `test/test_launch.py`, `test/test_world.py`, `test/test_runtime_config.py` | Проверки установки пакета, данных, генерации узлов, путей, отказов и запуска |
+
+Перед работой прочитать: инструкции AGENTS.md из задания; `docs/superpowers/specs/2026-11-08-grant-experiments.md`; `docs/superpowers/specs/2026-11-08-grant-scenes.md`; `src/iiwa_description/worlds/iiwa.wbt`; `src/iiwa_description/resource/protos/CollaborativeRobotTable.proto`; `src/iiwa_description/resource/protos/WorkspaceLimiter.proto`; `src/iiwa_description/objects/grant/`; `src/iiwa_description/urdf/tools/tool_active.xacro`; `src/iiwa_config/config/setting.yaml`; `src/iiwa_config/config/tools.yaml`; `src/iiwa_bringup/launch/iiwa.launch.py`; `src/iiwa_bringup/launch/supported/webots_spawn.launch.py`; `src/iiwa_utils/iiwa_utils/object_spawner.py`. При необходимости визуально сверить `output/reference_photo/grant/`, но помнить, что `output/` игнорируется Git.
+
+### Task 1: Ветка, пакет и версионируемые спецификации
+
+**Files:** создать `src/iiwa_experement/{package.xml,setup.py,setup.cfg,resource/iiwa_experement,iiwa_experement/__init__.py,doc/experements.md,doc/experiment-scenes.md}`.
+
+**Interfaces:** производит установленный пакет `iiwa_experement` и доступные через его share-директорию MD файлы; остальные задачи используют эту директорию через `ament_index_python`.
+
+- [ ] **Step 1: Проверить состояние Git и ветку.** Выполнить `git status --short` и `git branch --show-current`; при необходимости `git switch experement`. Не сбрасывать существующие удаления старых миров и прототипа. Проверить `git branch --show-current` → `experement`.
+- [ ] **Step 2: Создать минимальный `ament_python` пакет.** В `package.xml` указать прямые зависимости `rclpy`, `ament_index_python`, `webots_ros2_msgs`, `std_srvs`, `python3-yaml`, `iiwa_description`, `iiwa_bringup`, `iiwa_config`, `launch`, `launch_ros`; тестовую зависимость `python3-pytest`. В `setup.py` установить существующие `doc/` в `share/iiwa_experement`; добавлять `worlds/`, `config/`, `launch/` по мере создания этих папок. Зарегистрировать `scene_spawner = iiwa_experement.scene_spawner:main` после задачи 5.
+- [ ] **Step 3: Скопировать спецификации без изменения смысла** из `docs/superpowers/specs/2026-11-08-grant-experiments.md` и `docs/superpowers/specs/2026-11-08-grant-scenes.md` в `doc/experements.md` и `doc/experiment-scenes.md`. Проверить совпадение содержимого; копии внутри пакета устанавливаются вместе с ним.
+- [ ] **Step 4: Проверить регистрацию пакета.** `source /opt/ros/jazzy/setup.bash && colcon list --base-paths src | rg '^iiwa_experement\s'` → ровно одна строка; после появления всех устанавливаемых ресурсов выполнить сборку `colcon build --base-paths src --packages-up-to iiwa_experement --symlink-install`.
+- [ ] **Step 5: Коммит.** `git add src/iiwa_experement` и `git commit -m "feat: scaffold iiwa_experement package"`.
+
+### Task 2: Универсальный базовый мир
+
+**Files:** создать `worlds/grant/iiwa-grant.wbt`, `test/test_world.py`.
+
+**Interfaces:** мир открывается Webots по `pkg://iiwa_experement/worlds/grant/iiwa-grant.wbt`; объекты эксперимента добавляет задача 5, робот — существующий bringup.
+
+- [ ] **Step 1: Написать тест `test_world_matches_base_layout`.** Проверить `basicTimeStep 32`, стол `translation 0 0 -0.007`, оба `WorkspaceLimiter`, отсутствие статического робота, люка, корпуса и винтов. Проверить, что шаблон содержит только две известные локальные ссылки на `CollaborativeRobotTable.proto` и `WorkspaceLimiter.proto`, которые задача 6 заменит на абсолютные перед запуском. Добавить `test/test_package_layout.py::test_expected_resources_are_installed`, который после сборки проверяет наличие WBT, `scenes.yaml`, launch и двух MD в share-директории; пока будущие ресурсы не созданы, проверять в нём только уже готовые файлы.
+- [ ] **Step 2: Запустить тест, убедиться в ожидаемом отказе** из-за отсутствующего мира.
+- [ ] **Step 3: Скопировать основу `iiwa.wbt` в новый файл и добавить его в `setup.py`.** В исходном шаблоне использовать относительные пути `../../../iiwa_description/resource/protos/CollaborativeRobotTable.proto` и `../../../iiwa_description/resource/protos/WorkspaceLimiter.proto`: они работают из `src`, но **не** из изолированного `install/`; задача 6 заменит их при запуске. Убрать неиспользуемый `Object.proto`. Сохранить фон, свет, стол, ограждения, `basicTimeStep`; выбрать точку обзора, показывающую всю рабочую зону. Установить ненулевое трение `TableMaterial` (ориентир `0.6` из прежней grant-сцены), не менять исходный `iiwa.wbt`.
+- [ ] **Step 4: Запустить тест и сборку пакета.** Тест проходит; после установки `ros2 pkg prefix iiwa_experement` показывает пакет, а шаблон мира находится в `share/iiwa_experement/worlds/grant/`.
+- [ ] **Step 5: Коммит.** `git commit -m "feat: add universal grant Webots world"`.
+
+### Task 3: Данные 20 сценариев и строгая проверка
+
+**Files:** создать `config/scenes.yaml`, `iiwa_experement/scenario.py`, `test/test_scenario.py`.
+
+**Interfaces:** `load_scenario(experiment_id: int, manifest_path: pathlib.Path) -> ScenarioSpec`. `ScenarioSpec` содержит ID, позы стапеля/корпуса/стойки, состояние и позу люка (`absent|free|installed`), необязательные позы лотка и 10 винтов, перечень рабочих инструментов, метаданные отказов. Задача 4 использует только этот объект, а не читает YAML повторно.
+
+- [ ] **Step 1: Написать тесты для всех ID 1–20.** Проверить наборы: 1–5 — свободный люк, нет лотка, только присоска; 6–9 — нет люка и лотка, герметик; 10 — свободный люк, герметик и присоска; 11–15 — установленный люк и ровно 10 винтов; 16–20 — свободный люк и ровно 10 винтов. Проверить особые позы № 2, 3, 5, 19 и метаданные отказов № 4, 5, 9, 15, 20 по `doc/experiment-scenes.md`.
+- [ ] **Step 2: Добавить отрицательные тесты.** ID `0`, `21`, неизвестное имя инструмента, 9/11 винтов, повторяющееся имя, винт вне лотка, несовпадающие XY корпуса и стапеля должны давать `ValueError` с понятным сообщением.
+- [ ] **Step 3: Запустить тесты и увидеть ожидаемые отказы** до реализации загрузчика.
+- [ ] **Step 4: Реализовать компактный YAML манифест и `load_scenario`, добавить YAML в `setup.py`.** Хранить общую геометрию один раз; в эксперименте указывать только выбор объектов и отличия. Десять винтов задавать сеткой/позами из спецификации; для № 19 переносить **все десять** вместе с лотком. Никакого случайного расположения. Состояния ошибок в YAML — метаданные для будущего исполнителя, они не меняют начальные позы, кроме уже установленного люка в опытах 11–15.
+- [ ] **Step 5: Запустить `python3 -m pytest src/iiwa_experement/test/test_scenario.py -q` после sourcing ROS.** Ожидается 0 ошибок; проверить, что данные для 20 опытов читаются из установленного YAML.
+- [ ] **Step 6: Коммит.** `git commit -m "feat: define deterministic grant scenarios"`.
+
+### Task 4: Генератор Webots-объектов
+
+**Files:** создать `iiwa_experement/webots_nodes.py`, `test/test_webots_nodes.py`.
+
+**Interfaces:** `build_nodes(spec: ScenarioSpec, asset_root: pathlib.Path) -> list[str]`. Возвращает Webots VRML строки в порядке: стапель → корпус → стойка → рабочие инструменты → лоток → 10 винтов → люк. `asset_root` равен `get_package_share_directory('iiwa_description')/objects/grant`.
+
+- [ ] **Step 1: Написать тесты узлов.** Проверить реальные URL OBJ, уникальные `name`, отсутствие `GrantToolRack.proto`, отсутствие дублированного `patron`, правильные позы и число винтов для сценариев 1, 6, 11, 16, 19. В № 19 у каждого винта должен быть тот же сдвиг, что у лотка. Для 20 ID проверить, что каждый требуемый OBJ существует.
+- [ ] **Step 2: Написать тесты геометрии.** Проверить преобразование вертикальной оси OBJ Y в Z Webots, повороты 45°/135°/60° люка, масштаб `10` **только** для `germetic-tool.obj`, нижнюю точку свободного люка на `Z=0.793`, установленного люка на верхней поверхности корпуса и 10 различных поз винтов внутри лотка.
+- [ ] **Step 3: Запустить тесты и увидеть ожидаемые отказы.**
+- [ ] **Step 4: Реализовать генератор.** Использовать отдельный `Solid` для стойки из `tool-magizine.obj` и отдельный `Solid` для каждого инструмента; стабильные имена `grant_fixture`, `grant_body`, `grant_rack`, `grant_cap`, `grant_tray`, `grant_vacuum`, `grant_magnetic_tool`, `grant_sealant_tool`, `grant_torque_tool`, `grant_screw_01`…`grant_screw_10`. Добавить простые проверенные `boundingObject` для физических объектов. Стапель, корпус, стойка и лоток фиксированы. Свободный люк имеет `Physics`; установленный зафиксирован. Для винтов и инструментов явно документировать начальное состояние «лежит в лотке/гнезде» и границу будущего механизма захвата; не выдавать статическую геометрию за реализованный захват.
+- [ ] **Step 5: Запустить тесты и проверить установленный asset root.** `python3 -m pytest src/iiwa_experement/test/test_webots_nodes.py -q` → 0 ошибок; ни один URL не указывает в `output/` или в исходную директорию вместо установленного пакета.
+- [ ] **Step 6: Коммит.** `git commit -m "feat: generate grant Webots objects"`.
+
+### Task 5: Детерминированный спавнер и готовность сцены
+
+**Files:** создать `iiwa_experement/scene_spawner.py`, `test/test_scene_spawner.py`; дополнить `setup.py` console script.
+
+**Interfaces:** `scene_spawner` принимает ROS параметр `experiment_id: int` и путь к `scenes.yaml` (по умолчанию установленный файл); вызывает `/Ros2Supervisor/spawn_node_from_string` по одному узлу; после всех успешных ответов оставляет доступным сервис `/iiwa_experement/scene_ready` типа `std_srvs/srv/Trigger` (`success=true`, сообщение с ID). До завершения и при отказе сервис не сообщает готовность.
+
+- [ ] **Step 1: Написать тест успешного порядка вызовов.** Подставить fake client; сверить порядок и число отправленных строк с `build_nodes`; готовность наступает только после последнего `success=true`.
+- [ ] **Step 2: Написать тесты отказов.** Нет supervisor за заданный тайм-аут, `success=false`, исключение и тайм-аут ответа должны завершаться ошибкой с ID последнего объекта. При тайм-ауте **не повторять** запрос автоматически. Если Webots уже содержит то же имя, не скрывать отказ дубликата.
+- [ ] **Step 3: Запустить тесты и увидеть ожидаемые отказы.**
+- [ ] **Step 4: Реализовать ноду по образцу `iiwa_utils/object_spawner.py`, но без случайного выбора.** Использовать `get_package_share_directory` для YAML и OBJ; дождаться supervisor service; отправлять узлы последовательно; логировать эксперимент, каждый ID объекта и окончательную готовность. Оставить ноду активной для сервиса `scene_ready`.
+- [ ] **Step 5: Запустить тесты и коммит.** `python3 -m pytest src/iiwa_experement/test/test_scene_spawner.py -q` → 0 ошибок; `git commit -m "feat: spawn deterministic grant scene"`.
+
+### Task 6: Запуск одного эксперимента командой ROS 2
+
+**Files:** создать `iiwa_experement/runtime_config.py`, `launch/experiment.launch.py`, `test/test_runtime_config.py`, `test/test_launch.py`, `doc/README.md`.
+
+**Interfaces:** `prepare_runtime_files(base_setting: pathlib.Path, world_template: pathlib.Path, description_share: pathlib.Path, output_dir: pathlib.Path) -> tuple[pathlib.Path, pathlib.Path]` возвращает `(world_path, setting_path)`. Команда `ros2 launch iiwa_experement experiment.launch.py experiment_id:=N` работает для `N=1..20`; необязательные `rviz:=true|false`, `scene_config:=<путь к YAML>` и `base_setting:=<путь к YAML>`. Launch передаёт временный `setting_path` в `iiwa_bringup/launch/iiwa.launch.py` с `simulate:=true` и запускает локальный `scene_spawner` с тем же ID и `scene_config`.
+
+- [ ] **Step 1: Написать тесты `validate_experiment_id(raw: str) -> int`.** `1`, `20` проходят; `0`, `21`, `abc`, пустая строка вызывают ошибку до создания действий Webots. Проверить, что launch включает именно `iiwa_bringup` и один `scene_spawner`, а `scene_config` передаётся ему без изменения.
+- [ ] **Step 2: Написать `test_runtime_config.py`.** На временном каталоге проверить, что `prepare_runtime_files` заменяет обе локальные `EXTERNPROTO` на абсолютные существующие пути из `description_share`, не меняет внешние `https://` PROTO, пишет отдельный WBT и YAML. В YAML проверить путь на временный WBT, `digital_twin.webots.objects.enabled: false`, `count: 0`, `tool.active: patron`, сохранение камеры `d455_top`, позы робота `-0.25 0 0.79` и контроллеров. Добавить отрицательные случаи отсутствующего PROTO и отсутствующего шаблона. Проверить `iiwa_utils.setting_loader.build_settings(..., check_files=True)` на готовом YAML; также проверить включение `patron.xacro` в `tool_active.xacro` и наличие `patron` в сгенерированном URDF.
+- [ ] **Step 3: Запустить тесты и увидеть ожидаемые отказы.**
+- [ ] **Step 4: Реализовать `runtime_config` и launch, добавить launch в `setup.py`.** По умолчанию читать установленный `iiwa_config/config/setting.yaml`; получить `description_share` через `get_package_share_directory('iiwa_description')`; подставить абсолютные пути PROTO в копию мира и записать оба временных файла в отдельную директорию запуска. Принять только один ID и проверить его в `OpaqueFunction` до включения Webots; передать ID в спавнер. Очистить временную директорию при завершении launch. Не запускать legacy `object_spawner`; не менять FRI-ветку существующего bringup и не запускать физического робота. Не подменять `patron` пустым фланцем.
+- [ ] **Step 5: Написать `doc/README.md` и завершить `test_package_layout.py`.** Дать команды сборки, запуска № 1, 6, 11, 16, 19, 20, проверки `/iiwa_experement/scene_ready`, остановки и выбора другого ID новым запуском. Объяснить: файл мира расположен в новом пакете, OBJ остаются в `iiwa_description`, 10 винтов всегда присутствуют в сценах 11–20; исполнение навыков и привязка инструмента к `patron` ещё не входят в пакет. После сборки тест установки должен проверить все WBT, YAML, launch и MD в share-директории.
+- [ ] **Step 6: Запустить тесты и коммит.** `python3 -m pytest src/iiwa_experement/test/test_runtime_config.py src/iiwa_experement/test/test_launch.py -q` → 0 ошибок; `git commit -m "feat: launch grant experiment by id"`.
+
+### Task 7: Интеграционная проверка и передача
+
+**Files:** при необходимости исправлять только файлы `src/iiwa_experement/`; обновить `doc/README.md` наблюдениями.
+
+**Interfaces:** установленный пакет запускает 20 сцен из одного мира, логирует выбранный ID и сообщает готовность лишь после успешного спавна всех объектов.
+
+- [ ] **Step 1: Сборка и пакетные тесты.** Из корня: `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`. Исправить ошибки нового пакета до перехода дальше.
+- [ ] **Step 2: Проверить все 20 конфигураций без Webots.** Тесты должны подтвердить состав, координаты, уникальные имена, наличие 10 винтов в опытах 11–20 и отсутствие винтов в 1–10. Сверить по строкам `doc/experiment-scenes.md`.
+- [ ] **Step 3: Проверить Webots запуск в графической среде.** По отдельности запустить `ros2 launch iiwa_experement experiment.launch.py experiment_id:=1`, затем 6, 11, 16, 19, 20. Дождаться `scene_ready`; осмотреть положение люка, стапеля, лотка, десяти винтов, инструментов и видимость верхней камеры. После каждого опыта завершить запуск и начать свежий мир. Если графический дисплей недоступен, записать это как непроверенный пункт, не объявлять визуальную проверку пройденной.
+- [ ] **Step 4: Проверить ошибки запуска.** `experiment_id:=0` и `experiment_id:=21` должны завершиться до Webots. Создать временную копию YAML с девятью винтами вместо десяти и передать через `scene_config:=<путь>`: спавнер должен дать отказ без объявления готовности. Штатные файлы после проверки не менять. Отсутствующий OBJ проверяется тестом задачи 4 с временным `asset_root`.
+- [ ] **Step 5: Финальная проверка границ.** `git diff --check`; `git status --short`; `git branch --show-current` → `experement`. Проверить, что изменённые файлы реализации находятся только в `src/iiwa_experement/`, кроме ранее существовавших удалений; исходный `iiwa.wbt`, OBJ и `cobot-setting.yaml` не переписаны. Отдельно сообщить, что старый `src/iiwa_utils/test/test_body_hatch_worlds.py` ссылается на удалённые миры/PROTO и не является тестом нового пакета.
+- [ ] **Step 6: Коммит завершения и отчёт.** Если потребовались интеграционные исправления, сделать отдельный коммит. В отчёте указать команды и результаты, какие Webots сценарии просмотрены, какие проверки не удалось выполнить, и точную команду запуска любого `N`.
+
+## Приёмка
+
+План выполнен, когда из установленного рабочего пространства работает `ros2 launch iiwa_experement experiment.launch.py experiment_id:=N` для каждого `N` от 1 до 20; Webots загружает один и тот же `iiwa-grant.wbt`, а спавнер создаёт соответствующий набор объектов и выдаёт готовность. В опытах 11–20 лоток содержит 10 винтов, `patron` находится на роботе во всех опытах, и нужные рабочие инструменты находятся в отдельных гнёздах стойки. Реализация и её тесты лежат в `src/iiwa_experement/`; источники OBJ/MTL остаются в `iiwa_description`.

docs/superpowers/plans/2026-11-10-llm-graph-execution.md

@@ -0,0 +1,173 @@
+# План реализации исполнения графов Qwen3 в Webots
+
+> **Для агента-разработчика:** выполнять задачи по порядку и отмечать выполненные шаги чекбоксами. Этот документ — план разработки, а [спецификация](../specs/2026-11-10-llm-graph-execution-design.md) задаёт поведение.
+
+**Цель:** получать актуальный `scene_json`, принимать готовый JSON-граф Qwen3 и безопасно исполнять его в Webots независимо от эталонного сценария выбранного эксперимента.
+
+**Архитектура:** пакет `iiwa_scene_context` формирует снимок из живого supervisor и публикует контракт для модели. Пакет `iiwa_llm_plan_executor` проверяет граф, исполняет узлы через реестр навыков и ограниченный адаптер MoveItPy. Существующий `iiwa_web` даёт тонкие HTTP/MCP маршруты; `iiwa_experement` только создаёт исходную сцену и управляет виртуальными захватами.
+
+**Стек:** Ubuntu 24.04, ROS 2 Jazzy, Python 3, `rclpy`, MoveItPy 2.12, Webots, `webots_ros2_driver`, FastAPI, FastMCP, pytest.
+
+**Спецификация:** [Согласованная архитектура](../specs/2026-11-10-llm-graph-execution-design.md).
+
+## Общие ограничения
+
+- Работать в ветке `experement`. До первой правки выполнить `git status --short --branch`. Не сбрасывать чужие изменения. Код этого этапа только для Webots, запуск `simulate:=true`; физический FRI не подключать.
+- Создать ровно два дополнительных ROS 2 пакета: `src/iiwa_scene_context` и `src/iiwa_llm_plan_executor`. Изменения `iiwa_experement` и `iiwa_web` ограничить интеграцией. Не делать систему оценки Qwen3, её обучение или вызов модели: внешний клиент передаёт **готовый** граф.
+- Номер эксперимента выбирает исходную сцену, но не задаёт робототехническое задание. Исполнять только переданный граф. Не вставлять операции взятия инструмента, герметизации, установки люка или передачи винтов на основании номера эксперимента.
+- Внутри *одного* навыка допустимы обязательные геометрические фазы: безопасный подход сверху, осевое опускание, действие, осевой подъём к сохранённой высоте. Явные узлы этих фаз не должны вызывать повторное действие. Неизвестный объект, навык, инструмент, цель, положение или неоднозначная зависимость дают отказ до движения либо `replan_required`.
+- `release(vacuum)` не перемещает инструмент к стойке. Чтобы вернуть присоску, граф должен явно содержать `transfer(vacuum,vacuum_socket)` и `release(vacuum,vacuum_socket)` либо `dock_tool(vacuum,vacuum_socket)` в достигнутом гнезде. `release(vacuum)` из `work` без безопасной цели отклоняется; присоска остаётся в `patron`. `grab(cap)` без присоски не отклоняется на входе: при исполнении проверяется живой supervisor, контакт не выполняется, возвращается `grasp_failed` с причиной `required tool vacuum is not mounted` и выбирается переход графа для этого события; автоматического взятия инструмента нет. Винт подносится к отверстию и передаётся виртуальному человеку: без вставки и затяжки. Навык `tighten` в профиле Grant первого этапа не поддерживается.
+- `detect`: перейти в `work`, дождаться свежего кадра верхней камеры, получить точную позу из supervisor. При настроенном близком осмотре получить также новый кадр камеры на роботе; RGB/D используется для контроля, а не для вычисления позы.
+- Рабочий кончик направлен вниз; при рабочих движениях наклон не более 10° и J7 в жёстком коридоре ±90° относительно `work` (сейчас 0 рад). Применять ограничения к *траектории*, не только к конечной позе. Если траектория недоступна, остановиться; не переходить на ветвь с разворотом J7 около −180°. Поворот кисти и переход в `work` только после осевого отвода от контакта.
+- Один активный граф на робота. Внешний клиент передаёт ответ Qwen3 через `/iiwa_llm_plan_executor/submit`; принятый граф можно прочитать из ROS 2 топика `/iiwa_llm_plan_executor/plan`, а события и итог — из отдельного `/iiwa_llm_plan_executor/status`. `status=blocked` не запускает движение. `replan_required` выдаёт новый снимок сцены, но не вызывает Qwen3. `completed` означает выполненный граф и проверенные постусловия, а не оценку соответствия произвольному текстовому запросу.
+- Движение запускается после готовности сцены, supervisor, контроллера, MoveIt, свежего состояния робота и задержки минимум 3,0 с симуляционного времени. `mode=view` и `mode=execute` сохраняют свою нынешнюю семантику.
+
+Поток данных: `/iiwa_scene_context/scene_json` → внешний вызов Qwen3 → `/iiwa_llm_plan_executor/submit` → `/iiwa_llm_plan_executor/plan` (сам принятый граф) → `/iiwa_llm_plan_executor/status` (ход и итог). После реализации граф можно посмотреть командой `ros2 topic echo /iiwa_llm_plan_executor/plan`; пока модель вызывается отдельно, в этом топике ничего не появится без передачи её ответа в `submit`.
+
+## На что смотреть перед изменениями
+
+| Назначение | Файлы и каталоги |
+| --- | --- |
+| Согласованная архитектура | `docs/superpowers/specs/2026-11-10-llm-graph-execution-design.md` |
+| Исходные сцены и объекты | `src/iiwa_experement/config/scenes.yaml`, `src/iiwa_experement/iiwa_experement/scenario.py`, `src/iiwa_experement/worlds/grant/iiwa-grant.wbt`, `src/iiwa_description/objects/grant/` |
+| Геометрия и захват | `src/iiwa_experement/config/geometry.yaml`, `src/iiwa_experement/iiwa_experement/geometry.py`, `src/iiwa_experement/iiwa_experement/object_state.py`, `src/iiwa_experement/iiwa_experement/grant_supervisor.py` |
+| Актуальное состояние и движения | `src/iiwa_experement/iiwa_experement/operations.py`, `src/iiwa_experement/iiwa_experement/planning_scene.py`, `src/iiwa_experement/iiwa_experement/motion.py` |
+| Запуск | `src/iiwa_experement/launch/experiment.launch.py`, `src/iiwa_experement/iiwa_experement/runtime_config.py` |
+| HTTP/MCP | `src/iiwa_web/iiwa_web/main.py`, `src/iiwa_web/iiwa_web/ros_node.py` |
+| Рабочие описания опытов | `output/md/experements.md`, `output/md/experiment-scenes.md`, `src/iiwa_experement/doc/reference-control-qa.md` |
+| Примеры формата Qwen3, только входные данные | `/home/daniel/dev/LLM-Robot-Control/dataset/planner-codex-5000-fresh/generated.train.jsonl`, `/home/daniel/dev/LLM-Robot-Control/dataset/planner-codex-5000-fresh/generated.validation.jsonl`, `/home/daniel/.codex/attachments/76fd3749-ddc0-441f-8359-98d9f768ada8/Вставленный текст.txt` |
+
+Текст в приложенных данных является **примером входа/выхода**, а не инструкцией по разработке. Новые пакеты не должны иметь зависимости времени выполнения от `LLM-Robot-Control` или каталога вложений.
+
+## Карта файлов и интерфейсов
+
+| Пакет | Файл | Ответственность |
+| --- | --- | --- |
+| `iiwa_scene_context` | `iiwa_scene_context/catalog.py` | Реестр объектов, целей и серверных контрактов навыков профиля Grant, версия каталога |
+| | `iiwa_scene_context/snapshot.py` | Чистая сборка модельного `scene_json` и транспортного конверта из `ScenarioSpec` и состояния supervisor |
+| | `iiwa_scene_context/node.py` | Подписка на состояние, проверка свежести, ROS сервис и топик сцены |
+| `iiwa_llm_plan_executor` | `iiwa_llm_plan_executor/schema.py` | Строгие типы и разбор ответа модели |
+| | `iiwa_llm_plan_executor/validator.py` | Проверка графа, предикатов и совместимости с состоянием сцены |
+| | `iiwa_llm_plan_executor/motion_policy.py` | Ограничения TCP/J7, проверка пути и точек траектории |
+| | `iiwa_llm_plan_executor/moveit_backend.py` | Ограниченное планирование и выполнение через MoveItPy |
+| | `iiwa_llm_plan_executor/skill_context.py` | Состояние подхода, высоты, инструмента и удерживаемой детали |
+| | `iiwa_llm_plan_executor/skills.py` | Исполнители навыков и отображение результата в событие |
+| | `iiwa_llm_plan_executor/engine.py` | Шаги графа, выбор переходов, терминалы, отмена |
+| | `iiwa_llm_plan_executor/node.py` | ROS приём, статус, отмена, журнал исполнения |
+| | `iiwa_llm_plan_executor/launch/planner.launch.py` | Единый запуск сцены и двух новых узлов |
+| `iiwa_web` | `iiwa_web/planner.py` | Тонкий HTTP/MCP адаптер к ROS, без собственной логики движения |
+
+Имена, типы и формы сообщений зафиксировать в `docs/interfaces.md` каждого нового пакета до интеграции: `scene_json`, конверт снимка, вход `submit`, принятый граф, ответ, статус, коды ошибок. Для JSON в ROS использовать `std_srvs/Trigger` (снимок), `std_msgs/String` (снимок, принятый граф и статус) и `webots_ros2_msgs/SetString` (`submit`/`cancel`). Для запроса статуса по ID добавить `GetRunStatus.srv` в **существующий** `iiwa_msgs`: запрос `string run_id`, ответ `bool found` и `string status_json`. Третьего нового пакета нет. `SetString` возвращает только признак принятия; `run_id` задаёт клиент.
+
+## Вопросы для проверки при рецензии
+
+- Повторный запрос `submit` с тем же `run_id` не должен запускать второй экземпляр движения (задача 7, тест повторной отправки).
+- Если supervisor устарел или пропал между двумя узлами, результат не должен трактоваться как `found`/`success` (задачи 2 и 7, тест потери свежести).
+- Граф с циклом или недостижимыми узлами не должен зависнуть или обойти предел шагов (задача 4, тест циклов и лимита).
+- После неудачного захвата возможны два состояния — пустой `patron` либо удерживаемый объект; продолжать без `inspect_state` нельзя (задачи 4 и 6, тест неопределённого владения).
+- Даже при допустимой конечной позе промежуточный J7 или TCP может выйти за пределы; траектория должна быть отклонена до исполнения (задача 5, тест промежуточной точки).
+
+---
+
+### Задача 1. Режим сцены для внешнего графа
+
+**Файлы:** изменить `src/iiwa_experement/iiwa_experement/runtime_config.py`, `src/iiwa_experement/launch/experiment.launch.py`; тесты `src/iiwa_experement/test/test_runtime_config.py`, `src/iiwa_experement/test/test_launch.py`.
+
+**Интерфейс:** `mode:=planner` создаёт тот же мир, спавнер, supervisor и bringup, что `execute`, но **не** запускает `reference_runner`. Supervisor в этом режиме доступен для команд захвата (`read_only=False`). TCP gizmo остаётся выключенным.
+
+- [x] Написать тесты: `validate_mode("planner")` проходит; `runtime_actions(..., mode="planner")` создаёт writable supervisor и не создаёт `reference_runner`; `view` и `execute` сохраняют прежние действия.
+- [x] Запустить `pytest -q src/iiwa_experement/test/test_runtime_config.py src/iiwa_experement/test/test_launch.py` и убедиться в ожидаемом падении новых тестов.
+- [x] Добавить режим в валидатор и изменить вычисление `read_only` на `mode == "view"`; проверку добавления эталонного runner оставить только для `execute`.
+- [x] Повторить тесты; отдельным коммитом зафиксировать этот режим.
+
+### Задача 2. Пакет снимка сцены
+
+**Файлы:** создать `src/iiwa_scene_context/package.xml`, `setup.py`, `setup.cfg`, `resource/iiwa_scene_context`, `iiwa_scene_context/{__init__,catalog,snapshot,node}.py`, `test/test_snapshot.py`, `test/test_node.py`, `docs/interfaces.md`.
+
+**Интерфейсы:** `build_scene(spec: ScenarioSpec, supervisor_state: Mapping, catalog: SkillCatalog) -> dict` создаёт модельный JSON. `build_envelope(..., scene_revision: int, stamp: float) -> dict` добавляет `experiment_id`, `state_version`, `scene_revision`, `timestamp`, `scene_json`. ROS: `/iiwa_scene_context/get_scene` (`Trigger`, `message` — JSON-конверт), `/iiwa_scene_context/scene_json` (`String`, transient-local).
+
+- [x] Написать тесты для сцен 1 и 20: выдаются только присутствующие объекты и цели, в сцене с лотком не менее 10 отдельных винтов; `cap`, `vacuum`, `vacuum_socket` и отверстия имеют стабильные ID; у каждого присутствующего инструмента есть отдельная именованная цель гнезда; в `scene_json` нет координат и внутренних полей supervisor; логическое состояние инструмента/детали отражает живой snapshot.
+- [x] Написать тесты свежести: до `scene_ready` либо при устаревшем `object_state` сервис отвечает `success=False`; `scene_revision` растёт при изменении `state.version`, но не от каждого обновления позы удерживаемого объекта; поздний подписчик получает текущий снимок.
+- [x] Убедиться в ожидаемом падении `pytest -q src/iiwa_scene_context/test`; реализовать каталог и чистую сборку, затем узел с подпиской на `/iiwa_experement/object_state` и `/iiwa_experement/scene_ready`.
+- [x] Повторить тесты. Записать версию контракта, точную форму полей и правило `scene_revision` в `docs/interfaces.md`; зафиксировать пакет отдельным коммитом.
+
+### Задача 3. Разбор и валидация графа
+
+**Файлы:** создать `src/iiwa_llm_plan_executor/package.xml`, `setup.py`, `setup.cfg`, `resource/iiwa_llm_plan_executor`, `iiwa_llm_plan_executor/{__init__,schema,validator}.py`, `test/test_schema.py`, `test/test_validator.py`, `docs/interfaces.md`. Зависимость пакета от `iiwa_scene_context` и `iiwa_experement` объявить в `package.xml`.
+
+**Интерфейсы:** `parse_response(raw: str | Mapping) -> PlanGraph`; `validate_graph(graph: PlanGraph, scene: SceneEnvelope, catalog: SkillCatalog) -> ValidationResult`. Каталог и допустимые исходы приходят из серверного `iiwa_scene_context/catalog.py`, а не из присланного графом `skill_contracts`.
+
+- [x] Написать тесты: показанный пользователем формат `status/entry/plan/transitions` разбирается; `status=blocked` не допускается к исполнению; дубликаты ID, неизвестные навыки/объекты/цели/события, пропущенный `entry`, несуществующие `next`, недостижимые узлы, пустые обязательные переходы и превышение лимитов отвергаются.
+- [x] Написать тесты ветвей отказа: `collision` не ведёт к продолжению операции; `grasp_failed` не превращается в `success`; цикл с допустимым восстановлением имеет ограничение 256 узлов и отдельный предел выполненных шагов; неверный `scene_revision` отклоняется.
+- [x] Написать тест на исходную присоску в стойке: граф `grab(cap)` без явного `grab(vacuum)` принимается, при исполнении даёт `grasp_failed` до контакта и идёт по ветке `grasp_failed`; граф `grab(vacuum) → transfer(vacuum,work) → completed` принимается и оставляет инструмент в `patron`; граф `grab(vacuum) → transfer(vacuum,work) → release(vacuum)` отклоняется. Граф с явным `transfer(vacuum,vacuum_socket) → dock_tool(vacuum,vacuum_socket)` допускает возврат. Не выводить задачу из `experiment_id`.
+- [x] Прочитать несовпадение имён в приложенных наборах. Ввести версию контракта и только явно проверенные алиасы `pick`, `move_to_storage`, `inspect`; `align` и Grant-расширения — самостоятельные записи каталога. Неизвестный `tighten` отклоняется до движения. Проверить тестами и зафиксировать.
+- [x] Запустить `pytest -q src/iiwa_llm_plan_executor/test/test_schema.py src/iiwa_llm_plan_executor/test/test_validator.py` сначала до, затем после реализации; сохранить контракт и отдельный коммит.
+
+### Задача 4. Проверка достижимости ограниченных движений
+
+**Файлы:** создать `src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/{motion_policy,moveit_backend}.py`, `config/motion_limits.yaml`, `test/test_motion_policy.py`, `test/test_moveit_backend.py`.
+
+**Интерфейсы:** `MotionPolicy.validate_trajectory(points, tcp_poses, current_joints) -> MotionCheck`; `MoveItBackend.plan_and_execute(goal: Transform, phase: str, held_object: str | None) -> MotionOutcome`; `MoveItBackend.stop() -> None`. Все планируемые траектории проходят один и тот же проверяющий слой.
+
+- [x] Сначала проверить в установленном Jazzy/MoveItPy возможность задать `set_path_constraints`, семя IK из текущих суставов и исполнить траекторию через действующий контроллер Webots. Описать проверенные вызовы и ограничения в `docs/interfaces.md`. Если связка не исполняет план, не заменять её обычным `MoveToPose.action` без ограничений: зафиксировать блокирующую несовместимость и безопасный вариант реализации.
+- [x] Написать тесты: J7 в `work` равен 0; границы −π/2 и +π/2 включительны; промежуточная точка за границей, скачок почти на π, наклон TCP >10°, несвежие `joint_states` и неактуальная планировочная сцена отвергаются до отправки контроллеру.
+- [x] Реализовать проверку в `motion_policy.py`, затем MoveItPy backend с `current-state` IK seed, ограничением `joint7` и рабочего наклона, проверкой всей траектории, синхронизацией с `PlanningSceneBridge` и подтверждением достигнутой позы. Путь с удерживаемым объектом проверять вместе с прикреплённой геометрией.
+- [x] Прогнать `pytest -q src/iiwa_llm_plan_executor/test/test_motion_policy.py src/iiwa_llm_plan_executor/test/test_moveit_backend.py`. В Webots выполнить один безопасный переход `work → над инструментом → work` и сохранить в QA журнале J7 по всем точкам и минимальный зазор; зафиксировать коммит.
+
+### Задача 5. Контекст исполнения и навыки
+
+**Файлы:** создать `src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/{skill_context,skills}.py`, `test/test_skills.py`. Использовать `iiwa_experement/geometry.py`, `planning_scene.py` и протокол `/iiwa_experement/object_command`; не вызывать `reference_scenarios.py`.
+
+**Интерфейсы:** `SkillContext` хранит текущий `scene_revision`, удерживаемый объект/инструмент, сохранённую безопасную позу подхода, фазу контакта и последнюю проверенную позу. `SkillRegistry.execute(node: PlanNode, context: SkillContext) -> SkillOutcome(event, details)`. Команды supervisor отправлять с `request_id` и подтверждать по следующему `object_state`.
+
+- [x] Написать тесты фаз на фиктивном backend: `approach` идёт над целью и сохраняет высоту; `grab` без предварительного подхода выполняет подход, осевое опускание, захват и подъём; явные `approach/lower/lift` не дублируют движение. После `release` сначала осевой подъём, только затем горизонтальный уход или поворот.
+- [x] Написать тесты инструментов: `grab(vacuum)` стыкуется кончиком `patron` к нужной точке; `release(vacuum)` из `work` **не двигает робот и не отделяет инструмент**; только после явного `transfer(vacuum,vacuum_socket)` навыки `release(vacuum,vacuum_socket)` и `dock_tool(vacuum,vacuum_socket)` опускаются в гнездо, отделяют и поднимаются. Отсутствие/занятость гнезда даёт отказ. `grab(cap)` без присоски не начинает захват: проверяется supervisor, контакт не выполняется, возвращается `grasp_failed`.
+- [x] Написать тесты объектов: `transfer/align/release(cap,hatch_seat)` реально переносит и сажает люк; `dispense` движется вдоль заданного участка; `handoff_screw(screw_1,hole_1)` подносит винт и отпускает виртуальному человеку, не занимает отверстие установленным винтом. Неподдерживаемый тип цели отвергается.
+- [x] Написать тесты `detect`: сначала осевой отвод и `work`, затем свежий верхний кадр + поза из supervisor; близкий кадр по настройке; просроченный кадр возвращает `failed`. `inspect_state` различает `empty`, `holding`, `placed`, `unknown`.
+- [x] Реализовать навыки реестра из спецификации, используя общие геометрические примитивы. Каждый результат отображать в одно событие контракта; неизвестный исход не подменять успехом. Прогнать `pytest -q src/iiwa_llm_plan_executor/test/test_skills.py` и зафиксировать коммит.
+
+### Задача 6. Движок графа и ROS узел
+
+**Файлы:** создать `src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/{engine,node}.py`, `test/test_engine.py`, `test/test_node.py`, `src/iiwa_msgs/srv/GetRunStatus.srv`; изменить `src/iiwa_msgs/CMakeLists.txt`; дополнить `docs/interfaces.md`.
+
+**Интерфейсы:** `GraphEngine.submit(run_id: str, response_json: str, scene_revision: int) -> AdmissionResult`; `get_status(run_id: str) -> RunStatus`; `cancel(run_id: str) -> CancelResult`. ROS: `/iiwa_llm_plan_executor/submit` (`SetString`; JSON `{run_id, scene_revision, response}`), `/iiwa_llm_plan_executor/plan` (`String`; JSON `{run_id, scene_revision, response}`, исходный принятый граф без изменений, transient-local QoS, глубина 1), `/iiwa_llm_plan_executor/status` (`String`; события и итог), `/iiwa_llm_plan_executor/get_status` (`iiwa_msgs/srv/GetRunStatus`), `/iiwa_llm_plan_executor/cancel` (`SetString` с `run_id`).
+
+- [x] Написать тесты: до готовности сцены запрос отклоняется; после задержки 3,0 с принимается; одновременно активен один `run_id`; повтор того же ID идемпотентен; новый ID при активном запуске отклоняется без второго движения. Принятый граф публикуется в `/iiwa_llm_plan_executor/plan` до первого движения, ровно один раз на `run_id`; отклонённый не публикуется; поздний подписчик получает последний принятый граф.
+- [x] Написать тесты исполнения: каждое событие выбирает ровно одно ребро; `completed` завершается, `replan_required` даёт новый снимок, `operator_required` останавливается и уведомляет; `status=blocked` не отправляет движений; переходы и причина остановки видны в статусе.
+- [x] Написать тесты отказов: потеря свежести сцены/суставов, неизвестный исход навыка, ошибка supervisor, столкновение, отмена и превышение предела шагов вызывают `stop`; удерживаемый инструмент/деталь не «исчезает» из статуса. Не продолжать по старой сцене после смены `scene_revision`, кроме изменений, сделанных и подтверждённых текущим графом.
+- [x] Реализовать `GraphEngine` без ROS, затем сервис `GetRunStatus.srv`, ROS обёртку и машинно-читаемый журнал с `run_id`, ID узла, навыком, событием, следующей вершиной, снимком состояния и причиной терминала. Выполнение в отдельном рабочем потоке; колбэки статуса и отмены остаются доступными. Прогнать `pytest -q src/iiwa_llm_plan_executor/test/test_engine.py src/iiwa_llm_plan_executor/test/test_node.py`; зафиксировать коммит.
+
+### Задача 7. HTTP и MCP через существующий сервер
+
+**Файлы:** создать `src/iiwa_web/iiwa_web/planner.py`, `src/iiwa_web/test/test_planner.py`; изменить `src/iiwa_web/iiwa_web/main.py`.
+
+**Интерфейс:** `GET /planner/scene`, `POST /planner/runs` с `{run_id, scene_revision, response}`, `GET /planner/runs/{run_id}`, `POST /planner/runs/{run_id}/cancel`. Тот же маршрутизатор включить в `_schema_app` до `FastMCP.from_fastapi` и в основной `app`; MCP получает соответствующие инструменты без второго сервера.
+
+- [x] Написать тесты HTTP: корректный JSON снимка, принятие графа без ожидания его завершения, чтение статуса, отмена, ответ при недоступном ROS сервисе, конфликт второго запуска. Проверить, что MCP строится из того же маршрутизатора.
+- [x] Реализовать адаптер через существующий `get_bridge().call_service`; он не интерпретирует узлы и не держит собственный `scene_revision`. В `POST /stop` добавить отмену активного графа до штатной команды `cobot/stop`.
+- [x] Прогнать `pytest -q src/iiwa_web/test/test_planner.py src/iiwa_web/test/test_trajectory_admission.py`; зафиксировать коммит.
+
+### Задача 8. Совместный запуск и документация пользователя
+
+**Файлы:** создать `src/iiwa_llm_plan_executor/launch/planner.launch.py`, `src/iiwa_llm_plan_executor/config/planner.yaml`, `src/iiwa_llm_plan_executor/test/test_launch.py`, `src/iiwa_llm_plan_executor/doc/README.md`; обновить `setup.py` пакета для установки launch/config/doc и при необходимости `src/iiwa_experement/doc/README.md`.
+
+**Интерфейс запуска:** `ros2 launch iiwa_llm_plan_executor planner.launch.py experiment_id:=20`. Launch включает `iiwa_experement/experiment.launch.py mode:=planner experiment_id:=20` и узлы сцены/графа. Уже включённый в `iiwa_bringup` сервер `iiwa_web` запускается **один раз** по `web.enabled` из `base_setting` (в текущем `cobot-setting.yaml` это `true`); новый launch не создаёт второй сервер. Для запуска только с ROS передать настройки с `web.enabled: false`. Значения задержки, таймаутов и J7/наклона берутся из конфигурации.
+
+- [x] Написать тесты launch: в новом режиме есть scene context и executor, нет `reference_runner` и второго экземпляра `iiwa_web`, `simulate:=true`; `experiment_id` 1..20, недопустимый ID и неизвестный режим проверяются до запуска Webots.
+- [x] Реализовать launch с готовностью `scene_ready` и задержкой; описать команды запуска, получение `scene_json`, передачу графа через ROS и HTTP, чтение `/iiwa_llm_plan_executor/plan` и `/iiwa_llm_plan_executor/status`, отмену, события/терминалы и то, что Qwen3 вызывается снаружи.
+- [x] Прогнать `pytest -q src/iiwa_llm_plan_executor/test/test_launch.py` и `colcon build --base-paths src --symlink-install --packages-up-to iiwa_experement iiwa_scene_context iiwa_llm_plan_executor iiwa_web`; зафиксировать коммит.
+
+### Задача 9. Проверки всей цепочки в Webots
+
+**Файлы:** создать `src/iiwa_llm_plan_executor/doc/webots-qa.md`; по результатам исправить только затронутые файлы и тесты задач 1–8.
+
+- [x] `source /opt/ros/jazzy/setup.bash`, затем `colcon build --base-paths src --symlink-install`, `source install/setup.bash`, `colcon test --base-paths src --packages-select iiwa_experement iiwa_scene_context iiwa_llm_plan_executor iiwa_web`, `colcon test-result --verbose`. Сохранить конкретные результаты.
+- [ ] Запустить **одну и ту же** сцену 20 и последовательно передавать разные короткие графы: только `detect(cap)`; `grab(vacuum) → transfer(vacuum,work) → completed` с инструментом в `patron`; отдельным следующим графом `transfer(vacuum,vacuum_socket) → dock_tool(vacuum,vacuum_socket)`; после восстановления сцены — захват и установка люка; отдельно один винт к `hole_1`. Никакой опыт не должен автоматически выполнять полный эталон.
+- [x] Проверить отказ: показанный граф люка без явного взятия присоски отклоняется **до движения**; ошибка захвата ведёт к `inspect_state` или `replan_required`; ложный/несвежий `scene_revision`, отмена и столкновение останавливают дальнейшие движения.
+- [ ] В журнале подтвердить верхний подход, осевой спуск и подъём, фактическое перемещение люка/винта, сохранение инструмента в `patron` после завершения первого графа, возврат в гнездо только по второму явному графу, J7 внутри коридора на **всех** точках, отсутствие столкновений. Записать границы проверки: это Webots, а не проверка физического робота или качества Qwen3.
+- [ ] После исправлений повторить затронутые тесты и `git diff --check`. Зафиксировать итоговый отчёт и передать пользователю команды запуска и ссылки на интерфейсы.
+
+## Критерий готовности
+
+Другой процесс может запросить актуальный `scene_json` через ROS/HTTP/MCP, получить граф от Qwen3 вне этой системы, передать его в ROS/HTTP/MCP, прочитать принятый граф из `/iiwa_llm_plan_executor/plan` и наблюдать каждый узел, событие, состояние и терминал в `/iiwa_llm_plan_executor/status`. Робот в Webots исполняет только данный граф с ограничениями TCP/J7 и подтверждённым захватом; при невозможности безопасного выполнения останавливается и сообщает причину. Система сравнения модели с эталоном остаётся отдельным следующим этапом.
+

docs/superpowers/plans/2026-11-11-cobot-llm-motion-integration.md

@@ -0,0 +1,69 @@
+# Интеграция графов LLM в штатный запуск `cobot`
+
+> Для код-агента. Реализуй задачи по порядку в ветке `experement` по текущей рабочей копии. Здесь описаны изменения и критерии приёмки.
+
+После `cobot run` робот должен принимать готовый JSON-граф LLM и перемещать установленный оператором инструмент в нужную точку. Обычный Webots использует `digital_twin.webots.world`, а запуск контроллера использует FRI. Опыты Grant по-прежнему запускаются в Webots через существующие launch-файлы с явным номером опыта. Внешний клиент вызывает LLM и отправляет готовый граф.
+
+| Запуск | Исполнитель | Источник контекста для LLM |
+| --- | --- | --- |
+| `cobot run local` или `cobot run docker` → «Симулятор Webots» | Базовые перемещения через штатный MoveIt | `iiwa_scene_context` публикует снимок базовой сцены |
+| `cobot run local` или `cobot run docker` → «Контроллер» | Те же перемещения через штатный MoveIt и FRI при включении в YAML | Именованные позиции и телеметрия робота через штатные интерфейсы |
+| `ros2 launch iiwa_llm_plan_executor planner.launch.py experiment_id:=20` | Существующие навыки Grant | Существующий `scene_json` от Grant Supervisor |
+
+Для первой версии предлагаю поддержать именованную позицию и явную TCP-позу, включать исполнение графов на физическом роботе отдельной настройкой и покрыть обе среды `cobot run`. Навыки захвата, переноса объектов, стыковки инструмента и технологических операций входят только в профиль Grant. На физическом роботе `scene_json` пока отсутствует. Имя инструмента, его TCP, коллизионная модель и нагрузка берутся из действующей конфигурации робота и контроллера.
+
+## 1. Подключить узлы к штатному запуску
+
+- [ ] Добавить в `cobot-setting.yaml` и `src/iiwa_config/config/setting.yaml` одинаковый блок `llm_planner`. В блок входят `enabled: true`, `hardware_enabled: false` и `velocity_scale: 0.10`. Значение масштаба `0.10` служит начальным значением для Webots. Для физического прогона оператор задаёт согласованное значение. В `src/iiwa_utils/iiwa_utils/setting_loader.py` добавить типизированное чтение блока, строгие булевы значения и конечный масштаб в диапазоне `(0, 1]`. Отсутствие блока в старом YAML означает выключенную интеграцию. При выключенном `enabled` штатный запуск работает независимо от `hardware_enabled`.
+- [ ] Создать `src/iiwa_llm_plan_executor/launch/cobot.launch.py` с аргументами `setting`, `simulate`, `rviz`. Включить `iiwa_bringup/iiwa.launch.py` один раз с теми же аргументами. При `enabled: true` и `simulate: true` добавить `scene_context` и `llm_plan_executor` в базовом режиме с `use_sim_time: true`. При `simulate: false` и двух включённых флагах добавить только исполнитель с `use_sim_time: false`. В обоих режимах использовать настройки и пути штатного bringup. Проверять конфигурацию до создания процессов.
+- [ ] Использовать общий конструктор команды запуска в `cobot/commands/run.py` для `_run_local` и `_run_docker`. Сохранить меню, подключение ROS overlay, передачу `simulate`, монтирование `cobot-setting.yaml` и остановку дочернего дерева. Для устаревшей сборки вывести команду `cobot rebuild --symlink-install` или `cobot docker-setup` и вернуть ошибку запуска.
+- [ ] Объявить зависимости нового launch в `src/iiwa_llm_plan_executor/package.xml` и проверить установку через `setup.py`. Оставить направление зависимости от нового launch к `iiwa_bringup`. Обратная зависимость образует цикл `iiwa_bringup → iiwa_llm_plan_executor → iiwa_experement → iiwa_bringup`. Существующие `planner.launch.py` и `experiment.launch.py` продолжают включать низкоуровневый bringup и свои узлы Grant.
+- [ ] Тесты в `cobot/test/` и `src/iiwa_llm_plan_executor/test/` покрывают матрицу Webots/FRI × флаги, старый и неверный YAML, аргументы local/Docker и единственный экземпляр контроллера, MoveIt и `iiwa_web`. При запуске обычного Webots отсутствуют процессы `scene_spawner`, `grant_supervisor` и `reference_runner`. Для существующего launch Grant сохранить прежние тесты, включая отказ при `simulate:=false`.
+
+## 2. Принимать базовые графы без сцены Grant
+
+- [ ] В `src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/{node,engine,validator,skills}.py` выделить режим `base` с собственными проверками и двумя навыками. Сохранить общий разбор JSON, публикацию оригинала, `run_id`, повторную отправку, один активный запуск, статус, журнал и отмену. Сценозависимые проверки ревизии, владения и готовности оставить в режиме Grant. В `base` определять готовность по актуальному полному `/joint_states`, готовности штатного сервера движения и активному контроллеру. Проверять состояние перед запуском графа и перед каждым шагом. Выход FRI из рабочего состояния, потеря обратной связи или недоступный контроллер прерывают граф и запрещают следующий шаг. Учитывать уже реализованную обработку `COMMANDING_ACTIVE` в `iiwa_controller`.
+- [ ] Сохранить формат `status`, `entry`, `plan`, `transitions` и интерфейс `/iiwa_llm_plan_executor/submit_plan` (`iiwa_msgs/srv/SubmitPlan`). Для `move_to_named` поле `target` задаёт имя `group_state` из активного SRDF для `planning.planning_group`. Для `move_to_pose` требовать ровно шесть чисел `x`, `y`, `z` в метрах и `a`, `b`, `c` в радианах в `params`. Углы задаются в порядке ZYX по соглашению KUKA ABC, фрейм фиксирован `base_link`, TCP-линк берётся из `planning.pose_link`. Для обоих навыков `object` пустой. У `move_to_pose` поле `target` также пустое. Имена, числа, допустимые поля и переходы проверить для всех узлов при приёме. Отклонять неизвестные команды, ссылки на объекты Grant, `NaN`/`Inf`, внешние фреймы и попытки переопределить масштаб скорости. Публиковать результат шага `success` или `motion_failed` по результату сервера. Для `motion_failed` разрешать только переход в `operator_required` или `replan_required`. При отсутствии такого перехода завершать граф как `failed` с причиной.
+- [ ] Передавать TCP-цель в штатный action `cobot/move_to_pose` (`iiwa_msgs/action/MoveToPose`). Для именованного движения добавить отменяемый `cobot/move_to_named_pose` (`iiwa_msgs/action/MoveToNamed`) в `src/iiwa_planning/scripts/move_to_pose_server.py` и `src/iiwa_msgs`. Вынести существующее планирование именованной цели из `cobot/move_to_named` в общий участок, чтобы оба интерфейса использовали SRDF, Pilz PTP, лимиты и `MotionSerialiser`. Штатный сервис сохраняет контракт. Для TCP-цели применять настроенный планировщик и масштаб, согласованные с MoveIt. Успех шага требует успешного результата action, включая выполнение траектории контроллером.
+- [ ] Подключить отмену графа к goal handle MoveIt и штатной остановке. Проверить окно между отправкой цели и получением goal handle, потерю контроллера и повторный submit после отмены. `POST /stop` должен остановить граф, отправленный через ROS, HTTP или MCP. Текущий `_active_run_id` в `src/iiwa_web/iiwa_web/planner.py` учитывает только HTTP-запросы. Источник активного запуска и его отмены должен находиться в executor. Штатный `/cobot/stop` должен инвалидировать активную операцию и ожидающие шаги графа. В базовом режиме графы и обычные запросы MoveIt проходят через одну сериализацию. Проверить, что прямой HTTP-ввод траекторий не перебивает активный граф.
+- [ ] Добавить тесты базового графа с подменой ROS action-сервера. Покрыть успешные именованную и TCP-цели, отсутствие `scene_json` на FRI, отказ при неизвестном имени и некорректных координатах, `not_ready` при устаревших суставах, ошибку контроллера, отмену до принятия цели и в процессе исполнения, дубликат и параллельный запуск. Прогнать существующие тесты Grant и `test_motion_priority.py`.
+
+## 3. Выдавать честный `scene_json` в базовом Webots
+
+- [ ] В `src/iiwa_scene_context/iiwa_scene_context/{node,snapshot}.py` добавить режим `base`. Источник актуальности базового снимка — `/joint_states` и TF из `base_link` к настроенному TCP, если трансформ доступен. Публиковать тип базового профиля, метку времени, наблюдаемое состояние, доступные базовые навыки и имена поз из активного SRDF. В пустом мире набор наблюдаемых объектов задачи пуст. У базового снимка нет номера опыта. Отсутствующие данные о камерах, объектах и установленном инструменте обозначать явно. Сцена без объектов задачи не подтверждает отсутствие препятствий для MoveIt.
+- [ ] Сохранить `/iiwa_scene_context/get_scene`, QoS `RELIABLE`/`TRANSIENT_LOCAL` для `/iiwa_scene_context/scene_json` и действующий формат Grant. Базовый контекст не подписывается на `/iiwa_experement/object_state` и не ждёт `/iiwa_experement/scene_ready`. Возвращать понятный отказ до первого валидного состояния и при его устаревании. Для FRI сервис сцены не запускается, а HTTP/MCP сообщает, что `scene_json` доступен только в Webots. Маршруты отправки, статуса и отмены работают при включённом `web.enabled` в обоих режимах.
+- [ ] Тесты проверяют свежесть и содержимое пустой сцены без Grant, достоверность источников полей и неизменный контракт сцены опыта. Проверить, что пропажа базового `scene_json` сама по себе не блокирует допустимую команду при наличии актуальной телеметрии и готового MoveIt.
+
+## 4. Собрать и проверить запуск из установленных пакетов
+
+- [ ] В обоих `docker/jazzy/{ros-iiwa7,ros-iiwa7-webots}/Dockerfile` добавить исходники `iiwa_experement`, `iiwa_scene_context` и `iiwa_llm_plan_executor` в контекст перед `rosdep` и `colcon`. Проверить зависимости из `package.xml` и доступность launch, конфигов и ROS-интерфейсов в release-образах после удаления `src`. Docker для контроллера не требует приложения Webots. Через `cobot docker-setup` собрать оба образа. Проверить `cobot run docker` на новом Webots-образе и стенде FRI с имитацией аппаратуры.
+- [ ] С ROS 2 Jazzy запустить `cobot rebuild --symlink-install`, подключить `install/setup.bash` и выполнить `colcon test --base-paths src` и `colcon test-result --verbose`. Для общего запуска `pytest` по нескольким пакетам использовать `--import-mode=importlib` из-за совпадающих имён `test_launch.py`. Сверить результаты с исходным состоянием рабочей копии, включая незакоммиченный `src/iiwa_msgs/srv/SubmitPlan.srv`.
+- [ ] В обычном Webots запустить `cobot run local` → «Симулятор Webots». Проверить один набор штатных узлов и базовый снимок через `ros2 service call /iiwa_scene_context/get_scene std_srvs/srv/Trigger '{}'`. Сначала отправить отклоняемый граф и убедиться, что контроллер не получил цель. Затем отправить разрешённую именованную цель и допустимую TCP-цель, выбранную для установленной модели и сцены. Для обеих сверить `/plan`, `/status`, фактическую позу и `completed`. Отдельно проверить отмену, общий stop и чистое завершение по Ctrl-C. Повторить на Docker. Проверить, что ручной запуск Grant с `experiment_id:=20` отдаёт прежнюю сцену и принимает прежний граф.
+- [ ] Для FRI сначала проверить ветку запуска с имитацией action-сервера и контроллера либо с тестовым FRI-стендом из `src/iiwa_controller/test/`. Убедиться, что нет Webots и сервиса сцены, выключенная настройка исключает executor, а при включении граф до готовности возвращает `not_ready`. Физическую приёмку выполнять с оператором после проверки сети, активного режима FRI, лимитов, модели инструмента, Load Data и рабочего пространства. Начать с одной заранее проверенной именованной позиции на согласованной скорости. В отчёте указать отдельно результаты Webots, FRI-имитации и физической проверки.
+- [ ] Описать операторский запуск и примеры графов в `src/iiwa_llm_plan_executor/doc/README.md`, контракт базовой сцены в `src/iiwa_scene_context/docs/interfaces.md` и настройки в `doc/lwc-doc/docs/getting-started/configuration{,.en}.md`.
+
+Пример тела `POST /planner/runs` для Webots. Перед запуском проверьте, что `work` есть в активном SRDF и применим к текущей сцене.
+
+```json
+{
+  "run_id": "base-webots-1",
+  "response": {
+    "status": "ready",
+    "entry": "go",
+    "plan": [{
+      "id": "go",
+      "skill": "move_to_named",
+      "target": "work",
+      "transitions": [
+        {"event": "success", "next": "completed"},
+        {"event": "motion_failed", "next": "operator_required"}
+      ]
+    }]
+  }
+}
+```
+
+## Ссылки для реализации
+
+- Проектные контракты по запуску, API и остановке. [Deployment](../../../openspec/specs/deployment/spec.md), [External API](../../../openspec/specs/external-api/spec.md), [Motion arbitration](../../../openspec/specs/motion-arbitration/spec.md).
+- Документация ROS 2 Jazzy по вложенным launch-файлам и зависимостям. [Launch](https://github.com/ros2/ros2_documentation/blob/jazzy/source/Tutorials/Intermediate/Launch/Using-ROS2-Launch-For-Large-Projects.rst), [rosdep](https://github.com/ros2/ros2_documentation/blob/jazzy/source/Tutorials/Intermediate/Rosdep.rst).

docs/superpowers/specs/2026-11-08-grant-experiments.md

@@ -0,0 +1,225 @@
+# Эксперименты для LLM-планировщика сборки люка
+
+Цель набора — проверить, как LLM-планировщик преобразует пользовательский запрос и `scene_json` в граф действий `skill` с переходами по событиям. В наборе 20 экспериментов: четыре категории по пять заданий. Сложность внутри каждой категории возрастает.
+
+Эксперименты опираются на технологическую карту «ТехнКарта вер. 1.0.xlsx» и модели из `src/iiwa_description/objects/grant`. В сцене работает один робот-манипулятор. Корпус закреплён в стапеле; оператора, мобильной платформы и вспомогательного робота нет. Отверстия для четырёх винтов считаются подготовленными, поскольку модели сверлильного шпинделя среди доступных объектов нет. Во всех экспериментах на роботе постоянно установлен `patron`: рабочие инструменты вставляются в него. После установки люк удерживается виртуальной фиксацией: это позволяет роботу отпустить присоску и сменить рабочий инструмент. Начальные положения объектов описаны отдельно в [experiment-scenes.md](experiment-scenes.md).
+
+## Контракт для экспериментов
+
+В исходном примере доступны `detect`, `approach`, `lower`, `grab`, `lift`, `transfer`, `align`, `release`, `retreat`, `capture`, `verify`, `stop`, `wait_scene_clear`, `inspect_state`, `notify_operator`. План должен вставлять присоску из стойки в постоянный `patron`, если начальное состояние содержит `tool_docked`; перед `grab` требуется контактное `lower`, а перед отпусканием люка в посадочном месте — его опускание до контакта. После отпускания нужен безопасный отвод и возврат. `dock_tool` извлекает рабочий инструмент из `patron`; сам `patron` остаётся на роботе.
+
+Для остальных операций соответствующий `scene_json` расширяется следующими навыками:
+
+| Навык | Действие | Основные исходы |
+| --- | --- | --- |
+| `dispense` | Пройти соплом именованный участок контура с условной подачей герметика | `completed`, `interrupted`, `failed` |
+| `press` | Прижать установленный люк в заданной зоне | `success`, `failed` |
+| `seat_screw` | Подтвердить, что винт после осевого опускания остался в отверстии | `seated`, `not_seated`, `failed` |
+| `tighten` | Затянуть конкретный винт до момента, заданного в сцене | `success`, `torque_not_reached`, `failed` |
+| `dock_tool` | Вернуть инструмент в его гнездо и отсоединить от робота | `success`, `failed` |
+
+Контракты `verify` и `inspect_state` нужно расширить на контур герметика и винты. Для тестов достаточно состояния симулятора; `capture` не входит в основные маршруты. Предлагаемые идентификаторы: `cap`, `hatch_seat`, `body`, `sealant_path`, `side_1`…`side_4`, `hole_1`…`hole_4`, `screw_1`…`screw_10`, `screw_tray`, `patron`, `vacuum`, `sealant_tool`, `magnetic_tool`, `torque_tool`.
+
+Каждый эксперимент начинается в домашней позиции. К объекту робот идёт через безопасную точку над ним; после контакта сначала отходит по оси инструмента. В конце он возвращает инструмент в стойку и идёт домой. Ветви движения при `collision` должны вести к остановке; при неопределённом состоянии нельзя продолжать перенос или отпускание детали. Число повторных попыток задаёт `recovery_attempts_per_component`.
+
+## I. Захват и установка люка
+
+### 1. Базовая установка
+
+**Запрос модели:** «Возьми люк присоской, перенеси на корпус, установи, отпусти и проверь итоговое положение».
+
+**Условия:** люк лежит в известной области; присоска в стойке; посадочное место свободно.
+
+**Действия робота:** найти присоску → подойти и опуститься к гнезду → захватить инструмент → подняться → найти люк → подойти над ним и опуститься до контакта → включить захват → поднять люк → найти посадочное место → перенести люк над ним → совместить → опустить до посадки → отпустить → отвести присоску → проверить положение люка → вернуть присоску в стойку → домой.
+
+**Проверка плана:** работа с деталью не начинается раньше подтверждённого захвата инструмента; `verify` следует за `release`.
+
+### 2. Люк с изменённой исходной ориентацией
+
+**Запрос:** «Установи люк на корпус, даже если он лежит повёрнутым относительно посадочного места».
+
+**Условия:** положение люка обнаруживается во время выполнения; его начальный угол отличается от целевого.
+
+**Действия робота:** взять присоску → обнаружить фактическую позу люка → подойти и захватить его → поднять выше стапеля → перенести в безопасную точку над корпусом → повернуть и совместить с `hatch_seat` через `align` → опустить → отпустить → отвести инструмент → проверить установку → вернуть инструмент → домой.
+
+**Проверка плана:** план использует результат `detect`, а не предполагает исходную ориентацию детали из текста запроса.
+
+### 3. Люк и посадочное место обнаруживаются отдельно
+
+**Запрос:** «Найди люк и его посадочное место, затем установи люк и проверь результат».
+
+**Условия:** обе позы уточняются при выполнении.
+
+**Действия робота:** взять присоску → `detect(cap)` → подойти, опуститься, захватить и поднять люк → `detect(hatch_seat)` → перенести над найденной точкой → совместить → опустить → отпустить → отвести инструмент → проверить → вернуть инструмент → домой.
+
+**Ветви и проверка:** `not_found` у любого обнаружения ведёт к `replan_required`; перенос к посадочному месту не начинается, пока оно не найдено.
+
+### 4. Неудачный первый захват
+
+**Запрос:** «Установи люк; если присоска не удержала его с первого раза, безопасно повтори попытку».
+
+**Условия:** исполнитель один раз возвращает `grasp_failed` при захвате люка; `recovery_attempts_per_component = 1`.
+
+**Действия робота:** взять присоску → найти люк → подойти и опуститься → попытаться захватить. При `grasp_failed`: остановиться → проверить, пуст ли захват и где находится люк → заново обнаружить люк → повторить подход, опускание и захват один раз → поднять, перенести, совместить, установить, отпустить и проверить → вернуть инструмент → домой.
+
+**Проверка плана:** после `grasp_failed` робот не выполняет `lift` вслепую; второй отказ заканчивается остановкой и уведомлением.
+
+### 5. Исправление положения после установки
+
+**Запрос:** «Установи люк и проверь положение. Если он оказался вне допуска, поправь его одной повторной установкой».
+
+**Условия:** первая посадка даёт `verify:not_ok`; люк остаётся доступным для повторного захвата.
+
+**Действия робота:** выполнить установку как в эксперименте 1 → получить `not_ok` → обнаружить фактическое положение люка → подойти присоской, опуститься, снова захватить и поднять → повторно обнаружить посадочное место → перенести, совместить, опустить и отпустить → повторить `verify` → вернуть инструмент → домой при `ok`.
+
+**Проверка плана:** не более одной повторной установки; при неизвестном состоянии детали или второй неуспешной проверке граф заканчивается `operator_required`.
+
+## II. Работа с герметиком
+
+Для этих экспериментов `sealant_path` разбит на именованные участки `side_1`…`side_4`. Герметик условный: проверяются маршрут робота и изменение состояния контура в симуляторе.
+
+### 6. Один прямой участок
+
+**Запрос:** «Нанеси герметик на первый прямой участок посадочного контура корпуса».
+
+**Условия:** инструмент в стойке; корпус закреплён.
+
+**Действия робота:** найти инструмент → подойти, опуститься и взять его → подняться → найти `side_1` → подойти над началом участка → опустить сопло → выполнить `dispense(side_1)` → отвести сопло вверх → вернуть инструмент → домой → проверить, что обработан именно `side_1`.
+
+**Проверка плана:** нет подачи при перемещении от стойки к корпусу.
+
+### 7. Два смежных участка
+
+**Запрос:** «Нанеси непрерывную линию герметика на первую и вторую стороны посадочного контура».
+
+**Условия:** стороны соединяются одним углом.
+
+**Действия робота:** взять инструмент → обнаружить контур → подойти к началу `side_1` → опустить сопло → пройти `side_1` → перейти через угол без отвода от рабочей высоты → пройти `side_2` → остановить подачу → поднять инструмент → вернуть в стойку → домой → проверить оба участка.
+
+**Проверка плана:** план сохраняет порядок смежных участков и не поднимает сопло посреди непрерывной линии.
+
+### 8. Замкнутый контур
+
+**Запрос:** «Нанеси герметик по всему замкнутому контуру посадочного места люка».
+
+**Условия:** четыре стороны доступны инструменту.
+
+**Действия робота:** взять инструмент → найти контур → подойти к выбранному началу → опустить сопло → последовательно выполнить `dispense` для четырёх сторон и углов → завершить контур в точке начала → поднять сопло → вернуть инструмент → домой → проверить полноту контура.
+
+**Проверка плана:** перечислены все четыре участка ровно по одному разу; конечная проверка относится ко всему контуру.
+
+### 9. Прерывание подачи
+
+**Запрос:** «Нанеси герметик по контуру; если подача прервётся, возобнови её с места разрыва и закончи контур».
+
+**Условия:** на третьем участке один раз приходит `dispense:interrupted`.
+
+**Действия робота:** взять инструмент → обработать первые два участка → начать третий → при прерывании остановить действие и отвести сопло → проверить состояние обработанных участков и последнюю подтверждённую точку → снова подойти к началу недостающего отрезка → опустить сопло → закончить третий и четвёртый участки → проверить контур → вернуть инструмент → домой.
+
+**Проверка плана:** повторно не проходится весь контур; неизвестная точка разрыва ведёт к `replan_required`.
+
+### 10. Герметик и установка люка
+
+**Запрос:** «Нанеси герметик по всему посадочному контуру, затем возьми люк присоской и установи его на корпус».
+
+**Условия:** люк лежит отдельно; для его установки после отпускания предусмотрена виртуальная фиксация.
+
+**Действия робота:** взять инструмент герметика → обработать четыре участка → проверить контур → вернуть инструмент → взять присоску → найти и захватить люк → поднять и перенести его над корпусом → совместить с посадочным местом → опустить и прижать → отпустить → проверить положение люка → вернуть присоску → домой.
+
+**Проверка плана:** присоска не берётся, пока инструмент герметика не возвращён; люк не опускается на неполный контур.
+
+## III. Подача винтов и затяжка
+
+В этих сценах люк уже установлен и зафиксирован, а четыре отверстия подготовлены. В лотке всегда находятся десять одинаковых винтов. Для задания можно выбрать любой доступный винт; проверка плана должна требовать нужное число **разных** винтов, а не заранее заданные номера. Магнитный инструмент переносит винт; `seat_screw` подтверждает, что винт остаётся в отверстии после отвода магнита.
+
+### 11. Один винт
+
+**Запрос:** «Возьми винт из лотка и установи его в первое отверстие люка».
+
+**Действия робота:** взять магнитный инструмент → найти один доступный винт в лотке → подойти и опуститься к нему → захватить и поднять → найти `hole_1` → перенести винт над отверстием → совместить оси → опустить → выполнить `seat_screw` → отпустить винт → отвести инструмент → проверить наличие винта в отверстии → вернуть инструмент → домой.
+
+**Проверка плана:** `release` невозможен до подтверждённой посадки винта.
+
+### 12. Два винта по диагонали
+
+**Запрос:** «Установи два винта в диагонально противоположные отверстия люка».
+
+**Действия робота:** взять магнитный инструмент → установить первый винт по цепочке «найти → захватить → поднять → перенести → совместить → опустить → посадить → отпустить» → подняться над люком → вернуться к лотку → повторить для диагонально противоположного отверстия → проверить обе точки → вернуть инструмент → домой.
+
+**Проверка плана:** второму винту соответствуют другая ячейка лотка и другое отверстие.
+
+### 13. Четыре винта крестовым порядком
+
+**Запрос:** «Установи четыре винта в отверстия люка крестовым порядком».
+
+**Действия робота:** взять магнитный инструмент → последовательно взять винты из четырёх ячеек и установить их в порядке `hole_1 → hole_3 → hole_2 → hole_4`; перед каждым переносом подтвердить захват, перед отпусканием — посадку → проверить наличие четырёх винтов → вернуть инструмент → домой.
+
+**Проверка плана:** нет повторного использования ячейки или отверстия; после каждой установки робот возвращается к лотку с пустым магнитным захватом.
+
+### 14. Установка и затяжка четырёх винтов
+
+**Запрос:** «Установи четыре винта крестовым порядком, затем затяни каждый до заданного момента».
+
+**Действия робота:** выполнить четыре установки как в эксперименте 13 → вернуть магнитный инструмент → взять инструмент затяжки → для каждого из четырёх винтов подойти сверху, совместить инструмент с осью, опуститься, выполнить `tighten`, отвести инструмент → проверить четыре результата затяжки → вернуть инструмент → домой.
+
+**Проверка плана:** `tighten` не вызывается для винта без подтверждённого `seat_screw`.
+
+### 15. Ошибка захвата и недостигнутый момент
+
+**Запрос:** «Установи и затяни четыре винта. Если один винт не захватится или затяжка не достигнет нужного момента, повтори соответствующее действие один раз».
+
+**Условия:** первый захват третьего выбранного винта даёт `grasp_failed`; первая затяжка в `hole_4` даёт `torque_not_reached`.
+
+**Действия робота:** установить первые два винта → на третьем остановиться, проверить состояние захвата и повторно обнаружить винт → повторить его захват один раз → установить третий и четвёртый → сменить магнитный инструмент на затяжной → затянуть четыре винта; для четвёртого после проверки состояния повторить затяжку один раз → проверить все четыре точки → вернуть инструмент → домой.
+
+**Проверка плана:** ошибки обрабатываются у соответствующего винта; уже установленные винты не вынимаются и не устанавливаются повторно.
+
+## IV. Полная сборка и перепланирование
+
+### 16. Герметик, люк и один винт
+
+**Запрос:** «Нанеси герметик на заданный участок, установи люк и закрепи его одним винтом».
+
+**Действия робота:** взять инструмент герметика → обработать участок → вернуть инструмент → взять присоску → захватить, перенести, совместить, опустить и отпустить люк → проверить его положение → вернуть присоску → взять магнитный инструмент → установить один винт → вернуть магнит → взять инструмент затяжки → затянуть винт → вернуть инструмент → домой.
+
+**Проверка плана:** зависимости «герметик → люк → винт → затяжка» соблюдены.
+
+### 17. Полный контур и два крепления
+
+**Запрос:** «Нанеси полный контур герметика, установи люк и закрепи его двумя диагональными винтами».
+
+**Действия робота:** пройти четыре участка герметика и проверить контур → вернуть инструмент → присоской установить и проверить люк → вернуть присоску → магнитным инструментом установить два диагональных винта → вернуть магнит → затянуть оба винта → проверить две затяжки → вернуть инструмент → домой.
+
+**Проверка плана:** если контур не подтверждён, план не переходит к установке люка.
+
+### 18. Полная сборка
+
+**Запрос:** «Нанеси герметик по контуру, установи люк, поставь четыре винта крестовым порядком и затяни их».
+
+**Действия робота:** полный контур герметика → проверка → смена на присоску → захват и установка люка → проверка → смена на магнит → четыре установки винтов → смена на инструмент затяжки → четыре затяжки → итоговая проверка люка и крепежа → вернуть инструмент → домой.
+
+**Проверка плана:** у каждого из четырёх винтов есть собственные узлы установки и затяжки; итоговая проверка охватывает всю сборку.
+
+### 19. Полная сборка при смещённых деталях
+
+**Запрос:** «Собери узел полностью. Люк и лоток могут лежать не в номинальных местах — используй их фактическое положение».
+
+**Условия:** симулятор смещает люк и лоток в допустимой рабочей области; `detect` возвращает их актуальные позы.
+
+**Действия робота:** нанести и проверить герметик → обнаружить фактическую позу люка, захватить и установить его → проверить → обнаружить фактическую позу лотка и по очереди найти четыре винта → установить их → затянуть → проверить сборку → вернуть инструмент → домой.
+
+**Проверка плана:** план не подставляет прежние координаты люка и винтов после нового `detect`; зависимости операций сохраняются.
+
+### 20. Полная сборка с несколькими сбоями
+
+**Запрос:** «Выполни полную сборку. При прерывании подачи герметика, неудачном захвате винта или ошибке установки люка восстанови работу, если состояние известно и разрешена одна повторная попытка».
+
+**Условия:** один раз прерывается `dispense`, первый захват одного винта неудачен, первая проверка положения люка возвращает `not_ok`.
+
+**Действия робота:** начать контур → определить необработанный отрезок и завершить его → проверить контур → установить люк → при `not_ok` заново обнаружить и один раз переставить его → проверить → устанавливать винты; при `grasp_failed` остановиться, проверить состояние и один раз повторить захват нужного винта → затянуть четыре винта → проверить четыре установки, четыре затяжки и положение люка → вернуть инструмент → домой.
+
+**Проверка плана:** восстановление начинается от подтверждённого состояния; лимит попыток соблюдён; любое `unknown` или повторный отказ ведёт к остановке и уведомлению.
+
+## Общие критерии оценки графа
+
+Для каждого ответа планировщика следует проверить существование всех узлов, указанных в `next`; соблюдение зависимостей операций; отсутствие физически невозможных переходов, например `transfer` без подтверждённого захвата; корректное завершение аварийных ветвей; соблюдение лимита повторных попыток. Семантическая проверка графа полезнее посимвольного сравнения с одним образцом JSON.

docs/superpowers/specs/2026-11-08-grant-reference-control-design.md

@@ -0,0 +1,48 @@
+# Эталонное управление экспериментами Grant
+
+## Назначение
+
+Эталонный исполнитель физически перемещает робот iiwa 7 в Webots для всех 20 экспериментов. Его журнал действий и результатов нужен для сравнения с планами LLM. Один запуск выбирает один эксперимент и один режим: `view` показывает готовую начальную сцену без движения, `execute` после готовности сцены и настраиваемой задержки автоматически выполняет эталонный сценарий. Оба режима используют существующую команду `ros2 launch iiwa_experement experiment.launch.py experiment_id:=N mode:=...`.
+
+## Принятый подход
+
+Пользователь выбрал детерминированный захват: отдельный supervisor пакета `iiwa_experement` перемещает взятый объект вслед за фактической позой конца `patron`; состояние объекта и прикреплённая геометрия синхронизируются с MoveIt. Физические соединители Webots не применяются. Постоянный `patron` никогда не снимается. Установка люка, движение с ним и движения вдоль участков герметика и между отверстиями должны быть видимыми траекториями робота; перемещать деталь прямо к цели без движения робота нельзя. Исключение — удаление переданного винта из сцены после подтверждённого отпускания у отверстия, что моделирует получение винта человеком вне сцены.
+
+Рассмотренная альтернатива — Webots `Connector`/`VacuumGripper`. Она даёт более физический контакт, но требует переделки модели робота и инструментов и добавляет нестабильность контактной физики в эталонные прогоны. Для повторяемого сравнения планов выбран supervisor и явный автомат состояний.
+
+## Источники и ограничения
+
+- Ветка разработки: `experement`; вся новая логика, конфигурация, launch и документация лежат в `src/iiwa_experement/`. Существующие OBJ/MTL и постоянная модель `patron` остаются в `src/iiwa_description/`.
+- Мир: `src/iiwa_experement/worlds/grant/iiwa-grant.wbt`; начальные позы и состав объектов: `src/iiwa_experement/config/scenes.yaml`. Справочные файлы: `src/iiwa_experement/doc/experements.md`, `src/iiwa_experement/doc/experiment-scenes.md`, а также исходные `docs/superpowers/specs/2026-11-08-grant-experiments.md` и `docs/superpowers/specs/2026-11-08-grant-scenes.md`.
+- Базовый мир и геометрия: `src/iiwa_description/worlds/iiwa.wbt`, `src/iiwa_description/resource/protos/CollaborativeRobotTable.proto`, `src/iiwa_description/objects/grant/` и `output/reference_photo/grant/`.
+- В `patron.xacro` фрейм `tcp` находится на 0,0552 м от начала звена `patron`; текущий сервер движения использует `pose_link: tcp`. Координаты Webots и `base_link` отличаются позой робота `(-0.25, 0, 0.79)`; преобразование надо брать из активных настроек/TF, а не дублировать в коде.
+- Текущий `/Ros2Supervisor` имеет создание и удаление узлов, но не сервис произвольного перемещения. Для этого нужен отдельный supervisor в пакете эксперимента; существующий спавнер остаётся владельцем начальной сцены.
+- Исполнитель запускается только в симуляции. Пакет не должен посылать движения реальному контроллеру FRI.
+
+## Контракт режимов запуска
+
+`mode:=view` — режим по умолчанию для обратной совместимости. Проверяет ID и конфигурацию, открывает мир, создаёт начальные объекты, оставляет робот в исходном положении и публикует `scene_ready`. Не запускает эталонный исполнитель, не захватывает и не меняет объекты. `mode:=execute` делает то же, затем ждёт `scene_ready=true`, готовности controller/MoveIt/supervisor и свежих `joint_states`, после чего ждёт `start_delay_sec` секунд **симуляционного времени** (по умолчанию 3,0 с) и начинает движение. Неверный режим или отрицательная/нечисловая задержка отклоняются до запуска Webots. Тайм-аут готовности даёт ошибку без движения. При паузе Webots симуляционная задержка не истекает.
+
+## Геометрия и захват
+
+Для каждого инструмента задаются калиброванные преобразования `T_tcp_mount` и `T_tcp_work_tip`, корпус столкновений и рабочая ось. Модель инструмента не крепится по центру: локальная ось OBJ направлена по Y, после импорта по Z; длина вакуумного, магнитного и затяжного инструмента 0,14 м, герметик масштабируется в 10 раз до 0,14 м. Успешное `grab`/`dock_tool` разрешается только после достижения позы контакта и проверки расстояния/ориентации. В Webots объект становится управляемым supervisor и следует за фактической позой `tcp`; в MoveIt объект удаляется из мира и прикрепляется к роботу. Обратные операции сначала подтверждают посадку в гнезде/на корпусе, потом меняют владение. Допускается не более одного рабочего инструмента в `patron` и одной переносимой детали на соответствующем инструменте.
+
+Люк захватывается присоской, переносится роботом, ориентируется относительно `hatch_seat`, опускается до посадочной позы и после отпускания фиксируется в сцене. Для опыта 5 первая установка намеренно смещена по X на 0,003 м, после чего выполняется одна корректировка. Герметик — состояние четырёх именованных сторон, но сопло действительно проходит их геометрические траектории в Webots. Прерывание на стороне 3 восстанавливается с последней подтверждённой точки, а не повторяет весь контур.
+
+Винты не вставляются и не затягиваются. Магнит берёт один из десяти ещё доступных винтов; робот переносит его вдоль безопасных переходов, подводит к оси конкретного `hole_i` и к позе передачи над отверстием. Только после достигнутой позы выполняется `release`/`handoff_screw`: винт исчезает из сцены, состояние становится `delivered(screw_i, hole_i)`, отверстие остаётся пустым. Оператор в сцене не моделируется. `seat_screw`, `tighten`, проверки момента и затяжной инструмент исключаются из эталонных сценариев и документации.
+
+## Движение и предотвращение столкновений
+
+MoveIt получает геометрию стола, стапеля, корпуса, стойки, лотка, оставшихся винтов, люка и инструментов. Переносимый инструмент/деталь представлен как прикреплённый объект с проверенным преобразованием и габаритами. Состояние сцены подтверждается в MoveIt до следующего планирования. Свободное перемещение выполняется через проверенную безопасную область над препятствиями, контактный подход и отход — короткими осевыми движениями; непрерывные стороны герметика проходят подряд на рабочей высоте. Высоты, позы и допуски хранятся в конфигурации и подтверждаются измерением геометрии, IK и прогонами Webots. Контакт разрешается только между рабочим кончиком и текущей целью в соответствующей фазе, без общего отключения проверок столкновений.
+
+После каждого движения проверяются результат action, фактическая поза TCP, состояние крепления и отсутствие запрещённого контакта. При коллизии, недостижимой позе, устаревшей позе, пропаже объекта или неизвестном состоянии — остановка, запись причины и запрет следующего действия. Никакая фиксированная высота или зазор не считаются гарантией безопасности без проверки всех 20 сцен.
+
+## Эталонные сценарии и результат
+
+Сохраняются 4 категории по 5 задач с нарастанием сложности: 1–5 установка люка; 6–10 герметик и люк; 11–15 передача одного, двух, трёх, четырёх, затем четырёх винтов с восстановлением; 16–20 композиции герметика, люка и соответственно одного, двух, четырёх, четырёх при смещённой геометрии, четырёх с отказами. Опыты 14–20 не используют затяжку. Сбой захвата в 15 и 20 относится к выбранному винту; события прерывания герметика и неверной первой посадки люка остаются.
+
+Для каждого прогона нужен машиночитаемый журнал в `~/.ros/iiwa_experement/runs/` по умолчанию (путь переопределяется аргументом `trace_dir`) с `experiment_id`, временем симуляции, именем операции/объекта/цели, исходом, фактическими позами и переходом состояния. Финальный отчёт различает `completed`, `stopped`, `failed` и содержит обработанные стороны, позу люка, пары доставленных винтов и оставшиеся винты. Это эталон поведения для последующего сравнения с планом LLM; автоматический оценщик LLM в данную работу не входит.
+
+## Проверка результата
+
+Сборка и тесты пакета ROS 2 Jazzy, затем Webots-прогоны всех 20 опытов в `execute`. Для каждого подтверждаются движение робота и объекта, отсутствие запрещённых пересечений, корректное положение инструментов на кончике `patron`, возврат инструмента и домашняя поза в успешном маршруте. Отдельно проверяются отказы 4, 5, 9, 15, 20 и `view`: в нём робот и все объекты остаются неподвижными. Настоящее оборудование не применяется.

docs/superpowers/specs/2026-11-08-grant-scenes.md

@@ -0,0 +1,91 @@
+# Начальная сцена для каждого эксперимента
+
+Этот документ задаёт положения объектов для 20 экспериментов из [experements.md](experements.md). Единицы измерения — метры; координаты даны в мировой системе Webots. Это проект начальной компоновки для будущего `src/iiwa_description/worlds/grant/iiwa-grant.wbt`, а не описание уже созданного мира.
+
+## Основа мира и постоянные элементы
+
+Базовый мир — `src/iiwa_description/worlds/iiwa.wbt`. В нём есть стол, ограждения и освещение. Робот появляется через запуск в точке `(-0.25, 0, 0.79)`, с нулевым поворотом вокруг Z. Верх столешницы находится на `Z = 0.793`; её рабочая область по столешнице приблизительно `X = −0.64…0.64`, `Y = −0.54…0.54`. Для точки в мировой системе: `X_base = X_world + 0.25`, `Y_base = Y_world`, `Z_base = Z_world − 0.79`.
+
+`patron` **всегда остаётся на роботе**. В начале каждого эксперимента в него не вставлен ни один из четырёх рабочих инструментов. Робот берёт нужный инструмент из стойки и вставляет в `patron`; при возврате извлекает только рабочий инструмент. Отсутствие рабочего инструмента не означает отсутствие `patron`.
+
+| Объект | Центр `(X, Y, Z)` | Ориентация и состояние |
+| --- | --- | --- |
+| Стапель | `(0.30, 0.12, 0.803)` | Вертикальная ось OBJ переведена в Z Webots, затем поворот 45° вокруг Z; закреплён |
+| Корпус | `(0.30, 0.12, 0.823)` | Та же ориентация; закреплён на опорах стапеля |
+| Посадочное место `hatch_seat` | Центр `(0.30, 0.12)` | Поза связана с корпусом; устанавливаемый люк имеет начало в `(0.30, 0.12, 0.82594)` |
+| Свободный люк, стандартная поза | `(0.35, −0.22, 0.790667)` | Поворот 45°; лежит отдельно на столе |
+| Стойка инструментов | `(0.12, −0.42, 0.793)` | Фиксированная стойка с отдельными инструментами в гнёздах |
+| Лоток винтов | `(0.20, 0.46, 0.793)` | Длинная сторона вдоль мировой оси X; нижняя поверхность на столе; у левого края стола, впереди слева от стойки |
+
+Стойка и корпус присутствуют во всех сценах. Свободный люк, уже установленный люк, лоток, винты и рабочие инструменты появляются по таблице ниже. Установленный люк виртуально фиксируется после отпускания присоски. В опытах с крепежом он уже установлен и зафиксирован при старте.
+
+### Гнёзда инструментов
+
+Положения соответствуют четырём гнёздам модели `tool-magizine.obj` при указанной позе стойки. Нужные для опыта инструменты появляются как отдельные объекты, доступные для извлечения и вставки в `patron`.
+
+| Инструмент | Центр гнезда `(X, Y, Z)` |
+| --- | --- |
+| Присоска `vacuum` | `(0.255, −0.42, 0.803)` |
+| Магнитный `magnetic_tool` | `(0.165, −0.42, 0.803)` |
+| Герметик `sealant_tool` | `(0.075, −0.42, 0.803)` |
+| Затяжка `torque_tool` | `(−0.015, −0.42, 0.803)` |
+
+### Десять винтов в лотке
+
+Во **всех** экспериментах с лотком при старте в нём находятся десять одинаковых винтов. Даже если задача требует установить только один, два или четыре, остальные остаются на месте и усложняют поиск и выбор. Винты стоят вертикально; их нижняя точка запланирована на `Z = 0.7954`, у внутреннего дна лотка. Центры соседних винтов разнесены на 20 мм вдоль X и 30 мм между рядами Y. Четыре первых номера размещены по углам сетки, остальные заполняют промежутки.
+
+| Винт | `(X, Y, Z)` | Винт | `(X, Y, Z)` |
+| --- | --- | --- | --- |
+| `screw_1` | `(0.160, 0.445, 0.7954)` | `screw_6` | `(0.200, 0.445, 0.7954)` |
+| `screw_2` | `(0.240, 0.445, 0.7954)` | `screw_7` | `(0.220, 0.445, 0.7954)` |
+| `screw_3` | `(0.160, 0.475, 0.7954)` | `screw_8` | `(0.180, 0.475, 0.7954)` |
+| `screw_4` | `(0.240, 0.475, 0.7954)` | `screw_9` | `(0.200, 0.475, 0.7954)` |
+| `screw_5` | `(0.180, 0.445, 0.7954)` | `screw_10` | `(0.220, 0.475, 0.7954)` |
+
+Все десять винтов взаимозаменяемы. Для установки в четыре отверстия планировщик может выбрать **любые четыре разных доступных винта**. Оценка графа не должна требовать конкретные номера `screw_i`. В опыте 15 отказ задаётся третьей попытке захвата винта, который план выбрал третьим; отказ затяжки задаётся отверстию `hole_4`.
+
+При стандартной позе корпуса предварительные центры отверстий, полученные из геометрии люка, таковы:
+
+| Отверстие | `(X, Y)` | Отверстие | `(X, Y)` |
+| --- | --- | --- | --- |
+| `hole_1` | `(0.2243, 0.1052)` | `hole_3` | `(0.3757, 0.1348)` |
+| `hole_2` | `(0.2852, 0.0443)` | `hole_4` | `(0.3148, 0.1957)` |
+
+Противоположные пары — `hole_1`/`hole_3` и `hole_2`/`hole_4`; крестовой порядок: `hole_1 → hole_3 → hole_2 → hole_4`. Точную ось посадки винта и высоту головки нужно проверить в Webots перед построением контактной траектории.
+
+## Размещение по экспериментам
+
+В таблице «стандартный корпус» означает стапель `(0.30, 0.12, 0.803)` и корпус `(0.30, 0.12, 0.823)` из раздела выше. Указанные инструменты находятся в своих гнёздах; `patron` на роботе во всех строках. Если есть лоток, в нём **всегда десять винтов** в точках из предыдущей таблицы. Герметик до начала опыта не нанесён.
+
+| № | Стапель и корпус | Люк при старте | Лоток и винты | Рабочие инструменты в стойке | Отличие сценария |
+| ---: | --- | --- | --- | --- | --- |
+| 1 | Стандартные | Свободный `(0.35, −0.22, 0.790667)`, 45° | Нет | Присоска | Базовая установка |
+| 2 | Стандартные | Свободный `(0.35, −0.22, 0.790667)`, **135°** | Нет | Присоска | Люк нужно повернуть на 90° при переносе |
+| 3 | Стапель `(0.29, 0.14, 0.803)`, корпус `(0.29, 0.14, 0.823)` | Свободный `(0.33, −0.22, 0.790667)`, 45° | Нет | Присоска | Посадочное место смещено вместе с корпусом; целевой люк `(0.29, 0.14, 0.82594)` |
+| 4 | Стандартные | Стандартный свободный | Нет | Присоска | Первая попытка захвата даст `grasp_failed` |
+| 5 | Стандартные | Стандартный свободный | Нет | Присоска | После первой установки люк окажется в `(0.303, 0.12, 0.82594)` и проверка даст `not_ok` |
+| 6 | Стандартные | Нет | Нет | Герметик | Обрабатывается `side_1` |
+| 7 | Стандартные | Нет | Нет | Герметик | Обрабатываются смежные `side_1`, `side_2` |
+| 8 | Стандартные | Нет | Нет | Герметик | Обрабатывается полный `sealant_path` |
+| 9 | Стандартные | Нет | Нет | Герметик | На `side_3` при выполнении прервётся подача |
+| 10 | Стандартные | Стандартный свободный | Нет | Герметик, присоска | Герметик, затем установка люка |
+| 11 | Стандартные | Уже установлен `(0.30, 0.12, 0.82594)`, 45° | Стандартный лоток, **10 винтов** | Магнитный | Установить один из десяти винтов |
+| 12 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Установить два разных винта |
+| 13 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Установить четыре разных винта |
+| 14 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный, затяжной | Установить и затянуть четыре винта |
+| 15 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный, затяжной | Сбой третьего захвата и первой затяжки `hole_4` |
+| 16 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Участок герметика, люк, один винт |
+| 17 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Полный контур, люк, два винта |
+| 18 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Полная сборка |
+| 19 | Стандартные | Свободный `(0.37, −0.20, 0.790667)`, **60°** | Лоток `(0.18, 0.48, 0.793)`, **10 винтов** | Все четыре | Координаты всех десяти винтов смещены вместе с лотком на `(−0.02, +0.02)` |
+| 20 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Исходная геометрия обычная; сбои вводятся во время выполнения |
+
+В опыте 3 смещается вся пара «стапель + корпус» без изменения взаимной высоты и поворота. Координаты `hatch_seat` и четырёх отверстий сдвигаются вместе с ними на `(−0.01, +0.02)`. В опыте 19 сдвигается лоток **со всеми десятью винтами**; ни один винт не остаётся в старой точке.
+
+Опыты 4, 5, 9, 15 и 20 требуют также управляемых событий исполнителя. Эти события и последующие изменения положения детали не являются дополнительными объектами начальной сцены. Герметик удобнее учитывать как состояние именованных участков контура, а не как заранее лежащую на корпусе модель.
+
+## Геометрические и технические ограничения
+
+По габаритам OBJ стапель занимает в плане примерно `X = 0.0985…0.5015`, `Y = −0.0815…0.3215`; стандартный свободный люк — `X = 0.2663…0.4337`, `Y = −0.3037…−0.1363`; стойка — `X = −0.09…0.33`, `Y = −0.49…−0.35`; лоток — `X = 0.14…0.26`, `Y = 0.425…0.495`. Эти прямоугольники не пересекаются. Лоток перенесён к левому краю стола со стороны `+Y`, чтобы все винты были в достижимой зоне; зазор от лотка до стапеля по Y около 104 мм. Зазор от свободного люка до стойки по Y около 46 мм, до стапеля около 55 мм. Это проверка начальной компоновки, не проверка траекторий инструмента.
+
+Перед реализацией контактных действий нужно проверить в Webots достижимость TCP и коллизии при подъёме люка, при извлечении инструмента из гнезда и при работе между десятью винтами. В `iiwa.wbt` трение `TableMaterial` равно нулю; лоток и винты понадобится удерживать до захвата или задать подходящую контактную модель. Действующий спавнер `object_spawner.py` создаёт случайные OBJ, поэтому для этой таблицы потребуется детерминированная загрузка по номеру эксперимента. Стойка должна содержать отдельно управляемые рабочие инструменты; удалённый `GrantToolRack.proto` для нового мира не используется.

docs/superpowers/specs/2026-11-10-llm-graph-execution-design.md

@@ -0,0 +1,53 @@
+# Исполнение графов Qwen3 в Webots: согласованная архитектура
+
+## Цель и границы
+
+Новая система получает текущую сцену Webots, выдаёт `scene_json` для внешнего вызова Qwen3, принимает готовый JSON-граф модели и выполняет только его. Номер одного из 20 опытов задаёт **исходный набор объектов**, но не диктует цель или эталонную последовательность. Например, в сцене опыта 20 допустим отдельный запрос «возьми присоску, переместись в рабочую позицию, верни присоску»: исполнитель не должен самостоятельно наносить герметик или переносить люк. Вызов Qwen3, оценивание качества модели и инфраструктура сравнения с эталоном входят в последующий проект, а не в этот.
+
+Создать два ROS 2 пакета: `iiwa_scene_context` (снимок сцены и `scene_json`) и `iiwa_llm_plan_executor` (проверка и исполнение графа). Минимальные изменения существующих `iiwa_experement` и `iiwa_web` допустимы для запуска мира и для API/MCP. Реальный робот пока не подключать. Будущая замена источника сцены и физических навыков должна быть возможна через адаптеры, без переписывания валидатора графа.
+
+## Что обнаружено в проекте
+
+`iiwa_experement` уже создаёт сцены, предоставляет `/iiwa_experement/scene_ready`, состояние объектов `/iiwa_experement/object_state`, команды захвата `/iiwa_experement/object_command`, измеренную геометрию, мост сцены MoveIt и эталонные операции. В `mode=view` supervisor работает только на чтение; `mode=execute` сразу запускает эталон. Для внешнего графа требуется третий `mode=planner`: supervisor с разрешёнными действиями, без эталонного runner, без TCP gizmo. Старые два режима сохраняют прежнюю семантику.
+
+`iiwa_web` уже публикует FastAPI и получает MCP через `FastMCP.from_fastapi`. Для новых функций следует добавить тонкие маршруты в этот сервер, а не запускать второй HTTP/MCP сервер.
+
+Приложенные `generated.train.jsonl` (4499 строк) и `generated.validation.jsonl` (500 строк) содержат общий процесс переноса в хранилище, без Grant-объектов и инструментов. Они используют в ответах `pick`, `move_to_storage`, `inspect`. Текущий `planner_dataset/assets/response-format.json` описывает другой список навыков, а показанный пользователем граф добавляет `align`. Поэтому исполнитель не должен слепо импортировать какую-либо одну из этих схем; нужна собственная версия контракта, совместимая с опубликованными `available_skills` и с оговорёнными алиасами.
+
+## Подход к исполнению
+
+Выбран **граф + реестр навыков**. Движения, неизбежные внутри одного навыка, задаются его проверяемым контрактом: например, `grab` может сам подойти сверху, опуститься, захватить и подняться до сохранённой высоты, если явных узлов `approach`/`lower`/`lift` не было. Если они уже выполнялись, повторного движения нет. Это геометрические фазы **того же** действия, а не добавление нового технологического задания.
+
+Исполнитель не придумывает следующую деталь, нужный инструмент или цель из номера эксперимента. Если `grab(cap)` требует вакуумный инструмент, а в `patron` его нет, граф не отклоняется на входе: при исполнении узла проверяется живое состояние supervisor, захват не начинается, контакт не выполняется, и узел возвращает `grasp_failed` с причиной `required tool vacuum is not mounted`; далее исполняется ровно тот переход `grasp_failed`, который указала модель. Если в графе нет явного узла `grab(vacuum)`, инструмент автоматически не берётся. `release` инструмента **не возвращает его автоматически**. Для возврата граф должен явно содержать перемещение к гнезду `vacuum_socket` и затем `release(vacuum, vacuum_socket)` либо `dock_tool(vacuum, vacuum_socket)`; оба последних навыка только выполняют стыковку и отделение в уже достигнутом гнезде. `release(vacuum)` из `work` без подходящего места для инструмента отклоняется, присоска остаётся в `patron`. Если граф заканчивается в `work` с присоской в `patron`, она остаётся там до следующего явного задания. Новый процесс может подключить другой адаптер и свои контракты; неизвестный навык или объект не исполняется.
+
+Рассмотренный, но отвергнутый подход — переписывать граф модели и автоматически вставлять недостающие технологические действия. Это могло бы выполнить лишние операции при произвольных запросах. Второй отвергнутый подход — напрямую переводить ID навыка в фиксированную траекторию без проверки текущей позы и состояния; он не учитывает смещения объектов и ветви событий.
+
+## Снимок сцены и интерфейсы
+
+`iiwa_scene_context` собирает **текущий** состав и логическое состояние из `ScenarioSpec` и живого supervisor, а не из эталонного маршрута. Внешний `scene_json` сохраняет поля, ожидаемые Qwen3: `domain`, `components`, `destinations`, `tools`, `initial_state`, `cameras`, `available_skills`, `skill_contracts`, `service_objects`, `recovery_attempts_per_component`. ID соответствуют Webots: `cap`, `vacuum`, `vacuum_socket`, `screw_1`…`screw_10`, `hatch_seat`, `hole_1`…`hole_4`, `work` и т. д. Гнездо каждого присутствующего инструмента публикуется как отдельная именованная цель. Список зависит от реально присутствующих объектов. Абсолютные координаты в модельный `scene_json` не передаются: навык `detect` получает актуальную позу при исполнении.
+
+Транспортный конверт содержит `experiment_id`, `scene_revision`, время снимка, версию состояния supervisor и сам объект `scene_json`. `scene_revision` меняется при дискретном изменении сцены или владения объектом, но не на каждом кадре движения удерживаемой детали. Версия графа передаётся отдельно; исполнитель отвергает граф, построенный по устаревшей сцене. Снимок доступен как ROS 2 сервис `/iiwa_scene_context/get_scene` (ответ с JSON), ROS 2 топик `/iiwa_scene_context/scene_json` с transient-local QoS, HTTP `GET /planner/scene` и одноимённый инструмент MCP через существующий `iiwa_web`. Контракты навыков берутся из серверного реестра, а не из текста, который прислала модель.
+
+## Приём и переходы графа
+
+`iiwa_llm_plan_executor` получает готовый ответ Qwen3, `scene_revision` и `run_id` через ROS 2 сервис `/iiwa_llm_plan_executor/submit`; HTTP `POST /planner/runs` и MCP передают то же через адаптер `iiwa_web`. Модель в этой версии вызывается **вне** проекта. После успешной проверки исполнитель публикует принятый граф без изменений в ROS 2 топик `/iiwa_llm_plan_executor/plan` (`std_msgs/String`, JSON-конверт с `run_id`, `scene_revision` и исходным `response`, QoS transient-local, глубина 1), прежде чем выполнять первый узел. Отклонённый граф в этом топике не появляется; причина отказа доступна в ответе приёма и статусе. Выполнение асинхронно, не более одного активного графа на робота. Отдельный топик `/iiwa_llm_plan_executor/status` сообщает текущий узел, полученное событие, выбранное ребро и итоговый терминал; HTTP и MCP дают те же сведения. Отмена останавливает движение и сохраняет состояние захвата. Для запроса статуса **по `run_id`** добавить сервис `GetRunStatus.srv` в существующий `iiwa_msgs` (`string run_id` в запросе, `bool found` и `string status_json` в ответе); стандартный `Trigger` не имеет поля запроса.
+
+Предзапусковая проверка: строгий JSON без лишних исполняемых полей; `status=blocked` не запускает робот; уникальные ID, известный `entry`, только разрешённые навыки/объекты/цели/события/терминалы, все ссылки существуют, достижимость узлов, предел размера/числа шагов, безопасные ветви для отказа и столкновения, допустимые условия владения инструментом и деталью. Каждый результат навыка выбирает **ровно одно** ребро графа. Терминалы: `completed`, `replan_required`, `operator_required`. На `replan_required` система останавливается и отдаёт свежий `scene_json`; повторный вызов Qwen3 делает внешний клиент. `completed` означает выполнение переданного графа и его зарегистрированных постусловий, а не доказательство, что модель правильно поняла произвольный русский запрос.
+
+## Семантика движения и наблюдения
+
+При `detect` робот сначала отходит от контакта по оси к сохранённой высоте, затем переходит в `work`. Ждёт свежий кадр `/d455_top/image_raw`, берёт точную позу из supervisor и возвращает `found`/`not_found`/`failed`. Для объекта, требующего близкого контроля, конфигурация навыка задаёт безопасную точку обзора и повторный кадр с `/camera/color` или `/camera/depth`; поза всё ещё берётся из supervisor. Задача компьютерного зрения по RGB/D в этот этап не входит.
+
+`approach` ставит рабочий кончик над объектом и сохраняет позу/высоту подхода. `lower` движется вдоль оси к контакту. `grab` при необходимости выполняет недостающие фазы подхода и опускания, подтверждает захват через supervisor и поднимается **к сохранённой позе подхода**. Явный `lift` после этого подтверждает достигнутую высоту и не дублирует подъём. `transfer` идёт к указанной цели на безопасной высоте, `align` меняет ориентацию только на высоте. `release` опускается в указанном безопасном месте установки/передачи, отпускает и поднимается к сохранённой высоте; для инструмента разрешено только его именованное гнездо. `dock_tool` выполняет ту же локальную стыковку инструмента в гнезде без скрытого переноса к стойке. `retreat` не выполняет горизонтальное перемещение до осевого отвода. Для винта отпускание у отверстия означает передачу виртуальному человеку, без вкручивания.
+
+Рабочий кончик направлен вниз. Предпочтительная ось 7 — значение в `work` (сейчас 0 рад); начальный жёсткий коридор ±90° и максимальный наклон рабочего инструмента 10° задаются в конфигурации и уточняются проверкой достижимости. Перед каждым планом используются текущие суставы как IK seed, ограничения MoveIt на `joint7` и ориентацию, а перед исполнением — проверка каждой точки траектории, столкновений и скачков суставов. При недостижимости в этом коридоре исполнитель сообщает отказ; не выбирает ветвь IK с поворотом примерно на −180°. Поворот кисти или возврат в `work` допустим только после осевого подъёма. На каждом переходе между узлами проверяется свежесть состояния сцены и робота.
+
+Для этого `iiwa_llm_plan_executor` нужен собственный адаптер планирования на установленном MoveItPy: текущий `MoveToPose.action` не передаёт ограничение `joint7`. Он повторно использует геометрию и обновление сцены из `iiwa_experement`, но не запускает эталонную последовательность. Физический слой остаётся сменным для будущей адаптации к реальному роботу.
+
+## Поддерживаемые навыки первого этапа
+
+Реестр поддерживает имена из показанного графа и необходимые действия с текущими объектами Grant: `detect`, `approach`, `lower`, `grab`, `lift`, `transfer`, `align`, `release`, `retreat`, `capture`, `verify`, `stop`, `wait_scene_clear`, `inspect_state`, `notify_operator`, `dispense`, `dock_tool`, `handoff_screw`. Для графов из обучающего набора описать и проверить совместимые алиасы `pick`, `move_to_storage`, `inspect`; их запуск разрешать лишь при поддерживаемом типе объекта и места назначения в текущей сцене. Любой другой навык (включая `tighten`, если у отверстия нет установленного винта) получает отказ до движения. Каталог можно расширять новым обработчиком и контрактом, без изменения движка графа.
+
+## Проверки и результат этапа
+
+Пакетные тесты проверяют формат снимка, контракт навыков, валидацию графа и переходы, физические фазы на поддельном backend, ограничения суставов и API/MCP адаптеры. Минимальные пробы Webots должны подтвердить произвольные короткие запросы в одной и той же сцене: взять и вернуть присоску; обнаружить люк; взять люк и установить; выбрать один из десяти винтов и передать у указанного отверстия; остановка и перепланирование после отказа. Это проверка работоспособности двух пакетов, **не** система оценивания Qwen3 и не сравнение 20 экспериментов с эталоном.

output/md/experements.md

@@ -0,0 +1,225 @@
+# Эксперименты для LLM-планировщика сборки люка
+
+Цель набора — проверить, как LLM-планировщик преобразует пользовательский запрос и `scene_json` в граф действий `skill` с переходами по событиям. В наборе 20 экспериментов: четыре категории по пять заданий. Сложность внутри каждой категории возрастает.
+
+Эксперименты опираются на технологическую карту «ТехнКарта вер. 1.0.xlsx» и модели из `src/iiwa_description/objects/grant`. В сцене работает один робот-манипулятор. Корпус закреплён в стапеле; оператора, мобильной платформы и вспомогательного робота нет. Отверстия для четырёх винтов считаются подготовленными, поскольку модели сверлильного шпинделя среди доступных объектов нет. Во всех экспериментах на роботе постоянно установлен `patron`: рабочие инструменты вставляются в него. После установки люк удерживается виртуальной фиксацией: это позволяет роботу отпустить присоску и сменить рабочий инструмент. Начальные положения объектов описаны отдельно в [experiment-scenes.md](experiment-scenes.md).
+
+## Контракт для экспериментов
+
+В исходном примере доступны `detect`, `approach`, `lower`, `grab`, `lift`, `transfer`, `align`, `release`, `retreat`, `capture`, `verify`, `stop`, `wait_scene_clear`, `inspect_state`, `notify_operator`. План должен вставлять присоску из стойки в постоянный `patron`, если начальное состояние содержит `tool_docked`; перед `grab` требуется контактное `lower`, а перед отпусканием люка в посадочном месте — его опускание до контакта. После отпускания нужен безопасный отвод и возврат. `dock_tool` извлекает рабочий инструмент из `patron`; сам `patron` остаётся на роботе.
+
+Для остальных операций соответствующий `scene_json` расширяется следующими навыками:
+
+| Навык | Действие | Основные исходы |
+| --- | --- | --- |
+| `dispense` | Пройти соплом именованный участок контура с условной подачей герметика | `completed`, `interrupted`, `failed` |
+| `press` | Прижать установленный люк в заданной зоне | `success`, `failed` |
+| `seat_screw` | Подтвердить, что винт после осевого опускания остался в отверстии | `seated`, `not_seated`, `failed` |
+| `tighten` | Затянуть конкретный винт до момента, заданного в сцене | `success`, `torque_not_reached`, `failed` |
+| `dock_tool` | Вернуть инструмент в его гнездо и отсоединить от робота | `success`, `failed` |
+
+Контракты `verify` и `inspect_state` нужно расширить на контур герметика и винты. Для тестов достаточно состояния симулятора; `capture` не входит в основные маршруты. Предлагаемые идентификаторы: `cap`, `hatch_seat`, `body`, `sealant_path`, `side_1`…`side_4`, `hole_1`…`hole_4`, `screw_1`…`screw_10`, `screw_tray`, `patron`, `vacuum`, `sealant_tool`, `magnetic_tool`, `torque_tool`.
+
+Каждый эксперимент начинается в домашней позиции. К объекту робот идёт через безопасную точку над ним; после контакта сначала отходит по оси инструмента. В конце он возвращает инструмент в стойку и идёт домой. Ветви движения при `collision` должны вести к остановке; при неопределённом состоянии нельзя продолжать перенос или отпускание детали. Число повторных попыток задаёт `recovery_attempts_per_component`.
+
+## I. Захват и установка люка
+
+### 1. Базовая установка
+
+**Запрос модели:** «Возьми люк присоской, перенеси на корпус, установи, отпусти и проверь итоговое положение».
+
+**Условия:** люк лежит в известной области; присоска в стойке; посадочное место свободно.
+
+**Действия робота:** найти присоску → подойти и опуститься к гнезду → захватить инструмент → подняться → найти люк → подойти над ним и опуститься до контакта → включить захват → поднять люк → найти посадочное место → перенести люк над ним → совместить → опустить до посадки → отпустить → отвести присоску → проверить положение люка → вернуть присоску в стойку → домой.
+
+**Проверка плана:** работа с деталью не начинается раньше подтверждённого захвата инструмента; `verify` следует за `release`.
+
+### 2. Люк с изменённой исходной ориентацией
+
+**Запрос:** «Установи люк на корпус, даже если он лежит повёрнутым относительно посадочного места».
+
+**Условия:** положение люка обнаруживается во время выполнения; его начальный угол отличается от целевого.
+
+**Действия робота:** взять присоску → обнаружить фактическую позу люка → подойти и захватить его → поднять выше стапеля → перенести в безопасную точку над корпусом → повернуть и совместить с `hatch_seat` через `align` → опустить → отпустить → отвести инструмент → проверить установку → вернуть инструмент → домой.
+
+**Проверка плана:** план использует результат `detect`, а не предполагает исходную ориентацию детали из текста запроса.
+
+### 3. Люк и посадочное место обнаруживаются отдельно
+
+**Запрос:** «Найди люк и его посадочное место, затем установи люк и проверь результат».
+
+**Условия:** обе позы уточняются при выполнении.
+
+**Действия робота:** взять присоску → `detect(cap)` → подойти, опуститься, захватить и поднять люк → `detect(hatch_seat)` → перенести над найденной точкой → совместить → опустить → отпустить → отвести инструмент → проверить → вернуть инструмент → домой.
+
+**Ветви и проверка:** `not_found` у любого обнаружения ведёт к `replan_required`; перенос к посадочному месту не начинается, пока оно не найдено.
+
+### 4. Неудачный первый захват
+
+**Запрос:** «Установи люк; если присоска не удержала его с первого раза, безопасно повтори попытку».
+
+**Условия:** исполнитель один раз возвращает `grasp_failed` при захвате люка; `recovery_attempts_per_component = 1`.
+
+**Действия робота:** взять присоску → найти люк → подойти и опуститься → попытаться захватить. При `grasp_failed`: остановиться → проверить, пуст ли захват и где находится люк → заново обнаружить люк → повторить подход, опускание и захват один раз → поднять, перенести, совместить, установить, отпустить и проверить → вернуть инструмент → домой.
+
+**Проверка плана:** после `grasp_failed` робот не выполняет `lift` вслепую; второй отказ заканчивается остановкой и уведомлением.
+
+### 5. Исправление положения после установки
+
+**Запрос:** «Установи люк и проверь положение. Если он оказался вне допуска, поправь его одной повторной установкой».
+
+**Условия:** первая посадка даёт `verify:not_ok`; люк остаётся доступным для повторного захвата.
+
+**Действия робота:** выполнить установку как в эксперименте 1 → получить `not_ok` → обнаружить фактическое положение люка → подойти присоской, опуститься, снова захватить и поднять → повторно обнаружить посадочное место → перенести, совместить, опустить и отпустить → повторить `verify` → вернуть инструмент → домой при `ok`.
+
+**Проверка плана:** не более одной повторной установки; при неизвестном состоянии детали или второй неуспешной проверке граф заканчивается `operator_required`.
+
+## II. Работа с герметиком
+
+Для этих экспериментов `sealant_path` разбит на именованные участки `side_1`…`side_4`. Герметик условный: проверяются маршрут робота и изменение состояния контура в симуляторе.
+
+### 6. Один прямой участок
+
+**Запрос:** «Нанеси герметик на первый прямой участок посадочного контура корпуса».
+
+**Условия:** инструмент в стойке; корпус закреплён.
+
+**Действия робота:** найти инструмент → подойти, опуститься и взять его → подняться → найти `side_1` → подойти над началом участка → опустить сопло → выполнить `dispense(side_1)` → отвести сопло вверх → вернуть инструмент → домой → проверить, что обработан именно `side_1`.
+
+**Проверка плана:** нет подачи при перемещении от стойки к корпусу.
+
+### 7. Два смежных участка
+
+**Запрос:** «Нанеси непрерывную линию герметика на первую и вторую стороны посадочного контура».
+
+**Условия:** стороны соединяются одним углом.
+
+**Действия робота:** взять инструмент → обнаружить контур → подойти к началу `side_1` → опустить сопло → пройти `side_1` → перейти через угол без отвода от рабочей высоты → пройти `side_2` → остановить подачу → поднять инструмент → вернуть в стойку → домой → проверить оба участка.
+
+**Проверка плана:** план сохраняет порядок смежных участков и не поднимает сопло посреди непрерывной линии.
+
+### 8. Замкнутый контур
+
+**Запрос:** «Нанеси герметик по всему замкнутому контуру посадочного места люка».
+
+**Условия:** четыре стороны доступны инструменту.
+
+**Действия робота:** взять инструмент → найти контур → подойти к выбранному началу → опустить сопло → последовательно выполнить `dispense` для четырёх сторон и углов → завершить контур в точке начала → поднять сопло → вернуть инструмент → домой → проверить полноту контура.
+
+**Проверка плана:** перечислены все четыре участка ровно по одному разу; конечная проверка относится ко всему контуру.
+
+### 9. Прерывание подачи
+
+**Запрос:** «Нанеси герметик по контуру; если подача прервётся, возобнови её с места разрыва и закончи контур».
+
+**Условия:** на третьем участке один раз приходит `dispense:interrupted`.
+
+**Действия робота:** взять инструмент → обработать первые два участка → начать третий → при прерывании остановить действие и отвести сопло → проверить состояние обработанных участков и последнюю подтверждённую точку → снова подойти к началу недостающего отрезка → опустить сопло → закончить третий и четвёртый участки → проверить контур → вернуть инструмент → домой.
+
+**Проверка плана:** повторно не проходится весь контур; неизвестная точка разрыва ведёт к `replan_required`.
+
+### 10. Герметик и установка люка
+
+**Запрос:** «Нанеси герметик по всему посадочному контуру, затем возьми люк присоской и установи его на корпус».
+
+**Условия:** люк лежит отдельно; для его установки после отпускания предусмотрена виртуальная фиксация.
+
+**Действия робота:** взять инструмент герметика → обработать четыре участка → проверить контур → вернуть инструмент → взять присоску → найти и захватить люк → поднять и перенести его над корпусом → совместить с посадочным местом → опустить и прижать → отпустить → проверить положение люка → вернуть присоску → домой.
+
+**Проверка плана:** присоска не берётся, пока инструмент герметика не возвращён; люк не опускается на неполный контур.
+
+## III. Подача винтов и затяжка
+
+В этих сценах люк уже установлен и зафиксирован, а четыре отверстия подготовлены. В лотке всегда находятся десять одинаковых винтов. Для задания можно выбрать любой доступный винт; проверка плана должна требовать нужное число **разных** винтов, а не заранее заданные номера. Магнитный инструмент переносит винт; `seat_screw` подтверждает, что винт остаётся в отверстии после отвода магнита.
+
+### 11. Один винт
+
+**Запрос:** «Возьми винт из лотка и установи его в первое отверстие люка».
+
+**Действия робота:** взять магнитный инструмент → найти один доступный винт в лотке → подойти и опуститься к нему → захватить и поднять → найти `hole_1` → перенести винт над отверстием → совместить оси → опустить → выполнить `seat_screw` → отпустить винт → отвести инструмент → проверить наличие винта в отверстии → вернуть инструмент → домой.
+
+**Проверка плана:** `release` невозможен до подтверждённой посадки винта.
+
+### 12. Два винта по диагонали
+
+**Запрос:** «Установи два винта в диагонально противоположные отверстия люка».
+
+**Действия робота:** взять магнитный инструмент → установить первый винт по цепочке «найти → захватить → поднять → перенести → совместить → опустить → посадить → отпустить» → подняться над люком → вернуться к лотку → повторить для диагонально противоположного отверстия → проверить обе точки → вернуть инструмент → домой.
+
+**Проверка плана:** второму винту соответствуют другая ячейка лотка и другое отверстие.
+
+### 13. Четыре винта крестовым порядком
+
+**Запрос:** «Установи четыре винта в отверстия люка крестовым порядком».
+
+**Действия робота:** взять магнитный инструмент → последовательно взять винты из четырёх ячеек и установить их в порядке `hole_1 → hole_3 → hole_2 → hole_4`; перед каждым переносом подтвердить захват, перед отпусканием — посадку → проверить наличие четырёх винтов → вернуть инструмент → домой.
+
+**Проверка плана:** нет повторного использования ячейки или отверстия; после каждой установки робот возвращается к лотку с пустым магнитным захватом.
+
+### 14. Установка и затяжка четырёх винтов
+
+**Запрос:** «Установи четыре винта крестовым порядком, затем затяни каждый до заданного момента».
+
+**Действия робота:** выполнить четыре установки как в эксперименте 13 → вернуть магнитный инструмент → взять инструмент затяжки → для каждого из четырёх винтов подойти сверху, совместить инструмент с осью, опуститься, выполнить `tighten`, отвести инструмент → проверить четыре результата затяжки → вернуть инструмент → домой.
+
+**Проверка плана:** `tighten` не вызывается для винта без подтверждённого `seat_screw`.
+
+### 15. Ошибка захвата и недостигнутый момент
+
+**Запрос:** «Установи и затяни четыре винта. Если один винт не захватится или затяжка не достигнет нужного момента, повтори соответствующее действие один раз».
+
+**Условия:** первый захват третьего выбранного винта даёт `grasp_failed`; первая затяжка в `hole_4` даёт `torque_not_reached`.
+
+**Действия робота:** установить первые два винта → на третьем остановиться, проверить состояние захвата и повторно обнаружить винт → повторить его захват один раз → установить третий и четвёртый → сменить магнитный инструмент на затяжной → затянуть четыре винта; для четвёртого после проверки состояния повторить затяжку один раз → проверить все четыре точки → вернуть инструмент → домой.
+
+**Проверка плана:** ошибки обрабатываются у соответствующего винта; уже установленные винты не вынимаются и не устанавливаются повторно.
+
+## IV. Полная сборка и перепланирование
+
+### 16. Герметик, люк и один винт
+
+**Запрос:** «Нанеси герметик на заданный участок, установи люк и закрепи его одним винтом».
+
+**Действия робота:** взять инструмент герметика → обработать участок → вернуть инструмент → взять присоску → захватить, перенести, совместить, опустить и отпустить люк → проверить его положение → вернуть присоску → взять магнитный инструмент → установить один винт → вернуть магнит → взять инструмент затяжки → затянуть винт → вернуть инструмент → домой.
+
+**Проверка плана:** зависимости «герметик → люк → винт → затяжка» соблюдены.
+
+### 17. Полный контур и два крепления
+
+**Запрос:** «Нанеси полный контур герметика, установи люк и закрепи его двумя диагональными винтами».
+
+**Действия робота:** пройти четыре участка герметика и проверить контур → вернуть инструмент → присоской установить и проверить люк → вернуть присоску → магнитным инструментом установить два диагональных винта → вернуть магнит → затянуть оба винта → проверить две затяжки → вернуть инструмент → домой.
+
+**Проверка плана:** если контур не подтверждён, план не переходит к установке люка.
+
+### 18. Полная сборка
+
+**Запрос:** «Нанеси герметик по контуру, установи люк, поставь четыре винта крестовым порядком и затяни их».
+
+**Действия робота:** полный контур герметика → проверка → смена на присоску → захват и установка люка → проверка → смена на магнит → четыре установки винтов → смена на инструмент затяжки → четыре затяжки → итоговая проверка люка и крепежа → вернуть инструмент → домой.
+
+**Проверка плана:** у каждого из четырёх винтов есть собственные узлы установки и затяжки; итоговая проверка охватывает всю сборку.
+
+### 19. Полная сборка при смещённых деталях
+
+**Запрос:** «Собери узел полностью. Люк и лоток могут лежать не в номинальных местах — используй их фактическое положение».
+
+**Условия:** симулятор смещает люк и лоток в допустимой рабочей области; `detect` возвращает их актуальные позы.
+
+**Действия робота:** нанести и проверить герметик → обнаружить фактическую позу люка, захватить и установить его → проверить → обнаружить фактическую позу лотка и по очереди найти четыре винта → установить их → затянуть → проверить сборку → вернуть инструмент → домой.
+
+**Проверка плана:** план не подставляет прежние координаты люка и винтов после нового `detect`; зависимости операций сохраняются.
+
+### 20. Полная сборка с несколькими сбоями
+
+**Запрос:** «Выполни полную сборку. При прерывании подачи герметика, неудачном захвате винта или ошибке установки люка восстанови работу, если состояние известно и разрешена одна повторная попытка».
+
+**Условия:** один раз прерывается `dispense`, первый захват одного винта неудачен, первая проверка положения люка возвращает `not_ok`.
+
+**Действия робота:** начать контур → определить необработанный отрезок и завершить его → проверить контур → установить люк → при `not_ok` заново обнаружить и один раз переставить его → проверить → устанавливать винты; при `grasp_failed` остановиться, проверить состояние и один раз повторить захват нужного винта → затянуть четыре винта → проверить четыре установки, четыре затяжки и положение люка → вернуть инструмент → домой.
+
+**Проверка плана:** восстановление начинается от подтверждённого состояния; лимит попыток соблюдён; любое `unknown` или повторный отказ ведёт к остановке и уведомлению.
+
+## Общие критерии оценки графа
+
+Для каждого ответа планировщика следует проверить существование всех узлов, указанных в `next`; соблюдение зависимостей операций; отсутствие физически невозможных переходов, например `transfer` без подтверждённого захвата; корректное завершение аварийных ветвей; соблюдение лимита повторных попыток. Семантическая проверка графа полезнее посимвольного сравнения с одним образцом JSON.

output/md/experiment-scenes.md

@@ -0,0 +1,91 @@
+# Начальная сцена для каждого эксперимента
+
+Этот документ задаёт положения объектов для 20 экспериментов из [experements.md](experements.md). Единицы измерения — метры; координаты даны в мировой системе Webots. Это проект начальной компоновки для будущего `src/iiwa_description/worlds/grant/iiwa-grant.wbt`, а не описание уже созданного мира.
+
+## Основа мира и постоянные элементы
+
+Базовый мир — `src/iiwa_description/worlds/iiwa.wbt`. В нём есть стол, ограждения и освещение. Робот появляется через запуск в точке `(-0.25, 0, 0.79)`, с нулевым поворотом вокруг Z. Верх столешницы находится на `Z = 0.793`; её рабочая область по столешнице приблизительно `X = −0.64…0.64`, `Y = −0.54…0.54`. Для точки в мировой системе: `X_base = X_world + 0.25`, `Y_base = Y_world`, `Z_base = Z_world − 0.79`.
+
+`patron` **всегда остаётся на роботе**. В начале каждого эксперимента в него не вставлен ни один из четырёх рабочих инструментов. Робот берёт нужный инструмент из стойки и вставляет в `patron`; при возврате извлекает только рабочий инструмент. Отсутствие рабочего инструмента не означает отсутствие `patron`.
+
+| Объект | Центр `(X, Y, Z)` | Ориентация и состояние |
+| --- | --- | --- |
+| Стапель | `(0.30, 0.12, 0.803)` | Вертикальная ось OBJ переведена в Z Webots, затем поворот 45° вокруг Z; закреплён |
+| Корпус | `(0.30, 0.12, 0.823)` | Та же ориентация; закреплён на опорах стапеля |
+| Посадочное место `hatch_seat` | Центр `(0.30, 0.12)` | Поза связана с корпусом; устанавливаемый люк имеет начало в `(0.30, 0.12, 0.82594)` |
+| Свободный люк, стандартная поза | `(0.35, −0.22, 0.790667)` | Поворот 45°; лежит отдельно на столе |
+| Стойка инструментов | `(0.12, −0.42, 0.793)` | Фиксированная стойка с отдельными инструментами в гнёздах |
+| Лоток винтов | `(−0.08, 0.34, 0.793)` | Длинная сторона вдоль мировой оси X; нижняя поверхность на столе |
+
+Стойка и корпус присутствуют во всех сценах. Свободный люк, уже установленный люк, лоток, винты и рабочие инструменты появляются по таблице ниже. Установленный люк виртуально фиксируется после отпускания присоски. В опытах с крепежом он уже установлен и зафиксирован при старте.
+
+### Гнёзда инструментов
+
+Положения соответствуют четырём гнёздам модели `tool-magizine.obj` при указанной позе стойки. Нужные для опыта инструменты появляются как отдельные объекты, доступные для извлечения и вставки в `patron`.
+
+| Инструмент | Центр гнезда `(X, Y, Z)` |
+| --- | --- |
+| Присоска `vacuum` | `(0.255, −0.42, 0.803)` |
+| Магнитный `magnetic_tool` | `(0.165, −0.42, 0.803)` |
+| Герметик `sealant_tool` | `(0.075, −0.42, 0.803)` |
+| Затяжка `torque_tool` | `(−0.015, −0.42, 0.803)` |
+
+### Десять винтов в лотке
+
+Во **всех** экспериментах с лотком при старте в нём находятся десять одинаковых винтов. Даже если задача требует установить только один, два или четыре, остальные остаются на месте и усложняют поиск и выбор. Винты стоят вертикально; их нижняя точка запланирована на `Z = 0.7954`, у внутреннего дна лотка. Центры соседних винтов разнесены на 20 мм вдоль X и 30 мм между рядами Y. Четыре первых номера размещены по углам сетки, остальные заполняют промежутки.
+
+| Винт | `(X, Y, Z)` | Винт | `(X, Y, Z)` |
+| --- | --- | --- | --- |
+| `screw_1` | `(−0.120, 0.325, 0.7954)` | `screw_6` | `(−0.080, 0.325, 0.7954)` |
+| `screw_2` | `(−0.040, 0.325, 0.7954)` | `screw_7` | `(−0.060, 0.325, 0.7954)` |
+| `screw_3` | `(−0.120, 0.355, 0.7954)` | `screw_8` | `(−0.100, 0.355, 0.7954)` |
+| `screw_4` | `(−0.040, 0.355, 0.7954)` | `screw_9` | `(−0.080, 0.355, 0.7954)` |
+| `screw_5` | `(−0.100, 0.325, 0.7954)` | `screw_10` | `(−0.060, 0.355, 0.7954)` |
+
+Все десять винтов взаимозаменяемы. Для установки в четыре отверстия планировщик может выбрать **любые четыре разных доступных винта**. Оценка графа не должна требовать конкретные номера `screw_i`. В опыте 15 отказ задаётся третьей попытке захвата винта, который план выбрал третьим; отказ затяжки задаётся отверстию `hole_4`.
+
+При стандартной позе корпуса предварительные центры отверстий, полученные из геометрии люка, таковы:
+
+| Отверстие | `(X, Y)` | Отверстие | `(X, Y)` |
+| --- | --- | --- | --- |
+| `hole_1` | `(0.2243, 0.1052)` | `hole_3` | `(0.3757, 0.1348)` |
+| `hole_2` | `(0.2852, 0.0443)` | `hole_4` | `(0.3148, 0.1957)` |
+
+Противоположные пары — `hole_1`/`hole_3` и `hole_2`/`hole_4`; крестовой порядок: `hole_1 → hole_3 → hole_2 → hole_4`. Точную ось посадки винта и высоту головки нужно проверить в Webots перед построением контактной траектории.
+
+## Размещение по экспериментам
+
+В таблице «стандартный корпус» означает стапель `(0.30, 0.12, 0.803)` и корпус `(0.30, 0.12, 0.823)` из раздела выше. Указанные инструменты находятся в своих гнёздах; `patron` на роботе во всех строках. Если есть лоток, в нём **всегда десять винтов** в точках из предыдущей таблицы. Герметик до начала опыта не нанесён.
+
+| № | Стапель и корпус | Люк при старте | Лоток и винты | Рабочие инструменты в стойке | Отличие сценария |
+| ---: | --- | --- | --- | --- | --- |
+| 1 | Стандартные | Свободный `(0.35, −0.22, 0.790667)`, 45° | Нет | Присоска | Базовая установка |
+| 2 | Стандартные | Свободный `(0.35, −0.22, 0.790667)`, **135°** | Нет | Присоска | Люк нужно повернуть на 90° при переносе |
+| 3 | Стапель `(0.29, 0.14, 0.803)`, корпус `(0.29, 0.14, 0.823)` | Свободный `(0.33, −0.22, 0.790667)`, 45° | Нет | Присоска | Посадочное место смещено вместе с корпусом; целевой люк `(0.29, 0.14, 0.82594)` |
+| 4 | Стандартные | Стандартный свободный | Нет | Присоска | Первая попытка захвата даст `grasp_failed` |
+| 5 | Стандартные | Стандартный свободный | Нет | Присоска | После первой установки люк окажется в `(0.303, 0.12, 0.82594)` и проверка даст `not_ok` |
+| 6 | Стандартные | Нет | Нет | Герметик | Обрабатывается `side_1` |
+| 7 | Стандартные | Нет | Нет | Герметик | Обрабатываются смежные `side_1`, `side_2` |
+| 8 | Стандартные | Нет | Нет | Герметик | Обрабатывается полный `sealant_path` |
+| 9 | Стандартные | Нет | Нет | Герметик | На `side_3` при выполнении прервётся подача |
+| 10 | Стандартные | Стандартный свободный | Нет | Герметик, присоска | Герметик, затем установка люка |
+| 11 | Стандартные | Уже установлен `(0.30, 0.12, 0.82594)`, 45° | Стандартный лоток, **10 винтов** | Магнитный | Установить один из десяти винтов |
+| 12 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Установить два разных винта |
+| 13 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Установить четыре разных винта |
+| 14 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный, затяжной | Установить и затянуть четыре винта |
+| 15 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный, затяжной | Сбой третьего захвата и первой затяжки `hole_4` |
+| 16 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Участок герметика, люк, один винт |
+| 17 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Полный контур, люк, два винта |
+| 18 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Полная сборка |
+| 19 | Стандартные | Свободный `(0.37, −0.20, 0.790667)`, **60°** | Лоток `(−0.10, 0.36, 0.793)`, **10 винтов** | Все четыре | Координаты всех десяти винтов смещены вместе с лотком на `(−0.02, +0.02)` |
+| 20 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Исходная геометрия обычная; сбои вводятся во время выполнения |
+
+В опыте 3 смещается вся пара «стапель + корпус» без изменения взаимной высоты и поворота. Координаты `hatch_seat` и четырёх отверстий сдвигаются вместе с ними на `(−0.01, +0.02)`. В опыте 19 сдвигается лоток **со всеми десятью винтами**; ни один винт не остаётся в старой точке.
+
+Опыты 4, 5, 9, 15 и 20 требуют также управляемых событий исполнителя. Эти события и последующие изменения положения детали не являются дополнительными объектами начальной сцены. Герметик удобнее учитывать как состояние именованных участков контура, а не как заранее лежащую на корпусе модель.
+
+## Геометрические и технические ограничения
+
+По габаритам OBJ стапель занимает в плане примерно `X = 0.0985…0.5015`, `Y = −0.0815…0.3215`; стандартный свободный люк — `X = 0.2663…0.4337`, `Y = −0.3037…−0.1363`; стойка — `X = −0.09…0.33`, `Y = −0.49…−0.35`; лоток — `X = −0.14…−0.02`, `Y = 0.305…0.375`. Эти прямоугольники не пересекаются. Зазор от свободного люка до стойки по Y около 46 мм, до стапеля около 55 мм; от лотка до стапеля по X около 118 мм. Это проверка начальной компоновки, не проверка траекторий инструмента.
+
+Перед реализацией контактных действий нужно проверить в Webots достижимость TCP и коллизии при подъёме люка, при извлечении инструмента из гнезда и при работе между десятью винтами. В `iiwa.wbt` трение `TableMaterial` равно нулю; лоток и винты понадобится удерживать до захвата или задать подходящую контактную модель. Действующий спавнер `object_spawner.py` создаёт случайные OBJ, поэтому для этой таблицы потребуется детерминированная загрузка по номеру эксперимента. Стойка должна содержать отдельно управляемые рабочие инструменты; удалённый `GrantToolRack.proto` для нового мира не используется.

output/md/reference-experiments.md

@@ -0,0 +1,180 @@
+# Эталонное выполнение 20 экспериментов сборки люка
+
+Документ описывает **эталонное (reference) управление** набором из 20
+экспериментов «сборка люка» на KUKA LBR iiwa 7 R800 в симуляторе Webots, а
+также устройство системы управления эталонным исполнителем. Эталон служит
+образцом поведения: с ним сравниваются графы действий, которые строит
+LLM-планировщик по пользовательскому запросу и `scene_json`.
+
+Пакет: `src/iiwa_experement/`. Сцены: `config/scenes.yaml`, геометрия:
+`config/geometry.yaml`, маршруты: `iiwa_experement/reference_scenarios.py`.
+Запускается только симуляция Webots; физический робот через FRI не
+используется.
+
+## Общие правила эталона
+
+- Робот стартует из `home`, первое движение — в рабочую позу `work`; дальше
+  маршрут планируется из текущей рабочей позы. В конце техпроцесса — возврат
+  в `work`, и только последнее движение маршрута — в `home`. Поза
+  `transport` запрещена и не используется нигде, включая аварийные ветви.
+- На роботе постоянно установлен `patron`; рабочие инструменты (`vacuum`,
+  `sealant_tool`, `magnetic_tool`, `torque_tool`) берутся из стойки и
+  вставляются в него. Сам `patron` никогда не снимается.
+- После захвата инструмента — обязательный вертикальный подъём, затем
+  боковой выход из стойки по +Y, и только потом горизонтальные перемещения.
+- После **каждого** контакта (захват, посадка, передача винта, имитация
+  затяжки, контур герметика) выполняется обязательный осевой подъём до
+  любого бокового движения. Если все осевые участки отклонены
+  планировщиком, операция останавливается, а не продолжается на глубине
+  контакта.
+- Захваченные инструмент, люк и винт прикрепляются к роботу в сцене MoveIt,
+  поэтому перенос планируется с учётом переносимого тела.
+- TCP при работе направлен вниз. Если планировщик отклоняет подход из
+  текущей ветви IK, операция один раз переконфигурируется через `work` и
+  повторяет подход; событие `reconfigure` пишется в журнал.
+- Винты не вставляются и не затягиваются: робот подносит винт к отверстию и
+  передаёт его виртуальному оператору; отверстие остаётся пустым. В опытах
+  с `torque_tool` выполняется имитация затяжки: подход к пустому отверстию,
+  видимый поворот фланца на 45° вокруг оси инструмента, раскрутка обратно,
+  осевой подъём.
+- Каждая ветвь отказа имеет не более одной повторной попытки; при
+  неопределённом состоянии выполнение останавливается.
+
+## Что происходит в каждом эксперименте
+
+### I. Захват и установка люка (1–5) — инструмент: присоска `vacuum`
+
+| № | Сцена | Что делает робот |
+|---:|---|---|
+| 1 | Люк лежит свободно в номинальной позе (0.35, −0.22), корпус в стапеле | Берёт присоску из стойки → захватывает люк → переносит на посадочное место корпуса → опускает, отпускает → `inspect_cap` (ok) → возвращает присоску → домой |
+| 2 | Люк повёрнут на 135° вместо 45° | Как №1, но фактическая поза люка читается из состояния сцены; измеренная относительная ориентация учитывается при посадке — люк ставится ровно несмотря на поворот |
+| 3 | Смещены и корпус со стапелем (0.29, 0.14), и люк (0.33, −0.22) | Как №1, но обе позы — люк и посадочное место — берутся из фактического состояния сцены, а не из номинала |
+| 4 | Как №1 + сбой: первый захват люка возвращает `grasp_failed` | Первый подход к люку без захвата (симуляция срыва) → `inspect_state` → ровно одна повторная попытка захвата → штатная установка и проверка |
+| 5 | Как №1 + сбой: первая посадка смещена на 3 мм по X | Люк ставится со смещением → `inspect_cap` даёт `not_ok` (допуск 2 мм) → повторный захват → правильная посадка → `inspect_cap` ok |
+
+### II. Нанесение герметика (6–9) — инструмент: `sealant_tool`, люка в сцене нет
+
+| № | Сцена | Что делает робот |
+|---:|---|---|
+| 6 | Контур = только `side_1` | Берёт инструмент → подходит к началу участка, опускает сопло → проходит `side_1` на рабочей высоте → подъём → `inspect_sealant` → возврат инструмента |
+| 7 | Контур = `side_1` + `side_2` | Два смежных участка проходятся непрерывно (`dispense_contour`): сопло не поднимается на углу → подъём и проверка в конце |
+| 8 | Полный замкнутый контур `side_1…side_4` | Все четыре стороны одним непрерывным движением, контур замыкается в точке начала → проверка полноты |
+| 9 | Полный контур + сбой: прерывание подачи на середине `side_3` | `side_1`, `side_2`, половина `side_3` → остановка, `inspect_state` → возобновление `side_3` с точки разрыва (0.5 → 1.0), а не с начала → `side_4` → проверка |
+
+### III. Передача винтов виртуальному оператору (11–15) — люк уже установлен, лоток с 10 винтами
+
+| № | Сцена | Что делает робот |
+|---:|---|---|
+| 11 | `magnetic_tool`; цель — `hole_1` | Берёт магнит → захватывает `screw_1` из лотка → подъём → `work` → перенос к отверстию → передача оператору → осевой подъём → возврат инструмента |
+| 12 | Отверстия `hole_1`, `hole_3` (диагональ) | Две передачи: `screw_1→hole_1`, `screw_2→hole_3`; после каждой — подъём и возврат к лотку с пустым магнитом |
+| 13 | Отверстия `hole_1`, `hole_2`, `hole_3` | Три разных винта (`screw_1`, `screw_2`, `screw_5`) в три разных отверстия |
+| 14 | Крестовой порядок `hole_1→hole_3→hole_2→hole_4`; есть `torque_tool` | Четыре передачи (`screw_1/2/5/6`) → возврат магнита → затяжной инструмент: имитация затяжки каждого пустого отверстия (подход, поворот 45°, раскрутка, подъём) → возврат инструмента |
+| 15 | Как №14 + сбой: первый захват третьего винта (`screw_5`) неудачен | Две штатные передачи → `grasp_failed` на третьем винте → `inspect_state` → одна повторная попытка → четвёртый винт → имитация затяжки всех четырёх отверстий |
+
+### IV. Полная сборка и перепланирование (16–20) — все четыре инструмента в стойке
+
+| № | Сцена | Что делает робот |
+|---:|---|---|
+| 16 | Люк свободен, контур = `side_1`, один винт (`hole_1`) | Герметик на `side_1` → возврат инструмента → присоска: установка люка и проверка → возврат присоски → магнит: передача `screw_1→hole_1` → `torque_tool`: имитация затяжки `hole_1` |
+| 17 | Полный контур, два диагональных винта | Полный контур герметика с проверкой → установка люка с проверкой → передачи `screw_1→hole_1`, `screw_2→hole_3` → имитация затяжки обоих отверстий |
+| 18 | Полная сборка, крестовой порядок | Полный контур → люк → четыре передачи `hole_1→3→2→4` → имитация затяжки всех четырёх |
+| 19 | Как №18, но люк (0.37, −0.20, 60°) и лоток (0.18, 0.48) смещены | Полная сборка, как №18, но все позы люка и винтов берутся из фактического состояния сцены после смещения |
+| 20 | Как №18 + три сбоя сразу | Прерывание подачи на `side_3` (возобновление с точки разрыва) → люк: первая посадка смещена → `not_ok` → одна перестановка → винты: `grasp_failed` на первом винте → одна повторная попытка → все четыре передачи и имитация затяжки |
+
+Ключевые зависимости, проверяемые во всех опытах: инструмент не берётся,
+пока предыдущий не возвращён в стойку; люк не ставится на неподтверждённый
+контур герметика; передача винта возможна только после подтверждённого
+захвата; каждая ветвь отказа — ровно одна повторная попытка; любое
+неопределённое состояние ведёт к остановке.
+
+## Система управления эталонным исполнителем
+
+### Состав
+
+```
+experiment.launch.py (mode:=view|execute)
+ ├─ iiwa_bringup/iiwa.launch.py   Webots + контроллеры + MoveIt (move_group и MoveItPy)
+ ├─ scene_spawner                 расставляет объекты сцены по scenes.yaml
+ ├─ grant_supervisor              Webots Supervisor (extern): перенос/фиксация объектов,
+ │                                команды attach/detach/grab/handoff/inspect, публикация
+ │                                состояния сцены; в view — read_only
+ └─ reference_runner (только execute)
+     ├─ SupervisorClient          синхронный клиент /iiwa_experement/object_command
+     ├─ PlanningSceneBridge       зеркало сцены Webots в MoveIt: мировые объекты,
+     │                            attached_collision_objects, ACM из iiwa7.srdf,
+     │                            проверка синхронизации через /monitored_planning_scene
+     │                            и сервис /get_planning_scene
+     ├─ MotionAdapter             защищённый клиент /cobot/move_to_pose, move_to_joints,
+     │                            move_to_named, /cobot/stop: свежесть /joint_states,
+     │                            синхронность сцены, ожидание неподвижности, проверка
+     │                            достигнутой позы по TF, одна корректирующая повторная
+     │                            отправка цели, безопасная остановка при любой ошибке
+     ├─ ReferenceOperations       технологические операции: take_tool/dock_tool,
+     │                            pick/place_cap, dispense/dispense_contour,
+     │                            pick_screw/handoff_screw, tighten_hole (имитация),
+     │                            ветви отказов; паттерн «подход → контакт → осевой
+     │                            подъём → перенос» с обязательными зазорами
+     ├─ reference_scenarios       детерминированные маршруты 20 опытов из шагов
+     │                            (move_named/операция/проверка), старт в work,
+     │                            финал work → home
+     └─ TraceWriter               журнал JSONL на каждый прогон: шаги, события,
+                                  итог (status, delivered, cap, sealant, home_reached)
+```
+
+### Последовательность запуска (mode:=execute)
+
+1. Launch валидирует `experiment_id` (1–20), `mode`, `start_delay_sec`,
+   `trace_dir`; готовит временные копии мира Webots и `setting.yaml`.
+2. Стартуют Webots, контроллеры, MoveIt; `scene_spawner` расставляет объекты
+   выбранного опыта; `grant_supervisor` подключается как extern-контроллер
+   (Webots ждёт его до начала симуляции — тупик исключён порядком запуска).
+3. `reference_runner` ждёт: сервис готовности сцены → серверы движения и
+   MoveIt → задержку `start_delay_sec` **по времени симуляции**. Только после
+   этого начинается первое движение — переход в `work`.
+4. Маршрут выполняется по шагам; перед каждым движением проверяются свежесть
+   состояния робота и синхронность планировочной сцены. Переносимые объекты
+   прикрепляются к роботу в MoveIt (`attach`), чтобы столкновения
+   проверялись с учётом инструмента и детали.
+5. Последняя строка журнала — `event=result` со статусом `completed` или
+   `stopped` и причиной остановки.
+
+### Режимы
+
+- `mode:=view` — сцена открывается и остаётся неподвижной: runner не
+  стартует, supervisor работает в read_only, журнал не создаётся.
+- `mode:=execute` — полный прогон эталонного маршрута с журналом.
+
+```bash
+source /opt/ros/jazzy/setup.bash
+colcon build --base-paths src --packages-up-to iiwa_experement --symlink-install
+source install/setup.bash
+
+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
+```
+
+### Политика безопасности движений
+
+- Допустимые именованные позы: только `work` (рабочая) и `home` (финальная).
+  `transport` запрещена и не зеркалируется в конфигурацию исполнителя.
+- Перед планированием робот должен быть неподвижен (окно стабильных нулевых
+  скоростей суставов); цель проверяется по установившейся позе TCP, а не по
+  первому отсчёту контроллера.
+- Захват/перенос: удаление объекта из мировой сцены ↔ прикрепление к роботу
+  выполняются атомарно с проверкой синхронизации, чтобы не было ни
+  дублирования, ни «слепого» переноса.
+- Отказ планирования, устаревшее состояние, рассинхрон сцены или отклонение
+  позы сверх допуска (6 мм / 0,08 рад) — остановка робота и блокировка
+  дальнейших движений до конца прогона.
+
+## Связанные документы
+
+- Задания для LLM-планировщика: [experements.md](experements.md)
+- Начальные позы объектов: [experiment-scenes.md](experiment-scenes.md)
+- Измерения, политика движений и результаты прогонов:
+  `src/iiwa_experement/doc/reference-control-qa.md`
+- Спецификация и план эталона:
+  `docs/superpowers/specs/2026-11-08-grant-reference-control-design.md`,
+  `docs/superpowers/plans/2026-11-08-grant-reference-control.md`

src/iiwa_config/config/moveit/iiwa7.srdf

@@ -37,66 +37,16 @@
     <end_effector name="patron" parent_link="patron" group="iiwa_arm"/>
 
     <disable_collisions link1="base_link" link2="link1" reason="Adjacent"/>
-    <disable_collisions link1="base_link" link2="link2" reason="Never"/>
-    <disable_collisions link1="base_link" link2="link3" reason="Never"/>
-    <disable_collisions link1="base_link" link2="link4" reason="Never"/>
     <disable_collisions link1="link1" link2="link2" reason="Adjacent"/>
-    <disable_collisions link1="link1" link2="link3" reason="Never"/>
-    <disable_collisions link1="link1" link2="link4" reason="Never"/>
-    <disable_collisions link1="link1" link2="link5" reason="Never"/>
-    <disable_collisions link1="link1" link2="link6" reason="Never"/>
-    <disable_collisions link1="link1" link2="link7" reason="Never"/>
     <disable_collisions link1="link2" link2="link3" reason="Adjacent"/>
-    <disable_collisions link1="link2" link2="link4" reason="Never"/>
-    <disable_collisions link1="link2" link2="link5" reason="Never"/>
-    <disable_collisions link1="link2" link2="link6" reason="Never"/>
-    <disable_collisions link1="link2" link2="link7" reason="Never"/>
     <disable_collisions link1="link3" link2="link4" reason="Adjacent"/>
-    <disable_collisions link1="link3" link2="link5" reason="Never"/>
-    <disable_collisions link1="link3" link2="link6" reason="Never"/>
-    <disable_collisions link1="link3" link2="link7" reason="Never"/>
     <disable_collisions link1="link4" link2="link5" reason="Adjacent"/>
-    <disable_collisions link1="link4" link2="link6" reason="Never"/>
-    <disable_collisions link1="link4" link2="link7" reason="Never"/>
     <disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
-    <disable_collisions link1="link5" link2="link7" reason="Never"/>
     <disable_collisions link1="link6" link2="link7" reason="Adjacent"/>
 
-    <disable_collisions link1="link1" link2="patron" reason="Never"/>
-    <disable_collisions link1="link2" link2="patron" reason="Never"/>
-    <disable_collisions link1="link3" link2="patron" reason="Never"/>
-    <disable_collisions link1="link4" link2="patron" reason="Never"/>
-    <disable_collisions link1="link5" link2="patron" reason="Never"/>
-    <disable_collisions link1="link6" link2="patron" reason="Never"/>
-    <disable_collisions link1="link7" link2="patron" reason="Adjacent"/>
     <disable_collisions link1="link7" link2="camera_holder" reason="Adjacent"/>
-    <disable_collisions link1="link6" link2="camera_holder" reason="Never"/>
-    <disable_collisions link1="link5" link2="camera_holder" reason="Never"/>
-    <disable_collisions link1="link4" link2="camera_holder" reason="Never"/>
-    <disable_collisions link1="link3" link2="camera_holder" reason="Never"/>
-    <disable_collisions link1="link2" link2="camera_holder" reason="Never"/>
-    <disable_collisions link1="link1" link2="camera_holder" reason="Never"/>
-    <disable_collisions link1="base_link" link2="camera_holder" reason="Never"/>
     <disable_collisions link1="camera_holder" link2="patron" reason="Adjacent"/>
     <disable_collisions link1="camera_holder" link2="camera_corner" reason="Adjacent"/>
-    <disable_collisions link1="camera_corner" link2="patron" reason="Never"/>
-    <disable_collisions link1="camera_corner" link2="link7" reason="Never"/>
-    <disable_collisions link1="camera_corner" link2="link6" reason="Never"/>
-    <disable_collisions link1="camera_corner" link2="link5" reason="Never"/>
-    <disable_collisions link1="camera_corner" link2="link4" reason="Never"/>
-    <disable_collisions link1="camera_corner" link2="link3" reason="Never"/>
-    <disable_collisions link1="camera_corner" link2="link2" reason="Never"/>
-    <disable_collisions link1="camera_corner" link2="link1" reason="Never"/>
-    <disable_collisions link1="camera_corner" link2="base_link" reason="Never"/>
     <disable_collisions link1="camera_corner" link2="camera_hand" reason="Adjacent"/>
-    <disable_collisions link1="camera_hand" link2="camera_holder" reason="Never"/>
-    <disable_collisions link1="camera_hand" link2="patron" reason="Never"/>
-    <disable_collisions link1="camera_hand" link2="link7" reason="Never"/>
-    <disable_collisions link1="camera_hand" link2="link6" reason="Never"/>
-    <disable_collisions link1="camera_hand" link2="link5" reason="Never"/>
-    <disable_collisions link1="camera_hand" link2="link4" reason="Never"/>
-    <disable_collisions link1="camera_hand" link2="link3" reason="Never"/>
-    <disable_collisions link1="camera_hand" link2="link2" reason="Never"/>
-    <disable_collisions link1="camera_hand" link2="link1" reason="Never"/>
-    <disable_collisions link1="camera_hand" link2="base_link" reason="Never"/>
+    <disable_collisions link1="camera_holder" link2="camera_hand" reason="Adjacent"/>
 </robot>

src/iiwa_config/config/setting.yaml

@@ -9,7 +9,7 @@ robot:
 
 digital_twin:
   tcp_gizmo:
-    enabled: true             # TCP-гизма в Webots; только в режиме симуляции
+    enabled: false           # TCP-гизма в Webots; только в режиме симуляции
     publish_period: 0.032      # период управления [с]
     command_timeout: 0.3       # максимальный возраст цели при отпускании мыши [с]
     state_timeout: 0.5         # допустимый возраст состояния робота [с]
@@ -23,7 +23,7 @@ digital_twin:
     controller_timer: "50"
 
     objects:
-      enabled: true
+      enabled: false
       count: 5                    # точное общее количество; модели могут повторяться
       models:
         - path: pkg://iiwa_description/objects/hammer/hammer1.obj

src/iiwa_config/config/tools.yaml

@@ -24,40 +24,10 @@ tools:
     tip_link: "patron"
     tcp_link: "tcp"
     collisions:
-      - [link1, patron, Never]
-      - [link2, patron, Never]
-      - [link3, patron, Never]
-      - [link4, patron, Never]
-      - [link5, patron, Never]
-      - [link6, patron, Never]
-      - [link7, patron, Adjacent]
+      # Only fixed-joint or constructive camera-assembly contact pairs are
+      # disabled.  Every camera-to-robot pair must be checked by MoveIt.
       - [link7, camera_holder, Adjacent]
-      - [link6, camera_holder, Never]
-      - [link5, camera_holder, Never]
-      - [link4, camera_holder, Never]
-      - [link3, camera_holder, Never]
-      - [link2, camera_holder, Never]
-      - [link1, camera_holder, Never]
-      - [base_link, camera_holder, Never]
       - [camera_holder, patron, Adjacent]
       - [camera_holder, camera_corner, Adjacent]
-      - [camera_corner, patron, Never]
-      - [camera_corner, link7, Never]
-      - [camera_corner, link6, Never]
-      - [camera_corner, link5, Never]
-      - [camera_corner, link4, Never]
-      - [camera_corner, link3, Never]
-      - [camera_corner, link2, Never]
-      - [camera_corner, link1, Never]
-      - [camera_corner, base_link, Never]
       - [camera_corner, camera_hand, Adjacent]
-      - [camera_hand, camera_holder, Never]
-      - [camera_hand, patron, Never]
-      - [camera_hand, link7, Never]
-      - [camera_hand, link6, Never]
-      - [camera_hand, link5, Never]
-      - [camera_hand, link4, Never]
-      - [camera_hand, link3, Never]
-      - [camera_hand, link2, Never]
-      - [camera_hand, link1, Never]
-      - [camera_hand, base_link, Never]
+      - [camera_holder, camera_hand, Adjacent]

src/iiwa_description/resource/protos/GrantToolRack.proto

@@ -1,122 +0,0 @@
-#VRML_SIM R2025a utf8
-
-# Original-size grant magazine. The sealant OBJ export is 10 times smaller
-# than the other tool meshes, so scale only that tool visual.
-# The OBJ meshes use Y as vertical; the default rotation maps Y to world Z.
-PROTO GrantToolRack [
-  field SFVec3f translation 0 0 0
-  field SFRotation rotation 1 0 0 1.57079632679
-]
-{
-  Solid {
-    translation IS translation
-    rotation IS rotation
-    name "grant_tool_magazine"
-    locked TRUE
-    children [
-      Shape {
-        appearance PBRAppearance {
-          baseColor 0.72 0.47 0.14
-          roughness 0.56
-          metalness 0.35
-        }
-        geometry Mesh {
-          url [ "../../objects/grant/tool-magizine.obj" ]
-        }
-      }
-      Solid {
-        translation -0.135 0.01 0
-        name "grant_vacuum_tool_docked"
-        locked TRUE
-        children [
-          Shape {
-            appearance PBRAppearance {
-              baseColor 0.16 0.47 0.72
-              roughness 0.5
-              metalness 0.25
-            }
-            geometry Mesh {
-              url [ "../../objects/grant/vacum-tool.obj" ]
-            }
-          }
-        ]
-        boundingObject Pose {
-          translation 0.00054 0.07 0
-          children [ Box { size 0.0821 0.14 0.081 } ]
-        }
-      }
-      Solid {
-        translation -0.045 0.01 0
-        name "grant_magnetic_tool_docked"
-        locked TRUE
-        children [
-          Shape {
-            appearance PBRAppearance {
-              baseColor 0.43 0.30 0.65
-              roughness 0.4
-              metalness 0.45
-            }
-            geometry Mesh {
-              url [ "../../objects/grant/magnit-tool.obj" ]
-            }
-          }
-        ]
-        boundingObject Pose {
-          translation 0 0.07 0
-          children [ Box { size 0.036 0.14 0.036 } ]
-        }
-      }
-      Solid {
-        translation 0.045 0.01 0
-        name "grant_sealant_tool_docked"
-        locked TRUE
-        children [
-          Transform {
-            scale 10 10 10
-            children [
-              Shape {
-                appearance PBRAppearance {
-                  baseColor 0.23 0.57 0.31
-                  roughness 0.6
-                  metalness 0.15
-                }
-                geometry Mesh {
-                  url [ "../../objects/grant/germetic-tool.obj" ]
-                }
-              }
-            ]
-          }
-        ]
-        boundingObject Pose {
-          translation 0 0.07 0
-          children [ Box { size 0.05608 0.14 0.05608 } ]
-        }
-      }
-      Solid {
-        translation 0.135 0.01 0
-        name "grant_wrench_tool_docked"
-        locked TRUE
-        children [
-          Shape {
-            appearance PBRAppearance {
-              baseColor 0.80 0.32 0.18
-              roughness 0.4
-              metalness 0.45
-            }
-            geometry Mesh {
-              url [ "../../objects/grant/wranch-tool.obj" ]
-            }
-          }
-        ]
-        boundingObject Pose {
-          translation 0 0.07 0
-          children [ Box { size 0.049 0.14 0.049 } ]
-        }
-      }
-    ]
-    boundingObject Pose {
-      translation 0 0.005 0
-      children [ Box { size 0.42 0.01 0.14 } ]
-    }
-  }
-}

src/iiwa_description/urdf/tools/patron.xacro

@@ -86,9 +86,9 @@
             </geometry>
         </visual>
         <collision>
-            <origin xyz="0.0 0.014 0.0125" rpy="0.0 0.0 0.0"/>
+            <origin xyz="0.002 0.0155 0.013" rpy="0.0 0.0 0.0"/>
             <geometry>
-                <box size="0.12 0.025 0.025"/>
+                <box size="0.124 0.031 0.026"/>
             </geometry>
         </collision>
     </link>

src/iiwa_description/worlds/grant/.body_hatch.wbproj

@@ -1,9 +0,0 @@
-Webots Project File version R2025a
-perspectives: 000000ff00000000fd0000000300000000000001ca0000039ffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000000000010000023a00000390fc0200000001fb0000001400540065007800740045006400690074006f00720100000014000003900000003f00ffffff000000030000078000000039fc0100000002fb0000000e0043006f006e0073006f006c006501000001d0000005b00000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000005440000039000000004000000040000000100000008fc00000000
-simulationViewPerspectives: 000000ff00000001000000020000016c000004a00100000002010000000101
-sceneTreePerspectives: 000000ff00000001000000030000001c000002b2000000fa0100000002010000000201
-maximizedDockId: -1
-centralWidgetVisible: 1
-orthographicViewHeight: 1
-textFiles: -1
-consoles: Console:All:All

src/iiwa_description/worlds/grant/body_hatch.wbt

@@ -1,188 +0,0 @@
-#VRML_SIM R2025a utf8
-
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackground.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/floors/protos/RectangleArena.proto"
-EXTERNPROTO "../../resource/protos/CollaborativeRobotTable.proto"
-EXTERNPROTO "../../resource/protos/WorkspaceLimiter.proto"
-EXTERNPROTO "../../resource/protos/GrantToolRack.proto"
-
-# Based on ../iiwa.wbt. The robot is spawned by iiwa_bringup at
-# (-0.25, 0, 0.79), facing world +X. All workpieces stay on the table
-# in front of its base. The tabletop is at world Z = 0.793 m.
-# The workpiece OBJ meshes use Y as their vertical axis. The shared axis-angle
-# rotation first maps Y to world Z, then turns the workpieces 45 degrees
-# around world Z. Coordinates and dimensions are metres.
-#
-# Base scene: the hatch lies separately on the tabletop for pickup and
-# transfer. The robot starts without an attached tool; the magazine holds
-# four static tools. For an already installed
-# hatch, use body_hatch_installed.wbt. For experiment 02, offset
-# BODY_HATCH_BODY by +0.003 m in X. Experiment 03 keeps an empty fixture;
-# experiment 12 omits the hatch.
-
-WorldInfo {
-  title "Body and hatch - grant experiments"
-  basicTimeStep 32
-  optimalThreadCount 4
-  contactProperties [
-    ContactProperties {
-      material1 "TableMaterial"
-      coulombFriction [ 0.6 ]
-      bounce 0
-      bounceVelocity 0
-      softERP 1
-      softCFM 0.0001
-    }
-    ContactProperties {
-      material1 "tool"
-      material2 "TableMaterial"
-      bounce 0
-      softCFM 0.002
-      maxContactJoints 100
-    }
-    ContactProperties {
-      material1 "default"
-      material2 "default"
-      maxContactJoints 20
-      bounce 0
-      softCFM 0.001
-    }
-  ]
-}
-Viewpoint {
-  orientation 0.19663259328507318 -0.09057124279298501 -0.9762850368805829 3.9867817928674034
-  position 1.75 -1.85 1.77
-  followType "Mounted Shot"
-  ambientOcclusionRadius 3
-}
-TexturedBackground {
-}
-TexturedBackgroundLight {
-  texture "noon_sunny_empty"
-  luminosity 2
-}
-RectangleArena {
-  name "Arena"
-  floorSize 12 12
-  wallHeight 2
-}
-CollaborativeRobotTable {
-  translation 0 0 -0.007
-  locked TRUE
-  showCabinet TRUE
-}
-DEF limiter Group {
-  children [
-    WorkspaceLimiter {
-      translation -1.56 -0.219999 1.01
-      rotation 0 0 1 1.5708
-      name "WorkspaceLimiter_back"
-      locked TRUE
-      scale 2.32 1 2
-    }
-    WorkspaceLimiter {
-      translation -0.04 1.18 1.01
-      name "WorkspaceLimiter_right"
-      locked TRUE
-      scale 2.43 1 2
-    }
-  ]
-}
-DirectionalLight {
-  direction -0.1 0.1 0
-  intensity 2
-}
-
-# Fixture plate: bottom Y=-0.010 m rests on the tabletop. The two rails
-# support the body at Y=+0.0161 m without intersecting its underside.
-DEF BODY_HATCH_STAPEL Solid {
-  translation 0.30 0.12 0.803
-  rotation 0.862856209461 0.357406744337 0.357406744337 1.717771517458
-  name "body_hatch_stapel"
-  locked TRUE
-  children [
-    Shape {
-      appearance PBRAppearance {
-        baseColor 0.30 0.34 0.39
-        roughness 0.55
-        metalness 0.55
-      }
-      geometry Mesh {
-        url [ "../../objects/grant/stapel.obj" ]
-      }
-    }
-  ]
-  boundingObject Group {
-    children [
-      Pose {
-        translation 0 -0.005 0
-        children [ Box { size 0.35 0.01 0.22 } ]
-      }
-      Pose {
-        translation -0.104 0.008 0
-        children [ Box { size 0.014 0.016 0.156 } ]
-      }
-      Pose {
-        translation 0.104 0.008 0
-        children [ Box { size 0.014 0.016 0.156 } ]
-      }
-    ]
-  }
-}
-
-# The body shares the fixture centre. Its +0.020 m height puts the
-# underside on top of the fixture rails.
-DEF BODY_HATCH_BODY Solid {
-  translation 0.30 0.12 0.823
-  rotation 0.862856209461 0.357406744337 0.357406744337 1.717771517458
-  name "body_hatch_body"
-  locked TRUE
-  children [
-    Shape {
-      appearance PBRAppearance {
-        baseColor 0.64 0.68 0.72
-        roughness 0.45
-        metalness 0.45
-      }
-      geometry Mesh {
-        url [ "../../objects/grant/base-detail.obj" ]
-      }
-    }
-  ]
-  boundingObject Pose {
-    translation 0 0.000695 0
-    children [ Box { size 0.30 0.00915 0.17 } ]
-  }
-}
-DEF BODY_HATCH_CAP Solid {
-  translation 0.35 -0.25 0.790667
-  rotation 0.862856209461 0.357406744337 0.357406744337 1.717771517458
-  name "body_hatch_cap"
-  children [
-    Shape {
-      appearance PBRAppearance {
-        baseColor 0.78 0.82 0.86
-        roughness 0.35
-        metalness 0.55
-      }
-      geometry Mesh {
-        url [ "../../objects/grant/cap.obj" ]
-      }
-    }
-  ]
-  boundingObject Pose {
-    translation 0 0.004165 0
-    children [ Box { size 0.1384 0.00367 0.0984 } ]
-  }
-  physics Physics {
-    mass 0.1
-  }
-}
-
-# The rack is in the front-left work area, clear of the loose hatch.
-# Its 180-degree yaw faces the tool sockets toward the robot.
-GrantToolRack {
-  translation 0.12 -0.42 0.793
-  rotation 0 0.70710678118 0.70710678118 3.14159265359
-}

src/iiwa_description/worlds/grant/body_hatch_installed.wbt

@@ -1,182 +0,0 @@
-#VRML_SIM R2025a utf8
-
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackground.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/floors/protos/RectangleArena.proto"
-EXTERNPROTO "../../resource/protos/CollaborativeRobotTable.proto"
-EXTERNPROTO "../../resource/protos/WorkspaceLimiter.proto"
-EXTERNPROTO "../../resource/protos/GrantToolRack.proto"
-
-# Based on ../iiwa.wbt. The robot is spawned by iiwa_bringup at
-# (-0.25, 0, 0.79), facing world +X. All workpieces stay on the table
-# in front of its base. The tabletop is at world Z = 0.793 m.
-# The workpiece OBJ meshes use Y as their vertical axis. The shared axis-angle
-# rotation first maps Y to world Z, then turns the workpieces 45 degrees
-# around world Z. Coordinates and dimensions are metres.
-#
-# Installed scene for experiments 04, 14 and 15: the hatch is fixed on
-# the body surface. The magazine holds the four static tool models.
-# For a loose hatch that the robot can pick up, use body_hatch.wbt.
-
-WorldInfo {
-  title "Body with installed hatch and tool magazine"
-  basicTimeStep 32
-  optimalThreadCount 4
-  contactProperties [
-    ContactProperties {
-      material1 "TableMaterial"
-      coulombFriction [ 0.6 ]
-      bounce 0
-      bounceVelocity 0
-      softERP 1
-      softCFM 0.0001
-    }
-    ContactProperties {
-      material1 "tool"
-      material2 "TableMaterial"
-      bounce 0
-      softCFM 0.002
-      maxContactJoints 100
-    }
-    ContactProperties {
-      material1 "default"
-      material2 "default"
-      maxContactJoints 20
-      bounce 0
-      softCFM 0.001
-    }
-  ]
-}
-Viewpoint {
-  orientation 0.19663259328507318 -0.09057124279298501 -0.9762850368805829 3.9867817928674034
-  position 1.75 -1.85 1.77
-  followType "Mounted Shot"
-  ambientOcclusionRadius 3
-}
-TexturedBackground {
-}
-TexturedBackgroundLight {
-  texture "noon_sunny_empty"
-  luminosity 2
-}
-RectangleArena {
-  name "Arena"
-  floorSize 12 12
-  wallHeight 2
-}
-CollaborativeRobotTable {
-  translation 0 0 -0.007
-  locked TRUE
-  showCabinet TRUE
-}
-DEF limiter Group {
-  children [
-    WorkspaceLimiter {
-      translation -1.56 -0.219999 1.01
-      rotation 0 0 1 1.5708
-      name "WorkspaceLimiter_back"
-      locked TRUE
-      scale 2.32 1 2
-    }
-    WorkspaceLimiter {
-      translation -0.04 1.18 1.01
-      name "WorkspaceLimiter_right"
-      locked TRUE
-      scale 2.43 1 2
-    }
-  ]
-}
-DirectionalLight {
-  direction -0.1 0.1 0
-  intensity 2
-}
-
-# Fixture plate: bottom Y=-0.010 m rests on the tabletop. The two rails
-# support the body at Y=+0.0161 m without intersecting its underside.
-DEF BODY_HATCH_STAPEL Solid {
-  translation 0.30 0.12 0.803
-  rotation 0.862856209461 0.357406744337 0.357406744337 1.717771517458
-  name "body_hatch_stapel"
-  locked TRUE
-  children [
-    Shape {
-      appearance PBRAppearance {
-        baseColor 0.30 0.34 0.39
-        roughness 0.55
-        metalness 0.55
-      }
-      geometry Mesh {
-        url [ "../../objects/grant/stapel.obj" ]
-      }
-    }
-  ]
-  boundingObject Group {
-    children [
-      Pose {
-        translation 0 -0.005 0
-        children [ Box { size 0.35 0.01 0.22 } ]
-      }
-      Pose {
-        translation -0.104 0.008 0
-        children [ Box { size 0.014 0.016 0.156 } ]
-      }
-      Pose {
-        translation 0.104 0.008 0
-        children [ Box { size 0.014 0.016 0.156 } ]
-      }
-    ]
-  }
-}
-
-# The body shares the fixture centre. Its +0.020 m height puts the
-# underside on top of the fixture rails.
-DEF BODY_HATCH_BODY Solid {
-  translation 0.30 0.12 0.823
-  rotation 0.862856209461 0.357406744337 0.357406744337 1.717771517458
-  name "body_hatch_body"
-  locked TRUE
-  children [
-    Shape {
-      appearance PBRAppearance {
-        baseColor 0.64 0.68 0.72
-        roughness 0.45
-        metalness 0.45
-      }
-      geometry Mesh {
-        url [ "../../objects/grant/base-detail.obj" ]
-      }
-    }
-  ]
-  boundingObject Pose {
-    translation 0 0.000695 0
-    children [ Box { size 0.30 0.00915 0.17 } ]
-  }
-}
-DEF BODY_HATCH_CAP Solid {
-  translation 0.30 0.12 0.82594
-  rotation 0.862856209461 0.357406744337 0.357406744337 1.717771517458
-  name "body_hatch_cap"
-  locked TRUE
-  children [
-    Shape {
-      appearance PBRAppearance {
-        baseColor 0.78 0.82 0.86
-        roughness 0.35
-        metalness 0.55
-      }
-      geometry Mesh {
-        url [ "../../objects/grant/cap.obj" ]
-      }
-    }
-  ]
-  boundingObject Pose {
-    translation 0 0.004165 0
-    children [ Box { size 0.1384 0.00367 0.0984 } ]
-  }
-}
-
-# The rack is visible in the front-left work area, facing the robot.
-GrantToolRack {
-  translation 0.12 -0.42 0.793
-  rotation 0 0.70710678118 0.70710678118 3.14159265359
-}

src/iiwa_experement/config/geometry.yaml

@@ -0,0 +1,192 @@
+# Calibrated working geometry for the grant reference executor.
+#
+# All lengths are metres, all angles are degrees unless a key says rad.
+# Every transform below names its parent and child frame.  The numeric values
+# were measured from the meshes in iiwa_description/objects/grant, from
+# patron.xacro and from the actual Webots scene.  They are deliberately kept
+# in one file so the executor never hard-codes a working pose.
+schema_version: 1
+units: m
+angle_units: deg
+
+frames:
+  world: {parent: null}
+  base_link: {parent: world}
+  patron: {parent: base_link}
+  tcp: {parent: patron}
+
+# Webots spawns the robot at this fixed world pose (setting.yaml transform).
+world_to_base:
+  xyz: [-0.25, 0.0, 0.79]
+  rpy_deg: [0.0, 0.0, 0.0]
+  parent: world
+  child: base_link
+
+# patron.xacro: patron_tcp origin is 55.2 mm along patron Z, rotated -90 deg.
+patron_to_tcp:
+  xyz: [0.0, 0.0, 0.0552]
+  rpy_deg: [0.0, 0.0, -90.0]
+  parent: patron
+  child: tcp
+
+# The mount (top of a working tool) sits exactly at tcp.  The working tip is
+# 0.140 m further along tcp +Z, which points towards the work piece when the
+# arm is in a working pose.
+tool_mounts:
+  vacuum: {xyz: [0.0, 0.0, 0.0], rpy_deg: [0.0, 0.0, 0.0]}
+  magnetic_tool: {xyz: [0.0, 0.0, 0.0], rpy_deg: [0.0, 0.0, 0.0]}
+  sealant_tool: {xyz: [0.0, 0.0, 0.0], rpy_deg: [0.0, 0.0, 0.0]}
+  torque_tool: {xyz: [0.0, 0.0, 0.0], rpy_deg: [0.0, 0.0, 0.0]}
+
+tool_work_tips:
+  vacuum: {xyz: [0.0, 0.0, 0.140], rpy_deg: [0.0, 0.0, 0.0]}
+  magnetic_tool: {xyz: [0.0, 0.0, 0.140], rpy_deg: [0.0, 0.0, 0.0]}
+  sealant_tool: {xyz: [0.0, 0.0, 0.140], rpy_deg: [0.0, 0.0, 0.0]}
+  torque_tool: {xyz: [0.0, 0.0, 0.140], rpy_deg: [0.0, 0.0, 0.0]}
+
+# Transform from tcp to the Webots node origin (bottom of the vertical OBJ).
+# The OBJ Y axis is imported as Webots Z, so the node is Rx(-90) relative to a
+# tcp that points down.
+tool_tcp_to_nodes:
+  vacuum: {xyz: [0.0, 0.0, 0.140], rpy_deg: [-90.0, 0.0, 0.0]}
+  magnetic_tool: {xyz: [0.0, 0.0, 0.140], rpy_deg: [-90.0, 0.0, 0.0]}
+  sealant_tool: {xyz: [0.0, 0.0, 0.140], rpy_deg: [-90.0, 0.0, 0.0]}
+  torque_tool: {xyz: [0.0, 0.0, 0.140], rpy_deg: [-90.0, 0.0, 0.0]}
+
+# 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]}
+  # 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]}
+  torque_tool: {size: [0.049, 0.049, 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
+# face; the suction cup touches the highest point (local Y = 0.006).
+cap_tcp_to_node:
+  xyz: [0.0, 0.0, 0.146]
+  rpy_deg: [-90.0, 0.0, 0.0]
+cap_collision:
+  # Expressed in tcp: the plate is horizontal, 3.67 mm thick.
+  size: [0.1384, 0.0984, 0.00367]
+  offset: [0.0, 0.0, 0.141835]
+# Installed hatch pose relative to the body node (body local Y is world Z).
+body_to_installed_cap:
+  xyz: [0.0, 0.00294, 0.0]
+  rpy_deg: [0.0, 0.0, 0.0]
+
+# Screws stand with their head up.  The magnet touches the head, 9 mm above
+# the node origin; the screw node is therefore 149 mm below tcp.
+screw_tcp_to_node:
+  xyz: [0.0, 0.0, 0.149]
+  rpy_deg: [-90.0, 0.0, 0.0]
+screw_collision:
+  size: [0.0055, 0.0055, 0.009]
+  offset: [0.0, 0.0, 0.1445]
+
+# Hole centres are measured in the world frame of the standard, unshifted
+# body at 45 deg.  They rotate with the body.  The values agree with
+# doc/experiment-scenes.md.
+hole_reference_yaw_deg: 45.0
+hole_z_above_body_origin: 0.00894
+handoff_gap: 0.002
+holes:
+  hole_1: {dx: -0.0757, dy: -0.0148}
+  hole_2: {dx: -0.0148, dy: -0.0757}
+  hole_3: {dx: 0.0757, dy: 0.0148}
+  hole_4: {dx: 0.0148, dy: 0.0757}
+
+# Sealant ring: a closed rectangle just inside the hatch rim.  Half sizes are
+# in the hatch body plane; the ring is traversed counter-clockwise starting at
+# the hole_1 side.  Segment endpoints are derived from these measured values.
+sealant_reference_yaw_deg: 45.0
+sealant_rect:
+  half_x: 0.058
+  half_z: 0.037
+sealant_z_above_body_top: 0.0015
+sealant_sides:
+  side_1: {start: [-0.06718, -0.01485], end: [-0.01485, -0.06718]}
+  side_2: {start: [-0.01485, -0.06718], end: [0.06718, 0.01485]}
+  side_3: {start: [0.06718, 0.01485], end: [0.01485, 0.06718]}
+  side_4: {start: [0.01485, 0.06718], end: [-0.06718, -0.01485]}
+
+# Approach / transfer clearances and limits.
+approach_gap: 0.060
+contact_gap: 0.003
+transfer_gap: 0.080
+seat_gap: 0.002
+position_tolerance: 0.006
+orientation_tolerance_rad: 0.08
+speeds:
+  transfer: 0.35
+  approach: 0.18
+  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.
+# Only the two poses the reference executor is allowed to use are mirrored
+# here: 'work' is the neutral working pose, 'home' is the final pose of a
+# route.  The 'transport' pose from the global MoveIt config is deliberately
+# not listed: it is forbidden in the reference routes.
+named_joints:
+  home: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+  work: [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0]
+
+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:
+  table:
+    # Top at Z = 0.786.  The base_link collision mesh reaches 2.05 mm below
+    # its frame origin (world Z = 0.788), so a table top at 0.789 was actually
+    # a permanent 1 mm penetration that the contact check flagged only
+    # intermittently.  The 2 mm gap below the mesh bottom keeps the base
+    # collision-free while the slab still blocks downward motion.
+    type: box
+    size: [1.28, 1.08, 0.045]
+    offset_z: 0.7635
+  # The fixture is a frame, not a solid block: a low base plate and two
+  # side rails.  A full box would cover the hatch seat and make placement
+  # impossible.
+  fixture_base:
+    type: box
+    size: [0.350, 0.220, 0.010]
+    offset_z: -0.005
+  fixture_rail:
+    type: box
+    size: [0.014, 0.156, 0.016]
+    offset_z: 0.008
+    offset_x: 0.104
+  body:
+    type: box
+    size: [0.300, 0.170, 0.00915]
+    offset_z: 0.000695
+  rack:
+    # The magazine is represented by its base plate only.  Keep it just
+    # below the tool sockets so the vertical tool bodies do not overlap it.
+    type: box
+    size: [0.420, 0.140, 0.010]
+    offset_z: -0.015
+  tray_base:
+    type: box
+    size: [0.120, 0.070, 0.0024]
+    offset_z: 0.0012
+  tray_wall_x:
+    type: box
+    size: [0.002, 0.070, 0.010]
+    offset_z: 0.005
+    offset_x: 0.059
+  tray_wall_y:
+    type: box
+    size: [0.120, 0.002, 0.010]
+    offset_z: 0.005
+    offset_y: 0.034

src/iiwa_experement/config/moveit_cpp.yaml

@@ -0,0 +1,61 @@
+# Package-local MoveItCpp settings for the reference executor.
+#
+# The stock iiwa_config copy publishes the MoveItPy maintained scene on
+# /monitored_planning_scene (the same topic move_group publishes) but listens
+# to /planning_scene.  The executor publishes its diffs to move_group, so the
+# MoveItPy monitor must instead listen to move_group's maintained scene.
+#
+# publish_planning_scene_topic is the topic this monitor subscribes to;
+# monitored_planning_scene_topic is where it republishes its own scene.
+planning_scene_monitor_options:
+  name: "planning_scene_monitor"
+  robot_description: "robot_description"
+  joint_state_topic: "/joint_states"
+  attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
+  publish_planning_scene_topic: "/monitored_planning_scene"
+  monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
+  wait_for_initial_state_timeout: 10.0
+
+planning_pipelines:
+  pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp"]
+
+plan_request_params:
+  planning_attempts: 1
+  planning_pipeline: ompl
+  max_velocity_scaling_factor: 0.5
+  max_acceleration_scaling_factor: 1.0
+
+ompl_rrtc:
+  plan_request_params:
+    planning_attempts: 1
+    planning_pipeline: ompl
+    planner_id: "RRTConnectkConfigDefault"
+    max_velocity_scaling_factor: 1.0
+    max_acceleration_scaling_factor: 1.0
+    planning_time: 1.0
+
+pilz_ptp:
+  plan_request_params:
+    planning_attempts: 1
+    planning_pipeline: pilz_industrial_motion_planner
+    planner_id: "PTP"
+    max_velocity_scaling_factor: 1.0
+    max_acceleration_scaling_factor: 1.0
+    planning_time: 0.8
+
+pilz_lin:
+  plan_request_params:
+    planning_attempts: 1
+    planning_pipeline: pilz_industrial_motion_planner
+    planner_id: "LIN"
+    max_velocity_scaling_factor: 1.0
+    max_acceleration_scaling_factor: 1.0
+    planning_time: 0.8
+
+chomp_planner:
+  plan_request_params:
+    planning_attempts: 1
+    planning_pipeline: chomp
+    max_velocity_scaling_factor: 1.0
+    max_acceleration_scaling_factor: 1.0
+    planning_time: 1.5

src/iiwa_experement/config/scenes.yaml

@@ -0,0 +1,197 @@
+# Machine-readable source for the twenty grant experiments.
+#
+# Units are metres and degrees in the Webots world frame. The robot is spawned
+# by iiwa_bringup at (-0.25, 0, 0.79). The tabletop is at Z = 0.793.
+#
+# The shared poses below are copied from doc/experiment-scenes.md. Experiments
+# only list the objects they add and the poses that differ. No value is chosen
+# at run time. `patron` is part of the robot URDF in every experiment and is
+# therefore not listed as a scene object.
+
+schema_version: 1
+
+poses:
+  fixture:
+    x: 0.30
+    y: 0.12
+    z: 0.803
+    yaw_deg: 45
+  body:
+    x: 0.30
+    y: 0.12
+    z: 0.823
+    yaw_deg: 45
+  rack:
+    x: 0.12
+    y: -0.42
+    z: 0.793
+    yaw_deg: 0
+  tray:
+    # Left/front edge of the reachable workspace, clear of the tool rack and
+    # the fixture. Moving the whole tray also moves its ten screws.
+    x: 0.20
+    y: 0.46
+    z: 0.793
+    yaw_deg: 0
+  cap_free:
+    x: 0.35
+    y: -0.22
+    z: 0.790667
+    yaw_deg: 45
+  cap_installed:
+    x: 0.30
+    y: 0.12
+    z: 0.82594
+    yaw_deg: 45
+  tools:
+    vacuum:
+      x: 0.255
+      y: -0.42
+      z: 0.803
+      yaw_deg: 0
+    magnetic_tool:
+      x: 0.165
+      y: -0.42
+      z: 0.803
+      yaw_deg: 0
+    sealant_tool:
+      x: 0.075
+      y: -0.42
+      z: 0.803
+      yaw_deg: 0
+    torque_tool:
+      x: -0.015
+      y: -0.42
+      z: 0.803
+      yaw_deg: 0
+
+# Screw offsets are relative to the tray centre. The order is the screw_1 ...
+# screw_10 order from doc/experiment-scenes.md; only the first four are at the
+# grid corners. All screws rest with their lower point at Z = 0.7954.
+screws:
+  z: 0.7954
+  layout:
+    - {name: screw_1, dx: -0.040, dy: -0.015}
+    - {name: screw_2, dx: 0.040, dy: -0.015}
+    - {name: screw_3, dx: -0.040, dy: 0.015}
+    - {name: screw_4, dx: 0.040, dy: 0.015}
+    - {name: screw_5, dx: -0.020, dy: -0.015}
+    - {name: screw_6, dx: 0.000, dy: -0.015}
+    - {name: screw_7, dx: 0.020, dy: -0.015}
+    - {name: screw_8, dx: -0.020, dy: 0.015}
+    - {name: screw_9, dx: 0.000, dy: 0.015}
+    - {name: screw_10, dx: 0.020, dy: 0.015}
+
+experiments:
+  "1":
+    cap: free
+    tools: [vacuum]
+  "2":
+    cap: free
+    cap_pose: {x: 0.35, y: -0.22, z: 0.790667, yaw_deg: 135}
+    tools: [vacuum]
+  "3":
+    fixture: {x: 0.29, y: 0.14, z: 0.803, yaw_deg: 45}
+    body: {x: 0.29, y: 0.14, z: 0.823, yaw_deg: 45}
+    cap: free
+    cap_pose: {x: 0.33, y: -0.22, z: 0.790667, yaw_deg: 45}
+    tools: [vacuum]
+  "4":
+    cap: free
+    tools: [vacuum]
+    failures:
+      - {skill: grab, outcome: grasp_failed, attempt: 1}
+  "5":
+    cap: free
+    tools: [vacuum]
+    failures:
+      - skill: verify
+        outcome: not_ok
+        target: cap
+        attempt: 1
+        details: {after_first_install_x: 0.303}
+  "6":
+    cap: absent
+    tools: [sealant_tool]
+    sealant: [side_1]
+  "7":
+    cap: absent
+    tools: [sealant_tool]
+    sealant: [side_1, side_2]
+  "8":
+    cap: absent
+    tools: [sealant_tool]
+    sealant: [side_1, side_2, side_3, side_4]
+  "9":
+    cap: absent
+    tools: [sealant_tool]
+    sealant: [side_1, side_2, side_3, side_4]
+    failures:
+      - {skill: dispense, outcome: interrupted, target: side_3, attempt: 1}
+  "10":
+    cap: free
+    tools: [sealant_tool, vacuum]
+    sealant: [side_1, side_2, side_3, side_4]
+  "11":
+    cap: installed
+    tray: true
+    tools: [magnetic_tool]
+    screw_holes: [hole_1]
+  "12":
+    cap: installed
+    tray: true
+    tools: [magnetic_tool]
+    screw_holes: [hole_1, hole_3]
+  "13":
+    cap: installed
+    tray: true
+    tools: [magnetic_tool]
+    screw_holes: [hole_1, hole_2, hole_3]
+  "14":
+    cap: installed
+    tray: true
+    tools: [magnetic_tool, torque_tool]
+    screw_holes: [hole_1, hole_3, hole_2, hole_4]
+  "15":
+    cap: installed
+    tray: true
+    tools: [magnetic_tool, torque_tool]
+    screw_holes: [hole_1, hole_3, hole_2, hole_4]
+    failures:
+      - {skill: grab, outcome: grasp_failed, target: third_selected_screw, attempt: 1}
+  "16":
+    cap: free
+    tray: true
+    tools: [vacuum, magnetic_tool, sealant_tool, torque_tool]
+    sealant: [side_1]
+    screw_holes: [hole_1]
+  "17":
+    cap: free
+    tray: true
+    tools: [vacuum, magnetic_tool, sealant_tool, torque_tool]
+    sealant: [side_1, side_2, side_3, side_4]
+    screw_holes: [hole_1, hole_3]
+  "18":
+    cap: free
+    tray: true
+    tools: [vacuum, magnetic_tool, sealant_tool, torque_tool]
+    sealant: [side_1, side_2, side_3, side_4]
+    screw_holes: [hole_1, hole_3, hole_2, hole_4]
+  "19":
+    cap: free
+    cap_pose: {x: 0.37, y: -0.20, z: 0.790667, yaw_deg: 60}
+    tray: true
+    tray_pose: {x: 0.18, y: 0.48, z: 0.793, yaw_deg: 0}
+    tools: [vacuum, magnetic_tool, sealant_tool, torque_tool]
+    sealant: [side_1, side_2, side_3, side_4]
+    screw_holes: [hole_1, hole_3, hole_2, hole_4]
+  "20":
+    cap: free
+    tray: true
+    tools: [vacuum, magnetic_tool, sealant_tool, torque_tool]
+    sealant: [side_1, side_2, side_3, side_4]
+    screw_holes: [hole_1, hole_3, hole_2, hole_4]
+    failures:
+      - {skill: dispense, outcome: interrupted, target: side_3, attempt: 1}
+      - {skill: grab, outcome: grasp_failed, target: first_selected_screw, attempt: 1}
+      - {skill: verify, outcome: not_ok, target: cap, attempt: 1}

src/iiwa_experement/doc/README.md

@@ -0,0 +1,207 @@
+# iiwa_experement
+
+Пакет запускает один из двадцати детерминированных опытов в Webots. Один
+запуск создаёт один свежий мир: шаблон `worlds/grant/iiwa-grant.wbt` копируется
+во временную директорию, в копии заменяются пути к PROTO, затем существующий
+`iiwa_bringup` запускает Webots, робота iiwa 7, контроллеры и камеры. После
+этого `scene_spawner` добавляет объекты выбранного сценария.
+
+В режиме `execute` эталонный исполнитель перемещает робота, вставляет рабочие
+инструменты в постоянный `patron`, переносит люк, проводит сопло по контуру,
+передаёт винты виртуальному оператору и выполняет имитацию затяжки затяжным
+инструментом. Винты не вставляются: после передачи отверстие остаётся пустым,
+а `torque_tool` подходит к отверстию, поворачивается вокруг своей оси и
+отходит. Режим `planner` готовит ту же записываемую сцену (supervisor
+принимает захваты), но **не** запускает `reference_runner`: готовый граф
+передаётся снаружи в `iiwa_llm_plan_executor`. Режим `view` (по умолчанию)
+оставляет сцену неподвижной.
+
+## Сборка
+
+```bash
+source /opt/ros/jazzy/setup.bash
+colcon build --base-paths src --packages-up-to iiwa_experement --symlink-install
+source install/setup.bash
+```
+
+## Запуск
+
+Обязательный параметр `experiment_id` принимает только целое число от 1 до 20.
+Режим по умолчанию — `view`; он готовит сцену и не двигает робота. Режим
+`execute` запускает эталонный исполнитель.
+
+```bash
+# только осмотр сцены
+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 до старта
+Webots. В `execute` первое движение начинается не раньше, чем одновременно:
+`scene_ready=true`, supervisor нашёл все объекты, доступны action/service
+движения, `/joint_states` и TF свежие, сцена подтверждена в MoveIt, а по
+`/clock` прошло `start_delay_sec` секунд **симуляционного** времени. При
+паузе Webots задержка не истекает. Тайм-аут готовности даёт `failed` без
+движения.
+
+| Параметр | По умолчанию | Значение |
+| --- | --- | --- |
+| `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`.
+- Винты не вставляются. После подтверждённой передачи у отверстия винт
+  убирается из сцены, отверстие остаётся пустым. В опытах 14–20 берётся
+  `torque_tool`, подходит к каждому отверстию, выполняет поворот на 45° и
+  возвращается в стойку. Это видимая имитация затяжки без установки винта.
+
+Управляемые ветви: опыт 4 — один неудачный захват люка; 5 — первая посадка
+со смещением 3 мм и одна корректировка; 9 — прерывание `side_3` и
+продолжение с подтверждённой точки; 15 — неудачный захват третьего выбранного
+винта и одна повторная попытка; 20 — все три события. Повторный отказ или
+неизвестное состояние дают `stopped` без продолжения.
+
+## Готовность сцены
+
+Сервис доступен с момента старта ноды, но сообщает успех только после
+последнего успешного вызова supervisor:
+
+```bash
+ros2 service call /iiwa_experement/scene_ready std_srvs/srv/Trigger {}
+```
+
+Успешный ответ содержит `success: true` и сообщение вида
+`experiment 11: 17 objects spawned`. Пока объекты добавляются или если спавн
+завершился ошибкой, `success` остаётся `false`, а в `message` находится причина
+и имя последнего объекта.
+
+## Состав сцен
+
+| Опыты | Люк | Лоток и винты | Рабочие инструменты |
+| --- | --- | --- | --- |
+| 1–5 | свободный | нет | присоска |
+| 6–9 | отсутствует | нет | герметик |
+| 10 | свободный | нет | герметик, присоска |
+| 11–13 | установлен | 10 винтов | магнитный |
+| 14–15 | установлен | 10 винтов | магнитный, затяжной (имитация) |
+| 16–20 | свободный | 10 винтов | присоска, магнитный, герметик, затяжной (имитация) |
+
+Во всех опытах `patron` уже находится на роботе: он включён в URDF через
+`urdf/tools/tool_active.xacro`. Рабочие инструменты лежат отдельно в четырёх
+гнёзах стойки. Управляемые объекты создаются зафиксированными: свободный люк
+не имеет физики, чтобы supervisor мог детерминированно вести его за TCP без
+скачков. Захват и посадка подтверждаются состоянием supervisor и фактической
+позой; это не физический `Connector`.
+
+Ограничение текущего окружения: MoveItPy-монитор сервера движения неустойчиво
+планирует с прикреплёнными объектами. Поэтому переносимый инструмент/деталь
+ведёт Webots-supervisor, а в MoveIt на время переноса остаются все статические
+объекты; контактные позы и безопасные высоты переноса заданы калиброванной
+геометрией и проверяются перед каждой операцией. Это не отключает проверки
+столкновений для робота и статической сцены.
+
+Координаты и наборы объектов заданы только в `config/scenes.yaml`; генератор
+не выбирает случайные позы. В опытах 11–20 всегда ровно десять винтов, даже
+если сценарий использует один, два или четыре. Лоток стоит у левого края стола
+(`+Y`) впереди слева от стойки, в достижимой зоне; в опыте 19 лоток и все
+десять винтов смещены вместе.
+
+## Ресурсы
+
+- Шаблон мира и код: `src/iiwa_experement/`.
+- OBJ/MTL: `src/iiwa_description/objects/grant/`; они не копируются.
+- PROTO стола и ограждений: `src/iiwa_description/resource/protos/`.
+- Спецификации: `doc/experements.md`, `doc/experiment-scenes.md`.
+
+Старые `body_hatch*.wbt` и `GrantToolRack.proto` не восстанавливаются и не
+используются. Стойка собрана из `tool-magizine.obj` и отдельных объектов
+инструментов.
+
+## Проверки
+
+```bash
+colcon test --base-paths src --packages-select iiwa_experement
+colcon test-result --verbose
+```
+
+Тесты пакета проверяют:
+
+- установку мира, YAML, launch, документации и обоих режимов;
+- все 20 сценариев, включая 10 винтов в опытах 11–20 и имитацию затяжки
+  без установки винтов;
+- геометрию: длину инструментов, `tcp` на 0,0552 м, посадку люка, контур,
+  отверстия, композицию преобразований и отсутствие нечисловых/неположительных
+  зазоров;
+- автомат состояний: захват только нужным инструментом, один переносимый
+  объект, запрет второго инструмента, повторной передачи и чужого отверстия;
+- 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`; это не тест нового
+пакета.
+
+## Проверка в Webots
+
+Все 20 опытов выполнены в Webots R2025a в режиме `execute`; каждый завершился
+`completed`, доставил нужное число винтов, установил люк (где он есть), прошёл
+требуемые участки герметика, вернул все инструменты и пришёл домой. Ветви
+отказов 4, 5, 9, 15 и 20 отработали с одной разрешённой попыткой; винты не устанавливаются,
+затяжка выполняется имитационно. Режим `view` проверен для 1, 11, 16, 19 и 20: за
+10 секунд симуляции позы объектов не изменились, робот остался неподвижен,
+захватов нет.
+
+Подробная таблица, численные измерения, смещение TCP в `view`, пути к журналам
+и ограничения приведены в [reference-control-qa.md](reference-control-qa.md).
+Журналы последнего прогона: `/tmp/opencode/traces_all/` и
+`/tmp/opencode/logs_all/`; сводка — `/tmp/opencode/summary.json`.

src/iiwa_experement/doc/experements.md

@@ -0,0 +1,228 @@
+# Эксперименты для LLM-планировщика сборки люка
+
+Цель набора — проверить, как LLM-планировщик преобразует пользовательский запрос и `scene_json` в граф действий `skill` с переходами по событиям. В наборе 20 экспериментов: четыре категории по пять заданий. Сложность внутри каждой категории возрастает.
+
+**Обновление эталона:** по решению заказчика в опытах 14–20 затяжной инструмент `torque_tool` используется для **имитации** затяжки: винт передаётся виртуальному оператору и удаляется, отверстие остаётся пустым, а инструмент подходит к отверстию и выполняет видимый поворот. Винты по-прежнему не вставляются.
+
+Эталонное исполнение этих заданий описано в [спецификации](../../../docs/superpowers/specs/2026-11-08-grant-reference-control-design.md) и [плане](../../../docs/superpowers/plans/2026-11-08-grant-reference-control.md) репозитория. Винты в эталоне не вставляются: робот подносит винт к отверстию и передаёт его виртуальному оператору, отверстие остаётся пустым.
+
+Эксперименты опираются на технологическую карту «ТехнКарта вер. 1.0.xlsx» и модели из `src/iiwa_description/objects/grant`. В сцене работает один робот-манипулятор. Корпус закреплён в стапеле; оператора, мобильной платформы и вспомогательного робота нет. Отверстия для четырёх винтов считаются подготовленными, поскольку модели сверлильного шпинделя среди доступных объектов нет. Во всех экспериментах на роботе постоянно установлен `patron`: рабочие инструменты вставляются в него. После установки люк удерживается виртуальной фиксацией: это позволяет роботу отпустить присоску и сменить рабочий инструмент. Начальные положения объектов описаны отдельно в [experiment-scenes.md](experiment-scenes.md).
+
+## Контракт для экспериментов
+
+В исходном примере доступны `detect`, `approach`, `lower`, `grab`, `lift`, `transfer`, `align`, `release`, `retreat`, `capture`, `verify`, `stop`, `wait_scene_clear`, `inspect_state`, `notify_operator`. План должен вставлять присоску из стойки в постоянный `patron`, если начальное состояние содержит `tool_docked`; перед `grab` требуется контактное `lower`, а перед отпусканием люка в посадочном месте — его опускание до контакта. После отпускания нужен безопасный отвод и возврат. `dock_tool` извлекает рабочий инструмент из `patron`; сам `patron` остаётся на роботе.
+
+Для остальных операций соответствующий `scene_json` расширяется следующими навыками:
+
+| Навык | Действие | Основные исходы |
+| --- | --- | --- |
+| `dispense` | Пройти соплом именованный участок контура с условной подачей герметика | `completed`, `interrupted`, `failed` |
+| `press` | Прижать установленный люк в заданной зоне | `success`, `failed` |
+| `handoff_screw` | Поднести винт к отверстию и передать его виртуальному оператору | `delivered`, `grasp_failed`, `pose_out_of_tolerance` |
+| `dock_tool` | Вернуть инструмент в его гнездо и отсоединить от робота | `success`, `failed` |
+
+Контракты `verify` и `inspect_state` нужно расширить на контур герметика и винты. Для тестов достаточно состояния симулятора; `capture` не входит в основные маршруты. Предлагаемые идентификаторы: `cap`, `hatch_seat`, `body`, `sealant_path`, `side_1`…`side_4`, `hole_1`…`hole_4`, `screw_1`…`screw_10`, `screw_tray`, `patron`, `vacuum`, `sealant_tool`, `magnetic_tool`, `torque_tool`.
+
+Каждый эксперимент начинается в домашней позиции. К объекту робот идёт через безопасную точку над ним; после контакта сначала отходит по оси инструмента. В конце он возвращает инструмент в стойку и идёт домой. Ветви движения при `collision` должны вести к остановке; при неопределённом состоянии нельзя продолжать перенос или отпускание детали. Число повторных попыток задаёт `recovery_attempts_per_component`.
+
+## I. Захват и установка люка
+
+### 1. Базовая установка
+
+**Запрос модели:** «Возьми люк присоской, перенеси на корпус, установи, отпусти и проверь итоговое положение».
+
+**Условия:** люк лежит в известной области; присоска в стойке; посадочное место свободно.
+
+**Действия робота:** найти присоску → подойти и опуститься к гнезду → захватить инструмент → подняться → найти люк → подойти над ним и опуститься до контакта → включить захват → поднять люк → найти посадочное место → перенести люк над ним → совместить → опустить до посадки → отпустить → отвести присоску → проверить положение люка → вернуть присоску в стойку → домой.
+
+**Проверка плана:** работа с деталью не начинается раньше подтверждённого захвата инструмента; `verify` следует за `release`.
+
+### 2. Люк с изменённой исходной ориентацией
+
+**Запрос:** «Установи люк на корпус, даже если он лежит повёрнутым относительно посадочного места».
+
+**Условия:** положение люка обнаруживается во время выполнения; его начальный угол отличается от целевого.
+
+**Действия робота:** взять присоску → обнаружить фактическую позу люка → подойти и захватить его → поднять выше стапеля → перенести в безопасную точку над корпусом → повернуть и совместить с `hatch_seat` через `align` → опустить → отпустить → отвести инструмент → проверить установку → вернуть инструмент → домой.
+
+**Проверка плана:** план использует результат `detect`, а не предполагает исходную ориентацию детали из текста запроса.
+
+### 3. Люк и посадочное место обнаруживаются отдельно
+
+**Запрос:** «Найди люк и его посадочное место, затем установи люк и проверь результат».
+
+**Условия:** обе позы уточняются при выполнении.
+
+**Действия робота:** взять присоску → `detect(cap)` → подойти, опуститься, захватить и поднять люк → `detect(hatch_seat)` → перенести над найденной точкой → совместить → опустить → отпустить → отвести инструмент → проверить → вернуть инструмент → домой.
+
+**Ветви и проверка:** `not_found` у любого обнаружения ведёт к `replan_required`; перенос к посадочному месту не начинается, пока оно не найдено.
+
+### 4. Неудачный первый захват
+
+**Запрос:** «Установи люк; если присоска не удержала его с первого раза, безопасно повтори попытку».
+
+**Условия:** исполнитель один раз возвращает `grasp_failed` при захвате люка; `recovery_attempts_per_component = 1`.
+
+**Действия робота:** взять присоску → найти люк → подойти и опуститься → попытаться захватить. При `grasp_failed`: остановиться → проверить, пуст ли захват и где находится люк → заново обнаружить люк → повторить подход, опускание и захват один раз → поднять, перенести, совместить, установить, отпустить и проверить → вернуть инструмент → домой.
+
+**Проверка плана:** после `grasp_failed` робот не выполняет `lift` вслепую; второй отказ заканчивается остановкой и уведомлением.
+
+### 5. Исправление положения после установки
+
+**Запрос:** «Установи люк и проверь положение. Если он оказался вне допуска, поправь его одной повторной установкой».
+
+**Условия:** первая посадка даёт `verify:not_ok`; люк остаётся доступным для повторного захвата.
+
+**Действия робота:** выполнить установку как в эксперименте 1 → получить `not_ok` → обнаружить фактическое положение люка → подойти присоской, опуститься, снова захватить и поднять → повторно обнаружить посадочное место → перенести, совместить, опустить и отпустить → повторить `verify` → вернуть инструмент → домой при `ok`.
+
+**Проверка плана:** не более одной повторной установки; при неизвестном состоянии детали или второй неуспешной проверке граф заканчивается `operator_required`.
+
+## II. Работа с герметиком
+
+Для этих экспериментов `sealant_path` разбит на именованные участки `side_1`…`side_4`. Герметик условный: проверяются маршрут робота и изменение состояния контура в симуляторе.
+
+### 6. Один прямой участок
+
+**Запрос:** «Нанеси герметик на первый прямой участок посадочного контура корпуса».
+
+**Условия:** инструмент в стойке; корпус закреплён.
+
+**Действия робота:** найти инструмент → подойти, опуститься и взять его → подняться → найти `side_1` → подойти над началом участка → опустить сопло → выполнить `dispense(side_1)` → отвести сопло вверх → вернуть инструмент → домой → проверить, что обработан именно `side_1`.
+
+**Проверка плана:** нет подачи при перемещении от стойки к корпусу.
+
+### 7. Два смежных участка
+
+**Запрос:** «Нанеси непрерывную линию герметика на первую и вторую стороны посадочного контура».
+
+**Условия:** стороны соединяются одним углом.
+
+**Действия робота:** взять инструмент → обнаружить контур → подойти к началу `side_1` → опустить сопло → пройти `side_1` → перейти через угол без отвода от рабочей высоты → пройти `side_2` → остановить подачу → поднять инструмент → вернуть в стойку → домой → проверить оба участка.
+
+**Проверка плана:** план сохраняет порядок смежных участков и не поднимает сопло посреди непрерывной линии.
+
+### 8. Замкнутый контур
+
+**Запрос:** «Нанеси герметик по всему замкнутому контуру посадочного места люка».
+
+**Условия:** четыре стороны доступны инструменту.
+
+**Действия робота:** взять инструмент → найти контур → подойти к выбранному началу → опустить сопло → последовательно выполнить `dispense` для четырёх сторон и углов → завершить контур в точке начала → поднять сопло → вернуть инструмент → домой → проверить полноту контура.
+
+**Проверка плана:** перечислены все четыре участка ровно по одному разу; конечная проверка относится ко всему контуру.
+
+### 9. Прерывание подачи
+
+**Запрос:** «Нанеси герметик по контуру; если подача прервётся, возобнови её с места разрыва и закончи контур».
+
+**Условия:** на третьем участке один раз приходит `dispense:interrupted`.
+
+**Действия робота:** взять инструмент → обработать первые два участка → начать третий → при прерывании остановить действие и отвести сопло → проверить состояние обработанных участков и последнюю подтверждённую точку → снова подойти к началу недостающего отрезка → опустить сопло → закончить третий и четвёртый участки → проверить контур → вернуть инструмент → домой.
+
+**Проверка плана:** повторно не проходится весь контур; неизвестная точка разрыва ведёт к `replan_required`.
+
+### 10. Герметик и установка люка
+
+**Запрос:** «Нанеси герметик по всему посадочному контуру, затем возьми люк присоской и установи его на корпус».
+
+**Условия:** люк лежит отдельно; для его установки после отпускания предусмотрена виртуальная фиксация.
+
+**Действия робота:** взять инструмент герметика → обработать четыре участка → проверить контур → вернуть инструмент → взять присоску → найти и захватить люк → поднять и перенести его над корпусом → совместить с посадочным местом → опустить и прижать → отпустить → проверить положение люка → вернуть присоску → домой.
+
+**Проверка плана:** присоска не берётся, пока инструмент герметика не возвращён; люк не опускается на неполный контур.
+
+## III. Подача винтов виртуальному оператору
+
+В этих сценах люк уже установлен и зафиксирован, а четыре отверстия подготовлены. В лотке всегда находятся десять одинаковых винтов. Для задания можно выбрать любой доступный винт; проверка плана должна требовать нужное число **разных** винтов, а не заранее заданные номера. Магнитный инструмент переносит винт к конкретному отверстию. Винт не вставляется: после подтверждённой позы передачи робот отпускает винт виртуальному оператору, винт удаляется из сцены, а отверстие остаётся пустым. Затяжка выполняется имитационно: в опытах 14–20 берётся затяжной инструмент, подходит к каждому отверстию и выполняет видимый поворот.
+
+### 11. Один винт
+
+**Запрос:** «Возьми винт из лотка и поднеси его к первому отверстию люка для передачи оператору».
+
+**Действия робота:** взять магнитный инструмент → найти один доступный винт в лотке → подойти и опуститься к нему → захватить и поднять → найти `hole_1` → перенести винт над отверстием → совместить оси → коротко опуститься до точки передачи → отпустить винт виртуальному оператору → отвести инструмент по оси → вернуть инструмент → домой.
+
+**Проверка плана:** передача невозможна до подтверждённого захвата винта и достижения позы передачи у нужного отверстия; отверстие после передачи остаётся пустым.
+
+### 12. Два винта по диагонали
+
+**Запрос:** «Передай два винта в диагонально противоположные отверстия люка».
+
+**Действия робота:** взять магнитный инструмент → передать первый винт по цепочке «найти → захватить → поднять → перенести → совместить → опустить → передать → отвести» → подняться над люком → вернуться к лотку → повторить для диагонально противоположного отверстия → проверить обе передачи → вернуть инструмент → домой.
+
+**Проверка плана:** второму винту соответствуют другая ячейка лотка и другое отверстие.
+
+### 13. Три винта в разные отверстия
+
+**Запрос:** «Передай три разных винта в три разных отверстия люка».
+
+**Действия робота:** взять магнитный инструмент → последовательно взять три винта из разных ячеек и передать их в отверстия `hole_1`, `hole_2`, `hole_3`; перед каждым переносом подтвердить захват, перед передачей — достигнутую позу → проверить три пары винт-отверстие → вернуть инструмент → домой.
+
+**Проверка плана:** нет повторного использования ячейки или отверстия; после каждой передачи робот возвращается к лотку с пустым магнитным захватом.
+
+### 14. Четыре винта крестовым порядком
+
+**Запрос:** «Передай четыре винта в отверстия люка в порядке `hole_1 → hole_3 → hole_2 → hole_4`».
+
+**Действия робота:** выполнить четыре передачи в крестовом порядке, каждый раз подтверждая захват нового винта и позу передачи у нужного отверстия → проверить четыре пары винт-отверстие → вернуть магнитный инструмент → домой.
+
+**Проверка плана:** каждый винт используется ровно один раз; затяжной инструмент не берётся.
+
+### 15. Ошибка захвата винта
+
+**Запрос:** «Передай четыре винта; если один винт не захватится с первого раза, проверь состояние и повтори захват один раз».
+
+**Условия:** первый захват третьего выбранного винта даёт `grasp_failed`; разрешена одна повторная попытка.
+
+**Действия робота:** передать первые два винта → на третьем остановиться, проверить состояние захвата и повторно обнаружить винт → повторить его захват один раз → передать третий и четвёртый винты → проверить четыре пары → вернуть инструмент → домой.
+
+**Проверка плана:** ошибка обрабатывается у соответствующего винта; уже переданные винты не возвращаются в сцену; повторный отказ ведёт к остановке.
+
+## IV. Полная сборка и перепланирование
+
+### 16. Герметик, люк и один винт
+
+**Запрос:** «Нанеси герметик на заданный участок, установи люк и передай один винт к отверстию».
+
+**Действия робота:** взять инструмент герметика → обработать участок → вернуть инструмент → взять присоску → захватить, перенести, совместить, опустить и отпустить люк → проверить его положение → вернуть присоску → взять магнитный инструмент → перенести один винт к отверстию и передать оператору → вернуть инструмент → домой.
+
+**Проверка плана:** зависимости «герметик → люк → винт» соблюдены; винт не устанавливается.
+
+### 17. Полный контур и две передачи
+
+**Запрос:** «Нанеси полный контур герметика, установи люк и передай два диагональных винта».
+
+**Действия робота:** пройти четыре участка герметика и проверить контур → вернуть инструмент → присоской установить и проверить люк → вернуть присоску → магнитным инструментом передать два диагональных винта → проверить две пары → вернуть инструмент → домой.
+
+**Проверка плана:** если контур не подтверждён, план не переходит к установке люка.
+
+### 18. Полная сборка
+
+**Запрос:** «Нанеси герметик по контуру, установи люк и передай четыре винта крестовым порядком».
+
+**Действия робота:** полный контур герметика → проверка → смена на присоску → захват и установка люка → проверка → смена на магнит → четыре передачи винтов → итоговая проверка люка и пар винт-отверстие → вернуть инструмент → домой.
+
+**Проверка плана:** у каждого из четырёх винтов есть собственные узлы захвата и передачи; итоговая проверка охватывает всю сборку.
+
+### 19. Полная сборка при смещённых деталях
+
+**Запрос:** «Собери узел полностью. Люк и лоток могут лежать не в номинальных местах — используй их фактическое положение».
+
+**Условия:** симулятор смещает люк и лоток в допустимой рабочей области; `detect` возвращает их актуальные позы.
+
+**Действия робота:** нанести и проверить герметик → обнаружить фактическую позу люка, захватить и установить его → проверить → обнаружить фактическую позу лотка и по очереди найти четыре винта → передать их в четыре отверстия → проверить сборку → вернуть инструмент → домой.
+
+**Проверка плана:** план не подставляет прежние координаты люка и винтов после нового `detect`; зависимости операций сохраняются.
+
+### 20. Полная сборка с несколькими сбоями
+
+**Запрос:** «Выполни полную сборку. При прерывании подачи герметика, неудачном захвате винта или ошибке установки люка восстанови работу, если состояние известно и разрешена одна повторная попытка».
+
+**Условия:** один раз прерывается `dispense` на `side_3`, первый захват одного винта неудачен, первая проверка положения люка возвращает `not_ok`.
+
+**Действия робота:** начать контур → определить необработанный отрезок и завершить его с последней подтверждённой точки → проверить контур → установить люк → при `not_ok` заново обнаружить и один раз переставить его → проверить → передавать винты; при `grasp_failed` остановиться, проверить состояние и один раз повторить захват нужного винта → проверить четыре пары винт-отверстие и положение люка → вернуть инструмент → домой.
+
+**Проверка плана:** восстановление начинается от подтверждённого состояния; лимит попыток соблюдён; любое `unknown` или повторный отказ ведёт к остановке и уведомлению.
+
+## Общие критерии оценки графа
+
+Для каждого ответа планировщика следует проверить существование всех узлов, указанных в `next`; соблюдение зависимостей операций; отсутствие физически невозможных переходов, например `transfer` без подтверждённого захвата; корректное завершение аварийных ветвей; соблюдение лимита повторных попыток. Семантическая проверка графа полезнее посимвольного сравнения с одним образцом JSON.

src/iiwa_experement/doc/experiment-scenes.md

@@ -0,0 +1,93 @@
+# Начальная сцена для каждого эксперимента
+
+Этот документ задаёт положения объектов для 20 экспериментов из [experements.md](experements.md). Единицы измерения — метры; координаты даны в мировой системе Webots. Это проект начальной компоновки для будущего `src/iiwa_description/worlds/grant/iiwa-grant.wbt`, а не описание уже созданного мира.
+
+## Основа мира и постоянные элементы
+
+Базовый мир — `src/iiwa_description/worlds/iiwa.wbt`. В нём есть стол, ограждения и освещение. Робот появляется через запуск в точке `(-0.25, 0, 0.79)`, с нулевым поворотом вокруг Z. Верх столешницы находится на `Z = 0.793`; её рабочая область по столешнице приблизительно `X = −0.64…0.64`, `Y = −0.54…0.54`. Для точки в мировой системе: `X_base = X_world + 0.25`, `Y_base = Y_world`, `Z_base = Z_world − 0.79`.
+
+`patron` **всегда остаётся на роботе**. В начале каждого эксперимента в него не вставлен ни один из четырёх рабочих инструментов. Робот берёт нужный инструмент из стойки и вставляет в `patron`; при возврате извлекает только рабочий инструмент. Отсутствие рабочего инструмента не означает отсутствие `patron`.
+
+| Объект | Центр `(X, Y, Z)` | Ориентация и состояние |
+| --- | --- | --- |
+| Стапель | `(0.30, 0.12, 0.803)` | Вертикальная ось OBJ переведена в Z Webots, затем поворот 45° вокруг Z; закреплён |
+| Корпус | `(0.30, 0.12, 0.823)` | Та же ориентация; закреплён на опорах стапеля |
+| Посадочное место `hatch_seat` | Центр `(0.30, 0.12)` | Поза связана с корпусом; устанавливаемый люк имеет начало в `(0.30, 0.12, 0.82594)` |
+| Свободный люк, стандартная поза | `(0.35, −0.22, 0.790667)` | Поворот 45°; лежит отдельно на столе |
+| Стойка инструментов | `(0.12, −0.42, 0.793)` | Фиксированная стойка с отдельными инструментами в гнёздах |
+| Лоток винтов | `(0.20, 0.46, 0.793)` | Длинная сторона вдоль мировой оси X; нижняя поверхность на столе; у левого края стола, впереди слева от стойки |
+
+Стойка и корпус присутствуют во всех сценах. Свободный люк, уже установленный люк, лоток, винты и рабочие инструменты появляются по таблице ниже. Установленный люк виртуально фиксируется после отпускания присоски. В опытах с крепежом он уже установлен и зафиксирован при старте.
+
+### Гнёзда инструментов
+
+Положения соответствуют четырём гнёздам модели `tool-magizine.obj` при указанной позе стойки. Нужные для опыта инструменты появляются как отдельные объекты, доступные для извлечения и вставки в `patron`.
+
+| Инструмент | Центр гнезда `(X, Y, Z)` |
+| --- | --- |
+| Присоска `vacuum` | `(0.255, −0.42, 0.803)` |
+| Магнитный `magnetic_tool` | `(0.165, −0.42, 0.803)` |
+| Герметик `sealant_tool` | `(0.075, −0.42, 0.803)` |
+| Затяжка `torque_tool` | `(−0.015, −0.42, 0.803)` |
+
+### Десять винтов в лотке
+
+Во **всех** экспериментах с лотком при старте в нём находятся десять одинаковых винтов. Даже если задача требует установить только один, два или четыре, остальные остаются на месте и усложняют поиск и выбор. Винты стоят вертикально; их нижняя точка запланирована на `Z = 0.7954`, у внутреннего дна лотка. Центры соседних винтов разнесены на 20 мм вдоль X и 30 мм между рядами Y. Четыре первых номера размещены по углам сетки, остальные заполняют промежутки.
+
+| Винт | `(X, Y, Z)` | Винт | `(X, Y, Z)` |
+| --- | --- | --- | --- |
+| `screw_1` | `(0.160, 0.445, 0.7954)` | `screw_6` | `(0.200, 0.445, 0.7954)` |
+| `screw_2` | `(0.240, 0.445, 0.7954)` | `screw_7` | `(0.220, 0.445, 0.7954)` |
+| `screw_3` | `(0.160, 0.475, 0.7954)` | `screw_8` | `(0.180, 0.475, 0.7954)` |
+| `screw_4` | `(0.240, 0.475, 0.7954)` | `screw_9` | `(0.200, 0.475, 0.7954)` |
+| `screw_5` | `(0.180, 0.445, 0.7954)` | `screw_10` | `(0.220, 0.475, 0.7954)` |
+
+Все десять винтов взаимозаменяемы. Для передачи к отверстиям планировщик может выбрать **любые разные доступные винты** в нужном количестве. Оценка графа не должна требовать конкретные номера `screw_i`. В опыте 15 отказ задаётся первой попытке захвата винта, который план выбрал третьим. Винты не вставляются: отверстия после передачи остаются пустыми, а затяжка в опытах 14–20 имитируется затяжным инструментом.
+
+При стандартной позе корпуса предварительные центры отверстий, полученные из геометрии люка, таковы:
+
+| Отверстие | `(X, Y)` | Отверстие | `(X, Y)` |
+| --- | --- | --- | --- |
+| `hole_1` | `(0.2243, 0.1052)` | `hole_3` | `(0.3757, 0.1348)` |
+| `hole_2` | `(0.2852, 0.0443)` | `hole_4` | `(0.3148, 0.1957)` |
+
+Противоположные пары — `hole_1`/`hole_3` и `hole_2`/`hole_4`; крестовой порядок: `hole_1 → hole_3 → hole_2 → hole_4`. Точную ось посадки винта и высоту головки нужно проверить в Webots перед построением контактной траектории.
+
+## Размещение по экспериментам
+
+В таблице «стандартный корпус» означает стапель `(0.30, 0.12, 0.803)` и корпус `(0.30, 0.12, 0.823)` из раздела выше. Указанные инструменты находятся в своих гнёздах; `patron` на роботе во всех строках. Если есть лоток, в нём **всегда десять винтов** в точках из предыдущей таблицы. Герметик до начала опыта не нанесён.
+
+| № | Стапель и корпус | Люк при старте | Лоток и винты | Рабочие инструменты в стойке | Отличие сценария |
+| ---: | --- | --- | --- | --- | --- |
+| 1 | Стандартные | Свободный `(0.35, −0.22, 0.790667)`, 45° | Нет | Присоска | Базовая установка |
+| 2 | Стандартные | Свободный `(0.35, −0.22, 0.790667)`, **135°** | Нет | Присоска | Люк нужно повернуть на 90° при переносе |
+| 3 | Стапель `(0.29, 0.14, 0.803)`, корпус `(0.29, 0.14, 0.823)` | Свободный `(0.33, −0.22, 0.790667)`, 45° | Нет | Присоска | Посадочное место смещено вместе с корпусом; целевой люк `(0.29, 0.14, 0.82594)` |
+| 4 | Стандартные | Стандартный свободный | Нет | Присоска | Первая попытка захвата даст `grasp_failed` |
+| 5 | Стандартные | Стандартный свободный | Нет | Присоска | После первой установки люк окажется в `(0.303, 0.12, 0.82594)` и проверка даст `not_ok` |
+| 6 | Стандартные | Нет | Нет | Герметик | Обрабатывается `side_1` |
+| 7 | Стандартные | Нет | Нет | Герметик | Обрабатываются смежные `side_1`, `side_2` |
+| 8 | Стандартные | Нет | Нет | Герметик | Обрабатывается полный `sealant_path` |
+| 9 | Стандартные | Нет | Нет | Герметик | На `side_3` при выполнении прервётся подача |
+| 10 | Стандартные | Стандартный свободный | Нет | Герметик, присоска | Герметик, затем установка люка |
+| 11 | Стандартные | Уже установлен `(0.30, 0.12, 0.82594)`, 45° | Стандартный лоток, **10 винтов** | Магнитный | Передать один винт у `hole_1` |
+| 12 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Передать два винта в диагональные отверстия |
+| 13 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Передать три разных винта в `hole_1`, `hole_2`, `hole_3` |
+| 14 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный, затяжной | Четыре передачи в порядке `hole_1 → hole_3 → hole_2 → hole_4`; имитация затяжки |
+| 15 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный, затяжной | Те же четыре передачи и имитация затяжки; сбой первого захвата третьего винта |
+| 16 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик, затяжной | Участок герметика, люк, одна передача и имитация затяжки |
+| 17 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик, затяжной | Полный контур, люк, две диагональные передачи и имитация затяжки |
+| 18 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик, затяжной | Полная сборка, четыре передачи и имитация затяжки |
+| 19 | Стандартные | Свободный `(0.37, −0.20, 0.790667)`, **60°** | Лоток `(0.18, 0.48, 0.793)`, **10 винтов** | Присоска, магнитный, герметик, затяжной | Полная сборка; смещение лотка и всех винтов; имитация затяжки |
+| 20 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик, затяжной | Полная сборка; прерывание герметика, ошибка первой посадки люка, неудачный захват винта; имитация затяжки |
+
+В опыте 3 смещается вся пара «стапель + корпус» без изменения взаимной высоты и поворота. Координаты `hatch_seat` и четырёх отверстий сдвигаются вместе с ними на `(−0.01, +0.02)`. В опыте 19 сдвигается лоток **со всеми десятью винтами**; ни один винт не остаётся в старой точке.
+
+Опыты 4, 5, 9, 15 и 20 требуют также управляемых событий исполнителя. Эти события и последующие изменения положения детали не являются дополнительными объектами начальной сцены. Герметик удобнее учитывать как состояние именованных участков контура, а не как заранее лежащую на корпусе модель.
+
+## Геометрические и технические ограничения
+
+По габаритам OBJ стапель занимает в плане примерно `X = 0.0985…0.5015`, `Y = −0.0815…0.3215`; стандартный свободный люк — `X = 0.2663…0.4337`, `Y = −0.3037…−0.1363`; стойка — `X = −0.09…0.33`, `Y = −0.49…−0.35`; лоток — `X = 0.14…0.26`, `Y = 0.425…0.495`. Эти прямоугольники не пересекаются. Лоток перенесён к левому краю стола со стороны `+Y`, чтобы все винты были в достижимой зоне; зазор от лотка до стапеля по Y около 104 мм. Зазор от свободного люка до стойки по Y около 46 мм, до стапеля около 55 мм. Это проверка начальной компоновки, не проверка траекторий инструмента.
+
+Перед реализацией контактных действий нужно проверить в Webots достижимость TCP и коллизии при подъёме люка, при извлечении инструмента из гнезда и при работе между десятью винтами. В `iiwa.wbt` трение `TableMaterial` равно нулю; лоток и винты понадобится удерживать до захвата или задать подходящую контактную модель. Действующий спавнер `object_spawner.py` создаёт случайные OBJ, поэтому для этой таблицы потребуется детерминированная загрузка по номеру эксперимента. Стойка должна содержать отдельно управляемые рабочие инструменты; удалённый `GrantToolRack.proto` для нового мира не используется.
+
+В эталонных маршрутах 14–20 дополнительно используется `torque_tool` для имитации затяжки: винт не вставляется, отверстие остаётся пустым, инструмент выполняет видимый поворот у отверстия и возвращается в стойку. Численные преобразования, допуски и контур герметика описаны в [геометрии](../config/geometry.yaml) и [спецификации](../../../docs/superpowers/specs/2026-11-08-grant-reference-control-design.md).

src/iiwa_experement/doc/reference-control-qa.md

@@ -0,0 +1,224 @@
+# Проверка эталонного управления
+
+Документ фиксирует измерения, настройки безопасности и результаты прогонов
+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,786 м. Меш коллизии
+  `base_link` доходит до 2,05 мм ниже фрейма (мир: Z = 0,788); прежний верх
+  0,789 м давал постоянное проникновение на 1 мм, которое детектор контактов
+  замечал нестабильно и блокировал планирование (опыт 2, отказ
+  `place_cap:retreat`).
+- Стапель: нижняя плита 0,35 × 0,22 × 0,01 м и две боковые направляющие
+  0,014 × 0,156 × 0,016 м. Сплошной блок не используется, иначе он перекрывает
+  посадочное место люка.
+- Корпус: 0,30 × 0,17 × 0,00915 м; люк: 0,1384 × 0,0984 × 0,00367 м.
+  Бокс люка поднят на 1 мм над боксом корпуса (`CAP_BOX_LIFT`), бокс винта —
+  на 1 мм над дном лотка (`SCREW_BOX_LIFT`): касание с нулевым зазором
+  считается контактом сцены и блокирует все последующие планы из этого
+  состояния (например, `grant_body - grant_cap` после посадки люка).
+- Стойка: плита 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 м; затяжной — 0,049 × 0,049 × 0,14 м.
+- ACM строится из `iiwa7.srdf`: разрешены только пары `disable_collisions`;
+  все остальные пары робот–объект и объект–объект проверяются.
+
+## Политика движений
+
+- После захвата инструмент поднимается строго вертикально (8 см, при отказе
+  планировщика — 6/3/1,5 см), затем смещается по +Y от стойки с достигнутой
+  высоты, и только потом выполняется суставное перемещение. Подъём
+  обязателен: если все осевые участки отклонены, операция останавливается,
+  а не продолжается боковым движением на глубине контакта.
+- Маршрут начинается с перехода в `work` и заканчивается `work` → `home`.
+  `home` используется только как финальная поза. Между операциями робот
+  планирует прямо из текущей рабочей позы; повторные `work` не вставляются.
+- Если планировщик отклоняет подход из текущей ветви (дальние ячейки лотка,
+  отдельные гнёзда стойки), операция один раз переходит в `work` и повторяет
+  подход. Событие `reconfigure` с причиной пишется в журнал. Поза
+  `transport` в эталонных маршрутах запрещена и не используется нигде,
+  включая аварийные ветви.
+- После каждого контакта (передача винта, имитация затяжки, захват и посадка
+  люка, контур герметика) выполняется обязательный осевой подъём до
+  горизонтального перемещения. Захваченные инструмент, люк и винт
+  прикрепляются к роботу в сцене MoveIt (`bridge.attach`), поэтому перенос
+  планируется с учётом переносимого тела.
+- Трансферы и подходы планируются Pilz PTP — детерминированная суставная
+  интерполяция вместо случайных дуг OMPL RRTConnect. Короткие осевые
+  участки — Pilz LIN. OMPL остаётся только автоматическим запасным
+  вариантом, если PTP не нашёл решение. Суставные цели (`move_to_joints`:
+  переходы в `work`/`home`, поворот фланца) также идут через PTP с
+  откатом на OMPL при отказе.
+- TCP при работе направлен вниз; затяжной инструмент вращается вокруг своей
+  оси, не меняя точку контакта. После поворота выполняется подъём, и только
+  затем фланец возвращается в исходную ориентацию (untwist): раскрутка на
+  месте отверстия визуально откручивала бы имитацию.
+- Положение после суставного перемещения проверяется по установившимся
+  показаниям `/joint_states`, а не по первому отсчёту после ответа контроллера.
+  При кратковременном отклонении цель повторяется один раз.
+
+## Результаты тестов
+
+Финальный прогон: `colcon test` — 237 тестов (209 `iiwa_experement`, 28
+`iiwa_planning`), 0 ошибок, 0 падений. Тесты покрывают контракт сцен,
+геометрию, supervisor, сцену MoveIt (включая явное снятие прикреплённых
+объектов), движение, операции, маршруты и журнал.
+
+## Результаты 20 опытов
+
+> Повторная верификация после правок (запрет `transport`, обязательный
+> осевой подъём, untwist фланца, прикрепление переносимых объектов, честные
+> позы боксов, зазоры боксов, rest-to-rest) выполнена 10.11.2026: все 20
+> опытов завершились со статусом `completed` и `home_reached=true` в режиме
+> `execute`. Журналы: `~/.ros/iiwa_experement/runs/experiment_<id>_20261110T*.jsonl`.
+
+Каждый сценарий должен хотя бы один раз завершиться со статусом `completed`.
+При транзиентном отказе планировщика (`Планирование не удалось`) запуск
+повторялся с чистой сценой; таблица показывает подтверждённый успешный прогон.
+Внутри одного запуска маршрут по-прежнему останавливается на первом отказе.
+
+| № | Статус | Доставлено | Люк | Герметик | Инструмент возвращён | Дом |
+| ---: | --- | --- | --- | --- | --- | --- |
+| 1 | completed | — | ok | — | vacuum | да |
+| 2 | completed | — | ok | — | vacuum | да |
+| 3 | completed | — | ok | — | vacuum | да |
+| 4 | completed | — | ok | — | vacuum | да |
+| 5 | completed | — | ok | — | vacuum | да |
+| 6 | completed | — | — | ok | sealant_tool | да |
+| 7 | completed | — | — | ok | sealant_tool | да |
+| 8 | completed | — | — | ok | sealant_tool | да |
+| 9 | completed | — | — | ok | sealant_tool | да |
+| 10 | completed | — | ok | ok | sealant_tool, vacuum | да |
+| 11 | completed | screw_1→hole_1 | ok | — | magnetic_tool | да |
+| 12 | completed | screw_1→hole_1, screw_2→hole_3 | ok | — | magnetic_tool | да |
+| 13 | completed | screw_1→hole_1, screw_2→hole_2, screw_5→hole_3 | ok | — | magnetic_tool | да |
+| 14 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | — | magnetic_tool, torque_tool | да |
+| 15 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | — | magnetic_tool, torque_tool | да |
+| 16 | completed | screw_1→hole_1 | ok | ok | sealant_tool, vacuum, magnetic_tool, torque_tool | да |
+| 17 | completed | screw_1→hole_1, screw_2→hole_3 | ok | ok | sealant_tool, vacuum, magnetic_tool, torque_tool | да |
+| 18 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | ok | sealant_tool, vacuum, magnetic_tool, torque_tool | да |
+| 19 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | ok | sealant_tool, vacuum, magnetic_tool, torque_tool | да |
+| 20 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | ok | sealant_tool, vacuum, magnetic_tool, torque_tool | да |
+
+## Режим view
+
+Проверены опыты 1, 11, 16, 19, 20. После 10 секунд симуляции:
+
+| Опыт | Объектов | Винтов | Люк | TCP, смещение за 10 с | Захват |
+| ---: | ---: | ---: | --- | ---: | --- |
+| 1 | 5 | 0 | свободный | 0,11 мм (успокоение контроллера) | нет |
+| 11 | 16 | 10 | установлен | 0,001 мм | нет |
+| 16 | 19 | 10 | свободный | 0,0007 мм | нет |
+| 19 | 19 | 10 | свободный, смещён | 0,0006 мм | нет |
+| 20 | 19 | 10 | свободный | 0,0007 мм | нет |
+
+Позы объектов за 10 секунд не изменились. `mounted_tool=null`, `attached.tool=null`,
+`attached.payload=null`. Журналы: `/tmp/opencode/view_01.log`, `view_11.log`,
+`view_16.log`, `view_19.log`, `view_20.log`.
+
+## Журналы прогонов
+
+- JSONL: `/tmp/opencode/traces_all/experiment_<id>_<UTC>.jsonl`.
+- Логи запуска: `/tmp/opencode/logs_all/experiment_<id>.log`.
+- Сводка: `/tmp/opencode/summary.json`.
+- В каждом успешном журнале последняя строка `event=result` содержит
+  `status=completed`, пары винт-отверстие, `cap.ok=true` (где люк есть),
+  `sealant.ok=true` (где контур есть), `returned_tools` и `home_reached=true`.
+
+
+
+## Имитация затяжки
+
+В опытах 14, 15 и 16–20 после передач винтов берётся `torque_tool`. Для
+каждого отверстия выполняется `tighten_simulated`: подъём над отверстием,
+осевое опускание и поворот последнего сустава на 45°. Вращается только
+фланец, поэтому инструмент поворачивается вокруг своей оси, не уходя в другую
+ветвь IK. Если положительное направление упирается в предел, используется
+отрицательное. После поворота фланец возвращается в исходную конфигурацию
+(untwist), затем выполняется обязательный осевой подъём из отверстия, как
+после передачи винта. Отверстие остаётся пустым; винт не вставляется.
+Инструмент возвращается в стойку, затем робот уходит домой.
+
+## Ветви отказов
+
+- 4: первый захват люка не выполняется, состояние проверяется, затем одна
+  повторная попытка.
+- 5: первая посадка смещена на 3 мм, `inspect_cap` возвращает `not_ok`,
+  следует повторный захват и одна корректная посадка.
+- 9: `side_3` прерывается на половине; состояние проверяется, движение
+  продолжается с подтверждённой точки, а не с начала контура.
+- 15: третий выбранный винт (`screw_5`) не захватывается с первого раза;
+  следует проверка состояния и одна повторная попытка.
+- 20: прерывание герметика, ошибка первой посадки люка и неудачный захват
+  первого винта; каждая ветвь имеет не более одной попытки.
+
+## Ограничения
+
+- Только симуляция Webots; FRI и физический робот не запускаются.
+- Винты не вставляются; после передачи отверстие остаётся пустым.
+- Затяжка имитационная: `torque_tool` подходит к отверстию, поворачивается на
+  45° вокруг оси инструмента и отходит. Винт к этому моменту уже передан
+  оператору и удалён из сцены.
+- Прикреплённые объекты MoveIt моделируются: захваченные инструмент, люк и
+  винт переносятся из мировой сцены в `attached_collision_objects` с
+  разрешённым контактом звеньев `tcp`, `patron`, `link7` и держателя камеры.
+  Прикреплённые боксы люка и винта укорочены так, чтобы на позе захвата их
+  низ оставался на 1 мм выше опорной поверхности (корпус/стол/лоток): касание
+  с нулевым зазором считается контактом стартового состояния и блокирует
+  обязательный осевой подъём.
+- Поза бокса при возврате объекта в мир — это поза **центра бокса** с
+  плоской yaw-ориентацией (конвенция `build_world_objects`), а не поза узла
+  OBJ с поворотом Rx90. Док инструмента и восстановление винта/люка после
+  имитированного срыва используют `bridge.restore_world` с калиброванной
+  начальной позой; поставленный люк добавляется через `placed_cap_pose`.
+  Нарушение этой конвенции было причиной «ложных столкновений» (поставленный
+  люк становился вертикальной плитой, задокованный инструмент — лежачим).
+- MoveIt/MoveItPy может аварийно завершиться после очень длинной серии
+  планирований; один запуск выполняет один опыт, поэтому серия ограничена.
+- `Node.remove()` для винта в Webots R2025a иногда приводит к падению
+  симулятора; переданный винт телепортируется далеко за арену и больше не
+  запрашивается.

src/iiwa_experement/iiwa_experement/__init__.py

@@ -0,0 +1 @@
+"""Deterministic Webots scenes for the grant experiments."""

src/iiwa_experement/iiwa_experement/geometry.py

@@ -0,0 +1,547 @@
+"""Calibrated transforms, working tips and scene collision models.
+
+This module is pure Python (numpy/scipy only) so the same numbers are used by
+the unit tests, the MoveIt bridge, the motion adapter and the Webots
+supervisor.  Nothing here talks to ROS or Webots.
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any, Mapping
+
+import numpy as np
+import yaml
+from scipy.spatial.transform import Rotation
+
+from .scenario import SCREW_HOLES, SEALANT_SIDES, ScenarioSpec, Tool
+
+
+TOOL_GEOMETRY_KEYS = ('vacuum', 'magnetic_tool', 'sealant_tool', 'torque_tool')
+_SEALANT_SIDES = SEALANT_SIDES
+
+
+class GeometryError(ValueError):
+    """Raised when the geometry file is missing or inconsistent."""
+
+
+def _number(value: Any, label: str) -> float:
+    if isinstance(value, bool) or not isinstance(value, (int, float)):
+        raise GeometryError(f'{label} must be a finite number')
+    result = float(value)
+    if not np.isfinite(result):
+        raise GeometryError(f'{label} must be a finite number')
+    return result
+
+
+def _vector(value: Any, label: str, length: int) -> tuple[float, ...]:
+    if not isinstance(value, (list, tuple)) or len(value) != length:
+        raise GeometryError(f'{label} must be a list of {length} numbers')
+    return tuple(_number(item, f'{label}[{index}]') for index, item in enumerate(value))
+
+
+def _mapping(value: Any, label: str) -> Mapping[str, Any]:
+    if not isinstance(value, Mapping):
+        raise GeometryError(f'{label} must be a mapping')
+    return value
+
+
+@dataclass(frozen=True)
+class Box:
+    """Axis-aligned box in a named frame."""
+
+    size: tuple[float, float, float]
+    offset: tuple[float, float, float] = (0.0, 0.0, 0.0)
+
+    @property
+    def half_extents(self) -> tuple[float, float, float]:
+        return tuple(value / 2.0 for value in self.size)
+
+
+@dataclass(frozen=True)
+class Transform:
+    """Rigid transform ``parent -> child``.
+
+    ``rotation`` is a 3x3 row-major matrix.  Composition follows the usual
+    convention: ``parent_to_child.compose(child_to_grandchild)``.
+    """
+
+    xyz: tuple[float, float, float]
+    rotation: tuple[tuple[float, float, float], ...]
+    parent: str
+    child: str
+
+    @classmethod
+    def from_xyz_rpy(
+        cls,
+        xyz,
+        rpy_deg,
+        *,
+        parent: str,
+        child: str,
+    ) -> 'Transform':
+        values = _vector(xyz, f'{parent}->{child}.xyz', 3)
+        angles = _vector(rpy_deg, f'{parent}->{child}.rpy_deg', 3)
+        matrix = Rotation.from_euler('xyz', angles, degrees=True).as_matrix()
+        return cls(
+            xyz=values,
+            rotation=tuple(tuple(float(v) for v in row) for row in matrix),
+            parent=parent,
+            child=child,
+        )
+
+    @property
+    def matrix(self) -> np.ndarray:
+        return np.asarray(self.rotation, dtype=float)
+
+    @property
+    def position(self) -> np.ndarray:
+        return np.asarray(self.xyz, dtype=float)
+
+    def compose_relaxed(self, other: 'Transform') -> 'Transform':
+        """Compose two transforms whose frame labels are documentation only."""
+        rotation = self.matrix @ other.matrix
+        translation = self.position + self.matrix @ other.position
+        return Transform(
+            xyz=tuple(float(v) for v in translation),
+            rotation=tuple(tuple(float(v) for v in row) for row in rotation),
+            parent=self.parent,
+            child=other.child,
+        )
+
+    def compose(self, other: 'Transform') -> 'Transform':
+        if self.child != other.parent:
+            raise GeometryError(
+                f'cannot compose {self.parent}->{self.child} with '
+                f'{other.parent}->{other.child}'
+            )
+        rotation = self.matrix @ other.matrix
+        translation = self.position + self.matrix @ other.position
+        return Transform(
+            xyz=tuple(float(v) for v in translation),
+            rotation=tuple(tuple(float(v) for v in row) for row in rotation),
+            parent=self.parent,
+            child=other.child,
+        )
+
+    def inverse(self) -> 'Transform':
+        rotation = self.matrix.T
+        translation = -rotation @ self.position
+        return Transform(
+            xyz=tuple(float(v) for v in translation),
+            rotation=tuple(tuple(float(v) for v in row) for row in rotation),
+            parent=self.child,
+            child=self.parent,
+        )
+
+    def apply(self, point) -> tuple[float, float, float]:
+        result = self.matrix @ np.asarray(point, dtype=float) + self.position
+        return tuple(float(v) for v in result)
+
+    def distance_to(self, other: 'Transform') -> float:
+        if self.parent != other.parent:
+            raise GeometryError('cannot compare transforms in different parents')
+        return float(np.linalg.norm(self.position - other.position))
+
+    def angle_to(self, other: 'Transform', *, parent: str | None = None) -> float:
+        matrix = self.matrix
+        other_matrix = other.matrix
+        if parent is not None:
+            matrix = _frame_matrix(self, parent) @ matrix
+            other_matrix = _frame_matrix(other, parent) @ other_matrix
+        cosine = (np.trace(matrix.T @ other_matrix) - 1.0) / 2.0
+        return float(np.arccos(max(-1.0, min(1.0, cosine))))
+
+
+def _frame_matrix(transform: Transform, frame: str) -> np.ndarray:
+    # Kept small and explicit: only world/base_link are ever needed here.
+    del transform
+    if frame in ('base_link', 'world'):
+        return np.eye(3)
+    raise GeometryError(f'unsupported comparison frame {frame!r}')
+
+
+@dataclass(frozen=True)
+class HoleSpec:
+    name: str
+    dx: float
+    dy: float
+
+
+@dataclass(frozen=True)
+class SegmentSpec:
+    name: str
+    start: tuple[float, float]
+    end: tuple[float, float]
+
+
+@dataclass(frozen=True)
+class GeometrySpec:
+    world_to_base: Transform
+    patron_to_tcp: Transform
+    tool_mounts: dict[str, Transform]
+    tool_work_tips: dict[str, Transform]
+    tool_tcp_to_nodes: dict[str, Transform]
+    tool_collisions: dict[str, Box]
+    cap_tcp_to_node: Transform
+    cap_collision: Box
+    body_to_installed_cap: Transform
+    screw_tcp_to_node: Transform
+    screw_collision: Box
+    holes: dict[str, HoleSpec]
+    hole_reference_yaw_deg: float
+    hole_z_above_body_origin: float
+    handoff_gap: float
+    sealant_reference_yaw_deg: float
+    sealant_sides: dict[str, SegmentSpec]
+    sealant_z_above_body_top: float
+    approach_gap: float
+    contact_gap: float
+    transfer_gap: float
+    seat_gap: float
+    position_tolerance: float
+    orientation_tolerance_rad: float
+    speeds: dict[str, float]
+    scene_collisions: dict[str, Box]
+    safe_poses: dict[str, Transform]
+    named_joints: dict[str, tuple[float, ...]]
+
+    @property
+    def body_top_local(self) -> float:
+        body = self.scene_collisions['body']
+        return body.offset[2] + body.size[2] / 2.0
+
+
+def _load_transform(raw: Any, label: str, parent: str, child: str) -> Transform:
+    data = _mapping(raw, label)
+    return Transform.from_xyz_rpy(
+        data.get('xyz', [0.0, 0.0, 0.0]),
+        data.get('rpy_deg', [0.0, 0.0, 0.0]),
+        parent=parent,
+        child=child,
+    )
+
+
+def _load_box(raw: Any, label: str) -> Box:
+    data = _mapping(raw, label)
+    size = _vector(data.get('size'), f'{label}.size', 3)
+    offset = _vector(data.get('offset', [0.0, 0.0, 0.0]), f'{label}.offset', 3)
+    if any(value <= 0.0 for value in size):
+        raise GeometryError(f'{label}.size must be positive')
+    return Box(size=size, offset=offset)
+
+
+def load_geometry(path: Path | str) -> GeometrySpec:
+    """Load and validate ``config/geometry.yaml``."""
+    source = Path(path)
+    if not source.is_file():
+        raise FileNotFoundError(f'geometry file does not exist: {source}')
+    try:
+        raw = yaml.safe_load(source.read_text(encoding='utf-8'))
+    except yaml.YAMLError as error:
+        raise GeometryError(f'invalid YAML in {source}: {error}') from error
+    data = _mapping(raw, 'geometry')
+    if data.get('schema_version') != 1:
+        raise GeometryError('geometry schema_version must be 1')
+
+    world_to_base = _load_transform(
+        data.get('world_to_base'), 'world_to_base', 'world', 'base_link',
+    )
+    patron_to_tcp = _load_transform(
+        data.get('patron_to_tcp'), 'patron_to_tcp', 'patron', 'tcp',
+    )
+
+    tool_mounts_raw = _mapping(data.get('tool_mounts'), 'tool_mounts')
+    tool_tips_raw = _mapping(data.get('tool_work_tips'), 'tool_work_tips')
+    tool_nodes_raw = _mapping(data.get('tool_tcp_to_nodes'), 'tool_tcp_to_nodes')
+    tool_collisions_raw = _mapping(data.get('tool_collisions'), 'tool_collisions')
+    missing = [name for name in TOOL_GEOMETRY_KEYS if name not in tool_mounts_raw]
+    if missing:
+        raise GeometryError(f'tool_mounts is missing {missing}')
+    tool_mounts = {
+        name: _load_transform(
+            tool_mounts_raw[name], f'tool_mounts.{name}', 'tcp', f'{name}_mount',
+        )
+        for name in TOOL_GEOMETRY_KEYS
+    }
+    tool_work_tips = {
+        name: _load_transform(
+            tool_tips_raw[name], f'tool_work_tips.{name}', 'tcp', f'{name}_tip',
+        )
+        for name in TOOL_GEOMETRY_KEYS
+    }
+    tool_tcp_to_nodes = {
+        name: _load_transform(
+            tool_nodes_raw[name], f'tool_tcp_to_nodes.{name}', 'tcp', f'tool_{name}',
+        )
+        for name in TOOL_GEOMETRY_KEYS
+    }
+    tool_collisions = {
+        name: _load_box(tool_collisions_raw[name], f'tool_collisions.{name}')
+        for name in TOOL_GEOMETRY_KEYS
+    }
+
+    holes_raw = _mapping(data.get('holes'), 'holes')
+    holes = {}
+    for name in SCREW_HOLES:
+        if name not in holes_raw:
+            raise GeometryError(f'holes is missing {name}')
+        entry = _mapping(holes_raw[name], f'holes.{name}')
+        holes[name] = HoleSpec(
+            name=name,
+            dx=_number(entry.get('dx'), f'holes.{name}.dx'),
+            dy=_number(entry.get('dy'), f'holes.{name}.dy'),
+        )
+
+    sides_raw = _mapping(data.get('sealant_sides'), 'sealant_sides')
+    sealant_sides = {}
+    for name in _SEALANT_SIDES:
+        entry = _mapping(sides_raw.get(name), f'sealant_sides.{name}')
+        sealant_sides[name] = SegmentSpec(
+            name=name,
+            start=_vector(entry.get('start'), f'sealant_sides.{name}.start', 2),
+            end=_vector(entry.get('end'), f'sealant_sides.{name}.end', 2),
+        )
+
+    speeds_raw = _mapping(data.get('speeds'), 'speeds')
+    speeds = {
+        key: _number(speeds_raw.get(key), f'speeds.{key}')
+        for key in ('transfer', 'approach', 'contact', 'contour')
+    }
+    for key, value in speeds.items():
+        if not 0.0 < value <= 1.0:
+            raise GeometryError(f'speeds.{key} must be within (0, 1]')
+
+    joints_raw = _mapping(data.get('named_joints', {}), 'named_joints')
+    named_joints = {
+        name: _vector(values, f'named_joints.{name}', 7)
+        for name, values in joints_raw.items()
+    }
+
+    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():
+        box_data = _mapping(entry, f'scene_collisions.{name}')
+        size = _vector(box_data.get('size'), f'scene_collisions.{name}.size', 3)
+        offset = (
+            _number(box_data.get('offset_x', 0.0), f'scene_collisions.{name}.offset_x'),
+            _number(box_data.get('offset_y', 0.0), f'scene_collisions.{name}.offset_y'),
+            _number(box_data.get('offset_z', 0.0), f'scene_collisions.{name}.offset_z'),
+        )
+        scene_collisions[name] = Box(size=size, offset=offset)
+
+    positive = {
+        'approach_gap': data.get('approach_gap'),
+        'contact_gap': data.get('contact_gap'),
+        'transfer_gap': data.get('transfer_gap'),
+        'seat_gap': data.get('seat_gap'),
+        'position_tolerance': data.get('position_tolerance'),
+    }
+    values = {
+        key: _number(value, key) for key, value in positive.items()
+    }
+    for key, value in values.items():
+        if value <= 0.0:
+            raise GeometryError(f'{key} must be positive')
+    orientation_tolerance = _number(
+        data.get('orientation_tolerance_rad'), 'orientation_tolerance_rad'
+    )
+    if orientation_tolerance <= 0.0:
+        raise GeometryError('orientation_tolerance_rad must be positive')
+
+    cap_tcp_to_node = _load_transform(
+        data.get('cap_tcp_to_node'), 'cap_tcp_to_node', 'tcp', 'cap_node',
+    )
+    body_to_cap = _load_transform(
+        data.get('body_to_installed_cap'), 'body_to_installed_cap', 'body', 'cap_seat',
+    )
+    screw_tcp_to_node = _load_transform(
+        data.get('screw_tcp_to_node'), 'screw_tcp_to_node', 'tcp', 'screw_node',
+    )
+
+    return GeometrySpec(
+        world_to_base=world_to_base,
+        patron_to_tcp=patron_to_tcp,
+        tool_mounts=tool_mounts,
+        tool_work_tips=tool_work_tips,
+        tool_tcp_to_nodes=tool_tcp_to_nodes,
+        tool_collisions=tool_collisions,
+        cap_tcp_to_node=cap_tcp_to_node,
+        cap_collision=_load_box(data.get('cap_collision'), 'cap_collision'),
+        body_to_installed_cap=body_to_cap,
+        screw_tcp_to_node=screw_tcp_to_node,
+        screw_collision=_load_box(data.get('screw_collision'), 'screw_collision'),
+        holes=holes,
+        hole_reference_yaw_deg=_number(
+            data.get('hole_reference_yaw_deg'), 'hole_reference_yaw_deg'
+        ),
+        hole_z_above_body_origin=_number(
+            data.get('hole_z_above_body_origin'), 'hole_z_above_body_origin'
+        ),
+        handoff_gap=_number(data.get('handoff_gap'), 'handoff_gap'),
+        sealant_reference_yaw_deg=_number(
+            data.get('sealant_reference_yaw_deg'), 'sealant_reference_yaw_deg'
+        ),
+        sealant_sides=sealant_sides,
+        sealant_z_above_body_top=_number(
+            data.get('sealant_z_above_body_top'), 'sealant_z_above_body_top'
+        ),
+        approach_gap=values['approach_gap'],
+        contact_gap=values['contact_gap'],
+        transfer_gap=values['transfer_gap'],
+        seat_gap=values['seat_gap'],
+        position_tolerance=values['position_tolerance'],
+        orientation_tolerance_rad=orientation_tolerance,
+        speeds=speeds,
+        scene_collisions=scene_collisions,
+        safe_poses=safe_poses,
+        named_joints=named_joints,
+    )
+
+
+def object_pose_transform(
+    x: float,
+    y: float,
+    z: float,
+    yaw_deg: float,
+    *,
+    parent: str = 'world',
+    child: str,
+) -> Transform:
+    """Webots pose of an OBJ whose local Y axis becomes world Z."""
+    return Transform.from_xyz_rpy(
+        [x, y, z], [90.0, 0.0, yaw_deg], parent=parent, child=child,
+    )
+
+
+def _tool_transform(tool: Tool, parent: str = 'world') -> Transform:
+    return object_pose_transform(
+        tool.pose.x,
+        tool.pose.y,
+        tool.pose.z,
+        tool.pose.yaw_deg + 180.0,
+        parent=parent,
+        child=f'tool_{tool.name}',
+    )
+
+
+def _body_transform(spec: ScenarioSpec) -> Transform:
+    return object_pose_transform(
+        spec.body.x, spec.body.y, spec.body.z, spec.body.yaw_deg,
+        parent='world', child='body',
+    )
+
+
+def _down_transform(x: float, y: float, z: float, yaw_deg: float, child: str) -> Transform:
+    """Work-point pose whose local Z points down (tool working direction)."""
+    return Transform.from_xyz_rpy(
+        [x, y, z], [180.0, 0.0, yaw_deg], parent='world', child=child,
+    )
+
+
+def _rotate_xy(dx: float, dy: float, degrees: float) -> tuple[float, float]:
+    angle = np.radians(degrees)
+    cosine, sine = np.cos(angle), np.sin(angle)
+    return dx * cosine - dy * sine, dx * sine + dy * cosine
+
+
+def world_target(spec: ScenarioSpec, geometry: GeometrySpec, name: str) -> Transform:
+    """Return the named world pose from an already validated scenario.
+
+    Names:
+      ``base_link``, ``tool:<name>``, ``cap``, ``cap_seat``,
+      ``screw:<screw_i>``, ``hole:<hole_i>`` and ``sealant:<side_i>``.
+    """
+    if name == 'base_link':
+        return geometry.world_to_base
+    if name.startswith('tool:'):
+        tool_name = name.split(':', 1)[1]
+        tools = {tool.name: tool for tool in spec.tools}
+        if tool_name not in tools:
+            raise GeometryError(f'experiment {spec.experiment_id} has no tool {tool_name!r}')
+        return _tool_transform(tools[tool_name])
+    if name == 'cap':
+        if spec.cap_pose is None:
+            raise GeometryError(f'experiment {spec.experiment_id} has no hatch')
+        pose = spec.cap_pose
+        return object_pose_transform(
+            pose.x, pose.y, pose.z, pose.yaw_deg, child='cap',
+        )
+    if name == 'cap_seat':
+        return _body_transform(spec).compose(geometry.body_to_installed_cap)
+    if name.startswith('screw:'):
+        screw_name = name.split(':', 1)[1]
+        screws = {screw.name: screw for screw in spec.screws}
+        if screw_name not in screws:
+            raise GeometryError(
+                f'experiment {spec.experiment_id} has no screw {screw_name!r}'
+            )
+        screw = screws[screw_name]
+        return object_pose_transform(
+            screw.pose.x, screw.pose.y, screw.pose.z,
+            screw.pose.yaw_deg, child=screw.name,
+        )
+    if name.startswith('hole:'):
+        hole_name = name.split(':', 1)[1]
+        if hole_name not in geometry.holes:
+            raise GeometryError(f'unknown hole {hole_name!r}')
+        hole = geometry.holes[hole_name]
+        dx, dy = _rotate_xy(
+            hole.dx, hole.dy, spec.body.yaw_deg - geometry.hole_reference_yaw_deg,
+        )
+        # The target is the screw node at the handoff point: upright, its
+        # lower point just above the hole.  tcp_goal() turns it into a
+        # downward-facing tcp with the magnet above the screw head.
+        return object_pose_transform(
+            spec.body.x + dx,
+            spec.body.y + dy,
+            spec.body.z + geometry.hole_z_above_body_origin + geometry.handoff_gap,
+            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)
+    raise GeometryError(f'unknown world target {name!r}')
+
+
+def sealant_point(
+    geometry: GeometrySpec,
+    spec: ScenarioSpec,
+    side_name: str,
+    fraction: float,
+) -> Transform:
+    """Return a work-point transform along one named sealant segment."""
+    if side_name not in geometry.sealant_sides:
+        raise GeometryError(f'unknown sealant side {side_name!r}')
+    if not 0.0 <= fraction <= 1.0:
+        raise GeometryError('sealant fraction must be within 0..1')
+    segment = geometry.sealant_sides[side_name]
+    dx = segment.start[0] + (segment.end[0] - segment.start[0]) * fraction
+    dy = segment.start[1] + (segment.end[1] - segment.start[1]) * fraction
+    dx, dy = _rotate_xy(
+        dx, dy, spec.body.yaw_deg - geometry.sealant_reference_yaw_deg,
+    )
+    z = spec.body.z + geometry.body_top_local + geometry.sealant_z_above_body_top
+    return _down_transform(
+        spec.body.x + dx, spec.body.y + dy, z, spec.body.yaw_deg,
+        child=f'sealant_{side_name}',
+    )
+
+
+def tcp_goal(target: Transform, tcp_to_work_tip: Transform) -> Transform:
+    """Convert a work-point target into the required tcp pose."""
+    return target.compose_relaxed(tcp_to_work_tip.inverse())

src/iiwa_experement/iiwa_experement/grant_supervisor.py

@@ -0,0 +1,488 @@
+#!/usr/bin/env python3
+"""Webots supervisor for attachment and object ownership.
+
+The controller is started next to Webots through the experiment launch file.
+It exposes ``/iiwa_experement/object_command`` (webots_ros2_msgs/SetString)
+and publishes ``/iiwa_experement/object_state`` as JSON.  It never commands
+the robot; it only follows the actual ``tcp`` pose when an object is held.
+"""
+from __future__ import annotations
+
+import json
+import math
+import sys
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+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 webots_ros2_msgs.srv import SetString
+
+from .geometry import GeometrySpec, Transform, load_geometry
+from .object_state import CommandError, ObjectState, transform_json
+from .scenario import ScenarioSpec, load_scenario
+
+
+COMMAND_SERVICE = '/iiwa_experement/object_command'
+STATE_TOPIC = '/iiwa_experement/object_state'
+
+
+def screw_def_name(screw_id: str) -> str:
+    parts = screw_id.split('_')
+    if len(parts) != 2 or parts[0] != 'screw' or not parts[1].isdigit():
+        raise ValueError(f'invalid screw id {screw_id!r}')
+    return f'grant_screw_{int(parts[1]):02d}'
+
+
+def _matrix_from_values(values) -> tuple[tuple[float, float, float], ...]:
+    if not isinstance(values, (list, tuple)):
+        raise ValueError('rotation must contain 9 row-major values')
+    if len(values) == 3 and all(
+        isinstance(row, (list, tuple)) and len(row) == 3 for row in values
+    ):
+        return tuple(
+            tuple(float(value) for value in row) for row in values
+        )
+    if len(values) != 9:
+        raise ValueError('rotation must contain 9 row-major values')
+    return tuple(
+        tuple(float(values[row * 3 + column]) for column in range(3))
+        for row in range(3)
+    )
+
+
+def transform_from_json(data: dict[str, Any], child: str = 'target') -> Transform:
+    xyz = data.get('xyz')
+    if not isinstance(xyz, (list, tuple)) or len(xyz) != 3:
+        raise ValueError('pose.xyz must contain three numbers')
+    if 'rotation' in data:
+        rotation = _matrix_from_values(data['rotation'])
+    else:
+        from scipy.spatial.transform import Rotation
+
+        rotation = tuple(
+            tuple(float(value) for value in row)
+            for row in Rotation.from_euler(
+                'xyz', data.get('rpy_deg', [0.0, 0.0, 0.0]), degrees=True
+            ).as_matrix()
+        )
+    return Transform(
+        xyz=tuple(float(value) for value in xyz),
+        rotation=rotation,
+        parent='world',
+        child=child,
+    )
+
+
+def _axis_angle(rotation) -> list[float]:
+    from scipy.spatial.transform import Rotation
+
+    rotvec = Rotation.from_matrix(np.asarray(rotation, dtype=float)).as_rotvec()
+    angle = float(np.linalg.norm(rotvec))
+    if angle < 1e-12:
+        return [0.0, 0.0, 1.0, 0.0]
+    axis = (rotvec / angle).tolist()
+    return [float(axis[0]), float(axis[1]), float(axis[2]), angle]
+
+
+class GrantSupervisorNode(Node):
+    """ROS facade over :class:`ObjectState` and the Webots scene tree."""
+
+    def __init__(self) -> None:
+        super().__init__('grant_attachment_supervisor')
+        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('read_only', False)
+        experiment_id = int(self.get_parameter('experiment_id').value)
+        scene_config = Path(str(self.get_parameter('scene_config').value))
+        geometry_path = Path(str(self.get_parameter('geometry_path').value))
+        read_only = bool(self.get_parameter('read_only').value)
+        self.spec: ScenarioSpec = load_scenario(experiment_id, scene_config)
+        self.geometry: GeometrySpec = load_geometry(geometry_path)
+        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()
+        }
+        self.nodes: dict[str, Any] = {}
+        self.patron_node = None
+        self.robot = None
+        self.timestep = 32
+        self._last_publish_time = -1.0
+        self._publisher = self.create_publisher(String, STATE_TOPIC, 10)
+        self._service = self.create_service(
+            SetString, COMMAND_SERVICE, self.handle_command,
+        )
+        self.get_logger().info(
+            f'grant supervisor started for experiment {experiment_id}; '
+            f'read_only={read_only}'
+        )
+
+    # ------------------------------------------------------------------ Webots
+    def _pose_node(self, node):
+        """CadShape children can report an uninitialised matrix in Webots."""
+        if node is None:
+            return None
+        try:
+            parent = node.getParentNode()
+        except Exception:
+            parent = None
+        if parent is not None:
+            position = parent.getPosition()
+            if all(np.isfinite(position)):
+                return parent
+        return node
+
+    def connect(self) -> None:
+        from controller import Supervisor
+
+        self.robot = Supervisor()
+        self.timestep = int(self.robot.getBasicTimeStep())
+        self.patron_node = self._pose_node(self._find_node('patron_visual'))
+        required = self._required_defs()
+        for name in required:
+            node = self._find_node(name)
+            if node is not None:
+                self.nodes[name] = node
+                self._freeze(node)
+        self.get_logger().info(
+            f'supervisor found {len(self.nodes)}/{len(required)} scene objects'
+        )
+
+    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:
+            return None
+
+    @staticmethod
+    def _freeze(node) -> None:
+        try:
+            field = node.getField('locked')
+            if field is not None:
+                field.setSFBool(True)
+        except Exception:
+            pass
+        # All managed objects are teleported by the supervisor.  Leaving a
+        # physics node attached lets Webots integrate huge velocities while
+        # the pose is overwritten every step.
+        try:
+            physics = node.getField('physics')
+            if physics is not None:
+                physics.remove()
+        except Exception:
+            pass
+
+    def _pose_of(self, node) -> Transform | None:
+        if node is None:
+            return None
+        position = node.getPosition()
+        orientation = node.getOrientation()
+        return Transform(
+            xyz=tuple(float(value) for value in position),
+            rotation=_matrix_from_values(orientation),
+            parent='world',
+            child='object',
+        )
+
+    def _set_pose(self, node, transform: Transform) -> None:
+        node.getField('translation').setSFVec3f(
+            [float(value) for value in transform.xyz]
+        )
+        rotation = node.getField('rotation')
+        if rotation is not None:
+            rotation.setSFRotation(_axis_angle(transform.rotation))
+
+    def tcp_pose(self) -> Transform | None:
+        patron = self._pose_of(self.patron_node)
+        if patron is None:
+            return None
+        return patron.compose_relaxed(self.geometry.patron_to_tcp)
+
+    def _refresh_nodes(self) -> None:
+        if self.robot is None:
+            return
+        if self.patron_node is None:
+            self.patron_node = self._pose_node(self._find_node('patron_visual'))
+        for name in self._required_defs():
+            if name not in self.nodes:
+                node = self._find_node(name)
+                if node is not None:
+                    self.nodes[name] = node
+                    self._freeze(node)
+
+    def _required_defs(self) -> list[str]:
+        names = ['grant_fixture', 'grant_body', 'grant_rack']
+        names.extend(f'grant_{tool}' for tool in self.spec.tool_names)
+        if self.spec.tray_pose is not None:
+            names.append('grant_tray')
+            names.extend(f'grant_screw_{index:02d}' for index in range(1, 11))
+        if self.spec.cap_state != 'absent':
+            names.append('grant_cap')
+        return [name for name in names if name not in self.removed_defs]
+
+    def _tool_node(self):
+        if self.tool_held is None:
+            return None
+        return self.nodes.get(f'grant_{self.tool_held}')
+
+    def _payload_node(self):
+        if self.payload is None:
+            return None
+        kind, name = self.payload
+        if kind == 'cap':
+            return self.nodes.get('grant_cap')
+        if kind == 'screw':
+            return self.nodes.get(screw_def_name(name))
+        return None
+
+    def _payload_relative(self) -> Transform | None:
+        if self.payload is None:
+            return None
+        kind, _ = self.payload
+        if kind == 'cap':
+            return self.cap_relative or self.geometry.cap_tcp_to_node
+        if kind == 'screw':
+            return self.geometry.screw_tcp_to_node
+        return None
+
+    def follow(self) -> None:
+        tcp = self.tcp_pose()
+        if tcp is None:
+            return
+        if self.tool_held is not None:
+            node = self._tool_node()
+            relative = self.geometry.tool_tcp_to_nodes[self.tool_held]
+            if node is not None:
+                pose = tcp.compose_relaxed(relative)
+                self._set_pose(node, pose)
+                self.state.tools[self.tool_held]['pose'] = pose
+        if self.payload is not None:
+            node = self._payload_node()
+            relative = self._payload_relative()
+            if node is not None and relative is not None:
+                pose = tcp.compose_relaxed(relative)
+                self._set_pose(node, pose)
+                kind, name = self.payload
+                if kind == 'cap':
+                    self.state.cap_pose = pose
+                elif kind == 'screw' and name in self.state.screws:
+                    self.state.screws[name]['pose'] = pose
+
+    # ------------------------------------------------------------------ state
+    def _actual_pose(self, node) -> Transform | None:
+        return self._pose_of(node)
+
+    def _command_pose(self, command: dict[str, Any]) -> Transform:
+        target = command.get('target')
+        if not isinstance(target, dict):
+            raise CommandError('command requires a target pose')
+        return transform_from_json(target, child=str(command.get('object') or 'target'))
+
+    def handle_command(self, request: SetString.Request, response: SetString.Response):
+        try:
+            command = json.loads(request.value)
+            if not isinstance(command, dict):
+                raise CommandError('command must be a JSON object')
+            request_id = str(command.get('request_id', ''))
+            operation = str(command.get('op', ''))
+            accepted, error = self._apply(operation, command, request_id)
+        except Exception as error:  # never leak an exception into Webots
+            accepted = False
+            error = str(error)
+            self.state.last_error = error
+        # The detailed outcome is published in the next object_state message;
+        # the service response only carries the accepted flag.
+        response.success = bool(accepted)
+        self.last_command_result = {
+            'request_id': request_id,
+            'accepted': bool(accepted),
+            'error': None if accepted else error,
+            'version': self.state.version,
+        }
+        self.publish_state(force=True)
+        return response
+
+    def _apply(self, operation: str, command: dict[str, Any], request_id: str):
+        try:
+            return self._apply_impl(operation, command, request_id)
+        except CommandError as error:
+            self.state.last_error = str(error)
+            return False, str(error)
+
+    def _apply_impl(self, operation: str, command: dict[str, Any], request_id: str):
+        if operation == 'inspect':
+            return True, None
+        # A command can arrive before the per-step follow update.  Apply the
+        # current tcp to the held object first, otherwise the attachment
+        # appears one control step behind (about 11 mm at transfer speed).
+        self.follow()
+        tcp = self.tcp_pose()
+        if tcp is None:
+            raise CommandError('actual tcp pose is unavailable')
+        kind = str(command.get('object') or '')
+        if operation == 'attach_tool':
+            node = self.nodes.get(f'grant_{kind}')
+            object_pose = self._actual_pose(node)
+            if object_pose is None:
+                raise CommandError(f'tool node grant_{kind} is unavailable')
+            self.state.attach_tool(kind, tcp, object_pose, request_id=request_id)
+            self.tool_held = kind
+            return True, None
+        if operation == 'dock_tool':
+            node = self.nodes.get(f'grant_{kind}')
+            object_pose = self._actual_pose(node)
+            if object_pose is None:
+                raise CommandError(f'tool node grant_{kind} is unavailable')
+            rack = self.rack_poses[kind]
+            self.state.dock_tool(
+                kind, tcp, rack, object_pose, request_id=request_id,
+            )
+            self.tool_held = None
+            self._set_pose(node, rack)
+            self.state.tools[kind]['pose'] = rack
+            return True, None
+        if operation == 'grab_cap':
+            node = self.nodes.get('grant_cap')
+            object_pose = self._actual_pose(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':
+            target = self._command_pose(command)
+            node = self.nodes.get('grant_cap')
+            object_pose = self._actual_pose(node)
+            if object_pose is None:
+                raise CommandError('hatch node is unavailable')
+            self.state.place_cap(target, tcp, object_pose, request_id=request_id)
+            self.payload = None
+            self._set_pose(node, target)
+            self._freeze(node)
+            self.state.cap_pose = target
+            return True, None
+        if operation == 'grab_screw':
+            node = self.nodes.get(screw_def_name(kind))
+            object_pose = self._actual_pose(node)
+            if object_pose is None:
+                raise CommandError(f'screw node for {kind} is unavailable')
+            self.state.grab_screw(kind, tcp, object_pose, request_id=request_id)
+            self.payload = ('screw', kind)
+            return True, None
+        if operation == 'handoff_screw':
+            hole = str(command.get('target_name') or '')
+            target = self._command_pose(command)
+            node = self.nodes.get(screw_def_name(kind))
+            object_pose = self._actual_pose(node)
+            if object_pose is None:
+                raise CommandError(f'screw node for {kind} is unavailable')
+            self.state.handoff_screw(
+                kind, hole, target, tcp, object_pose, request_id=request_id,
+            )
+            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.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
+            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}')
+
+    def publish_state(self, *, force: bool = False) -> None:
+        if self.robot is None:
+            return
+        now = float(self.robot.getTime())
+        if not force and now - self._last_publish_time < 0.1:
+            return
+        self._last_publish_time = now
+        payload = self.state.snapshot()
+        payload['simulation_time'] = now
+        payload['ready'] = self.patron_node is not None and all(
+            name in self.nodes for name in self._required_defs()
+        )
+        payload['attached'] = {
+            'tool': self.tool_held,
+            'payload': None if self.payload is None else list(self.payload),
+        }
+        tcp = self.tcp_pose()
+        payload['tcp'] = transform_json(tcp)
+        payload['last_command_result'] = self.last_command_result
+        payload['objects'] = {
+            name: transform_json(self._pose_of(node))
+            for name, node in self.nodes.items()
+        }
+        message = String()
+        message.data = json.dumps(payload)
+        self._publisher.publish(message)
+
+    def run(self) -> None:
+        self.connect()
+        while self.robot.step(self.timestep) != -1:
+            try:
+                rclpy.spin_once(self, timeout_sec=0.0)
+                self._refresh_nodes()
+                self.follow()
+                self.publish_state()
+            except Exception as error:  # keep the controller alive for diagnosis
+                self.get_logger().error(f'supervisor step failed: {error}')
+
+
+def _publish_unavailable(node: Node, message: str) -> None:
+    del node
+    print(f'[iiwa_experement] grant supervisor: {message}', file=sys.stderr)
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    exit_code = 0
+    node = None
+    try:
+        node = GrantSupervisorNode()
+        node.run()
+    except (KeyboardInterrupt, ExternalShutdownException):
+        pass
+    except Exception as error:
+        print(
+            f'[iiwa_experement] grant supervisor: {error}',
+            file=sys.stderr,
+        )
+        exit_code = 1
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+    if exit_code:
+        raise SystemExit(exit_code)
+
+if __name__ == '__main__':
+    main()

src/iiwa_experement/iiwa_experement/motion.py

@@ -0,0 +1,673 @@
+"""Safe wrapper around the existing ``cobot/move_to_pose`` motion server.
+
+Every call performs the same guard sequence:
+fresh joint state -> synchronized planning scene -> available server ->
+accepted goal -> successful result -> measured tcp pose.  Any failure stops
+the robot and blocks all later motion.
+"""
+from __future__ import annotations
+
+import math
+import time
+from dataclasses import dataclass
+from typing import Any, Callable
+
+import numpy as np
+from geometry_msgs.msg import PoseStamped
+from rclpy.action import ActionClient
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.time import Time
+from scipy.spatial.transform import Rotation
+from sensor_msgs.msg import JointState
+from std_srvs.srv import Trigger
+import tf2_ros
+
+from iiwa_msgs.action import MoveToJoints, MoveToPose
+from iiwa_msgs.srv import MoveToNamedPose
+
+from .geometry import GeometrySpec, Transform
+
+
+MOVE_ACTION = '/cobot/move_to_pose'
+JOINTS_ACTION = '/cobot/move_to_joints'
+NAMED_SERVICE = '/cobot/move_to_named'
+STOP_SERVICE = '/cobot/stop'
+PLANNERS = ('ptp', 'lin', 'ompl')
+
+
+@dataclass(frozen=True)
+class MotionResult:
+    success: bool
+    message: str
+    label: str
+    sent: bool = False
+    goal: Transform | None = None
+    actual: Transform | None = None
+    duration_sec: float = 0.0
+
+
+def transform_from_pose(message, parent: str, child: str) -> Transform:
+    orientation = message.orientation
+    matrix = Rotation.from_quat([
+        orientation.x, orientation.y, orientation.z, orientation.w,
+    ]).as_matrix()
+    return Transform(
+        xyz=(
+            float(message.position.x),
+            float(message.position.y),
+            float(message.position.z),
+        ),
+        rotation=tuple(tuple(float(value) for value in row) for row in matrix),
+        parent=parent,
+        child=child,
+    )
+
+
+def transform_from_tf(message, parent: str, child: str) -> Transform:
+    translation = message.transform.translation
+    rotation = message.transform.rotation
+    matrix = Rotation.from_quat([
+        rotation.x, rotation.y, rotation.z, rotation.w,
+    ]).as_matrix()
+    return Transform(
+        xyz=(
+            float(translation.x),
+            float(translation.y),
+            float(translation.z),
+        ),
+        rotation=tuple(tuple(float(value) for value in row) for row in matrix),
+        parent=parent,
+        child=child,
+    )
+
+
+def _pose_distance(first: Transform, second: Transform) -> tuple[float, float]:
+    distance = float(np.linalg.norm(first.position - second.position))
+    cosine = (np.trace(first.matrix.T @ second.matrix) - 1.0) / 2.0
+    angle = float(np.arccos(max(-1.0, min(1.0, cosine))))
+    return distance, angle
+
+
+class MotionAdapter:
+    """Guarded motion client used by every reference operation."""
+
+    def __init__(
+        self,
+        node: Node,
+        geometry: GeometrySpec,
+        *,
+        action_client: Any | None = None,
+        joints_client: Any | None = None,
+        named_client: Any | None = None,
+        stop_client: Any | None = None,
+        tf_buffer: Any | None = None,
+        scene_check: Callable[[], tuple[bool, str]] | None = None,
+        pose_provider: Callable[[], Transform | None] | None = None,
+        freshness_check: Callable[[], tuple[bool, str]] | None = None,
+        spin_until: Callable[[Any, float], bool] | None = None,
+        position_tolerance: float | None = None,
+        orientation_tolerance: float | None = None,
+        command_timeout_sec: float = 90.0,
+        settle_time_sec: float = 1.0,
+        stability_timeout_sec: float = 5.0,
+        stability_epsilon: float = 0.001,
+        stability_angle: float = 0.01,
+        pose_correction_attempts: int = 2,
+        joint_correction_attempts: int = 2,
+        joint_position_tolerance: float = 0.05,
+    ) -> None:
+        self.node = node
+        self.geometry = geometry
+        self.action_client = action_client or ActionClient(
+            node, MoveToPose, MOVE_ACTION,
+        )
+        self.joints_client = joints_client
+        if self.joints_client is None and node is not None:
+            self.joints_client = ActionClient(node, MoveToJoints, JOINTS_ACTION)
+        self.named_client = named_client or node.create_client(
+            MoveToNamedPose, NAMED_SERVICE,
+        )
+        self.stop_client = stop_client or node.create_client(
+            Trigger, STOP_SERVICE,
+        )
+        self.tf_buffer = tf_buffer
+        self._tf_listener = None
+        if tf_buffer is None:
+            self.tf_buffer = tf2_ros.Buffer()
+            self._tf_listener = tf2_ros.TransformListener(self.tf_buffer, node)
+        self._scene_check = scene_check
+        self._pose_provider = pose_provider
+        self._freshness_check = freshness_check
+        self._spin_until = spin_until or self._default_spin_until
+        self.position_tolerance = (
+            geometry.position_tolerance
+            if position_tolerance is None else float(position_tolerance)
+        )
+        self.orientation_tolerance = (
+            geometry.orientation_tolerance_rad
+            if orientation_tolerance is None else float(orientation_tolerance)
+        )
+        self.command_timeout_sec = float(command_timeout_sec)
+        self.settle_time_sec = max(0.0, float(settle_time_sec))
+        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.joint_correction_attempts = max(1, int(joint_correction_attempts))
+        self.joint_position_tolerance = float(joint_position_tolerance)
+        self._joint_positions: tuple[float, ...] | None = None
+        self._joint_velocities: tuple[float, ...] | None = None
+        self.stopped = False
+        self.stop_reason: str | None = None
+        self._last_joint_time = None
+        if freshness_check is None:
+            node.create_subscription(JointState, '/joint_states', self._on_joint_state, 10)
+
+    # ------------------------------------------------------------------ freshness
+    def _on_joint_state(self, message: JointState) -> None:
+        self._last_joint_time = self.node.get_clock().now()
+        if len(message.position) == 7:
+            self._joint_positions = tuple(float(value) for value in message.position)
+        if len(message.velocity) == 7:
+            self._joint_velocities = tuple(
+                float(value) for value in message.velocity
+            )
+
+    def joint_positions(self) -> tuple[float, ...] | None:
+        return self._joint_positions
+
+    def wait_stationary(
+        self, timeout_sec: float = 3.0, tolerance: float = 0.01,
+    ) -> bool:
+        """Wait for fresh, near-zero joint velocities before planning.
+
+        The planning pipeline rejects a motion whose recorded start state still
+        carries velocity.  The controller action can finish before the next
+        zero-velocity sample reaches the planning server, so gate the next
+        plan on a short stable window.
+        """
+        if self.node is None or self._freshness_check is not None:
+            return True
+        import rclpy
+
+        deadline = time.monotonic() + max(0.0, float(timeout_sec))
+        stable_since = None
+        while time.monotonic() < deadline:
+            rclpy.spin_once(self.node, timeout_sec=0.02)
+            velocities = self._joint_velocities
+            if velocities is None or len(velocities) != 7 or any(
+                abs(value) > tolerance for value in velocities
+            ):
+                stable_since = None
+                continue
+            stable_since = stable_since or time.monotonic()
+            if time.monotonic() - stable_since >= 0.20:
+                return True
+        self.node.get_logger().warning(
+            'joint_states did not settle before planning'
+        )
+        return False
+
+    def _default_freshness(self) -> tuple[bool, str]:
+        if self._last_joint_time is None:
+            return False, 'no /joint_states received yet'
+        age = (self.node.get_clock().now() - self._last_joint_time).nanoseconds / 1e9
+        if age > 1.0:
+            return False, f'/joint_states is stale ({age:.2f} s old)'
+        if self.tf_buffer is not None:
+            try:
+                self.tf_buffer.lookup_transform('base_link', 'tcp', Time())
+            except Exception as error:
+                return False, f'tcp transform is unavailable: {error}'
+        return True, 'ok'
+
+    def _fresh(self) -> tuple[bool, str]:
+        if self._freshness_check is None:
+            return self._default_freshness()
+        return self._freshness_check()
+
+    def _scene(self) -> tuple[bool, str]:
+        if self._scene_check is None:
+            return False, 'planning scene bridge is not configured'
+        return self._scene_check()
+
+    # ------------------------------------------------------------------ transforms
+    def _default_spin_until(self, future, timeout_sec: float) -> bool:
+        import rclpy
+
+        rclpy.spin_until_future_complete(
+            self.node, future, timeout_sec=timeout_sec,
+        )
+        return future.done()
+
+    def world_to_base(self) -> Transform:
+        # The URDF ships a ``world_base_joint`` with an identity origin, so
+        # robot_state_publisher always provides a synthetic world->base_link
+        # transform at zero.  The physical Webots placement comes from the
+        # launch setting and is stored in GeometrySpec.world_to_base.
+        return self.geometry.world_to_base
+
+    def update_world_to_base(self, transform: Transform) -> None:
+        self.geometry = GeometrySpec(
+            **{**self.geometry.__dict__, 'world_to_base': transform}
+        )
+
+    def actual_tcp(self) -> Transform | None:
+        if self._pose_provider is not None:
+            return self._pose_provider()
+        if self.tf_buffer is None:
+            return None
+        try:
+            message = self.tf_buffer.lookup_transform('base_link', 'tcp', Time())
+            return transform_from_tf(message, 'base_link', 'tcp')
+        except Exception:
+            return None
+
+    def to_base_goal(self, goal: Transform) -> Transform:
+        return self.world_to_base().inverse().compose_relaxed(goal)
+
+    def _abc(self, goal: Transform) -> tuple[float, float, float]:
+        euler = Rotation.from_matrix(goal.matrix).as_euler('ZYX')
+        return float(euler[0]), float(euler[1]), float(euler[2])
+
+    # ------------------------------------------------------------------ guards
+    def _guard(self, label: str, speed: float, planner: str) -> MotionResult | None:
+        if self.stopped:
+            return MotionResult(
+                False,
+                f'motion is blocked after stop: {self.stop_reason}',
+                label,
+            )
+        if not math.isfinite(float(speed)) or not 0.0 < float(speed) <= 1.0:
+            return MotionResult(False, f'invalid speed {speed!r}', label)
+        if planner not in PLANNERS:
+            return MotionResult(False, f'unknown planner {planner!r}', label)
+        fresh, message = self._fresh()
+        if not fresh:
+            self.stop(message)
+            return MotionResult(False, message, label)
+        synchronized, message = self._scene()
+        if not synchronized:
+            self.stop(message)
+            return MotionResult(False, message, label)
+        return None
+
+    # ------------------------------------------------------------------ motion
+    def move_pose(
+        self,
+        goal: Transform,
+        planner: str = 'ptp',
+        speed: float = 0.2,
+        label: str = '',
+    ) -> MotionResult:
+        label = label or 'move_pose'
+        blocked = self._guard(label, speed, planner)
+        if blocked is not None:
+            return blocked
+        if not self.action_client.wait_for_server(timeout_sec=5.0):
+            return MotionResult(False, f'{MOVE_ACTION} is unavailable', label)
+
+        base_goal = self.to_base_goal(goal)
+        a, b, c = self._abc(base_goal)
+        request = MoveToPose.Goal()
+        request.x, request.y, request.z = (float(value) for value in base_goal.xyz)
+        request.a, request.b, request.c = a, b, c
+        request.speed = float(speed)
+        request.planner = planner
+        request.frame_id = 'base_link'
+
+        started = time.monotonic()
+        last_actual: Transform | None = None
+        for attempt in range(self.pose_correction_attempts):
+            future = self.action_client.send_goal_async(request)
+            if not self._spin_until(future, self.command_timeout_sec):
+                return self._fail(label, 'goal response timed out', base_goal, started)
+            try:
+                goal_handle = future.result()
+            except Exception as error:
+                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()
+            if not self._spin_until(result_future, self.command_timeout_sec):
+                return self._fail(label, 'motion result timed out', base_goal, started)
+            try:
+                wrapped = result_future.result()
+                result = wrapped.result
+            except Exception as error:
+                return self._fail(label, f'motion result failed: {error}', base_goal, started)
+            if not result.success:
+                return self._fail(
+                    label, result.message or 'motion server reported failure',
+                    base_goal, started,
+                )
+
+            actual = self._settle_and_measure()
+            last_actual = actual
+            if actual is None:
+                return self._fail(
+                    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,
+                )
+            # The Webots trajectory controller reports success on the desired
+            # trajectory state while the measured arm is still creeping to the
+            # held final setpoint.  Give it a bounded window to arrive before
+            # re-issuing the goal; re-issuing at once looks like a violent
+            # sideways snap of 5-10 cm.
+            converged = self._wait_for_converged_pose(base_goal, timeout_sec=8.0)
+            if converged is not None:
+                if self.node is not None:
+                    self.node.get_logger().info(
+                        f'{label}: tcp converged to the goal after a short wait'
+                    )
+                return MotionResult(
+                    True, 'motion completed', label, True, base_goal, converged,
+                    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=last_actual,
+            )
+        return self._fail(label, 'motion failed without a result', base_goal, started)
+
+    def move_named(self, name: str, speed: float = 0.2, label: str = '') -> MotionResult:
+        label = label or f'move_named({name})'
+        self.wait_stationary()
+        joints = self.geometry.named_joints.get(name)
+        if self.joints_client is not None and joints is not None:
+            result = self.move_joints(joints, speed, label)
+            if result.success:
+                return result
+            planning_failed = (
+                'Планирование' in result.message
+                or 'planning' in result.message.lower()
+                or 'недостижима' in result.message
+            )
+            if not planning_failed:
+                return result
+            # A joint-space plan can be rejected in an unusual scene; retry
+            # the named service once as the fallback path.
+            self.reset_stop()
+        result = self._move_named_service(name, speed, label)
+        # 'home' and 'work' are the only named poses the reference executor
+        # may use; the forbidden 'transport' pose is never a fallback.  A
+        # rejected 'work' already tried its calibrated safe Cartesian pose
+        # inside the service path, so a failure here is reported as-is.
+        return result
+
+    def move_joints(
+        self,
+        joints,
+        speed: float = 0.2,
+        label: str = '',
+    ) -> MotionResult:
+        label = label or 'move_joints'
+        values = tuple(float(value) for value in joints)
+        if len(values) != 7:
+            return MotionResult(False, 'joint goal must contain 7 values', label)
+        blocked = self._guard(label, speed, 'ptp')
+        if blocked is not None:
+            return blocked
+        if self.joints_client is None:
+            return MotionResult(False, f'{JOINTS_ACTION} is unavailable', label)
+        if not self.joints_client.wait_for_server(timeout_sec=5.0):
+            return MotionResult(False, f'{JOINTS_ACTION} is unavailable', label)
+
+        request = MoveToJoints.Goal()
+        request.joints = list(values)
+        request.speed = float(speed)
+        started = time.monotonic()
+        last_error = 0.0
+        for attempt in range(self.joint_correction_attempts):
+            future = self.joints_client.send_goal_async(request)
+            if not self._spin_until(future, self.command_timeout_sec):
+                return self._fail(label, 'joint goal response timed out', None, started)
+            try:
+                goal_handle = future.result()
+            except Exception as error:
+                return self._fail(
+                    label, f'joint goal call failed: {error}', None, started,
+                )
+            if goal_handle is None or not goal_handle.accepted:
+                return self._fail(label, 'joint goal was rejected', None, started)
+            result_future = goal_handle.get_result_async()
+            if not self._spin_until(result_future, self.command_timeout_sec):
+                return self._fail(label, 'joint result timed out', None, started)
+            try:
+                result = result_future.result().result
+            except Exception as error:
+                return self._fail(
+                    label, f'joint result failed: {error}', None, started,
+                )
+            if not result.success:
+                return self._fail(
+                    label, result.message or 'joint motion failed', None, started,
+                )
+            actual = self._settle_joints()
+            if actual is None or len(actual) != 7:
+                break
+            last_error = max(abs(a - b) for a, b in zip(actual, values))
+            if last_error <= self.joint_position_tolerance:
+                break
+            if attempt + 1 < self.joint_correction_attempts:
+                # The controller can stop short after a transient Webots
+                # contact.  Re-issue the same verified joint goal once.
+                if self.node is not None:
+                    self.node.get_logger().warning(
+                        f'{label}: joint positions {last_error:.4f} rad off; '
+                        're-issuing the goal'
+                    )
+                continue
+            return self._fail(
+                label,
+                f'joint positions are {last_error:.4f} rad from the joint goal',
+                None,
+                started,
+            )
+        return MotionResult(
+            True, 'joint motion completed', label, True, None, self.actual_tcp(),
+            time.monotonic() - started,
+        )
+
+    def _move_named_service(
+        self, name: str, speed: float, label: str,
+    ) -> MotionResult:
+        blocked = self._guard(label, speed, 'ptp')
+        if blocked is not None:
+            return blocked
+        if not self.named_client.wait_for_service(timeout_sec=5.0):
+            return MotionResult(False, f'{NAMED_SERVICE} is unavailable', label)
+
+        request = MoveToNamedPose.Request()
+        request.name = str(name)
+        request.speed = float(speed)
+        request.accel_scale = float(speed)
+        started = time.monotonic()
+        future = self.named_client.call_async(request)
+        if not self._spin_until(future, self.command_timeout_sec):
+            return self._fail(label, 'named motion timed out', None, started)
+        try:
+            response = future.result()
+        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,
+            )
+        self._settle()
+        return MotionResult(
+            True, response.message or 'motion completed', label,
+            True, None, self.actual_tcp(), time.monotonic() - started,
+        )
+
+    def _wait_for_converged_pose(
+        self, goal: Transform, timeout_sec: float,
+    ) -> Transform | None:
+        """Wait for the measured tcp to reach the goal within tolerances.
+
+        Returns the converged pose, or None when the window elapsed.
+        """
+        if self.node is None:
+            return None
+        import rclpy
+
+        deadline = time.monotonic() + max(0.0, float(timeout_sec))
+        while time.monotonic() < deadline:
+            rclpy.spin_once(self.node, timeout_sec=0.05)
+            current = self.actual_tcp()
+            if current is not None:
+                distance, angle = _pose_distance(current, goal)
+                if (
+                    distance <= self.position_tolerance
+                    and angle <= self.orientation_tolerance
+                ):
+                    return current
+        return None
+
+    def _settle(self) -> None:
+        if self.settle_time_sec <= 0.0 or self.node is None:
+            return
+        import rclpy
+
+        end = time.monotonic() + self.settle_time_sec
+        while time.monotonic() < end:
+            rclpy.spin_once(
+                self.node,
+                timeout_sec=min(0.05, max(0.0, end - time.monotonic())),
+            )
+
+    def _settle_joints(self) -> tuple[float, ...] | None:
+        """Wait until the measured joint positions stop changing.
+
+        The joint action result can arrive while the Webots controller is
+        still following the last trajectory points.  Comparing that first
+        sample produced false 'joint positions are ... off' failures.
+        """
+        self._settle()
+        if self.node is None:
+            return self._joint_positions
+        import rclpy
+
+        deadline = time.monotonic() + self.stability_timeout_sec
+        last = self._joint_positions
+        stable_since = None
+        while time.monotonic() < deadline:
+            rclpy.spin_once(self.node, timeout_sec=0.05)
+            current = self._joint_positions
+            if current is None or last is None or len(current) != len(last):
+                last = current
+                stable_since = None
+                continue
+            if max(abs(a - b) for a, b in zip(current, last)) <= 0.001:
+                stable_since = stable_since or time.monotonic()
+                if time.monotonic() - stable_since >= 0.25:
+                    return current
+            else:
+                stable_since = None
+            last = current
+        return self._joint_positions
+
+    def _settle_and_measure(self) -> Transform | None:
+        """Wait until the controller stops moving before verifying tcp.
+
+        The action result is delivered when the trajectory finishes, but the
+        joint trajectory controller can still be settling.  Treating the
+        first sample as final caused false out-of-tolerance stops.
+        """
+        self._settle()
+        import rclpy
+
+        deadline = time.monotonic() + self.stability_timeout_sec
+        last = self.actual_tcp()
+        stable_since = None
+        while time.monotonic() < deadline and self.node is not None:
+            rclpy.spin_once(self.node, timeout_sec=0.05)
+            current = self.actual_tcp()
+            if current is None:
+                last = None
+                stable_since = None
+                continue
+            if last is not None:
+                distance, angle = _pose_distance(current, last)
+                if distance <= self.stability_epsilon and angle <= self.stability_angle:
+                    stable_since = stable_since or time.monotonic()
+                    if time.monotonic() - stable_since >= 0.25:
+                        return current
+                else:
+                    stable_since = None
+            last = current
+        return last
+
+    def _fail(
+        self,
+        label: str,
+        message: str,
+        goal: Transform | None,
+        started: float,
+        *,
+        actual: Transform | None = None,
+    ) -> MotionResult:
+        self.stop(message)
+        return MotionResult(
+            False, message, label, True, goal, actual,
+            time.monotonic() - started,
+        )
+
+    def stop(self, reason: str) -> bool:
+        self.stopped = True
+        if self.stop_reason is None:
+            self.stop_reason = reason
+        try:
+            if self.stop_client.wait_for_service(timeout_sec=1.0):
+                future = self.stop_client.call_async(Trigger.Request())
+                self._spin_until(future, 5.0)
+                return bool(future.done() and future.result().success)
+        except Exception:
+            pass
+        return False
+
+    def reset_stop(self) -> None:
+        """Only for a new run; never used to resume after a failed motion."""
+        self.stopped = False
+        self.stop_reason = None

src/iiwa_experement/iiwa_experement/object_state.py

@@ -0,0 +1,376 @@
+"""Deterministic object ownership for the grant reference executor.
+
+The state machine is deliberately independent of ROS and Webots.  The
+supervisor translates accepted commands into Webots field updates and the
+reference runner uses the same transitions in its table tests.
+"""
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, replace
+from typing import Any
+
+import numpy as np
+
+from .geometry import GeometrySpec, Transform, object_pose_transform, world_target
+from .scenario import ScenarioSpec
+
+
+CAP_STATES = ('absent', 'free', 'installed', 'held')
+SCREW_STATES = ('tray', 'held', 'delivered')
+TOOL_STATES = ('docked', 'mounted')
+
+
+class CommandError(RuntimeError):
+    """A command was rejected; the state is unchanged."""
+
+
+def transform_json(transform: Transform | None) -> dict[str, Any] | None:
+    if transform is None:
+        return None
+    return {
+        'xyz': [float(value) for value in transform.xyz],
+        'rotation': [
+            [float(value) for value in row] for row in transform.rotation
+        ],
+        'parent': transform.parent,
+        'child': transform.child,
+    }
+
+
+def _position_distance(first: Transform, second: Transform) -> float:
+    return float(np.linalg.norm(first.position - second.position))
+
+
+def _angle_distance(first: Transform, second: Transform) -> float:
+    cosine = (np.trace(first.matrix.T @ second.matrix) - 1.0) / 2.0
+    return float(np.arccos(max(-1.0, min(1.0, cosine))))
+
+
+class ObjectState:
+    """Ownership and delivery state for one experiment."""
+
+    def __init__(
+        self,
+        spec: ScenarioSpec,
+        geometry: GeometrySpec,
+        *,
+        read_only: bool = False,
+    ) -> None:
+        self.spec = spec
+        self.geometry = geometry
+        self.read_only = read_only
+        self.version = 0
+        self.last_request_id: str | None = None
+        self.last_error: str | None = None
+        self.mounted_tool: str | None = None
+        self.held_object: str | None = None
+        self.cap_state = spec.cap_state
+        self.cap_pose = (
+            world_target(spec, geometry, 'cap')
+            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] = {
+                'state': 'docked',
+                'pose': world_target(spec, geometry, f'tool:{tool.name}'),
+            }
+        self.screws: dict[str, dict[str, Any]] = {}
+        for screw in spec.screws:
+            self.screws[screw.name] = {
+                'state': 'tray',
+                'pose': world_target(spec, geometry, f'screw:{screw.name}'),
+                'hole': None,
+            }
+        self.holes = {
+            name: {'state': 'empty', 'screw': None}
+            for name in ('hole_1', 'hole_2', 'hole_3', 'hole_4')
+        }
+
+    # ------------------------------------------------------------------ helpers
+    def _reject(self, message: str) -> None:
+        self.last_error = message
+        raise CommandError(message)
+
+    def _accept(self, request_id: str | None) -> None:
+        self.version += 1
+        self.last_request_id = request_id
+        self.last_error = None
+
+    def _require_not_read_only(self) -> None:
+        if self.read_only:
+            self._reject('supervisor is in read-only view mode')
+
+    def _require_instrument(self, instrument: str, operation: str) -> None:
+        if self.mounted_tool != instrument:
+            self._reject(
+                f'{operation} requires {instrument!r} in patron; '
+                f'mounted tool is {self.mounted_tool!r}'
+            )
+
+    def _require_free_hand(self, operation: str) -> None:
+        if self.held_object is not None:
+            self._reject(
+                f'{operation} is forbidden while {self.held_object!r} is held'
+            )
+
+    def _require_close(
+        self,
+        actual: Transform,
+        expected: Transform,
+        operation: str,
+    ) -> None:
+        if actual.parent != expected.parent:
+            self._reject(f'{operation}: pose frames do not match')
+        distance = _position_distance(actual, expected)
+        angle = _angle_distance(actual, expected)
+        if distance > self.geometry.position_tolerance:
+            self._reject(
+                f'{operation}: tcp is {distance:.4f} m from the contact pose '
+                f'(limit {self.geometry.position_tolerance:.4f} m)'
+            )
+        if angle > self.geometry.orientation_tolerance_rad:
+            self._reject(
+                f'{operation}: tcp orientation error is {angle:.4f} rad '
+                f'(limit {self.geometry.orientation_tolerance_rad:.4f} rad)'
+            )
+
+    def _expected_tcp(self, node_pose: Transform, tcp_to_node: Transform) -> Transform:
+        return node_pose.compose_relaxed(tcp_to_node.inverse())
+
+    # ------------------------------------------------------------------ tools
+    def attach_tool(
+        self,
+        name: str,
+        tcp: Transform,
+        object_pose: Transform,
+        *,
+        request_id: str | None = None,
+    ) -> None:
+        self._require_not_read_only()
+        if self.mounted_tool is not None:
+            self._reject(f'another tool {self.mounted_tool!r} is already mounted')
+        if name not in self.tools:
+            self._reject(f'tool {name!r} is not present in this experiment')
+        tool = self.tools[name]
+        if tool['state'] != 'docked':
+            self._reject(f'tool {name!r} is not in its socket')
+        if _position_distance(object_pose, tool['pose']) > self.geometry.position_tolerance:
+            self._reject(
+                f'tool {name!r} moved away from its socket; refusing blind attach'
+            )
+        self._require_free_hand('attach_tool')
+        self._require_close(
+            tcp,
+            self._expected_tcp(object_pose, self.geometry.tool_tcp_to_nodes[name]),
+            f'attach_tool({name})',
+        )
+        tool['state'] = 'mounted'
+        self.mounted_tool = name
+        self._accept(request_id)
+
+    def dock_tool(
+        self,
+        name: str,
+        tcp: Transform,
+        target: Transform,
+        object_pose: Transform,
+        *,
+        request_id: str | None = None,
+    ) -> None:
+        self._require_not_read_only()
+        if self.mounted_tool != name:
+            self._reject(f'tool {name!r} is not the mounted tool')
+        if name not in self.tools:
+            self._reject(f'tool {name!r} is not present in this experiment')
+        # The tool follows tcp, so the contact pose is only meaningful when
+        # compared with the known socket target.
+        self._require_close(
+            tcp,
+            self._expected_tcp(target, self.geometry.tool_tcp_to_nodes[name]),
+            f'dock_tool({name})',
+        )
+        self._require_close(
+            object_pose,
+            tcp.compose_relaxed(self.geometry.tool_tcp_to_nodes[name]),
+            f'dock_tool({name}) attached object',
+        )
+        tool = self.tools[name]
+        tool['state'] = 'docked'
+        tool['pose'] = target
+        self.mounted_tool = None
+        self._accept(request_id)
+
+    # ------------------------------------------------------------------ hatch
+    def grab_cap(
+        self,
+        tcp: Transform,
+        object_pose: Transform,
+        *,
+        request_id: str | None = None,
+    ) -> None:
+        self._require_not_read_only()
+        self._require_instrument('vacuum', 'grab_cap')
+        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})')
+        # The suction cup is rotationally symmetric.  Require the correct
+        # position and a downward tcp, but allow any grasp yaw; the measured
+        # relative rotation is stored and reused for placement.
+        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'
+        self._accept(request_id)
+
+    def place_cap(
+        self,
+        target: Transform,
+        tcp: Transform,
+        object_pose: Transform,
+        *,
+        request_id: str | None = None,
+    ) -> None:
+        self._require_not_read_only()
+        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, relative),
+            'place_cap',
+        )
+        if _position_distance(object_pose, tcp.compose_relaxed(relative)) > (
+            self.geometry.position_tolerance
+        ):
+            self._reject('place_cap: hatch does not follow the suction cup')
+        self.cap_state = 'installed'
+        self.cap_pose = target
+        self.held_object = None
+        self._accept(request_id)
+
+    # ------------------------------------------------------------------ screws
+    def grab_screw(
+        self,
+        screw_id: str,
+        tcp: Transform,
+        object_pose: Transform,
+        *,
+        request_id: str | None = None,
+    ) -> None:
+        self._require_not_read_only()
+        self._require_instrument('magnetic_tool', 'grab_screw')
+        self._require_free_hand('grab_screw')
+        if screw_id not in self.screws:
+            self._reject(f'screw {screw_id!r} is not present in this experiment')
+        screw = self.screws[screw_id]
+        if screw['state'] != 'tray':
+            self._reject(f'screw {screw_id!r} is unavailable ({screw["state"]})')
+        self._require_close(
+            tcp,
+            self._expected_tcp(object_pose, self.geometry.screw_tcp_to_node),
+            f'grab_screw({screw_id})',
+        )
+        screw['state'] = 'held'
+        screw['pose'] = object_pose
+        self.held_object = f'screw:{screw_id}'
+        self._accept(request_id)
+
+    def handoff_screw(
+        self,
+        screw_id: str,
+        hole_id: str,
+        target: Transform,
+        tcp: Transform,
+        object_pose: Transform,
+        *,
+        request_id: str | None = None,
+    ) -> None:
+        self._require_not_read_only()
+        self._require_instrument('magnetic_tool', 'handoff_screw')
+        if self.held_object != f'screw:{screw_id}':
+            self._reject(f'screw {screw_id!r} is not held')
+        if screw_id not in self.screws:
+            self._reject(f'screw {screw_id!r} is not present in this experiment')
+        screw = self.screws[screw_id]
+        if screw['state'] != 'held':
+            self._reject(f'screw {screw_id!r} is not held ({screw["state"]})')
+        if hole_id not in self.holes:
+            self._reject(f'unknown hole {hole_id!r}')
+        if self.holes[hole_id]['state'] != 'empty':
+            self._reject(f'hole {hole_id!r} already has a delivered screw')
+        if screw['hole'] == hole_id:
+            self._reject(f'screw {screw_id!r} was already delivered to {hole_id!r}')
+        self._require_close(
+            tcp,
+            self._expected_tcp(target, self.geometry.screw_tcp_to_node),
+            f'handoff_screw({screw_id},{hole_id})',
+        )
+        expected_object = tcp.compose_relaxed(self.geometry.screw_tcp_to_node)
+        if _position_distance(object_pose, expected_object) > self.geometry.position_tolerance:
+            self._reject('handoff_screw: screw does not follow the magnet')
+        screw['state'] = 'delivered'
+        screw['hole'] = hole_id
+        screw['pose'] = target
+        self.holes[hole_id]['state'] = 'empty'
+        self.holes[hole_id]['screw'] = screw_id
+        self.held_object = None
+        self._accept(request_id)
+
+    # ------------------------------------------------------------------ inspect
+    def snapshot(self) -> dict[str, Any]:
+        return {
+            'version': self.version,
+            'experiment_id': self.spec.experiment_id,
+            'mounted_tool': self.mounted_tool,
+            'held_object': self.held_object,
+            'read_only': self.read_only,
+            'cap': {
+                'state': self.cap_state,
+                'pose': transform_json(self.cap_pose),
+            },
+            'tools': {
+                name: {
+                    'state': entry['state'],
+                    'pose': transform_json(entry['pose']),
+                }
+                for name, entry in self.tools.items()
+            },
+            'screws': {
+                name: {
+                    'state': entry['state'],
+                    'pose': transform_json(entry['pose']),
+                    'hole': entry['hole'],
+                }
+                for name, entry in self.screws.items()
+            },
+            'holes': {
+                name: {
+                    'state': entry['state'],
+                    'screw': entry['screw'],
+                }
+                for name, entry in self.holes.items()
+            },
+            'last_request_id': self.last_request_id,
+            'last_error': self.last_error,
+        }
+
+    def delivered_pairs(self) -> list[tuple[str, str]]:
+        return [
+            (name, entry['hole'])
+            for name, entry in self.screws.items()
+            if entry['state'] == 'delivered'
+        ]

src/iiwa_experement/iiwa_experement/operations.py

@@ -0,0 +1,935 @@
+"""Reference operations: tools, hatch, sealant contour and screw handoff."""
+from __future__ import annotations
+
+import json
+import math
+import uuid
+from dataclasses import dataclass, field
+from typing import Any, Callable, Iterable
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+from webots_ros2_msgs.srv import SetString
+
+from .geometry import (
+    GeometrySpec,
+    Transform,
+    object_pose_transform,
+    sealant_point,
+    tcp_goal,
+    world_target,
+)
+from .motion import MotionAdapter, MotionResult
+from .planning_scene import (
+    PlanningSceneBridge,
+    attached_cap,
+    attached_screw,
+    attached_tool,
+    cap_world_box,
+    placed_cap_pose,
+    screw_object_id,
+)
+from .scenario import ScenarioSpec
+
+
+COMMAND_SERVICE = '/iiwa_experement/object_command'
+STATE_TOPIC = '/iiwa_experement/object_state'
+
+
+class OperationError(RuntimeError):
+    """An operation could not be completed safely."""
+
+
+@dataclass(frozen=True)
+class OperationResult:
+    success: bool
+    operation: str
+    message: str
+    events: tuple[dict[str, Any], ...] = field(default_factory=tuple)
+
+
+def _translation(xyz) -> Transform:
+    return Transform.from_xyz_rpy(
+        [float(value) for value in xyz], [0.0, 0.0, 0.0],
+        parent='tcp', child='offset',
+    )
+
+
+class SupervisorClient:
+    """Synchronous facade over the Webots attachment supervisor."""
+
+    def __init__(self, node: Node, *, timeout_sec: float = 15.0) -> None:
+        self.node = node
+        self.timeout_sec = float(timeout_sec)
+        self.latest: dict[str, Any] | None = None
+        self._client = node.create_client(SetString, COMMAND_SERVICE)
+        self._subscription = node.create_subscription(
+            String, STATE_TOPIC, self._on_state, 10,
+        )
+
+    def _on_state(self, message: String) -> None:
+        try:
+            self.latest = json.loads(message.data)
+        except Exception:
+            pass
+
+    def _spin(self, seconds: float = 0.05) -> None:
+        import time
+
+        finish = time.monotonic() + seconds
+        while time.monotonic() < finish:
+            rclpy.spin_once(self.node, timeout_sec=0.02)
+
+    def wait_ready(self, timeout_sec: float = 60.0) -> dict[str, Any]:
+        import time
+
+        end = time.monotonic() + timeout_sec
+        while time.monotonic() < end:
+            rclpy.spin_once(self.node, timeout_sec=0.05)
+            if self.latest and self.latest.get('ready'):
+                return self.latest
+        raise OperationError('supervisor did not become ready')
+
+    def wait_version(self, minimum: int, timeout_sec: float | None = None) -> dict[str, Any]:
+        import time
+
+        end = time.monotonic() + (self.timeout_sec if timeout_sec is None else timeout_sec)
+        while time.monotonic() < end:
+            rclpy.spin_once(self.node, timeout_sec=0.05)
+            if self.latest and self.latest.get('version', -1) >= minimum:
+                return self.latest
+        raise OperationError(f'supervisor state version {minimum} was not published')
+
+    def command(
+        self,
+        op: str,
+        object_id: str = '',
+        *,
+        target: Transform | None = None,
+        target_name: str = '',
+        expected_state: dict[str, Any] | None = None,
+    ) -> dict[str, Any]:
+        if not self._client.wait_for_service(timeout_sec=5.0):
+            raise OperationError(f'{COMMAND_SERVICE} is unavailable')
+        request_id = uuid.uuid4().hex
+        payload: dict[str, Any] = {
+            'request_id': request_id,
+            'op': op,
+            'object': object_id,
+            'target_name': target_name,
+        }
+        if target is not None:
+            payload['target'] = _transform_payload(target)
+        if expected_state is not None:
+            payload['expected_state'] = expected_state
+        request = SetString.Request()
+        request.value = json.dumps(payload)
+        future = self._client.call_async(request)
+        self._spin(0.0)
+        import time
+
+        end = time.monotonic() + self.timeout_sec
+        while not future.done() and time.monotonic() < end:
+            rclpy.spin_once(self.node, timeout_sec=0.05)
+        if not future.done():
+            raise OperationError(f'supervisor command {op} timed out')
+        accepted = bool(future.result().success)
+        # The detailed result is published atomically after the transition.
+        state = self.latest
+        last = (state or {}).get('last_command_result') or {}
+        if last.get('request_id') != request_id:
+            end = time.monotonic() + 2.0
+            while time.monotonic() < end:
+                rclpy.spin_once(self.node, timeout_sec=0.02)
+                state = self.latest
+                last = (state or {}).get('last_command_result') or {}
+                if last.get('request_id') == request_id:
+                    break
+        result = last
+        if not accepted or not result.get('accepted', False):
+            reason = result.get('error') or 'supervisor rejected the command'
+            raise OperationError(f'{op}({object_id or "-"}) rejected: {reason}')
+        return state or {}
+
+    @property
+    def objects(self) -> dict[str, Any]:
+        return dict((self.latest or {}).get('objects') or {})
+
+    def object_pose(self, name: str) -> Transform:
+        entry = self.objects.get(name)
+        if not isinstance(entry, dict):
+            raise OperationError(f'object {name!r} is not present in the supervisor state')
+        return _transform_from_payload(entry, child=name)
+
+    def logical_cap(self) -> dict[str, Any]:
+        return dict((self.latest or {}).get('cap') or {})
+
+    def logical_screws(self) -> dict[str, Any]:
+        return dict((self.latest or {}).get('screws') or {})
+
+
+def _transform_payload(transform: Transform) -> dict[str, Any]:
+    return {
+        'xyz': [float(value) for value in transform.xyz],
+        'rotation': [
+            [float(value) for value in row] for row in transform.rotation
+        ],
+    }
+
+
+def _transform_from_payload(data: dict[str, Any], child: str = 'object') -> Transform:
+    xyz = data.get('xyz')
+    rotation = data.get('rotation')
+    if not isinstance(xyz, (list, tuple)) or len(xyz) != 3:
+        raise OperationError('object pose has no valid xyz')
+    if not isinstance(rotation, (list, tuple)) or len(rotation) != 3:
+        raise OperationError('object pose has no valid rotation')
+    if any(not isinstance(row, (list, tuple)) or len(row) != 3 for row in rotation):
+        raise OperationError('object pose rotation must be a 3x3 matrix')
+    return Transform(
+        xyz=tuple(float(value) for value in xyz),
+        rotation=tuple(tuple(float(value) for value in row) for row in rotation),
+        parent='world',
+        child=child,
+    )
+
+
+class ReferenceOperations:
+    """Sequence of visible robot operations for one experiment."""
+
+    def __init__(
+        self,
+        node: Node,
+        spec: ScenarioSpec,
+        geometry: GeometrySpec,
+        supervisor: SupervisorClient,
+        bridge: PlanningSceneBridge,
+        motion: MotionAdapter,
+        *,
+        event_sink: Callable[[dict[str, Any]], None] | None = None,
+    ) -> None:
+        self.node = node
+        self.spec = spec
+        self.geometry = geometry
+        self.supervisor = supervisor
+        self.bridge = bridge
+        self.motion = motion
+        self._event_sink = event_sink
+        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:
+        event = {'operation': operation, **payload}
+        self.events.append(event)
+        if self._event_sink is not None:
+            self._event_sink(event)
+
+    def _require(self, condition: bool, message: str) -> None:
+        if not condition:
+            raise OperationError(message)
+
+    def _move(
+        self,
+        goal: Transform,
+        planner: str,
+        speed: float,
+        label: str,
+        *,
+        allow_fallback: bool = True,
+    ) -> MotionResult:
+        attempts = 3
+        result = None
+        planning_failed = False
+        for attempt in range(attempts):
+            # Rest-to-rest planning: the server's response adapter rejects any
+            # plan whose start state is still moving.  Wait for the arm to
+            # settle before every attempt instead of rapid-firing retries
+            # into the same non-stationary window.
+            self.motion.wait_stationary()
+            result = self.motion.move_pose(
+                goal,
+                planner=planner,
+                speed=speed,
+                label=label if attempt == 0 else f'{label}:retry{attempt}',
+            )
+            if result.success:
+                if attempt:
+                    self._emit('motion_retry', label=label, planner=planner,
+                               attempt=attempt, reason=result.message)
+                return result
+            planning_failed = (
+                'Планирование' in result.message
+                or 'planning' in result.message.lower()
+                or 'недостижима' in result.message
+            )
+            if not planning_failed:
+                break
+            if attempt + 1 < attempts:
+                # A failed search executed no motion; replan from the same
+                # measured, still-synchronized state.
+                self.motion.reset_stop()
+                continue
+        if allow_fallback and planner != 'ompl' and planning_failed:
+            self.motion.reset_stop()
+            self.motion.wait_stationary()
+            fallback = self.motion.move_pose(
+                goal, planner='ompl', speed=speed, label=f'{label}:ompl',
+            )
+            if fallback.success:
+                self._emit(
+                    'motion_fallback', label=label, original=planner,
+                    reason=result.message if result is not None else '',
+                )
+                return fallback
+            result = fallback
+        raise OperationError(f'{label}: {result.message if result is not None else "motion failed"}')
+
+    def _above(self, goal: Transform, gap: float) -> Transform:
+        return goal.compose_relaxed(_translation([0.0, 0.0, -gap]))
+
+    def _contact(
+        self,
+        goal: Transform,
+        label: str,
+        *,
+        approach_gap: float | None = None,
+        transfer_gap: float | None = None,
+        remove_before_contact: str | None = None,
+        remove_phase: str = 'transfer',
+    ) -> None:
+        approach_gap = (
+            self.geometry.approach_gap if approach_gap is None else approach_gap
+        )
+        transfer_gap = (
+            self.geometry.transfer_gap if transfer_gap is None else transfer_gap
+        )
+        if remove_before_contact is not None and remove_phase == 'transfer':
+            # The object is the intended contact target.  Remove it from the
+            # world scene for the whole approach so the attached copy cannot
+            # duplicate it and the short axial moves are not rejected by a
+            # false contact with the target itself.
+            self.bridge.remove(remove_before_contact)
+            self._require_synchronized()
+        self.motion.wait_stationary()
+        transfer = self._above(goal, transfer_gap)
+        approach = self._above(goal, approach_gap)
+        # Transfers and approaches use Pilz PTP: deterministic joint-space
+        # interpolation instead of OMPL's random wide elbow sweeps.  OMPL
+        # remains the automatic fallback of _move when PTP cannot plan.
+        self._move(transfer, 'ptp', self.geometry.speeds['transfer'], f'{label}:transfer')
+        try:
+            self._move(
+                approach, 'ptp', self.geometry.speeds['approach'],
+                f'{label}:approach',
+            )
+        except OperationError as error:
+            message = str(error)
+            if 'Планирование' not in message and 'недостижима' not in message:
+                raise
+            # A pre-contact point can sit on a different IK branch.  The
+            # collision-checked direct path to the contact is safer than
+            # forcing an infeasible axial segment.
+            self._emit('approach_skipped', label=f'{label}:approach',
+                       reason=message)
+            self.motion.reset_stop()
+        if remove_before_contact is not None and remove_phase == 'contact':
+            self.bridge.remove(remove_before_contact)
+            self._require_synchronized()
+        self._move(goal, 'lin', self.geometry.speeds['contact'], f'{label}:contact')
+
+    def _retreat(self, goal: Transform, label: str) -> None:
+        """Lift axially away from a contact pose before any lateral motion.
+
+        On real hardware a sideways move straight out of a contact pose drags
+        the tool or the part across the fixture, so the lift is mandatory:
+        the gaps only shrink, and if every axial segment is rejected the
+        operation stops instead of continuing at contact depth.
+        """
+        lifted = False
+        last_error: OperationError | None = None
+        for index, gap in enumerate((self.geometry.approach_gap, 0.03, 0.015)):
+            lift_label = f'{label}:retreat' if index == 0 else f'{label}:retreat:{gap:.3f}'
+            try:
+                self._move(
+                    self._above(goal, gap), 'lin',
+                    self.geometry.speeds['approach'], lift_label,
+                )
+                lifted = True
+                break
+            except OperationError as error:
+                if 'Планирование' not in str(error):
+                    raise
+                last_error = error
+                self.motion.reset_stop()
+        if not lifted:
+            raise OperationError(
+                f'{label}: the axial retreat was rejected ({last_error})'
+            )
+        self._move(
+            self._above(goal, self.geometry.transfer_gap), 'ptp',
+            self.geometry.speeds['transfer'], f'{label}:clear',
+        )
+
+    def _exit_rack(self, goal: Transform, label: str) -> None:
+        """Lift straight up out of the rack, then step sideways out of it.
+
+        The lift is mandatory: a lateral move at socket depth would drag the
+        tool (or the empty patron) through the magazine on real hardware.
+        The sideways step starts from the height that was actually reached.
+        """
+        height = None
+        last_error: OperationError | None = None
+        for index, gap in enumerate((
+            self.geometry.transfer_gap, self.geometry.approach_gap, 0.03, 0.015,
+        )):
+            lift_label = f'{label}:lift' if index == 0 else f'{label}:lift:{gap:.3f}'
+            try:
+                self._move(
+                    self._above(goal, gap), 'lin',
+                    self.geometry.speeds['approach'], lift_label,
+                )
+                height = gap
+                break
+            except OperationError as error:
+                if 'Планирование' not in str(error):
+                    raise
+                last_error = error
+                self.motion.reset_stop()
+        if height is None:
+            raise OperationError(
+                f'{label}: the axial lift out of the rack was rejected '
+                f'({last_error})'
+            )
+        base = self._above(goal, height)
+        for offset in (0.10, 0.06, 0.03):
+            exit_pose = Transform.from_xyz_rpy(
+                [0.0, offset, 0.0], [0.0, 0.0, 0.0],
+                parent='world', child='rack_exit_offset',
+            ).compose_relaxed(base)
+            exit_label = (
+                f'{label}:exit' if offset == 0.10 else f'{label}:exit:{offset:.2f}'
+            )
+            try:
+                self._move(
+                    exit_pose, 'lin', self.geometry.speeds['transfer'], exit_label,
+                )
+                return
+            except OperationError as error:
+                if 'Планирование' not in str(error):
+                    raise
+                last_error = error
+                self.motion.reset_stop()
+        raise OperationError(
+            f'{label}: no lateral rack exit was plannable ({last_error})'
+        )
+
+    def _contact_with_recovery(
+        self, goal: Transform, label: str, *, prepare=None, **kwargs,
+    ) -> None:
+        """Contact a target, reconfiguring through 'work' if planning fails.
+
+        'work' is the only neutral pose the routes are allowed to use:
+        'transport' is forbidden entirely and 'home' is reserved for the end
+        of the route.  A single retry from 'work' switches the IK branch
+        without leaving the working area.
+        """
+        try:
+            self._contact(goal, label, **kwargs)
+        except OperationError as error:
+            if 'Планирование' not in str(error):
+                raise
+            self._emit('reconfigure', label=label, target='work',
+                       reason=str(error))
+            if prepare is not None:
+                prepare()
+            self.motion.reset_stop()
+            named = self.motion.move_named('work', 0.3)
+            if not getattr(named, 'success', False):
+                raise error
+            self.motion.reset_stop()
+            self._contact(goal, f'{label}:recovery', **kwargs)
+
+    def _require_synchronized(self) -> None:
+        try:
+            self.bridge.assert_synchronized(*self.bridge.expected_sets())
+        except Exception as error:
+            raise OperationError(f'planning scene is not synchronized: {error}') from error
+        # The relay to /planning_scene is asynchronous for the MoveItPy
+        # monitor.  Give it a short, bounded window before planning.
+        if self.node is None:
+            return
+        import time
+
+        end = time.monotonic() + 0.5
+        while time.monotonic() < end:
+            rclpy.spin_once(self.node, timeout_sec=0.05)
+
+    def _expect_latest(self, timeout: float = 2.0) -> dict[str, Any]:
+        import time
+
+        end = time.monotonic() + timeout
+        while time.monotonic() < end:
+            rclpy.spin_once(self.node, timeout_sec=0.02)
+            if self.supervisor.latest is not None:
+                return self.supervisor.latest
+        raise OperationError('supervisor state is unavailable')
+
+    # ------------------------------------------------------------------ tools
+    def take_tool(self, tool_name: str) -> OperationResult:
+        self._emit('take_tool', object=tool_name, phase='start')
+        try:
+            self._require_synchronized()
+            tool_pose = self.supervisor.object_pose(f'grant_{tool_name}')
+            goal = tcp_goal(tool_pose, self.geometry.tool_tcp_to_nodes[tool_name])
+            self._contact_with_recovery(
+                goal, f'take_tool({tool_name})',
+                remove_before_contact=f'grant_{tool_name}',
+            )
+            self.supervisor.command('attach_tool', tool_name)
+            # The held tool becomes part of the robot in the planning scene,
+            # so the lift, the rack exit and every later transfer are planned
+            # with the tool body instead of dragging it through the magazine.
+            self.bridge.attach(attached_tool(tool_name, self.geometry))
+            self._require_synchronized()
+            self._exit_rack(goal, f'take_tool({tool_name})')
+            self._emit('take_tool', object=tool_name, phase='done',
+                       tool=tool_name, tcp=goal.xyz)
+            return OperationResult(True, 'take_tool', f'{tool_name} mounted', tuple(self.events))
+        except Exception as error:
+            self._emit('take_tool', object=tool_name, phase='failed', error=str(error))
+            raise
+
+    def dock_tool(self, tool_name: str) -> OperationResult:
+        self._emit('dock_tool', object=tool_name, phase='start')
+        try:
+            self._require_synchronized()
+            rack_pose = world_target(self.spec, self.geometry, f'tool:{tool_name}')
+            goal = tcp_goal(rack_pose, self.geometry.tool_tcp_to_nodes[tool_name])
+            # Remove the held tool from MoveIt before entering the socket
+            # region.  The supervisor keeps following tcp, and the contact
+            # pose is verified by both the motion adapter and supervisor.
+            self.bridge.remove(f'grant_{tool_name}')
+            self._require_synchronized()
+            self._contact_with_recovery(goal, f'dock_tool({tool_name})')
+            self.supervisor.command('dock_tool', tool_name)
+            # The tool is back in its calibrated socket: restore the exact
+            # initial box pose instead of reusing the rotated node pose.
+            self.bridge.restore_world(f'grant_{tool_name}')
+            self._require_synchronized()
+            self._exit_rack(goal, f'dock_tool({tool_name})')
+            self._emit('dock_tool', object=tool_name, phase='done')
+            return OperationResult(True, 'dock_tool', f'{tool_name} docked', tuple(self.events))
+        except Exception as error:
+            self._emit('dock_tool', object=tool_name, phase='failed', error=str(error))
+            raise
+
+    # ------------------------------------------------------------------ hatch
+    def pick_cap(self) -> OperationResult:
+        self._emit('pick_cap', object='cap', phase='start')
+        try:
+            self._require_synchronized()
+            cap_pose = self.supervisor.object_pose('grant_cap')
+            # 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',
+            )
+            self.supervisor.command('grab_cap', 'cap')
+            # Plan the retreat and the transfer with the hatch attached to the
+            # robot, not as a blind spot removed from the scene.
+            self.bridge.attach(attached_cap(self.geometry))
+            self._require_synchronized()
+            self._retreat(goal, 'pick_cap')
+            self._emit('pick_cap', object='cap', phase='done', tcp=goal.xyz)
+            return OperationResult(True, 'pick_cap', 'hatch grasped', tuple(self.events))
+        except Exception as error:
+            self._emit('pick_cap', object='cap', phase='failed', error=str(error))
+            raise
+
+    def place_cap(self, target_pose: Transform | None = None) -> OperationResult:
+        self._emit('place_cap', object='cap', phase='start')
+        try:
+            self._require_synchronized()
+            target = target_pose or world_target(self.spec, self.geometry, 'cap_seat')
+            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',
+                remove_phase='contact',
+            )
+            self.supervisor.command('place_cap', 'cap', target=target)
+            # The collision box of the installed hatch is flat, thinner than
+            # the plate and centred on it: it must clear both the body box
+            # below and the suction-cup box above at the contact pose.
+            self.bridge.add_world(
+                'grant_cap', cap_world_box(self.geometry),
+                placed_cap_pose(target, self.spec.body.yaw_deg),
+            )
+            self._require_synchronized()
+            self._retreat(goal, 'place_cap')
+            self._emit('place_cap', object='cap', phase='done', target=target.xyz)
+            return OperationResult(True, 'place_cap', 'hatch placed', tuple(self.events))
+        except Exception as error:
+            self._emit('place_cap', object='cap', phase='failed', error=str(error))
+            raise
+
+    # ------------------------------------------------------------------ sealant
+    def dispense(
+        self,
+        segment_name: str,
+        start_fraction: float = 0.0,
+        end_fraction: float = 1.0,
+        *,
+        lift: bool = True,
+    ) -> OperationResult:
+        self._emit('dispense', object=segment_name, phase='start')
+        try:
+            self._require_synchronized()
+            start = sealant_point(
+                self.geometry, self.spec, segment_name, start_fraction,
+            )
+            end = sealant_point(
+                self.geometry, self.spec, segment_name, end_fraction,
+            )
+            tip = self.geometry.tool_work_tips['sealant_tool']
+            start_goal = tcp_goal(start, tip)
+            end_goal = tcp_goal(end, tip)
+            if start_fraction <= 0.0 and end_fraction >= 1.0:
+                self._contact(start_goal, f'dispense({segment_name})')
+            else:
+                self._contact(start_goal, f'dispense({segment_name}):resume')
+            self._move(
+                end_goal, 'lin', self.geometry.speeds['contour'],
+                f'dispense({segment_name}):path', allow_fallback=False,
+            )
+            self.last_sealant_point = (segment_name, end_fraction)
+            self.completed_sealant.add(segment_name)
+            if lift:
+                self._retreat(end_goal, f'dispense({segment_name})')
+            self._emit('dispense', object=segment_name, phase='done',
+                       start_fraction=start_fraction, end_fraction=end_fraction)
+            return OperationResult(True, 'dispense', f'{segment_name} completed', tuple(self.events))
+        except Exception as error:
+            self._emit('dispense', object=segment_name, phase='failed', error=str(error))
+            raise
+
+    def dispense_contour(self, segments: Iterable[str]) -> OperationResult:
+        """Traverse adjacent sides continuously without lifting the nozzle."""
+        segments = list(segments)
+        self._emit('dispense_contour', segments=list(segments), phase='start')
+        try:
+            self._require_synchronized()
+            tip = self.geometry.tool_work_tips['sealant_tool']
+            first = sealant_point(self.geometry, self.spec, segments[0], 0.0)
+            first_goal = tcp_goal(first, tip)
+            self._contact(first_goal, 'dispense_contour:start')
+            for segment in segments:
+                end = sealant_point(self.geometry, self.spec, segment, 1.0)
+                end_goal = tcp_goal(end, tip)
+                self._move(
+                    end_goal, 'lin', self.geometry.speeds['contour'],
+                    f'dispense_contour:{segment}', allow_fallback=False,
+                )
+                self.completed_sealant.add(segment)
+                self.last_sealant_point = (segment, 1.0)
+                self._emit('dispense', object=segment, phase='done')
+            self._retreat(end_goal, 'dispense_contour')
+            self._emit('dispense_contour', segments=list(segments), phase='done')
+            return OperationResult(True, 'dispense_contour', 'contour completed', tuple(self.events))
+        except Exception as error:
+            self._emit('dispense_contour', phases='failed', error=str(error))
+            raise
+
+    # ------------------------------------------------------------------ screws
+    def pick_screw(self, screw_id: str) -> OperationResult:
+        def_name = screw_object_id(screw_id)
+        self._emit('pick_screw', object=screw_id, phase='start')
+        try:
+            self._require_synchronized()
+            screw_pose = self.supervisor.object_pose(def_name)
+            goal = tcp_goal(screw_pose, self.geometry.screw_tcp_to_node)
+            def restore_screw() -> None:
+                # The first attempt removed the screw from the planning scene
+                # before its transfer was rejected.  Restore its calibrated
+                # tray pose for the retry.
+                self.bridge.restore_world(def_name)
+                self._require_synchronized()
+
+            self._contact_with_recovery(
+                goal, f'pick_screw({screw_id})',
+                remove_before_contact=def_name,
+                prepare=restore_screw,
+            )
+            self.supervisor.command('grab_screw', screw_id)
+            self.bridge.attach(attached_screw(screw_id, self.geometry))
+            self._require_synchronized()
+            # The axial lift is mandatory even at far tray cells: continuing
+            # laterally with the screw at tray depth would drag it across the
+            # tray walls on real hardware.
+            self._retreat(goal, f'pick_screw({screw_id})')
+            self._emit('pick_screw', object=screw_id, phase='done')
+            return OperationResult(True, 'pick_screw', f'{screw_id} grasped', tuple(self.events))
+        except Exception as error:
+            self._emit('pick_screw', object=screw_id, phase='failed', error=str(error))
+            raise
+
+    def handoff_screw(self, screw_id: str, hole_id: str) -> OperationResult:
+        def_name = screw_object_id(screw_id)
+        self._emit('handoff_screw', object=screw_id, target=hole_id, phase='start')
+        try:
+            self._require_synchronized()
+            hole = world_target(self.spec, self.geometry, f'hole:{hole_id}')
+            goal = tcp_goal(hole, self.geometry.screw_tcp_to_node)
+            self._contact_with_recovery(
+                goal, f'handoff_screw({screw_id},{hole_id})',
+                remove_before_contact=def_name,
+                remove_phase='contact',
+            )
+            self.supervisor.command(
+                'handoff_screw', screw_id, target=hole, target_name=hole_id,
+            )
+            self._require_synchronized()
+            # Always lift axially out of the handoff pose before the route
+            # continues: a joint-space or lateral move at hole height would
+            # drag the magnet across the body on real hardware.
+            self._retreat(goal, f'handoff_screw({screw_id},{hole_id})')
+            self._emit('delivered', object=screw_id, target=hole_id,
+                       phase='done', hole=hole_id)
+            return OperationResult(
+                True, 'handoff_screw', f'{screw_id} delivered to {hole_id}',
+                tuple(self.events),
+            )
+        except Exception as error:
+            self._emit('handoff_screw', object=screw_id, target=hole_id,
+                       phase='failed', error=str(error))
+            raise
+
+    # ------------------------------------------------------------------ tightening
+    def tighten_hole(self, hole_id: str, twist_deg: float = 45.0) -> OperationResult:
+        """Simulate tightening at an empty hole with the torque tool.
+
+        The screw has already been handed to the virtual operator and removed
+        from the scene.  The torque tool approaches the hole, rotates around
+        its own axis, returns and retracts.  Nothing is inserted or retained,
+        so the hole stays empty.
+        """
+        self._emit('tighten_simulated', object=hole_id, phase='start',
+                   twist_deg=twist_deg)
+        try:
+            self._require_synchronized()
+            hole = world_target(self.spec, self.geometry, f'hole:{hole_id}')
+            # Reuse the calibrated handoff pose: an upright tool node 4 mm
+            # above the hole with the same body yaw as the screw delivery.
+            target = object_pose_transform(
+                hole.xyz[0], hole.xyz[1], hole.xyz[2] + 0.004,
+                self.spec.body.yaw_deg, child='tighten_target',
+            )
+            tip = self.geometry.tool_tcp_to_nodes['torque_tool']
+            goal = tcp_goal(target, tip)
+            self._contact_with_recovery(goal, f'tighten_hole({hole_id})')
+            twist_error = None
+            twist_done = False
+            joint_twist = False
+            twisted_goal = goal
+            # Prefer a pure flange rotation: it twists the tool about its own
+            # axis without moving the arm to another IK branch.  A Cartesian
+            # twist can push joint 6 into its limit, after which 'work' is no
+            # longer reachable.
+            joints = None
+            provider = getattr(self.motion, 'joint_positions', None)
+            if provider is not None:
+                joints = provider()
+            if joints is not None and len(joints) == 7:
+                joints = tuple(float(value) for value in joints)
+                for sign in (1.0, -1.0):
+                    target_joints = list(joints)
+                    target_joints[6] += math.radians(sign * twist_deg)
+                    self.motion.wait_stationary()
+                    result = self.motion.move_joints(
+                        target_joints, self.geometry.speeds['contour'],
+                        f'tighten_hole({hole_id}):joint_twist',
+                    )
+                    if getattr(result, 'success', False):
+                        twist_done = True
+                        joint_twist = True
+                        self._emit('tighten_twist', object=hole_id,
+                                   joint=7, delta_deg=sign * twist_deg)
+                        break
+                    reset = getattr(self.motion, 'reset_stop', None)
+                    if reset is not None:
+                        reset()
+            if not twist_done:
+                for suffix, offset in (
+                    ('twist:positive', twist_deg),
+                    ('twist:negative', -twist_deg),
+                ):
+                    candidate = goal.compose_relaxed(Transform.from_xyz_rpy(
+                        [0.0, 0.0, 0.0], [0.0, 0.0, offset],
+                        parent='tcp', child='tighten_twist',
+                    ))
+                    self.motion.wait_stationary()
+                    try:
+                        self._move(
+                            candidate, 'lin', self.geometry.speeds['contour'],
+                            f'tighten_hole({hole_id}):{suffix}',
+                        )
+                        twist_done = True
+                        twisted_goal = candidate
+                        break
+                    except OperationError as error:
+                        # Holes on the far side of the body can sit against
+                        # the wrist limit in one direction.  A failed plan
+                        # executes no motion, so the opposite twist is safe.
+                        if 'Планирование' not in str(error):
+                            raise
+                        self.motion.reset_stop()
+                        twist_error = error
+                if not twist_done:
+                    raise twist_error
+            # Lift axially out of the hole BEFORE undoing the twist: rotating
+            # the flange back at the contact point would visibly unscrew the
+            # simulated tightening.
+            self._retreat(twisted_goal, f'tighten_hole({hole_id})')
+            if joint_twist:
+                # Return only joint 7 to its pre-twist value; the other
+                # joints stay where the retreat left them.
+                current = provider() if provider is not None else None
+                self._require(
+                    current is not None and len(current) == 7,
+                    f'tighten_hole({hole_id}): joint state unavailable for '
+                    'the untwist',
+                )
+                untwist_joints = list(current)
+                untwist_joints[6] = joints[6]
+                self.motion.wait_stationary()
+                untwist = self.motion.move_joints(
+                    untwist_joints, self.geometry.speeds['contour'],
+                    f'tighten_hole({hole_id}):joint_untwist',
+                )
+                if not getattr(untwist, 'success', False):
+                    raise OperationError(
+                        f'tighten_hole({hole_id}): the joint untwist failed: '
+                        f'{untwist.message}'
+                    )
+            else:
+                # Pure rotation about the tool axis at the safe height; the
+                # raised untwisted pose shares the retreat position.
+                self._move(
+                    self._above(goal, self.geometry.transfer_gap), 'lin',
+                    self.geometry.speeds['contour'],
+                    f'tighten_hole({hole_id}):untwist',
+                )
+            self._emit('tighten_simulated', object=hole_id, phase='done',
+                       twist_deg=twist_deg)
+            return OperationResult(
+                True, 'tighten_simulated',
+                f'{hole_id} tightening simulated', tuple(self.events),
+            )
+        except Exception as error:
+            self._emit('tighten_simulated', object=hole_id, 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.restore_world('grant_cap')
+                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.restore_world(def_name)
+                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,
+        tolerance: float | None = None,
+    ) -> dict[str, Any]:
+        self.supervisor.command('inspect')
+        target = target_pose or world_target(self.spec, self.geometry, 'cap_seat')
+        state = self._expect_latest()
+        cap = state.get('cap') or {}
+        if cap.get('state') != 'installed':
+            return {'ok': False, 'reason': f'hatch state is {cap.get("state")!r}'}
+        pose_entry = cap.get('pose')
+        if not pose_entry:
+            return {'ok': False, 'reason': 'hatch pose is unknown'}
+        pose = _transform_from_payload(pose_entry, child='cap')
+        distance = float(
+            (pose.position - target.position) @ (pose.position - target.position)
+        ) ** 0.5
+        limit = (
+            self.geometry.position_tolerance if tolerance is None else float(tolerance)
+        )
+        ok = distance <= limit
+        return {
+            'ok': ok,
+            'distance': distance,
+            'target': target.xyz,
+            'actual': pose.xyz,
+        }
+
+    def inspect_sealant(self) -> dict[str, Any]:
+        expected = list(self.spec.sealant_sides)
+        missing = [side for side in expected if side not in self.completed_sealant]
+        return {
+            'ok': not missing,
+            'completed': sorted(self.completed_sealant),
+            'missing': missing,
+        }
+
+    def inspect_state(self) -> dict[str, Any]:
+        self.supervisor.command('inspect')
+        return self._expect_latest()

src/iiwa_experement/iiwa_experement/planning_scene.py

@@ -0,0 +1,948 @@
+"""Mirror the Webots scene into MoveIt and keep attached objects in sync.
+
+The pure builders in this module are used by the unit tests; the
+``PlanningSceneBridge`` class is the thin ROS adapter used by the reference
+runner.  A trajectory is allowed only after ``assert_synchronized`` has seen
+the expected object ids in the monitored planning scene.
+"""
+from __future__ import annotations
+
+import copy
+import math
+import time
+import xml.etree.ElementTree as ET
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Callable, Iterable, Sequence
+
+import numpy as np
+import rclpy
+from ament_index_python.packages import get_package_share_directory
+from geometry_msgs.msg import Pose
+from moveit_msgs.msg import (
+    AllowedCollisionEntry,
+    AttachedCollisionObject,
+    CollisionObject,
+    PlanningScene,
+)
+from moveit_msgs.srv import GetPlanningScene
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from rclpy.parameter import Parameter
+from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
+from shape_msgs.msg import SolidPrimitive
+
+from .geometry import Box, GeometrySpec, Transform, object_pose_transform
+from .scenario import ScenarioSpec
+
+
+SCENE_TOPIC = '/planning_scene'
+MONITORED_TOPIC = '/monitored_planning_scene'
+GET_SCENE_SERVICE = '/get_planning_scene'
+SCENE_FRAME = 'base_link'
+# The tool is rigidly bolted to the flange, so contact between its collision
+# box and the wrist/flange links is expected and allowed.  Without 'link7'
+# the wide vacuum box reads as a false collision against the wrist housing.
+TOUCH_LINKS = (
+    'tcp', 'patron', 'link7', 'camera_holder', 'camera_corner',
+    'camera_hand', 'camera_optical_frame',
+)
+ROBOT_LINKS = (
+    'base_link', 'link1', 'link2', 'link3', 'link4', 'link5', 'link6',
+    'link7', 'patron', 'camera_holder', 'camera_corner', 'camera_hand',
+    'camera_optical_frame', 'tcp',
+)
+DEFAULT_SRDF = 'iiwa_config/config/moveit/iiwa7.srdf'
+
+
+def load_srdf_disabled_pairs(path) -> list[tuple[str, str]]:
+    root = ET.parse(str(path)).getroot()
+    pairs = []
+    for element in root.findall('disable_collisions'):
+        first = element.get('link1')
+        second = element.get('link2')
+        if first and second:
+            pairs.append((first, second))
+    return pairs
+
+
+def build_collision_matrix(
+    object_ids: Iterable[str],
+    *,
+    disabled_pairs: Iterable[tuple[str, str]] = (),
+    extra_allowed: Iterable[tuple[str, str]] = (),
+) -> tuple[list[str], list[list[bool]]]:
+    """Build a complete ACM: only explicitly disabled pairs are allowed.
+
+    This mirrors the SRDF and keeps every other robot/object and object/object
+    pair checked.  It replaces an empty ACM that would otherwise make the
+    planning scene treat adjacent robot links as collisions.
+    """
+    names = list(ROBOT_LINKS)
+    for name in object_ids:
+        if name not in names:
+            names.append(name)
+    matrix = [[False] * len(names) for _ in names]
+    for index in range(len(names)):
+        matrix[index][index] = True
+    for first, second in list(disabled_pairs) + list(extra_allowed):
+        if first in names and second in names:
+            row = names.index(first)
+            column = names.index(second)
+            matrix[row][column] = True
+            matrix[column][row] = True
+    return names, matrix
+
+
+class SceneSynchronizationError(RuntimeError):
+    """The MoveIt scene does not match the requested object set."""
+
+
+def screw_object_id(screw_id: str) -> str:
+    return f'grant_screw_{int(screw_id.split("_")[1]):02d}'
+
+
+# Lift from an object node pose to the collision-box centre, matching
+# build_world_objects.  A node origin sits on the lower face/axis of the OBJ
+# and is rotated so the local Y axis becomes world Z; the collision box is a
+# flat, yaw-only box centred above the support surface.
+#
+# The installed hatch is sandwiched between two hard constraints: its box
+# must clear the conservative body box below AND the suction-cup box above at
+# the place contact pose.  A zero-distance touch on either side reads as a
+# scene contact and blocks every later plan from that state, so the world
+# model of the hatch is 1 mm thinner than the real plate, leaving 0.5 mm of
+# clearance on both sides.
+CAP_BOX_LIFT = 0.004165
+CAP_BOX_THICKNESS = 0.00267
+SCREW_BOX_LIFT = 0.0055
+TOOL_BOX_LIFT = 0.070
+
+
+def cap_world_box(geometry: GeometrySpec) -> Box:
+    """Collision box of the hatch as a world object (thinner than the plate)."""
+    return Box(
+        size=(
+            geometry.cap_collision.size[0],
+            geometry.cap_collision.size[1],
+            CAP_BOX_THICKNESS,
+        ),
+        offset=geometry.cap_collision.offset,
+    )
+
+
+def placed_cap_pose(seat: Transform, body_yaw_deg: float) -> Transform:
+    """Collision-box pose of the installed hatch: flat, centred on the plate."""
+    return Transform.from_xyz_rpy(
+        [seat.xyz[0], seat.xyz[1], seat.xyz[2] + CAP_BOX_LIFT],
+        [0.0, 0.0, body_yaw_deg],
+        parent='world',
+        child='cap',
+    )
+
+
+@dataclass(frozen=True)
+class SceneObject:
+    object_id: str
+    box: Box
+    pose: Transform
+
+
+@dataclass(frozen=True)
+class AttachedSpec:
+    object_id: str
+    box: Box
+    relative_pose: Transform
+    touch_links: tuple[str, ...]
+    frame_id: str | None = None
+
+
+def _box_at(pose: Transform, box: Box) -> Box:
+    del pose
+    return box
+
+
+def _pose_with_offset(pose: Transform, offset) -> Transform:
+    translation = pose.position + pose.matrix @ np.asarray(offset, dtype=float)
+    return Transform(
+        xyz=tuple(float(value) for value in translation),
+        rotation=pose.rotation,
+        parent=pose.parent,
+        child=pose.child,
+    )
+
+
+def _rotated_pose(x: float, y: float, z: float, yaw_deg: float) -> Transform:
+    return Transform.from_xyz_rpy(
+        [x, y, z], [0.0, 0.0, yaw_deg], parent='world', child='scene_object',
+    )
+
+
+def build_world_objects(spec: ScenarioSpec, geometry: GeometrySpec) -> list[SceneObject]:
+    """Return the collision objects present in the initial Webots scene."""
+    objects: list[SceneObject] = []
+    table = geometry.scene_collisions['table']
+    objects.append(SceneObject(
+        'grant_table', table, _rotated_pose(0.0, 0.0, table.offset[2], 0.0),
+    ))
+
+    fixture = geometry.scene_collisions['fixture_base']
+    objects.append(SceneObject(
+        'grant_fixture',
+        fixture,
+        _rotated_pose(
+            spec.fixture.x,
+            spec.fixture.y,
+            spec.fixture.z + fixture.offset[2],
+            spec.fixture.yaw_deg,
+        ),
+    ))
+    rail = geometry.scene_collisions['fixture_rail']
+    sine = math.sin(math.radians(spec.fixture.yaw_deg))
+    cosine = math.cos(math.radians(spec.fixture.yaw_deg))
+    for sign, suffix in ((-1.0, 'a'), (1.0, 'b')):
+        objects.append(SceneObject(
+            f'grant_fixture_rail_{suffix}',
+            rail,
+            _rotated_pose(
+                spec.fixture.x + sign * rail.offset[0] * cosine,
+                spec.fixture.y + sign * rail.offset[0] * sine,
+                spec.fixture.z + rail.offset[2],
+                spec.fixture.yaw_deg,
+            ),
+        ))
+
+    body = geometry.scene_collisions['body']
+    objects.append(SceneObject(
+        'grant_body',
+        body,
+        _rotated_pose(
+            spec.body.x,
+            spec.body.y,
+            spec.body.z + body.offset[2],
+            spec.body.yaw_deg,
+        ),
+    ))
+
+    rack = geometry.scene_collisions['rack']
+    objects.append(SceneObject(
+        'grant_rack',
+        rack,
+        _rotated_pose(
+            spec.rack.x,
+            spec.rack.y,
+            spec.rack.z + rack.offset[2],
+            spec.rack.yaw_deg,
+        ),
+    ))
+
+    if spec.tray_pose is not None:
+        tray = spec.tray_pose
+        base = geometry.scene_collisions['tray_base']
+        objects.append(SceneObject(
+            'grant_tray',
+            base,
+            _rotated_pose(
+                tray.x, tray.y, tray.z + base.offset[2], tray.yaw_deg,
+            ),
+        ))
+        wall_x = geometry.scene_collisions['tray_wall_x']
+        for sign, suffix in ((-1.0, 'a'), (1.0, 'b')):
+            dx, dy = math.cos(math.radians(tray.yaw_deg)) * sign * wall_x.offset[0], \
+                math.sin(math.radians(tray.yaw_deg)) * sign * wall_x.offset[0]
+            objects.append(SceneObject(
+                f'grant_tray_wall_x_{suffix}',
+                wall_x,
+                _rotated_pose(
+                    tray.x + dx,
+                    tray.y + dy,
+                    tray.z + wall_x.offset[2],
+                    tray.yaw_deg,
+                ),
+            ))
+        wall_y = geometry.scene_collisions['tray_wall_y']
+        for sign, suffix in ((-1.0, 'a'), (1.0, 'b')):
+            dx = -math.sin(math.radians(tray.yaw_deg)) * sign * wall_y.offset[1]
+            dy = math.cos(math.radians(tray.yaw_deg)) * sign * wall_y.offset[1]
+            objects.append(SceneObject(
+                f'grant_tray_wall_y_{suffix}',
+                wall_y,
+                _rotated_pose(
+                    tray.x + dx,
+                    tray.y + dy,
+                    tray.z + wall_y.offset[2],
+                    tray.yaw_deg,
+                ),
+            ))
+        for screw in spec.screws:
+            objects.append(SceneObject(
+                screw_object_id(screw.name),
+                geometry.screw_collision,
+                _rotated_pose(
+                    screw.pose.x,
+                    screw.pose.y,
+                    screw.pose.z + SCREW_BOX_LIFT,
+                    screw.pose.yaw_deg,
+                ),
+            ))
+
+    for tool in spec.tools:
+        collision = geometry.tool_collisions[tool.name]
+        size = (collision.size[0], collision.size[1], 0.140)
+        objects.append(SceneObject(
+            f'grant_{tool.name}',
+            Box(size=size, offset=(0.0, 0.0, 0.070)),
+            _rotated_pose(
+                tool.pose.x, tool.pose.y, tool.pose.z + TOOL_BOX_LIFT,
+                tool.pose.yaw_deg + 180.0,
+            ),
+        ))
+
+    if spec.cap_state != 'absent':
+        cap_pose = spec.cap_pose
+        assert cap_pose is not None
+        objects.append(SceneObject(
+            'grant_cap',
+            cap_world_box(geometry),
+            _rotated_pose(
+                cap_pose.x, cap_pose.y, cap_pose.z + CAP_BOX_LIFT,
+                cap_pose.yaw_deg,
+            ),
+        ))
+    return objects
+
+
+def attached_tool(name: str, geometry: GeometrySpec) -> AttachedSpec:
+    collision = geometry.tool_collisions[name]
+    return AttachedSpec(
+        object_id=f'grant_{name}',
+        box=Box(
+            size=(collision.size[0], collision.size[1], 0.140),
+            offset=collision.offset,
+        ),
+        relative_pose=Transform.from_xyz_rpy(
+            [0.0, 0.0, collision.offset[2] + 0.0552],
+            [0.0, 0.0, 0.0],
+            parent='patron',
+            child=f'grant_{name}',
+        ),
+        touch_links=TOUCH_LINKS,
+    )
+
+
+def attached_cap(geometry: GeometrySpec) -> AttachedSpec:
+    # The plate box stops 1 mm short of the support surface (body/table at
+    # the grasp pose): a zero-distance touch reads as a scene contact and
+    # blocks the mandatory axial retreat right after the grasp.
+    box = Box(
+        size=(geometry.cap_collision.size[0], geometry.cap_collision.size[1],
+              0.00267),
+        offset=(0.0, 0.0, 0.141335),
+    )
+    return AttachedSpec(
+        object_id='grant_cap',
+        box=box,
+        relative_pose=Transform.from_xyz_rpy(
+            [0.0, 0.0, 0.141335 + 0.0552],
+            [0.0, 0.0, 0.0],
+            parent='patron',
+            child='grant_cap',
+        ),
+        touch_links=TOUCH_LINKS,
+    )
+
+
+def attached_screw(screw_id: str, geometry: GeometrySpec) -> AttachedSpec:
+    # Same 1 mm clearance at the tray floor as in attached_cap.
+    box = Box(size=(0.0055, 0.0055, 0.008), offset=(0.0, 0.0, 0.144))
+    object_id = screw_object_id(screw_id)
+    return AttachedSpec(
+        object_id=object_id,
+        box=box,
+        relative_pose=Transform.from_xyz_rpy(
+            [0.0, 0.0, 0.144 + 0.0552],
+            [0.0, 0.0, 0.0],
+            parent='patron',
+            child=object_id,
+        ),
+        touch_links=TOUCH_LINKS,
+    )
+
+
+def _pose_message(transform: Transform) -> Pose:
+    from scipy.spatial.transform import Rotation
+
+    quaternion = Rotation.from_matrix(transform.matrix).as_quat()
+    pose = Pose()
+    pose.position.x, pose.position.y, pose.position.z = (
+        float(transform.xyz[0]),
+        float(transform.xyz[1]),
+        float(transform.xyz[2]),
+    )
+    pose.orientation.x = float(quaternion[0])
+    pose.orientation.y = float(quaternion[1])
+    pose.orientation.z = float(quaternion[2])
+    pose.orientation.w = float(quaternion[3])
+    return pose
+
+
+def collision_object(
+    object_id: str,
+    box: Box,
+    pose: Transform,
+    *,
+    operation: int = CollisionObject.ADD,
+) -> CollisionObject:
+    message = CollisionObject()
+    message.header.frame_id = SCENE_FRAME
+    message.id = object_id
+    message.operation = operation
+    message.pose = _pose_message(pose)
+    primitive = SolidPrimitive()
+    primitive.type = SolidPrimitive.BOX
+    primitive.dimensions = [float(value) for value in box.size]
+    message.primitives.append(primitive)
+    identity = Pose()
+    identity.orientation.w = 1.0
+    message.primitive_poses.append(identity)
+    return message
+
+
+def attached_object_message(
+    spec: AttachedSpec,
+    *,
+    link_name: str | None = None,
+    operation: int = CollisionObject.ADD,
+) -> AttachedCollisionObject:
+    message = AttachedCollisionObject()
+    message.link_name = link_name or spec.relative_pose.parent
+    message.object = collision_object(
+        spec.object_id, spec.box, spec.relative_pose, operation=operation,
+    )
+    # For attached objects the object pose is relative to the link.
+    message.object.header.frame_id = (
+        spec.frame_id if spec.frame_id is not None else message.link_name
+    )
+    message.touch_links = list(spec.touch_links)
+    return message
+
+
+def update_contact_entry(
+    names: Sequence[str],
+    values,
+    first: str,
+    second: str,
+    allowed: bool,
+) -> tuple[list[str], list[list[bool]]]:
+    """Return a copy of an ACM with exactly one pair changed."""
+    names = list(names)
+    matrix = [list(row) for row in values]
+    for name in (first, second):
+        if name not in names:
+            names.append(name)
+            for row in matrix:
+                row.append(allowed)
+            matrix.append([allowed] * len(names))
+    first_index = names.index(first)
+    second_index = names.index(second)
+    matrix[first_index][second_index] = allowed
+    matrix[second_index][first_index] = allowed
+    return names, matrix
+
+
+def scene_ids(scene: PlanningScene) -> tuple[set[str], set[str]]:
+    world = {obj.id for obj in scene.world.collision_objects}
+    attached = {obj.object.id for obj in scene.robot_state.attached_collision_objects}
+    return world, attached
+
+
+class PlanningSceneBridge:
+    """Thin ROS adapter around a ``moveit_msgs/PlanningScene`` diff stream."""
+
+    def __init__(
+        self,
+        *,
+        node: Node | None = None,
+        use_sim_time: bool = True,
+        srdf_path: str | None = None,
+        spin_fn: Callable[[float], None] | None = None,
+        on_timing: Callable[[dict], None] | None = None,
+    ) -> None:
+        self._owns_node = node is None
+        self._on_timing = on_timing
+        if node is None:
+            node = Node('planning_scene_bridge')
+            node.set_parameters([Parameter(
+                'use_sim_time', value=use_sim_time,
+            )])
+        self.node = node
+        self._spin_fn = spin_fn
+        self.geometry: GeometrySpec | None = None
+        self.spec: ScenarioSpec | None = None
+        self.base_to_world: Transform | None = None
+        self.last_scene: PlanningScene | None = None
+        self._publisher = self.node.create_publisher(
+            PlanningScene, SCENE_TOPIC, 10,
+        )
+        self._subscription = self.node.create_subscription(
+            PlanningScene,
+            MONITORED_TOPIC,
+            self._on_scene,
+            QoSProfile(
+                depth=10,
+                reliability=ReliabilityPolicy.RELIABLE,
+                durability=DurabilityPolicy.VOLATILE,
+            ),
+        )
+        self._get_scene = self.node.create_client(
+            GetPlanningScene, GET_SCENE_SERVICE,
+        )
+        self._world_objects: dict[str, SceneObject] = {}
+        self._initial_world: dict[str, SceneObject] = {}
+        self._attached: dict[str, AttachedSpec] = {}
+        self._seen_world: set[str] = set()
+        self._seen_attached: set[str] = set()
+        self._latest_joint_state = None
+        self._last_full_scene: PlanningScene | None = None
+        self._acm_names: list[str] = []
+        self._acm_values: list[list[bool]] = []
+        self._disabled_pairs = []
+        srdf = srdf_path or str(
+            Path(get_package_share_directory('iiwa_config'))
+            / 'config' / 'moveit' / 'iiwa7.srdf'
+        )
+        try:
+            self._disabled_pairs = load_srdf_disabled_pairs(srdf)
+        except Exception:
+            self._disabled_pairs = []
+        self._joint_subscription = self.node.create_subscription(
+            JointState, '/joint_states', self._on_joint_state, 10,
+        )
+
+    def _on_joint_state(self, message: JointState) -> None:
+        self._latest_joint_state = message
+
+    def _on_scene(self, message: PlanningScene) -> None:
+        if not message.is_diff:
+            self._last_full_scene = copy.deepcopy(message)
+        self._seen_world.update(
+            obj.id for obj in message.world.collision_objects
+        )
+        self._seen_attached.update(
+            entry.object.id
+            for entry in message.robot_state.attached_collision_objects
+        )
+        if not message.is_diff:
+            self.last_scene = copy.deepcopy(message)
+            return
+        if self.last_scene is None:
+            self.last_scene = PlanningScene()
+            self.last_scene.is_diff = False
+        scene = self.last_scene
+        world = {obj.id: obj for obj in scene.world.collision_objects}
+        for obj in message.world.collision_objects:
+            if obj.operation == CollisionObject.REMOVE:
+                world.pop(obj.id, None)
+            else:
+                world[obj.id] = copy.deepcopy(obj)
+        scene.world.collision_objects = list(world.values())
+        attached = {
+            entry.object.id: entry
+            for entry in scene.robot_state.attached_collision_objects
+        }
+        for entry in message.robot_state.attached_collision_objects:
+            if entry.object.operation == CollisionObject.REMOVE:
+                attached.pop(entry.object.id, None)
+            else:
+                attached[entry.object.id] = copy.deepcopy(entry)
+        scene.robot_state.attached_collision_objects = list(attached.values())
+
+    def _spin_once(self, timeout_sec: float) -> None:
+        """Let the owning executor spin, or spin locally in the reference run."""
+        if self._spin_fn is not None:
+            self._spin_fn(timeout_sec)
+            return
+        rclpy.spin_once(self.node, timeout_sec=timeout_sec)
+
+    def _spin(self, seconds: float = 0.05) -> None:
+        end = time.monotonic() + seconds
+        while time.monotonic() < end:
+            self._spin_once(0.01)
+
+    def _publish(self, scene: PlanningScene) -> None:
+        self._seen_world.clear()
+        self._seen_attached.clear()
+        self._publisher.publish(scene)
+        # A short non-blocking turn lets rclpy deliver the message to local
+        # subscribers.  The authoritative synchronization check happens in
+        # assert_synchronized() through /get_planning_scene.
+        self._spin_once(0.0)
+
+    def _fetch_acm(self) -> None:
+        if getattr(self, '_get_scene', None) is None or self.node is None:
+            return
+        if not self._get_scene.service_is_ready():
+            if not self._get_scene.wait_for_service(timeout_sec=1.0):
+                return
+        request = GetPlanningScene.Request()
+        request.components.components = 128
+        future = self._get_scene.call_async(request)
+        end = time.monotonic() + 3.0
+        while time.monotonic() < end and not future.done():
+            self._spin_once(0.05)
+        if future.done():
+            try:
+                matrix = future.result().scene.allowed_collision_matrix
+                self._acm_names = list(matrix.entry_names)
+                self._acm_values = [
+                    list(row.enabled) for row in matrix.entry_values
+                ]
+            except Exception:
+                pass
+
+    def _rebuild_acm(self) -> None:
+        extra = []
+        for spec in self._attached.values():
+            extra.extend((spec.object_id, link) for link in spec.touch_links)
+        self._acm_names, self._acm_values = build_collision_matrix(
+            list(self._world_objects) + list(self._attached),
+            disabled_pairs=self._disabled_pairs,
+            extra_allowed=extra,
+        )
+
+    def _full_scene(self) -> PlanningScene:
+        scene = PlanningScene()
+        scene.is_diff = False
+        # Never overwrite the planner's current robot state from a planning
+        # scene update: the monitor receives /joint_states directly and a
+        # stale embedded sample makes the next trajectory start from an old
+        # pose.  ``is_diff`` leaves the state untouched.
+        scene.robot_state.is_diff = True
+        for obj in self._world_objects.values():
+            scene.world.collision_objects.append(collision_object(
+                obj.object_id, obj.box, self._base_pose(obj.pose),
+            ))
+        for spec in self._attached.values():
+            scene.robot_state.attached_collision_objects.append(
+                attached_object_message(spec)
+            )
+        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
+            ]
+        return scene
+
+    def _refresh_joint_state(self) -> None:
+        """Make sure a measured joint sample exists before a full scene update.
+
+        The full scene is published with ``robot_state.is_diff=True`` so it
+        never overwrites MoveIt's current state.  Waiting for the *next*
+        sample on every scene update added a fixed delay between motions
+        without improving that guarantee; the executor already checks joint
+        state freshness immediately before every plan.
+        """
+        if self.node is None or self._latest_joint_state is not None:
+            return
+        end = time.monotonic() + 0.1
+        while time.monotonic() < end:
+            self._spin_once(0.01)
+            if self._latest_joint_state is not None:
+                return
+
+    def _relay_full_scene(self) -> None:
+        """Give MoveItPy the exact move_group scene, including its ACM.
+
+        MoveItPy's monitor always listens on ``/planning_scene`` even when its
+        configured topics differ.  Fetching the maintained scene from
+        move_group and republishing it once keeps both planners identical
+        without a feedback loop.
+        """
+        started = time.monotonic()
+        relayed = False
+        try:
+            relayed = self._relay_full_scene_impl()
+        finally:
+            on_timing = getattr(self, '_on_timing', None)
+            if on_timing is not None:
+                try:
+                    on_timing({
+                        'stage': 'planning_scene_relay',
+                        'started_at': started,
+                        'finished_at': time.monotonic(),
+                        'duration_sec': time.monotonic() - started,
+                        'relayed': relayed,
+                    })
+                except Exception:
+                    pass
+
+    def _relay_full_scene_impl(self) -> bool:
+        if getattr(self, '_get_scene', None) is None or self.node is None:
+            return False
+        if not self._get_scene.service_is_ready():
+            if not self._get_scene.wait_for_service(timeout_sec=1.0):
+                return False
+        expected_world = set(self._world_objects)
+        expected_attached = set(self._attached)
+        deadline = time.monotonic() + 3.0
+        while time.monotonic() < deadline:
+            request = GetPlanningScene.Request()
+            request.components.components = 2 | 4 | 8 | 16 | 128
+            future = self._get_scene.call_async(request)
+            end = time.monotonic() + 1.0
+            while time.monotonic() < end and not future.done():
+                self._spin_once(0.05)
+            if not future.done():
+                continue
+            try:
+                scene = future.result().scene
+            except Exception:
+                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)
+                self._spin()
+                return True
+        # Never relay a stale scene: that would wipe the objects from the
+        # MoveItPy monitor and make the next motion unverified.
+        return False
+
+    def _publish_full(self) -> None:
+        self._refresh_joint_state()
+        self._publish(self._full_scene())
+        self._relay_full_scene()
+
+    @staticmethod
+    def _diff() -> PlanningScene:
+        scene = PlanningScene()
+        scene.is_diff = True
+        scene.robot_state.is_diff = True
+        return scene
+
+    def update_world_to_base(self, world_to_base: Transform) -> None:
+        self.base_to_world = world_to_base.inverse()
+
+    def _base_pose(self, pose: Transform) -> Transform:
+        if self.base_to_world is None:
+            raise SceneSynchronizationError('base transform is not initialised')
+        return self.base_to_world.compose_relaxed(pose)
+
+    def initialize(
+        self,
+        spec: ScenarioSpec,
+        geometry: GeometrySpec,
+        *,
+        world_to_base: Transform | None = None,
+    ) -> None:
+        self.spec = spec
+        self.geometry = geometry
+        self.base_to_world = (world_to_base or geometry.world_to_base).inverse()
+        self._world_objects = {
+            obj.object_id: obj for obj in build_world_objects(spec, geometry)
+        }
+        self._initial_world = dict(self._world_objects)
+        self._attached.clear()
+        extra = []
+        for spec in self._attached.values():
+            extra.extend((spec.object_id, link) for link in spec.touch_links)
+        self._acm_names, self._acm_values = build_collision_matrix(
+            list(self._world_objects) + list(self._attached),
+            disabled_pairs=self._disabled_pairs,
+            extra_allowed=extra,
+        )
+        self._publish_full()
+        self.assert_synchronized(
+            self._world_objects, (), seen_world=set(self._world_objects),
+        )
+
+    def _publish_attached_removal(self, object_id: str) -> None:
+        """Send an explicit attached-object removal diff.
+
+        Full scenes carry the attached list inside ``robot_state`` with
+        ``is_diff=True``; a consumer that applies that list as a diff never
+        treats an omission as a removal and keeps a ghost attachment.  The
+        explicit REMOVE entry is unambiguous for every consumer.
+        """
+        publisher = getattr(self, '_publisher', None)
+        if publisher is None or self.node is None:
+            return
+        scene = self._diff()
+        removal = AttachedCollisionObject()
+        removal.link_name = 'patron'
+        removal.object.id = object_id
+        removal.object.operation = CollisionObject.REMOVE
+        scene.robot_state.attached_collision_objects.append(removal)
+        publisher.publish(scene)
+        self._spin()
+
+    def attach(self, spec: AttachedSpec) -> None:
+        self._world_objects.pop(spec.object_id, None)
+        self._attached[spec.object_id] = spec
+        self._rebuild_acm()
+        self._publish_full()
+
+    def detach(self, object_id: str, world_pose: Transform) -> None:
+        if object_id not in self._attached:
+            raise SceneSynchronizationError(
+                f'cannot detach {object_id!r}: it is not attached'
+            )
+        spec = self._attached.pop(object_id)
+        self._publish_attached_removal(object_id)
+        object_spec = SceneObject(object_id, spec.box, world_pose)
+        self._world_objects[object_id] = object_spec
+        self._rebuild_acm()
+        self._publish_full()
+
+    def remove(self, object_id: str) -> None:
+        if self._attached.pop(object_id, None) is not None:
+            self._publish_attached_removal(object_id)
+        self._world_objects.pop(object_id, None)
+        self._rebuild_acm()
+        self._publish_full()
+
+    def add_world(self, object_id: str, box: Box, world_pose: Transform) -> None:
+        if self._attached.pop(object_id, None) is not None:
+            self._publish_attached_removal(object_id)
+        self._world_objects[object_id] = SceneObject(object_id, box, world_pose)
+        self._rebuild_acm()
+        self._publish_full()
+
+    def restore_world(self, object_id: str) -> None:
+        """Return an object to the exact box pose it had at scene initialisation.
+
+        Use this when an object physically returns to its calibrated support
+        pose (a tool docked in its socket, a part restored after a simulated
+        grasp failure): the initial pose already carries the correct
+        box-centre lift and flat orientation.
+        """
+        original = self._initial_world.get(object_id)
+        if original is None:
+            raise SceneSynchronizationError(
+                f'cannot restore {object_id!r}: it was not in the initial scene'
+            )
+        if self._attached.pop(object_id, None) is not None:
+            self._publish_attached_removal(object_id)
+        self._world_objects[object_id] = original
+        self._rebuild_acm()
+        self._publish_full()
+
+    def allow_contact(self, first: str, second: str, allowed: bool = True) -> None:
+        """Change exactly one ACM pair; all other checks stay active."""
+        if self.last_scene is None or not self.last_scene.allowed_collision_matrix.entry_names:
+            raise SceneSynchronizationError(
+                'cannot update contact permissions without a monitored scene'
+            )
+        if not self._acm_names:
+            raise SceneSynchronizationError(
+                'cannot update contact permissions without an ACM'
+            )
+        self._acm_names, self._acm_values = update_contact_entry(
+            self._acm_names, self._acm_values, first, second, allowed,
+        )
+        self._publish_full()
+
+    def expected_sets(self) -> tuple[set[str], set[str]]:
+        return set(self._world_objects), set(self._attached)
+
+    def _query_scene(self, timeout_sec: float) -> tuple[set[str], set[str]] | None:
+        if getattr(self, '_get_scene', None) is None or self.node is None:
+            return
+        if not self._get_scene.service_is_ready():
+            if not self._get_scene.wait_for_service(timeout_sec=1.0):
+                return None
+        request = GetPlanningScene.Request()
+        request.components.components = 8 | 16 | 4 | 128
+        future = self._get_scene.call_async(request)
+        end = time.monotonic() + timeout_sec
+        while time.monotonic() < end and not future.done():
+            self._spin_once(0.05)
+        if not future.done():
+            return None
+        try:
+            response = future.result()
+        except Exception:
+            return None
+        return scene_ids(response.scene)
+
+    def assert_synchronized(
+        self,
+        expected_world: Iterable[str] | None = None,
+        expected_attached: Iterable[str] | None = None,
+        *,
+        seen_world: Iterable[str] | None = None,
+        seen_attached: Iterable[str] | None = None,
+        timeout_sec: float = 8.0,
+    ) -> None:
+        world_expected = set(expected_world) if expected_world is not None else set()
+        attached_expected = (
+            set(expected_attached) if expected_attached is not None else set()
+        )
+        world_seen_expected = set(seen_world) if seen_world is not None else set()
+        attached_seen_expected = (
+            set(seen_attached)
+            if seen_attached is not None
+            else set(attached_expected)
+        )
+        end = time.monotonic() + timeout_sec
+        last_world: set[str] = set()
+        last_attached: set[str] = set()
+        while time.monotonic() < end:
+            self._spin_once(0.05)
+            # 1. Every expected id must have appeared in a monitored scene
+            #    (published by the MoveItPy or move_group monitor).
+            served = (
+                world_seen_expected <= self._seen_world
+                and attached_seen_expected <= self._seen_attached
+            )
+            # 2. The move_group service is the authoritative full snapshot.
+            snapshot = self._query_scene(timeout_sec=1.0)
+            if snapshot is not None:
+                last_world, last_attached = snapshot
+                if world_expected <= last_world and attached_expected <= last_attached:
+                    if not served:
+                        # move_group already has the scene, but MoveItPy's
+                        # monitor also needs to have seen the same ids before
+                        # the next plan is allowed.
+                        continue
+                    return
+            elif served:
+                if self.last_scene is not None:
+                    last_world, last_attached = scene_ids(self.last_scene)
+                return
+        raise SceneSynchronizationError(
+            'MoveIt scene is not synchronized: '
+            f'missing world={sorted(world_expected - last_world)}, '
+            f'missing attached={sorted(attached_expected - last_attached)}; '
+            f'seen world={sorted(self._seen_world)}, '
+            f'seen attached={sorted(self._seen_attached)}'
+        )
+
+    def destroy(self) -> None:
+        if self._owns_node:
+            self.node.destroy_node()

src/iiwa_experement/iiwa_experement/reference_runner.py

@@ -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()

src/iiwa_experement/iiwa_experement/reference_scenarios.py

@@ -0,0 +1,290 @@
+"""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]:
+    # The route starts at 'work'; later tool changes reconfigure there first
+    # so the approach starts from a known branch.  The task itself follows
+    # directly from the tool exit.
+    steps: list[ReferenceStep] = []
+    if not first:
+        steps.append(_work())
+    steps.append(ReferenceStep('take_tool', (tool,)))
+    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.append(ReferenceStep('pick_cap'))
+    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', 'tolerance': 0.002}),
+            ReferenceStep('pick_cap'),
+            ReferenceStep('place_cap'),
+            ReferenceStep('inspect_cap', (), {'expect': 'ok'}),
+        ])
+    else:
+        steps.extend([
+            ReferenceStep('place_cap'),
+            ReferenceStep('inspect_cap', (), {'expect': 'ok'}),
+        ])
+    steps.append(ReferenceStep('dock_tool', ('vacuum',)))
+    return steps
+
+
+def _sealant_steps(
+    spec: ScenarioSpec,
+    *,
+    interrupted: bool = False,
+    first_tool: 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'}),
+        ReferenceStep('dock_tool', ('sealant_tool',)),
+    ])
+    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 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)),
+        ])
+    steps.append(_work())
+    steps.append(ReferenceStep('dock_tool', ('magnetic_tool',)))
+    if 'torque_tool' in spec.tool_names:
+        steps.append(_work())
+        steps.append(ReferenceStep('take_tool', ('torque_tool',)))
+        for hole in spec.screw_holes:
+            steps.append(ReferenceStep('tighten_hole', (hole,)))
+        steps.append(_work())
+        steps.append(ReferenceStep('dock_tool', ('torque_tool',)))
+    return steps
+
+
+def build_reference_sequence(spec: ScenarioSpec) -> tuple[ReferenceStep, ...]:
+    """Return the deterministic route for one experiment."""
+    experiment = spec.experiment_id
+    steps: list[ReferenceStep] = [_work()]
+
+    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,
+        ))
+        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,
+        ))
+        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,
+        ))
+
+    # The technical process always ends at 'work'; only the very last move is
+    # the final return to 'home'.
+    steps.append(_work())
+    steps.append(_home())
+    cleaned: list[ReferenceStep] = []
+    for step in steps:
+        if (
+            cleaned
+            and step.kind == 'move_named'
+            and step.args == ('work',)
+            and cleaned[-1].kind == 'move_named'
+            and cleaned[-1].args == ('work',)
+        ):
+            continue
+        cleaned.append(step)
+    return tuple(cleaned)
+
+
+def _planning_failure(message: str) -> bool:
+    text = message.lower()
+    return 'планирование' in text or 'planning' in text or 'недостижима' in text
+
+
+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 and _planning_failure(result.message):
+                    # A rejected plan executed no motion.  The start-state
+                    # guard can trip on a startup clock/telemetry race, so
+                    # wait for the arm to settle and retry exactly once.
+                    trace.event({'event': 'named_retry', 'step': step.as_dict(),
+                                 'reason': result.message})
+                    motion.wait_stationary(timeout_sec=5.0)
+                    motion.reset_stop()
+                    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':
+                kwargs = dict(step.kwargs)
+                expect = kwargs.pop('expect', None)
+                result = operations.inspect_cap(**kwargs)
+                trace.event({'event': 'inspect_cap', 'result': result})
+                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(),
+    )

src/iiwa_experement/iiwa_experement/runtime_config.py

@@ -0,0 +1,193 @@
+"""Create the per-launch world and settings copies.
+
+The installed world template keeps the two local PROTO references for source
+tree use.  Webots must open a copy whose local EXTERNPROTO paths are absolute,
+because the temporary world lives outside the installed package.  The same
+launch also needs a settings copy that points at that world and disables the
+legacy random object spawner.
+"""
+from __future__ import annotations
+
+import math
+import re
+from pathlib import Path
+from typing import Any
+
+import yaml
+
+
+EXPERIMENT_IDS = tuple(range(1, 21))
+MODES = ('view', 'execute', 'planner')
+DEFAULT_START_DELAY_SEC = 3.0
+DEFAULT_TRACE_DIR = '~/.ros/iiwa_experement/runs'
+LOCAL_PROTOS = (
+    'CollaborativeRobotTable.proto',
+    'WorkspaceLimiter.proto',
+)
+_EXTERNPROTO_RE = re.compile(r'(EXTERNPROTO\s+")([^"]+)(")')
+
+
+def validate_experiment_id(raw: str) -> int:
+    """Accept only a decimal experiment number in 1..20."""
+    text = str(raw).strip()
+    if not text.isdigit():
+        raise ValueError(f'experiment_id must be an integer in 1..20, got {raw!r}')
+    value = int(text)
+    if text != str(value):
+        raise ValueError(f'experiment_id must be a decimal integer, got {raw!r}')
+    if value not in EXPERIMENT_IDS:
+        raise ValueError(f'experiment_id {value} is outside 1..20')
+    return value
+
+
+def validate_mode(raw: str) -> str:
+    """Accept only the documented view/execute modes."""
+    text = str(raw).strip().lower()
+    if text not in MODES:
+        raise ValueError(f'mode must be one of {list(MODES)}, got {raw!r}')
+    return text
+
+
+def validate_start_delay(raw: str) -> float:
+    """Return a finite non-negative simulation-time delay."""
+    text = str(raw).strip()
+    try:
+        value = float(text)
+    except (TypeError, ValueError) as error:
+        raise ValueError(
+            f'start_delay_sec must be a finite non-negative number, got {raw!r}'
+        ) from error
+    if not math.isfinite(value) or value < 0.0:
+        raise ValueError(
+            f'start_delay_sec must be a finite non-negative number, got {raw!r}'
+        )
+    return value
+
+
+def validate_trace_dir(raw: str) -> str:
+    text = str(raw).strip()
+    if not text:
+        raise ValueError('trace_dir must not be empty')
+    return text
+
+
+def _absolute_proto(source: str, description_share: Path) -> str:
+    if source.startswith(('http://', 'https://')):
+        return source
+    proto_dir = description_share / 'resource' / 'protos'
+    candidate = proto_dir / Path(source).name
+    if not candidate.is_file():
+        raise FileNotFoundError(
+            f'EXTERNPROTO {source!r} was not found in {proto_dir}'
+        )
+    return str(candidate.resolve())
+
+
+def _materialize_world(
+    world_template: Path,
+    description_share: Path,
+    output_dir: Path,
+) -> Path:
+    if not world_template.is_file():
+        raise FileNotFoundError(f'world template does not exist: {world_template}')
+    text = world_template.read_text(encoding='utf-8')
+
+    def replace(match: re.Match[str]) -> str:
+        return match.group(1) + _absolute_proto(match.group(2), description_share) + match.group(3)
+
+    resolved = _EXTERNPROTO_RE.sub(replace, text)
+    for name in LOCAL_PROTOS:
+        expected = description_share / 'resource' / 'protos' / name
+        if str(expected.resolve()) not in resolved:
+            raise FileNotFoundError(
+                f'world template does not reference the installed {name}'
+            )
+    world_path = output_dir / world_template.name
+    world_path.write_text(resolved, encoding='utf-8')
+    return world_path
+
+
+def _mapping(value: Any, label: str) -> dict:
+    if not isinstance(value, dict):
+        raise ValueError(f'{label} must be a mapping')
+    return value
+
+
+def _materialize_setting(
+    base_setting: Path,
+    world_path: Path,
+    output_dir: Path,
+    moveit_cpp_path: Path | None = None,
+) -> Path:
+    if not base_setting.is_file():
+        raise FileNotFoundError(f'base settings file does not exist: {base_setting}')
+    data = _mapping(
+        yaml.safe_load(base_setting.read_text(encoding='utf-8')) or {},
+        'settings',
+    )
+    digital_twin = _mapping(data.get('digital_twin'), 'digital_twin')
+    webots = _mapping(digital_twin.get('webots'), 'digital_twin.webots')
+    objects = _mapping(webots.setdefault('objects', {}), 'digital_twin.webots.objects')
+    objects['enabled'] = False
+    objects['count'] = 0
+    webots['world'] = str(world_path.resolve())
+    # The interactive TCP gizmo must not exist in reference runs: it draws an
+    # overlay ball and can preempt the reference route.  cobot-setting.yaml
+    # only applies to `cobot run`, not to this launch.
+    gizmo = _mapping(
+        digital_twin.setdefault('tcp_gizmo', {}), 'digital_twin.tcp_gizmo',
+    )
+    gizmo['enabled'] = False
+
+    tool = _mapping(data.setdefault('tool', {}), 'tool')
+    tool['active'] = 'patron'
+
+    # The reference executor runs many rest-to-rest plans back to back.  The
+    # Webots position controller keeps a heavy tool oscillating at a few
+    # hundredths of rad/s for seconds after a stop, which trips the default
+    # 0.01 rad/s start-state guard and aborts otherwise valid plans.  Loosen
+    # the guard for these launches only; the runner still waits for the arm
+    # to settle before every plan and verifies the reached pose afterwards.
+    planning = _mapping(data.setdefault('planning', {}), 'planning')
+    planning['stopped_velocity_tolerance'] = 0.08
+
+    if moveit_cpp_path is not None:
+        controller = _mapping(data.setdefault('controller', {}), 'controller')
+        moveit = _mapping(controller.setdefault('moveit', {}), 'controller.moveit')
+        moveit['moveit_cpp'] = str(Path(moveit_cpp_path).resolve())
+
+    setting_path = output_dir / 'setting.yaml'
+    setting_path.write_text(
+        yaml.safe_dump(data, sort_keys=False, allow_unicode=True),
+        encoding='utf-8',
+    )
+    return setting_path
+
+
+def prepare_runtime_files(
+    base_setting: Path | str,
+    world_template: Path | str,
+    description_share: Path | str,
+    output_dir: Path | str,
+    moveit_cpp_path: Path | str | None = None,
+) -> tuple[Path, Path]:
+    """Write an isolated world/settings pair and return ``(world, setting)``.
+
+    Both local PROTO references are resolved against the installed
+    ``iiwa_description`` share.  External https PROTO references are preserved
+    verbatim.  The base settings keep their cameras, robot transform and
+    controller paths; only the world path and the legacy object spawner change.
+    """
+    base_setting = Path(base_setting)
+    world_template = Path(world_template)
+    description_share = Path(description_share)
+    output_dir = Path(output_dir)
+    output_dir.mkdir(parents=True, exist_ok=True)
+    world_path = _materialize_world(world_template, description_share, output_dir)
+    setting_path = _materialize_setting(
+        base_setting,
+        world_path,
+        output_dir,
+        None if moveit_cpp_path is None else Path(moveit_cpp_path),
+    )
+    return world_path, setting_path

src/iiwa_experement/iiwa_experement/scenario.py

@@ -0,0 +1,463 @@
+"""Strict loader for the deterministic grant experiment manifest.
+
+The manifest in ``config/scenes.yaml`` is the only source of object poses for
+the twenty experiments.  Everything in this module is pure Python so that the
+same validation runs in unit tests and at launch time.
+"""
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any, Mapping
+
+import yaml
+
+
+CAP_STATES = ('absent', 'free', 'installed')
+TOOL_NAMES = ('vacuum', 'magnetic_tool', 'sealant_tool', 'torque_tool')
+EXPERIMENT_IDS = tuple(range(1, 21))
+SEALANT_SIDES = ('side_1', 'side_2', 'side_3', 'side_4')
+SCREW_HOLES = ('hole_1', 'hole_2', 'hole_3', 'hole_4')
+
+# Bounds of lotok.obj after its local Y axis is mapped to Webots Z. They are
+# repeated here so validation does not need a Webots process or the mesh file.
+TRAY_HALF_LENGTH = 0.060
+TRAY_HALF_WIDTH = 0.035
+SCREW_RADIUS = 0.00275
+
+# Failure metadata is consumed by the reference executor. It never changes
+# the initial pose of an object. Screws are handed to the virtual operator:
+# there is deliberately no tightening or seating failure.
+_ALLOWED_FAILURES = {
+    'grab': {'grasp_failed'},
+    'verify': {'not_ok'},
+    'dispense': {'interrupted'},
+}
+
+
+class ScenarioError(ValueError):
+    """Raised when the manifest or a requested experiment is invalid."""
+
+
+@dataclass(frozen=True)
+class Pose:
+    x: float
+    y: float
+    z: float
+    yaw_deg: float
+
+    @property
+    def xy(self) -> tuple[float, float]:
+        return self.x, self.y
+
+
+@dataclass(frozen=True)
+class Tool:
+    name: str
+    pose: Pose
+
+
+@dataclass(frozen=True)
+class Screw:
+    name: str
+    pose: Pose
+
+
+@dataclass(frozen=True)
+class Failure:
+    skill: str
+    outcome: str
+    target: str | None = None
+    attempt: int | None = None
+    details: tuple[tuple[str, Any], ...] = ()
+
+    def as_dict(self) -> dict[str, Any]:
+        result: dict[str, Any] = {'skill': self.skill, 'outcome': self.outcome}
+        if self.target is not None:
+            result['target'] = self.target
+        if self.attempt is not None:
+            result['attempt'] = self.attempt
+        if self.details:
+            result['details'] = dict(self.details)
+        return result
+
+
+@dataclass(frozen=True)
+class ScenarioSpec:
+    experiment_id: int
+    fixture: Pose
+    body: Pose
+    rack: Pose
+    cap_state: str
+    cap_pose: Pose | None
+    tools: tuple[Tool, ...]
+    tray_pose: Pose | None
+    screws: tuple[Screw, ...]
+    failures: tuple[Failure, ...]
+    sealant_sides: tuple[str, ...] = ()
+    screw_holes: tuple[str, ...] = ()
+
+    @property
+    def tool_names(self) -> tuple[str, ...]:
+        return tuple(tool.name for tool in self.tools)
+
+    @property
+    def delivered_screw_count(self) -> int:
+        """Number of screws handed to the operator in this experiment."""
+        return len(self.screw_holes)
+
+
+def _mapping(value: Any, label: str) -> Mapping[str, Any]:
+    if not isinstance(value, Mapping):
+        raise ScenarioError(f'{label} must be a mapping')
+    return value
+
+
+def _number(value: Any, label: str) -> float:
+    if isinstance(value, bool) or not isinstance(value, (int, float)):
+        raise ScenarioError(f'{label} must be a finite number')
+    result = float(value)
+    if not math.isfinite(result):
+        raise ScenarioError(f'{label} must be a finite number')
+    return result
+
+
+def _pose(raw: Any, label: str) -> Pose:
+    data = _mapping(raw, label)
+    for key in ('x', 'y', 'z'):
+        if key not in data:
+            raise ScenarioError(f'{label}.{key} is required')
+    return Pose(
+        x=_number(data['x'], f'{label}.x'),
+        y=_number(data['y'], f'{label}.y'),
+        z=_number(data['z'], f'{label}.z'),
+        yaw_deg=_number(data.get('yaw_deg', 0.0), f'{label}.yaw_deg'),
+    )
+
+
+def _merge_pose(base: Pose, override: Any, label: str) -> Pose:
+    if override is None:
+        return base
+    data = _mapping(override, label)
+    merged = {
+        'x': base.x,
+        'y': base.y,
+        'z': base.z,
+        'yaw_deg': base.yaw_deg,
+    }
+    merged.update(data)
+    return _pose(merged, label)
+
+
+def _same_angle(first: float, second: float) -> bool:
+    delta = (first - second) % 360.0
+    return math.isclose(min(delta, 360.0 - delta), 0.0, abs_tol=1e-9)
+
+
+def _require_mapping(data: Mapping[str, Any], key: str) -> Mapping[str, Any]:
+    if key not in data:
+        raise ScenarioError(f"manifest is missing '{key}'")
+    return _mapping(data[key], key)
+
+
+def _experiment_table(data: Mapping[str, Any]) -> dict[int, Mapping[str, Any]]:
+    raw = _require_mapping(data, 'experiments')
+    parsed: dict[int, Mapping[str, Any]] = {}
+    for raw_id, entry in raw.items():
+        text = str(raw_id).strip()
+        if isinstance(raw_id, bool) or not text.isdigit():
+            raise ScenarioError(f'invalid experiment id {raw_id!r} in manifest')
+        experiment_id = int(text)
+        if experiment_id not in EXPERIMENT_IDS or text != str(experiment_id):
+            raise ScenarioError(f'experiment id {raw_id!r} is outside 1..20')
+        if experiment_id in parsed:
+            raise ScenarioError(f'duplicate experiment id {experiment_id} in manifest')
+        parsed[experiment_id] = _mapping(entry, f'experiments.{experiment_id}')
+    if set(parsed) != set(EXPERIMENT_IDS):
+        missing = sorted(set(EXPERIMENT_IDS) - set(parsed))
+        extra = sorted(set(parsed) - set(EXPERIMENT_IDS))
+        raise ScenarioError(
+            f'manifest must define experiments 1..20; missing={missing}, extra={extra}'
+        )
+    return parsed
+
+
+def _tool_poses(poses: Mapping[str, Any]) -> dict[str, Pose]:
+    raw = _mapping(poses.get('tools'), 'poses.tools')
+    missing = [name for name in TOOL_NAMES if name not in raw]
+    extra = sorted(set(raw) - set(TOOL_NAMES))
+    if missing or extra:
+        raise ScenarioError(
+            f'poses.tools must define {list(TOOL_NAMES)}; missing={missing}, extra={extra}'
+        )
+    return {name: _pose(raw[name], f'poses.tools.{name}') for name in TOOL_NAMES}
+
+
+def _tools(raw: Any, poses: Mapping[str, Pose]) -> tuple[Tool, ...]:
+    if not isinstance(raw, list) or not raw:
+        raise ScenarioError('experiment tools must be a nonempty list')
+    tools: list[Tool] = []
+    seen: set[str] = set()
+    for index, name in enumerate(raw):
+        if not isinstance(name, str):
+            raise ScenarioError(f'tools[{index}] must be a tool name')
+        if name not in TOOL_NAMES:
+            raise ScenarioError(
+                f'unknown tool {name!r}; expected one of {list(TOOL_NAMES)}'
+            )
+        if name in seen:
+            raise ScenarioError(f'duplicate tool {name!r}')
+        seen.add(name)
+        tools.append(Tool(name, poses[name]))
+    return tuple(tools)
+
+
+def _rotate_offset(dx: float, dy: float, yaw_deg: float) -> tuple[float, float]:
+    angle = math.radians(yaw_deg)
+    cosine, sine = math.cos(angle), math.sin(angle)
+    return dx * cosine - dy * sine, dx * sine + dy * cosine
+
+
+def _screws(
+    raw_entry: Mapping[str, Any],
+    raw_manifest: Mapping[str, Any],
+    experiment_id: int,
+    tray_pose: Pose | None,
+) -> tuple[Screw, ...]:
+    explicit = raw_entry.get('screws')
+    if tray_pose is None:
+        if explicit is not None:
+            raise ScenarioError(
+                f'experiment {experiment_id} has screws but no tray'
+            )
+        return ()
+
+    if explicit is None:
+        screw_data = _mapping(raw_manifest.get('screws'), 'screws')
+        layout = screw_data.get('layout')
+        if not isinstance(layout, list):
+            raise ScenarioError('screws.layout must be a list')
+        z = _number(screw_data.get('z'), 'screws.z')
+    else:
+        if not isinstance(explicit, list):
+            raise ScenarioError(f'experiments.{experiment_id}.screws must be a list')
+        layout = explicit
+        screw_data = _mapping(raw_manifest.get('screws'), 'screws')
+        z = _number(screw_data.get('z'), 'screws.z')
+
+    if len(layout) != 10:
+        raise ScenarioError(
+            f'experiment {experiment_id} has {len(layout)} screws; '
+            'a tray scenario must contain exactly 10 screws'
+        )
+
+    result: list[Screw] = []
+    names: set[str] = set()
+    for index, raw_screw in enumerate(layout):
+        label = f'experiments.{experiment_id}.screws[{index}]'
+        data = _mapping(raw_screw, label)
+        name = data.get('name')
+        if not isinstance(name, str) or not name:
+            raise ScenarioError(f'{label}.name must be a nonempty string')
+        if name in names:
+            raise ScenarioError(f'duplicate screw name {name!r}')
+        names.add(name)
+        dx = _number(data.get('dx'), f'{label}.dx')
+        dy = _number(data.get('dy'), f'{label}.dy')
+        if abs(dx) > TRAY_HALF_LENGTH - SCREW_RADIUS or abs(dy) > TRAY_HALF_WIDTH - SCREW_RADIUS:
+            raise ScenarioError(
+                f'screw {name!r} at offset ({dx}, {dy}) is outside the tray'
+            )
+        world_dx, world_dy = _rotate_offset(dx, dy, tray_pose.yaw_deg)
+        result.append(Screw(
+            name=name,
+            pose=Pose(tray_pose.x + world_dx, tray_pose.y + world_dy, z, tray_pose.yaw_deg),
+        ))
+    return tuple(result)
+
+
+def _named_list(
+    raw: Any,
+    label: str,
+    allowed: tuple[str, ...],
+    experiment_id: int,
+) -> tuple[str, ...]:
+    if raw is None:
+        return ()
+    if not isinstance(raw, list):
+        raise ScenarioError(f'{label} must be a list')
+    result: list[str] = []
+    for index, name in enumerate(raw):
+        if not isinstance(name, str) or name not in allowed:
+            raise ScenarioError(
+                f'{label}[{index}] must be one of {list(allowed)}'
+            )
+        if name in result:
+            raise ScenarioError(f'{label} contains duplicate {name!r}')
+        result.append(name)
+    return tuple(result)
+
+
+def _failures(raw: Any, experiment_id: int) -> tuple[Failure, ...]:
+    if raw is None:
+        return ()
+    if not isinstance(raw, list):
+        raise ScenarioError(f'experiments.{experiment_id}.failures must be a list')
+    result: list[Failure] = []
+    for index, entry in enumerate(raw):
+        label = f'experiments.{experiment_id}.failures[{index}]'
+        data = _mapping(entry, label)
+        skill = data.get('skill')
+        outcome = data.get('outcome')
+        if skill not in _ALLOWED_FAILURES:
+            raise ScenarioError(f'{label}.skill must be one of {sorted(_ALLOWED_FAILURES)}')
+        if outcome not in _ALLOWED_FAILURES[skill]:
+            raise ScenarioError(
+                f'{label}.outcome {outcome!r} is not valid for skill {skill!r}'
+            )
+        target = data.get('target')
+        if target is not None and not isinstance(target, str):
+            raise ScenarioError(f'{label}.target must be a string')
+        attempt = data.get('attempt')
+        if attempt is not None:
+            if isinstance(attempt, bool) or not isinstance(attempt, int) or attempt < 1:
+                raise ScenarioError(f'{label}.attempt must be a positive integer')
+        details_raw = data.get('details', {})
+        details = _mapping(details_raw, f'{label}.details')
+        result.append(Failure(
+            skill=skill,
+            outcome=outcome,
+            target=target,
+            attempt=attempt,
+            details=tuple(sorted(details.items())),
+        ))
+    return tuple(result)
+
+
+def load_scenario(experiment_id: int, manifest_path: Path | str) -> ScenarioSpec:
+    """Load and fully validate one experiment from the YAML manifest."""
+    if isinstance(experiment_id, bool) or not isinstance(experiment_id, int):
+        raise ScenarioError(f'experiment id must be an integer in 1..20, got {experiment_id!r}')
+    if experiment_id not in EXPERIMENT_IDS:
+        raise ScenarioError(f'experiment id {experiment_id} is outside 1..20')
+
+    path = Path(manifest_path)
+    if not path.is_file():
+        raise FileNotFoundError(f'scenario manifest does not exist: {path}')
+    try:
+        raw = yaml.safe_load(path.read_text(encoding='utf-8'))
+    except yaml.YAMLError as error:
+        raise ScenarioError(f'invalid YAML in {path}: {error}') from error
+    data = _mapping(raw, 'manifest')
+    if data.get('schema_version') != 1:
+        raise ScenarioError('manifest schema_version must be 1')
+
+    experiments = _experiment_table(data)
+    entry = experiments[experiment_id]
+    poses = _require_mapping(data, 'poses')
+    fixture = _merge_pose(
+        _pose(poses.get('fixture'), 'poses.fixture'),
+        entry.get('fixture'),
+        f'experiments.{experiment_id}.fixture',
+    )
+    body = _merge_pose(
+        _pose(poses.get('body'), 'poses.body'),
+        entry.get('body'),
+        f'experiments.{experiment_id}.body',
+    )
+    if not (
+        math.isclose(fixture.x, body.x, abs_tol=1e-9)
+        and math.isclose(fixture.y, body.y, abs_tol=1e-9)
+        and _same_angle(fixture.yaw_deg, body.yaw_deg)
+    ):
+        raise ScenarioError(
+            f'experiment {experiment_id}: fixture and body must share X, Y and yaw'
+        )
+    rack = _pose(poses.get('rack'), 'poses.rack')
+    tool_poses = _tool_poses(poses)
+
+    cap_state = entry.get('cap', 'free')
+    if cap_state not in CAP_STATES:
+        raise ScenarioError(
+            f'experiment {experiment_id}: cap must be one of {list(CAP_STATES)}'
+        )
+    if cap_state == 'absent':
+        if entry.get('cap_pose') is not None:
+            raise ScenarioError(f'experiment {experiment_id}: an absent cap cannot have a pose')
+        cap_pose = None
+    elif cap_state == 'free':
+        cap_pose = _merge_pose(
+            _pose(poses.get('cap_free'), 'poses.cap_free'),
+            entry.get('cap_pose'),
+            f'experiments.{experiment_id}.cap_pose',
+        )
+    else:
+        cap_pose = _merge_pose(
+            _pose(poses.get('cap_installed'), 'poses.cap_installed'),
+            entry.get('cap_pose'),
+            f'experiments.{experiment_id}.cap_pose',
+        )
+        if not (
+            math.isclose(cap_pose.x, body.x, abs_tol=1e-9)
+            and math.isclose(cap_pose.y, body.y, abs_tol=1e-9)
+            and _same_angle(cap_pose.yaw_deg, body.yaw_deg)
+        ):
+            raise ScenarioError(
+                f'experiment {experiment_id}: an installed cap must share the body pose'
+            )
+
+    tray_enabled = entry.get('tray', False)
+    if not isinstance(tray_enabled, bool):
+        raise ScenarioError(f'experiments.{experiment_id}.tray must be true or false')
+    if tray_enabled:
+        tray_pose = _merge_pose(
+            _pose(poses.get('tray'), 'poses.tray'),
+            entry.get('tray_pose'),
+            f'experiments.{experiment_id}.tray_pose',
+        )
+    else:
+        if entry.get('tray_pose') is not None:
+            raise ScenarioError(f'experiment {experiment_id}: tray_pose requires tray: true')
+        tray_pose = None
+
+    sealant_sides = _named_list(
+        entry.get('sealant'), f'experiments.{experiment_id}.sealant',
+        SEALANT_SIDES, experiment_id,
+    )
+    if sealant_sides and 'sealant_tool' not in entry.get('tools', []):
+        raise ScenarioError(
+            f'experiment {experiment_id}: sealant sides require the sealant_tool'
+        )
+
+    screw_holes = _named_list(
+        entry.get('screw_holes'), f'experiments.{experiment_id}.screw_holes',
+        SCREW_HOLES, experiment_id,
+    )
+    if tray_pose is None and screw_holes:
+        raise ScenarioError(
+            f'experiment {experiment_id}: screw holes require a screw tray'
+        )
+    if tray_pose is not None and not screw_holes:
+        raise ScenarioError(
+            f'experiment {experiment_id}: a screw tray requires screw_holes'
+        )
+    if screw_holes and 'magnetic_tool' not in entry.get('tools', []):
+        raise ScenarioError(
+            f'experiment {experiment_id}: screw holes require the magnetic_tool'
+        )
+
+    return ScenarioSpec(
+        experiment_id=experiment_id,
+        fixture=fixture,
+        body=body,
+        rack=rack,
+        cap_state=cap_state,
+        cap_pose=cap_pose,
+        tools=_tools(entry.get('tools'), tool_poses),
+        tray_pose=tray_pose,
+        screws=_screws(entry, data, experiment_id, tray_pose),
+        failures=_failures(entry.get('failures'), experiment_id),
+        sealant_sides=sealant_sides,
+        screw_holes=screw_holes,
+    )

src/iiwa_experement/iiwa_experement/scene_spawner.py

@@ -0,0 +1,243 @@
+"""Deterministic scene spawner and readiness service."""
+from __future__ import annotations
+
+import re
+import time
+from dataclasses import dataclass, field
+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 sensor_msgs.msg import JointState
+from std_srvs.srv import Trigger
+from webots_ros2_msgs.srv import SpawnNodeFromString
+
+from .scenario import ScenarioSpec, load_scenario
+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 = 60.0
+
+_NAME_RE = re.compile(r'name "([^"]+)"')
+
+
+class SpawnError(RuntimeError):
+    """A spawn request failed or its outcome is unknown."""
+
+
+@dataclass
+class Readiness:
+    ready: bool = False
+    message: str = 'scene not ready'
+
+
+@dataclass
+class ScenePlan:
+    experiment_id: int
+    spec: ScenarioSpec
+    nodes: list[str]
+    readiness: Readiness = field(default_factory=Readiness)
+
+
+def node_name(data: str) -> str:
+    match = _NAME_RE.search(data)
+    return match.group(1) if match else 'unnamed object'
+
+
+def build_plan(
+    experiment_id: int,
+    scene_config: Path | str,
+    asset_root: Path | str,
+) -> ScenePlan:
+    """Validate the experiment and prepare every node before spawning."""
+    spec = load_scenario(experiment_id, scene_config)
+    return ScenePlan(experiment_id, spec, build_nodes(spec, asset_root))
+
+
+def spawn_nodes(
+    node,
+    client,
+    nodes: list[str],
+    *,
+    service_timeout_sec: float = DEFAULT_SERVICE_TIMEOUT,
+    response_timeout_sec: float = DEFAULT_RESPONSE_TIMEOUT,
+    spin_until_future_complete=None,
+) -> None:
+    """Spawn nodes sequentially through the Webots supervisor.
+
+    A timeout on the response is ambiguous: the first request may still have
+    been executed. It is reported as an error and is never retried, because a
+    retry could create a duplicate object.
+    """
+    spin = spin_until_future_complete or rclpy.spin_until_future_complete
+    try:
+        available = client.wait_for_service(timeout_sec=service_timeout_sec)
+    except Exception as error:  # pragma: no cover - depends on rclpy internals
+        raise SpawnError(f'Webots supervisor service failed: {error}') from error
+    if not available:
+        raise SpawnError(
+            f'Webots supervisor service {SPAWN_SERVICE} unavailable after '
+            f'{service_timeout_sec:g} s; no objects spawned'
+        )
+
+    logger = node.get_logger()
+    total = len(nodes)
+    for index, data in enumerate(nodes, start=1):
+        name = node_name(data)
+        future = client.call_async(SpawnNodeFromString.Request(data=data))
+        spin(node, future, timeout_sec=response_timeout_sec)
+        if not future.done():
+            raise SpawnError(
+                f'{name}: spawn request timed out after {response_timeout_sec:g} s; '
+                'result unknown, no retry'
+            )
+        try:
+            response = future.result()
+        except Exception as error:
+            raise SpawnError(f'{name}: spawn call failed: {error}') from error
+        if response is None:
+            raise SpawnError(f'{name}: spawn returned no response')
+        if not response.success:
+            raise SpawnError(
+                f'{name}: Webots rejected the object ({index - 1}/{total} accepted)'
+            )
+        logger.info(f'Spawned {index}/{total}: {name}')
+
+
+def execute_plan(
+    plan: ScenePlan,
+    client,
+    node,
+    *,
+    service_timeout_sec: float = DEFAULT_SERVICE_TIMEOUT,
+    response_timeout_sec: float = DEFAULT_RESPONSE_TIMEOUT,
+    spin_until_future_complete=None,
+) -> None:
+    """Run a plan and publish readiness only after the final success."""
+    try:
+        spawn_nodes(
+            node,
+            client,
+            plan.nodes,
+            service_timeout_sec=service_timeout_sec,
+            response_timeout_sec=response_timeout_sec,
+            spin_until_future_complete=spin_until_future_complete,
+        )
+    except Exception as error:
+        plan.readiness.ready = False
+        plan.readiness.message = (
+            f'experiment {plan.experiment_id}: {error}'
+        )
+        raise
+    plan.readiness.ready = True
+    plan.readiness.message = (
+        f'experiment {plan.experiment_id}: {len(plan.nodes)} objects spawned'
+    )
+
+
+class SceneSpawnerNode(Node):
+    def __init__(self) -> None:
+        super().__init__('scene_spawner')
+        default_config = str(
+            Path(get_package_share_directory('iiwa_experement'))
+            / 'config'
+            / 'scenes.yaml'
+        )
+        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)
+
+    def _handle_ready(self, request, response):
+        response.success = self._readiness.ready
+        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)
+        asset_root = (
+            Path(get_package_share_directory('iiwa_description'))
+            / 'objects'
+            / 'grant'
+        )
+
+        self.get_logger().info(
+            f'Preparing experiment {experiment_id} from {scene_config}'
+        )
+        plan = build_plan(experiment_id, scene_config, asset_root)
+        self._readiness = plan.readiness
+        client = self.create_client(SpawnNodeFromString, SPAWN_SERVICE)
+        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)
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = SceneSpawnerNode()
+    exit_code = 0
+    try:
+        node.run()
+        rclpy.spin(node)
+    except (KeyboardInterrupt, ExternalShutdownException):
+        pass
+    except Exception as error:
+        node.get_logger().error(f'Scene spawn failed: {error}')
+        exit_code = 1
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+    if exit_code:
+        raise SystemExit(exit_code)
+
+
+if __name__ == '__main__':
+    main()

src/iiwa_experement/iiwa_experement/trace.py

@@ -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()

src/iiwa_experement/iiwa_experement/webots_nodes.py

@@ -0,0 +1,353 @@
+"""Turn a validated ScenarioSpec into deterministic Webots ``Solid`` nodes.
+
+The generated nodes are sent one at a time to
+``/Ros2Supervisor/spawn_node_from_string``.  Their order is:
+
+    fixture -> body -> rack -> working tools -> tray -> screws -> cap
+
+All positions are the initial scene only.  The fixture, body, rack and tray are
+fixed.  A loose hatch has physics; an installed hatch is locked.  Screws and
+tools are static in their tray/socket: this package does not implement grasp,
+tool docking or dynamic hatch fixation, so a later skill executor must release
+them before a robot can pick them up.
+"""
+from __future__ import annotations
+
+import json
+import math
+import re
+from pathlib import Path
+
+from .scenario import ScenarioSpec
+
+
+ASSETS = {
+    'fixture': 'stapel.obj',
+    'body': 'base-detail.obj',
+    'rack': 'tool-magizine.obj',
+    'tray': 'lotok.obj',
+    'screw': 'vint.obj',
+    'cap': 'cap.obj',
+    'vacuum': 'vacum-tool.obj',
+    'magnetic_tool': 'magnit-tool.obj',
+    'sealant_tool': 'germetic-tool.obj',
+    'torque_tool': 'wranch-tool.obj',
+}
+
+TOOL_NODE_NAMES = {
+    'vacuum': 'grant_vacuum',
+    'magnetic_tool': 'grant_magnetic_tool',
+    'sealant_tool': 'grant_sealant_tool',
+    'torque_tool': 'grant_torque_tool',
+}
+
+TOOL_SIZE = {
+    'vacuum': (0.0821, 0.14, 0.081),
+    'magnetic_tool': (0.036, 0.14, 0.036),
+    'sealant_tool': (0.05608, 0.14, 0.05608),
+    'torque_tool': (0.049, 0.14, 0.049),
+}
+
+TOOL_APPEARANCE = {
+    'vacuum': '0.16 0.47 0.72',
+    'magnetic_tool': '0.43 0.30 0.65',
+    'sealant_tool': '0.23 0.57 0.31',
+    'torque_tool': '0.80 0.32 0.18',
+}
+
+
+def rotation_matrix(yaw_deg: float) -> tuple[tuple[float, float, float], ...]:
+    """Rz(yaw) @ Rx(90): the OBJ Y axis becomes Webots Z."""
+    angle = math.radians(yaw_deg)
+    cosine, sine = math.cos(angle), math.sin(angle)
+    return (
+        (cosine, 0.0, sine),
+        (sine, 0.0, -cosine),
+        (0.0, 1.0, 0.0),
+    )
+
+
+def rotation_axis_angle(yaw_deg: float) -> tuple[float, float, float, float]:
+    """Return the Webots axis-angle equivalent of :func:`rotation_matrix`."""
+    matrix = rotation_matrix(yaw_deg)
+    trace = matrix[0][0] + matrix[1][1] + matrix[2][2]
+    cosine = max(-1.0, min(1.0, (trace - 1.0) / 2.0))
+    angle = math.acos(cosine)
+    if angle < 1e-12:
+        return 0.0, 0.0, 1.0, 0.0
+    if math.pi - angle < 1e-9:
+        # 180 degrees: recover the axis from the symmetric part.
+        diagonal = [matrix[i][i] for i in range(3)]
+        axis = [math.sqrt(max(0.0, (diagonal[i] + 1.0) / 2.0)) for i in range(3)]
+        return tuple(axis) + (angle,)
+    scale = 2.0 * math.sin(angle)
+    axis = (
+        (matrix[2][1] - matrix[1][2]) / scale,
+        (matrix[0][2] - matrix[2][0]) / scale,
+        (matrix[1][0] - matrix[0][1]) / scale,
+    )
+    return tuple(axis) + (angle,)
+
+
+def _vector(values) -> str:
+    return ' '.join(format(value, '.12g') for value in values)
+
+
+def _asset_path(asset_root: Path, name: str) -> Path:
+    path = asset_root / ASSETS[name]
+    if not path.is_file():
+        raise FileNotFoundError(f'Webots asset does not exist: {path}')
+    source = path.resolve()
+    for line in source.read_text(encoding='utf-8', errors='replace').splitlines():
+        parts = line.split()
+        if parts and parts[0].lower() == 'mtllib':
+            for material in parts[1:]:
+                material_path = source.parent / material
+                if not material_path.is_file():
+                    raise FileNotFoundError(
+                        f'Webots material does not exist: {material_path} '
+                        f'(referenced by {source})'
+                    )
+    return path
+
+
+def _mesh_shape(asset: Path, color: str, scale: float | None = None) -> str:
+    shape = f'''Shape {{
+  appearance PBRAppearance {{
+    baseColor {color}
+    roughness 0.5
+    metalness 0.3
+  }}
+  geometry Mesh {{
+    url [ {json.dumps(str(asset))} ]
+  }}
+}}'''
+    if scale is None:
+        return shape
+    return f'''Transform {{
+  scale {_vector((scale, scale, scale))}
+  children [
+{_indent(shape, 4)}
+  ]
+}}'''
+
+
+def _indent(text: str, spaces: int) -> str:
+    prefix = ' ' * spaces
+    return '\n'.join(prefix + line for line in text.splitlines())
+
+
+def _solid(
+    *,
+    name: str,
+    translation,
+    rotation,
+    children: str,
+    bounding: str,
+    locked: bool,
+    physics_mass: float | None = None,
+) -> str:
+    lines = [
+        f'DEF {name} Solid {{',
+        f'  translation {_vector(translation)}',
+        f'  rotation {_vector(rotation)}',
+        f'  name "{name}"',
+    ]
+    if locked:
+        lines.append('  locked TRUE')
+    lines.extend([
+        '  children [',
+        _indent(children, 4),
+        '  ]',
+        '  boundingObject ' + bounding,
+    ])
+    if physics_mass is not None:
+        lines.append(
+            f'  physics Physics {{ density -1 mass {format(physics_mass, ".12g")} }}'
+        )
+    lines.append('}')
+    return '\n'.join(lines)
+
+
+def _fixture_node(spec: ScenarioSpec, asset: Path) -> str:
+    pose = spec.fixture
+    bounding = '''Group {
+  children [
+    Pose {
+      translation 0 -0.005 0
+      children [ Box { size 0.35 0.01 0.22 } ]
+    }
+    Pose {
+      translation -0.104 0.008 0
+      children [ Box { size 0.014 0.016 0.156 } ]
+    }
+    Pose {
+      translation 0.104 0.008 0
+      children [ Box { size 0.014 0.016 0.156 } ]
+    }
+  ]
+}'''
+    return _solid(
+        name='grant_fixture',
+        translation=(pose.x, pose.y, pose.z),
+        rotation=rotation_axis_angle(pose.yaw_deg),
+        children=_mesh_shape(asset, '0.30 0.34 0.39'),
+        bounding=bounding,
+        locked=True,
+    )
+
+
+def _body_node(spec: ScenarioSpec, asset: Path) -> str:
+    pose = spec.body
+    bounding = '''Pose {
+  translation 0 0.000695 0
+  children [ Box { size 0.30 0.00915 0.17 } ]
+}'''
+    return _solid(
+        name='grant_body',
+        translation=(pose.x, pose.y, pose.z),
+        rotation=rotation_axis_angle(pose.yaw_deg),
+        children=_mesh_shape(asset, '0.64 0.68 0.72'),
+        bounding=bounding,
+        locked=True,
+    )
+
+
+def _rack_node(spec: ScenarioSpec, asset: Path) -> str:
+    pose = spec.rack
+    # The magazine model was validated with its local X turned around, so the
+    # tool sockets face the robot. It still maps the OBJ Y axis to Webots Z.
+    rotation = rotation_axis_angle(pose.yaw_deg + 180.0)
+    bounding = '''Pose {
+  translation 0 0.005 0
+  children [ Box { size 0.42 0.01 0.14 } ]
+}'''
+    return _solid(
+        name='grant_rack',
+        translation=(pose.x, pose.y, pose.z),
+        rotation=rotation,
+        children=_mesh_shape(asset, '0.72 0.47 0.14'),
+        bounding=bounding,
+        locked=True,
+    )
+
+
+def _tool_node(tool, asset: Path) -> str:
+    scale = 10.0 if tool.name == 'sealant_tool' else None
+    bounding = f'''Pose {{
+  translation 0 0.07 0
+  children [ Box {{ size {_vector(TOOL_SIZE[tool.name])} }} ]
+}}'''
+    return _solid(
+        name=TOOL_NODE_NAMES[tool.name],
+        translation=(tool.pose.x, tool.pose.y, tool.pose.z),
+        rotation=rotation_axis_angle(tool.pose.yaw_deg + 180.0),
+        children=_mesh_shape(asset, TOOL_APPEARANCE[tool.name], scale=scale),
+        bounding=bounding,
+        locked=True,
+    )
+
+
+def _tray_node(spec: ScenarioSpec, asset: Path) -> str:
+    pose = spec.tray_pose
+    assert pose is not None
+    bounding = '''Group {
+  children [
+    Pose {
+      translation 0 0.0012 0
+      children [ Box { size 0.12 0.0024 0.07 } ]
+    }
+    Pose {
+      translation -0.059 0.005 0
+      children [ Box { size 0.002 0.01 0.07 } ]
+    }
+    Pose {
+      translation 0.059 0.005 0
+      children [ Box { size 0.002 0.01 0.07 } ]
+    }
+    Pose {
+      translation 0 0.005 -0.034
+      children [ Box { size 0.12 0.01 0.002 } ]
+    }
+    Pose {
+      translation 0 0.005 0.034
+      children [ Box { size 0.12 0.01 0.002 } ]
+    }
+  ]
+}'''
+    return _solid(
+        name='grant_tray',
+        translation=(pose.x, pose.y, pose.z),
+        rotation=rotation_axis_angle(pose.yaw_deg),
+        children=_mesh_shape(asset, '0.25 0.25 0.28'),
+        bounding=bounding,
+        locked=True,
+    )
+
+
+def _screw_node(index: int, screw, asset: Path) -> str:
+    match = re.fullmatch(r'screw_(\d+)', screw.name)
+    number = int(match.group(1)) if match else index
+    bounding = '''Pose {
+  translation 0 0.0045 0
+  children [ Box { size 0.0055 0.009 0.0055 } ]
+}'''
+    return _solid(
+        name=f'grant_screw_{number:02d}',
+        translation=(screw.pose.x, screw.pose.y, screw.pose.z),
+        rotation=rotation_axis_angle(screw.pose.yaw_deg),
+        children=_mesh_shape(asset, '0.62 0.64 0.67'),
+        bounding=bounding,
+        locked=True,
+    )
+
+
+def _cap_node(spec: ScenarioSpec, asset: Path) -> str:
+    pose = spec.cap_pose
+    assert pose is not None
+    bounding = '''Pose {
+  translation 0 0.004165 0
+  children [ Box { size 0.1384 0.00367 0.0984 } ]
+}'''
+    # All managed objects are positioned by the attachment supervisor.
+    # A free cap with physics can be launched by the supervisor's per-step
+    # teleport, so it is locked just like an installed cap.
+    return _solid(
+        name='grant_cap',
+        translation=(pose.x, pose.y, pose.z),
+        rotation=rotation_axis_angle(pose.yaw_deg),
+        children=_mesh_shape(asset, '0.78 0.82 0.86'),
+        bounding=bounding,
+        locked=True,
+    )
+
+
+def build_nodes(spec: ScenarioSpec, asset_root: Path | str) -> list[str]:
+    """Return the deterministic Webots node strings for one experiment."""
+    root = Path(asset_root)
+    if not root.is_dir():
+        raise FileNotFoundError(f'Webots asset directory does not exist: {root}')
+
+    required = ['fixture', 'body', 'rack']
+    required.extend(tool.name for tool in spec.tools)
+    if spec.tray_pose is not None:
+        required.extend(['tray', 'screw'])
+    if spec.cap_state != 'absent':
+        required.append('cap')
+    assets = {name: _asset_path(root, name) for name in required}
+
+    nodes = [
+        _fixture_node(spec, assets['fixture']),
+        _body_node(spec, assets['body']),
+        _rack_node(spec, assets['rack']),
+    ]
+    nodes.extend(_tool_node(tool, assets[tool.name]) for tool in spec.tools)
+    if spec.tray_pose is not None:
+        nodes.append(_tray_node(spec, assets['tray']))
+        nodes.extend(
+            _screw_node(index, screw, assets['screw'])
+            for index, screw in enumerate(spec.screws, start=1)
+        )
+    if spec.cap_state != 'absent':
+        nodes.append(_cap_node(spec, assets['cap']))
+    return nodes

src/iiwa_experement/launch/experiment.launch.py

@@ -0,0 +1,310 @@
+"""Launch one deterministic grant experiment in the Webots simulator."""
+from __future__ import annotations
+
+import shutil
+import sys
+import tempfile
+from pathlib import Path
+
+from ament_index_python.packages import get_package_share_directory, get_package_prefix
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+    RegisterEventHandler,
+)
+from launch.event_handlers import OnShutdown
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from webots_ros2_driver.utils import controller_url_prefix
+
+from iiwa_experement.runtime_config import (
+    DEFAULT_START_DELAY_SEC,
+    DEFAULT_TRACE_DIR,
+    prepare_runtime_files,
+    validate_experiment_id,
+    validate_mode,
+    validate_start_delay,
+    validate_trace_dir,
+)
+
+
+BRINGUP_LAUNCH = ('launch', 'iiwa.launch.py')
+SCENE_SPAWNER = 'scene_spawner'
+SUPERVISOR = 'grant_supervisor'
+SUPERVISOR_ROBOT = 'GrantAttachmentSupervisor'
+
+
+def spawner_parameters(experiment_id: int, scene_config: str) -> dict:
+    """Parameters passed unchanged to the local scene_spawner node."""
+    return {
+        'experiment_id': int(experiment_id),
+        'scene_config': str(scene_config),
+    }
+
+
+def supervisor_parameters(
+    experiment_id: int,
+    scene_config: str,
+    geometry_path: str,
+    read_only: bool,
+) -> dict:
+    return {
+        'experiment_id': int(experiment_id),
+        'scene_config': str(scene_config),
+        'geometry_path': str(geometry_path),
+        'read_only': bool(read_only),
+    }
+
+
+def runner_parameters(
+    experiment_id: int,
+    scene_config: str,
+    geometry_path: str,
+    start_delay_sec: float,
+    trace_dir: str,
+) -> dict:
+    return {
+        'experiment_id': int(experiment_id),
+        'scene_config': str(scene_config),
+        'geometry_path': str(geometry_path),
+        'start_delay_sec': float(start_delay_sec),
+        'trace_dir': str(trace_dir),
+    }
+
+
+def supervisor_process(
+    experiment_id: int,
+    scene_config: str,
+    geometry_path: str,
+    *,
+    read_only: bool,
+) -> ExecuteProcess:
+    """Start the Webots supervisor beside Webots.
+
+    The process must exist before the scene is spawned: Webots waits for all
+    ``<extern>`` controllers, so a later start would deadlock the simulation.
+    """
+    parameters = supervisor_parameters(
+        experiment_id, scene_config, geometry_path, read_only,
+    )
+    arguments = []
+    for key, value in parameters.items():
+        text = str(value).lower() if isinstance(value, bool) else str(value)
+        arguments.extend(['-p', f'{key}:={text}'])
+    return ExecuteProcess(
+        cmd=[
+            sys.executable, '-m', 'iiwa_experement.grant_supervisor',
+            '--ros-args', *arguments,
+        ],
+        name=SUPERVISOR,
+        output='screen',
+        additional_env={
+            'WEBOTS_CONTROLLER_URL': controller_url_prefix('1234') + SUPERVISOR_ROBOT,
+            'WEBOTS_HOME': get_package_prefix('webots_ros2_driver'),
+        },
+    )
+
+
+def mode_read_only(mode: str) -> bool:
+    """Only ``view`` is passive; execute and planner accept grab commands."""
+    return mode == 'view'
+
+
+def runtime_actions(
+    experiment_id: int,
+    scene_config: str,
+    setting_path: str,
+    rviz: str = 'false',
+    cleanup_dir: str | None = None,
+    *,
+    mode: str = 'view',
+    start_delay_sec: float = DEFAULT_START_DELAY_SEC,
+    trace_dir: str = DEFAULT_TRACE_DIR,
+    geometry_path: str | None = None,
+) -> list:
+    """Build the actions after validation has already succeeded."""
+    package_share = Path(get_package_share_directory('iiwa_experement'))
+    if geometry_path is None:
+        geometry_path = str(package_share / 'config' / 'geometry.yaml')
+    # Only the passive view mode locks the supervisor.  Both the reference
+    # executor and the external-graph planner need to accept grab/dock
+    # commands; the planner mode simply never starts the reference runner.
+    read_only = mode_read_only(mode)
+    actions = [
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution([
+                    FindPackageShare('iiwa_bringup'), *BRINGUP_LAUNCH,
+                ])
+            ),
+            launch_arguments={
+                'simulate': 'true',
+                'rviz': str(rviz),
+                'setting': str(setting_path),
+            }.items(),
+        ),
+        Node(
+            package='iiwa_experement',
+            executable=SCENE_SPAWNER,
+            name=SCENE_SPAWNER,
+            output='screen',
+            parameters=[spawner_parameters(experiment_id, scene_config)],
+        ),
+        supervisor_process(
+            experiment_id,
+            scene_config,
+            str(geometry_path),
+            read_only=read_only,
+        ),
+    ]
+    if mode == 'execute':
+        actions.append(Node(
+            package='iiwa_experement',
+            executable='reference_runner',
+            name='reference_runner',
+            output='screen',
+            parameters=[
+                runner_parameters(
+                    experiment_id, scene_config, geometry_path,
+                    start_delay_sec, trace_dir,
+                ),
+                {'use_sim_time': True},
+            ],
+        ))
+    if cleanup_dir is not None:
+        actions.append(RegisterEventHandler(
+            OnShutdown(on_shutdown=[OpaqueFunction(
+                function=_remove_directory,
+                kwargs={'directory': cleanup_dir},
+            )])
+        ))
+    return actions
+
+
+def _remove_directory(context, *args, directory: str, **kwargs):
+    del context, args, kwargs
+    shutil.rmtree(directory, ignore_errors=True)
+    return []
+
+
+def _setup(context, *args, **kwargs):
+    del args, kwargs
+    raw_id = LaunchConfiguration('experiment_id').perform(context)
+    try:
+        experiment_id = validate_experiment_id(raw_id)
+    except ValueError as error:
+        raise RuntimeError(f'Invalid experiment_id: {error}') from error
+
+    try:
+        mode = validate_mode(LaunchConfiguration('mode').perform(context))
+    except ValueError as error:
+        raise RuntimeError(f'Invalid mode: {error}') from error
+    try:
+        start_delay_sec = validate_start_delay(
+            LaunchConfiguration('start_delay_sec').perform(context)
+        )
+    except ValueError as error:
+        raise RuntimeError(f'Invalid start_delay_sec: {error}') from error
+    try:
+        trace_dir = validate_trace_dir(
+            LaunchConfiguration('trace_dir').perform(context)
+        )
+    except ValueError as error:
+        raise RuntimeError(f'Invalid trace_dir: {error}') from error
+
+    scene_config = str(LaunchConfiguration('scene_config').perform(context))
+    base_setting = str(LaunchConfiguration('base_setting').perform(context))
+    rviz = str(LaunchConfiguration('rviz').perform(context)).lower()
+    geometry_path = str(
+        Path(get_package_share_directory('iiwa_experement'))
+        / 'config'
+        / 'geometry.yaml'
+    )
+
+    package_share = Path(get_package_share_directory('iiwa_experement'))
+    description_share = Path(get_package_share_directory('iiwa_description'))
+    world_template = package_share / 'worlds' / 'grant' / 'iiwa-grant.wbt'
+
+    output_dir = Path(tempfile.mkdtemp(prefix='iiwa_experement_'))
+    try:
+        world_path, setting_path = prepare_runtime_files(
+            base_setting=base_setting,
+            world_template=world_template,
+            description_share=description_share,
+            output_dir=output_dir,
+            moveit_cpp_path=package_share / 'config' / 'moveit_cpp.yaml',
+        )
+    except Exception:
+        shutil.rmtree(output_dir, ignore_errors=True)
+        raise
+
+    print(
+        f'[iiwa_experement] experiment {experiment_id} mode={mode} '
+        f'delay={start_delay_sec:g}s: {world_path}'
+    )
+    return runtime_actions(
+        experiment_id=experiment_id,
+        scene_config=scene_config,
+        setting_path=str(setting_path),
+        rviz=rviz,
+        cleanup_dir=str(output_dir),
+        mode=mode,
+        start_delay_sec=start_delay_sec,
+        trace_dir=trace_dir,
+        geometry_path=geometry_path,
+    )
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'experiment_id',
+            default_value='',
+            description='Experiment number 1..20; required.',
+        ),
+        DeclareLaunchArgument(
+            'mode',
+            default_value='view',
+            description=(
+                'view = open the prepared scene without motion; '
+                'execute = run the reference sequence after start_delay_sec; '
+                'planner = writable scene for an externally supplied graph, '
+                'no reference sequence.'
+            ),
+        ),
+        DeclareLaunchArgument(
+            'start_delay_sec',
+            default_value=f'{DEFAULT_START_DELAY_SEC:g}',
+            description='Simulation-time delay before the first execute motion.',
+        ),
+        DeclareLaunchArgument(
+            'trace_dir',
+            default_value=DEFAULT_TRACE_DIR,
+            description='Directory for one JSONL trace per run.',
+        ),
+        DeclareLaunchArgument(
+            'rviz',
+            default_value='false',
+            description='true = start RViz together with Webots.',
+        ),
+        DeclareLaunchArgument(
+            'scene_config',
+            default_value=PathJoinSubstitution([
+                FindPackageShare('iiwa_experement'), 'config', 'scenes.yaml',
+            ]),
+            description='Scenario manifest; passed unchanged to scene_spawner.',
+        ),
+        DeclareLaunchArgument(
+            'base_setting',
+            default_value=PathJoinSubstitution([
+                FindPackageShare('iiwa_config'), 'config', 'setting.yaml',
+            ]),
+            description='Base settings used to build the temporary setting.yaml.',
+        ),
+        OpaqueFunction(function=_setup),
+    ])

src/iiwa_experement/package.xml

@@ -0,0 +1,39 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>iiwa_experement</name>
+  <version>2026.11.8</version>
+  <description>Deterministic Webots scenes for the twenty KUKA iiwa 7 grant experiments</description>
+  <maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>iiwa_bringup</exec_depend>
+  <exec_depend>iiwa_config</exec_depend>
+  <exec_depend>iiwa_description</exec_depend>
+  <exec_depend>iiwa_msgs</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>moveit_msgs</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-scipy</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>rosgraph_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>webots_ros2_driver</exec_depend>
+  <exec_depend>webots_ros2_msgs</exec_depend>
+
+  <test_depend>iiwa_utils</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/iiwa_experement/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/iiwa_experement
+[install]
+install_scripts=$base/lib/iiwa_experement

src/iiwa_experement/setup.py

@@ -0,0 +1,33 @@
+from glob import glob
+
+from setuptools import find_packages, setup
+
+package_name = 'iiwa_experement'
+
+setup(
+    name=package_name,
+    version='2026.11.8',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/doc', glob('doc/*.md')),
+        ('share/' + package_name + '/config', glob('config/*.yaml')),
+        ('share/' + package_name + '/launch', glob('launch/*.py')),
+        ('share/' + package_name + '/worlds/grant', glob('worlds/grant/*.wbt')),
+    ],
+    install_requires=['setuptools', 'PyYAML'],
+    tests_require=['pytest'],
+    zip_safe=True,
+    maintainer='daniel',
+    maintainer_email='grabardm@ml-dev.ru',
+    description='Deterministic Webots scenes for the twenty KUKA iiwa 7 grant experiments',
+    license='Apache-2.0',
+    entry_points={
+        'console_scripts': [
+            'scene_spawner = iiwa_experement.scene_spawner:main',
+            'grant_supervisor = iiwa_experement.grant_supervisor:main',
+            'reference_runner = iiwa_experement.reference_runner:main',
+        ],
+    },
+)

src/iiwa_experement/test/test_geometry.py

@@ -0,0 +1,225 @@
+"""Calibrated geometry: tool mounting, hatch, contour and holes."""
+from pathlib import Path
+
+import numpy as np
+import pytest
+import yaml
+from ament_index_python.packages import get_package_share_directory
+
+from iiwa_experement.geometry import (
+    GeometryError,
+    Transform,
+    load_geometry,
+    object_pose_transform,
+    sealant_point,
+    tcp_goal,
+    world_target,
+)
+from iiwa_experement.scenario import load_scenario
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+SCENES = PACKAGE / 'config' / 'scenes.yaml'
+GEOMETRY = PACKAGE / 'config' / 'geometry.yaml'
+ASSET_ROOT = (
+    Path(get_package_share_directory('iiwa_description')) / 'objects' / 'grant'
+)
+
+
+def geometry():
+    return load_geometry(GEOMETRY)
+
+
+def scenario(experiment_id):
+    return load_scenario(experiment_id, SCENES)
+
+
+def obj_y_bounds(asset):
+    low, high = np.inf, -np.inf
+    for line in (ASSET_ROOT / asset).read_text(encoding='utf-8', errors='replace').splitlines():
+        if line.startswith('v '):
+            y = float(line.split()[2])
+            low, high = min(low, y), max(high, y)
+    return low, high
+
+
+def test_tool_lengths_and_sealant_scale():
+    for asset in ('vacum-tool.obj', 'magnit-tool.obj', 'wranch-tool.obj'):
+        low, high = obj_y_bounds(asset)
+        assert low == pytest.approx(0.0, abs=1e-12)
+        assert high == pytest.approx(0.140, abs=1e-12)
+    low, high = obj_y_bounds('germetic-tool.obj')
+    assert low == pytest.approx(0.0, abs=1e-12)
+    assert high == pytest.approx(0.014, abs=1e-12)
+    assert high * 10.0 == pytest.approx(0.140)
+
+
+def test_patron_tcp_offset_is_55_2_mm():
+    spec = geometry()
+    assert spec.patron_to_tcp.parent == 'patron'
+    assert spec.patron_to_tcp.child == 'tcp'
+    assert spec.patron_to_tcp.position == pytest.approx([0.0, 0.0, 0.0552])
+
+
+def test_all_tools_mount_at_tcp_and_end_fourteen_centimetres_away():
+    spec = geometry()
+    for name in ('vacuum', 'magnetic_tool', 'sealant_tool'):
+        mount = spec.tool_mounts[name]
+        tip = spec.tool_work_tips[name]
+        assert mount.parent == tip.parent == 'tcp'
+        assert mount.position == pytest.approx([0.0, 0.0, 0.0])
+        assert tip.position == pytest.approx([0.0, 0.0, 0.140])
+        node = spec.tool_tcp_to_nodes[name]
+        assert node.position == pytest.approx([0.0, 0.0, 0.140])
+        # The node transform must reproduce an upright OBJ.
+        world_tcp = Transform.from_xyz_rpy(
+            [0.0, 0.0, 0.93], [180.0, 0.0, 45.0], parent='world', child='tcp',
+        )
+        tool_mount = world_tcp.compose(mount)
+        assert tool_mount.parent == 'world'
+        assert tool_mount.child == f'{name}_mount'
+        node_world = world_tcp.compose(node)
+        # Node origin is the working tip, 0.14 below the mount.
+        assert node_world.position == pytest.approx([0.0, 0.0, 0.79], abs=1e-12)
+        assert np.linalg.det(node_world.matrix) == pytest.approx(1.0)
+
+
+def test_ten_screws_have_distinct_ids_and_poses():
+    for experiment_id in range(11, 21):
+        spec = scenario(experiment_id)
+        assert len(spec.screws) == 10
+        assert len({screw.name for screw in spec.screws}) == 10
+        assert len({(screw.pose.x, screw.pose.y) for screw in spec.screws}) == 10
+
+
+def test_experiment_three_moves_holes_with_the_body():
+    standard = scenario(1)
+    shifted = scenario(3)
+    g = geometry()
+    for hole in ('hole_1', 'hole_2', 'hole_3', 'hole_4'):
+        ref = world_target(standard, g, f'hole:{hole}')
+        moved = world_target(shifted, g, f'hole:{hole}')
+        assert moved.position[:2] == pytest.approx(
+            ref.position[:2] + np.array([-0.01, 0.02]), abs=1e-9,
+        )
+        assert moved.position[2] == pytest.approx(ref.position[2], abs=1e-12)
+
+
+def test_standard_holes_match_the_documented_coordinates():
+    g = geometry()
+    spec = scenario(11)
+    expected = {
+        'hole_1': (0.2243, 0.1052),
+        'hole_2': (0.2852, 0.0443),
+        'hole_3': (0.3757, 0.1348),
+        'hole_4': (0.3148, 0.1957),
+    }
+    for name, xy in expected.items():
+        target = world_target(spec, g, f'hole:{name}')
+        assert target.position[:2] == pytest.approx(xy, abs=1e-9)
+        assert target.position[2] == pytest.approx(0.823 + 0.00894 + 0.002, abs=1e-12)
+
+
+def test_experiment_nineteen_moves_screws_with_the_tray():
+    g = geometry()
+    standard = scenario(11)
+    shifted = scenario(19)
+    delta = np.array([-0.02, 0.02, 0.0])
+    for index in range(1, 11):
+        name = f'screw_{index}'
+        ref = world_target(standard, g, f'screw:{name}')
+        moved = world_target(shifted, g, f'screw:{name}')
+        assert moved.position == pytest.approx(ref.position + delta, abs=1e-9)
+
+
+def test_cap_seat_follows_the_body():
+    g = geometry()
+    for experiment_id in (1, 3, 11):
+        spec = scenario(experiment_id)
+        body = object_pose_transform(
+            spec.body.x, spec.body.y, spec.body.z, spec.body.yaw_deg, child='body',
+        )
+        seat = world_target(spec, g, 'cap_seat')
+        expected = body.compose(g.body_to_installed_cap)
+        assert seat.position == pytest.approx(expected.position, abs=1e-12)
+        assert seat.matrix == pytest.approx(expected.matrix, abs=1e-12)
+
+
+def test_cap_collision_center_is_below_tcp():
+    g = geometry()
+    assert g.cap_collision.offset[2] == pytest.approx(0.146 - 0.004165, abs=1e-9)
+    assert g.cap_collision.size[2] == pytest.approx(0.00367, abs=1e-12)
+
+
+def test_sealant_contour_is_closed_and_ordered():
+    g = geometry()
+    spec = scenario(18)
+    sides = ('side_1', 'side_2', 'side_3', 'side_4')
+    starts = [world_target(spec, g, f'sealant:{side}') for side in sides]
+    ends = [sealant_point(g, spec, side, 1.0) for side in sides]
+    for index in range(4):
+        assert starts[index].position[2] == pytest.approx(ends[index].position[2])
+        assert starts[(index + 1) % 4].position[:2] == pytest.approx(
+            ends[index].position[:2], abs=1e-9,
+        )
+
+
+def test_tcp_goal_places_tip_on_the_target_and_points_down():
+    g = geometry()
+    spec = scenario(18)
+    target = sealant_point(g, spec, 'side_1', 0.5)
+    tcp = tcp_goal(target, g.tool_work_tips['sealant_tool'])
+    assert tcp.position == pytest.approx(
+        target.position + np.array([0.0, 0.0, 0.140]), abs=1e-12,
+    )
+    assert tcp.matrix[2, 2] == pytest.approx(-1.0, abs=1e-12)
+
+
+def test_tool_and_cap_node_targets_produce_downward_tcp():
+    g = geometry()
+    spec = scenario(1)
+    for target, relative in (
+        (world_target(spec, g, 'tool:vacuum'), g.tool_tcp_to_nodes['vacuum']),
+        (world_target(spec, g, 'cap'), g.cap_tcp_to_node),
+    ):
+        tcp = tcp_goal(target, relative)
+        assert tcp.matrix[2, 2] == pytest.approx(-1.0, abs=1e-9)
+
+
+def test_missing_frames_are_rejected(tmp_path):
+    data = yaml.safe_load(GEOMETRY.read_text(encoding='utf-8'))
+    data['patron_to_tcp']['parent'] = 'missing'
+    data['patron_to_tcp'].pop('parent')
+    path = tmp_path / 'geometry.yaml'
+    path.write_text(yaml.safe_dump(data, sort_keys=False), encoding='utf-8')
+    # The loader supplies parent/child itself, so a missing key is not an
+    # error; a missing transform block is.
+    data.pop('patron_to_tcp')
+    path.write_text(yaml.safe_dump(data, sort_keys=False), encoding='utf-8')
+    with pytest.raises(GeometryError, match='patron_to_tcp'):
+        load_geometry(path)
+
+
+def test_nonnumeric_and_nonpositive_values_are_rejected(tmp_path):
+    data = yaml.safe_load(GEOMETRY.read_text(encoding='utf-8'))
+    data['approach_gap'] = 'far'
+    path = tmp_path / 'geometry.yaml'
+    path.write_text(yaml.safe_dump(data, sort_keys=False), encoding='utf-8')
+    with pytest.raises(GeometryError, match='approach_gap'):
+        load_geometry(path)
+
+    data = yaml.safe_load(GEOMETRY.read_text(encoding='utf-8'))
+    data['contact_gap'] = 0.0
+    path.write_text(yaml.safe_dump(data, sort_keys=False), encoding='utf-8')
+    with pytest.raises(GeometryError, match='contact_gap'):
+        load_geometry(path)
+
+
+def test_unknown_targets_are_rejected():
+    g = geometry()
+    with pytest.raises(GeometryError, match='unknown world target'):
+        world_target(scenario(1), g, 'nothing')
+    with pytest.raises(GeometryError, match='has no tool'):
+        world_target(scenario(1), g, 'tool:magnetic_tool')
+    with pytest.raises(GeometryError, match='has no screw'):
+        world_target(scenario(1), g, 'screw:screw_1')

src/iiwa_experement/test/test_grant_supervisor.py

@@ -0,0 +1,244 @@
+"""Supervisor command plumbing with fake Webots nodes (no simulator run)."""
+import math
+from pathlib import Path
+
+import numpy as np
+import pytest
+
+from iiwa_experement.geometry import load_geometry, tcp_goal, world_target
+from iiwa_experement.grant_supervisor import (
+    COMMAND_SERVICE,
+    GrantSupervisorNode,
+    _axis_angle,
+    transform_from_json,
+)
+from iiwa_experement.object_state import ObjectState, transform_json
+from iiwa_experement.scenario import load_scenario
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+SCENES = PACKAGE / 'config' / 'scenes.yaml'
+GEOMETRY = PACKAGE / 'config' / 'geometry.yaml'
+
+
+class FakeField:
+    def __init__(self, value):
+        self.value = value
+        self.removed = False
+
+    def setSFVec3f(self, value):
+        self.value = list(value)
+
+    def setSFRotation(self, value):
+        self.value = list(value)
+
+    def setSFBool(self, value):
+        self.value = bool(value)
+
+    def remove(self):
+        self.removed = True
+
+
+class FakeNode:
+    def __init__(self, transform):
+        self.translation = FakeField(list(transform.xyz))
+        self.rotation = FakeField(_axis_angle(transform.rotation))
+        self.locked = FakeField(True)
+        self.removed = False
+        self._matrix = transform.matrix
+
+    def getPosition(self):
+        return list(self.translation.value)
+
+    def getOrientation(self):
+        return [float(value) for value in self._matrix.reshape(-1)]
+
+    def getField(self, name):
+        if name == 'translation':
+            return self.translation
+        if name == 'rotation':
+            return self.rotation
+        if name == 'locked':
+            return self.locked
+        return None
+
+    def remove(self):
+        self.removed = True
+
+
+class FakeRobot:
+    def getTime(self):
+        return 0.0
+
+
+def make_node(experiment_id=1):
+    geometry = load_geometry(GEOMETRY)
+    spec = load_scenario(experiment_id, SCENES)
+    node = object.__new__(GrantSupervisorNode)
+    node.spec = spec
+    node.geometry = geometry
+    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()
+    }
+    node.robot = FakeRobot()
+    node.patron_node = None
+    node.nodes = {}
+    for tool in spec.tools:
+        pose = world_target(spec, geometry, f'tool:{tool.name}')
+        node.nodes[f'grant_{tool.name}'] = FakeNode(pose)
+    if spec.tray_pose is not None:
+        for index in range(1, 11):
+            name = f'screw_{index}'
+            node.nodes[f'grant_screw_{index:02d}'] = FakeNode(
+                world_target(spec, geometry, f'screw:{name}')
+            )
+    if spec.cap_state != 'absent':
+        node.nodes['grant_cap'] = FakeNode(world_target(spec, geometry, 'cap'))
+    return node
+
+
+def set_tcp(node, tcp):
+    patron = tcp.compose_relaxed(node.geometry.patron_to_tcp.inverse())
+    node.patron_node = FakeNode(patron)
+
+
+def test_command_service_name_is_stable():
+    assert COMMAND_SERVICE == '/iiwa_experement/object_command'
+
+
+def test_transform_json_roundtrip():
+    from iiwa_experement.geometry import Transform
+
+    transform = Transform.from_xyz_rpy(
+        [0.1, -0.2, 0.3], [10.0, 20.0, 30.0], parent='world', child='x',
+    )
+    payload = transform_json(transform)
+    restored = transform_from_json(payload)
+    assert restored.xyz == pytest.approx(transform.xyz)
+    assert restored.matrix == pytest.approx(transform.matrix)
+
+
+def test_invalid_pose_payload_is_rejected():
+    with pytest.raises(ValueError, match='xyz'):
+        transform_from_json({'rotation': [1, 0, 0, 0, 1, 0, 0, 0, 1]})
+    with pytest.raises(ValueError, match='rotation'):
+        transform_from_json({'xyz': [0, 0, 0], 'rotation': [1, 0, 0]})
+
+
+def test_axis_angle_identity_and_half_turn():
+    assert _axis_angle(np.eye(3)) == [0.0, 0.0, 1.0, 0.0]
+    from scipy.spatial.transform import Rotation
+
+    axis = _axis_angle(Rotation.from_euler('z', 90, degrees=True).as_matrix())
+    assert abs(axis[3]) == pytest.approx(math.pi / 2)
+    assert abs(axis[2]) == pytest.approx(1.0)
+
+
+def test_attach_requires_tcp_at_the_socket_and_rejects_far_pose():
+    node = make_node(1)
+    pose = node.state.tools['vacuum']['pose']
+    good = tcp_goal(pose, node.geometry.tool_tcp_to_nodes['vacuum'])
+    set_tcp(node, good)
+    accepted, error = node._apply('attach_tool', {'object': 'vacuum'}, 'r1')
+    assert accepted is True
+    assert error is None
+    assert node.tool_held == 'vacuum'
+
+    other = make_node(1)
+    set_tcp(other, good.compose_relaxed(
+        type(good).from_xyz_rpy(
+            [0.0, 0.0, 0.08], [0, 0, 0], parent='world', child='tcp',
+        )
+    ))
+    accepted, error = other._apply('attach_tool', {'object': 'vacuum'}, 'r2')
+    assert accepted is False
+    assert 'tcp is' in error
+    assert other.tool_held is None
+    assert other.state.version == 0
+
+
+def test_follow_moves_the_attached_tool_and_dock_returns_it_to_the_rack():
+    node = make_node(1)
+    rack_pose = node.state.tools['vacuum']['pose']
+    tcp = tcp_goal(rack_pose, node.geometry.tool_tcp_to_nodes['vacuum'])
+    set_tcp(node, tcp)
+    assert node._apply('attach_tool', {'object': 'vacuum'}, 'attach')[0]
+    node.follow()
+    moved = node.nodes['grant_vacuum'].getPosition()
+    assert moved == pytest.approx(rack_pose.position, abs=1e-9)
+    accepted, _ = node._apply('dock_tool', {'object': 'vacuum'}, 'dock')
+    assert accepted
+    assert node.tool_held is None
+    assert node.state.mounted_tool is None
+    assert node.nodes['grant_vacuum'].getPosition() == pytest.approx(
+        rack_pose.position, abs=1e-9,
+    )
+
+
+def test_grab_and_handoff_remove_only_the_selected_screw():
+    node = make_node(11)
+    magnetic = node.state.tools['magnetic_tool']['pose']
+    tcp = tcp_goal(magnetic, node.geometry.tool_tcp_to_nodes['magnetic_tool'])
+    set_tcp(node, tcp)
+    assert node._apply('attach_tool', {'object': 'magnetic_tool'}, 'a')[0]
+
+    screw = node.state.screws['screw_1']['pose']
+    screw_node = node.nodes['grant_screw_01']
+    tcp = tcp_goal(screw, node.geometry.screw_tcp_to_node)
+    set_tcp(node, tcp)
+    assert node._apply('grab_screw', {'object': 'screw_1'}, 'g')[0]
+    node.follow()
+    assert node.nodes['grant_screw_01'].getPosition() == pytest.approx(
+        screw.position, abs=1e-9,
+    )
+
+    hole = world_target(node.spec, node.geometry, 'hole:hole_1')
+    tcp = tcp_goal(hole, node.geometry.screw_tcp_to_node)
+    set_tcp(node, tcp)
+    node.follow()
+    target = transform_json(hole)
+    accepted, error = node._apply(
+        'handoff_screw',
+        {'object': 'screw_1', 'target': target, 'target_name': 'hole_1'},
+        'h',
+    )
+    assert accepted, error
+    assert screw_node.translation.value == pytest.approx([100.0, 100.0, -100.0])
+    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'
+
+
+def test_follow_refuses_when_tcp_is_unknown():
+    node = make_node(1)
+    node.patron_node = None
+    node.tool_held = 'vacuum'
+    node.follow()
+    assert node.nodes['grant_vacuum'].getPosition() == pytest.approx(
+        node.state.tools['vacuum']['pose'].position, abs=1e-9,
+    )
+
+
+def test_mounted_tool_and_payload_follow_together():
+    node = make_node(1)
+    tool_pose = node.state.tools['vacuum']['pose']
+    tcp = tcp_goal(tool_pose, node.geometry.tool_tcp_to_nodes['vacuum'])
+    set_tcp(node, tcp)
+    assert node._apply('attach_tool', {'object': 'vacuum'}, 't')[0]
+    cap_pose = node.state.cap_pose
+    cap_tcp = tcp_goal(cap_pose, node.geometry.cap_tcp_to_node)
+    set_tcp(node, cap_tcp)
+    assert node._apply('grab_cap', {'object': 'cap'}, 'c')[0]
+    node.follow()
+    expected_tool = cap_tcp.compose_relaxed(node.geometry.tool_tcp_to_nodes['vacuum'])
+    expected_cap = cap_tcp.compose_relaxed(node.geometry.cap_tcp_to_node)
+    assert node.nodes['grant_vacuum'].getPosition() == pytest.approx(
+        expected_tool.position, abs=1e-9,
+    )
+    assert node.nodes['grant_cap'].getPosition() == pytest.approx(
+        expected_cap.position, abs=1e-9,
+    )

src/iiwa_experement/test/test_launch.py

@@ -0,0 +1,223 @@
+"""Launch validation and action wiring for view/execute modes."""
+import importlib.util
+from pathlib import Path
+
+import pytest
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+    RegisterEventHandler,
+)
+from launch_ros.actions import Node
+
+from iiwa_experement.runtime_config import (
+    DEFAULT_START_DELAY_SEC,
+    DEFAULT_TRACE_DIR,
+    validate_experiment_id,
+    validate_mode,
+    validate_start_delay,
+    validate_trace_dir,
+)
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+LAUNCH = PACKAGE / 'launch' / 'experiment.launch.py'
+
+
+def load_launch_module():
+    spec = importlib.util.spec_from_file_location('experiment_launch', LAUNCH)
+    module = importlib.util.module_from_spec(spec)
+    assert spec.loader is not None
+    spec.loader.exec_module(module)
+    return module
+
+
+@pytest.mark.parametrize('raw, expected', [('1', 1), ('20', 20), (' 11 ', 11)])
+def test_valid_experiment_ids(raw, expected):
+    assert validate_experiment_id(raw) == expected
+
+
+@pytest.mark.parametrize('raw', ['', '0', '21', 'abc', '-1', '1.0', '01', '  '])
+def test_invalid_experiment_ids_are_rejected(raw):
+    with pytest.raises(ValueError):
+        validate_experiment_id(raw)
+
+
+@pytest.mark.parametrize('raw, expected', [
+    ('view', 'view'),
+    ('execute', 'execute'),
+    ('planner', 'planner'),
+    (' VIEW ', 'view'),
+])
+def test_valid_modes(raw, expected):
+    assert validate_mode(raw) == expected
+
+
+@pytest.mark.parametrize('raw', ['', 'run', 'VIEW '[:-1] + 'x', '1', 'plan'])
+def test_invalid_modes_are_rejected(raw):
+    with pytest.raises(ValueError):
+        validate_mode(raw)
+
+
+@pytest.mark.parametrize('raw, expected', [('0', 0.0), ('3', 3.0), ('2.5', 2.5)])
+def test_valid_start_delays(raw, expected):
+    assert validate_start_delay(raw) == expected
+
+
+@pytest.mark.parametrize('raw', ['', '-1', 'abc', 'nan', 'inf'])
+def test_invalid_start_delays_are_rejected(raw):
+    with pytest.raises(ValueError):
+        validate_start_delay(raw)
+
+
+def test_trace_dir_must_not_be_empty():
+    assert validate_trace_dir('~/.ros/run') == '~/.ros/run'
+    with pytest.raises(ValueError):
+        validate_trace_dir('  ')
+
+
+def test_view_starts_bringup_spawner_and_read_only_supervisor():
+    module = load_launch_module()
+    actions = module.runtime_actions(
+        experiment_id=19,
+        scene_config='/tmp/custom scenes.yaml',
+        setting_path='/tmp/run/setting.yaml',
+        rviz='false',
+        mode='view',
+    )
+    includes = [a for a in actions if isinstance(a, IncludeLaunchDescription)]
+    nodes = [a for a in actions if isinstance(a, Node)]
+    processes = [a for a in actions if type(a) is ExecuteProcess]
+    assert len(includes) == 1
+    assert len(nodes) == 1
+    assert len(processes) == 1
+    arguments = dict(includes[0].launch_arguments)
+    assert arguments['simulate'] == 'true'
+    assert arguments['rviz'] == 'false'
+    assert arguments['setting'] == '/tmp/run/setting.yaml'
+    spawner = nodes[0]
+    assert spawner.node_package == 'iiwa_experement'
+    assert spawner.node_executable == 'scene_spawner'
+    assert module.spawner_parameters(19, '/tmp/custom scenes.yaml') == {
+        'experiment_id': 19,
+        'scene_config': '/tmp/custom scenes.yaml',
+    }
+    parameters = module.supervisor_parameters(
+        19, '/tmp/custom scenes.yaml', '/tmp/geometry.yaml', True,
+    )
+    assert parameters['experiment_id'] == 19
+    assert parameters['read_only'] is True
+    # A view run has no motion client or trajectory targets.
+    assert not any(
+        isinstance(action, Node) and action.node_executable == 'reference_runner'
+        for action in actions
+    )
+
+
+def test_execute_adds_the_reference_runner_after_the_supervisor():
+    module = load_launch_module()
+    actions = module.runtime_actions(
+        experiment_id=1,
+        scene_config='/tmp/scenes.yaml',
+        setting_path='/tmp/setting.yaml',
+        mode='execute',
+        start_delay_sec=4.5,
+    )
+    runner = [
+        action for action in actions
+        if isinstance(action, Node) and action.node_executable == 'reference_runner'
+    ]
+    assert len(runner) == 1
+    payload = module.runner_parameters(
+        1, '/tmp/scenes.yaml', '/tmp/geometry.yaml', 4.5, DEFAULT_TRACE_DIR,
+    )
+    assert payload['experiment_id'] == 1
+    assert payload['start_delay_sec'] == 4.5
+    assert payload['trace_dir'] == DEFAULT_TRACE_DIR
+    parameters = module.supervisor_parameters(
+        1, '/tmp/scenes.yaml', '/tmp/geometry.yaml', False,
+    )
+    assert parameters['read_only'] is False
+    assert any(type(action) is ExecuteProcess for action in actions)
+
+
+def _process_arguments(process):
+    return [item[0].text for item in process.cmd if item and item[0].text]
+
+
+def test_planner_is_writable_and_has_no_reference_runner():
+    module = load_launch_module()
+    actions = module.runtime_actions(
+        experiment_id=20,
+        scene_config='/tmp/scenes.yaml',
+        setting_path='/tmp/setting.yaml',
+        mode='planner',
+    )
+    assert not any(
+        isinstance(action, Node) and action.node_executable == 'reference_runner'
+        for action in actions
+    )
+    processes = [action for action in actions if type(action) is ExecuteProcess]
+    assert len(processes) == 1
+    assert 'read_only:=false' in _process_arguments(processes[0])
+    assert module.mode_read_only('planner') is False
+    parameters = module.supervisor_parameters(
+        20, '/tmp/scenes.yaml', '/tmp/geometry.yaml', False,
+    )
+    assert parameters['read_only'] is False
+
+
+def test_view_remains_read_only_after_adding_the_planner_mode():
+    module = load_launch_module()
+    actions = module.runtime_actions(
+        experiment_id=1,
+        scene_config='/tmp/scenes.yaml',
+        setting_path='/tmp/setting.yaml',
+        mode='view',
+    )
+    processes = [action for action in actions if type(action) is ExecuteProcess]
+    assert 'read_only:=true' in _process_arguments(processes[0])
+    assert module.mode_read_only('view') is True
+    assert module.mode_read_only('execute') is False
+
+
+def test_runtime_actions_can_register_cleanup(tmp_path):
+    module = load_launch_module()
+    actions = module.runtime_actions(
+        experiment_id=1,
+        scene_config='/tmp/scenes.yaml',
+        setting_path='/tmp/setting.yaml',
+        cleanup_dir=str(tmp_path / 'run'),
+    )
+    assert isinstance(actions[-1], RegisterEventHandler)
+    target = tmp_path / 'run'
+    target.mkdir()
+    (target / 'world.wbt').write_text('temporary', encoding='utf-8')
+    module._remove_directory(None, directory=str(target))
+    assert not target.exists()
+
+
+def test_launch_description_declares_all_arguments_and_validates_in_opaque_function():
+    module = load_launch_module()
+    description = module.generate_launch_description()
+    arguments = {
+        entity.name: entity.default_value
+        for entity in description.entities
+        if isinstance(entity, DeclareLaunchArgument)
+    }
+    assert set(arguments) == {
+        'experiment_id', 'mode', 'start_delay_sec', 'trace_dir',
+        'rviz', 'scene_config', 'base_setting',
+    }
+    assert [substitution.text for substitution in arguments['experiment_id']] == ['']
+    assert [substitution.text for substitution in arguments['mode']] == ['view']
+    assert [substitution.text for substitution in arguments['start_delay_sec']] == [
+        f'{DEFAULT_START_DELAY_SEC:g}'
+    ]
+    assert [substitution.text for substitution in arguments['trace_dir']] == [
+        DEFAULT_TRACE_DIR
+    ]
+    opaque = [e for e in description.entities if isinstance(e, OpaqueFunction)]
+    assert len(opaque) == 1

src/iiwa_experement/test/test_motion.py

@@ -0,0 +1,291 @@
+"""Motion guards: stale state, unsynchronized scene, failures and stop."""
+from pathlib import Path
+
+import pytest
+
+from scipy.spatial.transform import Rotation
+
+from iiwa_experement.geometry import Transform, load_geometry
+from iiwa_experement.motion import MotionAdapter
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+GEOMETRY = PACKAGE / 'config' / 'geometry.yaml'
+
+
+class FakeFuture:
+    def __init__(self, value=None, done=True):
+        self._value = value
+        self._done = done
+
+    def done(self):
+        return self._done
+
+    def result(self):
+        return self._value
+
+
+class FakeResult:
+    def __init__(self, success=True, message='ok'):
+        self.success = success
+        self.message = message
+
+
+class FakeWrapped:
+    def __init__(self, result):
+        self.result = result
+
+
+class FakeGoalHandle:
+    def __init__(self, accepted=True, result=None):
+        self.accepted = accepted
+        self._result = result or FakeResult()
+
+    def get_result_async(self):
+        return FakeFuture(FakeWrapped(self._result))
+
+
+class FakeActionClient:
+    def __init__(self, *, available=True, accepted=True, result=None, timeout=False):
+        self.available = available
+        self.accepted = accepted
+        self.result = result
+        self.timeout = timeout
+        self.requests = []
+
+    def wait_for_server(self, timeout_sec=0.0):
+        return self.available
+
+    def send_goal_async(self, request):
+        self.requests.append(request)
+        if self.timeout:
+            return FakeFuture(None, done=False)
+        return FakeFuture(FakeGoalHandle(self.accepted, self.result))
+
+
+class FakeService:
+    def __init__(self, *, available=True, success=True, message='ok', timeout=False):
+        self.available = available
+        self.success = success
+        self.message = message
+        self.timeout = timeout
+        self.requests = []
+
+    def wait_for_service(self, timeout_sec=0.0):
+        return self.available
+
+    def call_async(self, request):
+        self.requests.append(request)
+        if self.timeout:
+            return FakeFuture(None, done=False)
+        return FakeFuture(FakeResult(self.success, self.message))
+
+
+class StopService(FakeService):
+    def __init__(self):
+        super().__init__(success=True, message='stopped')
+        self.calls = 0
+
+    def call_async(self, request):
+        self.calls += 1
+        return super().call_async(request)
+
+
+def geometry():
+    return load_geometry(GEOMETRY)
+
+
+def goal():
+    return Transform.from_xyz_rpy(
+        [0.20, 0.10, 0.90], [180.0, 0.0, 45.0], parent='world', child='tcp',
+    )
+
+
+def make_adapter(
+    *,
+    action=None,
+    joints=None,
+    named=None,
+    stop=None,
+    fresh=True,
+    scene=True,
+    actual=None,
+    spin=None,
+):
+    g = geometry()
+    action = action or FakeActionClient()
+    named = named or FakeService()
+    stop = stop or StopService()
+    adapter = MotionAdapter(
+        node=None,
+        geometry=g,
+        action_client=action,
+        joints_client=joints,
+        named_client=named,
+        stop_client=stop,
+        tf_buffer=object(),
+        scene_check=lambda: (scene, 'scene is stale' if not scene else 'ok'),
+        freshness_check=lambda: (fresh, 'joint state is stale' if not fresh else 'ok'),
+        pose_provider=lambda: actual if actual is not None else adapter.to_base_goal(goal()),
+        spin_until=spin or (lambda future, timeout: future.done()),
+    )
+    return adapter, action, named, stop
+
+
+def test_successful_move_populates_abc_and_verifies_actual_pose():
+    adapter, action, _, _ = make_adapter()
+    result = adapter.move_pose(goal(), planner='ptp', speed=0.2, label='test')
+    assert result.success, result.message
+    assert len(action.requests) == 1
+    request = action.requests[0]
+    expected = adapter.to_base_goal(goal())
+    assert (request.x, request.y, request.z) == pytest.approx(expected.xyz)
+    assert request.speed == pytest.approx(0.2)
+    assert request.planner == 'ptp'
+    assert request.frame_id == 'base_link'
+    assert result.goal.xyz == pytest.approx(expected.xyz)
+
+
+def test_stale_joint_state_blocks_before_sending_and_stops():
+    stop = StopService()
+    adapter, action, _, _ = make_adapter(fresh=False, stop=stop)
+    result = adapter.move_pose(goal(), label='stale')
+    assert not result.success
+    assert 'stale' in result.message
+    assert action.requests == []
+    assert adapter.stopped is True
+    assert stop.calls == 1
+
+
+def test_unsynchronized_scene_blocks_before_sending_and_stops():
+    adapter, action, _, _ = make_adapter(scene=False)
+    result = adapter.move_pose(goal(), label='scene')
+    assert not result.success
+    assert 'scene' in result.message
+    assert action.requests == []
+    assert adapter.stopped is True
+
+
+def test_invalid_speed_and_planner_are_rejected_without_stopping():
+    adapter, action, _, _ = make_adapter()
+    assert not adapter.move_pose(goal(), speed=0.0).success
+    assert not adapter.move_pose(goal(), speed=2.0).success
+    assert not adapter.move_pose(goal(), planner='teleport').success
+    assert action.requests == []
+    assert adapter.stopped is False
+
+
+def test_rejected_goal_stops_and_blocks_the_next_move():
+    action = FakeActionClient(accepted=False)
+    adapter, action, _, stop = make_adapter(action=action)
+    result = adapter.move_pose(goal(), label='rejected')
+    assert not result.success
+    assert 'rejected' in result.message
+    assert adapter.stopped is True
+    assert stop.calls == 1
+    count = len(action.requests)
+    blocked = adapter.move_pose(goal(), label='again')
+    assert not blocked.success
+    assert 'blocked' in blocked.message
+    assert len(action.requests) == count
+
+
+def test_timeout_stops_the_robot():
+    adapter, _, _, stop = make_adapter(action=FakeActionClient(timeout=True))
+    result = adapter.move_pose(goal(), label='timeout')
+    assert not result.success
+    assert 'timed out' in result.message
+    assert adapter.stopped is True
+    assert stop.calls == 1
+
+
+def test_actual_pose_out_of_tolerance_stops_the_robot():
+    actual = goal().compose_relaxed(Transform.from_xyz_rpy(
+        [0.0, 0.0, 0.05], [0, 0, 0], parent='world', child='tcp',
+    ))
+    adapter, _, _, stop = make_adapter(actual=actual)
+    result = adapter.move_pose(goal(), label='mismatch')
+    assert not result.success
+    assert 'from the goal' in result.message
+    assert adapter.stopped is True
+    assert stop.calls == 1
+
+
+def test_named_motion_uses_the_joint_action_when_available():
+    joints = FakeActionClient()
+    adapter, action, _, _ = make_adapter(joints=joints)
+    result = adapter.move_named('home', speed=0.2)
+    assert result.success
+    assert len(joints.requests) == 1
+    assert list(joints.requests[0].joints) == [0.0] * 7
+    assert joints.requests[0].speed == pytest.approx(0.2)
+
+
+def test_named_motion_uses_the_existing_service():
+    adapter, _, named, _ = make_adapter()
+    result = adapter.move_named('home', speed=0.2)
+    assert result.success
+    assert len(named.requests) == 1
+    assert named.requests[0].name == 'home'
+    assert named.requests[0].speed == pytest.approx(0.2)
+
+
+def test_failed_named_motion_stops():
+    named = FakeService(success=False, message='unknown state')
+    adapter, _, _, stop = make_adapter(named=named)
+    result = adapter.move_named('missing')
+    assert not result.success
+    assert 'unknown state' in result.message
+    assert adapter.stopped is True
+    assert stop.calls == 1
+
+
+def test_unavailable_server_is_reported_without_motion():
+    adapter, action, _, _ = make_adapter(action=FakeActionClient(available=False))
+    result = adapter.move_pose(goal(), label='no server')
+    assert not result.success
+    assert 'unavailable' in result.message
+    assert action.requests == []
+    assert adapter.stopped is False
+
+
+def test_work_fallback_uses_the_safe_pose_and_never_transport():
+    planning_error = 'Планирование не удалось: injected failure'
+    joints = FakeActionClient(result=FakeResult(False, planning_error))
+    named = FakeService(success=False, message=planning_error)
+    action = FakeActionClient()
+
+    def echo_pose():
+        request = action.requests[-1]
+        matrix = Rotation.from_euler(
+            'ZYX', [request.a, request.b, request.c],
+        ).as_matrix()
+        return Transform(
+            xyz=(request.x, request.y, request.z),
+            rotation=tuple(tuple(float(v) for v in row) for row in matrix),
+            parent='base_link', child='tcp',
+        )
+
+    g = geometry()
+    # The forbidden pose is not mirrored into the executor at all.
+    assert 'transport' not in g.named_joints
+    adapter = MotionAdapter(
+        node=None,
+        geometry=g,
+        action_client=action,
+        joints_client=joints,
+        named_client=named,
+        stop_client=StopService(),
+        tf_buffer=object(),
+        scene_check=lambda: (True, 'ok'),
+        freshness_check=lambda: (True, 'ok'),
+        pose_provider=echo_pose,
+        spin_until=lambda future, timeout: future.done(),
+    )
+    result = adapter.move_named('work', speed=0.2)
+    assert result.success, result.message
+    # Both named paths were rejected, so the calibrated safe Cartesian pose
+    # was planned with OMPL instead of reconfiguring through 'transport'.
+    assert action.requests[-1].planner == 'ompl'
+    for request in joints.requests:
+        assert list(request.joints) == list(g.named_joints['work'])

src/iiwa_experement/test/test_object_state.py

@@ -0,0 +1,227 @@
+"""Object ownership rejects unsafe transitions and never mutates on failure."""
+from pathlib import Path
+
+import pytest
+
+from iiwa_experement.geometry import load_geometry, tcp_goal, world_target
+from iiwa_experement.object_state import CommandError, ObjectState
+from iiwa_experement.scenario import load_scenario
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+SCENES = PACKAGE / 'config' / 'scenes.yaml'
+GEOMETRY = PACKAGE / 'config' / 'geometry.yaml'
+
+
+def make_state(experiment_id, *, read_only=False):
+    return ObjectState(
+        load_scenario(experiment_id, SCENES),
+        load_geometry(GEOMETRY),
+        read_only=read_only,
+    )
+
+
+def attach_tool(state, name):
+    tool_pose = state.tools[name]['pose']
+    tcp = tcp_goal(tool_pose, state.geometry.tool_tcp_to_nodes[name])
+    state.attach_tool(name, tcp, tool_pose, request_id=f'attach-{name}')
+    return tcp
+
+
+def grab_cap(state):
+    cap_pose = state.cap_pose
+    tcp = tcp_goal(cap_pose, state.geometry.cap_tcp_to_node)
+    state.grab_cap(tcp, cap_pose, request_id='grab-cap')
+    return tcp
+
+
+def place_cap(state):
+    target = world_target(state.spec, state.geometry, 'cap_seat')
+    tcp = tcp_goal(target, state.geometry.cap_tcp_to_node)
+    object_pose = tcp.compose_relaxed(state.geometry.cap_tcp_to_node)
+    state.place_cap(target, tcp, object_pose, request_id='place-cap')
+
+
+def grab_screw(state, screw_id):
+    screw_pose = state.screws[screw_id]['pose']
+    tcp = tcp_goal(screw_pose, state.geometry.screw_tcp_to_node)
+    state.grab_screw(screw_id, tcp, screw_pose, request_id=f'grab-{screw_id}')
+    return tcp
+
+
+def handoff(state, screw_id, hole_id):
+    target = world_target(state.spec, state.geometry, f'hole:{hole_id}')
+    tcp = tcp_goal(target, state.geometry.screw_tcp_to_node)
+    object_pose = tcp.compose_relaxed(state.geometry.screw_tcp_to_node)
+    state.handoff_screw(
+        screw_id, hole_id, target, tcp, object_pose,
+        request_id=f'handoff-{screw_id}-{hole_id}',
+    )
+
+
+def test_initial_state_for_all_cap_modes():
+    free = make_state(1)
+    assert free.mounted_tool is None
+    assert free.cap_state == 'free'
+    assert free.screws == {}
+    installed = make_state(11)
+    assert installed.cap_state == 'installed'
+    assert len(installed.screws) == 10
+    absent = make_state(6)
+    assert absent.cap_state == 'absent'
+    assert absent.cap_pose is None
+    assert absent.screws == {}
+
+
+def test_attach_and_dock_tool_changes_only_that_tool():
+    state = make_state(1)
+    attach_tool(state, 'vacuum')
+    assert state.mounted_tool == 'vacuum'
+    assert state.tools['vacuum']['state'] == 'mounted'
+    assert state.version == 1
+    rack_pose = state.tools['vacuum']['pose']
+    attached_pose = rack_pose
+    tcp = tcp_goal(rack_pose, state.geometry.tool_tcp_to_nodes['vacuum'])
+    state.dock_tool('vacuum', tcp, rack_pose, attached_pose)
+    assert state.mounted_tool is None
+    assert state.tools['vacuum']['state'] == 'docked'
+    assert state.version == 2
+
+
+def test_tool_requires_contact_and_socket_state():
+    state = make_state(1)
+    tool_pose = state.tools['vacuum']['pose']
+    good = tcp_goal(tool_pose, state.geometry.tool_tcp_to_nodes['vacuum'])
+    far = good.compose_relaxed(
+        type(good).from_xyz_rpy(
+            [0.0, 0.0, 0.05], [0.0, 0.0, 0.0],
+            parent=good.parent, child=good.child,
+        )
+    )
+    with pytest.raises(CommandError, match='tcp is'):
+        state.attach_tool('vacuum', far, tool_pose)
+    assert state.version == 0
+    assert state.mounted_tool is None
+
+
+def test_second_tool_is_rejected_without_state_change():
+    state = make_state(16)
+    attach_tool(state, 'vacuum')
+    version = state.version
+    pose = state.tools['magnetic_tool']['pose']
+    tcp = tcp_goal(pose, state.geometry.tool_tcp_to_nodes['magnetic_tool'])
+    with pytest.raises(CommandError, match='another tool'):
+        state.attach_tool('magnetic_tool', tcp, pose)
+    assert state.version == version
+    assert state.mounted_tool == 'vacuum'
+
+
+def test_cap_needs_the_vacuum_tool_and_a_free_hand():
+    state = make_state(1)
+    with pytest.raises(CommandError, match='requires'):
+        grab_cap(state)
+    assert state.version == 0
+    attach_tool(state, 'vacuum')
+    grab_cap(state)
+    assert state.cap_state == 'held'
+    assert state.held_object == 'cap'
+    with pytest.raises(CommandError, match='forbidden while'):
+        state.grab_cap(state.cap_pose, state.cap_pose)
+    assert state.version == 2
+
+
+def test_place_cap_seats_only_with_the_hatch_held():
+    state = make_state(1)
+    attach_tool(state, 'vacuum')
+    grab_cap(state)
+    place_cap(state)
+    assert state.cap_state == 'installed'
+    assert state.held_object is None
+    target = world_target(state.spec, state.geometry, 'cap_seat')
+    assert state.cap_pose.position == pytest.approx(target.position, abs=1e-12)
+    with pytest.raises(CommandError, match='requires the hatch'):
+        place_cap(state)
+
+
+def test_screws_require_the_magnetic_tool_and_are_consumed_on_handoff():
+    state = make_state(11)
+    with pytest.raises(CommandError, match='requires'):
+        grab_screw(state, 'screw_1')
+    attach_tool(state, 'magnetic_tool')
+    grab_screw(state, 'screw_1')
+    assert state.screws['screw_1']['state'] == 'held'
+    handoff(state, 'screw_1', 'hole_1')
+    assert state.screws['screw_1']['state'] == 'delivered'
+    assert state.screws['screw_1']['hole'] == 'hole_1'
+    assert state.holes['hole_1']['state'] == 'empty'
+    assert state.holes['hole_1']['screw'] == 'screw_1'
+    assert len(state.delivered_pairs()) == 1
+    assert sum(
+        1 for entry in state.screws.values() if entry['state'] == 'tray'
+    ) == 9
+
+
+def test_repeated_handoff_and_unknown_hole_are_rejected():
+    state = make_state(11)
+    attach_tool(state, 'magnetic_tool')
+    grab_screw(state, 'screw_1')
+    handoff(state, 'screw_1', 'hole_1')
+    version = state.version
+    with pytest.raises(CommandError, match='not held'):
+        handoff(state, 'screw_1', 'hole_1')
+    assert state.version == version
+    grab_screw(state, 'screw_2')
+    target = world_target(state.spec, state.geometry, 'hole:hole_2')
+    tcp = tcp_goal(target, state.geometry.screw_tcp_to_node)
+    object_pose = tcp.compose_relaxed(state.geometry.screw_tcp_to_node)
+    with pytest.raises(CommandError, match='unknown hole'):
+        state.handoff_screw(
+            'screw_2', 'hole_9', target, tcp, object_pose,
+        )
+
+
+def test_a_screw_cannot_be_grabbed_twice():
+    state = make_state(11)
+    attach_tool(state, 'magnetic_tool')
+    grab_screw(state, 'screw_1')
+    with pytest.raises(CommandError, match='forbidden while'):
+        grab_screw(state, 'screw_2')
+
+
+def test_read_only_view_rejects_mutation_but_allows_inspection():
+    state = make_state(1, read_only=True)
+    before = state.snapshot()
+    with pytest.raises(CommandError, match='read-only'):
+        attach_tool(state, 'vacuum')
+    after = state.snapshot()
+    assert before['version'] == after['version'] == 0
+    assert after['last_error']
+
+
+def test_rejected_command_preserves_version_and_snapshot():
+    state = make_state(1)
+    attach_tool(state, 'vacuum')
+    snapshot = state.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)

src/iiwa_experement/test/test_operations.py

@@ -0,0 +1,319 @@
+"""Operation sequences are visible, ordered and fail closed."""
+import math
+from pathlib import Path
+
+import pytest
+
+from iiwa_experement.geometry import load_geometry, world_target
+from iiwa_experement.motion import MotionResult
+from iiwa_experement.operations import OperationError, ReferenceOperations
+from iiwa_experement.scenario import load_scenario
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+SCENES = PACKAGE / 'config' / 'scenes.yaml'
+GEOMETRY = PACKAGE / 'config' / 'geometry.yaml'
+
+
+class FakeSupervisor:
+    def __init__(self, spec, geometry):
+        self.spec = spec
+        self.geometry = geometry
+        self.commands = []
+        self.latest = {
+            'cap': {'state': spec.cap_state},
+            'screws': {},
+        }
+        self.fail_on = None
+
+    def object_pose(self, name):
+        mapping = {
+            'grant_cap': 'cap',
+            **{f'grant_{tool}': f'tool:{tool}' for tool in self.spec.tool_names},
+            **{
+                f'grant_screw_{index:02d}': f'screw:screw_{index}'
+                for index in range(1, 11)
+            },
+        }
+        target = mapping[name]
+        return world_target(self.spec, self.geometry, target)
+
+    def command(self, op, object_id='', **kwargs):
+        if self.fail_on == op:
+            raise OperationError(f'{op} rejected')
+        self.commands.append((op, object_id, kwargs))
+        return self.latest
+
+
+class FakeBridge:
+    def __init__(self):
+        self.attached = []
+        self.detached = []
+        self.removed = []
+        self.restored = []
+
+    def attach(self, spec):
+        self.attached.append(spec.object_id)
+
+    def detach(self, object_id, pose):
+        self.detached.append((object_id, pose))
+
+    def remove(self, object_id):
+        self.removed.append(object_id)
+        if object_id in self.attached:
+            self.attached.remove(object_id)
+
+    def restore_world(self, object_id):
+        if object_id in self.attached:
+            self.attached.remove(object_id)
+        self.restored.append(object_id)
+
+    def add_world(self, object_id, box, pose):
+        if object_id in self.attached:
+            self.attached.remove(object_id)
+        self.added_world = getattr(self, 'added_world', [])
+        self.added_world.append((object_id, pose))
+
+    def expected_sets(self):
+        return set(), set()
+
+    def assert_synchronized(self, *args, **kwargs):
+        return None
+
+
+class FakeMotion:
+    def __init__(self):
+        self.moves = []
+        self.named = []
+        self.fail_labels = set()
+        self.joint_moves = []
+        self.joint_values = (0.0,) * 7
+        self.fail_positive_joint = False
+
+    def move_pose(self, goal, planner='ptp', speed=0.2, label=''):
+        self.moves.append((label, planner, speed, goal))
+        if any(label.startswith(prefix) for prefix in self.fail_labels):
+            return MotionResult(False, 'Планирование не удалось: injected failure', label)
+        return MotionResult(True, 'ok', label, True, goal, goal)
+
+    def move_named(self, name, speed=0.2):
+        self.named.append((name, speed))
+        return MotionResult(True, 'ok', f'move_named({name})')
+
+    def joint_positions(self):
+        return self.joint_values
+
+    def move_joints(self, joints, speed=0.2, label=''):
+        self.joint_moves.append((label, tuple(joints)))
+        if self.fail_positive_joint and joints[6] > 0:
+            return MotionResult(False, 'injected joint failure', label)
+        self.joint_values = tuple(joints)
+        return MotionResult(True, 'ok', label)
+
+    def reset_stop(self):
+        return None
+
+    def wait_stationary(self, *args, **kwargs):
+        return True
+
+
+def make_operations(experiment_id):
+    spec = load_scenario(experiment_id, SCENES)
+    geometry = load_geometry(GEOMETRY)
+    supervisor = FakeSupervisor(spec, geometry)
+    bridge = FakeBridge()
+    motion = FakeMotion()
+    operations = ReferenceOperations(
+        node=None,
+        spec=spec,
+        geometry=geometry,
+        supervisor=supervisor,
+        bridge=bridge,
+        motion=motion,
+    )
+    return operations, supervisor, bridge, motion, geometry
+
+
+def labels(motion):
+    return [label for label, _, _, _ in motion.moves]
+
+
+def test_take_tool_follows_the_visible_axial_route():
+    operations, supervisor, bridge, motion, geometry = make_operations(1)
+    tip = world_target(operations.spec, geometry, 'tool:vacuum')
+    expected = tip.compose_relaxed(geometry.tool_tcp_to_nodes['vacuum'].inverse())
+    result = operations.take_tool('vacuum')
+    assert result.success
+    assert labels(motion) == [
+        'take_tool(vacuum):transfer',
+        'take_tool(vacuum):approach',
+        'take_tool(vacuum):contact',
+        'take_tool(vacuum):lift',
+        'take_tool(vacuum):exit',
+    ]
+    assert supervisor.commands[0][:2] == ('attach_tool', 'vacuum')
+    assert bridge.attached == ['grant_vacuum']
+    contact_goal = motion.moves[2][3]
+    assert contact_goal.xyz == pytest.approx(expected.xyz, abs=1e-9)
+    assert motion.moves[0][1] == 'ptp'
+    assert motion.moves[1][1] == 'ptp'
+    assert motion.moves[2][1] == 'lin'
+
+
+def test_dock_tool_returns_the_tool_to_its_socket():
+    operations, supervisor, bridge, motion, geometry = make_operations(1)
+    result = operations.dock_tool('vacuum')
+    assert result.success
+    assert supervisor.commands[0][:2] == ('dock_tool', 'vacuum')
+    assert bridge.restored == ['grant_vacuum']
+    assert labels(motion)[-1] == 'dock_tool(vacuum):exit'
+
+
+def test_cap_pick_and_place_use_the_hatch_node_transform():
+    operations, supervisor, bridge, motion, geometry = make_operations(1)
+    assert operations.pick_cap().success
+    assert bridge.attached == ['grant_cap']
+    assert supervisor.commands[0][:2] == ('grab_cap', 'cap')
+    assert operations.place_cap().success
+    assert bridge.added_world[0][0] == 'grant_cap'
+    assert supervisor.commands[1][0] == 'place_cap'
+
+
+def test_single_sealant_side_lowers_and_lifts():
+    operations, supervisor, bridge, motion, geometry = make_operations(6)
+    assert operations.dispense('side_1').success
+    assert labels(motion) == [
+        'dispense(side_1):transfer',
+        'dispense(side_1):approach',
+        'dispense(side_1):contact',
+        'dispense(side_1):path',
+        'dispense(side_1):retreat',
+        'dispense(side_1):clear',
+    ]
+    assert operations.completed_sealant == {'side_1'}
+    assert operations.inspect_sealant()['ok'] is True
+
+
+def test_contour_does_not_lift_between_adjacent_sides():
+    operations, supervisor, bridge, motion, geometry = make_operations(8)
+    assert operations.dispense_contour(
+        ['side_1', 'side_2', 'side_3', 'side_4']
+    ).success
+    names = labels(motion)
+    assert names[:3] == [
+        'dispense_contour:start:transfer',
+        'dispense_contour:start:approach',
+        'dispense_contour:start:contact',
+    ]
+    assert names[3:7] == [
+        'dispense_contour:side_1',
+        'dispense_contour:side_2',
+        'dispense_contour:side_3',
+        'dispense_contour:side_4',
+    ]
+    assert 'dispense_contour:side_1' in names
+    assert names[-2:] == [
+        'dispense_contour:retreat', 'dispense_contour:clear',
+    ]
+    assert operations.completed_sealant == {
+        'side_1', 'side_2', 'side_3', 'side_4',
+    }
+
+
+def test_screw_pick_and_handoff_are_one_motion_and_one_removal():
+    operations, supervisor, bridge, motion, geometry = make_operations(11)
+    assert operations.pick_screw('screw_1').success
+    assert bridge.attached == ['grant_screw_01']
+    assert supervisor.commands[0][:2] == ('grab_screw', 'screw_1')
+    assert operations.handoff_screw('screw_1', 'hole_1').success
+    assert supervisor.commands[1][0] == 'handoff_screw'
+    assert supervisor.commands[1][2]['target_name'] == 'hole_1'
+    assert bridge.removed == ['grant_screw_01', 'grant_screw_01']
+    assert bridge.attached == []
+
+
+def test_tighten_hole_is_a_visible_revolution_without_a_screw():
+    operations, supervisor, bridge, motion, geometry = make_operations(14)
+    assert operations.tighten_hole('hole_1').success
+    names = labels(motion)
+    assert names[:3] == [
+        'tighten_hole(hole_1):transfer',
+        'tighten_hole(hole_1):approach',
+        'tighten_hole(hole_1):contact',
+    ]
+    assert motion.joint_moves
+    label, joints = motion.joint_moves[0]
+    assert label == 'tighten_hole(hole_1):joint_twist'
+    assert joints[:6] == (0.0,) * 6
+    assert joints[6] == pytest.approx(math.radians(45.0))
+    # The flange is untwisted before the axial retreat, so joint 7 does not
+    # accumulate twist across holes.
+    untwist_label, untwist_joints = motion.joint_moves[1]
+    assert untwist_label == 'tighten_hole(hole_1):joint_untwist'
+    assert untwist_joints == (0.0,) * 7
+    assert names[-2:] == [
+        'tighten_hole(hole_1):retreat', 'tighten_hole(hole_1):clear',
+    ]
+    # The operation is a simulation only: no supervisor command changes state.
+    assert supervisor.commands == []
+    assert bridge.attached == []
+
+
+def test_screw_pick_reconfigures_through_work_when_a_cell_is_blocked():
+    operations, supervisor, bridge, motion, geometry = make_operations(11)
+    motion.fail_labels.add('pick_screw(screw_1):transfer')
+    assert operations.pick_screw('screw_1').success
+    assert motion.named[-1][0] == 'work'
+    assert 'pick_screw(screw_1):recovery:transfer' in labels(motion)
+    assert supervisor.commands[0][:2] == ('grab_screw', 'screw_1')
+
+
+def test_screw_pick_stops_instead_of_dragging_when_the_lift_is_rejected():
+    operations, supervisor, bridge, motion, geometry = make_operations(11)
+    motion.fail_labels.add('pick_screw(screw_1):retreat')
+    with pytest.raises(OperationError):
+        operations.pick_screw('screw_1')
+    assert operations.events[-1]['phase'] == 'failed'
+
+
+def test_handoff_screw_lifts_axially_before_the_next_move():
+    operations, supervisor, bridge, motion, geometry = make_operations(11)
+    assert operations.pick_screw('screw_1').success
+    motion.moves.clear()
+    assert operations.handoff_screw('screw_1', 'hole_1').success
+    names = labels(motion)
+    assert names[-2:] == [
+        'handoff_screw(screw_1,hole_1):retreat',
+        'handoff_screw(screw_1,hole_1):clear',
+    ]
+
+
+def test_tighten_hole_reverses_when_the_flange_limit_blocks_one_direction():
+    operations, supervisor, bridge, motion, geometry = make_operations(14)
+    motion.fail_positive_joint = True
+    assert operations.tighten_hole('hole_1').success
+    assert len(motion.joint_moves) == 3
+    assert motion.joint_moves[1][1][6] == pytest.approx(math.radians(-45.0))
+    # The untwist returns the flange to the contact configuration.
+    assert motion.joint_moves[2][0] == 'tighten_hole(hole_1):joint_untwist'
+    assert motion.joint_moves[2][1][6] == pytest.approx(0.0)
+
+
+def test_failed_motion_stops_the_operation_and_emits_failure():
+    operations, supervisor, bridge, motion, geometry = make_operations(1)
+    motion.fail_labels.update({
+        'take_tool(vacuum):contact',
+        'take_tool(vacuum):recovery:contact',
+    })
+    with pytest.raises(OperationError):
+        operations.take_tool('vacuum')
+    assert operations.events[-1]['phase'] == 'failed'
+    assert bridge.attached == []
+
+
+def test_failed_supervisor_command_is_not_ignored():
+    operations, supervisor, bridge, motion, geometry = make_operations(1)
+    supervisor.fail_on = 'attach_tool'
+    with pytest.raises(OperationError):
+        operations.take_tool('vacuum')
+    assert bridge.attached == []

src/iiwa_experement/test/test_package_layout.py

@@ -0,0 +1,66 @@
+"""The package installs the files that the launch entry point needs."""
+from pathlib import Path
+
+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 (
+        'rclpy',
+        'ament_index_python',
+        'webots_ros2_msgs',
+        'std_srvs',
+        'python3-yaml',
+        'iiwa_description',
+        'iiwa_bringup',
+        'iiwa_config',
+        'launch',
+        'launch_ros',
+    ):
+        assert f'<exec_depend>{dependency}</exec_depend>' in manifest
+
+
+def test_package_documentation_points_at_the_reference_specification():
+    root = PACKAGE.parents[1]
+    reference = (
+        root
+        / 'docs'
+        / 'superpowers'
+        / 'specs'
+        / '2026-11-08-grant-reference-control-design.md'
+    )
+    assert reference.is_file()
+    for relative in ('doc/experements.md', 'doc/experiment-scenes.md'):
+        text = (PACKAGE / relative).read_text(encoding='utf-8')
+        assert '2026-11-08-grant-reference-control-design.md' in text
+        assert 'tighten' not in text.lower() or 'не затяг' in text.lower()
+        assert 'seat_screw' not in text
+
+
+def test_expected_resources_are_installed():
+    share = Path(get_package_share_directory('iiwa_experement'))
+    for relative in (
+        'package.xml',
+        '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
+        assert path.is_file(), path
+        assert path.stat().st_size > 0, path

src/iiwa_experement/test/test_planning_scene.py

@@ -0,0 +1,228 @@
+"""The MoveIt scene must mirror Webots and keep attachments exclusive."""
+from pathlib import Path
+
+import numpy as np
+import pytest
+from moveit_msgs.msg import CollisionObject, PlanningScene
+
+from iiwa_experement.geometry import Box, Transform, load_geometry, world_target
+from iiwa_experement.planning_scene import (
+    PlanningSceneBridge,
+    SceneObject,
+    attached_cap,
+    build_collision_matrix,
+    load_srdf_disabled_pairs,
+    attached_screw,
+    attached_tool,
+    build_world_objects,
+    collision_object,
+    placed_cap_pose,
+    scene_ids,
+    update_contact_entry,
+)
+from iiwa_experement.scenario import load_scenario
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+SCENES = PACKAGE / 'config' / 'scenes.yaml'
+GEOMETRY = PACKAGE / 'config' / 'geometry.yaml'
+
+
+def spec(experiment_id):
+    return load_scenario(experiment_id, SCENES)
+
+
+class FakeBridge(PlanningSceneBridge):
+    def __init__(self, experiment_id=11):
+        geometry = load_geometry(GEOMETRY)
+        scenario = spec(experiment_id)
+        self.geometry = geometry
+        self.spec = scenario
+        self.base_to_world = geometry.world_to_base.inverse()
+        self._world_objects = {
+            obj.object_id: obj for obj in build_world_objects(scenario, geometry)
+        }
+        self._initial_world = dict(self._world_objects)
+        self._attached = {}
+        self._disabled_pairs = []
+        self._acm_names = []
+        self._acm_values = []
+        self._latest_joint_state = None
+        self.node = None
+        self.last_scene = PlanningScene()
+        self.published = []
+
+    def _publish(self, scene):
+        self.published.append(scene)
+
+    def assert_synchronized(self, *args, **kwargs):
+        return None
+
+
+def test_initial_scene_contains_every_experiment_object():
+    objects = build_world_objects(spec(11), load_geometry(GEOMETRY))
+    ids = {obj.object_id for obj in objects}
+    assert {'grant_table', 'grant_fixture', 'grant_fixture_rail_a',
+            'grant_fixture_rail_b', 'grant_body', 'grant_rack',
+            'grant_tray', 'grant_cap', 'grant_magnetic_tool'} <= ids
+    assert {f'grant_screw_{index:02d}' for index in range(1, 11)} <= ids
+    assert 'grant_vacuum' not in ids
+    # table, fixture base, two fixture rails, body, rack, tray base, four
+    # tray walls, ten screws, installed hatch and one magnetic tool.
+    assert len(ids) == 23
+
+
+def test_experiment_nineteen_moves_tray_and_screws():
+    standard = {obj.object_id: obj for obj in build_world_objects(
+        spec(11), load_geometry(GEOMETRY))}
+    shifted = {obj.object_id: obj for obj in build_world_objects(
+        spec(19), load_geometry(GEOMETRY))}
+    for index in range(1, 11):
+        name = f'grant_screw_{index:02d}'
+        assert shifted[name].pose.position == pytest.approx(
+            standard[name].pose.position + np.array([-0.02, 0.02, 0.0]),
+            abs=1e-9,
+        )
+
+
+def test_collision_boxes_match_measured_meshes():
+    objects = {obj.object_id: obj for obj in build_world_objects(
+        spec(16), load_geometry(GEOMETRY))}
+    assert objects['grant_body'].box.size == pytest.approx((0.30, 0.17, 0.00915))
+    assert objects['grant_cap'].box.size == pytest.approx((0.1384, 0.0984, 0.00267))
+    assert objects['grant_screw_01'].box.size == pytest.approx((0.0055, 0.0055, 0.009))
+    assert objects['grant_vacuum'].box.size[2] == pytest.approx(0.140)
+
+
+def test_collision_object_is_in_the_robot_model_frame():
+    pose = Transform.from_xyz_rpy([0.0, 0.0, 0.0], [0, 0, 0], parent='world', child='x')
+    message = collision_object('obj', Box((0.1, 0.2, 0.3)), pose)
+    assert message.header.frame_id == 'base_link'
+    assert message.id == 'obj'
+    assert list(message.primitives[0].dimensions) == pytest.approx([0.1, 0.2, 0.3])
+
+
+def test_attach_removes_the_world_object_and_adds_one_attachment():
+    bridge = FakeBridge(16)
+    tool = attached_tool('vacuum', bridge.geometry)
+    bridge.attach(tool)
+    assert tool.object_id not in bridge._world_objects
+    assert bridge._attached[tool.object_id] is tool
+    scene = bridge.published[-1]
+    assert scene.is_diff is False
+    assert tool.object_id not in [obj.id for obj in scene.world.collision_objects]
+    assert len(scene.robot_state.attached_collision_objects) == 1
+    assert scene.robot_state.attached_collision_objects[0].object.id == tool.object_id
+    assert scene.robot_state.attached_collision_objects[0].link_name == 'patron'
+    assert 'tcp' in scene.robot_state.attached_collision_objects[0].touch_links
+
+
+def test_detach_reverses_the_attachment():
+    bridge = FakeBridge(16)
+    tool = attached_tool('vacuum', bridge.geometry)
+    bridge.attach(tool)
+    bridge.detach(tool.object_id, bridge.geometry.world_to_base.compose_relaxed(
+        Transform.from_xyz_rpy([0, 0, 0], [0, 0, 0], parent='world', child='x')
+    ))
+    assert tool.object_id in bridge._world_objects
+    assert tool.object_id not in bridge._attached
+    scene = bridge.published[-1]
+    assert not scene.robot_state.attached_collision_objects
+    assert tool.object_id in [obj.id for obj in scene.world.collision_objects]
+
+
+def test_handoff_removes_only_the_delivered_screw():
+    bridge = FakeBridge(11)
+    screw = attached_screw('screw_1', bridge.geometry)
+    bridge.attach(screw)
+    assert 'grant_screw_02' in bridge._world_objects
+    bridge.remove(screw.object_id)
+    assert screw.object_id not in bridge._attached
+    assert screw.object_id not in bridge._world_objects
+    assert 'grant_screw_02' in bridge._world_objects
+
+
+def test_cap_and_screw_attachment_geometry_uses_tcp_offsets():
+    geometry = load_geometry(GEOMETRY)
+    cap = attached_cap(geometry)
+    screw = attached_screw('screw_3', geometry)
+    # The boxes stop 1 mm short of the support surface at the grasp pose.
+    assert cap.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.141335 + 0.0552))
+    assert cap.box.size == pytest.approx((0.1384, 0.0984, 0.00267))
+    assert screw.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.144 + 0.0552))
+    assert screw.box.size == pytest.approx((0.0055, 0.0055, 0.008))
+
+
+def test_contact_permission_changes_exactly_one_pair():
+    names = ['link1', 'link2', 'tool']
+    values = [[True, False, False], [False, True, False], [False, False, True]]
+    new_names, matrix = update_contact_entry(names, values, 'tool', 'link2', True)
+    assert new_names == names
+    assert matrix[2][1] is True and matrix[1][2] is True
+    assert matrix[2][0] is False and matrix[0][2] is False
+    assert matrix[0][1] is False
+
+
+def test_restore_world_returns_the_calibrated_box_pose():
+    bridge = FakeBridge(16)
+    tool = attached_tool('vacuum', bridge.geometry)
+    bridge.attach(tool)
+    bridge.restore_world(tool.object_id)
+    assert tool.object_id not in bridge._attached
+    restored = bridge._world_objects[tool.object_id]
+    expected = {
+        obj.object_id: obj
+        for obj in build_world_objects(bridge.spec, bridge.geometry)
+    }[tool.object_id]
+    assert restored.pose.xyz == pytest.approx(expected.pose.xyz)
+    assert restored.pose.rotation == expected.pose.rotation
+    with pytest.raises(Exception):
+        bridge.restore_world('grant_unknown')
+
+
+def test_placed_cap_box_is_flat_and_matches_the_preinstalled_cap():
+    geometry = load_geometry(GEOMETRY)
+    scenario = spec(1)
+    seat = world_target(scenario, geometry, 'cap_seat')
+    placed = placed_cap_pose(seat, scenario.body.yaw_deg)
+    # The box is flat (yaw only) and centred on the plate, like the
+    # pre-installed hatch of experiments 11-15.
+    preinstalled = {
+        obj.object_id: obj
+        for obj in build_world_objects(spec(11), geometry)
+    }['grant_cap']
+    assert placed.xyz == pytest.approx(preinstalled.pose.xyz)
+    assert np.asarray(placed.rotation) == pytest.approx(
+        np.asarray(preinstalled.pose.rotation)
+    )
+
+
+def test_srdf_collision_matrix_allows_only_disabled_pairs():
+    root = Path(__file__).resolve().parents[2]
+    srdf = root / 'iiwa_config' / 'config' / 'moveit' / 'iiwa7.srdf'
+    pairs = load_srdf_disabled_pairs(srdf)
+    assert ('link1', 'link2') in pairs
+    names, matrix = build_collision_matrix(
+        ['grant_body', 'grant_tool'], disabled_pairs=pairs,
+    )
+    assert 'base_link' in names and 'grant_body' in names
+    def allowed(first, second):
+        return matrix[names.index(first)][names.index(second)]
+    assert allowed('link1', 'link2') is True
+    assert allowed('base_link', 'link1') is True
+    assert allowed('link1', 'grant_body') is False
+    assert allowed('grant_body', 'grant_tool') is False
+    assert allowed('grant_body', 'grant_body') is True
+
+
+def test_scene_ids_reports_world_and_attached_sets():
+    scene = PlanningScene()
+    scene.world.collision_objects.append(CollisionObject(id='world_a'))
+    from moveit_msgs.msg import AttachedCollisionObject
+
+    attached = AttachedCollisionObject()
+    attached.object.id = 'attached_a'
+    scene.robot_state.attached_collision_objects.append(attached)
+    world, held = scene_ids(scene)
+    assert world == {'world_a'}
+    assert held == {'attached_a'}

src/iiwa_experement/test/test_reference_runner.py

@@ -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'

src/iiwa_experement/test/test_reference_scenarios.py

@@ -0,0 +1,304 @@
+"""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_routes_start_at_work_and_only_end_at_home():
+    for experiment_id in range(1, 21):
+        steps = route(experiment_id)
+        assert steps[0].kind == 'move_named'
+        assert steps[0].args == ('work',)
+        assert steps[-2].kind == 'move_named'
+        assert steps[-2].args == ('work',)
+        assert steps[-1].kind == 'move_named'
+        assert steps[-1].args == ('home',)
+        homes = [
+            index for index, step in enumerate(steps)
+            if step.kind == 'move_named' and step.args == ('home',)
+        ]
+        assert homes == [len(steps) - 1]
+        # The 'transport' pose is forbidden in the reference routes.
+        assert not any(
+            step.kind == 'move_named' and step.args == ('transport',)
+            for step in steps
+        )
+
+
+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', 'torque_tool']
+
+
+def test_tightening_simulation_matches_the_delivered_holes():
+    assert not any(
+        step.kind == 'tighten_hole' for step in route(11)
+    )
+    for experiment_id in range(14, 21):
+        tighten = [
+            step for step in route(experiment_id) if step.kind == 'tighten_hole'
+        ]
+        handoffs = [
+            step for step in route(experiment_id) if step.kind == 'handoff_screw'
+        ]
+        assert [step.args[0] for step in tighten] == [
+            step.args[1] for step in handoffs
+        ]
+    for experiment_id in (12, 13):
+        assert not any(
+            step.kind == 'tighten_hole' for step in route(experiment_id)
+        )
+
+
+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)

src/iiwa_experement/test/test_runtime_config.py

@@ -0,0 +1,161 @@
+"""Temporary runtime files point at installed assets and keep patron."""
+from pathlib import Path
+
+import pytest
+import yaml
+from ament_index_python.packages import get_package_share_directory
+
+from iiwa_experement.runtime_config import (
+    LOCAL_PROTOS,
+    prepare_runtime_files,
+    validate_mode,
+    validate_start_delay,
+    validate_trace_dir,
+)
+
+
+@pytest.mark.parametrize('raw, expected', [
+    ('view', 'view'),
+    (' Execute ', 'execute'),
+    ('planner', 'planner'),
+    (' PLANNER ', 'planner'),
+])
+def test_mode_validation(raw, expected):
+    assert validate_mode(raw) == expected
+
+
+@pytest.mark.parametrize('raw', ['', 'motion', 'execute!', 'plan'])
+def test_mode_validation_rejects_unknown_modes(raw):
+    with pytest.raises(ValueError):
+        validate_mode(raw)
+
+
+@pytest.mark.parametrize('raw, expected', [('0', 0.0), (' 3.0 ', 3.0)])
+def test_start_delay_validation(raw, expected):
+    assert validate_start_delay(raw) == expected
+
+
+@pytest.mark.parametrize('raw', ['', '-0.1', 'no', 'nan', 'inf'])
+def test_start_delay_validation_rejects_bad_values(raw):
+    with pytest.raises(ValueError):
+        validate_start_delay(raw)
+
+
+def test_trace_dir_validation():
+    assert validate_trace_dir('~/.ros/iiwa_experement/runs') == (
+        '~/.ros/iiwa_experement/runs'
+    )
+    with pytest.raises(ValueError):
+        validate_trace_dir('')
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+CONFIG_SHARE = Path(get_package_share_directory('iiwa_config'))
+DESCRIPTION_SHARE = Path(get_package_share_directory('iiwa_description'))
+PACKAGE_SHARE = Path(get_package_share_directory('iiwa_experement'))
+BASE_SETTING = CONFIG_SHARE / 'config' / 'setting.yaml'
+WORLD_TEMPLATE = PACKAGE_SHARE / 'worlds' / 'grant' / 'iiwa-grant.wbt'
+
+
+def prepare(tmp_path, **overrides):
+    arguments = {
+        'base_setting': BASE_SETTING,
+        'world_template': WORLD_TEMPLATE,
+        'description_share': DESCRIPTION_SHARE,
+        'output_dir': tmp_path / 'run',
+    }
+    arguments.update(overrides)
+    return prepare_runtime_files(**arguments)
+
+
+def test_world_copy_resolves_only_local_protos(tmp_path):
+    world_path, _ = prepare(tmp_path)
+    text = world_path.read_text(encoding='utf-8')
+    for name in LOCAL_PROTOS:
+        absolute = (DESCRIPTION_SHARE / 'resource' / 'protos' / name).resolve()
+        assert absolute.is_file()
+        assert f'EXTERNPROTO "{absolute}"' in text
+    assert 'EXTERNPROTO "../../../iiwa_description' not in text
+    assert text.count('https://raw.githubusercontent.com/cyberbotics/webots/R2025a/') == 3
+    assert world_path == tmp_path / 'run' / 'iiwa-grant.wbt'
+
+
+def test_setting_copy_disables_random_objects_and_keeps_patron_and_camera(tmp_path):
+    world_path, setting_path = prepare(tmp_path)
+    data = yaml.safe_load(setting_path.read_text(encoding='utf-8'))
+    webots = data['digital_twin']['webots']
+    assert webots['world'] == str(world_path.resolve())
+    assert webots['objects']['enabled'] is False
+    assert webots['objects']['count'] == 0
+    assert data['tool']['active'] == 'patron'
+    assert webots['transform'] == '-0.25 0 0.79'
+    assert webots['rotation'] == '0 0 1 0'
+    assert webots['controller_timer'] == '50'
+    assert any(
+        str(camera).endswith('config/cameras/d455_top.yaml')
+        for camera in webots['cameras']
+    )
+    assert data['controller']['controller_path'].endswith('moveit/iiwa_controller.yaml')
+    assert data['digital_twin']['rviz']['config'].endswith('rviz_moveit.rviz')
+
+
+def test_prepared_setting_is_accepted_by_the_existing_loader(tmp_path):
+    _, setting_path = prepare(tmp_path)
+    from iiwa_utils.setting_loader import build_settings
+
+    settings = build_settings(str(setting_path), check_files=True)
+    assert settings.tool.active == 'patron'
+    assert settings.digital_twin.webots.objects.enabled is False
+    assert settings.digital_twin.webots.objects.count == 0
+    assert settings.digital_twin.webots.cameras
+    assert settings.digital_twin.webots.cameras[0].endswith('d455_top.yaml')
+
+
+def test_tool_active_xacro_and_generated_urdf_include_patron(tmp_path):
+    from iiwa_utils import converter
+    from iiwa_utils.setting_loader import build_settings
+
+    tool_active = DESCRIPTION_SHARE / 'urdf' / 'tools' / 'tool_active.xacro'
+    assert 'patron.xacro' in tool_active.read_text(encoding='utf-8')
+    _, setting_path = prepare(tmp_path)
+    settings = build_settings(str(setting_path), check_files=True)
+    urdf = converter.load_robot_description(
+        settings.robot.description,
+        settings.robot.name,
+        {
+            'simulate': 'true',
+            'initial_positions_file': settings.controller.moveit.initial_positions,
+            'joint_limits_file': settings.controller.moveit.joint_limits,
+        },
+    )
+    assert 'name="patron"' in urdf
+    assert 'link name="patron"' in urdf
+    assert 'patron.dae' in urdf
+
+
+def test_missing_world_template_is_reported(tmp_path):
+    with pytest.raises(FileNotFoundError, match='world template'):
+        prepare(tmp_path, world_template=tmp_path / 'missing.wbt')
+
+
+def test_missing_proto_is_reported(tmp_path):
+    empty_description = tmp_path / 'description'
+    (empty_description / 'resource' / 'protos').mkdir(parents=True)
+    with pytest.raises(FileNotFoundError, match='CollaborativeRobotTable.proto'):
+        prepare(tmp_path, description_share=empty_description)
+
+
+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'
+    )

src/iiwa_experement/test/test_scenario.py

@@ -0,0 +1,347 @@
+"""The twenty experiments have exactly the composition from the specification."""
+import math
+from pathlib import Path
+
+import pytest
+import yaml
+from ament_index_python.packages import get_package_share_directory
+
+from iiwa_experement.scenario import (
+    CAP_STATES,
+    TOOL_NAMES,
+    ScenarioError,
+    load_scenario,
+)
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+SCENES = PACKAGE / 'config' / 'scenes.yaml'
+
+STANDARD_TOOLS = set(TOOL_NAMES)
+STANDARD_SCREWS = {
+    'screw_1': (0.160, 0.445),
+    'screw_2': (0.240, 0.445),
+    'screw_3': (0.160, 0.475),
+    'screw_4': (0.240, 0.475),
+    'screw_5': (0.180, 0.445),
+    'screw_6': (0.200, 0.445),
+    'screw_7': (0.220, 0.445),
+    'screw_8': (0.180, 0.475),
+    'screw_9': (0.200, 0.475),
+    'screw_10': (0.220, 0.475),
+}
+
+
+def scenario(experiment_id):
+    return load_scenario(experiment_id, SCENES)
+
+
+def variant(tmp_path, mutate):
+    data = yaml.safe_load(SCENES.read_text(encoding='utf-8'))
+    mutate(data)
+    path = tmp_path / 'scenes.yaml'
+    path.write_text(yaml.safe_dump(data, sort_keys=False), encoding='utf-8')
+    return path
+
+
+def test_all_twenty_experiments_load():
+    for experiment_id in range(1, 21):
+        spec = scenario(experiment_id)
+        assert spec.experiment_id == experiment_id
+        assert spec.cap_state in CAP_STATES
+        assert spec.tool_names
+        if experiment_id != 3:
+            assert (spec.fixture.x, spec.fixture.y) == (0.30, 0.12)
+            assert (spec.body.x, spec.body.y) == (0.30, 0.12)
+        assert (spec.rack.x, spec.rack.y, spec.rack.z) == (0.12, -0.42, 0.793)
+
+
+def test_cap_only_experiments_one_to_five():
+    for experiment_id in range(1, 6):
+        spec = scenario(experiment_id)
+        assert spec.cap_state == 'free'
+        assert spec.tray_pose is None
+        assert spec.screws == ()
+        assert spec.tool_names == ('vacuum',)
+
+
+def test_sealant_experiments_do_not_start_with_a_hatch_or_tray():
+    for experiment_id in range(6, 10):
+        spec = scenario(experiment_id)
+        assert spec.cap_state == 'absent'
+        assert spec.cap_pose is None
+        assert spec.tray_pose is None
+        assert spec.screws == ()
+        assert spec.tool_names == ('sealant_tool',)
+
+
+def test_experiment_ten_has_hatch_and_both_tools():
+    spec = scenario(10)
+    assert spec.cap_state == 'free'
+    assert spec.tray_pose is None
+    assert set(spec.tool_names) == {'sealant_tool', 'vacuum'}
+
+
+def test_screw_experiments_have_installed_hatch_and_ten_screws():
+    for experiment_id in range(11, 16):
+        spec = scenario(experiment_id)
+        assert spec.cap_state == 'installed'
+        assert spec.tray_pose is not None
+        assert len(spec.screws) == 10
+        if experiment_id in (14, 15):
+            assert set(spec.tool_names) == {'magnetic_tool', 'torque_tool'}
+        else:
+            assert spec.tool_names == ('magnetic_tool',)
+        assert len(spec.screw_holes) == {
+            11: 1, 12: 2, 13: 3, 14: 4, 15: 4,
+        }[experiment_id]
+
+
+def test_full_assembly_experiments_have_loose_hatch_tray_and_all_tools():
+    for experiment_id in range(16, 21):
+        spec = scenario(experiment_id)
+        assert spec.cap_state == 'free'
+        assert spec.tray_pose is not None
+        assert len(spec.screws) == 10
+        assert set(spec.tool_names) == {
+            'vacuum', 'magnetic_tool', 'sealant_tool', 'torque_tool',
+        }
+        assert len(spec.screw_holes) == {16: 1, 17: 2, 18: 4, 19: 4, 20: 4}[experiment_id]
+
+
+def test_sealant_coverage_matches_each_experiment():
+    assert scenario(6).sealant_sides == ('side_1',)
+    assert scenario(7).sealant_sides == ('side_1', 'side_2')
+    for experiment_id in (8, 9, 10, 17, 18, 19, 20):
+        assert scenario(experiment_id).sealant_sides == (
+            'side_1', 'side_2', 'side_3', 'side_4',
+        )
+    assert scenario(16).sealant_sides == ('side_1',)
+    for experiment_id in (1, 2, 3, 4, 5, 11, 12, 13, 14, 15):
+        assert scenario(experiment_id).sealant_sides == ()
+
+
+def test_screw_holes_are_unique_bounded_and_ordered():
+    for experiment_id in range(11, 21):
+        holes = scenario(experiment_id).screw_holes
+        assert len(holes) == len(set(holes))
+        assert set(holes) <= {'hole_1', 'hole_2', 'hole_3', 'hole_4'}
+    assert scenario(14).screw_holes == ('hole_1', 'hole_3', 'hole_2', 'hole_4')
+    assert scenario(15).screw_holes == ('hole_1', 'hole_3', 'hole_2', 'hole_4')
+
+
+def test_reference_experiments_never_request_tightening():
+    for experiment_id in range(1, 21):
+        spec = scenario(experiment_id)
+        assert all(
+            failure.skill not in ('tighten', 'seat_screw')
+            for failure in spec.failures
+        )
+
+
+def test_standard_screw_layout_matches_the_specification():
+    spec = scenario(11)
+    assert len({screw.name for screw in spec.screws}) == 10
+    assert len({screw.pose.xy for screw in spec.screws}) == 10
+    for screw in spec.screws:
+        assert screw.pose.xy == pytest.approx(STANDARD_SCREWS[screw.name], abs=1e-9)
+        assert screw.pose.z == pytest.approx(0.7954, abs=1e-12)
+
+
+def test_experiment_two_hatch_is_rotated_135_degrees():
+    spec = scenario(2)
+    assert spec.cap_pose is not None
+    assert spec.cap_pose.xy == (0.35, -0.22)
+    assert spec.cap_pose.yaw_deg == 135
+
+
+def test_experiment_three_moves_fixture_body_and_hatch_together():
+    spec = scenario(3)
+    assert (spec.fixture.x, spec.fixture.y, spec.fixture.z) == (0.29, 0.14, 0.803)
+    assert (spec.body.x, spec.body.y, spec.body.z) == (0.29, 0.14, 0.823)
+    assert spec.cap_pose is not None
+    assert (spec.cap_pose.x, spec.cap_pose.y) == (0.33, -0.22)
+
+
+def test_experiment_five_carries_the_misplaced_verification_metadata():
+    spec = scenario(5)
+    assert spec.cap_pose is not None
+    assert spec.cap_pose.xy == (0.35, -0.22)
+    assert len(spec.failures) == 1
+    failure = spec.failures[0]
+    assert (failure.skill, failure.outcome, failure.target, failure.attempt) == (
+        'verify', 'not_ok', 'cap', 1,
+    )
+    assert dict(failure.details)['after_first_install_x'] == 0.303
+
+
+def test_experiment_nineteen_moves_hatch_and_tray_with_all_screws():
+    spec = scenario(19)
+    assert spec.cap_pose is not None
+    assert spec.cap_pose.xy == (0.37, -0.20)
+    assert spec.cap_pose.yaw_deg == 60
+    assert spec.tray_pose is not None
+    assert (spec.tray_pose.x, spec.tray_pose.y, spec.tray_pose.z) == (0.18, 0.48, 0.793)
+    for screw in spec.screws:
+        expected = STANDARD_SCREWS[screw.name]
+        assert screw.pose.xy == pytest.approx(
+            (expected[0] - 0.02, expected[1] + 0.02), abs=1e-9
+        )
+
+
+def test_failure_metadata_for_experiments_four_nine_fifteen_twenty():
+    assert [(f.skill, f.outcome) for f in scenario(4).failures] == [
+        ('grab', 'grasp_failed'),
+    ]
+    assert [(f.skill, f.outcome, f.target) for f in scenario(9).failures] == [
+        ('dispense', 'interrupted', 'side_3'),
+    ]
+    fifteen = [(f.skill, f.outcome, f.target) for f in scenario(15).failures]
+    assert fifteen == [
+        ('grab', 'grasp_failed', 'third_selected_screw'),
+    ]
+    twenty = [(f.skill, f.outcome, f.target) for f in scenario(20).failures]
+    assert twenty == [
+        ('dispense', 'interrupted', 'side_3'),
+        ('grab', 'grasp_failed', 'first_selected_screw'),
+        ('verify', 'not_ok', 'cap'),
+    ]
+
+
+def test_installed_cap_sits_on_the_body():
+    for experiment_id in range(11, 16):
+        spec = scenario(experiment_id)
+        assert spec.cap_pose is not None
+        assert spec.cap_pose.xy == spec.body.xy
+        assert spec.cap_pose.yaw_deg == spec.body.yaw_deg
+
+
+def test_scenarios_are_read_from_the_installed_manifest():
+    installed = (
+        Path(get_package_share_directory('iiwa_experement')) / 'config' / 'scenes.yaml'
+    )
+    for experiment_id in range(1, 21):
+        assert load_scenario(experiment_id, installed).experiment_id == experiment_id
+
+
+@pytest.mark.parametrize('experiment_id', [0, 21, '1', 1.0, True, None])
+def test_invalid_experiment_ids_are_rejected(experiment_id):
+    with pytest.raises(ScenarioError):
+        load_scenario(experiment_id, SCENES)
+
+
+def test_unknown_tool_is_rejected(tmp_path):
+    def mutate(data):
+        data['experiments']['1']['tools'] = ['vacuum_cleaner']
+
+    with pytest.raises(ScenarioError, match='unknown tool'):
+        load_scenario(1, variant(tmp_path, mutate))
+
+
+@pytest.mark.parametrize('count', [9, 11])
+def test_incorrect_screw_count_is_rejected(tmp_path, count):
+    entries = [
+        {'name': f'screw_{index}', 'dx': -0.04 + index * 0.005, 'dy': 0.0}
+        for index in range(1, count + 1)
+    ]
+
+    def mutate(data):
+        data['experiments']['1']['tray'] = True
+        data['experiments']['1']['tools'] = ['magnetic_tool']
+        data['experiments']['1']['screw_holes'] = ['hole_1']
+        data['experiments']['1']['screws'] = entries
+
+    with pytest.raises(ScenarioError, match='exactly 10 screws'):
+        load_scenario(1, variant(tmp_path, mutate))
+
+
+def test_duplicate_screw_name_is_rejected(tmp_path):
+    entries = [
+        {'name': 'screw_1', 'dx': -0.04 + index * 0.005, 'dy': 0.0}
+        for index in range(10)
+    ]
+
+    def mutate(data):
+        data['experiments']['1']['tray'] = True
+        data['experiments']['1']['tools'] = ['magnetic_tool']
+        data['experiments']['1']['screw_holes'] = ['hole_1']
+        data['experiments']['1']['screws'] = entries
+
+    with pytest.raises(ScenarioError, match='duplicate screw name'):
+        load_scenario(1, variant(tmp_path, mutate))
+
+
+def test_screw_outside_the_tray_is_rejected(tmp_path):
+    entries = [
+        {'name': f'screw_{index}', 'dx': -0.04 + index * 0.005, 'dy': 0.0}
+        for index in range(10)
+    ]
+    entries[4]['dx'] = 0.2
+
+    def mutate(data):
+        data['experiments']['1']['tray'] = True
+        data['experiments']['1']['tools'] = ['magnetic_tool']
+        data['experiments']['1']['screw_holes'] = ['hole_1']
+        data['experiments']['1']['screws'] = entries
+
+    with pytest.raises(ScenarioError, match='outside the tray'):
+        load_scenario(1, variant(tmp_path, mutate))
+
+
+def test_fixture_and_body_must_share_xy(tmp_path):
+    def mutate(data):
+        data['experiments']['1']['body'] = {
+            'x': 0.31, 'y': 0.12, 'z': 0.823, 'yaw_deg': 45,
+        }
+
+    with pytest.raises(ScenarioError, match='fixture and body'):
+        load_scenario(1, variant(tmp_path, mutate))
+
+
+def test_tray_without_screws_and_screws_without_tray_are_rejected(tmp_path):
+    def no_screws(data):
+        data['experiments']['1']['screws'] = [
+            {'name': f'screw_{index}', 'dx': 0.0, 'dy': 0.0}
+            for index in range(10)
+        ]
+
+    with pytest.raises(ScenarioError, match='no tray'):
+        load_scenario(1, variant(tmp_path, no_screws))
+
+    def no_tray(data):
+        data['experiments']['1']['tray'] = False
+        data['experiments']['1']['tray_pose'] = {
+            'x': -0.08, 'y': 0.34, 'z': 0.793, 'yaw_deg': 0,
+        }
+
+    with pytest.raises(ScenarioError, match='tray_pose requires tray'):
+        load_scenario(1, variant(tmp_path, no_tray))
+
+
+def test_missing_manifest_is_reported(tmp_path):
+    with pytest.raises(FileNotFoundError):
+        load_scenario(1, tmp_path / 'missing.yaml')
+
+
+def test_invalid_sealant_side_is_rejected(tmp_path):
+    def mutate(data):
+        data['experiments']['6']['sealant'] = ['side_5']
+
+    with pytest.raises(ScenarioError, match='sealant'):
+        load_scenario(6, variant(tmp_path, mutate))
+
+
+def test_invalid_screw_hole_is_rejected(tmp_path):
+    def mutate(data):
+        data['experiments']['11']['screw_holes'] = ['hole_5']
+
+    with pytest.raises(ScenarioError, match='screw_holes'):
+        load_scenario(11, variant(tmp_path, mutate))
+
+
+def test_tray_without_screw_holes_is_rejected(tmp_path):
+    def mutate(data):
+        data['experiments']['11'].pop('screw_holes')
+
+    with pytest.raises(ScenarioError, match='requires screw_holes'):
+        load_scenario(11, variant(tmp_path, mutate))

src/iiwa_experement/test/test_scene_spawner.py

@@ -0,0 +1,172 @@
+"""The spawner sends every node once and only signals readiness at the end."""
+from pathlib import Path
+
+import pytest
+from ament_index_python.packages import get_package_share_directory
+
+from iiwa_experement.scenario import ScenarioError, load_scenario
+from iiwa_experement.scene_spawner import (
+    Readiness,
+    ScenePlan,
+    SpawnError,
+    build_plan,
+    execute_plan,
+    node_name,
+    spawn_nodes,
+)
+from iiwa_experement.webots_nodes import build_nodes
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+SCENES = PACKAGE / 'config' / 'scenes.yaml'
+ASSET_ROOT = (
+    Path(get_package_share_directory('iiwa_description')) / 'objects' / 'grant'
+)
+
+
+class FakeLogger:
+    def __init__(self):
+        self.messages = []
+
+    def info(self, message):
+        self.messages.append(message)
+
+
+class FakeNode:
+    def __init__(self):
+        self.logger = FakeLogger()
+
+    def get_logger(self):
+        return self.logger
+
+
+class FakeFuture:
+    def __init__(self, behavior):
+        self.behavior = behavior
+        self.completed = behavior != 'timeout'
+
+    def done(self):
+        return self.completed
+
+    def result(self):
+        if isinstance(self.behavior, Exception):
+            raise self.behavior
+        if self.behavior == 'reject':
+            return type('Response', (), {'success': False})()
+        return type('Response', (), {'success': True})()
+
+
+class FakeClient:
+    def __init__(self, behaviors=(), available=True):
+        self.behaviors = list(behaviors)
+        self.available = available
+        self.calls = []
+        self.readiness_during_calls = []
+        self.readiness = None
+
+    def wait_for_service(self, timeout_sec):
+        return self.available
+
+    def call_async(self, request):
+        self.calls.append(request.data)
+        if self.readiness is not None:
+            self.readiness_during_calls.append(self.readiness.ready)
+        index = len(self.calls) - 1
+        behavior = self.behaviors[index] if index < len(self.behaviors) else 'ok'
+        return FakeFuture(behavior)
+
+
+def fake_spin(node, future, timeout_sec):
+    return None
+
+
+def run_plan(nodes, client, readiness=None, **kwargs):
+    plan = ScenePlan(
+        experiment_id=1,
+        spec=load_scenario(1, SCENES),
+        nodes=list(nodes),
+        readiness=readiness or Readiness(),
+    )
+    client.readiness = plan.readiness
+    execute_plan(
+        plan,
+        client,
+        FakeNode(),
+        spin_until_future_complete=fake_spin,
+        **kwargs,
+    )
+    return plan
+
+
+def test_successful_spawn_sends_build_nodes_in_order_and_signals_at_the_end():
+    nodes = build_nodes(load_scenario(6, SCENES), ASSET_ROOT)
+    client = FakeClient()
+    plan = run_plan(nodes, client)
+    assert client.calls == nodes
+    assert client.readiness_during_calls == [False] * len(nodes)
+    assert plan.readiness.ready is True
+    assert plan.readiness.message == f'experiment 1: {len(nodes)} objects spawned'
+
+
+def test_node_name_is_used_for_progress_and_errors():
+    assert node_name('Solid {\n  name "grant_cap"\n}') == 'grant_cap'
+    assert node_name('Solid {}') == 'unnamed object'
+
+
+def test_missing_supervisor_fails_without_spawning():
+    client = FakeClient(available=False)
+    with pytest.raises(SpawnError, match='unavailable'):
+        run_plan(['Solid { name "grant_cap" }'], client)
+    assert client.calls == []
+
+
+def test_rejected_object_names_the_last_object_and_keeps_readiness_false():
+    client = FakeClient(['ok', 'reject'])
+    readiness = Readiness()
+    with pytest.raises(SpawnError, match='grant_body'):
+        run_plan(
+            ['Solid { name "grant_fixture" }', 'Solid { name "grant_body" }'],
+            client,
+            readiness,
+        )
+    assert len(client.calls) == 2
+    assert readiness.ready is False
+    assert 'grant_body' in readiness.message
+
+
+def test_exception_names_the_last_object():
+    client = FakeClient(['ok', RuntimeError('service exploded')])
+    with pytest.raises(SpawnError, match='grant_body.*service exploded'):
+        run_plan(
+            ['Solid { name "grant_fixture" }', 'Solid { name "grant_body" }'],
+            client,
+        )
+
+
+def test_response_timeout_is_not_retried():
+    client = FakeClient(['ok', 'timeout', 'ok'])
+    with pytest.raises(SpawnError, match='grant_body.*timed out.*no retry'):
+        run_plan(
+            ['Solid { name "grant_fixture" }', 'Solid { name "grant_body" }'],
+            client,
+        )
+    assert len(client.calls) == 2
+
+
+def test_duplicate_name_rejection_is_not_hidden():
+    client = FakeClient(['reject'])
+    with pytest.raises(SpawnError, match='grant_fixture.*rejected'):
+        run_plan(['Solid { name "grant_fixture" }'], client)
+
+
+def test_build_plan_rejects_an_invalid_id_before_spawning():
+    with pytest.raises(ScenarioError):
+        build_plan(0, SCENES, ASSET_ROOT)
+    with pytest.raises(ScenarioError):
+        build_plan(21, SCENES, ASSET_ROOT)
+
+
+def test_build_plan_uses_the_full_build_nodes_list():
+    plan = build_plan(19, SCENES, ASSET_ROOT)
+    assert plan.nodes == build_nodes(plan.spec, ASSET_ROOT)
+    assert plan.readiness.ready is False

src/iiwa_experement/test/test_webots_nodes.py

@@ -0,0 +1,282 @@
+"""Deterministic Webots nodes derived from each scenario."""
+import math
+import re
+from pathlib import Path
+
+import pytest
+from ament_index_python.packages import get_package_share_directory
+
+from iiwa_experement.scenario import TOOL_NAMES, load_scenario
+from iiwa_experement.webots_nodes import (
+    ASSETS,
+    TOOL_NODE_NAMES,
+    build_nodes,
+    rotation_axis_angle,
+    rotation_matrix,
+)
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+SCENES = PACKAGE / 'config' / 'scenes.yaml'
+ASSET_ROOT = (
+    Path(get_package_share_directory('iiwa_description')) / 'objects' / 'grant'
+)
+
+NAME_RE = re.compile(r'name "([^"]+)"')
+TRANSLATION_RE = re.compile(r'translation ([^\n]+)')
+ROTATION_RE = re.compile(r'rotation ([^\n]+)')
+URL_RE = re.compile(r'url \[ "([^"]+)" \]')
+
+
+def spec(experiment_id):
+    return load_scenario(experiment_id, SCENES)
+
+
+def nodes(experiment_id):
+    return build_nodes(spec(experiment_id), ASSET_ROOT)
+
+
+def by_name(node_list):
+    result = {}
+    for node in node_list:
+        match = NAME_RE.search(node)
+        assert match, node
+        result[match.group(1)] = node
+    return result
+
+
+def translation(node):
+    return tuple(map(float, TRANSLATION_RE.search(node).group(1).split()))
+
+
+def rotation(node):
+    return tuple(map(float, ROTATION_RE.search(node).group(1).split()))
+
+
+def obj_y_bounds(asset):
+    low, high = math.inf, -math.inf
+    for line in (ASSET_ROOT / asset).read_text(encoding='utf-8', errors='replace').splitlines():
+        if line.startswith('v '):
+            y = float(line.split()[2])
+            low, high = min(low, y), max(high, y)
+    return low, high
+
+
+def test_scenarios_one_six_eleven_sixteen_nineteen_are_complete():
+    expected = {
+        1: ['grant_fixture', 'grant_body', 'grant_rack', 'grant_vacuum', 'grant_cap'],
+        6: ['grant_fixture', 'grant_body', 'grant_rack', 'grant_sealant_tool'],
+        11: [
+            'grant_fixture', 'grant_body', 'grant_rack', 'grant_magnetic_tool',
+            'grant_tray', *[f'grant_screw_{i:02d}' for i in range(1, 11)],
+            'grant_cap',
+        ],
+        16: [
+            'grant_fixture', 'grant_body', 'grant_rack',
+            'grant_vacuum', 'grant_magnetic_tool', 'grant_sealant_tool',
+            'grant_torque_tool', 'grant_tray',
+            *[f'grant_screw_{i:02d}' for i in range(1, 11)], 'grant_cap',
+        ],
+        19: [
+            'grant_fixture', 'grant_body', 'grant_rack',
+            'grant_vacuum', 'grant_magnetic_tool', 'grant_sealant_tool',
+            'grant_torque_tool', 'grant_tray',
+            *[f'grant_screw_{i:02d}' for i in range(1, 11)], 'grant_cap',
+        ],
+    }
+    for experiment_id, names in expected.items():
+        assert [NAME_RE.search(node).group(1) for node in nodes(experiment_id)] == names
+
+
+def test_every_managed_node_has_a_stable_unique_def():
+    for experiment_id in range(1, 21):
+        node_list = nodes(experiment_id)
+        defs = [re.search(r'^DEF (\w+) Solid \{', node).group(1) for node in node_list]
+        assert len(defs) == len(set(defs))
+        assert len(defs) == len(node_list)
+        for node, def_name in zip(node_list, defs):
+            assert re.search(rf'name "{def_name}"', node)
+
+
+def test_node_names_are_unique_and_never_duplicate_patron():
+    for experiment_id in range(1, 21):
+        node_list = nodes(experiment_id)
+        names = [NAME_RE.search(node).group(1) for node in node_list]
+        assert len(names) == len(set(names))
+        assert 'patron' not in names
+        assert all('patron' not in node for node in node_list)
+        assert all('GrantToolRack' not in node for node in node_list)
+
+
+def test_all_mesh_urls_are_existing_installed_assets():
+    for experiment_id in range(1, 21):
+        for node in nodes(experiment_id):
+            urls = URL_RE.findall(node)
+            assert len(urls) == 1, node
+            path = Path(urls[0])
+            assert path.is_file(), path
+            assert path.parent == ASSET_ROOT
+            assert 'output' not in path.parts
+
+
+def test_every_required_obj_and_mtl_exists_for_all_twenty_scenarios():
+    for experiment_id in range(1, 21):
+        scenario = spec(experiment_id)
+        required = {'fixture', 'body', 'rack'}
+        required.update(tool.name for tool in scenario.tools)
+        if scenario.tray_pose is not None:
+            required.update({'tray', 'screw'})
+        if scenario.cap_state != 'absent':
+            required.add('cap')
+        for name in required:
+            asset = ASSET_ROOT / ASSETS[name]
+            assert asset.is_file(), (experiment_id, asset)
+            for line in asset.read_text(encoding='utf-8').splitlines():
+                if line.startswith('mtllib'):
+                    assert (ASSET_ROOT / line.split()[1]).is_file()
+
+
+def test_shared_pose_translations_match_the_scenario():
+    for experiment_id in (1, 2, 3, 10, 19):
+        scenario = spec(experiment_id)
+        parsed = by_name(nodes(experiment_id))
+        assert translation(parsed['grant_fixture']) == pytest.approx(
+            (scenario.fixture.x, scenario.fixture.y, scenario.fixture.z)
+        )
+        assert translation(parsed['grant_body']) == pytest.approx(
+            (scenario.body.x, scenario.body.y, scenario.body.z)
+        )
+        assert translation(parsed['grant_rack']) == pytest.approx(
+            (scenario.rack.x, scenario.rack.y, scenario.rack.z)
+        )
+
+
+def test_working_tools_are_in_their_own_sockets():
+    scenario = spec(16)
+    parsed = by_name(nodes(16))
+    for tool in scenario.tools:
+        assert translation(parsed[TOOL_NODE_NAMES[tool.name]]) == pytest.approx(
+            (tool.pose.x, tool.pose.y, tool.pose.z)
+        )
+    positions = {
+        translation(parsed[TOOL_NODE_NAMES[name]]) for name in scenario.tool_names
+    }
+    assert len(positions) == len(scenario.tool_names) == 4
+    assert 'grant_torque_tool' in parsed
+
+
+def test_experiment_nineteen_shifts_the_tray_and_all_ten_screws():
+    standard = by_name(nodes(11))
+    shifted = by_name(nodes(19))
+    tray = translation(shifted['grant_tray'])
+    assert tray == pytest.approx((0.18, 0.48, 0.793))
+    for index in range(1, 11):
+        name = f'grant_screw_{index:02d}'
+        old = translation(standard[name])
+        new = translation(shifted[name])
+        assert new[0] == pytest.approx(old[0] - 0.02)
+        assert new[1] == pytest.approx(old[1] + 0.02)
+    assert len({translation(shifted[f'grant_screw_{i:02d}']) for i in range(1, 11)}) == 10
+
+
+def test_obj_vertical_axis_maps_to_webots_z():
+    axis = (0.0, 1.0, 0.0)
+    for yaw in (0.0, 45.0, 60.0, 135.0, 180.0):
+        matrix = rotation_matrix(yaw)
+        x = sum(matrix[0][i] * axis[i] for i in range(3))
+        y = sum(matrix[1][i] * axis[i] for i in range(3))
+        z = sum(matrix[2][i] * axis[i] for i in range(3))
+        assert (x, y, z) == pytest.approx((0.0, 0.0, 1.0), abs=1e-12)
+        trace = sum(matrix[i][i] for i in range(3))
+        assert math.isclose(trace, math.cos(math.radians(yaw)), abs_tol=1e-12)
+
+
+def test_cap_rotations_match_45_135_and_60_degrees():
+    assert rotation(by_name(nodes(1))['grant_cap']) == pytest.approx(
+        rotation_axis_angle(45), abs=1e-9
+    )
+    assert rotation(by_name(nodes(2))['grant_cap']) == pytest.approx(
+        rotation_axis_angle(135), abs=1e-9
+    )
+    assert rotation(by_name(nodes(19))['grant_cap']) == pytest.approx(
+        rotation_axis_angle(60), abs=1e-9
+    )
+    assert rotation(by_name(nodes(1))['grant_cap'])[3] == pytest.approx(
+        1.717771517458, abs=1e-9
+    )
+
+
+def test_only_the_sealant_mesh_is_scaled_by_ten():
+    for experiment_id in range(1, 21):
+        scenario = spec(experiment_id)
+        parsed = by_name(nodes(experiment_id))
+        count = sum(node.count('scale 10 10 10') for node in parsed.values())
+        assert count == (1 if 'sealant_tool' in scenario.tool_names else 0)
+        if 'sealant_tool' in scenario.tool_names:
+            assert 'scale 10 10 10' in parsed['grant_sealant_tool']
+
+
+def test_loose_cap_touches_the_table_and_installed_cap_touches_the_body():
+    cap_min, _ = obj_y_bounds(ASSETS['cap'])
+    _, body_max = obj_y_bounds(ASSETS['body'])
+    for experiment_id in range(1, 21):
+        scenario = spec(experiment_id)
+        if scenario.cap_state == 'absent':
+            continue
+        cap = by_name(nodes(experiment_id))['grant_cap']
+        cap_z = translation(cap)[2]
+        if scenario.cap_state == 'free':
+            assert cap_z + cap_min == pytest.approx(0.793, abs=1e-9)
+        else:
+            body_z = translation(by_name(nodes(experiment_id))['grant_body'])[2]
+            assert cap_z + cap_min == pytest.approx(body_z + body_max, abs=1e-9)
+
+
+def test_all_caps_are_locked_and_supervisor_controlled():
+    for experiment_id in (1, 11):
+        cap = by_name(nodes(experiment_id))['grant_cap']
+        assert 'locked TRUE' in cap
+        assert 'physics Physics {' not in cap
+
+
+def test_ten_screws_are_distinct_and_inside_the_tray():
+    for experiment_id in (11, 19):
+        scenario = spec(experiment_id)
+        parsed = by_name(nodes(experiment_id))
+        positions = [
+            translation(parsed[f'grant_screw_{index:02d}']) for index in range(1, 11)
+        ]
+        assert len(set(positions)) == 10
+        tray = scenario.tray_pose
+        assert tray is not None
+        for x, y, z in positions:
+            assert abs(x - tray.x) < 0.06
+            assert abs(y - tray.y) < 0.035
+            assert z == pytest.approx(0.7954)
+
+
+def mirror_assets(tmp_path, missing):
+    for source in ASSET_ROOT.iterdir():
+        if source.name == missing:
+            continue
+        (tmp_path / source.name).symlink_to(source)
+
+
+def test_missing_obj_is_a_hard_error(tmp_path):
+    mirror_assets(tmp_path, ASSETS['screw'])
+    with pytest.raises(FileNotFoundError, match='vint.obj'):
+        build_nodes(spec(11), tmp_path)
+
+
+def test_missing_mtl_is_a_hard_error(tmp_path):
+    for source in ASSET_ROOT.iterdir():
+        target = tmp_path / source.name
+        if source.name in (ASSETS['screw'], 'vint.mtl'):
+            continue
+        target.symlink_to(source)
+    (tmp_path / ASSETS['screw']).write_text(
+        (ASSET_ROOT / ASSETS['screw']).read_text(encoding='utf-8'),
+        encoding='utf-8',
+    )
+    with pytest.raises(FileNotFoundError, match='vint.mtl'):
+        build_nodes(spec(11), tmp_path)

src/iiwa_experement/test/test_world.py

@@ -0,0 +1,58 @@
+"""The world template contains the shared table and one attachment supervisor."""
+import re
+from pathlib import Path
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+WORLD = PACKAGE / 'worlds' / 'grant' / 'iiwa-grant.wbt'
+
+
+def test_world_matches_base_layout():
+    text = WORLD.read_text(encoding='utf-8')
+    assert '#VRML_SIM R2025a utf8' in text
+    # Webots defaults to 32 ms when the field is omitted; both forms keep the
+    # deterministic simulation step.
+    assert 'basicTimeStep 32' in text or 'basicTimeStep' not in text
+    assert re.search(r'CollaborativeRobotTable \{\s+translation 0 0 -0\.007', text)
+    assert text.count('WorkspaceLimiter {') == 2
+    assert 'WorkspaceLimiter_back' in text
+    assert 'WorkspaceLimiter_right' in text
+    assert 'coulombFriction' in text and '0.6' in text
+    assert 'Object.proto' not in text
+
+
+def test_world_has_only_the_attachment_supervisor_robot():
+    text = WORLD.read_text(encoding='utf-8')
+    for asset in (
+        'stapel.obj',
+        'base-detail.obj',
+        'cap.obj',
+        'lotok.obj',
+        'vint.obj',
+        'tool-magizine.obj',
+        'vacum-tool.obj',
+        'magnit-tool.obj',
+        'germetic-tool.obj',
+        'wranch-tool.obj',
+    ):
+        assert asset not in text
+    assert 'Kuka' not in text
+    assert 'GrantToolRack' not in text
+    robots = re.findall(r'Robot \{', text)
+    assert len(robots) == 1
+    assert 'name "GrantAttachmentSupervisor"' in text
+    assert 'controller "<extern>"' in text
+    assert 'supervisor TRUE' in text
+    assert 'name "Ros2Supervisor"' not in text
+
+
+def test_world_only_references_the_two_local_protos():
+    text = WORLD.read_text(encoding='utf-8')
+    externprotos = re.findall(r'EXTERNPROTO "([^"]+)"', text)
+    local = [url for url in externprotos if not url.startswith(('http://', 'https://'))]
+    assert len(local) == 2
+    assert local[0].endswith('CollaborativeRobotTable.proto')
+    assert local[1].endswith('WorkspaceLimiter.proto')
+    for url in externprotos:
+        if url.startswith(('http://', 'https://')):
+            assert '/webots/R2025a/' in url

src/iiwa_experement/worlds/grant/.iiwa-grant.wbproj

@@ -1,8 +1,9 @@
 Webots Project File version R2025a
-perspectives: 000000ff00000000fd0000000300000000000001ca0000039ffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000000000010000023a00000390fc0200000001fb0000001400540065007800740045006400690074006f00720000000014000003900000003f00ffffff0000000300000780000000a0fc0100000002fb0000000e0043006f006e0073006f006c006501000001d0000005b00000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c010000000000000780000000000000000000000780000003cb00000004000000040000000100000008fc00000000
-simulationViewPerspectives: 000000ff00000001000000020000016c000004a00100000002010000000101
-sceneTreePerspectives: 000000ff00000001000000030000001c000000c0000000fa0100000002010000000201
+perspectives: 000000ff00000000fd0000000300000000000001ca0000039ffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000000000010000023a000002effc0200000001fb0000001400540065007800740045006400690074006f00720000000014000002ef0000003f00ffffff0000000300000780000000dafc0100000002fb0000000e0043006f006e0073006f006c006501000001d0000005b00000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c010000000000000780000000000000000000000780000003cb00000004000000040000000100000008fc00000000
+simulationViewPerspectives: 000000ff0000000100000002000000000000077e0100000002010000000101
+sceneTreePerspectives: 000000ff00000001000000030000001c000000c0000000000100000002010000000201
 maximizedDockId: -1
 centralWidgetVisible: 1
+renderingMode: PLAIN
 orthographicViewHeight: 1
 textFiles: -1

src/iiwa_experement/worlds/grant/iiwa-grant.wbt

@@ -0,0 +1,86 @@
+#VRML_SIM R2025a utf8
+
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackground.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/floors/protos/RectangleArena.proto"
+EXTERNPROTO "/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/protos/CollaborativeRobotTable.proto"
+EXTERNPROTO "/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/protos/WorkspaceLimiter.proto"
+
+WorldInfo {
+  title "KUKA iiwa grant experiments"
+  optimalThreadCount 4
+  contactProperties [
+    ContactProperties {
+      material1 "TableMaterial"
+      coulombFriction [
+        0.6
+      ]
+      bounce 0
+      bounceVelocity 0
+      softERP 1
+      softCFM 0.0001
+    }
+    ContactProperties {
+      material1 "tool"
+      material2 "TableMaterial"
+      bounce 0
+      softCFM 0.002
+      maxContactJoints 100
+    }
+    ContactProperties {
+      bounce 0
+      maxContactJoints 20
+    }
+  ]
+}
+Viewpoint {
+  orientation -0.16538833828406266 0.05829171450270743 0.9845043288782285 2.4736073370602876
+  position 4.897723425157077 -3.966075191122516 3.20774809692606
+  followType "None"
+  ambientOcclusionRadius 3
+}
+TexturedBackground {
+}
+TexturedBackgroundLight {
+  texture "noon_sunny_empty"
+  luminosity 2
+}
+RectangleArena {
+  name "Arena"
+  floorSize 12 12
+  wallHeight 2
+}
+CollaborativeRobotTable {
+  translation 0 0 -0.007
+  locked TRUE
+  showCabinet TRUE
+}
+DEF limiter Group {
+  children [
+    WorkspaceLimiter {
+      translation -1.56 -0.219999 1.01
+      rotation 0 0 1 1.5708
+      name "WorkspaceLimiter_back"
+      locked TRUE
+      scale 2.32 1 2
+    }
+    WorkspaceLimiter {
+      translation -0.04 1.18 1.01
+      name "WorkspaceLimiter_right"
+      locked TRUE
+      scale 2.43 1 2
+    }
+  ]
+}
+DirectionalLight {
+  direction -0.1 0.1 0
+  intensity 2
+}
+
+# External controller from iiwa_experement/grant_supervisor.py.  It is the
+# only process that changes object ownership and follows the actual tcp.
+Robot {
+  name "GrantAttachmentSupervisor"
+  controller "<extern>"
+  supervisor TRUE
+}

src/iiwa_llm_plan_executor/config/motion_limits.yaml

@@ -0,0 +1,32 @@
+# Hard limits for the constrained MoveItPy adapter.  They are deliberately
+# separate from the reference executor: the graph executor never falls back to
+# an unconstrained MoveToPose goal.
+schema_version: 1
+
+joint7:
+  # Preferred value is the neutral work pose (joint_7 = 0).  This is a
+  # preference only; the real per-joint limits below remain authoritative.
+  work_reference_rad: 0.0
+  corridor_rad: 1.5707963267948966
+
+tcp:
+  # The working tip points down; a working motion may tilt at most 10 degrees.
+  max_tilt_deg: 10.0
+
+trajectory:
+  # A near-pi jump indicates an IK branch change.  Reject it before execution.
+  max_joint_jump_rad: 0.6
+  max_cartesian_jump_m: 0.15
+  # The planned first point must match the measured joints used as IK seed.
+  start_tolerance_rad: 0.35
+  # Dense joint-space resampling used for collision/orientation validation.
+  max_sample_step_rad: 0.01
+
+freshness:
+  joint_state_max_age_sec: 1.0
+  scene_max_age_sec: 2.0
+
+collision:
+  # Minimum accepted collision distance (metres) for every sampled state.
+  # Camera-vs-robot pairs are enabled in the generated SRDF.
+  min_clearance_m: 0.003

src/iiwa_llm_plan_executor/config/planner.yaml

@@ -0,0 +1,15 @@
+# Defaults for `ros2 launch iiwa_llm_plan_executor planner.launch.py`.
+# Qwen3 is called outside this project; this file only configures Webots and
+# the two new nodes.
+start_delay_sec: 3.0
+scene_max_age_sec: 2.0
+max_steps: 512
+scene_freshness_sec: 2.0
+close_inspection_objects:
+  - cap
+top_camera_topic: /d455_top/image_raw/image_color
+hand_camera_topic: /camera/color/image_color
+frame_timeout_sec: 2.0
+trace_dir: ~/.ros/iiwa_llm_plan_executor/runs
+planning_group: iiwa_arm
+pose_link: tcp

src/iiwa_llm_plan_executor/doc/README.md

@@ -0,0 +1,132 @@
+# Executing external Qwen3 graphs in Webots
+
+## Краткая инструкция (русский)
+
+```bash
+source /opt/ros/jazzy/setup.bash
+colcon build --base-paths src --symlink-install
+source install/setup.bash
+ros2 launch iiwa_llm_plan_executor planner.launch.py experiment_id:=20
+```
+
+Дождитесь готовности сцены, прочитайте `/planner/scene` или
+`/iiwa_scene_context/get_scene`, передайте `scene_json` и инструкцию Qwen3
+снаружи, затем отправьте готовый граф в
+`/iiwa_llm_plan_executor/submit_plan` (`iiwa_msgs/SubmitPlan`) или через
+`POST /planner/runs` с телом `{"run_id": "...", "response": {...}}`.
+`scene_revision` не передаётся и не возвращается. Отклонённый запрос не
+регистрируется, поэтому тот же `run_id` можно использовать после исправления
+графа. Подробности: `docs/interfaces.md`.
+
+This package accepts a **ready-made** JSON graph.  It does not call Qwen3, does
+not evaluate the model and does not compare runs with a reference sequence.
+The experiment number selects only the initial Webots scene.
+
+## Start
+
+```bash
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+ros2 launch iiwa_llm_plan_executor planner.launch.py experiment_id:=20
+```
+
+The launch starts the standard `iiwa_bringup` simulation, the writable Grant
+supervisor (`mode=planner`), the scene context and the graph executor.  It
+never starts `reference_runner` and never starts a second `iiwa_web` server:
+the web server already comes from `iiwa_bringup` when `web.enabled` is true in
+`cobot-setting.yaml`.  For a ROS-only run set `web.enabled: false` in your
+settings before launch.
+
+Useful arguments: `rviz:=true`, `config:=/path/planner.yaml`.
+`simulate` must remain `true`; the physical FRI robot is not connected.
+
+## Read the scene and submit a graph
+
+Wait until the launch log shows the scene is ready (the snapshot is only
+served after `/iiwa_experement/scene_ready`).
+
+ROS:
+
+```bash
+ros2 service call /iiwa_scene_context/get_scene std_srvs/srv/Trigger {}
+ros2 topic echo /iiwa_scene_context/scene_json --qos-durability transient_local
+```
+
+Give `scene_json` and your instruction to Qwen3 outside this project.  Submit
+the answer and a client-chosen `run_id`; the executor obtains and validates its
+own fresh scene snapshot, so no scene revision travels through the API:
+
+```bash
+ros2 service call /iiwa_llm_plan_executor/submit_plan iiwa_msgs/srv/SubmitPlan \
+  "{run_id: 'demo-1', response: '{\"status\":\"ready\",\"entry\":\"detect\",\"plan\":[...]}'}"
+
+ros2 topic echo /iiwa_llm_plan_executor/plan --qos-durability transient_local
+ros2 topic echo /iiwa_llm_plan_executor/status
+```
+
+The legacy `/iiwa_llm_plan_executor/submit` (`webots_ros2_msgs/SetString`) still
+routes into the same admission path.  Rejections return `code` and `reason`
+from `SubmitPlan`; a rejected `run_id` is not registered and can be reused with
+a corrected graph.
+
+The accepted graph is published **unchanged** in
+`/iiwa_llm_plan_executor/plan` before the first movement.  A rejected graph
+never appears there; read the reason from
+`/iiwa_llm_plan_executor/status` or:
+
+```bash
+ros2 service call /iiwa_llm_plan_executor/get_status iiwa_msgs/srv/GetRunStatus \
+  "{run_id: demo-1}"
+ros2 service call /iiwa_llm_plan_executor/cancel webots_ros2_msgs/srv/SetString \
+  "{value: '{\"run_id\":\"demo-1\"}'}"
+```
+
+HTTP (existing `iiwa_web` server; default port 8007):
+
+```bash
+curl http://localhost:8007/planner/scene
+curl -X POST http://localhost:8007/planner/runs \
+  -H 'Content-Type: application/json' \
+  -d '{"run_id":"demo-1","response":"<Qwen JSON string>"}'
+curl http://localhost:8007/planner/runs/demo-1
+curl -X POST http://localhost:8007/planner/runs/demo-1/cancel
+```
+
+The same four operations are MCP tools built from the same router
+(`get_scene_planner_scene_get`, `submit_run_planner_runs_post`,
+`get_run_planner_runs`, `cancel_run_planner_runs`).
+
+## Nodes and terminals
+
+Only one graph is active per robot.  Repeating an identical accepted graph is
+idempotent (`duplicate`); another graph with the same accepted `run_id` is a
+`conflict`; a different `run_id` while a run is active is rejected (`busy`).
+Accepted run ids stay bound after every terminal state.
+
+Status events contain the run id, node id, skill, result event, the chosen
+next node, the mounted tool, the held object and, at the end, the terminal.
+The internal scene revision is never exposed.  Terminals:
+
+* `completed` — the submitted graph and its checked postconditions finished.
+* `replan_required` — the graph stopped and a fresh snapshot was emitted; the
+  external client decides whether to call Qwen3 again.
+* `operator_required` — a human decision is needed; the robot is stopped.
+* `failed` / `canceled` — safety stop (stale state, limits, collision, cancel,
+  step limit); the grasp state is preserved in the status.
+
+The JSONL log with every event is written to
+`~/.ros/iiwa_llm_plan_executor/runs/<run_id>.jsonl`.
+
+## Safety scope
+
+The executor uses Pilz PTP for free-space working-point moves and Pilz LIN for
+axial contact/retreat and the dispense contour.  Every plan is densely
+resampled and checked for real joint limits, self-collision (including the
+camera assembly), scene collision, camera clearance, downward TCP tilt (≤10°)
+and joint jumps before execution.  J7 may use its full safe joint range; the
+`work` value is only a preference.  A lateral PTP from a contact pose is
+preceded by an axial LIN retreat.  The only logged orientation exception is
+the initial empty-gripper reorientation from home to the downward `work`
+posture.  The executor never falls back to an unconstrained `MoveToPose` goal.
+This is a Webots verification, not a physical-robot validation and not a
+Qwen3 quality evaluation.

src/iiwa_llm_plan_executor/doc/webots-qa.md

@@ -0,0 +1,283 @@
+# Webots QA: external graph execution
+
+Scope: Webots only (`simulate:=true`).  No physical FRI robot was connected and
+no Qwen3 quality/score system was built or called.
+
+## Build and package tests
+
+Commands:
+
+```bash
+source /opt/ros/jazzy/setup.bash
+colcon build --base-paths src --symlink-install \
+  --packages-up-to iiwa_experement iiwa_scene_context iiwa_llm_plan_executor iiwa_web
+source install/setup.bash
+colcon test --base-paths src --packages-select \
+  iiwa_experement iiwa_scene_context iiwa_llm_plan_executor iiwa_web
+colcon test-result --verbose
+```
+
+Result: the five affected packages build successfully; `colcon test` reports
+`402 tests, 0 errors, 0 failures, 0 skipped` for `iiwa_msgs`,
+`iiwa_experement`, `iiwa_scene_context`, `iiwa_llm_plan_executor` and
+`iiwa_web`.  Four pre-existing `test_body_hatch_worlds.py` cases in other
+packages still fail because their `.wbt`/`.proto` files are absent from this
+checkout; they are unrelated to this change.
+
+`git diff --check` is clean.
+
+## Launch and scene contract
+
+```bash
+ros2 launch iiwa_llm_plan_executor planner.launch.py experiment_id:=20
+```
+
+* 19 scene objects spawned (fixture, body, cap, tray, 10 screws, 4 tools).
+* `/iiwa_experement/scene_ready` true; `/iiwa_scene_context/get_scene` served
+  `schema_version=2`, `contract_version=2`, `catalog_version=2`,
+  `state_version=0`, live poses and action rules.  `scene_revision` is not part
+  of the public envelope.
+* The camera contract was corrected to the topics actually published by the
+  existing Webots camera plugins: `/d455_top/image_raw/image_color` and
+  `/camera/color/image_color`.
+* `/iiwa_web` was started once by `iiwa_bringup` (`web.enabled: true`); the
+  planner launch did not start a second web server.
+
+## Graphs exercised in scene 20
+
+All graphs were submitted through `/iiwa_llm_plan_executor/submit` with a
+client-supplied `run_id`; the accepted graph appeared in
+`/iiwa_llm_plan_executor/plan` before the first movement.
+
+| Graph | Result |
+| --- | --- |
+| `detect(cap)` | `completed`; `found`, cap pose read from supervisor, fresh top+hand frames. |
+| `grab(vacuum) → transfer(vacuum,work) → completed` | `completed`; status ended with `mounted_tool: "vacuum"`, `held_object: null`. The cup stayed in `patron`. |
+| `transfer(vacuum,vacuum_socket) → dock_tool(vacuum,vacuum_socket) → completed` | `completed`; status ended with `mounted_tool: null`. This was a separate graph after the previous one. |
+| `grab(cap) → transfer(cap,hatch_seat) → align → release(cap,hatch_seat)` (cup already mounted) | Cap grasp, transfer, align and release succeeded; the supervisor reported the hatch installed. The final `verify` first failed because of a web-wrapper attribute shadowing bug (`latest`), which was fixed afterwards; a clean repeat of the whole cap graph was not obtained in this session. |
+| cap graph without explicit `grab(vacuum)`, with a `grasp_failed` edge | Admitted; execution returned `grasp_failed` with `reason: "required tool vacuum is not mounted"`, `next: "operator_required"`, `mounted_tool: null`, `held_object: null`. The supervisor state stayed at version 0 with the cap `free` and `vacuum` `docked`; the log contains no planner call for that run. `/plan` held the original graph, `/status`, `GET /planner/runs/...` and the JSONL log held the actual event/reason/ownership/next edge. |
+| invalid graph with a stray node key | `POST /planner/runs` returned 422 with `code=invalid` and the exact schema reason; `GET /planner/runs/<id>` returned 404 and no JSONL run log was created. The corrected graph with the **same** `run_id` was accepted, then `completed`. |
+| different graph with an already accepted `run_id` | 409 `code=conflict`; the original run and its single `/plan` publication were unchanged. |
+| identical accepted graph repeated with the same `run_id` | 200 `code=duplicate`; no second motion. |
+
+The screw handoff graph in experiment 11 was attempted in Webots.  The tool
+grab succeeded, but the subsequent screw approach was rejected by the strict
+motion policy: `screw_1` failed at `tcp tilt ... 10.26 deg (limit 10.00)`, and
+`screw_6` failed with `NO_IK_SOLUTION` at the far tray point.  This is the same
+planning limitation recorded below; it is not a physical-robot test.  The
+handoff logic itself remains covered by package tests.
+
+## 2026-11-11 contract and pause review
+
+### Live `scene_json`
+
+* Experiment 20: 14 components (fixture, body, cap, tray, 10 screws), four
+  tools, 14 destinations (work, hatch_seat, four holes, four sockets, four
+  contour sides).  The live supervisor pose overrides the manifest pose; held
+  objects are republished with their current pose and `source=webots_supervisor`.
+* Experiment 8: only fixture/body, `sealant_tool`, four contour sides and that
+  tool's socket; no cap, tray, screw or unrelated tool.  `dispense` is present,
+  `handoff_screw` is absent.
+* Experiment 11: cap, tray, 10 screws, `magnetic_tool`, four holes and its
+  socket; `handoff_screw` is present.
+* `docs/scene_json_experiment_20.json` is generated by the same `build_scene`
+  function; a unit test fails if the example drifts.
+
+### Dispense and tool return (experiment 8)
+
+Graph: `grab(sealant_tool) → dispense(side_1) → transfer(sealant_tool,
+sealant_tool_socket) → dock_tool(sealant_tool, sealant_tool_socket)`.
+Result: `completed`.  `side_1` was an existing contour destination, the
+dispense used a continuous LIN segment, and the tool was returned to its own
+socket.
+
+### Tool grab/return (experiment 20)
+
+Graph: `grab(vacuum) → transfer(vacuum, work) → transfer(vacuum, vacuum_socket)
+→ dock_tool(vacuum, vacuum_socket)`.  Result: `completed`; `mounted_tool`
+ended as `null`.
+
+### Timing before/after
+
+The same approach/retreat graph was run in experiment 20 before and after the
+planning-scene pause change.  The monotonic JSONL stages now show:
+
+| stage | before (s) | after (s) |
+| --- | --- | --- |
+| inter-motion gap (`since_previous_trajectory_sec`) | 0.0015 | 0.0013 |
+| `_wait_stationary` (normal) | 0.20–0.35 | 0.20–0.35 |
+| PlanningScene relay | 0.10–0.21 | 0.10–0.12 |
+| PlanningScene check | 0.10 | 0.10 |
+| MoveIt plan | 3.6–7.5 | 3.6–7.5 |
+| trajectory check | 0.002–0.004 | 0.002–0.004 |
+| controller execution | 4.0–12.2 | 4.0–12.2 |
+| supervisor response | 0.02–0.04 | 0.02–0.04 |
+
+The fixed `0.5 s` wait for the next `/joint_states` sample in every full
+planning-scene publish was removed; freshness is still checked immediately
+before every plan.  The monitored-scene rendezvous remains because it is the
+proof that MoveItPy also received the scene.  The only measured ~3 s interval
+is a genuine `_wait_stationary` of 3.3–3.7 s after a dock transfer while the
+Webots controller is still decelerating; it is deliberately kept because it is
+the required stationary-start check, not an added sleep.  `start_delay_sec`
+remains a single 3 s gate after scene load and is never applied per motion.
+
+### Tilt limitation
+
+The existing strict 10° tcp tilt limit rejected the far-tray screw approach in
+experiment 11 (`screw_1`: 10.26°; `screw_6`: `NO_IK_SOLUTION`).  No speed or
+tilt limit was raised to hide it.
+
+## J7 and tilt evidence
+
+Every executed trajectory passed `MotionPolicy` before the controller. The
+JSONL log records e.g.:
+
+```json
+{"event":"motion_planned","phase":"transfer","points":64,
+ "min_j7":-3.2e-07,"max_j7":0.874,"max_tilt_deg":180.0,
+ "reorientation":true,"max_joint_jump_rad":0.0349,
+ "max_cartesian_step_m":0.0387}
+```
+
+That is the explicit empty-gripper reorientation from home (tip up) to the
+downward `work` posture.  Working motions keep J7 inside the ±90 deg corridor
+and are rejected point-by-point when J7, tilt or jumps leave the configured
+bounds.  Out-of-corridor and stale-state graphs stopped without execution.
+
+## Remaining limitations found in Webots
+
+1. **OMPL goal sampling is not deterministic.** With both the J7 corridor and a
+   strict orientation goal region, OMPL reports “more than 80% of sampled goal
+   states fail to satisfy the constraints” and sometimes returns no plan. The
+   executor retries constrained plans, but identical graphs can succeed in one
+   run and return `replan_required` in another.
+2. **Pilz does not honor path constraints in this installation.** PTP can pick
+   the forbidden J7 branch; LIN with non-empty path constraints can return no
+   trajectory. The adapter therefore uses OMPL+J7 for tool transits, LIN for
+   part transits/approaches/contact, and enforces all tilt/J7/jump limits after
+   planning. This is a planning limitation, not a fallback to an unconstrained
+   `MoveToPose` action.
+3. **Tilt limit.** The original Webots runs returned some OMPL/LIN paths at
+   10.03–10.16 deg tilt against the 10 deg limit; the previous 0.25 deg
+   allowance accepted them. The review on 2026-11-11 replaced that allowance
+   with floating-point roundoff only, so those paths are now rejected before
+   execution. This stricter policy passed package tests but has not yet been
+   rechecked in Webots. A planner/adapter that honors the orientation
+   constraint end-to-end is still needed for reliable execution.
+4. **Controller start overshoot.** Webots can settle a joint ~3e-5 rad outside
+   MoveIt's soft limit; the start state is now clamped to the same
+   `soft_min_position`/`soft_max_position` band before planning.
+5. **Cancel and collision in Webots.** Cancel is covered by engine/node tests
+   (active run stops, grasp preserved). The Webots cancel probe attempted here
+   ended in a planner `replan_required` before the cancel instant, so it is not
+   counted as a successful Webots cancel demonstration. No synthetic collision
+   was injected; collision branches are covered by validator/back-end tests and
+   any planner/execution failure stops further motion.
+6. This is not a physical-robot validation and not an evaluation of Qwen3.
+
+## Reproduce
+
+```bash
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+ros2 launch iiwa_llm_plan_executor planner.launch.py experiment_id:=20
+ros2 service call /iiwa_scene_context/get_scene std_srvs/srv/Trigger {}
+# submit a graph through /iiwa_llm_plan_executor/submit_plan; see doc/README.md
+ros2 topic echo /iiwa_llm_plan_executor/plan --qos-durability transient_local
+ros2 topic echo /iiwa_llm_plan_executor/status
+```
+
+## Full Qwen cap graph (`qwen_cap_full_graph.json`)
+
+The complete graph from the external Qwen run was added as a test fixture and
+checked in scene 20.
+
+### Empty patron
+
+The integration test uses the real `GraphEngine`, real `SkillRegistry` and the
+full 20-node graph, with fake motion/supervisor adapters that forbid supervisor
+commands.  The executed node sequence is:
+
+```
+a_find_part → a_find_target → a_approach → a_pick(grasp_failed)
+→ a_stop_recover → a_state(empty) → a_retry_find_part → a_retry_approach
+→ a_retry_pick(grasp_failed) → stop_operator → notify → operator_required
+```
+
+The halt reasons are `required tool vacuum is not mounted`; no supervisor
+command is issued, no `lower`/`contact` phase is planned, the hatch stays
+`free`, and `mounted_tool`/`held_object` stay `null`.
+
+In Webots the full graph reached the requested actual branch:
+
+```
+a_pick(grasp_failed) → a_stop_recover → a_state(empty)
+→ a_retry_find_part → a_retry_approach → a_retry_pick(grasp_failed)
+→ stop_operator → notify → operator_required
+```
+
+The halt reasons were `required tool vacuum is not mounted`; the supervisor
+stayed at version 0, `mounted_tool`/`held_object` were `null`, the hatch stayed
+`free`, and no `grab_cap`/`attach_tool`/`place_cap` command appeared in the log.
+
+### Vacuum mounted
+
+With `vacuum` mounted, the graph's nominal success route is:
+
+```
+a_inspect(ok) → a_state(placed) → a_inspect(ok) → ...
+```
+
+It is a cycle and the graph contains no `completed` transition at all, so a
+nominal successful run cannot finish.  The full graph also has a conservative
+recovery issue: `grasp_failed` leads through `inspect_state: holding` to
+`a_retry_pick`, which would pick the hatch while already holding it.  The
+validator therefore rejects that recovery branch with its own ownership reason
+(`a_retry_pick ... while holding 'cap'`); this does not repair the model graph.
+Completion must not be claimed for this graph.
+
+## Motion planning fix (PTP/LIN, camera collision model)
+
+### Reproduction before the change
+
+The failing `approach(cap)` request was captured with diagnostics:
+
+```text
+phase=approach
+effective_phase=transfer_lin
+pipeline=pilz_industrial_motion_planner
+planner_id=LIN
+goal_xyz=(0.6, -0.22, 0.206667)          # base_link
+goal_quat=(-0.382683, 0.92388, 0, 0)
+start=(~0, ~0, ~0, ~0, ~0, ~0, ~0)
+MoveIt error: NO_IK_SOLUTION
+```
+
+All three attempts failed in IK.  The old error text about the J7 corridor was
+therefore misleading: the failing planner was LIN and the actual cause was
+`NO_IK_SOLUTION`, not a joint-7 boundary.
+
+### After the change
+
+* `detect(cap) → approach(cap)` in Webots: both nodes succeeded.  Transfers
+  and approaches used `pilz_industrial_motion_planner/PTP`; the measured
+  minimum sampled clearance was **0.0288 m** (required 0.003 m), maximum J7
+  was 0.875 rad.
+* A separate J7-rotation probe moved the arm to the `work` posture with
+  `joint7 = 2.0 rad` and then executed `approach(cap)`.  The accepted sampled
+  trajectory covered `joint7 = -1.225 … 2.000 rad`, i.e. well beyond the old
+  ±90 deg boundary, with no joint-limit, self-collision or clearance failure;
+  measured clearance was again **0.0288 m**.
+* Camera geometry: `camera_corner` and `camera_hand` keep at least **35 mm**
+  AABB clearance to `link6`/`link7` at `home` and `work`.  The D455 collision
+  box now matches its visible bounds.  Only constructive fixed contact pairs
+  are disabled in `tools.yaml` and the regenerated SRDF; all camera-to-robot
+  pairs remain checked.
+* Tests: `pytest src/iiwa_llm_plan_executor/test` → **122 passed**; camera /
+  tool collision + TCP tests in `iiwa_utils` → **7 passed**.  The existing
+  unrelated
+  `test_body_hatch_worlds.py` still fails because its world files are absent
+  from this checkout; it is not part of the motion change.
+
+Physical robot was not tested.

src/iiwa_llm_plan_executor/docs/interfaces.md

@@ -0,0 +1,143 @@
+# Интерфейсы `iiwa_llm_plan_executor`
+
+Версия контракта графа: **2** (совпадает с `iiwa_scene_context` 2).
+Исполнитель не вызывает Qwen3 и не переписывает граф модели. Система
+принимает готовый JSON-граф.
+
+## Ответ графа
+
+```json
+{
+  "status": "ready",
+  "entry": "p1_find",
+  "plan": [
+    {
+      "id": "p1_find",
+      "skill": "detect",
+      "object": "cap",
+      "target": "",
+      "params": {},
+      "transitions": [
+        {"event": "found", "next": "p1_grab"},
+        {"event": "not_found", "next": "replan_required"},
+        {"event": "failed", "next": "replan_required"}
+      ]
+    }
+  ]
+}
+```
+
+* `status` — `ready` или `blocked`; `blocked` никогда не исполняется.
+* `plan` — не более 256 узлов. Отклоняются неизвестные ключи, дубли `id`,
+  зарезервированные терминалы, отсутствующий `entry`, пропущенные события,
+  неизвестные `next`, недостижимые узлы и узлы без пути к терминалу.
+* Терминалы: `completed`, `replan_required`, `operator_required`.
+* `params` пока поддерживает только `dispense.start_fraction` /
+  `dispense.end_fraction` в диапазоне `0..1`.
+
+## Навыки
+
+Реализованные навыки: `detect`, `approach`, `lower`, `grab`, `lift`,
+`transfer`, `align`, `release`, `retreat`, `capture`, `verify`, `stop`,
+`wait_scene_clear`, `inspect_state`, `notify_operator`, `dispense`,
+`dock_tool`, `handoff_screw`.
+
+Алиасы: `pick` (= `grab`), `move_to_storage` (= `transfer` + осевой
+`lower`), `inspect` (= `verify`). `tighten` не входит в профиль и
+отклоняется до любого движения.
+
+Набор навыков и правила приходят из `iiwa_scene_context/catalog.py` и живого
+`scene_json`; модель не может добавить навык или контракт. В сцене без
+герметика `dispense` не рекламируется, без винтов — `handoff_screw`, без
+инструментов — `dock_tool`.
+
+## Приём графа
+
+`parse_response(raw) -> PlanGraph` выполняет строгую проверку формы.
+`validate_graph(graph, scene_envelope, catalog)` возвращает
+`ValidationResult(ok, code, reason, details)`.
+
+Порядок приёма:
+
+1. формат JSON и схема графа;
+2. готовность сцены/MoveIt/контроллера и истёкшая задержка старта;
+3. собственный свежий снимок сцены исполнителем;
+4. проверка свежести снимка;
+5. валидация структуры, ветвей и владения относительно этого снимка;
+6. публикация принятого графа.
+
+`RunRecord` создаётся **только после** успешного прохождения всех шагов и
+публикации. Поэтому отклонённый `run_id` свободен: исправленный граф можно
+отправить снова. Отклонённый запрос не появляется в
+`GET /planner/runs/{run_id}`, в `/iiwa_llm_plan_executor/plan` и в JSONL-журнале
+запусков. API возвращает конкретные `code`/`reason`.
+
+Коды отклонения: `invalid`, `rejected`, `blocked`, `replan_required`,
+`not_ready`, `stale_scene`, `busy`, `conflict`.
+
+Правила `run_id`:
+
+* пока запуск активен, другой `run_id` получает `busy`;
+* повтор идентичного принятого графа с тем же `run_id` — `duplicate`, второго
+  движения нет;
+* другой граф с уже принятым `run_id` — `conflict`;
+* после `completed`, `operator_required`, `replan_required`, `failed` или
+  `canceled` `run_id` остаётся занятым;
+* одновременные отправки защищены блокировкой, активный граф на робота один.
+
+Перед первым движением исполнитель повторно проверяет свежесть сцены и
+`state_version`. Если сцена изменилась или стала недоступной, движение не
+начинается. Во время выполнения внутренний счётчик (не публикуется) ловит
+внешнее изменение сцены; изменения владения, подтверждённые собственными
+`grab`/`release`/`dock_tool`/`handoff_screw`, разрешены.
+
+## ROS-интерфейсы
+
+| Направление | Имя | Тип | Payload |
+| --- | --- | --- | --- |
+| сервис | `/iiwa_llm_plan_executor/submit_plan` | `iiwa_msgs/SubmitPlan` | request `run_id`, `response` (JSON-строка графа). Ответ: `accepted`, `code`, `reason`, `status_json`. |
+| сервис | `/iiwa_llm_plan_executor/submit` | `webots_ros2_msgs/SetString` | Устаревший совместимый вход `{"run_id","response"}`; направляется в тот же механизм приёма. Поле `scene_revision` игнорируется. |
+| топик | `/iiwa_llm_plan_executor/plan` | `std_msgs/String` | `{"run_id","response"}` для принятого графа; transient-local, depth 1. Отклонённые графы сюда не попадают. |
+| топик | `/iiwa_llm_plan_executor/status` | `std_msgs/String` | События и терминал: `run_id`, `state`, `reason`, `node_id`, `skill`, `event`, `next`, `terminal`, `mounted_tool`, `held_object`, `timestamp`, `event_detail`/`timing`. Поля `scene_revision` нет. |
+| сервис | `/iiwa_llm_plan_executor/get_status` | `iiwa_msgs/GetRunStatus` | request `string run_id`; response `bool found`, `string status_json`. |
+| сервис | `/iiwa_llm_plan_executor/cancel` | `webots_ros2_msgs/SetString` | `{"run_id": "..."}` или строка; останавливает выполнение, сохраняя захват. |
+
+## Тайминги этапов
+
+Каждый запуск пишет JSONL в `~/.ros/iiwa_llm_plan_executor/runs/<run_id>.jsonl`.
+События `motion_timing` и `timing` содержат монотонные `started_at`,
+`finished_at`, `duration_sec` для этапов:
+
+* завершение предыдущей траектории — `since_previous_trajectory_sec`;
+* `_wait_stationary` — `wait_stationary`;
+* синхронизация PlanningScene — `planning_scene_check` /
+  `planning_scene_sync`;
+* построение плана MoveIt — `moveit_plan`;
+* проверка траектории — `trajectory_check`;
+* ожидание контроллера — `controller_execution`;
+* ответ supervisor — `supervisor_command` (`service_wait_sec`,
+  `service_call_sec`, `result_wait_sec`);
+* захват кадра — `frame_capture`.
+
+`start_delay_sec` — задержка после загрузки сцены, применяется один раз перед
+стартом графа. Это не пауза между движениями.
+
+## Терминалы и журнал
+
+`completed` — граф и его проверенные постусловия завершены.
+`replan_required` — нужен новый снимок и, возможно, новый вызов Qwen3.
+`operator_required` — нужно решение человека; робот остановлен.
+`failed` / `canceled` — безопасная остановка (устаревшее состояние, лимиты,
+столкновение, отмена, лимит шагов); состояние захвата сохраняется.
+
+## Ограничения движения
+
+Свободные перемещения между рабочими точками — Pilz PTP. Осевое опускание,
+подъём и нанесение герметика — Pilz LIN. Каждый план плотно пересэмплируется
+и проверяется по реальным лимитам суставов, self/scene collision, зазору до
+камер, наклону TCP (≤10°) и скачкам суставов. Боковое движение из контактной
+позы предваряется осевым LIN-отводом. Исполнитель не откатывается к
+неограниченному `MoveToPose`.
+
+Это проверка в Webots, а не валидация физического робота и не оценка
+качества Qwen3.

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/__init__.py

@@ -0,0 +1,23 @@
+"""Graph validation and execution for externally supplied Qwen3 plans."""
+from .schema import (
+    MAX_NODES,
+    TERMINALS,
+    PlanGraph,
+    PlanNode,
+    PlanSchemaError,
+    Transition,
+    parse_response,
+)
+from .validator import ValidationResult, validate_graph
+
+__all__ = [
+    'MAX_NODES',
+    'TERMINALS',
+    'PlanGraph',
+    'PlanNode',
+    'PlanSchemaError',
+    'Transition',
+    'ValidationResult',
+    'parse_response',
+    'validate_graph',
+]

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/engine.py

@@ -0,0 +1,624 @@
+"""ROS-free graph engine: admission, edge selection, terminals and status."""
+from __future__ import annotations
+
+import json
+import threading
+import time
+from dataclasses import dataclass, field
+from typing import Any, Callable, Mapping
+
+from iiwa_scene_context.catalog import DEFAULT_CATALOG, SkillCatalog
+
+from .schema import TERMINALS, PlanGraph, PlanSchemaError, parse_response
+from .validator import ValidationResult, validate_graph
+
+
+ACTIVE_STATES = ('admitted', 'running')
+TERMINAL_STATES = (
+    'completed', 'replan_required', 'operator_required', 'failed', 'canceled',
+)
+OWNERSHIP_SKILLS = ('grab', 'pick', 'release', 'dock_tool', 'handoff_screw')
+DEFAULT_MIN_START_DELAY_SEC = 3.0
+DEFAULT_SCENE_MAX_AGE_SEC = 2.0
+DEFAULT_MAX_STEPS = 512
+
+
+@dataclass(frozen=True)
+class AdmissionResult:
+    accepted: bool
+    code: str
+    message: str
+    run_id: str
+
+    def as_dict(self) -> dict[str, Any]:
+        return {
+            'accepted': self.accepted,
+            'code': self.code,
+            'message': self.message,
+            'run_id': self.run_id,
+        }
+
+
+@dataclass
+class RunRecord:
+    run_id: str
+    state: str = 'new'
+    reason: str = ''
+    state_version: int = 0
+    response: str = ''
+    canonical_response: str = ''
+    node_id: str | None = None
+    skill: str | None = None
+    event: str | None = None
+    next: str | None = None
+    mounted_tool: str | None = None
+    held_object: str | None = None
+    terminal: str | None = None
+    events: list[dict[str, Any]] = field(default_factory=list)
+    context: Any = None
+    cancel_requested: bool = False
+    done: threading.Event = field(default_factory=threading.Event)
+    thread: threading.Thread | None = None
+    details: dict[str, Any] = field(default_factory=dict)
+
+
+@dataclass(frozen=True)
+class RunStatus:
+    found: bool
+    run_id: str
+    state: str
+    reason: str
+    node_id: str | None
+    skill: str | None
+    event: str | None
+    next: str | None
+    terminal: str | None
+    mounted_tool: str | None
+    held_object: str | None
+    events: tuple[dict[str, Any], ...] = ()
+
+    def as_dict(self) -> dict[str, Any]:
+        return {
+            'found': self.found,
+            'run_id': self.run_id,
+            'state': self.state,
+            'reason': self.reason,
+            'node_id': self.node_id,
+            'skill': self.skill,
+            'event': self.event,
+            'next': self.next,
+            'terminal': self.terminal,
+            'mounted_tool': self.mounted_tool,
+            'held_object': self.held_object,
+            'events': list(self.events),
+        }
+
+    def to_json(self) -> str:
+        return json.dumps(self.as_dict(), ensure_ascii=False)
+
+
+@dataclass(frozen=True)
+class CancelResult:
+    accepted: bool
+    message: str
+    run_id: str
+
+
+def _canonical(graph: PlanGraph) -> str:
+    return json.dumps(
+        graph.as_dict(), sort_keys=True, ensure_ascii=False,
+        separators=(',', ':'),
+    )
+
+
+class GraphEngine:
+    def __init__(
+        self,
+        scene_provider: Callable[[], Mapping[str, Any] | None],
+        context_factory: Callable[[Mapping[str, Any], Mapping[str, Any]], Any],
+        skills: Any,
+        *,
+        catalog: SkillCatalog = DEFAULT_CATALOG,
+        clock: Callable[[], float] | None = None,
+        on_plan: Callable[[dict[str, Any]], None] | None = None,
+        on_status: Callable[[dict[str, Any]], None] | None = None,
+        on_replan: Callable[[dict[str, Any] | None], None] | None = None,
+        on_stop: Callable[[str], None] | None = None,
+        robot_fresh: Callable[[], tuple[bool, str]] | None = None,
+        threaded: bool = True,
+        min_start_delay_sec: float = DEFAULT_MIN_START_DELAY_SEC,
+        scene_max_age_sec: float = DEFAULT_SCENE_MAX_AGE_SEC,
+        max_steps: int = DEFAULT_MAX_STEPS,
+    ) -> None:
+        self.scene_provider = scene_provider
+        self.context_factory = context_factory
+        self.skills = skills
+        self.catalog = catalog
+        self._clock = clock or time.monotonic
+        self.on_plan = on_plan
+        self.on_status = on_status
+        self.on_replan = on_replan
+        self.on_stop = on_stop
+        self.robot_fresh = robot_fresh
+        self.threaded = bool(threaded)
+        self.min_start_delay_sec = float(min_start_delay_sec)
+        self.scene_max_age_sec = float(scene_max_age_sec)
+        self.max_steps = int(max_steps)
+        self.scene_ready = False
+        self.ready_at: float | None = None
+        self.motion_ready = False
+        self.readiness_reason = 'not ready'
+        self._runs: dict[str, RunRecord] = {}
+        self._lock = threading.RLock()
+
+    # ------------------------------------------------------------------ readiness
+    def update_readiness(
+        self,
+        *,
+        scene_ready: bool,
+        ready_at: float | None,
+        motion_ready: bool,
+        reason: str = '',
+    ) -> None:
+        self.scene_ready = bool(scene_ready)
+        self.ready_at = None if ready_at is None else float(ready_at)
+        self.motion_ready = bool(motion_ready)
+        self.readiness_reason = reason or (
+            'ready' if self.scene_ready and self.motion_ready else 'not ready'
+        )
+
+    def _not_ready(self, now: float) -> str | None:
+        if not self.scene_ready:
+            return f'scene is not ready: {self.readiness_reason}'
+        if not self.motion_ready:
+            return f'motion is not ready: {self.readiness_reason}'
+        if self.ready_at is None:
+            return 'scene_ready time is unknown'
+        remaining = self.min_start_delay_sec - (now - self.ready_at)
+        if remaining > 0.0:
+            return (
+                'start delay has not elapsed '
+                f'({remaining:.2f} s of {self.min_start_delay_sec:.1f} s left)'
+            )
+        return None
+
+    # ------------------------------------------------------------------ admission
+    def submit(
+        self,
+        run_id: str,
+        response_json: str | Mapping[str, Any],
+    ) -> AdmissionResult:
+        """Admit one graph against a fresh scene snapshot.
+
+        Nothing is registered until the format, readiness, freshness and
+        ownership checks have all passed and the accepted graph has been
+        published.  A rejected request therefore never occupies its run_id.
+        """
+        run_id = str(run_id).strip()
+        if not run_id:
+            return AdmissionResult(False, 'invalid', 'run_id must not be empty', '')
+        raw_response = (
+            response_json
+            if isinstance(response_json, str)
+            else json.dumps(response_json, ensure_ascii=False)
+        )
+        try:
+            graph = parse_response(raw_response)
+        except PlanSchemaError as error:
+            return AdmissionResult(False, 'invalid', str(error), run_id)
+        canonical = _canonical(graph)
+
+        with self._lock:
+            existing = self._runs.get(run_id)
+            if existing is not None:
+                if existing.canonical_response == canonical:
+                    return AdmissionResult(
+                        True, 'duplicate',
+                        f'run {run_id!r} was already admitted; no second motion',
+                        run_id,
+                    )
+                return AdmissionResult(
+                    False, 'conflict',
+                    f'run {run_id!r} is already bound to a different graph',
+                    run_id,
+                )
+            active = [
+                record for record in self._runs.values()
+                if record.state in ACTIVE_STATES
+            ]
+            if active:
+                return AdmissionResult(
+                    False, 'busy', 'another graph is already active', run_id,
+                )
+            now = self._clock()
+            reason = self._not_ready(now)
+            if reason is not None:
+                return AdmissionResult(False, 'not_ready', reason, run_id)
+            envelope = self._envelope()
+            if envelope is None:
+                return AdmissionResult(
+                    False, 'not_ready', 'no fresh scene snapshot', run_id,
+                )
+            stale = self._scene_age(envelope, now)
+            if stale is not None:
+                return AdmissionResult(False, 'stale_scene', stale, run_id)
+            validation: ValidationResult = validate_graph(
+                graph,
+                envelope,
+                self.catalog,
+            )
+            if not validation.ok:
+                return AdmissionResult(
+                    False, validation.code, validation.reason, run_id,
+                )
+            try:
+                if self.on_plan is not None:
+                    self.on_plan({
+                        'run_id': run_id,
+                        'response': raw_response,
+                    })
+            except Exception as error:
+                return AdmissionResult(
+                    False, 'rejected',
+                    f'plan publication failed: {error}', run_id,
+                )
+            record = RunRecord(
+                run_id=run_id,
+                state='admitted',
+                response=canonical,
+                canonical_response=canonical,
+                state_version=int(envelope.get('state_version', 0)),
+            )
+            self._runs[run_id] = record
+            if self.threaded:
+                thread = threading.Thread(
+                    target=self._execute,
+                    args=(run_id, graph, envelope),
+                    name=f'graph-run-{run_id}',
+                    daemon=True,
+                )
+                record.thread = thread
+                try:
+                    thread.start()
+                except Exception as error:
+                    # Startup failed before any motion: do not keep the run id
+                    # bound to a request that was never executed.
+                    self._runs.pop(run_id, None)
+                    return AdmissionResult(
+                        False, 'rejected',
+                        f'could not start the worker: {error}', run_id,
+                    )
+            return AdmissionResult(True, 'accepted', 'graph admitted', run_id)
+
+    # ------------------------------------------------------------------ helpers
+    def _envelope(self) -> Mapping[str, Any] | None:
+        try:
+            envelope = self.scene_provider()
+        except Exception:
+            return None
+        if not isinstance(envelope, Mapping):
+            return None
+        if not isinstance(envelope.get('scene_json'), Mapping):
+            return None
+        if 'state_version' not in envelope:
+            return None
+        return envelope
+
+    def _scene_age(self, envelope: Mapping[str, Any], now: float) -> str | None:
+        try:
+            stamp = float(envelope.get('timestamp'))
+        except (TypeError, ValueError):
+            return 'scene snapshot has no timestamp'
+        if now - stamp > self.scene_max_age_sec:
+            return (
+                f'scene snapshot is stale ({now - stamp:.2f} s > '
+                f'{self.scene_max_age_sec:.2f} s)'
+            )
+        return None
+
+    def _context(self, run: RunRecord, envelope: Mapping[str, Any]) -> Any:
+        context = self.context_factory(envelope, envelope['scene_json'])
+        initial = (envelope.get('scene_json') or {}).get('initial_state') or {}
+        if hasattr(context, 'mounted_tool'):
+            context.mounted_tool = initial.get('tool_mounted') or None
+        if hasattr(context, 'held_object'):
+            context.held_object = initial.get('held_object') or None
+        if hasattr(context, 'scene_revision'):
+            # Internal only: the executor never exposes this counter.
+            context.scene_revision = int(envelope.get('state_version', 0))
+        run.context = context
+        return context
+
+    def _status_payload(self, run: RunRecord, **extra) -> dict[str, Any]:
+        context = run.context
+        if context is not None:
+            if hasattr(context, 'mounted_tool'):
+                run.mounted_tool = context.mounted_tool
+            if hasattr(context, 'held_object'):
+                run.held_object = context.held_object
+        payload = {
+            'run_id': run.run_id,
+            'state': run.state,
+            'reason': run.reason,
+            'node_id': run.node_id,
+            'skill': run.skill,
+            'event': run.event,
+            'next': run.next,
+            'terminal': run.terminal,
+            'mounted_tool': run.mounted_tool,
+            'held_object': run.held_object,
+            'timestamp': self._clock(),
+        }
+        payload.update(extra)
+        return payload
+
+    def _emit_status(self, run: RunRecord, **extra) -> None:
+        payload = self._status_payload(run, **extra)
+        if self.on_status is not None:
+            try:
+                self.on_status(payload)
+            except Exception:
+                pass
+
+    def _record_event(
+        self,
+        run: RunRecord,
+        node,
+        event: str,
+        next_id: str,
+        outcome: Any,
+        *,
+        timing: dict[str, Any] | None = None,
+    ) -> None:
+        context = run.context
+        if context is not None:
+            if hasattr(context, 'mounted_tool'):
+                run.mounted_tool = context.mounted_tool
+            if hasattr(context, 'held_object'):
+                run.held_object = context.held_object
+        details = getattr(outcome, 'details', {}) or {}
+        entry = {
+            'run_id': run.run_id,
+            'node_id': node.id,
+            'skill': node.skill,
+            'event': event,
+            'reason': details.get('reason', ''),
+            'next': next_id,
+            'mounted_tool': run.mounted_tool,
+            'held_object': run.held_object,
+            'details': details,
+            'timestamp': self._clock(),
+        }
+        if timing:
+            entry['timing'] = timing
+        run.events.append(entry)
+        self._emit_status(run, event_detail=entry)
+
+    def _stop_motion(self, reason: str) -> None:
+        if self.on_stop is not None:
+            try:
+                self.on_stop(reason)
+            except Exception:
+                pass
+
+    def _terminal(self, run: RunRecord, state: str, reason: str) -> None:
+        run.state = state
+        run.reason = reason
+        run.terminal = state
+        if state in ('failed', 'canceled'):
+            self._stop_motion(reason)
+        self._emit_status(run, terminal_reason=reason)
+        run.done.set()
+
+    # ------------------------------------------------------------------ execution
+    def run_accepted(self, run_id: str, graph: PlanGraph | None = None,
+                     envelope: Mapping[str, Any] | None = None) -> None:
+        """Synchronously execute an already admitted run (tests, tools)."""
+        run = self._runs.get(str(run_id).strip())
+        if run is None or run.state != 'admitted':
+            return
+        graph = graph or parse_response(run.response)
+        envelope = envelope or self._envelope()
+        if envelope is None:
+            self._terminal(run, 'failed', 'scene snapshot is unavailable')
+            return
+        age = self._scene_age(envelope, self._clock())
+        if age is not None:
+            self._terminal(run, 'failed', age)
+            return
+        if int(envelope.get('state_version', -1)) != run.state_version:
+            self._terminal(
+                run, 'failed',
+                'scene changed before the first motion '
+                f'({run.state_version} -> {envelope.get("state_version")})',
+            )
+            return
+        self._execute(run.run_id, graph, envelope)
+
+    def _execute(self, run_id: str, graph: PlanGraph, first_envelope: Mapping[str, Any]) -> None:
+        with self._lock:
+            run = self._runs[run_id]
+            run.state = 'running'
+        try:
+            # Recheck immediately before any motion: the graph was admitted
+            # against this state version, and the worker must not start on a
+            # scene that changed or became stale in between.
+            now = self._clock()
+            current = self._envelope()
+            if current is None:
+                self._terminal(run, 'failed', 'fresh scene snapshot is unavailable')
+                return
+            age = self._scene_age(current, now)
+            if age is not None:
+                self._terminal(run, 'failed', age)
+                return
+            if int(current.get('state_version', -1)) != run.state_version:
+                self._terminal(
+                    run, 'failed',
+                    'scene changed before the first motion '
+                    f'({run.state_version} -> {current.get("state_version")})',
+                )
+                return
+            context = self._context(run, current)
+            envelope = current
+            node_id = graph.entry
+            steps = 0
+            while True:
+                step_started = time.monotonic()
+                if run.cancel_requested:
+                    self._terminal(run, 'canceled', 'cancel requested')
+                    return
+                if steps >= self.max_steps:
+                    self._terminal(
+                        run, 'failed',
+                        f'executed step limit of {self.max_steps} reached',
+                    )
+                    return
+                robot_started = time.monotonic()
+                if self.robot_fresh is not None:
+                    try:
+                        fresh, message = self.robot_fresh()
+                    except Exception as error:
+                        fresh, message = False, str(error)
+                    if not fresh:
+                        self._terminal(run, 'failed', f'robot state: {message}')
+                        return
+                robot_check_sec = time.monotonic() - robot_started
+                now = self._clock()
+                scene_started = time.monotonic()
+                current = self._envelope()
+                if current is None:
+                    self._terminal(run, 'failed', 'fresh scene snapshot is unavailable')
+                    return
+                age = self._scene_age(current, now)
+                if age is not None:
+                    self._terminal(run, 'failed', age)
+                    return
+                version = int(current.get('state_version', -1))
+                if version != run.state_version:
+                    previous_node = graph.node(node_id) if node_id in graph.ids else None
+                    acknowledged = (
+                        run.skill in OWNERSHIP_SKILLS
+                        and run.event == 'success'
+                        and previous_node is not None
+                        and version > run.state_version
+                    )
+                    if not acknowledged:
+                        self._terminal(
+                            run, 'failed',
+                            'scene state changed externally '
+                            f'({run.state_version} -> {version}); motion stopped',
+                        )
+                        return
+                    run.state_version = version
+                    if hasattr(context, 'scene_revision'):
+                        context.scene_revision = version
+                    envelope = current
+                scene_check_sec = time.monotonic() - scene_started
+                node = graph.node(node_id)
+                skill_started = time.monotonic()
+                outcome = self.skills.execute(node, context)
+                skill_sec = time.monotonic() - skill_started
+                timing = {
+                    'step': steps,
+                    'robot_check_sec': robot_check_sec,
+                    'scene_check_sec': scene_check_sec,
+                    'skill_sec': skill_sec,
+                    'step_sec': time.monotonic() - step_started,
+                }
+                event = str(outcome.event)
+                transitions = {
+                    transition.event: transition.next for transition in node.transitions
+                }
+                run.node_id = node.id
+                run.skill = node.skill
+                run.event = event
+                if event not in transitions:
+                    run.next = None
+                    self._record_event(
+                        run, node, event, '', outcome, timing=timing,
+                    )
+                    self._terminal(
+                        run, 'failed',
+                        f'unknown skill outcome {event!r} for node {node.id!r}',
+                    )
+                    return
+                next_id = transitions[event]
+                run.next = next_id
+                self._record_event(
+                    run, node, event, next_id, outcome, timing=timing,
+                )
+                steps += 1
+                if run.cancel_requested:
+                    self._terminal(run, 'canceled', 'cancel requested')
+                    return
+                if next_id in TERMINALS:
+                    if next_id == 'replan_required':
+                        fresh_scene = self._envelope()
+                        if self.on_replan is not None:
+                            try:
+                                self.on_replan(
+                                    None if fresh_scene is None else dict(fresh_scene),
+                                )
+                            except Exception:
+                                pass
+                    self._terminal(run, next_id, f'graph reached {next_id}')
+                    return
+                node_id = next_id
+        except Exception as error:
+            self._terminal(run, 'failed', f'execution error: {error}')
+
+    # ------------------------------------------------------------------ queries
+    def get_status(self, run_id: str) -> RunStatus:
+        run_id = str(run_id).strip()
+        with self._lock:
+            run = self._runs.get(run_id)
+            if run is None:
+                return RunStatus(
+                    False, run_id, 'unknown', 'run_id was not submitted',
+                    None, None, None, None, None, None, None, (),
+                )
+            return RunStatus(
+                True,
+                run.run_id,
+                run.state,
+                run.reason,
+                run.node_id,
+                run.skill,
+                run.event,
+                run.next,
+                run.terminal,
+                run.mounted_tool,
+                run.held_object,
+                tuple(run.events),
+            )
+
+    def cancel(self, run_id: str) -> CancelResult:
+        run_id = str(run_id).strip()
+        with self._lock:
+            run = self._runs.get(run_id)
+            if run is None:
+                return CancelResult(False, 'run_id is unknown', run_id)
+            if run.state not in ACTIVE_STATES:
+                return CancelResult(
+                    False, f'run is not active (state={run.state})', run_id,
+                )
+            run.cancel_requested = True
+            if run.state == 'admitted' and not self.threaded:
+                self._terminal(run, 'canceled', 'cancel requested')
+            self._stop_motion('cancel requested')
+            self._emit_status(run, cancel_requested=True)
+            return CancelResult(True, 'cancel requested', run_id)
+
+    def wait(self, run_id: str, timeout: float | None = None) -> bool:
+        run = self._runs.get(str(run_id).strip())
+        if run is None:
+            return False
+        return run.done.wait(timeout)
+
+    def active_run_id(self) -> str | None:
+        with self._lock:
+            for run in self._runs.values():
+                if run.state in ACTIVE_STATES:
+                    return run.run_id
+        return None

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/motion_policy.py

@@ -0,0 +1,329 @@
+"""Trajectory-level checks applied before anything reaches the controller.
+
+Every PTP/LIN plan is resampled and checked for joint limits, self/world
+collision, working orientation and joint jumps.  The old hard +/-90 deg joint-7
+corridor is only a preference; real joint limits remain authoritative.
+"""
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, field
+from pathlib import Path
+from typing import Any, Sequence
+
+import numpy as np
+import yaml
+
+from iiwa_experement.geometry import Transform
+
+
+class MotionPolicyError(ValueError):
+    """The limits file is missing or inconsistent."""
+
+
+@dataclass(frozen=True)
+class MotionLimits:
+    work_joint7_rad: float
+    joint7_corridor_rad: float
+    max_tilt_deg: float
+    max_joint_jump_rad: float
+    max_cartesian_jump_m: float
+    joint_state_max_age_sec: float
+    scene_max_age_sec: float
+    start_tolerance_rad: float = 0.35
+    min_clearance_m: float = 0.003
+    max_sample_step_rad: float = 0.01
+
+    @property
+    def joint7_min_rad(self) -> float:
+        # Kept for diagnostics/compatibility only.  It is no longer a hard
+        # rejection boundary: safe paths may leave the preferred +/-90 deg
+        # band while remaining inside the real joint limits.
+        return self.work_joint7_rad - self.joint7_corridor_rad
+
+    @property
+    def joint7_max_rad(self) -> float:
+        return self.work_joint7_rad + self.joint7_corridor_rad
+
+
+def _finite(value: Any, label: str) -> float:
+    if isinstance(value, bool) or not isinstance(value, (int, float)):
+        raise MotionPolicyError(f'{label} must be a finite number')
+    number = float(value)
+    if not math.isfinite(number):
+        raise MotionPolicyError(f'{label} must be a finite number')
+    return number
+
+
+def load_limits(path: Path | str) -> MotionLimits:
+    source = Path(path)
+    if not source.is_file():
+        raise FileNotFoundError(f'motion limits file does not exist: {source}')
+    raw = yaml.safe_load(source.read_text(encoding='utf-8'))
+    if not isinstance(raw, dict):
+        raise MotionPolicyError('motion limits must be a mapping')
+    if raw.get('schema_version') != 1:
+        raise MotionPolicyError('motion limits schema_version must be 1')
+    joint7 = raw.get('joint7') or {}
+    tcp = raw.get('tcp') or {}
+    trajectory = raw.get('trajectory') or {}
+    freshness = raw.get('freshness') or {}
+    corridor = _finite(joint7.get('corridor_rad'), 'joint7.corridor_rad')
+    tilt = _finite(tcp.get('max_tilt_deg'), 'tcp.max_tilt_deg')
+    jump = _finite(
+        trajectory.get('max_joint_jump_rad'), 'trajectory.max_joint_jump_rad',
+    )
+    cartesian = _finite(
+        trajectory.get('max_cartesian_jump_m'),
+        'trajectory.max_cartesian_jump_m',
+    )
+    joint_age = _finite(
+        freshness.get('joint_state_max_age_sec'),
+        'freshness.joint_state_max_age_sec',
+    )
+    scene_age = _finite(
+        freshness.get('scene_max_age_sec'), 'freshness.scene_max_age_sec',
+    )
+    collision = raw.get('collision') or {}
+    min_clearance = _finite(
+        collision.get('min_clearance_m', 0.003),
+        'collision.min_clearance_m',
+    )
+    if corridor <= 0 or tilt <= 0 or jump <= 0 or cartesian <= 0:
+        raise MotionPolicyError('motion limits must be positive')
+    if joint_age <= 0 or scene_age <= 0:
+        raise MotionPolicyError('freshness limits must be positive')
+    if min_clearance < 0:
+        raise MotionPolicyError('collision.min_clearance_m must be non-negative')
+    sample_step = _finite(
+        trajectory.get('max_sample_step_rad', 0.01),
+        'trajectory.max_sample_step_rad',
+    )
+    if sample_step <= 0:
+        raise MotionPolicyError('trajectory.max_sample_step_rad must be positive')
+    return MotionLimits(
+        work_joint7_rad=_finite(
+            joint7.get('work_reference_rad'), 'joint7.work_reference_rad',
+        ),
+        joint7_corridor_rad=corridor,
+        max_tilt_deg=tilt,
+        max_joint_jump_rad=jump,
+        max_cartesian_jump_m=cartesian,
+        joint_state_max_age_sec=joint_age,
+        scene_max_age_sec=scene_age,
+        start_tolerance_rad=_finite(
+            trajectory.get('start_tolerance_rad', 0.35),
+            'trajectory.start_tolerance_rad',
+        ),
+        min_clearance_m=min_clearance,
+        max_sample_step_rad=sample_step,
+    )
+
+
+@dataclass(frozen=True)
+class MotionCheck:
+    ok: bool
+    reason: str = ''
+    details: dict[str, Any] = field(default_factory=dict)
+
+    def as_dict(self) -> dict[str, Any]:
+        return {'ok': self.ok, 'reason': self.reason, 'details': dict(self.details)}
+
+
+def tcp_tilt_deg(pose: Transform) -> float:
+    matrix = pose.matrix
+    # The working direction is the tcp local +Z axis; it must point down.
+    z_axis = matrix[:, 2]
+    dot = float(np.clip(-z_axis[2], -1.0, 1.0))
+    return math.degrees(math.acos(dot))
+
+
+class MotionPolicy:
+    def __init__(
+        self,
+        limits: MotionLimits,
+        joint_limits: dict[str, tuple[float, float]] | None = None,
+    ) -> None:
+        self.limits = limits
+        self.joint_limits = dict(joint_limits or {})
+        self.joint_names = tuple(
+            f'joint{index}' for index in range(1, 8)
+        )
+
+    @property
+    def work_joint7(self) -> float:
+        return self.limits.work_joint7_rad
+
+    def check_sources(
+        self,
+        *,
+        joint_state_age: float | None,
+        scene_age: float | None,
+    ) -> MotionCheck:
+        for name, age, limit in (
+            ('joint_states', joint_state_age, self.limits.joint_state_max_age_sec),
+            ('planning_scene', scene_age, self.limits.scene_max_age_sec),
+        ):
+            if age is None:
+                return MotionCheck(False, f'{name} freshness is unknown')
+            if not math.isfinite(float(age)) or float(age) < 0.0:
+                return MotionCheck(False, f'{name} freshness is invalid')
+            if float(age) > limit:
+                return MotionCheck(
+                    False,
+                    f'{name} is stale ({float(age):.2f} s > {limit:.2f} s)',
+                )
+        return MotionCheck(True, 'fresh')
+
+    def validate_trajectory(
+        self,
+        points: Sequence[Sequence[float]],
+        tcp_poses: Sequence[Transform],
+        current_joints: Sequence[float] | None = None,
+        *,
+        start_tcp_pose: Transform | None = None,
+        held_object: str | None = None,
+    ) -> MotionCheck:
+        if not points:
+            return MotionCheck(False, 'trajectory is empty')
+        if len(points) != len(tcp_poses):
+            return MotionCheck(
+                False,
+                'every trajectory point needs a matching tcp pose',
+            )
+        parsed: list[tuple[float, ...]] = []
+        for index, point in enumerate(points):
+            values = tuple(float(value) for value in point)
+            if len(values) != 7:
+                return MotionCheck(
+                    False, f'point {index} must contain 7 joint values',
+                )
+            if any(not math.isfinite(value) for value in values):
+                return MotionCheck(False, f'point {index} has a non-finite value')
+            parsed.append(values)
+
+        if current_joints is not None:
+            current = tuple(float(value) for value in current_joints)
+            if len(current) != 7:
+                return MotionCheck(False, 'current joint state must have 7 values')
+            if any(
+                abs(a - b) > self.limits.start_tolerance_rad
+                for a, b in zip(parsed[0], current)
+            ):
+                return MotionCheck(
+                    False,
+                    'trajectory does not start at the current joint state',
+                )
+
+        min_j7 = min(point[6] for point in parsed)
+        max_j7 = max(point[6] for point in parsed)
+        j7_deviation = max(
+            abs(value - self.limits.work_joint7_rad) for value in (min_j7, max_j7)
+        )
+        epsilon = 1e-9
+        for index, values in enumerate(parsed):
+            for joint_index, value in enumerate(values):
+                name = self.joint_names[joint_index]
+                bounds = self.joint_limits.get(name)
+                if bounds is None:
+                    continue
+                low, high = float(bounds[0]), float(bounds[1])
+                if value < low - epsilon or value > high + epsilon:
+                    return MotionCheck(
+                        False,
+                        f'{name} at point {index} is {value:.4f} rad, '
+                        f'outside the joint limit [{low:.4f}, {high:.4f}]',
+                        {
+                            'joint': name,
+                            'value': value,
+                            'limit': [low, high],
+                            'point': index,
+                        },
+                    )
+
+        max_tilt = 0.0
+        start_tilt = (
+            tcp_tilt_deg(start_tcp_pose)
+            if isinstance(start_tcp_pose, Transform)
+            else 0.0
+        )
+        # A robot that starts outside the working posture (the calibrated home
+        # pose points the tip up) needs one empty-gripper reorientation to the
+        # downward working pose.  This exception is explicit and logged; J7,
+        # jumps and the final downward orientation are still checked.  Any
+        # motion with a held/mounted object is always fully tilt-checked.
+        reorientation = (
+            start_tilt > self.limits.max_tilt_deg + epsilon
+            and held_object is None
+        )
+        # Allow only floating-point roundoff; a planned path must obey the
+        # configured tilt limit rather than widening it by a visible angle.
+        tilt_epsilon_deg = 1e-6
+        tilt_limit_deg = self.limits.max_tilt_deg + tilt_epsilon_deg
+        for index, pose in enumerate(tcp_poses):
+            if not isinstance(pose, Transform):
+                return MotionCheck(False, f'tcp pose {index} is not a Transform')
+            tilt = tcp_tilt_deg(pose)
+            max_tilt = max(max_tilt, tilt)
+            if tilt > tilt_limit_deg and not reorientation:
+                return MotionCheck(
+                    False,
+                    f'tcp tilt at point {index} is {tilt:.2f} deg '
+                    f'(limit {self.limits.max_tilt_deg:.2f})',
+                    {'max_tilt_deg': tilt, 'point': index},
+                )
+        if reorientation:
+            final_tilt = tcp_tilt_deg(tcp_poses[-1])
+            if final_tilt > tilt_limit_deg:
+                return MotionCheck(
+                    False,
+                    'a reorientation motion must finish in the downward '
+                    f'working posture (final tilt {final_tilt:.2f} deg)',
+                    {'final_tilt_deg': final_tilt},
+                )
+
+        max_jump = 0.0
+        for index in range(1, len(parsed)):
+            jump = max(
+                abs(a - b) for a, b in zip(parsed[index], parsed[index - 1])
+            )
+            max_jump = max(max_jump, jump)
+            if jump > self.limits.max_joint_jump_rad + epsilon:
+                return MotionCheck(
+                    False,
+                    f'joint jump of {jump:.3f} rad before point {index} '
+                    f'(limit {self.limits.max_joint_jump_rad:.3f})',
+                    {'max_jump_rad': jump, 'point': index},
+                )
+
+        max_step = 0.0
+        for index in range(1, len(tcp_poses)):
+            step = float(
+                np.linalg.norm(
+                    tcp_poses[index].position - tcp_poses[index - 1].position
+                )
+            )
+            max_step = max(max_step, step)
+            if step > self.limits.max_cartesian_jump_m + epsilon:
+                return MotionCheck(
+                    False,
+                    f'cartesian jump of {step:.3f} m before point {index} '
+                    f'(limit {self.limits.max_cartesian_jump_m:.3f})',
+                    {'max_cartesian_step_m': step, 'point': index},
+                )
+
+        return MotionCheck(
+            True,
+            'trajectory accepted',
+            {
+                'points': len(parsed),
+                'min_j7': min_j7,
+                'max_j7': max_j7,
+                'max_j7_deviation_rad': j7_deviation,
+                'preferred_j7_rad': self.limits.work_joint7_rad,
+                'max_tilt_deg': max_tilt,
+                'reorientation': reorientation,
+                'max_joint_jump_rad': max_jump,
+                'max_cartesian_step_m': max_step,
+            },
+        )

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/moveit_backend.py

@@ -0,0 +1,742 @@
+"""Constrained MoveItPy adapter used by every graph skill.
+
+The adapter differs from the existing ``MoveToPose`` action on purpose: it
+seeds IK from the measured current joints, sets a joint-7 corridor and a
+working-orientation path constraint, checks the *whole* planned trajectory
+with :class:`MotionPolicy` and only then asks the trajectory execution manager
+to run it.
+"""
+from __future__ import annotations
+
+import math
+import time
+from contextlib import contextmanager
+from dataclasses import dataclass, field
+from typing import Any, Callable, Mapping
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+from iiwa_experement.geometry import GeometrySpec, Transform
+
+from .motion_policy import MotionCheck, MotionPolicy, tcp_tilt_deg
+
+
+class PlannerError(RuntimeError):
+    """The MoveIt plan could not be produced or executed."""
+
+
+@dataclass
+class PlannedMotion:
+    points: list[tuple[float, ...]]
+    tcp_poses: list[Transform]
+    current_joints: tuple[float, ...] | None = None
+    start_tcp_pose: Transform | None = None
+    handle: Any = None
+    details: dict[str, Any] = field(default_factory=dict)
+
+
+@dataclass(frozen=True)
+class MotionOutcome:
+    success: bool
+    code: str
+    message: str
+    details: dict[str, Any] = field(default_factory=dict)
+
+    def as_dict(self) -> dict[str, Any]:
+        return {
+            'success': self.success,
+            'code': self.code,
+            'message': self.message,
+            'details': dict(self.details),
+        }
+
+
+@contextmanager
+def _stage(timings: list[dict[str, Any]], name: str, **details):
+    started = time.monotonic()
+    try:
+        yield
+    finally:
+        finished = time.monotonic()
+        entry = {
+            'stage': name,
+            'started_at': started,
+            'finished_at': finished,
+            'duration_sec': finished - started,
+        }
+        entry.update(details)
+        timings.append(entry)
+
+
+_POSE_TO_MATRIX = None
+
+
+def _pose_message(transform: Transform, frame_id: str):
+    from geometry_msgs.msg import Pose, PoseStamped
+
+    quaternion = Rotation.from_matrix(transform.matrix).as_quat()
+    pose = Pose()
+    pose.position.x, pose.position.y, pose.position.z = (
+        float(transform.xyz[0]),
+        float(transform.xyz[1]),
+        float(transform.xyz[2]),
+    )
+    pose.orientation.x = float(quaternion[0])
+    pose.orientation.y = float(quaternion[1])
+    pose.orientation.z = float(quaternion[2])
+    pose.orientation.w = float(quaternion[3])
+    stamped = PoseStamped()
+    stamped.header.frame_id = frame_id
+    stamped.pose = pose
+    return stamped
+
+
+def _matrix_transform(matrix, parent: str, child: str) -> Transform:
+    values = np.asarray(matrix, dtype=float)
+    return Transform(
+        xyz=tuple(float(value) for value in values[:3, 3]),
+        rotation=tuple(
+            tuple(float(value) for value in row)
+            for row in values[:3, :3]
+        ),
+        parent=parent,
+        child=child,
+    )
+
+
+def _error_code_name(value: int | None) -> str:
+    if value is None:
+        return 'unknown'
+    from moveit_msgs.msg import MoveItErrorCodes
+
+    for name in dir(MoveItErrorCodes):
+        if name.isupper() and getattr(MoveItErrorCodes, name) == value:
+            return f'{name}({value})'
+    return f'code {value}'
+
+
+def _format_contacts(result: Any) -> str:
+    contacts = getattr(result, 'contacts', None)
+    pairs: list[str] = []
+    try:
+        for key in contacts.keys():
+            pairs.append(str(key))
+    except Exception:
+        try:
+            for item in contacts:
+                pairs.append(str(item))
+        except Exception:
+            pass
+    return ', '.join(sorted(set(pairs))) if pairs else 'unknown pair'
+
+
+class MoveItPyPlanner:
+    """Thin wrapper around MoveItPy with the constrained plan request."""
+
+    PHASES = {
+        # PTP: free-space changes between working points, objects, tools,
+        # placement points and safe alignments.  No J7 path constraint; the
+        # full sampled trajectory is checked afterwards.
+        'transfer': ('pilz_industrial_motion_planner', 'PTP', 0.25, 2.0),
+        'approach': ('pilz_industrial_motion_planner', 'PTP', 0.18, 2.0),
+        'observe': ('pilz_industrial_motion_planner', 'PTP', 0.18, 2.0),
+        'align': ('pilz_industrial_motion_planner', 'PTP', 0.10, 2.0),
+        'escape': ('pilz_industrial_motion_planner', 'PTP', 0.15, 2.0),
+        # LIN: axial contact/retreat and the continuous dispense contour.
+        'contact': ('pilz_industrial_motion_planner', 'LIN', 0.06, 2.0),
+        'lower': ('pilz_industrial_motion_planner', 'LIN', 0.06, 2.0),
+        'retreat': ('pilz_industrial_motion_planner', 'LIN', 0.12, 2.0),
+        'dispense': ('pilz_industrial_motion_planner', 'LIN', 0.09, 2.0),
+    }
+
+    def __init__(
+        self,
+        geometry: GeometrySpec,
+        policy: MotionPolicy,
+        *,
+        planning_group: str = 'iiwa_arm',
+        pose_link: str = 'tcp',
+        pose_frame: str = 'base_link',
+        moveit_factory: Callable[[], Any] | None = None,
+        joint_limits: Mapping[str, tuple[float, float]] | None = None,
+    ) -> None:
+        self.geometry = geometry
+        self.policy = policy
+        self.planning_group = planning_group
+        self.pose_link = pose_link
+        self.pose_frame = pose_frame
+        self._moveit_factory = moveit_factory
+        self.joint_limits = dict(joint_limits or {})
+        self._moveit = None
+        self._arm = None
+        self._model = None
+        self._group = None
+        self._scene_monitor = None
+        self._stopped = False
+
+    # ------------------------------------------------------------------ setup
+    def ensure_ready(self) -> None:
+        self._ensure()
+
+    def _ensure(self) -> None:
+        if self._arm is not None:
+            return
+        try:
+            if self._moveit_factory is not None:
+                self._moveit = self._moveit_factory()
+            else:
+                from moveit.planning import MoveItPy
+
+                self._moveit = MoveItPy(node_name='iiwa_llm_plan_executor_moveit')
+            self._model = self._moveit.get_robot_model()
+            self._arm = self._moveit.get_planning_component(self.planning_group)
+            self._group = self._model.get_joint_model_group(self.planning_group)
+            self._scene_monitor = self._moveit.get_planning_scene_monitor()
+        except Exception as error:
+            raise PlannerError(f'MoveItPy is unavailable: {error}') from error
+
+    def _goal_in_pose_frame(self, goal: Transform) -> Transform:
+        if goal.parent == self.pose_frame:
+            return goal
+        if goal.parent == 'world':
+            return self.geometry.world_to_base.inverse().compose_relaxed(goal)
+        raise PlannerError(
+            f'goal frame {goal.parent!r} is not world or {self.pose_frame!r}'
+        )
+
+    def _constraints(self, goal: Transform, *, include_orientation: bool = True):
+        del goal, include_orientation
+        from moveit_msgs.msg import Constraints
+
+        return Constraints()
+
+    def _parameters(self, phase: str):
+        from moveit.planning import PlanRequestParameters
+
+        pipeline, planner_id, speed, plan_time = self.PHASES.get(
+            phase, self.PHASES['transfer'],
+        )
+        parameters = PlanRequestParameters(self._moveit, self.planning_group)
+        parameters.planning_pipeline = pipeline
+        parameters.planner_id = planner_id
+        parameters.planning_time = plan_time
+        parameters.planning_attempts = 10 if pipeline == 'ompl' else 3
+        parameters.max_velocity_scaling_factor = speed
+        parameters.max_acceleration_scaling_factor = speed
+        return parameters
+
+    # ------------------------------------------------------------------ plan
+    def plan(
+        self,
+        goal: Transform,
+        phase: str = 'transfer',
+        held_object: str | None = None,
+    ) -> PlannedMotion:
+        self._ensure()
+        if self._stopped:
+            raise PlannerError('planner is stopped')
+        base_goal = self._goal_in_pose_frame(goal)
+        effective_phase = phase
+        print(
+            '[iiwa_llm_plan_executor] plan request '
+            f'phase={phase!r} goal_xyz={tuple(round(float(v), 6) for v in base_goal.xyz)} '
+            f'goal_quat={tuple(round(float(v), 6) for v in Rotation.from_matrix(base_goal.matrix).as_quat())}',
+            flush=True,
+        )
+        try:
+            self._arm.set_start_state_to_current_state()
+            start_state = self._arm.get_start_state()
+            names = list(self._group.active_joint_model_names)
+            measured = tuple(
+                float(value)
+                for value in start_state.get_joint_group_positions(
+                    self.planning_group,
+                )
+            )
+            bounds = list(self._group.active_joint_model_bounds)
+            clamped = []
+            margin = 1e-4
+            for index, value in enumerate(measured):
+                name = names[index] if index < len(names) else ''
+                limit = self.joint_limits.get(name)
+                if limit is not None:
+                    low, high = limit
+                else:
+                    try:
+                        low, high = bounds[index]
+                    except (IndexError, TypeError, ValueError):
+                        low, high = -3.2, 3.2
+                low = float(low) + margin
+                high = float(high) - margin
+                clamped.append(min(max(value, low), high))
+            if any(abs(a - b) > 1e-9 for a, b in zip(clamped, measured)):
+                print(
+                    '[iiwa_llm_plan_executor] clamping start state '
+                    f'measured={measured} clamped={clamped} bounds={bounds}',
+                    flush=True,
+                )
+            from moveit.core.robot_state import RobotState
+
+            clamped_state = RobotState(self._model)
+            clamped_state.set_joint_group_positions(
+                self.planning_group, clamped,
+            )
+            clamped_state.update()
+            self._arm.set_start_state(robot_state=clamped_state)
+            start_state = self._arm.get_start_state()
+            current = tuple(clamped)
+            print(
+                '[iiwa_llm_plan_executor] plan start '
+                f'measured={tuple(round(float(v), 6) for v in measured)} '
+                f'clamped={tuple(round(float(v), 6) for v in current)}',
+                flush=True,
+            )
+            start_matrix = start_state.get_global_link_transform(self.pose_link)
+            start_tcp = _matrix_transform(
+                start_matrix, self.pose_frame, self.pose_link,
+            )
+            reorientation = (
+                tcp_tilt_deg(start_tcp) > self.policy.limits.max_tilt_deg
+                and held_object is None
+            )
+            effective_phase = phase
+            # J7 has no artificial +/-90 deg corridor and PTP/LIN goal IK is
+            # allowed to use the whole joint range.  Safety comes from the
+            # dense post-plan joint/self/world/orientation checks below.
+            from moveit_msgs.msg import Constraints
+            self._arm.set_path_constraints(Constraints())
+            phase_params = self.PHASES.get(
+                effective_phase, self.PHASES['transfer'],
+            )
+            print(
+                '[iiwa_llm_plan_executor] plan pipeline '
+                f'phase={phase!r} pipeline={phase_params[0]!r} '
+                f'planner_id={phase_params[1]!r} planning_time={phase_params[3]}',
+                flush=True,
+            )
+            self._arm.set_goal_state(
+                pose_stamped_msg=_pose_message(base_goal, self.pose_frame),
+                pose_link=self.pose_link,
+            )
+            result = self._arm.plan(
+                single_plan_parameters=self._parameters(effective_phase),
+            )
+        except PlannerError:
+            raise
+        except Exception as error:
+            print(
+                '[iiwa_llm_plan_executor] plan exception '
+                f'phase={phase!r} effective_phase={effective_phase!r} '
+                f'error={error!r}',
+                flush=True,
+            )
+            raise PlannerError(f'constrained planning failed: {error}') from error
+        if result is None or result.trajectory is None:
+            error_code = (
+                None if result is None
+                else getattr(result.error_code, 'val', None)
+            )
+            error_name = _error_code_name(error_code)
+            print(
+                '[iiwa_llm_plan_executor] plan empty '
+                f'phase={phase!r} pipeline={effective_phase!r} '
+                f'error_code={error_name} goal_xyz={base_goal.xyz}',
+                flush=True,
+            )
+            raise PlannerError(
+                f'planning failed for phase {phase!r} '
+                f'({effective_phase}, error {error_name}); '
+                f'goal_xyz={tuple(round(float(v), 6) for v in base_goal.xyz)} '
+                f'start={tuple(round(float(v), 6) for v in current)}'
+            )
+        message = result.trajectory.get_robot_trajectory_msg()
+        names = list(message.joint_trajectory.joint_names)
+        expected = list(self._group.active_joint_model_names)
+        if set(expected) - set(names):
+            raise PlannerError(
+                f'planned trajectory misses joints {sorted(set(expected) - set(names))}'
+            )
+        index = {name: position for position, name in enumerate(names)}
+        raw_points: list[tuple[float, ...]] = []
+        for waypoint in message.joint_trajectory.points:
+            raw_points.append(tuple(
+                float(waypoint.positions[index[name]]) for name in expected
+            ))
+        points = self._densify(
+            raw_points, self.policy.limits.max_sample_step_rad,
+        )
+        poses: list[Transform] = []
+        from moveit.core.robot_state import RobotState
+
+        state = RobotState(self._model)
+        for ordered in points:
+            state.set_joint_group_positions(self.planning_group, ordered)
+            state.update()
+            matrix = state.get_global_link_transform(self.pose_link)
+            poses.append(_matrix_transform(matrix, self.pose_frame, self.pose_link))
+        clearance = self._check_collisions(points)
+        return PlannedMotion(
+            points=points,
+            tcp_poses=poses,
+            current_joints=current,
+            start_tcp_pose=start_tcp,
+            handle=result,
+            details={
+                'phase': phase,
+                'planner_phase': effective_phase,
+                'planner': self.PHASES.get(
+                    effective_phase, self.PHASES['transfer'],
+                )[:2],
+                'reorientation': reorientation,
+                'raw_points': len(raw_points),
+                'sampled_points': len(points),
+                'min_clearance_m': self.policy.limits.min_clearance_m,
+                'measured_clearance_m': clearance,
+            },
+        )
+
+    @staticmethod
+    def _densify(
+        points: list[tuple[float, ...]],
+        max_step: float,
+    ) -> list[tuple[float, ...]]:
+        if not points:
+            return []
+        result = [points[0]]
+        for previous, current_point in zip(points, points[1:]):
+            delta = max(
+                abs(a - b) for a, b in zip(current_point, previous)
+            )
+            steps = max(1, int(math.ceil(delta / max_step)))
+            for step in range(1, steps + 1):
+                fraction = step / steps
+                result.append(tuple(
+                    a + (b - a) * fraction
+                    for a, b in zip(previous, current_point)
+                ))
+        return result
+
+    def _check_collisions(self, points: list[tuple[float, ...]]) -> float | None:
+        if self._scene_monitor is None:
+            raise PlannerError('planning scene monitor is unavailable')
+        from moveit.core.collision_detection import (
+            CollisionRequest,
+            CollisionResult,
+        )
+        from moveit.core.robot_state import RobotState
+
+        requirement = float(self.policy.limits.min_clearance_m)
+        measured_min: float | None = None
+        state = RobotState(self._model)
+        with self._scene_monitor.read_only() as scene:
+            acm = getattr(scene, 'allowed_collision_matrix', None)
+            for index, sample in enumerate(points):
+                state.set_joint_group_positions(self.planning_group, sample)
+                state.update()
+                request = CollisionRequest()
+                request.distance = True
+                request.contacts = True
+                request.max_contacts = 20
+                request.max_contacts_per_pair = 2
+                collided = False
+                checked = False
+                last_error = None
+                distances: list[float] = []
+                contacts: list[str] = []
+                for function in (
+                    'check_collision_unpadded', 'check_self_collision',
+                ):
+                    for arguments in (
+                        (request, CollisionResult(), state, acm),
+                        (request, CollisionResult(), state),
+                    ):
+                        if acm is None and len(arguments) == 4:
+                            continue
+                        try:
+                            result = arguments[1]
+                            hit = bool(getattr(scene, function)(*arguments))
+                            checked = True
+                            collided = collided or hit
+                        except TypeError as error:
+                            last_error = error
+                            continue
+                        try:
+                            distances.append(float(result.distance))
+                        except Exception:
+                            pass
+                        formatted = _format_contacts(result)
+                        if formatted != 'unknown pair':
+                            contacts.append(formatted)
+                if not checked:
+                    raise PlannerError(
+                        'collision checking is unavailable: '
+                        f'{last_error}'
+                    )
+                distance = min(distances) if distances else None
+                if distance is not None:
+                    measured_min = (
+                        distance if measured_min is None
+                        else min(measured_min, distance)
+                    )
+                if collided or (
+                    distance is not None and distance < requirement
+                ):
+                    pairs = ', '.join(sorted(set(contacts))) or 'unknown pair'
+                    measured = (
+                        'unknown' if distance is None else f'{distance:.4f} m'
+                    )
+                    raise PlannerError(
+                        f'collision/clearance failure at sample {index}: '
+                        f'{pairs}; minimum distance {measured}, '
+                        f'required {requirement:.4f} m'
+                    )
+        return measured_min
+
+    def execute(self, planned: PlannedMotion) -> bool:
+        self._ensure()
+        try:
+            execution = self._moveit.execute(
+                planned.handle.trajectory, controllers=[],
+            )
+            return bool(execution)
+        except Exception as error:
+            raise PlannerError(f'trajectory execution failed: {error}') from error
+
+    def stop(self) -> None:
+        self._stopped = True
+        if self._moveit is None:
+            return
+        try:
+            self._moveit.get_trajectory_execution_manager().stop_execution()
+        except Exception:
+            pass
+
+    def reset_stop(self) -> None:
+        self._stopped = False
+
+
+class MoveItBackend:
+    """The single execution path used by every skill."""
+
+    def __init__(
+        self,
+        geometry: GeometrySpec,
+        policy: MotionPolicy,
+        *,
+        planner: Any | None = None,
+        freshness_guard: Callable[[], tuple[bool, str]] | None = None,
+        scene_guard: Callable[[], tuple[bool, str]] | None = None,
+        current_joints: Callable[[], tuple[float, ...] | None] | None = None,
+        before_plan: Callable[[], None] | None = None,
+        collision_checker: Callable[[Any], str | None] | None = None,
+        on_event: Callable[[dict[str, Any]], None] | None = None,
+    ) -> None:
+        self.geometry = geometry
+        self.policy = policy
+        self.planner = planner or MoveItPyPlanner(geometry, policy)
+        self._freshness_guard = freshness_guard
+        self._scene_guard = scene_guard
+        self._current_joints = current_joints
+        self._before_plan = before_plan
+        self._collision_checker = collision_checker
+        self._on_event = on_event
+        self.stopped = False
+        self.stop_reason: str | None = None
+        self._last_motion_end: float | None = None
+
+    def _emit(self, **payload) -> None:
+        if self._on_event is not None:
+            self._on_event(payload)
+
+    def _guard(self) -> MotionOutcome | None:
+        if self.stopped:
+            return MotionOutcome(
+                False, 'stopped',
+                f'motion is blocked after stop: {self.stop_reason}',
+            )
+        for name, guard in (
+            ('joint_states', self._freshness_guard),
+            ('planning_scene', self._scene_guard),
+        ):
+            if guard is None:
+                continue
+            try:
+                ok, message = guard()
+            except Exception as error:
+                return self._fail(f'{name} guard failed: {error}', 'stale_state')
+            if not ok:
+                return self._fail(message, 'stale_state')
+        return None
+
+    def _fail(self, message: str, code: str = 'failed', **details) -> MotionOutcome:
+        self.stop(message)
+        self._emit(event='motion_failed', code=code, message=message, **details)
+        return MotionOutcome(False, code, message, details)
+
+    def plan_and_execute(
+        self,
+        goal: Transform,
+        phase: str = 'transfer',
+        held_object: str | None = None,
+    ) -> MotionOutcome:
+        started = time.monotonic()
+        since_previous = (
+            None if self._last_motion_end is None
+            else max(0.0, started - self._last_motion_end)
+        )
+        timings: list[dict[str, Any]] = []
+        outcome: MotionOutcome | None = None
+        try:
+            outcome = self._plan_and_execute_impl(
+                goal, phase, held_object, timings,
+            )
+            return outcome
+        finally:
+            finished = time.monotonic()
+            self._emit(
+                event='motion_timing',
+                phase=phase,
+                held_object=held_object,
+                success=bool(outcome is not None and outcome.success),
+                started_at=started,
+                finished_at=finished,
+                total_duration_sec=finished - started,
+                since_previous_trajectory_sec=since_previous,
+                stages=timings,
+            )
+            if outcome is not None and outcome.success:
+                self._last_motion_end = finished
+
+    def _plan_and_execute_impl(
+        self,
+        goal: Transform,
+        phase: str,
+        held_object: str | None,
+        timings: list[dict[str, Any]],
+    ) -> MotionOutcome:
+        with _stage(timings, 'state_guard'):
+            blocked = self._guard()
+        if blocked is not None:
+            return blocked
+        if self._before_plan is not None:
+            try:
+                with _stage(timings, 'wait_stationary'):
+                    self._before_plan()
+            except Exception as error:
+                return self._fail(f'settle wait failed: {error}', 'stale_state')
+        planned = None
+        check: MotionCheck | None = None
+        last_error = ''
+        for attempt in range(3):
+            try:
+                with _stage(timings, 'moveit_plan', attempt=attempt):
+                    candidate = self.planner.plan(goal, phase, held_object)
+            except Exception as error:
+                last_error = str(error)
+                if attempt + 1 < 3:
+                    try:
+                        with _stage(timings, 'wait_stationary', attempt=attempt):
+                            self._before_plan and self._before_plan()
+                    except Exception:
+                        pass
+                    continue
+                return self._fail(last_error, 'planning_failed')
+            current = None
+            if self._current_joints is not None:
+                try:
+                    current = self._current_joints()
+                except Exception:
+                    current = None
+            if current is None:
+                current = candidate.current_joints
+            with _stage(timings, 'trajectory_check', attempt=attempt):
+                candidate_check = self.policy.validate_trajectory(
+                    candidate.points, candidate.tcp_poses, current,
+                    start_tcp_pose=candidate.start_tcp_pose,
+                    held_object=held_object,
+                )
+            if candidate_check.ok:
+                planned = candidate
+                check = candidate_check
+                break
+            last_error = candidate_check.reason
+            if attempt + 1 < 3:
+                try:
+                    with _stage(timings, 'wait_stationary', attempt=attempt):
+                        self._before_plan and self._before_plan()
+                except Exception:
+                    pass
+        if planned is None or check is None:
+            try:
+                self.planner.stop()
+            except Exception:
+                pass
+            return self._fail(
+                f'motion policy rejected every planned trajectory: {last_error}',
+                'motion_limit',
+            )
+        if self._collision_checker is not None:
+            try:
+                with _stage(timings, 'collision_check'):
+                    collision_reason = self._collision_checker(planned)
+            except Exception as error:
+                collision_reason = f'collision check failed: {error}'
+            if collision_reason:
+                try:
+                    self.planner.stop()
+                except Exception:
+                    pass
+                return self._fail(collision_reason, 'collision')
+        target = (
+            goal
+            if goal.parent == 'base_link'
+            else self.geometry.world_to_base.inverse().compose_relaxed(goal)
+        )
+        final = planned.tcp_poses[-1]
+        distance = float(np.linalg.norm(final.position - target.position))
+        cosine = (np.trace(final.matrix.T @ target.matrix) - 1.0) / 2.0
+        angle = float(math.acos(max(-1.0, min(1.0, cosine))))
+        if (
+            distance > self.geometry.position_tolerance
+            or angle > self.geometry.orientation_tolerance_rad
+        ):
+            return self._fail(
+                'planned final tcp is outside the goal tolerance '
+                f'({distance:.4f} m, {angle:.4f} rad)',
+                'goal_mismatch',
+            )
+        self._emit(
+            event='motion_planned', phase=phase, held_object=held_object,
+            planner_details=dict(planned.details), **check.details,
+        )
+        try:
+            with _stage(timings, 'controller_execution'):
+                executed = self.planner.execute(planned)
+        except Exception as error:
+            return self._fail(str(error), 'execution_failed')
+        if not executed:
+            return self._fail('trajectory execution did not succeed', 'execution_failed')
+        self._emit(event='motion_executed', phase=phase, **check.details)
+        return MotionOutcome(
+            True,
+            'ok',
+            'motion executed',
+            {'phase': phase, **check.details},
+        )
+
+    def stop(self, reason: str = 'stop requested') -> None:
+        self.stopped = True
+        if self.stop_reason is None:
+            self.stop_reason = reason
+        try:
+            self.planner.stop()
+        except Exception:
+            pass
+
+    def reset_stop(self) -> None:
+        self.stopped = False
+        self.stop_reason = None
+        reset = getattr(self.planner, 'reset_stop', None)
+        if reset is not None:
+            reset()

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py

@@ -0,0 +1,955 @@
+"""ROS facade for the graph engine: submit, plan, status, cancel, logs."""
+from __future__ import annotations
+
+import json
+import re
+import threading
+import time
+import uuid
+from pathlib import Path
+from typing import Any, Callable, Mapping
+
+import rclpy
+from ament_index_python.packages import get_package_share_directory
+from control_msgs.action import FollowJointTrajectory
+from rclpy.action import ActionClient
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor
+from rclpy.node import Node
+from rclpy.qos import (
+    DurabilityPolicy,
+    QoSProfile,
+    ReliabilityPolicy,
+    qos_profile_sensor_data,
+)
+from sensor_msgs.msg import Image, JointState
+from std_msgs.msg import String
+from std_srvs.srv import Trigger
+import webots_ros2_msgs.srv
+
+from iiwa_experement.geometry import GeometrySpec, load_geometry
+from iiwa_experement.planning_scene import (
+    PlanningSceneBridge,
+    attached_cap,
+    attached_screw,
+    attached_tool,
+    cap_world_box,
+    placed_cap_pose,
+    screw_object_id,
+)
+from iiwa_experement.scenario import ScenarioSpec, load_scenario
+from iiwa_msgs.srv import GetRunStatus, SubmitPlan
+from iiwa_scene_context.catalog import DEFAULT_CATALOG
+from iiwa_scene_context.node import SCENE_TOPIC
+
+from .engine import GraphEngine
+from .motion_policy import MotionPolicy, load_limits
+from .moveit_backend import MoveItBackend
+from .skill_context import SkillContext
+from .skills import SkillRegistry
+
+
+COMMAND_SERVICE = '/iiwa_experement/object_command'
+STATE_TOPIC = '/iiwa_experement/object_state'
+SCENE_SERVICE = '/iiwa_scene_context/get_scene'
+SUBMIT_SERVICE = '/iiwa_llm_plan_executor/submit'
+SUBMIT_PLAN_SERVICE = '/iiwa_llm_plan_executor/submit_plan'
+CANCEL_SERVICE = '/iiwa_llm_plan_executor/cancel'
+STATUS_SERVICE = '/iiwa_llm_plan_executor/get_status'
+PLAN_TOPIC = '/iiwa_llm_plan_executor/plan'
+STATUS_TOPIC = '/iiwa_llm_plan_executor/status'
+CONTROLLER_ACTION = '/iiwa_arm_controller/follow_joint_trajectory'
+DEFAULT_TRACE_DIR = '~/.ros/iiwa_llm_plan_executor/runs'
+
+
+def plan_qos() -> QoSProfile:
+    return QoSProfile(
+        depth=1,
+        reliability=ReliabilityPolicy.RELIABLE,
+        durability=DurabilityPolicy.TRANSIENT_LOCAL,
+    )
+
+
+def _safe_name(value: str) -> str:
+    return re.sub(r'[^A-Za-z0-9_.-]+', '_', value)[:80] or 'run'
+
+
+class RunLog:
+    """Machine-readable JSONL log, one file per run id."""
+
+    def __init__(self, directory: str) -> None:
+        self.directory = Path(directory).expanduser()
+        self._lock = threading.Lock()
+
+    def write(self, payload: Mapping[str, Any]) -> None:
+        try:
+            self.directory.mkdir(parents=True, exist_ok=True)
+            run_id = _safe_name(str(payload.get('run_id', 'run')))
+            path = self.directory / f'{run_id}.jsonl'
+            with self._lock, path.open('a', encoding='utf-8') as stream:
+                stream.write(json.dumps(payload, ensure_ascii=False) + '\n')
+        except Exception:
+            pass
+
+
+class SupervisorFacade:
+    """Thread-safe supervisor client that never spins its own node."""
+
+    def __init__(
+        self,
+        node: Node,
+        callback_group,
+        *,
+        on_timing: Callable[[dict[str, Any]], None] | None = None,
+    ) -> None:
+        self.node = node
+        self.on_timing = on_timing
+        self._lock = threading.Condition()
+        self._latest: dict[str, Any] | None = None
+        self._client = node.create_client(
+            webots_ros2_msgs.srv.SetString,
+            COMMAND_SERVICE,
+            callback_group=callback_group,
+        )
+        node.create_subscription(
+            String, STATE_TOPIC, self._on_state, 10,
+            callback_group=callback_group,
+        )
+
+    def _on_state(self, message: String) -> None:
+        try:
+            payload = json.loads(message.data)
+        except Exception:
+            return
+        if not isinstance(payload, dict):
+            return
+        with self._lock:
+            self._latest = payload
+            self._lock.notify_all()
+
+    def state(self) -> dict[str, Any]:
+        with self._lock:
+            return dict(self._latest or {})
+
+    # Alias used by the skills.
+    def latest_state(self) -> dict[str, Any]:
+        return self.state()
+
+    def latest(self) -> dict[str, Any]:
+        return self.state()
+
+    def command(
+        self,
+        op: str,
+        object_id: str = '',
+        *,
+        target=None,
+        target_name: str = '',
+    ) -> dict[str, Any]:
+        request_started = time.monotonic()
+        service_wait_started = time.monotonic()
+        if not self._client.service_is_ready():
+            if not self._client.wait_for_service(timeout_sec=5.0):
+                raise RuntimeError(f'{COMMAND_SERVICE} is unavailable')
+        service_wait_sec = time.monotonic() - service_wait_started
+        request_id = uuid.uuid4().hex
+        payload: dict[str, Any] = {
+            'request_id': request_id,
+            'op': op,
+            'object': object_id,
+            'target_name': target_name,
+        }
+        if target is not None:
+            payload['target'] = {
+                'xyz': [float(value) for value in target.xyz],
+                'rotation': [
+                    [float(value) for value in row] for row in target.rotation
+                ],
+            }
+        request = webots_ros2_msgs.srv.SetString.Request()
+        request.value = json.dumps(payload)
+        call_started = time.monotonic()
+        future = self._client.call_async(request)
+        done = threading.Event()
+        future.add_done_callback(lambda _: done.set())
+        if not done.wait(timeout=15.0):
+            raise RuntimeError(f'supervisor command {op} timed out')
+        service_call_sec = time.monotonic() - call_started
+        try:
+            response = future.result()
+        except Exception as error:
+            raise RuntimeError(f'supervisor command {op} failed: {error}') from error
+        accepted = bool(response.success)
+        result_started = time.monotonic()
+        state = self._wait_for_result(request_id, timeout=3.0)
+        result_wait_sec = time.monotonic() - result_started
+        last = (state or {}).get('last_command_result') or {}
+        if last.get('request_id') != request_id:
+            raise RuntimeError(
+                f'supervisor did not publish the result of {op}'
+            )
+        if not accepted or not last.get('accepted', False):
+            raise RuntimeError(
+                last.get('error') or f'supervisor rejected {op}'
+            )
+        if self.on_timing is not None:
+            try:
+                self.on_timing({
+                    'stage': 'supervisor_command',
+                    'op': op,
+                    'object': object_id,
+                    'started_at': request_started,
+                    'finished_at': time.monotonic(),
+                    'duration_sec': time.monotonic() - request_started,
+                    'service_wait_sec': service_wait_sec,
+                    'service_call_sec': service_call_sec,
+                    'result_wait_sec': result_wait_sec,
+                })
+            except Exception:
+                pass
+        return state
+
+    def _wait_for_result(self, request_id: str, timeout: float) -> dict[str, Any]:
+        deadline = time.monotonic() + timeout
+        with self._lock:
+            while True:
+                state = dict(self._latest or {})
+                last = state.get('last_command_result') or {}
+                if last.get('request_id') == request_id:
+                    return state
+                remaining = deadline - time.monotonic()
+                if remaining <= 0.0:
+                    return state
+                self._lock.wait(timeout=min(0.1, remaining))
+
+
+class SceneAdapter:
+    """Keep the MoveIt scene aligned with the supervisor's ownership state."""
+
+    def __init__(
+        self,
+        bridge: PlanningSceneBridge,
+        spec: ScenarioSpec,
+        geometry: GeometrySpec,
+        *,
+        on_timing: Callable[[dict[str, Any]], None] | None = None,
+    ) -> None:
+        self.bridge = bridge
+        self.spec = spec
+        self.geometry = geometry
+        self.on_timing = on_timing
+
+    def _sync(self) -> None:
+        started = time.monotonic()
+        self.bridge.assert_synchronized(*self.bridge.expected_sets())
+        if self.on_timing is not None:
+            try:
+                self.on_timing({
+                    'stage': 'planning_scene_sync',
+                    'started_at': started,
+                    'finished_at': time.monotonic(),
+                    'duration_sec': time.monotonic() - started,
+                })
+            except Exception:
+                pass
+
+    def attach_tool(self, tool: str) -> None:
+        self.bridge.attach(attached_tool(tool, self.geometry))
+        self._sync()
+
+    def dock_tool(self, tool: str) -> None:
+        self.bridge.restore_world(f'grant_{tool}')
+        self._sync()
+
+    def attach_cap(self) -> None:
+        self.bridge.attach(attached_cap(self.geometry))
+        self._sync()
+
+    def place_cap(self, seat) -> None:
+        self.bridge.add_world(
+            'grant_cap',
+            cap_world_box(self.geometry),
+            placed_cap_pose(seat, self.spec.body.yaw_deg),
+        )
+        self._sync()
+
+    def attach_screw(self, screw_id: str) -> None:
+        self.bridge.attach(attached_screw(screw_id, self.geometry))
+        self._sync()
+
+    def handoff_screw(self, screw_id: str) -> None:
+        self.bridge.remove(screw_object_id(screw_id))
+        self._sync()
+
+    # Hooks used before entering a contact so the carried geometry cannot
+    # collide with itself or with the target it is about to touch.
+    def prepare_grab_tool(self, tool: str) -> None:
+        self.bridge.remove(f'grant_{tool}')
+        self._sync()
+
+    def restore_tool(self, tool: str) -> None:
+        self.bridge.restore_world(f'grant_{tool}')
+        self._sync()
+
+    def prepare_grab_cap(self, object_id: str) -> None:
+        del object_id
+        self.bridge.remove('grant_cap')
+        self._sync()
+
+    def prepare_grab_screw(self, screw_id: str) -> None:
+        self.bridge.remove(screw_object_id(screw_id))
+        self._sync()
+
+    def restore_part(self, object_id: str) -> None:
+        name = 'grant_cap' if object_id == 'cap' else screw_object_id(object_id)
+        self.bridge.restore_world(name)
+        self._sync()
+
+    def prepare_dock_tool(self, tool: str) -> None:
+        self.bridge.remove(f'grant_{tool}')
+        self._sync()
+
+    def prepare_place_cap(self) -> None:
+        self.bridge.remove('grant_cap')
+        self._sync()
+
+    def prepare_handoff_screw(self, screw_id: str) -> None:
+        self.bridge.remove(screw_object_id(screw_id))
+        self._sync()
+
+
+class PlannerExecutorNode(Node):
+    def __init__(self) -> None:
+        super().__init__('iiwa_llm_plan_executor')
+        package_share = Path(get_package_share_directory('iiwa_experement'))
+        executor_share = Path(
+            get_package_share_directory('iiwa_llm_plan_executor')
+        )
+        self.declare_parameter('experiment_id', 20)
+        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(
+            'motion_limits_path',
+            str(executor_share / 'config' / 'motion_limits.yaml'),
+        )
+        self.declare_parameter(
+            'joint_limits_path',
+            str(
+                Path(get_package_share_directory('iiwa_config'))
+                / 'config' / 'moveit' / 'joint_limits.yaml'
+            ),
+        )
+        self.declare_parameter('start_delay_sec', 3.0)
+        self.declare_parameter('scene_max_age_sec', 2.0)
+        self.declare_parameter('max_steps', 512)
+        self.declare_parameter('planning_group', 'iiwa_arm')
+        self.declare_parameter('pose_link', 'tcp')
+        self.declare_parameter('close_inspection_objects', ['cap'])
+        self.declare_parameter('top_camera_topic', '/d455_top/image_raw/image_color')
+        self.declare_parameter('hand_camera_topic', '/camera/color/image_color')
+        self.declare_parameter('frame_timeout_sec', 2.0)
+        self.declare_parameter('controller_action', CONTROLLER_ACTION)
+        self.declare_parameter('trace_dir', DEFAULT_TRACE_DIR)
+
+        self.experiment_id = int(self.get_parameter('experiment_id').value)
+        self.spec = load_scenario(
+            self.experiment_id, str(self.get_parameter('scene_config').value),
+        )
+        self.geometry = load_geometry(
+            str(self.get_parameter('geometry_path').value),
+        )
+        self.limits = load_limits(
+            str(self.get_parameter('motion_limits_path').value),
+        )
+        self.policy = MotionPolicy(
+            self.limits, self._read_joint_limits(),
+        )
+        self.close_inspection = frozenset(
+            str(value) for value in self.get_parameter(
+                'close_inspection_objects',
+            ).value
+        )
+        self.frame_timeout = float(
+            self.get_parameter('frame_timeout_sec').value
+        )
+        self.scene_max_age = float(
+            self.get_parameter('scene_max_age_sec').value
+        )
+        self.top_topic = str(self.get_parameter('top_camera_topic').value)
+        self.hand_topic = str(self.get_parameter('hand_camera_topic').value)
+        self.log = RunLog(str(self.get_parameter('trace_dir').value))
+
+        self.callback_group = ReentrantCallbackGroup()
+        self._lock = threading.RLock()
+        self._latest_scene: dict[str, Any] | None = None
+        self._scene_ready_at: float | None = None
+        self._last_joint: tuple[float, ...] | None = None
+        self._last_joint_velocity: tuple[float, ...] | None = None
+        self._last_joint_at: float | None = None
+        self._moveit_ready = False
+        self._bridge_ready = False
+        self._motion_init_lock = threading.Lock()
+        self._last_plan: dict[str, Any] | None = None
+        self._last_status: dict[str, Any] | None = None
+        self._frame_conditions = {
+            self.top_topic: threading.Condition(),
+            self.hand_topic: threading.Condition(),
+        }
+        self._frame_counts = {self.top_topic: 0, self.hand_topic: 0}
+        self._frame_stamps: dict[str, float] = {}
+
+        # ------------------------------------------------------------ interfaces
+        self._plan_publisher = self.create_publisher(String, PLAN_TOPIC, plan_qos())
+        self._status_publisher = self.create_publisher(String, STATUS_TOPIC, 10)
+        self.create_service(
+            webots_ros2_msgs.srv.SetString,
+            SUBMIT_SERVICE,
+            self._handle_submit,
+            callback_group=self.callback_group,
+        )
+        self.create_service(
+            SubmitPlan,
+            SUBMIT_PLAN_SERVICE,
+            self._handle_submit_plan,
+            callback_group=self.callback_group,
+        )
+        self.create_service(
+            webots_ros2_msgs.srv.SetString,
+            CANCEL_SERVICE,
+            self._handle_cancel,
+            callback_group=self.callback_group,
+        )
+        self.create_service(
+            GetRunStatus,
+            STATUS_SERVICE,
+            self._handle_get_status,
+            callback_group=self.callback_group,
+        )
+        self.create_subscription(
+            String,
+            SCENE_TOPIC,
+            self._on_scene,
+            plan_qos(),
+            callback_group=self.callback_group,
+        )
+        self.create_subscription(
+            JointState,
+            '/joint_states',
+            self._on_joints,
+            10,
+            callback_group=self.callback_group,
+        )
+        for topic in (self.top_topic, self.hand_topic):
+            self.create_subscription(
+                Image,
+                topic,
+                lambda message, name=topic: self._on_image(name, message),
+                qos_profile_sensor_data,
+                callback_group=self.callback_group,
+            )
+
+        self.supervisor = SupervisorFacade(
+            self, self.callback_group, on_timing=self._on_timing,
+        )
+        self._scene_client = self.create_client(
+            Trigger, SCENE_SERVICE, callback_group=self.callback_group,
+        )
+        self._controller = ActionClient(
+            self, FollowJointTrajectory,
+            str(self.get_parameter('controller_action').value),
+            callback_group=self.callback_group,
+        )
+        self.bridge = PlanningSceneBridge(
+            node=self,
+            use_sim_time=True,
+            spin_fn=lambda timeout: time.sleep(max(0.0, float(timeout))),
+            on_timing=self._on_timing,
+        )
+        self.adapter = SceneAdapter(
+            self.bridge, self.spec, self.geometry, on_timing=self._on_timing,
+        )
+        self.motion = MoveItBackend(
+            self.geometry,
+            self.policy,
+            planner=self._make_planner(),
+            freshness_guard=self._robot_fresh,
+            scene_guard=self._scene_guard,
+            current_joints=lambda: self._last_joint,
+            before_plan=self._wait_stationary,
+            on_event=self._on_motion_event,
+        )
+        self.skills = SkillRegistry(
+            self.motion,
+            self.supervisor,
+            catalog=DEFAULT_CATALOG,
+            scene_adapter=self.adapter,
+        )
+        self.engine = GraphEngine(
+            self._scene_provider,
+            self._context_factory,
+            self.skills,
+            catalog=DEFAULT_CATALOG,
+            clock=self._now,
+            on_plan=self._publish_plan,
+            on_status=self._publish_status,
+            on_replan=self._on_replan,
+            on_stop=self._on_stop,
+            robot_fresh=self._robot_fresh,
+            threaded=True,
+            min_start_delay_sec=float(
+                self.get_parameter('start_delay_sec').value
+            ),
+            scene_max_age_sec=self.scene_max_age,
+            max_steps=int(self.get_parameter('max_steps').value),
+        )
+        self.create_timer(
+            0.5, self._readiness_tick, callback_group=self.callback_group,
+        )
+        self.get_logger().info(
+            f'graph executor started for experiment {self.experiment_id}; '
+            f'waiting for scene {SCENE_TOPIC}'
+        )
+
+    # ------------------------------------------------------------------ setup
+    def _read_joint_limits(self) -> dict[str, tuple[float, float]]:
+        import yaml
+
+        limits_path = Path(str(self.get_parameter('joint_limits_path').value))
+        joint_limits = {}
+        try:
+            raw = yaml.safe_load(limits_path.read_text(encoding='utf-8')) or {}
+            for name, entry in (raw.get('joint_limits') or {}).items():
+                if isinstance(entry, dict):
+                    low = max(
+                        float(entry['min_position']),
+                        float(entry.get('soft_min_position', entry['min_position'])),
+                    )
+                    high = min(
+                        float(entry['max_position']),
+                        float(entry.get('soft_max_position', entry['max_position'])),
+                    )
+                    joint_limits[name] = (low, high)
+        except Exception as error:
+            self.get_logger().warning(
+                f'falling back to model joint bounds: {error}'
+            )
+        return joint_limits
+
+    def _make_planner(self):
+        from .moveit_backend import MoveItPyPlanner
+
+        return MoveItPyPlanner(
+            self.geometry,
+            self.policy,
+            planning_group=str(self.get_parameter('planning_group').value),
+            pose_link=str(self.get_parameter('pose_link').value),
+            joint_limits=self._read_joint_limits(),
+        )
+
+    def _now(self) -> float:
+        return self.get_clock().now().nanoseconds / 1e9
+
+    def _on_joints(self, message: JointState) -> None:
+        if len(message.position) == 7:
+            self._last_joint = tuple(float(value) for value in message.position)
+            if len(message.velocity) == 7:
+                self._last_joint_velocity = tuple(
+                    float(value) for value in message.velocity
+                )
+            self._last_joint_at = self._now()
+
+    def _on_image(self, topic: str, message: Image) -> None:
+        condition = self._frame_conditions.get(topic)
+        if condition is None:
+            return
+        stamp = (
+            message.header.stamp.sec
+            + message.header.stamp.nanosec * 1e-9
+        )
+        if stamp <= 0.0:
+            stamp = self._now()
+        with condition:
+            self._frame_counts[topic] += 1
+            self._frame_stamps[topic] = stamp
+            condition.notify_all()
+
+    def _on_scene(self, message: String) -> None:
+        try:
+            envelope = json.loads(message.data)
+        except Exception:
+            return
+        if not isinstance(envelope, dict):
+            return
+        now = self._now()
+        with self._lock:
+            self._latest_scene = envelope
+            stamp = envelope.get('timestamp')
+            try:
+                stamp_value = float(stamp)
+            except (TypeError, ValueError):
+                stamp_value = None
+            # Ignore impossible future stamps from a stale/ghost publisher.
+            candidate = (
+                stamp_value
+                if stamp_value is not None and stamp_value <= now + 1.0
+                else now
+            )
+            if self._scene_ready_at is None or candidate < self._scene_ready_at:
+                self._scene_ready_at = candidate
+
+    def _scene_provider(self) -> dict[str, Any] | None:
+        with self._lock:
+            envelope = self._latest_scene
+        if envelope is None:
+            return None
+        try:
+            stamp = float(envelope.get('timestamp'))
+        except (TypeError, ValueError):
+            return None
+        if self._now() - stamp > self.scene_max_age:
+            return None
+        return envelope
+
+    def _robot_fresh(self) -> tuple[bool, str]:
+        if self._last_joint is None or self._last_joint_at is None:
+            return False, 'no /joint_states received yet'
+        age = self._now() - self._last_joint_at
+        if age > self.limits.joint_state_max_age_sec:
+            return False, f'/joint_states is stale ({age:.2f} s old)'
+        return True, 'ok'
+
+    def _wait_stationary(
+        self,
+        timeout_sec: float = 4.0,
+        tolerance: float = 0.01,
+    ) -> None:
+        start = time.monotonic()
+        stable_since = None
+        while time.monotonic() - start < timeout_sec:
+            velocities = self._last_joint_velocity
+            if velocities is not None and all(
+                abs(value) <= tolerance for value in velocities
+            ):
+                stable_since = stable_since or time.monotonic()
+                if time.monotonic() - stable_since >= 0.2:
+                    return
+            else:
+                stable_since = None
+            time.sleep(0.05)
+        raise RuntimeError(
+            f'robot did not become stationary within {timeout_sec:.2f} s'
+        )
+
+    def _scene_guard(self) -> tuple[bool, str]:
+        if not self._bridge_ready:
+            return False, 'planning scene bridge is not initialized'
+        started = time.monotonic()
+        try:
+            self.bridge.assert_synchronized(
+                *self.bridge.expected_sets(), timeout_sec=2.0,
+            )
+        except Exception as error:
+            return False, f'planning scene is not synchronized: {error}'
+        finally:
+            self._on_timing({
+                'stage': 'planning_scene_check',
+                'started_at': started,
+                'finished_at': time.monotonic(),
+                'duration_sec': time.monotonic() - started,
+            })
+        return True, 'ok'
+
+    def _ensure_motion_ready(self) -> None:
+        if self._moveit_ready and self._bridge_ready:
+            return
+        if not self._motion_init_lock.acquire(blocking=False):
+            return
+        try:
+            if not self._moveit_ready:
+                try:
+                    self.motion.planner.ensure_ready()
+                    self._moveit_ready = True
+                except Exception as error:
+                    self._moveit_ready = False
+                    self.get_logger().warning(f'MoveIt is not ready yet: {error}')
+            if self._moveit_ready and not self._bridge_ready:
+                try:
+                    self.bridge.initialize(self.spec, self.geometry)
+                    self._bridge_ready = True
+                except Exception as error:
+                    self._bridge_ready = False
+                    self.get_logger().warning(
+                        f'planning scene bridge is not ready yet: {error}'
+                    )
+        finally:
+            self._motion_init_lock.release()
+
+    def _readiness_tick(self) -> None:
+        self._ensure_motion_ready()
+        controller_ready = self._controller.server_is_ready()
+        fresh, _ = self._robot_fresh()
+        scene = self._scene_provider()
+        scene_ready = scene is not None
+        motion_ready = (
+            self._moveit_ready and self._bridge_ready and controller_ready
+            and fresh
+        )
+        self.engine.update_readiness(
+            scene_ready=scene_ready,
+            ready_at=self._scene_ready_at,
+            motion_ready=motion_ready,
+            reason=(
+                'ready' if scene_ready and motion_ready else
+                f'scene={scene_ready} moveit={self._moveit_ready} '
+                f'bridge={self._bridge_ready} controller={controller_ready} '
+                f'joints={fresh}'
+            ),
+        )
+
+    # ------------------------------------------------------------------ frames
+    def _frame_provider(self, name: str) -> float | None:
+        topic = self.top_topic if name == 'top' else self.hand_topic
+        condition = self._frame_conditions.get(topic)
+        if condition is None:
+            return None
+        started = time.monotonic()
+        deadline = started + self.frame_timeout
+        with condition:
+            baseline = self._frame_counts[topic]
+            while self._frame_counts[topic] <= baseline:
+                remaining = deadline - time.monotonic()
+                if remaining <= 0.0:
+                    self._on_timing({
+                        'stage': 'frame_capture',
+                        'camera': name,
+                        'started_at': started,
+                        'finished_at': time.monotonic(),
+                        'duration_sec': time.monotonic() - started,
+                        'captured': False,
+                    })
+                    return None
+                condition.wait(timeout=remaining)
+            self._on_timing({
+                'stage': 'frame_capture',
+                'camera': name,
+                'started_at': started,
+                'finished_at': time.monotonic(),
+                'duration_sec': time.monotonic() - started,
+                'captured': True,
+                'stamp': self._frame_stamps.get(topic),
+            })
+            return self._frame_stamps.get(topic)
+
+    # ------------------------------------------------------------------ engine
+    def _context_factory(self, envelope: Mapping[str, Any], scene_json) -> SkillContext:
+        del scene_json
+        context = SkillContext(
+            spec=self.spec,
+            geometry=self.geometry,
+            scene_revision=int(envelope.get('state_version', 0)),
+        )
+        context.frame_provider = self._frame_provider
+        context.scene_clear_provider = lambda: True
+        context.notifier = self._notify_operator
+        context.scene_adapter = self.adapter
+        context.close_inspection_objects = self.close_inspection
+        return context
+
+    def _publish_plan(self, payload: dict[str, Any]) -> None:
+        self._last_plan = dict(payload)
+        # Bind the timing/log context to the new run immediately; otherwise
+        # the first motion events of a run can be labelled with the previous
+        # run id until the first status message arrives.
+        self._last_status = {
+            'run_id': payload.get('run_id'),
+            'state': 'accepted',
+            'reason': '',
+        }
+        message = String()
+        message.data = json.dumps(payload, ensure_ascii=False)
+        self._plan_publisher.publish(message)
+        self.log.write({
+            'run_id': payload.get('run_id'),
+            'event': 'plan_accepted',
+            'response': payload.get('response'),
+        })
+        self.get_logger().info(
+            f"accepted graph for run {payload.get('run_id')}"
+        )
+
+    def _publish_status(self, payload: dict[str, Any]) -> None:
+        self._last_status = dict(payload)
+        message = String()
+        message.data = json.dumps(payload, ensure_ascii=False)
+        self._status_publisher.publish(message)
+        self.log.write(payload)
+
+    def _on_replan(self, envelope: dict[str, Any] | None) -> None:
+        run_id = (self._last_status or {}).get('run_id')
+        self.log.write({
+            'run_id': run_id,
+            'event': 'replan_required',
+        })
+        if envelope is not None:
+            self._publish_status({
+                'run_id': run_id,
+                'state': 'replan_scene',
+                'reason': 'fresh scene snapshot for the external planner',
+                'scene': envelope,
+                'timestamp': self._now(),
+            })
+
+    def _on_stop(self, reason: str) -> None:
+        self.motion.stop(reason)
+
+    def _current_run_id(self) -> str | None:
+        return (
+            (self._last_status or {}).get('run_id')
+            or (self._last_plan or {}).get('run_id')
+        )
+
+    def _on_timing(self, payload: dict[str, Any]) -> None:
+        """Write monotonic stage timings to the run JSONL log."""
+        self.log.write({
+            **payload,
+            'event': 'timing',
+            'run_id': self._current_run_id(),
+            'timestamp': self._now(),
+        })
+
+    def _on_motion_event(self, payload: dict[str, Any]) -> None:
+        self.log.write({
+            **payload,
+            'event': payload.get('event', 'motion'),
+            'run_id': self._current_run_id(),
+        })
+
+    def _notify_operator(self, reason: str, details: dict[str, Any]) -> bool:
+        self.get_logger().warning(f'operator required: {reason}')
+        self._publish_status({
+            'run_id': (self._last_status or {}).get('run_id'),
+            'state': 'operator_notification',
+            'reason': reason,
+            'details': details,
+            'timestamp': self._now(),
+        })
+        return True
+
+    # ------------------------------------------------------------------ services
+    def _submit_payload(self, run_id: str, graph):
+        """Shared admission path for both submit services."""
+        result = self.engine.submit(run_id, graph)
+        if result.accepted:
+            self.get_logger().info(
+                f'run {run_id}: {result.code} ({result.message})'
+            )
+        else:
+            self.get_logger().warning(
+                f'run {run_id} rejected: {result.code}: {result.message}'
+            )
+        return result
+
+    def _handle_submit(self, request, response):
+        """Legacy webots_ros2_msgs/SetString submission.
+
+        It is kept for compatibility and routes into the same admission path.
+        A legacy ``scene_revision`` field is ignored: admission always uses the
+        executor's own fresh snapshot.
+        """
+        try:
+            payload = json.loads(request.value)
+            if not isinstance(payload, dict):
+                raise ValueError('submit payload must be a JSON object')
+            run_id = str(payload.get('run_id', '')).strip()
+            graph = payload.get('response')
+            if not isinstance(graph, (str, dict)):
+                raise ValueError('response must be a JSON string or object')
+        except Exception as error:
+            response.success = False
+            self.get_logger().error(f'invalid submit request: {error}')
+            return response
+        result = self._submit_payload(run_id, graph)
+        response.success = bool(result.accepted)
+        return response
+
+    def _handle_submit_plan(self, request, response):
+        try:
+            run_id = str(request.run_id or '').strip()
+            graph = request.response
+            if not isinstance(graph, str) or not graph.strip():
+                raise ValueError('response must be a non-empty JSON string')
+        except Exception as error:
+            response.accepted = False
+            response.code = 'invalid'
+            response.reason = str(error)
+            response.status_json = ''
+            self.get_logger().error(f'invalid submit_plan request: {error}')
+            return response
+        result = self._submit_payload(run_id, graph)
+        response.accepted = bool(result.accepted)
+        response.code = result.code
+        response.reason = result.message
+        response.status_json = ''
+        if result.accepted:
+            status = self.engine.get_status(run_id)
+            if status.found:
+                response.status_json = status.to_json()
+        return response
+
+    def _handle_cancel(self, request, response):
+        try:
+            try:
+                payload = json.loads(request.value)
+                run_id = (
+                    str(payload.get('run_id', ''))
+                    if isinstance(payload, dict) else str(payload)
+                )
+            except json.JSONDecodeError:
+                run_id = str(request.value)
+            result = self.engine.cancel(run_id)
+            response.success = bool(result.accepted)
+        except Exception as error:
+            self.get_logger().error(f'cancel failed: {error}')
+            response.success = False
+        return response
+
+    def _handle_get_status(self, request, response):
+        status = self.engine.get_status(request.run_id)
+        response.found = bool(status.found)
+        response.status_json = status.to_json()
+        return response
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = None
+    exit_code = 0
+    try:
+        node = PlannerExecutorNode()
+        executor = MultiThreadedExecutor(num_threads=4)
+        executor.add_node(node)
+        executor.spin()
+    except (KeyboardInterrupt, ExternalShutdownException):
+        pass
+    except Exception as error:
+        print(f'[iiwa_llm_plan_executor] {error}')
+        exit_code = 1
+    finally:
+        if node is not None:
+            try:
+                node.motion.stop('node shutdown')
+            except Exception:
+                pass
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+    if exit_code:
+        raise SystemExit(exit_code)
+
+
+if __name__ == '__main__':
+    main()

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/schema.py

@@ -0,0 +1,251 @@
+"""Strict parsing of the externally supplied Qwen3 graph response."""
+from __future__ import annotations
+
+import json
+from dataclasses import dataclass, field
+from typing import Any, Mapping
+
+
+MAX_NODES = 256
+MAX_RESPONSE_BYTES = 200_000
+ALLOWED_STATUSES = ('ready', 'blocked')
+TERMINALS = ('completed', 'replan_required', 'operator_required')
+TOP_LEVEL_KEYS = ('status', 'entry', 'plan', 'transitions')
+NODE_KEYS = ('id', 'skill', 'object', 'target', 'transitions', 'params')
+
+
+class PlanSchemaError(ValueError):
+    """The response is not a well-formed plan graph."""
+
+
+@dataclass(frozen=True)
+class Transition:
+    event: str
+    next: str
+
+    def as_dict(self) -> dict[str, str]:
+        return {'event': self.event, 'next': self.next}
+
+
+@dataclass(frozen=True)
+class PlanNode:
+    id: str
+    skill: str
+    object: str = ''
+    target: str = ''
+    params: dict[str, Any] = field(default_factory=dict)
+    transitions: tuple[Transition, ...] = ()
+
+    def as_dict(self) -> dict[str, Any]:
+        payload: dict[str, Any] = {
+            'id': self.id,
+            'skill': self.skill,
+            'object': self.object,
+            'target': self.target,
+            'transitions': [entry.as_dict() for entry in self.transitions],
+        }
+        if self.params:
+            payload['params'] = dict(self.params)
+        return payload
+
+
+@dataclass(frozen=True)
+class PlanGraph:
+    status: str
+    entry: str
+    nodes: tuple[PlanNode, ...]
+    top_level_transitions: tuple[tuple[str, Transition], ...] = ()
+
+    def node(self, node_id: str) -> PlanNode:
+        for item in self.nodes:
+            if item.id == node_id:
+                return item
+        raise KeyError(node_id)
+
+    @property
+    def ids(self) -> tuple[str, ...]:
+        return tuple(item.id for item in self.nodes)
+
+    def as_dict(self) -> dict[str, Any]:
+        return {
+            'status': self.status,
+            'entry': self.entry,
+            'plan': [item.as_dict() for item in self.nodes],
+        }
+
+
+def _string(value: Any, label: str, *, allow_empty: bool = True) -> str:
+    if not isinstance(value, str):
+        raise PlanSchemaError(f'{label} must be a string')
+    text = value.strip()
+    if not allow_empty and not text:
+        raise PlanSchemaError(f'{label} must not be empty')
+    return text
+
+
+def _params(raw: Any, label: str) -> dict[str, Any]:
+    if raw is None:
+        return {}
+    if not isinstance(raw, Mapping):
+        raise PlanSchemaError(f'{label}.params must be an object')
+    result: dict[str, Any] = {}
+    for key, value in raw.items():
+        if not isinstance(key, str) or not key:
+            raise PlanSchemaError(f'{label}.params keys must be non-empty strings')
+        if isinstance(value, bool) or not isinstance(value, (int, float, str)):
+            raise PlanSchemaError(
+                f'{label}.params.{key} must be a number or string'
+            )
+        result[key] = value
+    return result
+
+
+def _transition(raw: Any, label: str) -> Transition:
+    if not isinstance(raw, Mapping):
+        raise PlanSchemaError(f'{label} must be an object')
+    extra = set(raw) - {'event', 'next'}
+    if extra:
+        raise PlanSchemaError(f'{label} has unknown keys {sorted(extra)}')
+    event = _string(raw.get('event'), f'{label}.event', allow_empty=False)
+    next_id = _string(raw.get('next'), f'{label}.next', allow_empty=False)
+    return Transition(event, next_id)
+
+
+def _node(raw: Any, index: int) -> PlanNode:
+    label = f'plan[{index}]'
+    if not isinstance(raw, Mapping):
+        raise PlanSchemaError(f'{label} must be an object')
+    extra = set(raw) - set(NODE_KEYS)
+    if extra:
+        raise PlanSchemaError(f'{label} has unknown keys {sorted(extra)}')
+    node_id = _string(raw.get('id'), f'{label}.id', allow_empty=False)
+    skill = _string(raw.get('skill'), f'{label}.skill', allow_empty=False)
+    obj = _string(raw.get('object', ''), f'{label}.object')
+    target = _string(raw.get('target', ''), f'{label}.target')
+    transitions_raw = raw.get('transitions', [])
+    if not isinstance(transitions_raw, list):
+        raise PlanSchemaError(f'{label}.transitions must be a list')
+    transitions: list[Transition] = []
+    seen: set[str] = set()
+    for t_index, entry in enumerate(transitions_raw):
+        transition = _transition(entry, f'{label}.transitions[{t_index}]')
+        if transition.event in seen:
+            raise PlanSchemaError(
+                f'{label} has duplicate event {transition.event!r}'
+            )
+        seen.add(transition.event)
+        transitions.append(transition)
+    return PlanNode(
+        id=node_id,
+        skill=skill,
+        object=obj,
+        target=target,
+        params=_params(raw.get('params'), label),
+        transitions=tuple(transitions),
+    )
+
+
+def _merge_top_level(
+    nodes: list[PlanNode],
+    raw: Any,
+) -> tuple[list[PlanNode], tuple[tuple[str, Transition], ...]]:
+    if raw is None:
+        return nodes, ()
+    if not isinstance(raw, list):
+        raise PlanSchemaError('transitions must be a list')
+    by_id = {node.id: node for node in nodes}
+    merged: dict[str, list[Transition]] = {
+        node.id: list(node.transitions) for node in nodes
+    }
+    top_level: list[tuple[str, Transition]] = []
+    for index, entry in enumerate(raw):
+        label = f'transitions[{index}]'
+        if not isinstance(entry, Mapping):
+            raise PlanSchemaError(f'{label} must be an object')
+        extra = set(entry) - {'from', 'event', 'to', 'next'}
+        if extra:
+            raise PlanSchemaError(f'{label} has unknown keys {sorted(extra)}')
+        source = _string(entry.get('from'), f'{label}.from', allow_empty=False)
+        next_id = entry.get('to', entry.get('next'))
+        transition = Transition(
+            _string(entry.get('event'), f'{label}.event', allow_empty=False),
+            _string(next_id, f'{label}.to', allow_empty=False),
+        )
+        if source not in by_id:
+            raise PlanSchemaError(f'{label}.from references unknown node {source!r}')
+        existing = {item.event for item in merged[source]}
+        if transition.event in existing:
+            raise PlanSchemaError(
+                f'{label}: node {source!r} already has event '
+                f'{transition.event!r}'
+            )
+        merged[source].append(transition)
+        top_level.append((source, transition))
+    result = [
+        PlanNode(
+            id=node.id,
+            skill=node.skill,
+            object=node.object,
+            target=node.target,
+            params=node.params,
+            transitions=tuple(merged[node.id]),
+        )
+        for node in nodes
+    ]
+    return result, tuple(top_level)
+
+
+def parse_response(raw: str | Mapping[str, Any]) -> PlanGraph:
+    """Parse a model response or raise :class:`PlanSchemaError`."""
+    if isinstance(raw, str):
+        if len(raw.encode('utf-8')) > MAX_RESPONSE_BYTES:
+            raise PlanSchemaError(
+                f'response exceeds {MAX_RESPONSE_BYTES} bytes'
+            )
+        try:
+            payload = json.loads(raw)
+        except json.JSONDecodeError as error:
+            raise PlanSchemaError(f'response is not valid JSON: {error}') from error
+    elif isinstance(raw, Mapping):
+        payload = dict(raw)
+    else:
+        raise PlanSchemaError('response must be a JSON string or object')
+    if not isinstance(payload, Mapping):
+        raise PlanSchemaError('response must be a JSON object')
+    extra = set(payload) - set(TOP_LEVEL_KEYS)
+    if extra:
+        raise PlanSchemaError(f'response has unknown keys {sorted(extra)}')
+    status = _string(payload.get('status'), 'status', allow_empty=False)
+    if status not in ALLOWED_STATUSES:
+        raise PlanSchemaError(
+            f'status must be one of {list(ALLOWED_STATUSES)}, got {status!r}'
+        )
+    entry = _string(payload.get('entry', ''), 'entry')
+    plan_raw = payload.get('plan')
+    if not isinstance(plan_raw, list):
+        raise PlanSchemaError('plan must be a list')
+    if len(plan_raw) > MAX_NODES:
+        raise PlanSchemaError(
+            f'plan contains {len(plan_raw)} nodes; limit is {MAX_NODES}'
+        )
+    nodes: list[PlanNode] = []
+    seen_ids: set[str] = set()
+    for index, raw_node in enumerate(plan_raw):
+        node = _node(raw_node, index)
+        if node.id in seen_ids:
+            raise PlanSchemaError(f'duplicate node id {node.id!r}')
+        if node.id in TERMINALS:
+            raise PlanSchemaError(
+                f'node id {node.id!r} is a reserved terminal'
+            )
+        seen_ids.add(node.id)
+        nodes.append(node)
+    nodes, top_level = _merge_top_level(nodes, payload.get('transitions'))
+    if status == 'ready':
+        if not nodes:
+            raise PlanSchemaError('a ready graph must contain at least one node')
+        if not entry:
+            raise PlanSchemaError('a ready graph must have a non-empty entry')
+    elif nodes or entry:
+        raise PlanSchemaError('a blocked response must not contain an executable plan')
+    return PlanGraph(status, entry, tuple(nodes), top_level)

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skill_context.py

@@ -0,0 +1,108 @@
+"""Execution context shared by the skill registry and the graph engine."""
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from typing import Any, Callable
+
+from iiwa_experement.geometry import GeometrySpec, Transform
+from iiwa_experement.scenario import ScenarioSpec
+
+
+@dataclass
+class SkillContext:
+    spec: ScenarioSpec
+    geometry: GeometrySpec
+    scene_revision: int = 0
+    mounted_tool: str | None = None
+    held_object: str | None = None
+    ownership_unknown: bool = False
+    approach_pose: Transform | None = None
+    approach_signature: tuple[str, str] | None = None
+    contact_pose: Transform | None = None
+    phase: str = 'free'  # free | approach | contact | lifted
+    transfer_target: str | None = None
+    last_verified_pose: Transform | None = None
+    cap_relative: Transform | None = None
+    supervisor_state: dict[str, Any] = field(default_factory=dict)
+    accepted_revisions: set[int] = field(default_factory=set)
+    close_inspection_objects: frozenset[str] = frozenset()
+    at_work: bool = False
+
+    # Injected sources, replaced in tests.
+    frame_provider: Callable[[str], float | None] | None = None
+    scene_clear_provider: Callable[[], bool] | None = None
+    notifier: Callable[[str, dict[str, Any]], bool] | None = None
+    scene_adapter: Any = None
+
+    # ------------------------------------------------------------------ phases
+    def mark_approach(self, pose: Transform, signature: tuple[str, str]) -> None:
+        self.approach_pose = pose
+        self.approach_signature = signature
+        self.contact_pose = None
+        self.phase = 'approach'
+
+    def mark_contact(self, pose: Transform) -> None:
+        self.contact_pose = pose
+        self.phase = 'contact'
+
+    def mark_lifted(self) -> None:
+        self.contact_pose = None
+        self.phase = 'lifted'
+
+    def reset_phase(self) -> None:
+        self.approach_pose = None
+        self.approach_signature = None
+        self.contact_pose = None
+        self.phase = 'free'
+        self.transfer_target = None
+
+    def approach_matches(self, signature: tuple[str, str]) -> bool:
+        return (
+            self.approach_pose is not None
+            and self.approach_signature == signature
+            and self.phase in ('approach', 'contact', 'lifted')
+        )
+
+    # ------------------------------------------------------------------ state
+    def update_from_state(self, state: dict[str, Any]) -> None:
+        if not isinstance(state, dict):
+            return
+        self.supervisor_state = state
+        mounted = state.get('mounted_tool')
+        held = state.get('held_object')
+        self.mounted_tool = mounted if isinstance(mounted, str) else None
+        self.held_object = held if isinstance(held, str) else None
+        version = state.get('version')
+        if isinstance(version, int):
+            self.accepted_revisions.add(self.scene_revision)
+        self.ownership_unknown = False
+
+    def capture_frame(self, name: str) -> float | None:
+        if self.frame_provider is None:
+            return None
+        try:
+            stamp = self.frame_provider(name)
+        except Exception:
+            return None
+        if stamp is None:
+            return None
+        try:
+            return float(stamp)
+        except (TypeError, ValueError):
+            return None
+
+    def scene_is_clear(self) -> bool:
+        if self.scene_clear_provider is None:
+            return True
+        try:
+            return bool(self.scene_clear_provider())
+        except Exception:
+            return False
+
+    def notify(self, reason: str, details: dict[str, Any]) -> bool:
+        if self.notifier is None:
+            return True
+        try:
+            return bool(self.notifier(reason, details))
+        except Exception:
+            return False

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py

@@ -0,0 +1,998 @@
+"""Skill registry: the only place where a graph node becomes robot motion."""
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from typing import Any
+
+import numpy as np
+
+from iiwa_experement.geometry import (
+    GeometrySpec,
+    Transform,
+    sealant_point,
+    tcp_goal,
+    world_target,
+)
+from iiwa_experement.scenario import ScenarioSpec
+from iiwa_scene_context.catalog import DEFAULT_CATALOG, SkillCatalog
+
+from .schema import PlanNode
+from .skill_context import SkillContext
+
+
+class SkillError(RuntimeError):
+    def __init__(self, event: str, message: str) -> None:
+        super().__init__(message)
+        self.event = event
+
+
+@dataclass(frozen=True)
+class SkillOutcome:
+    event: str
+    details: dict[str, Any] = field(default_factory=dict)
+
+    def as_dict(self) -> dict[str, Any]:
+        return {'event': self.event, 'details': dict(self.details)}
+
+
+def _translation(xyz) -> Transform:
+    return Transform.from_xyz_rpy(
+        [float(value) for value in xyz], [0.0, 0.0, 0.0],
+        parent='tcp', child='offset',
+    )
+
+
+def _above(goal: Transform, gap: float) -> Transform:
+    return goal.compose_relaxed(_translation([0.0, 0.0, -gap]))
+
+
+def _distance(first: Transform, second: Transform) -> float:
+    return float(np.linalg.norm(first.position - second.position))
+
+
+def _angle(first: Transform, second: Transform) -> float:
+    cosine = (np.trace(first.matrix.T @ second.matrix) - 1.0) / 2.0
+    return float(np.arccos(max(-1.0, min(1.0, cosine))))
+
+
+def _state_pose(entry: Any, child: str) -> Transform | None:
+    if not isinstance(entry, dict):
+        return None
+    xyz = entry.get('xyz')
+    rotation = entry.get('rotation')
+    if not isinstance(xyz, (list, tuple)) or len(xyz) != 3:
+        return None
+    if not isinstance(rotation, (list, tuple)) or len(rotation) != 3:
+        return None
+    try:
+        return Transform(
+            xyz=tuple(float(value) for value in xyz),
+            rotation=tuple(tuple(float(value) for value in row) for row in rotation),
+            parent='world',
+            child=child,
+        )
+    except (TypeError, ValueError):
+        return None
+
+
+def object_definition_name(object_id: str) -> str | None:
+    if object_id == 'cap':
+        return 'grant_cap'
+    if object_id.startswith('screw_') and object_id.split('_')[-1].isdigit():
+        return f'grant_screw_{int(object_id.split("_")[-1]):02d}'
+    for name in ('vacuum', 'magnetic_tool', 'sealant_tool', 'torque_tool'):
+        if object_id == name:
+            return f'grant_{name}'
+    return None
+
+
+class SkillRegistry:
+    def __init__(
+        self,
+        motion: Any,
+        supervisor: Any,
+        *,
+        catalog: SkillCatalog = DEFAULT_CATALOG,
+        scene_adapter: Any = None,
+    ) -> None:
+        self.motion = motion
+        self.supervisor = supervisor
+        self.catalog = catalog
+        self.scene_adapter = scene_adapter
+        self._handlers = {
+            'detect': self._detect,
+            'approach': self._approach_skill,
+            'lower': self._lower_skill,
+            'grab': self._grab,
+            'pick': self._grab,
+            'lift': self._lift,
+            'transfer': self._transfer,
+            'move_to_storage': self._move_to_storage,
+            'align': self._align,
+            'release': self._release,
+            'retreat': self._retreat,
+            'capture': self._capture,
+            'verify': self._verify,
+            'inspect': self._verify,
+            'stop': self._stop,
+            'wait_scene_clear': self._wait_scene_clear,
+            'inspect_state': self._inspect_state,
+            'notify_operator': self._notify_operator,
+            'dispense': self._dispense,
+            'dock_tool': self._dock_tool,
+            'handoff_screw': self._handoff_screw,
+        }
+
+    # ------------------------------------------------------------------ dispatch
+    def execute(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        handler = self._handlers.get(node.skill)
+        if handler is None:
+            return SkillOutcome('failed', {'reason': f'unknown skill {node.skill!r}'})
+        try:
+            return handler(node, context)
+        except SkillError as error:
+            return SkillOutcome(error.event, {'reason': str(error)})
+        except Exception as error:  # never turn an unexpected exception into success
+            return SkillOutcome('failed', {'reason': str(error)})
+
+    # ------------------------------------------------------------------ motion
+    def _state(self) -> dict[str, Any]:
+        try:
+            state = self.supervisor.latest()
+        except Exception:
+            return {}
+        return state if isinstance(state, dict) else {}
+
+    def _refresh_live_ownership(self, context: SkillContext) -> str | None:
+        """Read the tool and held object from the live supervisor.
+
+        Context memory is not authoritative between commands or runs: the
+        supervisor may have been changed by another accepted graph or by a
+        previous failed attempt.
+        """
+        state = self._state()
+        if not state:
+            return 'supervisor state is unavailable'
+        context.supervisor_state = state
+        mounted = state.get('mounted_tool')
+        held = state.get('held_object')
+        context.mounted_tool = mounted if isinstance(mounted, str) else None
+        context.held_object = held if isinstance(held, str) else None
+        return None
+
+    def _move(
+        self,
+        context: SkillContext,
+        goal: Transform,
+        phase: str,
+    ) -> Any:
+        lateral = phase in ('transfer', 'approach', 'align', 'observe', 'escape')
+        if (
+            lateral
+            and context.phase == 'contact'
+            and context.approach_pose is not None
+        ):
+            # A lateral PTP must never start at contact depth: first move
+            # straight up along the tool axis to the saved safe height.
+            retreat = self.motion.plan_and_execute(
+                context.approach_pose, 'retreat', context.held_object,
+            )
+            if not getattr(retreat, 'success', False):
+                raise SkillError(
+                    'failed',
+                    f'axial retreat before lateral motion failed: '
+                    f'{getattr(retreat, "message", "motion failed")}',
+                )
+            context.last_verified_pose = context.approach_pose
+            context.mark_lifted()
+        carried = context.held_object or context.mounted_tool
+        outcome = self.motion.plan_and_execute(goal, phase, carried)
+        if not getattr(outcome, 'success', False):
+            code = getattr(outcome, 'code', 'failed')
+            event = 'collision' if code == 'collision' else 'failed'
+            raise SkillError(
+                event, getattr(outcome, 'message', 'motion failed'),
+            )
+        context.last_verified_pose = goal
+        work = context.geometry.safe_poses['work']
+        context.at_work = (
+            _distance(goal, work) <= context.geometry.position_tolerance
+            and _angle(goal, work) <= context.geometry.orientation_tolerance_rad
+        )
+        return outcome
+
+    def _command(
+        self,
+        context: SkillContext,
+        op: str,
+        object_id: str = '',
+        *,
+        target: Transform | None = None,
+        target_name: str = '',
+        event: str = 'failed',
+    ) -> dict[str, Any]:
+        try:
+            state = self.supervisor.command(
+                op, object_id, target=target, target_name=target_name,
+            )
+        except Exception as error:
+            raise SkillError(event, str(error)) from error
+        if isinstance(state, dict):
+            context.update_from_state(state)
+        return state
+
+    # ------------------------------------------------------------------ geometry
+    def _live_pose(self, context: SkillContext, object_id: str) -> Transform:
+        state = self._state()
+        if object_id == 'cap':
+            pose = _state_pose((state.get('cap') or {}).get('pose'), 'cap')
+            if pose is not None:
+                return pose
+        if object_id.startswith('screw_'):
+            pose = _state_pose(
+                (state.get('screws') or {}).get(object_id, {}).get('pose'),
+                object_id,
+            )
+            if pose is not None:
+                return pose
+        if object_id in context.spec.tool_names:
+            pose = _state_pose(
+                (state.get('objects') or {}).get(f'grant_{object_id}'),
+                object_id,
+            )
+            if pose is not None:
+                return pose
+            pose = _state_pose(
+                (state.get('tools') or {}).get(object_id, {}).get('pose'),
+                object_id,
+            )
+            if pose is not None:
+                return pose
+        raise SkillError('failed', f'no live pose for {object_id!r}')
+
+    def _object_contact_goal(
+        self, context: SkillContext, object_id: str,
+    ) -> Transform:
+        if object_id in context.spec.tool_names:
+            return tcp_goal(
+                self._live_pose(context, object_id),
+                context.geometry.tool_tcp_to_nodes[object_id],
+            )
+        if object_id == 'cap':
+            goal = tcp_goal(
+                self._live_pose(context, object_id),
+                context.geometry.cap_tcp_to_node,
+            )
+            # The suction cup is rotationally symmetric.  A 180-degree wrist
+            # yaw offset keeps the cap grasp on the branch inside the hard
+            # joint-7 corridor; the measured relative pose makes the later
+            # placement use the same offset.
+            return goal.compose_relaxed(Transform.from_xyz_rpy(
+                [0.0, 0.0, 0.0], [0.0, 0.0, 180.0],
+                parent='tcp', child='cap_grasp_yaw',
+            ))
+        if object_id.startswith('screw_'):
+            return tcp_goal(
+                self._live_pose(context, object_id),
+                context.geometry.screw_tcp_to_node,
+            )
+        raise SkillError('failed', f'object {object_id!r} cannot be approached')
+
+    def _destination_goal(
+        self,
+        context: SkillContext,
+        object_id: str,
+        target: str,
+    ) -> Transform:
+        if target in ('', 'work'):
+            return context.geometry.safe_poses['work']
+        if target == 'hatch_seat':
+            seat = world_target(context.spec, context.geometry, 'cap_seat')
+            relative = context.cap_relative or context.geometry.cap_tcp_to_node
+            return tcp_goal(seat, relative)
+        if target.startswith('hole_'):
+            return tcp_goal(
+                world_target(context.spec, context.geometry, f'hole:{target}'),
+                context.geometry.screw_tcp_to_node,
+            )
+        if target.endswith('_socket'):
+            tool = target[: -len('_socket')]
+            return tcp_goal(
+                world_target(context.spec, context.geometry, f'tool:{tool}'),
+                context.geometry.tool_tcp_to_nodes[tool],
+            )
+        if target in context.spec.sealant_sides:
+            point = sealant_point(
+                context.geometry, context.spec, target, 0.0,
+            )
+            return tcp_goal(
+                point, context.geometry.tool_work_tips['sealant_tool'],
+            )
+        raise SkillError('failed', f'unknown target {target!r}')
+
+    def _contact_goal(
+        self,
+        context: SkillContext,
+        object_id: str,
+        target: str,
+    ) -> Transform:
+        if target in context.spec.sealant_sides:
+            return self._destination_goal(context, object_id, target)
+        if target and target != 'work':
+            return self._destination_goal(context, object_id, target)
+        return self._object_contact_goal(context, object_id)
+
+    def _signature(self, node: PlanNode) -> tuple[str, str]:
+        return (node.object, node.target)
+
+    def _ensure_approach(
+        self,
+        context: SkillContext,
+        node: PlanNode,
+        *,
+        gap: float | None = None,
+    ) -> Transform:
+        signature = self._signature(node)
+        if context.approach_matches(signature):
+            return context.approach_pose  # type: ignore[return-value]
+        goal = self._contact_goal(context, node.object, node.target)
+        safe = _above(
+            goal,
+            context.geometry.approach_gap if gap is None else gap,
+        )
+        self._move(context, safe, 'approach')
+        context.mark_approach(safe, signature)
+        return safe
+
+    def _ensure_contact(
+        self,
+        context: SkillContext,
+        node: PlanNode,
+    ) -> Transform:
+        self._ensure_approach(context, node)
+        if context.phase == 'contact' and context.contact_pose is not None:
+            return context.contact_pose
+        goal = self._contact_goal(context, node.object, node.target)
+        self._move(context, goal, 'lower')
+        context.mark_contact(goal)
+        return goal
+
+    def _lift_to_approach(
+        self,
+        context: SkillContext,
+        *,
+        label: str = 'lift',
+    ) -> None:
+        if context.phase == 'lifted':
+            return
+        if context.approach_pose is None:
+            raise SkillError('failed', f'{label}: saved approach pose is unknown')
+        self._move(context, context.approach_pose, 'retreat')
+        context.mark_lifted()
+
+    def _adapter(self, context: SkillContext) -> Any:
+        return context.scene_adapter or self.scene_adapter
+
+    # ------------------------------------------------------------------ watch
+    def _detect(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        # An axial withdrawal is mandatory before leaving a contact.
+        if context.phase == 'contact' and context.approach_pose is not None:
+            self._move(context, context.approach_pose, 'retreat')
+            context.mark_lifted()
+        work = context.geometry.safe_poses['work']
+        if not (context.at_work and context.phase == 'free'):
+            self._move(context, work, 'transfer')
+        context.reset_phase()
+        context.at_work = True
+        if context.capture_frame('top') is None:
+            return SkillOutcome('failed', {'reason': 'no fresh top camera frame'})
+        if node.object in context.close_inspection_objects:
+            if context.capture_frame('hand') is None:
+                return SkillOutcome(
+                    'failed', {'reason': 'no fresh close-inspection frame'},
+                )
+        found = self._object_present(context, node.object)
+        if not found:
+            return SkillOutcome('not_found', {'object': node.object})
+        pose = None
+        try:
+            pose = self._live_pose(context, node.object)
+            context.last_verified_pose = pose
+        except Exception:
+            pose = None
+        return SkillOutcome('found', {
+            'object': node.object,
+            'tcp': list(pose.xyz) if pose is not None else None,
+            'close_inspection': node.object in context.close_inspection_objects,
+        })
+
+    def _object_present(self, context: SkillContext, object_id: str) -> bool:
+        effective = object_id or ''
+        if effective in context.spec.tool_names:
+            return True
+        if effective == 'cap':
+            if context.spec.cap_state == 'absent':
+                return False
+            state = (self._state().get('cap') or {}).get('state', 'free')
+            return state != 'absent'
+        if effective.startswith('screw_'):
+            if not any(screw.name == effective for screw in context.spec.screws):
+                return False
+            state = (
+                (self._state().get('screws') or {}).get(effective, {}).get(
+                    'state', 'tray',
+                )
+            )
+            return state != 'delivered'
+        try:
+            self.catalog.destination_kind(effective, context.spec)
+            return True
+        except Exception:
+            return False
+
+    def _capture(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        stamp = context.capture_frame(node.object)
+        if stamp is None:
+            return SkillOutcome('failed', {'camera': node.object})
+        return SkillOutcome('success', {'camera': node.object, 'stamp': stamp})
+
+    # ------------------------------------------------------------------ phases
+    def _approach_skill(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        safe = self._ensure_approach(context, node)
+        return SkillOutcome('success', {
+            'object': node.object, 'target': node.target,
+            'approach_height': context.geometry.approach_gap,
+            'tcp': list(safe.xyz),
+        })
+
+    def _lower_skill(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        goal = self._ensure_contact(context, node)
+        return SkillOutcome('success', {
+            'object': node.object, 'target': node.target,
+            'tcp': list(goal.xyz),
+        })
+
+    def _lift(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        if context.phase == 'lifted':
+            return SkillOutcome('success', {'reused': True})
+        self._ensure_approach(context, node)
+        goal = context.approach_pose
+        self._lift_to_approach(context)
+        return SkillOutcome('success', {
+            'reused': False,
+            'tcp': list(goal.xyz) if goal is not None else None,
+        })
+
+    # ------------------------------------------------------------------ grab
+    def _grab(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        obj = node.object
+        unavailable = self._refresh_live_ownership(context)
+        if unavailable is not None:
+            return SkillOutcome('grasp_failed', {'reason': unavailable})
+        if obj in context.spec.tool_names:
+            return self._grab_tool(node, context)
+        if obj == 'cap':
+            if context.mounted_tool != 'vacuum':
+                return SkillOutcome('grasp_failed', {
+                    'reason': 'required tool vacuum is not mounted',
+                    'required_tool': 'vacuum',
+                    'mounted_tool': context.mounted_tool,
+                })
+            if context.held_object is not None:
+                return SkillOutcome('grasp_failed', {
+                    'reason': f'hand is not free: {context.held_object!r} is held',
+                    'held_object': context.held_object,
+                })
+            return self._grab_part(node, context, 'grab_cap', 'vacuum')
+        if obj.startswith('screw_'):
+            if context.mounted_tool != 'magnetic_tool':
+                return SkillOutcome('grasp_failed', {
+                    'reason': 'required tool magnetic_tool is not mounted',
+                    'required_tool': 'magnetic_tool',
+                    'mounted_tool': context.mounted_tool,
+                })
+            if context.held_object is not None:
+                return SkillOutcome('grasp_failed', {
+                    'reason': f'hand is not free: {context.held_object!r} is held',
+                    'held_object': context.held_object,
+                })
+            return self._grab_part(
+                node, context, 'grab_screw', 'magnetic_tool',
+            )
+        return SkillOutcome('failed', {'reason': f'{obj!r} cannot be grasped'})
+
+    def _grab_tool(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        tool = node.object
+        if context.mounted_tool == tool:
+            return SkillOutcome('success', {'reused': True, 'tool': tool})
+        adapter = self._adapter(context)
+        prepare = getattr(adapter, 'prepare_grab_tool', None)
+        self._ensure_approach(context, node)
+        if prepare is not None:
+            prepare(tool)
+        try:
+            self._ensure_contact(context, node)
+        except Exception:
+            restore = getattr(adapter, 'restore_tool', None)
+            if prepare is not None and restore is not None:
+                restore(tool)
+            raise
+        try:
+            self._command(
+                context, 'attach_tool', tool, event='grasp_failed',
+            )
+        except SkillError as error:
+            context.ownership_unknown = True
+            restore = getattr(adapter, 'restore_tool', None)
+            if restore is not None:
+                try:
+                    restore(tool)
+                except Exception:
+                    pass
+            return SkillOutcome('grasp_failed', {'reason': str(error)})
+        context.mounted_tool = tool
+        context.held_object = None
+        context.ownership_unknown = False
+        if adapter is not None:
+            try:
+                adapter.attach_tool(tool)
+            except Exception as error:
+                return SkillOutcome('failed', {'reason': f'planning scene: {error}'})
+        self._lift_to_approach(context)
+        pose = context.approach_pose
+        return SkillOutcome('success', {
+            'tool': tool,
+            'tcp': list(pose.xyz) if pose is not None else None,
+        })
+
+    def _grab_part(
+        self,
+        node: PlanNode,
+        context: SkillContext,
+        command: str,
+        tool: str,
+    ) -> SkillOutcome:
+        adapter = self._adapter(context)
+        if node.object == 'cap':
+            prepare = getattr(adapter, 'prepare_grab_cap', None)
+        else:
+            prepare = getattr(adapter, 'prepare_grab_screw', None)
+        self._ensure_approach(context, node)
+        if prepare is not None:
+            prepare(node.object)
+        try:
+            contact = self._ensure_contact(context, node)
+        except Exception:
+            restore = getattr(adapter, 'restore_part', None)
+            if prepare is not None and restore is not None:
+                restore(node.object)
+            raise
+        try:
+            self._command(context, command, node.object, event='grasp_failed')
+        except SkillError as error:
+            context.ownership_unknown = True
+            restore = getattr(adapter, 'restore_part', None)
+            if restore is not None:
+                try:
+                    restore(node.object)
+                except Exception:
+                    pass
+            return SkillOutcome('grasp_failed', {'reason': str(error)})
+        context.held_object = node.object
+        context.mounted_tool = tool
+        context.ownership_unknown = False
+        if node.object == 'cap':
+            cap_pose = self._live_pose(context, node.object)
+            context.cap_relative = contact.inverse().compose_relaxed(cap_pose)
+        if adapter is not None:
+            try:
+                if node.object == 'cap':
+                    adapter.attach_cap()
+                else:
+                    adapter.attach_screw(node.object)
+            except Exception as error:
+                return SkillOutcome('failed', {'reason': f'planning scene: {error}'})
+        self._lift_to_approach(context)
+        return SkillOutcome('success', {
+            'object': node.object, 'tcp': list(contact.xyz),
+        })
+
+    # ------------------------------------------------------------------ transfer
+    def _transfer(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        obj = node.object
+        target = node.target
+        unavailable = self._refresh_live_ownership(context)
+        if unavailable is not None:
+            return SkillOutcome('failed', {'reason': unavailable})
+        if obj in context.spec.tool_names:
+            if context.mounted_tool != obj:
+                return SkillOutcome('failed', {
+                    'reason': f'{obj!r} is not mounted',
+                })
+        elif obj == 'cap':
+            if context.held_object != 'cap':
+                return SkillOutcome('failed', {'reason': 'hatch is not held'})
+        elif obj.startswith('screw_'):
+            if context.held_object != obj:
+                return SkillOutcome('failed', {'reason': f'{obj!r} is not held'})
+        else:
+            return SkillOutcome('failed', {'reason': f'{obj!r} cannot be transferred'})
+        goal = self._destination_goal(context, obj, target)
+        safe = _above(goal, context.geometry.transfer_gap)
+        self._move(context, safe, 'transfer')
+        context.transfer_target = target
+        context.mark_approach(safe, (obj, target))
+        return SkillOutcome('success', {
+            'object': obj, 'target': target,
+            'approach_height': context.geometry.transfer_gap,
+            'tcp': list(safe.xyz),
+        })
+
+    def _move_to_storage(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        if context.held_object is None:
+            return SkillOutcome('failed', {'reason': 'nothing is held'})
+        transferred = self._transfer(node, context)
+        if transferred.event != 'success':
+            return transferred
+        lowered = self._lower_skill(node, context)
+        if lowered.event != 'success':
+            return lowered
+        return SkillOutcome('success', {
+            'object': node.object, 'target': node.target,
+            'tcp': list(context.contact_pose.xyz) if context.contact_pose else None,
+        })
+
+    def _align(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        if context.held_object is None and context.mounted_tool is None:
+            return SkillOutcome('failed', {'reason': 'nothing is held or mounted'})
+        if context.approach_pose is not None:
+            self._move(context, context.approach_pose, 'align')
+            context.phase = 'approach'
+            return SkillOutcome('success', {'reused': True})
+        safe = self._ensure_approach(context, node)
+        return SkillOutcome('success', {'tcp': list(safe.xyz)})
+
+    # ------------------------------------------------------------------ release
+    def _release(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        obj = node.object
+        target = node.target
+        unavailable = self._refresh_live_ownership(context)
+        if unavailable is not None:
+            return SkillOutcome('failed', {'reason': unavailable})
+        if obj in context.spec.tool_names:
+            return self._release_tool(node, context)
+        if obj == 'cap':
+            return self._release_cap(node, context)
+        if obj.startswith('screw_'):
+            return self._handoff_screw(node, context)
+        return SkillOutcome('failed', {'reason': f'{obj!r} cannot be released'})
+
+    def _release_tool(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        tool = node.object
+        socket = f'{tool}_socket'
+        if context.mounted_tool != tool:
+            return SkillOutcome('failed', {
+                'reason': f'required tool {tool} is not mounted',
+                'required_tool': tool,
+                'mounted_tool': context.mounted_tool,
+                'moved': False,
+            })
+        if node.target != socket:
+            # A bare release(vacuum) from work must not move the robot or
+            # detach the cup.  Only an explicit socket target is valid.
+            return SkillOutcome('failed', {
+                'reason': (
+                    f'release({tool}) requires the explicit socket target '
+                    f'{socket!r}; no motion'
+                ),
+                'moved': False,
+            })
+        if context.transfer_target != socket:
+            return SkillOutcome('failed', {
+                'reason': (
+                    f'release({tool},{socket}) requires an explicit preceding '
+                    'transfer to that socket'
+                ),
+                'moved': False,
+            })
+        adapter = self._adapter(context)
+        prepare = getattr(adapter, 'prepare_dock_tool', None)
+        if prepare is not None:
+            prepare(tool)
+        if not context.approach_matches((tool, socket)):
+            goal = self._destination_goal(context, tool, socket)
+            safe = _above(goal, context.geometry.approach_gap)
+            self._move(context, safe, 'transfer')
+            context.mark_approach(safe, (tool, socket))
+        if context.phase != 'contact':
+            goal = self._destination_goal(context, tool, socket)
+            self._move(context, goal, 'lower')
+            context.mark_contact(goal)
+        try:
+            self._command(context, 'dock_tool', tool)
+        except SkillError as error:
+            return SkillOutcome('failed', {'reason': str(error)})
+        context.mounted_tool = None
+        adapter = self._adapter(context)
+        if adapter is not None:
+            try:
+                adapter.dock_tool(tool)
+            except Exception as error:
+                return SkillOutcome('failed', {'reason': f'planning scene: {error}'})
+        self._lift_to_approach(context)
+        return SkillOutcome('success', {'tool': tool, 'target': socket})
+
+    def _release_cap(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        if context.held_object != 'cap':
+            return SkillOutcome('failed', {
+                'reason': 'hatch is not held',
+                'held_object': context.held_object,
+            })
+        target = node.target or 'hatch_seat'
+        self._ensure_approach(context, node)
+        adapter = self._adapter(context)
+        prepare = getattr(adapter, 'prepare_place_cap', None)
+        if prepare is not None:
+            prepare()
+        if context.phase != 'contact':
+            goal = self._destination_goal(context, 'cap', target)
+            self._move(context, goal, 'lower')
+            context.mark_contact(goal)
+        seat = world_target(context.spec, context.geometry, 'cap_seat')
+        try:
+            self._command(context, 'place_cap', 'cap', target=seat)
+        except SkillError as error:
+            return SkillOutcome('failed', {'reason': str(error)})
+        context.held_object = None
+        context.cap_relative = None
+        adapter = self._adapter(context)
+        if adapter is not None:
+            try:
+                adapter.place_cap(seat)
+            except Exception as error:
+                return SkillOutcome('failed', {'reason': f'planning scene: {error}'})
+        self._lift_to_approach(context)
+        return SkillOutcome('success', {'object': 'cap', 'target': target})
+
+    def _dock_tool(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        unavailable = self._refresh_live_ownership(context)
+        if unavailable is not None:
+            return SkillOutcome('failed', {'reason': unavailable})
+        if node.object not in context.spec.tool_names:
+            return SkillOutcome('failed', {'reason': f'{node.object!r} is not a tool'})
+        socket = f'{node.object}_socket'
+        if node.target != socket:
+            return SkillOutcome('failed', {
+                'reason': f'dock_tool requires target {socket!r}',
+            })
+        return self._release_tool(node, context)
+
+    def _handoff_screw(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        screw = node.object
+        hole = node.target
+        unavailable = self._refresh_live_ownership(context)
+        if unavailable is not None:
+            return SkillOutcome('grasp_failed', {'reason': unavailable})
+        if context.mounted_tool != 'magnetic_tool':
+            return SkillOutcome('grasp_failed', {
+                'reason': 'required tool magnetic_tool is not mounted',
+                'required_tool': 'magnetic_tool',
+                'mounted_tool': context.mounted_tool,
+            })
+        if context.held_object != screw:
+            return SkillOutcome('grasp_failed', {
+                'reason': f'{screw!r} is not held',
+                'held_object': context.held_object,
+            })
+        self._ensure_approach(context, node)
+        adapter = self._adapter(context)
+        prepare = getattr(adapter, 'prepare_handoff_screw', None)
+        if prepare is not None:
+            prepare(screw)
+        if context.phase != 'contact':
+            goal = self._destination_goal(context, screw, hole)
+            self._move(context, goal, 'lower')
+            context.mark_contact(goal)
+        target = world_target(context.spec, context.geometry, f'hole:{hole}')
+        try:
+            self._command(
+                context, 'handoff_screw', screw,
+                target=target, target_name=hole, event='grasp_failed',
+            )
+        except SkillError as error:
+            return SkillOutcome('grasp_failed', {'reason': str(error)})
+        context.held_object = None
+        adapter = self._adapter(context)
+        if adapter is not None:
+            try:
+                adapter.handoff_screw(screw)
+            except Exception as error:
+                return SkillOutcome('failed', {'reason': f'planning scene: {error}'})
+        self._lift_to_approach(context)
+        return SkillOutcome('success', {
+            'object': screw, 'target': hole,
+            'installed': False, 'tightened': False,
+        })
+
+    # ------------------------------------------------------------------ misc
+    def _retreat(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        del node
+        if context.phase == 'contact' and context.approach_pose is not None:
+            self._move(context, context.approach_pose, 'retreat')
+            context.mark_lifted()
+        work = context.geometry.safe_poses['work']
+        self._move(context, work, 'transfer')
+        context.reset_phase()
+        return SkillOutcome('success', {'tcp': list(work.xyz)})
+
+    def _dispense(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        unavailable = self._refresh_live_ownership(context)
+        if unavailable is not None:
+            return SkillOutcome('failed', {'reason': unavailable})
+        if context.mounted_tool != 'sealant_tool':
+            return SkillOutcome('failed', {
+                'reason': 'required tool sealant_tool is not mounted',
+                'required_tool': 'sealant_tool',
+                'mounted_tool': context.mounted_tool,
+            })
+        side = node.object or node.target
+        start_fraction = float(node.params.get('start_fraction', 0.0))
+        end_fraction = float(node.params.get('end_fraction', 1.0))
+        start = sealant_point(
+            context.geometry, context.spec, side, start_fraction,
+        )
+        end = sealant_point(
+            context.geometry, context.spec, side, end_fraction,
+        )
+        tip = context.geometry.tool_work_tips['sealant_tool']
+        start_goal = tcp_goal(start, tip)
+        end_goal = tcp_goal(end, tip)
+        signature = (side, f'{start_fraction:.3f}:{end_fraction:.3f}')
+        if not context.approach_matches(signature):
+            safe = _above(start_goal, context.geometry.approach_gap)
+            self._move(context, safe, 'approach')
+            context.mark_approach(safe, signature)
+        if context.phase != 'contact':
+            self._move(context, start_goal, 'lower')
+            context.mark_contact(start_goal)
+        self._move(context, end_goal, 'dispense')
+        context.mark_contact(end_goal)
+        self._lift_to_approach(context)
+        return SkillOutcome('success', {
+            'side': side,
+            'start_fraction': start_fraction,
+            'end_fraction': end_fraction,
+        })
+
+    def _stop(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        del node
+        try:
+            self.motion.stop('graph stop node')
+            # A stop node is a graph decision, not a terminal safety latch:
+            # a verified inspect_state may deliberately retry afterwards.
+            reset = getattr(self.motion, 'reset_stop', None)
+            if reset is not None:
+                reset()
+        except Exception as error:
+            return SkillOutcome('failed', {'reason': str(error)})
+        return SkillOutcome('success', {
+            'tool': context.mounted_tool,
+            'held': context.held_object,
+        })
+
+    def _wait_scene_clear(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        del node
+        if context.scene_is_clear():
+            return SkillOutcome('clear')
+        return SkillOutcome('timeout', {'reason': 'scene did not become clear'})
+
+    def _inspect_state(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        state = self._state()
+        obj = node.object
+        if not state:
+            context.ownership_unknown = True
+            return SkillOutcome('unknown', {'reason': 'supervisor state unavailable'})
+        mounted = state.get('mounted_tool')
+        held = state.get('held_object')
+        if held == obj:
+            context.mounted_tool = mounted if isinstance(mounted, str) else context.mounted_tool
+            context.held_object = obj
+            context.ownership_unknown = False
+            return SkillOutcome('holding', {'object': obj})
+        if obj == 'cap':
+            cap_state = (state.get('cap') or {}).get('state')
+            if cap_state == 'installed':
+                context.held_object = None
+                context.ownership_unknown = False
+                return SkillOutcome('placed', {'object': obj})
+            if cap_state == 'free':
+                context.held_object = None
+                context.ownership_unknown = False
+                return SkillOutcome('empty', {'object': obj})
+            if cap_state == 'held':
+                context.held_object = 'cap'
+                context.ownership_unknown = False
+                return SkillOutcome('holding', {'object': obj})
+        if obj.startswith('screw_'):
+            screw = (state.get('screws') or {}).get(obj, {})
+            screw_state = screw.get('state')
+            if screw_state == 'delivered':
+                context.held_object = None
+                context.ownership_unknown = False
+                return SkillOutcome('placed', {'object': obj})
+            if screw_state == 'held':
+                context.held_object = obj
+                context.ownership_unknown = False
+                return SkillOutcome('holding', {'object': obj})
+            if screw_state == 'tray':
+                context.held_object = None
+                context.ownership_unknown = False
+                return SkillOutcome('empty', {'object': obj})
+        if obj in context.spec.tool_names:
+            tool_state = (state.get('tools') or {}).get(obj, {}).get('state')
+            if tool_state == 'mounted':
+                context.mounted_tool = obj
+                context.ownership_unknown = False
+                return SkillOutcome('holding', {'object': obj})
+            if tool_state == 'docked':
+                context.mounted_tool = None
+                context.ownership_unknown = False
+                return SkillOutcome('empty', {'object': obj})
+        context.ownership_unknown = True
+        return SkillOutcome('unknown', {
+            'object': obj, 'mounted_tool': mounted, 'held_object': held,
+        })
+
+    def _notify_operator(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        reason = node.target or 'operator intervention required'
+        if context.notify(reason, {
+            'node': node.id, 'tool': context.mounted_tool,
+            'held': context.held_object,
+        }):
+            return SkillOutcome('success', {'reason': reason})
+        return SkillOutcome('failed', {'reason': reason})
+
+    def _verify(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
+        state = self._state()
+        obj = node.object
+        target = node.target
+        if not state:
+            return SkillOutcome('failed', {'reason': 'supervisor state unavailable'})
+        if obj == 'cap':
+            cap = state.get('cap') or {}
+            if cap.get('state') != 'installed':
+                return SkillOutcome('not_ok', {
+                    'reason': f'hatch state is {cap.get("state")!r}',
+                })
+            pose = _state_pose(cap.get('pose'), 'cap')
+            if pose is None:
+                return SkillOutcome('failed', {'reason': 'hatch pose unavailable'})
+            seat = target or 'hatch_seat'
+            try:
+                expected = world_target(
+                    context.spec, context.geometry, 'cap_seat',
+                )
+            except Exception as error:
+                return SkillOutcome('failed', {'reason': str(error)})
+            distance = _distance(pose, expected)
+            if distance <= context.geometry.position_tolerance:
+                return SkillOutcome('ok', {
+                    'object': obj, 'target': seat, 'distance': distance,
+                })
+            return SkillOutcome('not_ok', {
+                'object': obj, 'target': seat, 'distance': distance,
+            })
+        if obj.startswith('screw_'):
+            screw = (state.get('screws') or {}).get(obj, {})
+            if screw.get('state') == 'delivered' and screw.get('hole') == target:
+                return SkillOutcome('ok', {'object': obj, 'target': target})
+            return SkillOutcome('not_ok', {
+                'object': obj, 'target': target,
+                'state': screw.get('state'),
+            })
+        inspected = self._inspect_state(node, context)
+        if inspected.event == 'placed':
+            return SkillOutcome('ok', {'object': obj})
+        if inspected.event in ('empty', 'holding'):
+            return SkillOutcome('not_ok', {'object': obj, 'state': inspected.event})
+        return SkillOutcome('failed', {'object': obj})

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/validator.py

@@ -0,0 +1,720 @@
+"""Validate a parsed graph against the live scene and the server catalog."""
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from typing import Any, Iterable, Mapping
+
+from iiwa_scene_context.catalog import ALLOWED_PARAMS, SkillCatalog
+
+from .schema import TERMINALS, PlanGraph, PlanNode
+
+
+SAFE_COLLISION_SKILLS = {'stop', 'notify_operator'}
+SAFE_GRASP_FAILURE_SKILLS = {'inspect_state', 'stop'}
+SAFE_FAILURE_TERMINALS = {'replan_required', 'operator_required'}
+PROGRESS_EVENTS = {
+    'success', 'ok', 'found', 'clear', 'empty', 'holding', 'placed',
+}
+REQUIRES_OBJECT = {
+    'detect', 'approach', 'lower', 'grab', 'lift', 'transfer', 'align',
+    'release', 'retreat', 'verify', 'dispense', 'dock_tool',
+    'handoff_screw', 'pick', 'move_to_storage', 'inspect', 'inspect_state',
+    'capture',
+}
+
+
+@dataclass(frozen=True)
+class ValidationResult:
+    ok: bool
+    code: str
+    reason: str
+    details: dict[str, Any] = field(default_factory=dict)
+
+    @property
+    def accepted(self) -> bool:
+        return self.ok
+
+    def as_dict(self) -> dict[str, Any]:
+        return {
+            'ok': self.ok,
+            'code': self.code,
+            'reason': self.reason,
+            'details': dict(self.details),
+        }
+
+
+@dataclass(frozen=True)
+class _State:
+    mounted: str | None = None
+    held: str | None = None
+    unknown: bool = False
+    transfer_target: str | None = None
+    empty_failure: bool = False
+    cap_installed: bool = False
+
+
+@dataclass(frozen=True)
+class _Facts:
+    components: dict[str, str]
+    tools: dict[str, str]
+    destinations: dict[str, str]
+    cameras: set[str]
+    service_objects: set[str]
+    initial: _State
+    available_skills: set[str]
+    envelope: Mapping[str, Any]
+    cap_installed: bool
+
+
+def _scene_payload(scene: Mapping[str, Any]) -> tuple[Mapping[str, Any], Mapping[str, Any]]:
+    if 'scene_json' in scene:
+        payload = scene['scene_json']
+        if not isinstance(payload, Mapping):
+            raise ValueError('scene_json must be an object')
+        return scene, payload
+    return {}, scene
+
+
+def _facts(scene: Mapping[str, Any]) -> _Facts:
+    envelope, payload = _scene_payload(scene)
+    components = {
+        entry['id']: str((entry.get('attributes') or {}).get('kind', 'component'))
+        for entry in payload.get('components', [])
+        if isinstance(entry, Mapping) and isinstance(entry.get('id'), str)
+    }
+    tools = {
+        entry['id']: str(entry.get('socket', f"{entry['id']}_socket"))
+        for entry in payload.get('tools', [])
+        if isinstance(entry, Mapping) and isinstance(entry.get('id'), str)
+    }
+    destinations = {
+        entry['id']: str(entry.get('kind', 'destination'))
+        for entry in payload.get('destinations', [])
+        if isinstance(entry, Mapping) and isinstance(entry.get('id'), str)
+    }
+    cameras = set(payload.get('cameras', {}))
+    service_objects = set(payload.get('service_objects', {}))
+    cap_entry = next(
+        (
+            entry for entry in payload.get('components', [])
+            if isinstance(entry, Mapping) and entry.get('id') == 'cap'
+        ),
+        {},
+    )
+    cap_installed = str(cap_entry.get('state', '')) == 'installed'
+    initial_raw = payload.get('initial_state') or {}
+    initial = _State(
+        mounted=initial_raw.get('tool_mounted') or None,
+        held=initial_raw.get('held_object') or None,
+        cap_installed=cap_installed,
+    )
+    available = set(payload.get('available_skills', []))
+    return _Facts(
+        components=components,
+        tools=tools,
+        destinations=destinations,
+        cameras=cameras,
+        service_objects=service_objects,
+        initial=initial,
+        available_skills=available,
+        envelope=envelope,
+        cap_installed=cap_installed,
+    )
+
+
+def _reject(reason: str, *, code: str = 'rejected', **details) -> ValidationResult:
+    return ValidationResult(False, code, reason, details)
+
+
+def _replan(reason: str, **details) -> ValidationResult:
+    return ValidationResult(False, 'replan_required', reason, details)
+
+
+def _node_or_terminal(graph: PlanGraph, node_id: str) -> PlanNode | None:
+    if node_id in TERMINALS:
+        return None
+    return graph.node(node_id)
+
+
+def _structural_checks(graph: PlanGraph, facts: _Facts) -> ValidationResult | None:
+    ids = set(graph.ids)
+    for node in graph.nodes:
+        if not node.transitions:
+            return _reject(f'node {node.id!r} has no transitions')
+        if node.skill not in facts.available_skills:
+            return _reject(
+                f'node {node.id!r} uses unknown or unavailable skill '
+                f'{node.skill!r}'
+            )
+        if node.object:
+            known = (
+                node.object in facts.components
+                or node.object in facts.tools
+                or node.object in facts.cameras
+                or node.object in facts.service_objects
+                or node.object in facts.destinations
+            )
+            if not known:
+                return _reject(
+                    f'node {node.id!r} references unknown object '
+                    f'{node.object!r}'
+                )
+        if node.target:
+            known = (
+                node.target in facts.destinations
+                or node.target in facts.components
+                or node.target in facts.tools
+            )
+            if not known:
+                return _reject(
+                    f'node {node.id!r} references unknown target '
+                    f'{node.target!r}'
+                )
+        if node.params:
+            allowed_params = set(ALLOWED_PARAMS.get(node.skill, ()))
+            unknown_params = set(node.params) - allowed_params
+            if unknown_params:
+                return _reject(
+                    f'node {node.id!r} has unsupported params '
+                    f'{sorted(unknown_params)}'
+                )
+            if node.skill == 'dispense':
+                for key in ('start_fraction', 'end_fraction'):
+                    if key in node.params:
+                        value = node.params[key]
+                        if (
+                            isinstance(value, bool)
+                            or not isinstance(value, (int, float))
+                            or not 0.0 <= float(value) <= 1.0
+                        ):
+                            return _reject(
+                                f'node {node.id!r} param {key} must be '
+                                'within 0..1'
+                            )
+                start = float(node.params.get('start_fraction', 0.0))
+                end = float(node.params.get('end_fraction', 1.0))
+                if start > end:
+                    return _reject(
+                        f'node {node.id!r} dispense start_fraction exceeds '
+                        'end_fraction'
+                    )
+        allowed_events = set(facts.envelope.get('skill_contracts', {}).get(
+            node.skill, {},
+        ).get('outcomes', {}))
+        if not allowed_events:
+            # The server catalog remains authoritative even if the envelope
+            # was produced by an older snapshot.
+            from iiwa_scene_context.catalog import SKILL_OUTCOMES
+            allowed_events = set(SKILL_OUTCOMES.get(node.skill, {}))
+        events = {transition.event for transition in node.transitions}
+        missing = allowed_events - events
+        extra = events - allowed_events
+        if missing:
+            return _reject(
+                f'node {node.id!r} misses required transitions {sorted(missing)}'
+            )
+        if extra:
+            return _reject(
+                f'node {node.id!r} has unknown events {sorted(extra)}'
+            )
+        for transition in node.transitions:
+            if transition.next not in ids and transition.next not in TERMINALS:
+                return _reject(
+                    f'node {node.id!r} points to unknown node '
+                    f'{transition.next!r}'
+                )
+            if transition.next == node.id:
+                # A self-loop that does not inspect state would spin forever.
+                if node.skill not in ('inspect_state', 'wait_scene_clear'):
+                    return _reject(
+                        f'node {node.id!r} has a direct self-loop'
+                    )
+    if graph.entry not in ids:
+        return _reject(f'entry {graph.entry!r} is not a plan node')
+
+    reachable: set[str] = set()
+    stack = [graph.entry]
+    while stack:
+        current = stack.pop()
+        if current in reachable or current not in ids:
+            continue
+        reachable.add(current)
+        stack.extend(item.next for item in graph.node(current).transitions)
+    unreachable = ids - reachable
+    if unreachable:
+        return _reject(f'unreachable nodes {sorted(unreachable)}')
+
+    reverse: dict[str, set[str]] = {node_id: set() for node_id in ids}
+    for node in graph.nodes:
+        for transition in node.transitions:
+            if transition.next in ids:
+                reverse[transition.next].add(node.id)
+    can_finish = set(TERMINALS)
+    changed = True
+    while changed:
+        changed = False
+        for node_id in ids - can_finish:
+            if any(item in can_finish for item in _targets(graph.node(node_id))):
+                can_finish.add(node_id)
+                changed = True
+    dead = ids - can_finish
+    if dead:
+        return _reject(
+            f'nodes cannot reach a terminal: {sorted(dead)}'
+        )
+
+    for node in graph.nodes:
+        for transition in node.transitions:
+            target = _node_or_terminal(graph, transition.next)
+            if transition.event == 'collision':
+                if target is None and transition.next not in SAFE_FAILURE_TERMINALS:
+                    return _reject(
+                        f'collision in {node.id!r} must stop the operation'
+                    )
+                if target is not None and target.skill not in SAFE_COLLISION_SKILLS:
+                    return _reject(
+                        f'collision in {node.id!r} must reach stop/notify, '
+                        f'got {target.skill!r}'
+                    )
+            if transition.event == 'grasp_failed':
+                if target is None and transition.next not in SAFE_FAILURE_TERMINALS:
+                    return _reject(
+                        f'grasp_failed in {node.id!r} must not continue blindly'
+                    )
+                if target is not None and target.skill not in SAFE_GRASP_FAILURE_SKILLS:
+                    return _reject(
+                        f'grasp_failed in {node.id!r} must reach inspect_state '
+                        f'or stop, got {target.skill!r}'
+                    )
+            if transition.event in (
+                'not_found', 'failed', 'not_ok', 'timeout', 'unknown',
+            ) and transition.next == 'completed':
+                return _reject(
+                    f'{transition.event} in {node.id!r} cannot map to completed'
+                )
+    return None
+
+
+def _targets(node: PlanNode) -> list[str]:
+    return [item.next for item in node.transitions]
+
+
+def _precondition(
+    node: PlanNode,
+    state: _State,
+    facts: _Facts,
+) -> ValidationResult | None:
+    skill = node.skill
+    obj = node.object
+    target = node.target
+    if node.id in ('',) or skill not in facts.available_skills:
+        return _reject(f'unknown skill {skill!r}')
+
+    if skill in REQUIRES_OBJECT and not obj:
+        return _reject(f'{skill} in {node.id!r} requires an object')
+
+    if state.unknown and skill not in (
+        'inspect_state', 'stop', 'notify_operator', 'wait_scene_clear',
+    ):
+        return _replan(
+            f'{node.id!r} follows an unconfirmed grasp; inspect_state is required',
+        )
+
+    if skill in ('grab', 'pick'):
+        if obj in facts.tools:
+            if state.mounted is not None:
+                return _replan(
+                    f'{node.id!r} cannot grab {obj!r}: tool '
+                    f'{state.mounted!r} is already mounted'
+                )
+            if state.held is not None:
+                return _replan(
+                    f'{node.id!r} cannot grab {obj!r} while holding '
+                    f'{state.held!r}'
+                )
+            return None
+        if obj == 'cap':
+            if 'cap' not in facts.components:
+                return _reject(f'{node.id!r}: hatch is absent from this scene')
+            # A missing vacuum tool is a runtime grasp_failed condition, not
+            # a reason to reject the graph before execution.
+            if state.held is not None:
+                return _replan(
+                    f'{node.id!r} cannot grab the hatch while holding '
+                    f'{state.held!r}'
+                )
+            return None
+        if obj in facts.components and facts.components[obj] == 'screw':
+            # A missing magnetic tool is a runtime grasp_failed condition.
+            if state.held is not None:
+                return _replan(
+                    f'{node.id!r} cannot grab a screw while holding '
+                    f'{state.held!r}'
+                )
+            return None
+        return _reject(f'{node.id!r}: object {obj!r} cannot be grasped')
+
+    if skill == 'release':
+        if obj in facts.tools:
+            socket = facts.tools[obj]
+            if target != socket:
+                return _replan(
+                    f'{node.id!r}: release({obj}) must name its socket '
+                    f'{socket!r}'
+                )
+            if state.transfer_target != socket:
+                return _replan(
+                    f'{node.id!r}: release({obj},{socket}) requires an explicit '
+                    'preceding transfer to that socket'
+                )
+            return None
+        if obj == 'cap':
+            if state.held != 'cap':
+                return _replan(f'{node.id!r}: hatch is not held')
+            if facts.destinations.get(target) != 'seat':
+                return _replan(
+                    f'{node.id!r}: hatch release requires hatch_seat, '
+                    f'got {target!r}'
+                )
+            return None
+        if obj in facts.components and facts.components[obj] == 'screw':
+            if state.held != obj:
+                return _replan(f'{node.id!r}: {obj!r} is not held')
+            if facts.destinations.get(target) != 'hole':
+                return _replan(
+                    f'{node.id!r}: screw handoff requires a hole target'
+                )
+            return None
+        return _reject(f'{node.id!r}: object {obj!r} cannot be released')
+
+    if skill in ('transfer', 'move_to_storage'):
+        if obj in facts.tools:
+            if target not in ('work', facts.tools[obj]):
+                return _replan(
+                    f'{node.id!r}: tool transfer must target work or its socket'
+                )
+            return None
+        if obj == 'cap':
+            if state.held != 'cap':
+                return _replan(f'{node.id!r}: hatch is not held')
+            if target != 'hatch_seat' and target != 'work':
+                return _replan(
+                    f'{node.id!r}: hatch transfer must target hatch_seat or work'
+                )
+            return None
+        if obj in facts.components and facts.components[obj] == 'screw':
+            if state.held != obj:
+                return _replan(f'{node.id!r}: {obj!r} is not held')
+            if facts.destinations.get(target) != 'hole' and target != 'work':
+                return _replan(
+                    f'{node.id!r}: screw transfer must target a hole or work'
+                )
+            return None
+        return _reject(f'{node.id!r}: object {obj!r} cannot be transferred')
+
+    if skill == 'align':
+        if state.held is None and state.mounted is None:
+            return _replan(f'{node.id!r}: align requires a held part or tool')
+        return None
+
+    if skill == 'dispense':
+        side = obj or target
+        if facts.destinations.get(side) != 'contour':
+            return _reject(
+                f'{node.id!r}: dispense target {side!r} is not a sealant side'
+            )
+        return None
+
+    if skill == 'dock_tool':
+        socket = facts.tools.get(obj)
+        if socket is None:
+            return _reject(f'{node.id!r}: {obj!r} is not a present tool')
+        if target != socket:
+            return _replan(
+                f'{node.id!r}: dock_tool must target socket {socket!r}'
+            )
+        if state.transfer_target != socket:
+            return _replan(
+                f'{node.id!r}: dock_tool requires an explicit preceding '
+                'transfer to that socket'
+            )
+        return None
+
+    if skill == 'handoff_screw':
+        if obj not in facts.components or facts.components[obj] != 'screw':
+            return _reject(f'{node.id!r}: {obj!r} is not a present screw')
+        if state.held != obj:
+            return _replan(f'{node.id!r}: {obj!r} is not held')
+        if facts.destinations.get(target) != 'hole':
+            return _replan(
+                f'{node.id!r}: handoff_screw target must be a hole'
+            )
+        return None
+
+    if skill in ('capture',):
+        if obj not in facts.cameras:
+            return _reject(f'{node.id!r}: unknown camera {obj!r}')
+    return None
+
+
+def _success_feasible(
+    node: PlanNode,
+    state: _State,
+    facts: _Facts,
+) -> bool:
+    """Whether this node's nominal success can happen from ``state``.
+
+    A missing required tool must not reject the graph, but its success branch
+    must not be treated as already achieved either.  The validator therefore
+    skips only the success edge for that state and continues through the
+    failure events the graph actually declared.
+    """
+    skill = node.skill
+    obj = node.object
+    if skill in ('grab', 'pick'):
+        if obj in facts.tools:
+            return (
+                state.mounted in (None, obj)
+                and state.held is None
+            )
+        if obj == 'cap':
+            return state.mounted == 'vacuum' and state.held is None
+        if obj and facts.components.get(obj) == 'screw':
+            return state.mounted == 'magnetic_tool' and state.held is None
+        return False
+    if skill in ('transfer', 'move_to_storage'):
+        if obj in facts.tools:
+            return state.mounted == obj
+        if obj == 'cap':
+            return state.held == 'cap'
+        if obj and facts.components.get(obj) == 'screw':
+            return state.held == obj
+        return False
+    if skill == 'release':
+        if obj in facts.tools:
+            return state.mounted == obj
+        if obj == 'cap':
+            return state.held == 'cap'
+        if obj and facts.components.get(obj) == 'screw':
+            return state.held == obj and state.mounted == 'magnetic_tool'
+        return False
+    if skill == 'align':
+        return state.held is not None or state.mounted is not None
+    if skill == 'dispense':
+        return state.mounted == 'sealant_tool'
+    if skill == 'dock_tool':
+        return state.mounted == obj
+    if skill == 'handoff_screw':
+        return state.held == obj and state.mounted == 'magnetic_tool'
+    return True
+
+
+def _missing_tool_for_grasp(
+    node: PlanNode,
+    state: _State,
+    facts: _Facts,
+) -> bool:
+    """True when a grasp fails specifically because its tool is absent."""
+    if node.skill not in ('grab', 'pick'):
+        return False
+    obj = node.object
+    if obj == 'cap':
+        return state.mounted != 'vacuum'
+    if obj and facts.components.get(obj) == 'screw':
+        return state.mounted != 'magnetic_tool'
+    return False
+
+
+def _inspect_event_feasible(
+    node: PlanNode,
+    state: _State,
+    event: str,
+    facts: _Facts,
+) -> bool:
+    if node.skill != 'inspect_state':
+        return True
+    if state.unknown:
+        return True
+    if state.empty_failure:
+        return event == 'empty'
+    if node.object != 'cap':
+        return True
+    if event == 'empty':
+        return state.held is None and not state.cap_installed
+    if event == 'holding':
+        return state.held == 'cap'
+    if event == 'placed':
+        return state.held is None and state.cap_installed
+    return True
+
+
+def _apply_event(
+    node: PlanNode,
+    event: str,
+    state: _State,
+    facts: _Facts,
+) -> _State:
+    skill = node.skill
+    obj = node.object
+    if skill == 'inspect_state' and event in ('empty', 'holding', 'placed', 'unknown'):
+        if event == 'empty':
+            return _State(
+                state.mounted, None, False, state.transfer_target, False, False,
+            )
+        if event == 'holding':
+            return _State(
+                state.mounted, obj or None, False, state.transfer_target, False,
+                False,
+            )
+        if event == 'placed':
+            return _State(
+                state.mounted, None, False, state.transfer_target, False,
+                state.cap_installed or obj == 'cap',
+            )
+        return _State(
+            state.mounted, state.held, True, state.transfer_target, False,
+            state.cap_installed,
+        )
+    if event == 'grasp_failed':
+        missing_tool = _missing_tool_for_grasp(node, state, facts)
+        return _State(
+            state.mounted,
+            state.held,
+            not missing_tool,
+            state.transfer_target,
+            missing_tool,
+            state.cap_installed,
+        )
+    if event not in PROGRESS_EVENTS:
+        return state
+    if skill in ('grab', 'pick'):
+        if obj in facts.tools:
+            return _State(
+                obj, state.held, False, state.transfer_target, False,
+                state.cap_installed,
+            )
+        if obj == 'cap':
+            return _State(
+                state.mounted, 'cap', False, state.transfer_target, False, False,
+            )
+        if obj and facts.components.get(obj) == 'screw':
+            return _State(
+                state.mounted, obj, False, state.transfer_target, False,
+                state.cap_installed,
+            )
+        return state
+    if skill == 'release':
+        if obj in facts.tools:
+            return _State(
+                None, state.held, False, state.transfer_target, False,
+                state.cap_installed,
+            )
+        if obj == 'cap':
+            return _State(
+                state.mounted, None, False, state.transfer_target, False, True,
+            )
+        if obj:
+            return _State(
+                state.mounted, None, False, state.transfer_target, False,
+                state.cap_installed,
+            )
+        return state
+    if skill == 'dock_tool':
+        return _State(
+            None, state.held, False, state.transfer_target, False,
+            state.cap_installed,
+        )
+    if skill == 'handoff_screw':
+        return _State(
+            state.mounted, None, False, state.transfer_target, False,
+            state.cap_installed,
+        )
+    if skill == 'transfer':
+        return _State(
+            state.mounted,
+            state.held,
+            False,
+            node.target or state.transfer_target,
+            False,
+            state.cap_installed,
+        )
+    return state
+
+
+def _ownership_checks(graph: PlanGraph, facts: _Facts) -> ValidationResult | None:
+    initial = facts.initial
+    seen: set[tuple[str, _State]] = set()
+    queue: list[tuple[str, _State]] = [(graph.entry, initial)]
+    visited_states: dict[str, set[_State]] = {node_id: set() for node_id in graph.ids}
+    while queue:
+        node_id, state = queue.pop()
+        if (node_id, state) in seen:
+            continue
+        seen.add((node_id, state))
+        node = graph.node(node_id)
+        failure = _precondition(node, state, facts)
+        if failure is not None:
+            failure.details.update({
+                'node_id': node.id,
+                'skill': node.skill,
+                'mounted_tool': state.mounted,
+                'held_object': state.held,
+            })
+            return failure
+        visited_states[node.id].add(state)
+        for transition in node.transitions:
+            if transition.next not in graph.ids:
+                continue
+            if (
+                not _inspect_event_feasible(node, state, transition.event, facts)
+            ):
+                # Known empty state: only "empty" is physically possible.
+                # Unknown state remains conservative and allows every branch.
+                continue
+            if (
+                transition.event in PROGRESS_EVENTS
+                and not _success_feasible(node, state, facts)
+            ):
+                # Do not simulate a success that cannot happen (for example
+                # pick(cap) without the vacuum tool).  The grasp_failed edge
+                # from the same state is still traversed and checked.
+                continue
+            next_state = _apply_event(node, transition.event, state, facts)
+            if (transition.next, next_state) not in seen:
+                queue.append((transition.next, next_state))
+    return None
+
+
+def validate_graph(
+    graph: PlanGraph,
+    scene: Mapping[str, Any],
+    catalog: SkillCatalog,
+) -> ValidationResult:
+    """Return an admission decision; never mutates the graph or the scene."""
+    if graph.status == 'blocked':
+        return ValidationResult(
+            False, 'blocked', 'the model reported status=blocked; no motion',
+            {'status': graph.status},
+        )
+    if graph.status != 'ready':
+        return _reject(f'unsupported graph status {graph.status!r}')
+    if len(graph.nodes) > 256:
+        return _reject('graph exceeds the 256-node limit')
+    facts = _facts(scene)
+    structural = _structural_checks(graph, facts)
+    if structural is not None:
+        return structural
+    ownership = _ownership_checks(graph, facts)
+    if ownership is not None:
+        return ownership
+    return ValidationResult(
+        True,
+        'ok',
+        'graph accepted',
+        {
+            'nodes': len(graph.nodes),
+            'entry': graph.entry,
+            'state_version': facts.envelope.get('state_version'),
+            'available_skills': len(facts.available_skills),
+        },
+    )

src/iiwa_llm_plan_executor/launch/planner.launch.py

@@ -0,0 +1,279 @@
+"""Launch one Webots scene with the scene context and the graph executor.
+
+The experiment only selects the initial scene.  The reference runner is never
+started in this mode; the graph arrives from outside through
+``/iiwa_llm_plan_executor/submit``.
+"""
+from __future__ import annotations
+
+from pathlib import Path
+
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from iiwa_experement.runtime_config import (
+    validate_experiment_id,
+    validate_mode,
+)
+
+
+EXPERIMENT_LAUNCH = ('launch', 'experiment.launch.py')
+PLANNER_MODE = 'planner'
+DEFAULT_CONFIG = {
+    'start_delay_sec': 3.0,
+    'scene_max_age_sec': 2.0,
+    'max_steps': 512,
+    'scene_freshness_sec': 2.0,
+    'close_inspection_objects': ['cap'],
+    'top_camera_topic': '/d455_top/image_raw/image_color',
+    'hand_camera_topic': '/camera/color/image_color',
+    'frame_timeout_sec': 2.0,
+    'trace_dir': '~/.ros/iiwa_llm_plan_executor/runs',
+    'planning_group': 'iiwa_arm',
+    'pose_link': 'tcp',
+}
+
+
+def load_config(path: Path | str | None) -> dict:
+    data = dict(DEFAULT_CONFIG)
+    if path is not None:
+        source = Path(path)
+        if source.is_file():
+            loaded = yaml.safe_load(source.read_text(encoding='utf-8')) or {}
+            if isinstance(loaded, dict):
+                data.update(loaded)
+    return data
+
+
+def validate_arguments(
+    experiment_id_raw: str,
+    mode_raw: str,
+    simulate_raw: str,
+) -> tuple[int, str]:
+    """Validate everything before Webots is started."""
+    experiment_id = validate_experiment_id(experiment_id_raw)
+    mode = validate_mode(mode_raw)
+    if mode != PLANNER_MODE:
+        raise ValueError(
+            f'mode must be {PLANNER_MODE!r} for the graph executor, got {mode!r}'
+        )
+    simulate = str(simulate_raw).strip().lower()
+    if simulate not in ('true', '1', 'yes'):
+        raise ValueError(
+            'this executor stage only supports Webots (simulate:=true); '
+            'the physical FRI robot is not connected'
+        )
+    return experiment_id, mode
+
+
+def moveit_parameters(base_setting: str) -> list:
+    """Parameters MoveItPy reads from the process-wide ROS arguments.
+
+    MoveItPy creates its own node, but ROS 2 applies the process command-line
+    ``-p`` arguments to every node in the process.  The same parameter set is
+    used by the existing move_to_pose_server in iiwa_bringup.
+    """
+    from iiwa_utils import converter, setting_loader
+    from iiwa_utils.converter import effective_joint_limits
+    from moveit_configs_utils import MoveItConfigsBuilder
+
+    settings = setting_loader.build_settings(base_setting, check_files=True)
+    moveit_configs = (
+        MoveItConfigsBuilder('iiwa7', package_name='iiwa_config')
+        .robot_description(
+            file_path=settings.robot.description,
+            mappings={
+                'initial_positions_file': settings.controller.moveit.initial_positions,
+                'joint_limits_file': settings.controller.moveit.joint_limits,
+            },
+        )
+        .robot_description_semantic(file_path=settings.controller.moveit.srdf)
+        .robot_description_kinematics(file_path=settings.controller.moveit.kinematics)
+        .joint_limits(file_path=settings.controller.moveit.joint_limits)
+        .pilz_cartesian_limits(file_path=settings.controller.moveit.pilz_limits)
+        .trajectory_execution(file_path=settings.controller.moveit.moveit_controllers)
+        .moveit_cpp(file_path=settings.controller.moveit.moveit_cpp)
+        .to_moveit_configs()
+    )
+    moveit_configs.joint_limits['robot_description_planning'] = (
+        effective_joint_limits(moveit_configs.joint_limits['robot_description_planning'])
+    )
+    configuration = moveit_configs.to_dict()
+    # The executor checks every planned point itself.  The Ruckig smoothing
+    # chain can overshoot a joint limit on a valid OMPL path and then the
+    # custom LimitSplineDynamics adapter rejects the whole plan.  Keep the
+    # deterministic rest-boundary/time-parameterization and validation
+    # adapters for this process; move_group keeps the full pipeline.
+    ompl = configuration.get('ompl')
+    if isinstance(ompl, dict):
+        ompl['response_adapters'] = [
+            adapter for adapter in ompl.get('response_adapters', [])
+            if adapter not in (
+                'default_planning_response_adapters/AddRuckigTrajectorySmoothing',
+                'iiwa_planning/LimitSplineDynamics',
+            )
+        ]
+    robot_description = converter.load_robot_description(
+        settings.robot.description,
+        settings.robot.name,
+        {
+            'simulate': 'true',
+            'initial_positions_file': settings.controller.moveit.initial_positions,
+            'joint_limits_file': settings.controller.moveit.joint_limits,
+        },
+    )
+    return [
+        configuration,
+        {'robot_description': robot_description},
+        {'use_sim_time': True},
+    ]
+
+
+def runtime_actions(
+    *,
+    experiment_id: int,
+    scene_config: str,
+    base_setting: str,
+    rviz: str,
+    config: dict,
+    moveit_config_parameters: list | None = None,
+) -> list:
+    experiment = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(PathJoinSubstitution([
+            FindPackageShare('iiwa_experement'), *EXPERIMENT_LAUNCH,
+        ])),
+        launch_arguments={
+            'experiment_id': str(experiment_id),
+            'mode': PLANNER_MODE,
+            'scene_config': str(scene_config),
+            'base_setting': str(base_setting),
+            'rviz': str(rviz),
+        }.items(),
+    )
+    scene_context = Node(
+        package='iiwa_scene_context',
+        executable='scene_context',
+        name='iiwa_scene_context',
+        output='screen',
+        parameters=[{
+            'experiment_id': int(experiment_id),
+            'scene_config': str(scene_config),
+            'freshness_sec': float(config.get('scene_freshness_sec', 2.0)),
+            'use_sim_time': True,
+        }],
+    )
+    executor = Node(
+        package='iiwa_llm_plan_executor',
+        executable='llm_plan_executor',
+        name='iiwa_llm_plan_executor',
+        output='screen',
+        parameters=[
+            *(moveit_config_parameters or []),
+            {
+                'experiment_id': int(experiment_id),
+                'scene_config': str(scene_config),
+                'start_delay_sec': float(config.get('start_delay_sec', 3.0)),
+                'scene_max_age_sec': float(config.get('scene_max_age_sec', 2.0)),
+                'max_steps': int(config.get('max_steps', 512)),
+                'close_inspection_objects': list(
+                    config.get('close_inspection_objects', ['cap'])
+                ),
+                'top_camera_topic': str(
+                    config.get('top_camera_topic', '/d455_top/image_raw/image_color')
+                ),
+                'hand_camera_topic': str(
+                    config.get('hand_camera_topic', '/camera/color/image_color')
+                ),
+                'frame_timeout_sec': float(config.get('frame_timeout_sec', 2.0)),
+                'trace_dir': str(
+                    config.get('trace_dir', '~/.ros/iiwa_llm_plan_executor/runs')
+                ),
+                'planning_group': str(config.get('planning_group', 'iiwa_arm')),
+                'pose_link': str(config.get('pose_link', 'tcp')),
+                'use_sim_time': True,
+            },
+        ],
+    )
+    return [experiment, scene_context, executor]
+
+
+def _setup(context, *args, **kwargs):
+    del args, kwargs
+    raw_id = LaunchConfiguration('experiment_id').perform(context)
+    raw_mode = LaunchConfiguration('mode').perform(context)
+    raw_simulate = LaunchConfiguration('simulate').perform(context)
+    try:
+        experiment_id, _ = validate_arguments(raw_id, raw_mode, raw_simulate)
+    except ValueError as error:
+        raise RuntimeError(f'Invalid planner launch arguments: {error}') from error
+    config_path = LaunchConfiguration('config').perform(context)
+    config = load_config(config_path)
+    base_setting = str(LaunchConfiguration('base_setting').perform(context))
+    moveit_config_parameters = moveit_parameters(base_setting)
+    print(
+        f'[iiwa_llm_plan_executor] experiment {experiment_id} planner mode: '
+        f'no reference sequence; waiting for an external graph'
+    )
+    return runtime_actions(
+        experiment_id=experiment_id,
+        scene_config=str(LaunchConfiguration('scene_config').perform(context)),
+        base_setting=base_setting,
+        rviz=str(LaunchConfiguration('rviz').perform(context)).lower(),
+        config=config,
+        moveit_config_parameters=moveit_config_parameters,
+    )
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'experiment_id',
+            default_value='',
+            description='Experiment 1..20; selects only the initial scene.',
+        ),
+        DeclareLaunchArgument(
+            'mode',
+            default_value='planner',
+            description='Must remain planner for externally supplied graphs.',
+        ),
+        DeclareLaunchArgument(
+            'simulate',
+            default_value='true',
+            description='Must remain true; the physical robot is not connected.',
+        ),
+        DeclareLaunchArgument(
+            'rviz',
+            default_value='false',
+            description='true = start RViz together with Webots.',
+        ),
+        DeclareLaunchArgument(
+            'scene_config',
+            default_value=PathJoinSubstitution([
+                FindPackageShare('iiwa_experement'), 'config', 'scenes.yaml',
+            ]),
+            description='Scenario manifest used to build the initial scene.',
+        ),
+        DeclareLaunchArgument(
+            'base_setting',
+            default_value=PathJoinSubstitution([
+                FindPackageShare('iiwa_config'), 'config', 'setting.yaml',
+            ]),
+            description='Base settings for the temporary setting.yaml.',
+        ),
+        DeclareLaunchArgument(
+            'config',
+            default_value=PathJoinSubstitution([
+                FindPackageShare('iiwa_llm_plan_executor'),
+                'config',
+                'planner.yaml',
+            ]),
+            description='Executor defaults: delays, limits, cameras, logging.',
+        ),
+        OpaqueFunction(function=_setup),
+    ])

src/iiwa_llm_plan_executor/package.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>iiwa_llm_plan_executor</name>
+  <version>2026.11.10</version>
+  <description>Validate and execute externally supplied Qwen3 plan graphs in Webots</description>
+  <maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>iiwa_experement</exec_depend>
+  <exec_depend>iiwa_msgs</exec_depend>
+  <exec_depend>iiwa_scene_context</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>moveit_msgs</exec_depend>
+  <exec_depend>moveit_py</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-scipy</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>webots_ros2_msgs</exec_depend>
+
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/iiwa_llm_plan_executor/resource/iiwa_llm_plan_executor

@@ -0,0 +1 @@
+iiwa_llm_plan_executor

src/iiwa_llm_plan_executor/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/iiwa_llm_plan_executor
+[install]
+install_scripts=$base/lib/iiwa_llm_plan_executor

src/iiwa_llm_plan_executor/setup.py

@@ -0,0 +1,30 @@
+from glob import glob
+
+from setuptools import find_packages, setup
+
+package_name = 'iiwa_llm_plan_executor'
+
+setup(
+    name=package_name,
+    version='2026.11.10',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/config', glob('config/*.yaml')),
+        ('share/' + package_name + '/launch', glob('launch/*.py')),
+        ('share/' + package_name + '/doc', glob('doc/*.md') + glob('docs/*.md')),
+    ],
+    install_requires=['setuptools', 'PyYAML'],
+    tests_require=['pytest'],
+    zip_safe=True,
+    maintainer='daniel',
+    maintainer_email='grabardm@ml-dev.ru',
+    description='Validate and execute externally supplied Qwen3 plan graphs in Webots',
+    license='Apache-2.0',
+    entry_points={
+        'console_scripts': [
+            'llm_plan_executor = iiwa_llm_plan_executor.node:main',
+        ],
+    },
+)

src/iiwa_llm_plan_executor/test/data/qwen_cap_full_graph.json

@@ -0,0 +1,430 @@
+{
+  "status": "ready",
+  "entry": "a_find_part",
+  "plan": [
+    {
+      "id": "a_find_part",
+      "skill": "detect",
+      "object": "cap",
+      "target": "",
+      "transitions": [
+        {
+          "event": "found",
+          "next": "a_find_target"
+        },
+        {
+          "event": "not_found",
+          "next": "replan_required"
+        },
+        {
+          "event": "failed",
+          "next": "replan_required"
+        }
+      ]
+    },
+    {
+      "id": "a_find_target",
+      "skill": "detect",
+      "object": "hatch_seat",
+      "target": "",
+      "transitions": [
+        {
+          "event": "found",
+          "next": "a_approach"
+        },
+        {
+          "event": "not_found",
+          "next": "replan_required"
+        },
+        {
+          "event": "failed",
+          "next": "replan_required"
+        }
+      ]
+    },
+    {
+      "id": "a_approach",
+      "skill": "approach",
+      "object": "cap",
+      "target": "",
+      "transitions": [
+        {
+          "event": "success",
+          "next": "a_pick"
+        },
+        {
+          "event": "human_in_scene",
+          "next": "a_stop_human"
+        },
+        {
+          "event": "collision",
+          "next": "stop_collision"
+        },
+        {
+          "event": "failed",
+          "next": "stop_operator"
+        }
+      ]
+    },
+    {
+      "id": "a_pick",
+      "skill": "pick",
+      "object": "cap",
+      "target": "",
+      "transitions": [
+        {
+          "event": "success",
+          "next": "a_transfer"
+        },
+        {
+          "event": "grasp_failed",
+          "next": "a_stop_recover"
+        },
+        {
+          "event": "human_in_scene",
+          "next": "a_stop_human"
+        },
+        {
+          "event": "collision",
+          "next": "stop_collision"
+        },
+        {
+          "event": "failed",
+          "next": "stop_operator"
+        }
+      ]
+    },
+    {
+      "id": "a_transfer",
+      "skill": "move_to_storage",
+      "object": "cap",
+      "target": "hatch_seat",
+      "transitions": [
+        {
+          "event": "success",
+          "next": "a_release"
+        },
+        {
+          "event": "human_in_scene",
+          "next": "a_stop_human"
+        },
+        {
+          "event": "collision",
+          "next": "stop_collision"
+        },
+        {
+          "event": "failed",
+          "next": "stop_operator"
+        }
+      ]
+    },
+    {
+      "id": "a_release",
+      "skill": "release",
+      "object": "cap",
+      "target": "hatch_seat",
+      "transitions": [
+        {
+          "event": "success",
+          "next": "a_inspect"
+        },
+        {
+          "event": "human_in_scene",
+          "next": "a_stop_human"
+        },
+        {
+          "event": "collision",
+          "next": "stop_collision"
+        },
+        {
+          "event": "failed",
+          "next": "stop_operator"
+        }
+      ]
+    },
+    {
+      "id": "a_inspect",
+      "skill": "inspect",
+      "object": "cap",
+      "target": "hatch_seat",
+      "transitions": [
+        {
+          "event": "ok",
+          "next": "a_state"
+        },
+        {
+          "event": "not_ok",
+          "next": "stop_operator"
+        },
+        {
+          "event": "failed",
+          "next": "stop_operator"
+        }
+      ]
+    },
+    {
+      "id": "a_stop_human",
+      "skill": "stop",
+      "object": "robot",
+      "target": "",
+      "transitions": [
+        {
+          "event": "success",
+          "next": "a_wait_clear"
+        },
+        {
+          "event": "failed",
+          "next": "notify"
+        }
+      ]
+    },
+    {
+      "id": "a_wait_clear",
+      "skill": "wait_scene_clear",
+      "object": "workspace",
+      "target": "",
+      "transitions": [
+        {
+          "event": "clear",
+          "next": "a_state"
+        },
+        {
+          "event": "timeout",
+          "next": "notify"
+        },
+        {
+          "event": "failed",
+          "next": "notify"
+        }
+      ]
+    },
+    {
+      "id": "a_stop_recover",
+      "skill": "stop",
+      "object": "robot",
+      "target": "",
+      "transitions": [
+        {
+          "event": "success",
+          "next": "a_state"
+        },
+        {
+          "event": "failed",
+          "next": "notify"
+        }
+      ]
+    },
+    {
+      "id": "a_state",
+      "skill": "inspect_state",
+      "object": "cap",
+      "target": "hatch_seat",
+      "transitions": [
+        {
+          "event": "empty",
+          "next": "a_retry_find_part"
+        },
+        {
+          "event": "holding",
+          "next": "a_retry_find_part"
+        },
+        {
+          "event": "placed",
+          "next": "a_inspect"
+        },
+        {
+          "event": "unknown",
+          "next": "notify"
+        }
+      ]
+    },
+    {
+      "id": "a_retry_find_part",
+      "skill": "detect",
+      "object": "cap",
+      "target": "",
+      "transitions": [
+        {
+          "event": "found",
+          "next": "a_retry_approach"
+        },
+        {
+          "event": "not_found",
+          "next": "replan_required"
+        },
+        {
+          "event": "failed",
+          "next": "replan_required"
+        }
+      ]
+    },
+    {
+      "id": "a_retry_approach",
+      "skill": "approach",
+      "object": "cap",
+      "target": "",
+      "transitions": [
+        {
+          "event": "success",
+          "next": "a_retry_pick"
+        },
+        {
+          "event": "human_in_scene",
+          "next": "stop_operator"
+        },
+        {
+          "event": "collision",
+          "next": "stop_collision"
+        },
+        {
+          "event": "failed",
+          "next": "stop_operator"
+        }
+      ]
+    },
+    {
+      "id": "a_retry_pick",
+      "skill": "pick",
+      "object": "cap",
+      "target": "",
+      "transitions": [
+        {
+          "event": "success",
+          "next": "a_retry_find_target"
+        },
+        {
+          "event": "grasp_failed",
+          "next": "stop_operator"
+        },
+        {
+          "event": "human_in_scene",
+          "next": "stop_operator"
+        },
+        {
+          "event": "collision",
+          "next": "stop_collision"
+        },
+        {
+          "event": "failed",
+          "next": "stop_operator"
+        }
+      ]
+    },
+    {
+      "id": "a_retry_find_target",
+      "skill": "detect",
+      "object": "hatch_seat",
+      "target": "",
+      "transitions": [
+        {
+          "event": "found",
+          "next": "a_retry_transfer"
+        },
+        {
+          "event": "not_found",
+          "next": "replan_required"
+        },
+        {
+          "event": "failed",
+          "next": "replan_required"
+        }
+      ]
+    },
+    {
+      "id": "a_retry_transfer",
+      "skill": "move_to_storage",
+      "object": "cap",
+      "target": "hatch_seat",
+      "transitions": [
+        {
+          "event": "success",
+          "next": "a_retry_release"
+        },
+        {
+          "event": "human_in_scene",
+          "next": "stop_operator"
+        },
+        {
+          "event": "collision",
+          "next": "stop_collision"
+        },
+        {
+          "event": "failed",
+          "next": "stop_operator"
+        }
+      ]
+    },
+    {
+      "id": "a_retry_release",
+      "skill": "release",
+      "object": "cap",
+      "target": "hatch_seat",
+      "transitions": [
+        {
+          "event": "success",
+          "next": "a_inspect"
+        },
+        {
+          "event": "human_in_scene",
+          "next": "stop_operator"
+        },
+        {
+          "event": "collision",
+          "next": "stop_collision"
+        },
+        {
+          "event": "failed",
+          "next": "stop_operator"
+        }
+      ]
+    },
+    {
+      "id": "stop_collision",
+      "skill": "stop",
+      "object": "robot",
+      "target": "",
+      "transitions": [
+        {
+          "event": "success",
+          "next": "notify"
+        },
+        {
+          "event": "failed",
+          "next": "notify"
+        }
+      ]
+    },
+    {
+      "id": "stop_operator",
+      "skill": "stop",
+      "object": "robot",
+      "target": "",
+      "transitions": [
+        {
+          "event": "success",
+          "next": "notify"
+        },
+        {
+          "event": "failed",
+          "next": "notify"
+        }
+      ]
+    },
+    {
+      "id": "notify",
+      "skill": "notify_operator",
+      "object": "robot",
+      "target": "",
+      "transitions": [
+        {
+          "event": "success",
+          "next": "operator_required"
+        },
+        {
+          "event": "failed",
+          "next": "operator_required"
+        }
+      ]
+    }
+  ]
+}

src/iiwa_llm_plan_executor/test/test_engine.py

@@ -0,0 +1,484 @@
+"""Graph engine: admission, one-edge transitions, terminals and cancellation."""
+from __future__ import annotations
+
+import threading
+from pathlib import Path
+from types import SimpleNamespace
+
+from iiwa_experement.geometry import load_geometry
+from iiwa_experement.scenario import load_scenario
+from iiwa_llm_plan_executor.engine import GraphEngine
+from iiwa_llm_plan_executor.schema import parse_response
+from iiwa_scene_context.catalog import DEFAULT_CATALOG
+from iiwa_scene_context.snapshot import build_envelope, build_scene
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+SCENES = PACKAGE.parent / 'iiwa_experement' / 'config' / 'scenes.yaml'
+GEOMETRY = load_geometry(PACKAGE.parent / 'iiwa_experement' / 'config' / 'geometry.yaml')
+SAFE = {
+    'collision': 'operator_required',
+    'grasp_failed': 'operator_required',
+    'human_in_scene': 'operator_required',
+    'not_found': 'replan_required',
+    'failed': 'replan_required',
+    'not_ok': 'operator_required',
+    'timeout': 'operator_required',
+    'unknown': 'operator_required',
+}
+PROGRESS = {'success', 'ok', 'found', 'clear', 'empty', 'holding', 'placed'}
+
+
+class FakeContext:
+    def __init__(self, scene_json):
+        self.scene_json = scene_json
+        self.mounted_tool = None
+        self.held_object = None
+        self.scene_revision = 0
+        self.ownership_unknown = False
+
+
+class FakeSkills:
+    def __init__(self):
+        self.calls = []
+        self.events = {}
+        self.details = {}
+        self.on_execute = None
+        self.block = False
+        self.started = threading.Event()
+        self.release = threading.Event()
+
+    def execute(self, node, context):
+        if self.on_execute is not None:
+            self.on_execute(node, context)
+        if self.block:
+            self.started.set()
+            self.release.wait(timeout=5.0)
+        self.calls.append(node.id)
+        return SimpleNamespace(
+            event=self.events.get(node.id, 'success'),
+            details=self.details.get(node.id, {}),
+        )
+
+
+def envelope(revision=1, stamp=5.0, experiment_id=1):
+    spec = load_scenario(experiment_id, SCENES)
+    tools = {tool.name: {'state': 'docked'} for tool in spec.tools}
+    scene = build_scene(
+        spec,
+        {
+            'version': revision,
+            'mounted_tool': None,
+            'held_object': None,
+            'ready': True,
+            'cap': {'state': spec.cap_state},
+            'tools': tools,
+            'screws': {},
+            'holes': {},
+        },
+        DEFAULT_CATALOG,
+        GEOMETRY,
+        stamp=stamp,
+    )
+    return build_envelope(
+        scene,
+        state_version=revision,
+        stamp=stamp,
+        experiment_id=experiment_id,
+    )
+
+
+def node(node_id, skill, obj='', target='', next_on=None):
+    events = DEFAULT_CATALOG.outcomes(skill)
+    next_on = next_on or {}
+    transitions = []
+    for event in events:
+        if event in next_on:
+            destination = next_on[event]
+        elif event in PROGRESS:
+            destination = 'completed'
+        else:
+            destination = SAFE.get(event, 'operator_required')
+        transitions.append({'event': event, 'next': destination})
+    return {
+        'id': node_id,
+        'skill': skill,
+        'object': obj,
+        'target': target,
+        'transitions': transitions,
+    }
+
+
+def graph(entry, *nodes):
+    return parse_response({
+        'status': 'ready',
+        'entry': entry,
+        'plan': list(nodes),
+    })
+
+
+STOP_GRAPH = lambda: graph(
+    'a',
+    node('a', 'stop', 'robot', next_on={'success': 'b'}),
+    node('b', 'stop', 'robot', next_on={'success': 'completed'}),
+)
+
+GRAB_GRAPH = lambda: graph(
+    'g',
+    node('g', 'grab', 'vacuum', next_on={'success': 't'}),
+    node('t', 'transfer', 'vacuum', 'work',
+         next_on={'success': 'completed'}),
+)
+
+BAD_GRAPH = lambda: graph(
+    'g',
+    node('g', 'grab', 'vacuum', next_on={'success': 'ghost'}),
+)
+
+
+def make_engine(skills=None, *, clock=5.0, provider=None, threaded=False,
+                max_steps=512, **kwargs):
+    skills = skills or FakeSkills()
+    clock_box = {'now': clock}
+    state = {'envelope': envelope(1, clock_box['now'])}
+    if provider is not None:
+        state['provider'] = provider
+
+    def default_provider():
+        return state['envelope']
+    events = {
+        'plans': [],
+        'statuses': [],
+        'replans': [],
+        'stops': [],
+    }
+    engine = GraphEngine(
+        state.get('provider', default_provider),
+        lambda env, scene: FakeContext(scene),
+        skills,
+        clock=lambda: clock_box['now'],
+        on_plan=lambda payload: events['plans'].append(payload),
+        on_status=lambda payload: events['statuses'].append(payload),
+        on_replan=lambda payload: events['replans'].append(payload),
+        on_stop=lambda reason: events['stops'].append(reason),
+        robot_fresh=lambda: (True, 'ok'),
+        threaded=threaded,
+        max_steps=max_steps,
+        **kwargs,
+    )
+    engine.update_readiness(scene_ready=True, ready_at=0.0, motion_ready=True)
+    return engine, skills, events, state, clock_box
+
+
+def test_submission_waits_for_scene_and_the_three_second_delay():
+    engine, _, _, _, _ = make_engine(clock=2.0)
+    rejected = engine.submit('r1', STOP_GRAPH().as_dict())
+    assert rejected.accepted is False
+    assert rejected.code == 'not_ready'
+    assert 'start delay' in rejected.message
+    assert engine.get_status('r1').found is False
+
+    engine, _, _, _, _ = make_engine(clock=3.01)
+    accepted = engine.submit('r1', STOP_GRAPH().as_dict())
+    assert accepted.accepted is True
+    engine.run_accepted('r1')
+
+    engine.update_readiness(scene_ready=False, ready_at=0.0, motion_ready=False)
+    rejected = engine.submit('r2', STOP_GRAPH().as_dict())
+    assert rejected.code == 'not_ready'
+
+
+def test_rejected_graphs_are_not_registered_published_or_executed():
+    engine, skills, events, _, _ = make_engine()
+    blocked = engine.submit('r1', {'status': 'blocked', 'entry': '', 'plan': []})
+    assert blocked.code == 'blocked'
+    rejected = engine.submit('r2', BAD_GRAPH().as_dict())
+    assert rejected.accepted is False
+    assert rejected.code == 'rejected'
+    assert events['plans'] == []
+    assert skills.calls == []
+    assert engine.get_status('r1').found is False
+    assert engine.get_status('r2').found is False
+    assert not any(
+        event.get('state') == 'rejected' for event in events['statuses']
+    )
+
+
+def test_rejected_run_id_can_be_reused_with_a_corrected_graph():
+    engine, skills, events, _, _ = make_engine()
+    rejected = engine.submit('same-id', BAD_GRAPH().as_dict())
+    assert rejected.accepted is False
+    assert engine.get_status('same-id').found is False
+
+    accepted = engine.submit('same-id', STOP_GRAPH().as_dict())
+    assert accepted.accepted is True
+    assert accepted.code == 'accepted'
+    assert len(events['plans']) == 1
+    engine.run_accepted('same-id')
+    assert engine.get_status('same-id').state == 'completed'
+
+
+def test_cap_grab_without_vacuum_is_admitted_and_follows_grasp_failed():
+    engine, skills, events, _, _ = make_engine()
+    value = graph(
+        'g',
+        node('g', 'grab', 'cap', next_on={
+            'success': 'completed',
+            'grasp_failed': 'operator_required',
+        }),
+    )
+    accepted = engine.submit('r1', value.as_dict())
+    assert accepted.accepted is True
+    assert len(events['plans']) == 1
+    skills.events = {'g': 'grasp_failed'}
+    skills.details = {'g': {'reason': 'required tool vacuum is not mounted'}}
+    engine.run_accepted('r1')
+    status = engine.get_status('r1')
+    assert status.state == 'operator_required'
+    assert status.events[0]['event'] == 'grasp_failed'
+    assert status.events[0]['next'] == 'operator_required'
+    assert status.events[0]['reason']
+    assert status.mounted_tool is None
+    assert status.held_object is None
+
+
+def test_accepted_graph_is_published_once_before_the_first_motion():
+    engine, skills, events, _, _ = make_engine()
+    order = []
+
+    def on_plan(payload):
+        events['plans'].append(payload)
+        order.append(('plan', payload['run_id']))
+
+    engine.on_plan = on_plan
+    skills.on_execute = lambda node, context: order.append(('move', node.id))
+    accepted = engine.submit('r1', STOP_GRAPH().as_dict())
+    assert accepted.accepted
+    engine.run_accepted('r1')
+    assert order[0] == ('plan', 'r1')
+    assert order[1] == ('move', 'a')
+    assert len(events['plans']) == 1
+    status = engine.get_status('r1')
+    assert status.state == 'completed'
+    assert status.found is True
+    assert 'scene_revision' not in status.as_dict()
+
+
+def test_same_run_id_is_idempotent_and_a_different_graph_conflicts():
+    engine, _, events, _, _ = make_engine()
+    first = engine.submit('r1', STOP_GRAPH().as_dict())
+    assert first.accepted
+    duplicate = engine.submit('r1', STOP_GRAPH().as_dict())
+    assert duplicate.accepted is True
+    assert duplicate.code == 'duplicate'
+    assert len(events['plans']) == 1
+
+    conflict = engine.submit('r1', GRAB_GRAPH().as_dict())
+    assert conflict.accepted is False
+    assert conflict.code == 'conflict'
+    assert len(events['plans']) == 1
+
+    busy = engine.submit('r2', STOP_GRAPH().as_dict())
+    assert busy.accepted is False
+    assert busy.code == 'busy'
+    assert len(events['plans']) == 1
+
+
+def test_accepted_run_id_stays_bound_after_a_terminal_state():
+    engine, _, events, _, _ = make_engine()
+    assert engine.submit('r1', STOP_GRAPH().as_dict()).accepted
+    engine.run_accepted('r1')
+    assert engine.get_status('r1').state == 'completed'
+
+    duplicate = engine.submit('r1', STOP_GRAPH().as_dict())
+    assert duplicate.accepted is True
+    assert duplicate.code == 'duplicate'
+    conflict = engine.submit('r1', GRAB_GRAPH().as_dict())
+    assert conflict.accepted is False
+    assert conflict.code == 'conflict'
+    assert len(events['plans']) == 1
+
+
+def test_plan_publication_failure_does_not_register_the_run():
+    engine, skills, _, _, _ = make_engine()
+
+    def broken(_payload):
+        raise RuntimeError('publisher unavailable')
+
+    engine.on_plan = broken
+    rejected = engine.submit('r1', STOP_GRAPH().as_dict())
+    assert rejected.accepted is False
+    assert rejected.code == 'rejected'
+    assert 'publisher unavailable' in rejected.message
+    assert engine.get_status('r1').found is False
+    assert skills.calls == []
+
+
+def test_each_event_selects_exactly_one_edge_and_is_visible_in_status():
+    engine, skills, _, _, _ = make_engine()
+    engine.submit('r1', STOP_GRAPH().as_dict())
+    skills.events = {'a': 'success', 'b': 'success'}
+    engine.run_accepted('r1')
+    status = engine.get_status('r1')
+    assert status.state == 'completed'
+    assert [event['node_id'] for event in status.events] == ['a', 'b']
+    assert [event['next'] for event in status.events] == ['b', 'completed']
+    assert all(event['run_id'] == 'r1' for event in status.events)
+    assert all('scene_revision' not in event for event in status.events)
+
+
+def test_replan_returns_a_fresh_scene_and_operator_required_stops():
+    engine, skills, events, _, _ = make_engine()
+    replan_graph = graph(
+        'd',
+        node('d', 'detect', 'cap',
+             next_on={'found': 'replan_required'}),
+    )
+    engine.submit('r1', replan_graph.as_dict())
+    skills.events = {'d': 'found'}
+    engine.run_accepted('r1')
+    assert engine.get_status('r1').state == 'replan_required'
+    assert len(events['replans']) == 1
+    assert events['replans'][0]['state_version'] == 1
+
+    engine, skills, events, _, _ = make_engine()
+    operator_graph = graph(
+        'd',
+        node('d', 'detect', 'cap',
+             next_on={'found': 'operator_required'}),
+    )
+    engine.submit('r2', operator_graph.as_dict())
+    skills.events = {'d': 'found'}
+    engine.run_accepted('r2')
+    assert engine.get_status('r2').state == 'operator_required'
+
+
+def test_unknown_skill_outcome_stops_execution():
+    engine, skills, events, _, _ = make_engine()
+    engine.submit('r1', STOP_GRAPH().as_dict())
+    skills.events = {'a': 'bogus'}
+    engine.run_accepted('r1')
+    status = engine.get_status('r1')
+    assert status.state == 'failed'
+    assert 'unknown skill outcome' in status.reason
+    assert events['stops']
+
+
+def test_lost_freshness_and_missing_scene_stop_motion():
+    engine, _, events, _, _ = make_engine()
+    engine.robot_fresh = lambda: (False, 'joint_states is stale')
+    engine.submit('r1', STOP_GRAPH().as_dict())
+    engine.run_accepted('r1')
+    assert engine.get_status('r1').state == 'failed'
+    assert 'joint_states' in engine.get_status('r1').reason
+    assert events['stops']
+
+    engine, skills, events, state, _ = make_engine()
+    engine.submit('r2', STOP_GRAPH().as_dict())
+    skills.on_execute = lambda node, context: state.update(envelope=None)
+    engine.run_accepted('r2')
+    assert engine.get_status('r2').state == 'failed'
+    assert 'scene snapshot' in engine.get_status('r2').reason
+
+
+def test_scene_state_change_stops_non_ownership_nodes_but_not_own_graph_changes():
+    engine, skills, events, state, _ = make_engine()
+    engine.submit('r1', STOP_GRAPH().as_dict())
+    skills.on_execute = lambda node, context: state.update(envelope=envelope(2))
+    engine.run_accepted('r1')
+    assert engine.get_status('r1').state == 'failed'
+    assert 'changed externally' in engine.get_status('r1').reason
+
+    engine, skills, events, state, _ = make_engine()
+    engine.submit('r2', GRAB_GRAPH().as_dict())
+
+    def own_change(node, context):
+        if node.id == 'g':
+            context.mounted_tool = 'vacuum'
+            state.update(envelope=envelope(2))
+
+    skills.on_execute = own_change
+    engine.run_accepted('r2')
+    status = engine.get_status('r2')
+    assert status.state == 'completed'
+    assert status.mounted_tool == 'vacuum'
+
+
+def test_changed_scene_before_the_first_motion_stops_the_run():
+    engine, skills, events, state, _ = make_engine()
+    accepted = engine.submit('r1', STOP_GRAPH().as_dict())
+    assert accepted.accepted
+    state['envelope'] = envelope(2)
+    engine.run_accepted('r1')
+    status = engine.get_status('r1')
+    assert status.state == 'failed'
+    assert 'before the first motion' in status.reason
+    assert skills.calls == []
+    assert events['stops']
+
+
+def test_step_limit_stops_a_recovering_cycle():
+    engine, skills, _, _, _ = make_engine(max_steps=3)
+    cycle = graph(
+        'a',
+        node('a', 'wait_scene_clear', 'workspace',
+             next_on={'clear': 'b'}),
+        node('b', 'wait_scene_clear', 'workspace',
+             next_on={'clear': 'a'}),
+    )
+    engine.submit('r1', cycle.as_dict())
+    skills.events = {'a': 'clear', 'b': 'clear'}
+    engine.run_accepted('r1')
+    status = engine.get_status('r1')
+    assert status.state == 'failed'
+    assert 'step limit' in status.reason
+
+
+def test_cancellation_stops_the_active_graph_and_preserves_ownership():
+    engine, skills, events, _, _ = make_engine(threaded=True)
+    skills.block = True
+    skills.on_execute = (
+        lambda node, context: setattr(context, 'mounted_tool', node.object)
+        if node.skill == 'grab' else None
+    )
+    engine.submit('r1', GRAB_GRAPH().as_dict())
+    assert skills.started.wait(timeout=2.0)
+    result = engine.cancel('r1')
+    assert result.accepted
+    assert events['stops']
+    skills.release.set()
+    assert engine.wait('r1', timeout=2.0)
+    status = engine.get_status('r1')
+    assert status.state == 'canceled'
+    assert status.mounted_tool == 'vacuum'
+
+
+def test_cancel_rejects_unknown_or_finished_runs():
+    engine, _, _, _, _ = make_engine()
+    assert engine.cancel('missing').accepted is False
+    engine.submit('r1', STOP_GRAPH().as_dict())
+    engine.run_accepted('r1')
+    assert engine.cancel('r1').accepted is False
+
+
+def test_status_query_marks_unknown_ids():
+    engine, _, _, _, _ = make_engine()
+    status = engine.get_status('nope')
+    assert status.found is False
+    assert status.state == 'unknown'
+
+
+def test_not_ready_or_unavailable_scene_is_rejected_without_registration():
+    engine, _, _, state, _ = make_engine()
+    state['envelope'] = None
+    rejected = engine.submit('r1', STOP_GRAPH().as_dict())
+    assert rejected.code == 'not_ready'
+    assert engine.get_status('r1').found is False
+
+
+def test_stale_scene_timestamp_is_rejected_before_execution():
+    engine, _, _, state, _ = make_engine(clock=10.0)
+    state['envelope'] = envelope(1, stamp=1.0)
+    rejected = engine.submit('r1', STOP_GRAPH().as_dict())
+    assert rejected.code == 'stale_scene'
+    assert engine.get_status('r1').found is False

src/iiwa_llm_plan_executor/test/test_full_qwen_graph.py

@@ -0,0 +1,207 @@
+"""Integration tests with the complete Qwen graph submitted by the user."""
+from __future__ import annotations
+
+import copy
+import json
+from pathlib import Path
+
+from iiwa_experement.geometry import load_geometry, world_target
+from iiwa_experement.object_state import transform_json
+from iiwa_experement.scenario import load_scenario
+from iiwa_llm_plan_executor.engine import GraphEngine
+from iiwa_llm_plan_executor.moveit_backend import MotionOutcome
+from iiwa_llm_plan_executor.schema import parse_response
+from iiwa_llm_plan_executor.skill_context import SkillContext
+from iiwa_llm_plan_executor.skills import SkillRegistry
+from iiwa_llm_plan_executor.validator import validate_graph
+from iiwa_scene_context.catalog import DEFAULT_CATALOG
+from iiwa_scene_context.snapshot import build_envelope, build_scene
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+ASSETS = PACKAGE.parent / 'iiwa_experement' / 'config'
+SCENES = ASSETS / 'scenes.yaml'
+GRAPH_PATH = Path(__file__).resolve().parent / 'data' / 'qwen_cap_full_graph.json'
+GEOMETRY = load_geometry(ASSETS / 'geometry.yaml')
+SPEC = load_scenario(20, SCENES)
+FULL_GRAPH = parse_response(json.loads(GRAPH_PATH.read_text(encoding='utf-8')))
+
+
+class FakeMotion:
+    def __init__(self):
+        self.calls = []
+        self.stop_calls = []
+
+    def plan_and_execute(self, goal, phase, held_object):
+        self.calls.append((goal, phase, held_object))
+        return MotionOutcome(True, 'ok', 'ok')
+
+    def stop(self, reason='stop'):
+        self.stop_calls.append(reason)
+
+
+class FakeSupervisor:
+    def __init__(self):
+        self.commands = []
+        self.state = {
+            'version': 1,
+            'mounted_tool': None,
+            'held_object': None,
+            'cap': {
+                'state': 'free',
+                'pose': transform_json(world_target(SPEC, GEOMETRY, 'cap')),
+            },
+            'tools': {
+                tool.name: {'state': 'docked'}
+                for tool in SPEC.tools
+            },
+            'screws': {},
+            'holes': {},
+            'objects': {},
+        }
+
+    def latest(self):
+        return copy.deepcopy(self.state)
+
+    def command(self, *args, **kwargs):
+        self.commands.append((args, kwargs))
+        raise AssertionError(
+            'no supervisor command is allowed on the missing-tool path'
+        )
+
+
+def envelope(*, mounted: str | None = None):
+    tools = {tool.name: {'state': 'docked'} for tool in SPEC.tools}
+    if mounted is not None:
+        tools[mounted] = {'state': 'mounted'}
+    scene = build_scene(
+        SPEC,
+        {
+            'version': 1,
+            'mounted_tool': mounted,
+            'held_object': None,
+            'ready': True,
+            'cap': {'state': 'free'},
+            'tools': tools,
+            'screws': {},
+            'holes': {},
+        },
+        DEFAULT_CATALOG,
+        GEOMETRY,
+        stamp=5.0,
+    )
+    return build_envelope(
+        scene,
+        state_version=1,
+        stamp=5.0,
+        experiment_id=20,
+    )
+
+
+def make_engine(*, mounted: str | None = None):
+    scene_envelope = envelope(mounted=mounted)
+    motion = FakeMotion()
+    supervisor = FakeSupervisor()
+    registry = SkillRegistry(motion, supervisor)
+
+    def context_factory(env, scene_json):
+        del scene_json
+        context = SkillContext(
+            spec=SPEC,
+            geometry=GEOMETRY,
+            scene_revision=int(env['state_version']),
+        )
+        context.frame_provider = lambda name: 5.0
+        context.notifier = lambda reason, details: True
+        return context
+
+    engine = GraphEngine(
+        lambda: scene_envelope,
+        context_factory,
+        registry,
+        clock=lambda: 5.0,
+        threaded=False,
+        min_start_delay_sec=3.0,
+    )
+    engine.update_readiness(scene_ready=True, ready_at=0.0, motion_ready=True)
+    return engine, motion, supervisor, scene_envelope
+
+
+def test_full_qwen_graph_empty_patron_follows_actual_failure_path():
+    engine, motion, supervisor, scene_envelope = make_engine(mounted=None)
+    result = validate_graph(FULL_GRAPH, scene_envelope, DEFAULT_CATALOG)
+    assert result.ok is True, result.reason
+
+    accepted = engine.submit('full-empty', FULL_GRAPH.as_dict())
+    assert accepted.accepted is True
+    engine.run_accepted('full-empty')
+    status = engine.get_status('full-empty')
+    assert status.state == 'operator_required'
+    assert [event['node_id'] for event in status.events] == [
+        'a_find_part',
+        'a_find_target',
+        'a_approach',
+        'a_pick',
+        'a_stop_recover',
+        'a_state',
+        'a_retry_find_part',
+        'a_retry_approach',
+        'a_retry_pick',
+        'stop_operator',
+        'notify',
+    ]
+    pick_events = [
+        event for event in status.events if event['node_id'] == 'a_pick'
+    ]
+    state_events = [
+        event for event in status.events if event['node_id'] == 'a_state'
+    ]
+    retry_pick = [
+        event for event in status.events if event['node_id'] == 'a_retry_pick'
+    ]
+    assert pick_events[0]['event'] == 'grasp_failed'
+    assert pick_events[0]['reason'] == 'required tool vacuum is not mounted'
+    assert pick_events[0]['next'] == 'a_stop_recover'
+    assert state_events[0]['event'] == 'empty'
+    assert state_events[0]['next'] == 'a_retry_find_part'
+    assert retry_pick[0]['event'] == 'grasp_failed'
+    assert retry_pick[0]['next'] == 'stop_operator'
+    assert status.mounted_tool is None
+    assert status.held_object is None
+    assert supervisor.state['cap']['state'] == 'free'
+    assert supervisor.state['held_object'] is None
+    assert supervisor.commands == []
+    assert not any(phase in ('lower', 'contact') for _, phase, _ in motion.calls)
+
+
+def test_full_qwen_graph_with_vacuum_has_a_success_cycle_without_completed():
+    # The nominal successful branch is inspect ok -> state -> placed -> inspect.
+    a_inspect = FULL_GRAPH.node('a_inspect')
+    a_state = FULL_GRAPH.node('a_state')
+    assert {
+        transition.event: transition.next
+        for transition in a_inspect.transitions
+    }['ok'] == 'a_state'
+    assert {
+        transition.event: transition.next
+        for transition in a_state.transitions
+    }['placed'] == 'a_inspect'
+
+    all_nexts = {
+        transition.next
+        for node in FULL_GRAPH.nodes
+        for transition in node.transitions
+    }
+    assert 'completed' not in all_nexts
+
+    # With the vacuum mounted the graph is still not executable as a whole:
+    # the generic grasp_failed branch leads through inspect_state holding to a
+    # retry pick while the hatch is held, which the ownership check rejects.
+    result = validate_graph(
+        FULL_GRAPH,
+        envelope(mounted='vacuum'),
+        DEFAULT_CATALOG,
+    )
+    assert result.ok is False
+    assert 'a_retry_pick' in result.reason
+    assert 'holding' in result.reason

src/iiwa_llm_plan_executor/test/test_launch.py

@@ -0,0 +1,139 @@
+"""Launch validation and wiring for the planner scene."""
+from __future__ import annotations
+
+import importlib.util
+from pathlib import Path
+
+import pytest
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
+from launch_ros.actions import Node
+
+from iiwa_experement.runtime_config import EXPERIMENT_IDS, validate_experiment_id
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+LAUNCH = PACKAGE / 'launch' / 'planner.launch.py'
+CONFIG = PACKAGE / 'config' / 'planner.yaml'
+
+
+def load_launch_module():
+    spec = importlib.util.spec_from_file_location('planner_launch', LAUNCH)
+    module = importlib.util.module_from_spec(spec)
+    assert spec.loader is not None
+    spec.loader.exec_module(module)
+    return module
+
+
+def flat_parameters(node: Node):
+    def flatten(value):
+        if hasattr(value, 'text'):
+            return [value.text]
+        if isinstance(value, (list, tuple)):
+            result = []
+            for item in value:
+                result.extend(flatten(item))
+            return result
+        return [value]
+
+    values = []
+    for parameter_set in node._Node__parameters:
+        for value in parameter_set.values():
+            values.extend(flatten(value))
+    return values
+
+
+def test_valid_experiment_ids_and_planner_mode():
+    module = load_launch_module()
+    for experiment_id in EXPERIMENT_IDS:
+        parsed, mode = module.validate_arguments(
+            str(experiment_id), 'planner', 'true',
+        )
+        assert parsed == experiment_id
+        assert mode == 'planner'
+    assert validate_experiment_id(' 20 ') == 20
+
+
+@pytest.mark.parametrize('raw', ['', '0', '21', 'abc', '1.0'])
+def test_invalid_experiment_ids_fail_before_webots(raw):
+    module = load_launch_module()
+    with pytest.raises(ValueError):
+        module.validate_arguments(raw, 'planner', 'true')
+
+
+def test_unknown_mode_or_physical_robot_is_rejected_before_webots():
+    module = load_launch_module()
+    with pytest.raises(ValueError, match='mode'):
+        module.validate_arguments('20', 'execute', 'true')
+    with pytest.raises(ValueError, match='simulate'):
+        module.validate_arguments('20', 'planner', 'false')
+
+
+def test_runtime_actions_start_scene_context_and_executor_only():
+    module = load_launch_module()
+    config = module.load_config(CONFIG)
+    actions = module.runtime_actions(
+        experiment_id=20,
+        scene_config='/tmp/scenes.yaml',
+        base_setting='/tmp/setting.yaml',
+        rviz='false',
+        config=config,
+    )
+    includes = [action for action in actions if isinstance(action, IncludeLaunchDescription)]
+    nodes = [action for action in actions if isinstance(action, Node)]
+    assert len(includes) == 1
+    assert len(nodes) == 2
+    arguments = dict(includes[0].launch_arguments)
+    assert arguments['experiment_id'] == '20'
+    assert arguments['mode'] == 'planner'
+    assert arguments['scene_config'] == '/tmp/scenes.yaml'
+    assert arguments['base_setting'] == '/tmp/setting.yaml'
+
+    packages = {node.node_package for node in nodes}
+    executables = {node.node_executable for node in nodes}
+    assert packages == {'iiwa_scene_context', 'iiwa_llm_plan_executor'}
+    assert executables == {'scene_context', 'llm_plan_executor'}
+    assert 'reference_runner' not in executables
+    assert 'iiwa_web' not in packages
+
+    scene_node = next(node for node in nodes if node.node_executable == 'scene_context')
+    executor_node = next(
+        node for node in nodes if node.node_executable == 'llm_plan_executor'
+    )
+    scene_values = flat_parameters(scene_node)
+    executor_values = flat_parameters(executor_node)
+    assert 20 in scene_values
+    assert 20 in executor_values
+    assert 3.0 in executor_values
+    assert 512 in executor_values
+    assert True in executor_values
+    assert any('iiwa_arm' in str(value) for value in executor_values)
+
+
+def test_config_contains_the_documented_defaults():
+    module = load_launch_module()
+    config = module.load_config(CONFIG)
+    assert config['start_delay_sec'] == 3.0
+    assert config['scene_max_age_sec'] == 2.0
+    assert config['max_steps'] == 512
+    assert config['close_inspection_objects'] == ['cap']
+    assert config['trace_dir'].endswith('iiwa_llm_plan_executor/runs')
+
+
+def test_launch_description_declares_all_arguments_and_validates_in_opaque():
+    module = load_launch_module()
+    description = module.generate_launch_description()
+    arguments = {
+        entity.name: entity.default_value
+        for entity in description.entities
+        if isinstance(entity, DeclareLaunchArgument)
+    }
+    assert set(arguments) == {
+        'experiment_id', 'mode', 'simulate', 'rviz', 'scene_config',
+        'base_setting', 'config',
+    }
+    assert [item.text for item in arguments['experiment_id']] == ['']
+    assert [item.text for item in arguments['mode']] == ['planner']
+    assert [item.text for item in arguments['simulate']] == ['true']
+    assert len([
+        entity for entity in description.entities if isinstance(entity, OpaqueFunction)
+    ]) == 1

src/iiwa_llm_plan_executor/test/test_motion_policy.py

@@ -0,0 +1,185 @@
+"""Trajectory-level limits: J7 corridor, tilt, jumps and freshness."""
+from __future__ import annotations
+
+import math
+from pathlib import Path
+
+import numpy as np
+import pytest
+from scipy.spatial.transform import Rotation
+
+from iiwa_experement.geometry import Transform
+from iiwa_llm_plan_executor.motion_policy import MotionPolicy, load_limits
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+LIMITS = PACKAGE / 'config' / 'motion_limits.yaml'
+BASE = (0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0)
+
+
+def policy() -> MotionPolicy:
+    return MotionPolicy(load_limits(LIMITS))
+
+
+def point(j7: float, base=BASE):
+    values = list(base)
+    values[6] = j7
+    return tuple(values)
+
+
+def down_pose(tilt_deg: float = 0.0, xyz=(0.4, 0.0, 1.2)) -> Transform:
+    rotation = (
+        Rotation.from_euler('x', 180.0, degrees=True).as_matrix()
+        @ Rotation.from_euler('y', tilt_deg, degrees=True).as_matrix()
+    )
+    return Transform(
+        xyz=tuple(float(value) for value in xyz),
+        rotation=tuple(tuple(float(value) for value in row) for row in rotation),
+        parent='world',
+        child='tcp',
+    )
+
+
+def test_work_joint7_is_the_preference_not_a_hard_corridor():
+    limits = load_limits(LIMITS)
+    assert limits.work_joint7_rad == 0.0
+    checker = MotionPolicy(limits)
+
+
+def test_safe_joint7_beyond_the_old_ninety_degree_band_is_accepted():
+    checker = policy()
+    for value in (-2.0, 2.0):
+        result = checker.validate_trajectory(
+            [point(value)], [down_pose()],
+        )
+        assert result.ok, result.reason
+    result = checker.validate_trajectory(
+        [point(-1.8), point(-2.0), point(-1.8)],
+        [down_pose(), down_pose(), down_pose()],
+    )
+    assert result.ok, result.reason
+    assert result.details['max_j7_deviation_rad'] == pytest.approx(2.0)
+
+
+def test_real_joint_limit_is_still_enforced():
+    checker = MotionPolicy(
+        load_limits(LIMITS),
+        joint_limits={
+            'joint1': (-2.967, 2.967),
+            'joint2': (-2.09, 2.09),
+            'joint3': (-2.967, 2.967),
+            'joint4': (-2.09, 2.09),
+            'joint5': (-2.967, 2.967),
+            'joint6': (-2.0027, 2.0027),
+            'joint7': (-3.05, 3.05),
+        },
+    )
+    result = checker.validate_trajectory(
+        [point(3.2)], [down_pose()],
+    )
+    assert result.ok is False
+    assert 'joint7' in result.reason
+    assert 'joint limit' in result.reason
+
+
+def test_near_pi_joint_jump_is_rejected():
+    result = policy().validate_trajectory(
+        [point(-1.4), point(1.4)],
+        [down_pose(), down_pose()],
+    )
+    assert result.ok is False
+    assert 'jump' in result.reason
+
+
+def test_tcp_tilt_above_ten_degrees_is_rejected():
+    checker = policy()
+    assert checker.validate_trajectory(
+        [point(0.0)], [down_pose(9.9)],
+    ).ok
+    rejected = checker.validate_trajectory(
+        [point(0.0)], [down_pose(10.5)],
+    )
+    assert rejected.ok is False
+    assert 'tilt' in rejected.reason
+    assert checker.validate_trajectory(
+        [point(0.0)], [down_pose(10.05)],
+    ).ok is False
+
+
+def test_an_empty_gripper_may_reorient_to_the_working_posture():
+    checker = policy()
+    up = Transform.from_xyz_rpy(
+        [0.4, 0.0, 1.2], [0.0, 0.0, 0.0], parent='world', child='tcp',
+    )
+    accepted = checker.validate_trajectory(
+        [point(0.0), point(0.0)],
+        [up, down_pose()],
+        start_tcp_pose=up,
+        held_object=None,
+    )
+    assert accepted.ok is True
+    assert accepted.details['reorientation'] is True
+
+    held = checker.validate_trajectory(
+        [point(0.0), point(0.0)],
+        [up, down_pose()],
+        start_tcp_pose=up,
+        held_object='cap',
+    )
+    assert held.ok is False
+    assert 'tilt' in held.reason
+
+    stuck_up = checker.validate_trajectory(
+        [point(0.0), point(0.0)],
+        [up, up],
+        start_tcp_pose=up,
+        held_object=None,
+    )
+    assert stuck_up.ok is False
+    assert 'working posture' in stuck_up.reason
+
+
+def test_stale_joint_state_and_scene_are_rejected_before_motion():
+    checker = policy()
+    assert checker.check_sources(
+        joint_state_age=0.1, scene_age=0.2,
+    ).ok
+    assert checker.check_sources(
+        joint_state_age=1.5, scene_age=0.2,
+    ).ok is False
+    assert checker.check_sources(
+        joint_state_age=0.1, scene_age=2.5,
+    ).ok is False
+    assert checker.check_sources(
+        joint_state_age=None, scene_age=0.2,
+    ).ok is False
+
+
+def test_malformed_or_jumping_trajectories_are_rejected():
+    checker = policy()
+    assert checker.validate_trajectory(
+        [point(0.0)[:6]], [down_pose()],
+    ).ok is False
+    assert checker.validate_trajectory(
+        [point(0.0)], [],
+    ).ok is False
+    assert checker.validate_trajectory(
+        [point(0.0), point(0.0)], [down_pose(), down_pose()],
+        current_joints=(0.5, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0),
+    ).ok is False
+    far = down_pose(xyz=(0.9, 0.0, 1.2))
+    assert checker.validate_trajectory(
+        [point(0.0), point(0.0)], [down_pose(), far],
+    ).ok is False
+
+
+def test_details_report_the_checked_extremes():
+    result = policy().validate_trajectory(
+        [point(-0.2), point(0.1)],
+        [down_pose(2.0), down_pose(4.0)],
+    )
+    assert result.ok
+    assert result.details['min_j7'] == pytest.approx(-0.2)
+    assert result.details['max_j7'] == pytest.approx(0.1)
+    assert result.details['max_tilt_deg'] == pytest.approx(4.0, abs=1e-6)
+    assert result.details['preferred_j7_rad'] == pytest.approx(0.0)

src/iiwa_llm_plan_executor/test/test_moveit_backend.py

@@ -0,0 +1,238 @@
+"""The MoveIt backend rejects unsafe plans before asking for execution."""
+from __future__ import annotations
+
+import math
+from pathlib import Path
+
+from scipy.spatial.transform import Rotation
+
+from iiwa_experement.geometry import Transform, load_geometry
+from iiwa_llm_plan_executor.motion_policy import MotionPolicy, load_limits
+from iiwa_llm_plan_executor.moveit_backend import (
+    MotionOutcome,
+    MoveItBackend,
+    PlannedMotion,
+)
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+ASSETS = PACKAGE.parent / 'iiwa_experement' / 'config'
+LIMITS = load_limits(PACKAGE / 'config' / 'motion_limits.yaml')
+GEOMETRY = load_geometry(ASSETS / 'geometry.yaml')
+BASE = (0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0)
+
+
+def point(j7: float):
+    values = list(BASE)
+    values[6] = j7
+    return tuple(values)
+
+
+def down_pose(tilt=0.0, xyz=(0.4, 0.0, 1.2)) -> Transform:
+    rotation = (
+        Rotation.from_euler('x', 180.0, degrees=True).as_matrix()
+        @ Rotation.from_euler('y', tilt, degrees=True).as_matrix()
+    )
+    return Transform(
+        xyz=tuple(float(value) for value in xyz),
+        rotation=tuple(tuple(float(value) for value in row) for row in rotation),
+        parent='world',
+        child='tcp',
+    )
+
+
+GOAL = down_pose()
+BASE_GOAL = GEOMETRY.world_to_base.inverse().compose_relaxed(GOAL)
+
+
+class FakePlanner:
+    def __init__(self, *, planned=None, execute_ok=True, plan_error=None,
+                 execute_error=None):
+        self.planned = planned
+        self.execute_ok = execute_ok
+        self.plan_error = plan_error
+        self.execute_error = execute_error
+        self.plan_calls = []
+        self.execute_calls = []
+        self.stopped = 0
+        self.reset_calls = 0
+
+    def plan(self, goal, phase, held_object):
+        self.plan_calls.append((goal, phase, held_object))
+        if self.plan_error is not None:
+            raise self.plan_error
+        return self.planned
+
+    def execute(self, planned):
+        self.execute_calls.append(planned)
+        if self.execute_error is not None:
+            raise self.execute_error
+        return self.execute_ok
+
+    def stop(self):
+        self.stopped += 1
+
+    def reset_stop(self):
+        self.reset_calls += 1
+
+
+def planned(points, poses, current=None):
+    if current is None:
+        current = tuple(points[0])
+    return PlannedMotion(
+        points=list(points),
+        tcp_poses=list(poses),
+        current_joints=current,
+        handle='handle',
+    )
+
+
+def backend(planner, **kwargs):
+    checker = MotionPolicy(LIMITS)
+    return MoveItBackend(
+        GEOMETRY,
+        checker,
+        planner=planner,
+        freshness_guard=kwargs.pop('freshness_guard', lambda: (True, 'ok')),
+        scene_guard=kwargs.pop('scene_guard', lambda: (True, 'ok')),
+        **kwargs,
+    )
+
+
+def test_safe_plan_is_executed_once_and_the_final_pose_is_verified():
+    planner = FakePlanner(planned=planned(
+        [point(0.0), point(0.1)],
+        [BASE_GOAL, BASE_GOAL],
+    ))
+    result = backend(planner).plan_and_execute(GOAL, 'transfer', 'vacuum')
+    assert result.success is True
+    assert result.code == 'ok'
+    assert len(planner.plan_calls) == 1
+    assert planner.plan_calls[0][2] == 'vacuum'
+    assert len(planner.execute_calls) == 1
+
+
+def test_safe_joint7_beyond_the_old_corridor_reaches_execution():
+    planner = FakePlanner(planned=planned(
+        [point(-1.5), point(-1.7), point(-1.5)],
+        [BASE_GOAL, BASE_GOAL, BASE_GOAL],
+    ))
+    result = backend(planner).plan_and_execute(GOAL, 'transfer')
+    assert result.success is True
+    assert len(planner.execute_calls) == 1
+
+
+def test_motion_phase_selection_is_ptp_for_working_points_and_lin_for_axes():
+    from iiwa_llm_plan_executor.moveit_backend import MoveItPyPlanner
+
+    phases = MoveItPyPlanner.PHASES
+    for phase in ('transfer', 'approach', 'align', 'observe', 'escape'):
+        assert phases[phase][0] == 'pilz_industrial_motion_planner'
+        assert phases[phase][1] == 'PTP'
+    for phase in ('contact', 'lower', 'retreat', 'dispense'):
+        assert phases[phase][0] == 'pilz_industrial_motion_planner'
+        assert phases[phase][1] == 'LIN'
+
+
+def test_dense_resampling_covers_steps_between_waypoints():
+    from iiwa_llm_plan_executor.moveit_backend import MoveItPyPlanner
+
+    samples = MoveItPyPlanner._densify(
+        [(0.0,) * 7, (0.05,) * 7], 0.01,
+    )
+    assert len(samples) == 6
+    assert samples[0] == (0.0,) * 7
+    assert samples[-1] == (0.05,) * 7
+    assert max(
+        abs(a - b) for a, b in zip(samples[1], samples[0])
+    ) <= 0.01 + 1e-12
+
+
+def test_camera_self_collision_is_rejected_before_execution():
+    planner = FakePlanner(planned=planned([point(0.0)], [BASE_GOAL]))
+    result = backend(
+        planner,
+        collision_checker=lambda planned_motion: (
+            'collision/clearance failure: camera_hand <-> link6; '
+            'minimum distance 0.0005 m, required 0.0030 m'
+        ),
+    ).plan_and_execute(GOAL, 'transfer')
+    assert result.success is False
+    assert result.code == 'collision'
+    assert 'camera_hand' in result.message
+    assert 'link6' in result.message
+    assert planner.execute_calls == []
+
+
+def test_excessive_tilt_and_jump_are_rejected_without_execution():
+    tilted_goal = GEOMETRY.world_to_base.inverse().compose_relaxed(down_pose(20.0))
+    for points, poses in (
+        ([point(0.0)], [tilted_goal]),
+        ([point(-1.4), point(1.4)], [BASE_GOAL, BASE_GOAL]),
+    ):
+        planner = FakePlanner(planned=planned(points, poses))
+        result = backend(planner).plan_and_execute(GOAL, 'transfer')
+        assert result.code == 'motion_limit'
+        assert planner.execute_calls == []
+
+
+def test_stale_sources_reject_before_planning():
+    planner = FakePlanner(planned=planned([point(0.0)], [BASE_GOAL]))
+    result = backend(
+        planner, freshness_guard=lambda: (False, 'joint_states is stale'),
+    ).plan_and_execute(GOAL, 'transfer')
+    assert result.success is False
+    assert result.code == 'stale_state'
+    assert planner.plan_calls == []
+    assert planner.execute_calls == []
+
+
+def test_planning_and_execution_failures_are_reported_and_stop_motion():
+    planner = FakePlanner(plan_error=RuntimeError('no IK branch'))
+    result = backend(planner).plan_and_execute(GOAL, 'transfer')
+    assert result.code == 'planning_failed'
+    assert planner.execute_calls == []
+    assert planner.stopped >= 1
+
+    planner = FakePlanner(
+        planned=planned([point(0.0)], [BASE_GOAL]), execute_ok=False,
+    )
+    result = backend(planner).plan_and_execute(GOAL, 'transfer')
+    assert result.code == 'execution_failed'
+
+    planner = FakePlanner(
+        planned=planned([point(0.0)], [BASE_GOAL]),
+        execute_error=RuntimeError('controller lost'),
+    )
+    result = backend(planner).plan_and_execute(GOAL, 'transfer')
+    assert result.code == 'execution_failed'
+
+
+def test_mismatched_final_pose_is_rejected():
+    wrong = down_pose(tilt=0.0, xyz=(0.42, 0.0, 1.2))
+    planner = FakePlanner(planned=planned([point(0.0)], [wrong]))
+    result = backend(planner).plan_and_execute(GOAL, 'transfer')
+    assert result.code == 'goal_mismatch'
+    assert planner.execute_calls == []
+
+
+def test_stop_blocks_all_later_motion():
+    planner = FakePlanner(planned=planned([point(0.0)], [BASE_GOAL]))
+    path = backend(planner)
+    path.stop('operator pressed stop')
+    result = path.plan_and_execute(GOAL, 'transfer')
+    assert result.success is False
+    assert result.code == 'stopped'
+    assert planner.plan_calls == []
+    path.reset_stop()
+    assert path.plan_and_execute(GOAL, 'transfer').success is True
+
+
+def test_source_freshness_is_checked_for_non_finite_values():
+    checker = MotionPolicy(LIMITS)
+    assert checker.check_sources(
+        joint_state_age=math.nan, scene_age=0.1,
+    ).ok is False
+    assert checker.check_sources(
+        joint_state_age=0.1, scene_age=-1.0,
+    ).ok is False

src/iiwa_llm_plan_executor/test/test_node.py

@@ -0,0 +1,282 @@
+"""ROS wrapper helpers: request parsing, plan/status publication and logs."""
+from __future__ import annotations
+
+import json
+from pathlib import Path
+from types import SimpleNamespace
+
+import pytest
+
+from iiwa_experement.geometry import load_geometry
+from iiwa_experement.scenario import load_scenario
+from iiwa_llm_plan_executor.engine import AdmissionResult, RunStatus
+from iiwa_llm_plan_executor.node import (
+    PlannerExecutorNode,
+    RunLog,
+    _safe_name,
+    plan_qos,
+)
+from iiwa_msgs.srv import GetRunStatus, SubmitPlan
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+ASSETS = PACKAGE.parent / 'iiwa_experement' / 'config'
+
+
+class FakePublisher:
+    def __init__(self):
+        self.messages = []
+
+    def publish(self, message):
+        self.messages.append(message.data)
+
+
+class FakeEngine:
+    def __init__(self, admission=None, status=None, cancel=None):
+        self.admission = admission or AdmissionResult(True, 'accepted', 'ok', 'r1')
+        self.status = status
+        self.cancel_result = cancel
+        self.submitted = []
+        self.cancelled = []
+
+    def submit(self, run_id, graph):
+        self.submitted.append((run_id, graph))
+        return self.admission
+
+    def get_status(self, run_id):
+        return self.status or RunStatus(
+            True, run_id, 'completed', 'done', 'a', 'stop', 'success',
+            'completed', 'completed', 'vacuum', None, (),
+        )
+
+    def cancel(self, run_id):
+        self.cancelled.append(run_id)
+        return self.cancel_result or SimpleNamespace(
+            accepted=True, message='cancel requested',
+        )
+
+
+def fake_node(*, engine=None):
+    node = object.__new__(PlannerExecutorNode)
+    node.engine = engine or FakeEngine()
+    node._plan_publisher = FakePublisher()
+    node._status_publisher = FakePublisher()
+    node._last_plan = None
+    node._last_status = None
+    node.log = RunLog('/tmp/iiwa-plan-log-test')
+    node.get_logger = lambda: SimpleNamespace(
+        info=lambda *a, **k: None,
+        warning=lambda *a, **k: None,
+        error=lambda *a, **k: None,
+    )
+    node._now = lambda: 10.0
+    node._lock = __import__('threading').RLock()
+    return node
+
+
+def test_plan_qos_is_transient_local_depth_one():
+    qos = plan_qos()
+    assert qos.durability.name == 'TRANSIENT_LOCAL'
+    assert qos.depth == 1
+
+
+def test_stationary_wait_rejects_a_robot_that_keeps_moving():
+    node = object.__new__(PlannerExecutorNode)
+    node._last_joint_velocity = (0.2,) * 7
+    with pytest.raises(RuntimeError, match='did not become stationary'):
+        PlannerExecutorNode._wait_stationary(
+            node, timeout_sec=0.02, tolerance=0.01,
+        )
+
+
+def test_safe_name_and_run_log_are_machine_readable(tmp_path):
+    assert _safe_name('run/one 1') == 'run_one_1'
+    log = RunLog(str(tmp_path))
+    log.write({'run_id': 'r1', 'event': 'plan_accepted', 'scene_revision': 1})
+    log.write({'run_id': 'r1', 'event': 'completed', 'terminal': 'completed'})
+    lines = (tmp_path / 'r1.jsonl').read_text().splitlines()
+    assert [json.loads(line)['event'] for line in lines] == [
+        'plan_accepted', 'completed',
+    ]
+
+
+def test_plan_is_published_once_and_kept_for_late_subscribers():
+    node = fake_node()
+    payload = {'run_id': 'r1', 'response': '{}'}
+    PlannerExecutorNode._publish_plan(node, payload)
+    PlannerExecutorNode._publish_plan(node, payload)
+    assert len(node._plan_publisher.messages) == 2
+    assert node._last_plan == payload
+    assert json.loads(node._plan_publisher.messages[-1])['run_id'] == 'r1'
+    assert 'scene_revision' not in json.loads(
+        node._plan_publisher.messages[-1]
+    )
+
+
+def test_submit_parses_json_and_forwards_the_untouched_response():
+    node = fake_node()
+    request = SimpleNamespace(value=json.dumps({
+        'run_id': 'r1',
+        'scene_revision': 3,
+        'response': {'status': 'ready', 'entry': 'a', 'plan': []},
+    }))
+    response = SimpleNamespace(success=False)
+    PlannerExecutorNode._handle_submit(node, request, response)
+    assert response.success is True
+    assert node.engine.submitted[0][0] == 'r1'
+    assert node.engine.submitted[0][1] == {
+        'status': 'ready', 'entry': 'a', 'plan': [],
+    }
+    assert len(node.engine.submitted[0]) == 2
+
+
+def test_submit_plan_service_returns_code_and_reason_without_registration():
+    node = fake_node(engine=FakeEngine(
+        admission=AdmissionResult(False, 'not_ready', 'waiting for scene', 'r1'),
+    ))
+    response = SubmitPlan.Response()
+    PlannerExecutorNode._handle_submit_plan(
+        node,
+        SimpleNamespace(run_id='r1', response='{"status":"blocked","entry":"","plan":[]}'),
+        response,
+    )
+    assert response.accepted is False
+    assert response.code == 'not_ready'
+    assert response.reason == 'waiting for scene'
+    assert response.status_json == ''
+    assert len(node.engine.submitted) == 1
+
+
+def test_submit_plan_service_returns_the_status_of_an_accepted_run():
+    node = fake_node()
+    response = SubmitPlan.Response()
+    PlannerExecutorNode._handle_submit_plan(
+        node,
+        SimpleNamespace(run_id='r1', response='{"status":"ready","entry":"a","plan":[]}'),
+        response,
+    )
+    assert response.accepted is True
+    assert response.code == 'accepted'
+    status = json.loads(response.status_json)
+    assert status['run_id'] == 'r1'
+    assert 'scene_revision' not in status
+
+
+def test_rejected_submit_returns_false_without_registering_or_publishing():
+    node = fake_node(engine=FakeEngine(
+        admission=AdmissionResult(False, 'replan_required', 'need vacuum', 'r1'),
+    ))
+    published = []
+    node._publish_status = lambda payload: published.append(payload)
+    request = SimpleNamespace(value=json.dumps({
+        'run_id': 'r1',
+        'response': {'status': 'blocked', 'entry': '', 'plan': []},
+    }))
+    response = SimpleNamespace(success=True)
+    PlannerExecutorNode._handle_submit(node, request, response)
+    assert response.success is False
+    assert published == []
+
+
+def test_replan_scene_status_keeps_the_run_id():
+    node = fake_node()
+    node._last_status = {'run_id': 'run-42', 'state': 'running'}
+    envelope = {'state_version': 7, 'scene_json': {'components': []}}
+    PlannerExecutorNode._on_replan(node, envelope)
+    payload = json.loads(node._status_publisher.messages[-1])
+    assert payload['run_id'] == 'run-42'
+    assert 'scene_revision' not in payload
+    assert payload['scene'] == envelope
+
+
+def test_invalid_submit_json_is_rejected_without_calling_the_engine():
+    node = fake_node()
+    request = SimpleNamespace(value='{broken')
+    response = SimpleNamespace(success=True)
+    PlannerExecutorNode._handle_submit(node, request, response)
+    assert response.success is False
+    assert node.engine.submitted == []
+
+
+def test_status_service_returns_found_and_json():
+    node = fake_node()
+    request = GetRunStatus.Request(run_id='r1')
+    response = GetRunStatus.Response()
+    PlannerExecutorNode._handle_get_status(node, request, response)
+    assert response.found is True
+    payload = json.loads(response.status_json)
+    assert payload['run_id'] == 'r1'
+    assert payload['state'] == 'completed'
+    assert payload['mounted_tool'] == 'vacuum'
+
+
+def test_cancel_accepts_json_and_plain_run_ids():
+    node = fake_node()
+    response = SimpleNamespace(success=False)
+    PlannerExecutorNode._handle_cancel(
+        node, SimpleNamespace(value=json.dumps({'run_id': 'r1'})), response,
+    )
+    assert response.success is True
+    response = SimpleNamespace(success=False)
+    PlannerExecutorNode._handle_cancel(
+        node, SimpleNamespace(value='r2'), response,
+    )
+    assert node.engine.cancelled == ['r1', 'r2']
+
+
+def test_scene_provider_enforces_freshness():
+    node = fake_node()
+    node._now = lambda: 10.0
+    node.scene_max_age = 2.0
+    node._latest_scene = {
+        'timestamp': 9.0, 'state_version': 1, 'scene_json': {},
+    }
+    assert PlannerExecutorNode._scene_provider(node) is not None
+    node._latest_scene['timestamp'] = 7.0
+    assert PlannerExecutorNode._scene_provider(node) is None
+
+
+def test_context_factory_sets_sources_and_inspection_configuration():
+    node = fake_node()
+    node.spec = load_scenario(20, ASSETS / 'scenes.yaml')
+    node.geometry = load_geometry(ASSETS / 'geometry.yaml')
+    node.adapter = 'adapter'
+    node.close_inspection = frozenset({'cap'})
+    node._frame_provider = lambda name: 1.0
+    node._notify_operator = lambda reason, details: True
+    context = PlannerExecutorNode._context_factory(
+        node, {'state_version': 4}, {},
+    )
+    assert context.scene_revision == 4
+    assert context.scene_adapter == 'adapter'
+    assert context.close_inspection_objects == frozenset({'cap'})
+    assert context.capture_frame('top') == 1.0
+    assert context.notify('x', {}) is True
+
+
+def test_future_scene_timestamps_cannot_delay_the_start_gate():
+    from std_msgs.msg import String
+
+    node = fake_node()
+    node._scene_ready_at = None
+    node._latest_scene = None
+    message = String()
+    message.data = json.dumps({'timestamp': 500.0, 'state_version': 1})
+    PlannerExecutorNode._on_scene(node, message)
+    assert node._scene_ready_at == 10.0
+    message.data = json.dumps({'timestamp': 5.0, 'state_version': 1})
+    PlannerExecutorNode._on_scene(node, message)
+    assert node._scene_ready_at == 5.0
+
+
+def test_robot_freshness_uses_the_joint_limit_age():
+    node = fake_node()
+    node.limits = SimpleNamespace(joint_state_max_age_sec=1.0)
+    node._last_joint = None
+    node._last_joint_at = None
+    assert PlannerExecutorNode._robot_fresh(node)[0] is False
+    node._last_joint = (0.0,) * 7
+    node._last_joint_at = 9.5
+    assert PlannerExecutorNode._robot_fresh(node)[0] is True
+    node._last_joint_at = 8.0
+    assert PlannerExecutorNode._robot_fresh(node)[0] is False

src/iiwa_llm_plan_executor/test/test_schema.py

@@ -0,0 +1,153 @@
+"""Parsing tests for the externally supplied plan response."""
+from __future__ import annotations
+
+import json
+
+import pytest
+
+from iiwa_llm_plan_executor.schema import (
+    MAX_NODES,
+    PlanSchemaError,
+    parse_response,
+)
+
+
+def sample_graph():
+    return {
+        'status': 'ready',
+        'entry': 'p1_find',
+        'plan': [
+            {
+                'id': 'p1_find',
+                'skill': 'detect',
+                'object': 'cap',
+                'target': '',
+                'transitions': [
+                    {'event': 'found', 'next': 'p1_grab'},
+                    {'event': 'not_found', 'next': 'replan_required'},
+                    {'event': 'failed', 'next': 'replan_required'},
+                ],
+            },
+            {
+                'id': 'p1_grab',
+                'skill': 'grab',
+                'object': 'vacuum',
+                'target': '',
+                'transitions': [
+                    {'event': 'success', 'next': 'completed'},
+                    {'event': 'grasp_failed', 'next': 'operator_required'},
+                    {'event': 'human_in_scene', 'next': 'operator_required'},
+                    {'event': 'collision', 'next': 'operator_required'},
+                    {'event': 'failed', 'next': 'replan_required'},
+                ],
+            },
+        ],
+    }
+
+
+def test_shown_status_entry_plan_transitions_format_is_parsed():
+    graph = parse_response(json.dumps(sample_graph()))
+    assert graph.status == 'ready'
+    assert graph.entry == 'p1_find'
+    assert graph.ids == ('p1_find', 'p1_grab')
+    assert graph.node('p1_find').transitions[0].event == 'found'
+    assert graph.node('p1_grab').object == 'vacuum'
+
+
+def test_top_level_transitions_are_merged_into_nodes():
+    payload = sample_graph()
+    payload['plan'][0].pop('transitions')
+    payload['transitions'] = [
+        {'from': 'p1_find', 'event': 'found', 'to': 'p1_grab'},
+        {'from': 'p1_find', 'event': 'not_found', 'to': 'replan_required'},
+        {'from': 'p1_find', 'event': 'failed', 'to': 'replan_required'},
+    ]
+    graph = parse_response(payload)
+    events = {item.event for item in graph.node('p1_find').transitions}
+    assert events == {'found', 'not_found', 'failed'}
+
+
+def test_blocked_response_has_no_plan():
+    graph = parse_response({'status': 'blocked', 'entry': '', 'plan': []})
+    assert graph.status == 'blocked'
+    assert graph.nodes == ()
+
+
+def test_blocked_response_with_nodes_is_rejected():
+    with pytest.raises(PlanSchemaError):
+        parse_response({
+            'status': 'blocked',
+            'entry': 'a',
+            'plan': sample_graph()['plan'],
+        })
+
+
+@pytest.mark.parametrize('payload', [
+    {'status': 'ready', 'entry': 'a', 'plan': []},
+    {'status': 'ready', 'entry': '', 'plan': sample_graph()['plan']},
+    {'status': 'unknown', 'entry': 'a', 'plan': sample_graph()['plan']},
+    {'status': 'ready', 'entry': 'a', 'plan': sample_graph()['plan'],
+     'extra': 1},
+])
+def test_invalid_top_level_shapes_are_rejected(payload):
+    with pytest.raises(PlanSchemaError):
+        parse_response(payload)
+
+
+def test_duplicate_ids_are_rejected():
+    payload = sample_graph()
+    payload['plan'][1]['id'] = 'p1_find'
+    with pytest.raises(PlanSchemaError, match='duplicate node id'):
+        parse_response(payload)
+
+
+def test_unknown_node_keys_and_transition_events_are_rejected():
+    payload = sample_graph()
+    payload['plan'][0]['pose'] = {'x': 1}
+    with pytest.raises(PlanSchemaError, match='unknown keys'):
+        parse_response(payload)
+
+    payload = sample_graph()
+    payload['plan'][0]['transitions'].append(
+        {'event': 'found', 'next': 'p1_grab'},
+    )
+    with pytest.raises(PlanSchemaError, match='duplicate event'):
+        parse_response(payload)
+
+
+def test_reserved_terminal_ids_and_oversized_plans_are_rejected():
+    payload = sample_graph()
+    payload['plan'][0]['id'] = 'completed'
+    with pytest.raises(PlanSchemaError, match='reserved terminal'):
+        parse_response(payload)
+
+    nodes = []
+    for index in range(MAX_NODES + 1):
+        nodes.append({
+            'id': f'n{index}',
+            'skill': 'stop',
+            'object': 'robot',
+            'target': '',
+            'transitions': [
+                {'event': 'success', 'next': 'completed'},
+                {'event': 'failed', 'next': 'replan_required'},
+            ],
+        })
+    with pytest.raises(PlanSchemaError, match='limit'):
+        parse_response({'status': 'ready', 'entry': 'n0', 'plan': nodes})
+
+
+def test_params_are_strict_scalars():
+    payload = sample_graph()
+    payload['plan'][0]['params'] = {'start_fraction': 0.2, 'end_fraction': 1.0}
+    graph = parse_response(payload)
+    assert graph.node('p1_find').params['start_fraction'] == 0.2
+
+    payload['plan'][0]['params'] = {'bad': {'nested': 1}}
+    with pytest.raises(PlanSchemaError):
+        parse_response(payload)
+
+
+def test_invalid_json_is_reported():
+    with pytest.raises(PlanSchemaError, match='not valid JSON'):
+        parse_response('{not json')

src/iiwa_llm_plan_executor/test/test_skills.py

@@ -0,0 +1,606 @@
+"""Skill phases, tool ownership and object handling on a fake backend."""
+from __future__ import annotations
+
+import copy
+from pathlib import Path
+
+import pytest
+
+from iiwa_experement.geometry import load_geometry, world_target
+from iiwa_experement.object_state import transform_json
+from iiwa_experement.scenario import load_scenario
+from iiwa_llm_plan_executor.moveit_backend import MotionOutcome
+from iiwa_llm_plan_executor.schema import PlanNode
+from iiwa_llm_plan_executor.skill_context import SkillContext
+from iiwa_llm_plan_executor.skills import SkillRegistry
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+ASSETS = PACKAGE.parent / 'iiwa_experement' / 'config'
+SCENES = ASSETS / 'scenes.yaml'
+GEOMETRY = load_geometry(ASSETS / 'geometry.yaml')
+
+
+class FakeMotion:
+    def __init__(self, *, success=True):
+        self.calls = []
+        self.stop_calls = []
+        self.reset_calls = 0
+        self.success = success
+
+    def plan_and_execute(self, goal, phase, held_object):
+        self.calls.append((goal, phase, held_object))
+        return MotionOutcome(
+            self.success, 'ok' if self.success else 'motion_limit',
+            'ok' if self.success else 'rejected by policy',
+        )
+
+    def stop(self, reason='stop'):
+        self.stop_calls.append(reason)
+
+    def reset_stop(self):
+        self.reset_calls += 1
+
+
+class FakeSupervisor:
+    def __init__(self, spec, geometry):
+        self.spec = spec
+        self.geometry = geometry
+        self.commands = []
+        self.state = {
+            'version': 1,
+            'mounted_tool': None,
+            'held_object': None,
+            'cap': {'state': spec.cap_state, 'pose': None},
+            'tools': {},
+            'screws': {},
+            'holes': {},
+            'objects': {},
+            'tcp': None,
+        }
+        if spec.cap_state != 'absent':
+            self.state['cap']['pose'] = transform_json(
+                world_target(spec, geometry, 'cap')
+            )
+        for tool in spec.tools:
+            self.state['tools'][tool.name] = {
+                'state': 'docked',
+                'pose': transform_json(world_target(spec, geometry, f'tool:{tool.name}')),
+            }
+            self.state['objects'][f'grant_{tool.name}'] = (
+                self.state['tools'][tool.name]['pose']
+            )
+        for screw in spec.screws:
+            self.state['screws'][screw.name] = {
+                'state': 'tray',
+                'pose': transform_json(
+                    world_target(spec, geometry, f'screw:{screw.name}')
+                ),
+                'hole': None,
+            }
+            self.state['objects'][f'grant_screw_{int(screw.name.split("_")[1]):02d}'] = (
+                self.state['screws'][screw.name]['pose']
+            )
+        self.state['objects']['grant_cap'] = self.state['cap']['pose']
+
+    def latest(self):
+        return copy.deepcopy(self.state)
+
+    def _advance(self):
+        self.state['version'] += 1
+        return copy.deepcopy(self.state)
+
+    def command(self, op, object_id='', target=None, target_name=''):
+        self.commands.append((op, object_id, target_name))
+        if op == 'attach_tool':
+            if self.state['mounted_tool'] is not None:
+                raise RuntimeError('tool already mounted')
+            if object_id not in self.state['tools']:
+                raise RuntimeError('unknown tool')
+            if self.state['tools'][object_id]['state'] != 'docked':
+                raise RuntimeError('tool is not docked')
+            self.state['mounted_tool'] = object_id
+            self.state['tools'][object_id]['state'] = 'mounted'
+        elif op == 'dock_tool':
+            if self.state['mounted_tool'] != object_id:
+                raise RuntimeError('tool is not mounted')
+            self.state['mounted_tool'] = None
+            self.state['tools'][object_id]['state'] = 'docked'
+            self.state['tools'][object_id]['pose'] = transform_json(
+                world_target(self.spec, self.geometry, f'tool:{object_id}')
+            )
+        elif op == 'grab_cap':
+            if self.state['mounted_tool'] != 'vacuum':
+                raise RuntimeError('vacuum is not mounted')
+            if self.state['held_object'] is not None:
+                raise RuntimeError('hand is not free')
+            self.state['held_object'] = 'cap'
+            self.state['cap']['state'] = 'held'
+        elif op == 'place_cap':
+            if self.state['held_object'] != 'cap':
+                raise RuntimeError('hatch is not held')
+            self.state['held_object'] = None
+            self.state['cap']['state'] = 'installed'
+            self.state['cap']['pose'] = transform_json(target)
+        elif op == 'grab_screw':
+            if self.state['mounted_tool'] != 'magnetic_tool':
+                raise RuntimeError('magnet is not mounted')
+            if self.state['screws'][object_id]['state'] != 'tray':
+                raise RuntimeError('screw is unavailable')
+            self.state['held_object'] = object_id
+            self.state['screws'][object_id]['state'] = 'held'
+        elif op == 'handoff_screw':
+            if self.state['held_object'] != object_id:
+                raise RuntimeError('screw is not held')
+            if self.state['mounted_tool'] != 'magnetic_tool':
+                raise RuntimeError('magnet is not mounted')
+            self.state['held_object'] = None
+            self.state['screws'][object_id]['state'] = 'delivered'
+            self.state['screws'][object_id]['hole'] = target_name
+            self.state['screws'][object_id]['pose'] = transform_json(target)
+            self.state['holes'].setdefault(target_name, {'state': 'empty', 'screw': None})
+            self.state['holes'][target_name]['screw'] = object_id
+        elif op == 'inspect':
+            pass
+        else:
+            raise RuntimeError(f'unknown op {op}')
+        return self._advance()
+
+
+class FakeScene:
+    def __init__(self):
+        self.events = []
+
+    def attach_tool(self, tool):
+        self.events.append(('attach_tool', tool))
+
+    def dock_tool(self, tool):
+        self.events.append(('dock_tool', tool))
+
+    def attach_cap(self):
+        self.events.append(('attach_cap',))
+
+    def attach_screw(self, screw):
+        self.events.append(('attach_screw', screw))
+
+    def place_cap(self, seat):
+        self.events.append(('place_cap', seat))
+
+    def handoff_screw(self, screw):
+        self.events.append(('handoff_screw', screw))
+
+
+def make_registry(experiment_id=20, *, motion=None, adapter=None):
+    spec = load_scenario(experiment_id, SCENES)
+    supervisor = FakeSupervisor(spec, GEOMETRY)
+    motion = motion or FakeMotion()
+    registry = SkillRegistry(motion, supervisor, scene_adapter=adapter or FakeScene())
+    context = SkillContext(spec=spec, geometry=GEOMETRY, scene_revision=1)
+    context.scene_adapter = registry.scene_adapter
+    context.frame_provider = lambda name: 5.0
+    return registry, context, motion, supervisor
+
+
+def node(node_id, skill, obj='', target='', params=None):
+    return PlanNode(
+        id=node_id, skill=skill, object=obj, target=target,
+        params=params or {},
+    )
+
+
+def phases(motion):
+    return [phase for _, phase, _ in motion.calls]
+
+
+def test_approach_goes_above_the_target_and_saves_the_height():
+    registry, context, motion, _ = make_registry()
+    result = registry.execute(node('a', 'approach', 'cap'), context)
+    assert result.event == 'success'
+    assert phases(motion) == ['approach']
+    safe = motion.calls[0][0]
+    assert context.approach_pose is not None
+    assert context.phase == 'approach'
+    assert safe.xyz[2] > context.contact_pose.xyz[2] if context.contact_pose else True
+
+    repeated = registry.execute(node('a', 'approach', 'cap'), context)
+    assert repeated.event == 'success'
+    assert phases(motion) == ['approach']
+
+
+def test_mounted_tool_is_reported_as_carried_during_motion():
+    registry, context, motion, _ = make_registry()
+    context.mounted_tool = 'vacuum'
+    result = registry.execute(node('a', 'approach', 'cap'), context)
+    assert result.event == 'success'
+    assert motion.calls[0][2] == 'vacuum'
+
+
+def test_approach_refuses_to_use_manifest_pose_when_supervisor_pose_is_missing():
+    registry, context, motion, supervisor = make_registry()
+    supervisor.state['cap']['pose'] = None
+    result = registry.execute(node('a', 'approach', 'cap'), context)
+    assert result.event == 'failed'
+    assert motion.calls == []
+
+
+def test_grab_keeps_target_in_collision_scene_during_approach():
+    class RecordingScene(FakeScene):
+        def prepare_grab_cap(self, object_id):
+            self.events.append(('prepare_grab_cap', object_id))
+
+    adapter = RecordingScene()
+    registry, context, motion, supervisor = make_registry(
+        motion=FakeMotion(success=False), adapter=adapter,
+    )
+    context.mounted_tool = 'vacuum'
+    supervisor.state['mounted_tool'] = 'vacuum'
+    result = registry.execute(node('g', 'grab', 'cap'), context)
+    assert result.event == 'failed'
+    assert phases(motion) == ['approach']
+    assert adapter.events == []
+
+
+def test_grab_restores_target_when_lowering_fails():
+    class RecordingScene(FakeScene):
+        def prepare_grab_cap(self, object_id):
+            self.events.append(('prepare_grab_cap', object_id))
+
+        def restore_part(self, object_id):
+            self.events.append(('restore_part', object_id))
+
+    class FailOnLower(FakeMotion):
+        def plan_and_execute(self, goal, phase, held_object):
+            self.calls.append((goal, phase, held_object))
+            return MotionOutcome(
+                phase != 'lower',
+                'ok' if phase != 'lower' else 'motion_limit',
+                'ok' if phase != 'lower' else 'rejected by policy',
+            )
+
+    adapter = RecordingScene()
+    registry, context, motion, supervisor = make_registry(
+        motion=FailOnLower(), adapter=adapter,
+    )
+    context.mounted_tool = 'vacuum'
+    supervisor.state['mounted_tool'] = 'vacuum'
+    result = registry.execute(node('g', 'grab', 'cap'), context)
+    assert result.event == 'failed'
+    assert phases(motion) == ['approach', 'lower']
+    assert adapter.events == [
+        ('prepare_grab_cap', 'cap'),
+        ('restore_part', 'cap'),
+    ]
+
+
+def test_grab_cap_runs_missing_phases_and_lifts_to_saved_height():
+    registry, context, motion, supervisor = make_registry()
+    context.mounted_tool = 'vacuum'
+    supervisor.state['mounted_tool'] = 'vacuum'
+    supervisor.state['tools']['vacuum']['state'] = 'mounted'
+    result = registry.execute(node('g', 'grab', 'cap'), context)
+    assert result.event == 'success'
+    assert phases(motion) == ['approach', 'lower', 'retreat']
+    assert supervisor.commands == [('grab_cap', 'cap', '')]
+    assert context.held_object == 'cap'
+    assert context.phase == 'lifted'
+    assert ('attach_cap',) in registry.scene_adapter.events
+
+
+def test_explicit_phases_are_not_repeated_by_grab():
+    registry, context, motion, supervisor = make_registry()
+    context.mounted_tool = 'vacuum'
+    supervisor.state['mounted_tool'] = 'vacuum'
+    assert registry.execute(node('a', 'approach', 'cap'), context).event == 'success'
+    assert registry.execute(node('l', 'lower', 'cap'), context).event == 'success'
+    assert registry.execute(node('g', 'grab', 'cap'), context).event == 'success'
+    assert phases(motion) == ['approach', 'lower', 'retreat']
+    assert registry.execute(node('up', 'lift', 'cap'), context).event == 'success'
+    assert phases(motion) == ['approach', 'lower', 'retreat']
+
+
+def test_grab_cap_without_vacuum_reports_grasp_failed_without_motion_or_command():
+    registry, context, motion, supervisor = make_registry()
+    before = copy.deepcopy(supervisor.state)
+    result = registry.execute(node('g', 'grab', 'cap'), context)
+    assert result.event == 'grasp_failed'
+    assert result.details['reason'] == 'required tool vacuum is not mounted'
+    assert result.details['required_tool'] == 'vacuum'
+    assert motion.calls == []
+    assert supervisor.commands == []
+    assert registry.scene_adapter.events == []
+    assert supervisor.state['cap']['state'] == before['cap']['state']
+    assert supervisor.state['held_object'] is None
+    assert context.held_object is None
+    assert context.mounted_tool is None
+
+
+def test_supervisor_state_overrides_stale_context_for_tool_checks():
+    registry, context, motion, supervisor = make_registry()
+    context.mounted_tool = 'vacuum'  # stale memory; supervisor says no tool
+    result = registry.execute(node('g', 'grab', 'cap'), context)
+    assert result.event == 'grasp_failed'
+    assert result.details['reason'] == 'required tool vacuum is not mounted'
+    assert motion.calls == []
+    assert supervisor.commands == []
+    assert context.mounted_tool is None
+
+
+def test_grab_screw_without_magnetic_tool_reports_grasp_failed_without_motion():
+    registry, context, motion, supervisor = make_registry()
+    before = copy.deepcopy(supervisor.state)
+    result = registry.execute(node('g', 'grab', 'screw_1'), context)
+    assert result.event == 'grasp_failed'
+    assert result.details['reason'] == 'required tool magnetic_tool is not mounted'
+    assert result.details['required_tool'] == 'magnetic_tool'
+    assert motion.calls == []
+    assert supervisor.commands == []
+    assert registry.scene_adapter.events == []
+    assert supervisor.state['screws']['screw_1']['state'] == before['screws']['screw_1']['state']
+    assert supervisor.state['held_object'] is None
+    assert context.held_object is None
+
+
+def test_tool_grab_docks_at_the_socket_and_lifts_out():
+    registry, context, motion, supervisor = make_registry()
+    result = registry.execute(node('g', 'grab', 'vacuum'), context)
+    assert result.event == 'success'
+    assert phases(motion) == ['approach', 'lower', 'retreat']
+    assert supervisor.commands == [('attach_tool', 'vacuum', '')]
+    assert context.mounted_tool == 'vacuum'
+    assert ('attach_tool', 'vacuum') in registry.scene_adapter.events
+
+
+def test_bare_release_of_the_suction_cup_does_not_move_or_detach():
+    registry, context, motion, supervisor = make_registry()
+    context.mounted_tool = 'vacuum'
+    supervisor.state['mounted_tool'] = 'vacuum'
+    result = registry.execute(node('r', 'release', 'vacuum', ''), context)
+    assert result.event == 'failed'
+    assert motion.calls == []
+    assert supervisor.commands == []
+    assert context.mounted_tool == 'vacuum'
+
+
+def test_release_at_socket_requires_the_explicit_transfer():
+    registry, context, motion, supervisor = make_registry()
+    context.mounted_tool = 'vacuum'
+    supervisor.state['mounted_tool'] = 'vacuum'
+    result = registry.execute(
+        node('r', 'release', 'vacuum', 'vacuum_socket'), context,
+    )
+    assert result.event == 'failed'
+    assert motion.calls == []
+    assert context.mounted_tool == 'vacuum'
+
+
+def test_explicit_transfer_then_dock_keeps_tool_in_patron_until_dock():
+    registry, context, motion, supervisor = make_registry()
+    assert registry.execute(node('g', 'grab', 'vacuum'), context).event == 'success'
+    transfer = registry.execute(
+        node('t', 'transfer', 'vacuum', 'work'), context,
+    )
+    assert transfer.event == 'success'
+    assert context.mounted_tool == 'vacuum'
+    assert phases(motion)[-1] == 'transfer'
+
+    to_socket = registry.execute(
+        node('t2', 'transfer', 'vacuum', 'vacuum_socket'), context,
+    )
+    assert to_socket.event == 'success'
+    dock = registry.execute(
+        node('d', 'dock_tool', 'vacuum', 'vacuum_socket'), context,
+    )
+    assert dock.event == 'success'
+    assert context.mounted_tool is None
+    assert supervisor.commands[-1][0] == 'dock_tool'
+    assert ('dock_tool', 'vacuum') in registry.scene_adapter.events
+    assert phases(motion)[-2:] == ['lower', 'retreat']
+
+
+def test_cap_transfer_align_and_release_place_the_hatch():
+    registry, context, motion, supervisor = make_registry()
+    context.mounted_tool = 'vacuum'
+    context.held_object = 'cap'
+    supervisor.state['mounted_tool'] = 'vacuum'
+    supervisor.state['held_object'] = 'cap'
+    supervisor.state['cap']['state'] = 'held'
+    assert registry.execute(
+        node('t', 'transfer', 'cap', 'hatch_seat'), context,
+    ).event == 'success'
+    assert registry.execute(node('a', 'align', 'cap', 'hatch_seat'), context).event == 'success'
+    before = len(motion.calls)
+    result = registry.execute(node('r', 'release', 'cap', 'hatch_seat'), context)
+    assert result.event == 'success'
+    assert phases(motion)[before:] == ['lower', 'retreat']
+    assert context.held_object is None
+    assert supervisor.state['cap']['state'] == 'installed'
+    assert ('place_cap',) in [tuple(event[:1]) for event in registry.scene_adapter.events]
+    assert supervisor.commands[-1][0] == 'place_cap'
+
+
+def test_release_lifts_axially_before_any_lateral_motion():
+    registry, context, motion, _ = make_registry()
+    context.mounted_tool = 'vacuum'
+    context.held_object = 'cap'
+    context.transfer_target = 'hatch_seat'
+    seat = world_target(context.spec, context.geometry, 'cap_seat')
+    from iiwa_experement.geometry import tcp_goal
+    contact = tcp_goal(seat, context.geometry.cap_tcp_to_node)
+    from iiwa_llm_plan_executor.skills import _above
+    safe = _above(contact, context.geometry.approach_gap)
+    context.mark_approach(safe, ('cap', 'hatch_seat'))
+    context.mark_contact(contact)
+    registry.supervisor.state['mounted_tool'] = 'vacuum'
+    registry.supervisor.state['held_object'] = 'cap'
+    registry.supervisor.state['cap']['state'] = 'held'
+    result = registry.execute(node('r', 'release', 'cap', 'hatch_seat'), context)
+    assert result.event == 'success'
+    assert phases(motion)[-1] == 'retreat'
+    assert motion.calls[-1][1] == 'retreat'
+    assert motion.calls[-1][0].xyz[2] > contact.xyz[2]
+
+
+def test_transfer_from_contact_lifts_axially_before_lateral_ptp():
+    registry, context, motion, supervisor = make_registry()
+    context.mounted_tool = 'vacuum'
+    context.held_object = 'cap'
+    supervisor.state['mounted_tool'] = 'vacuum'
+    supervisor.state['held_object'] = 'cap'
+    supervisor.state['cap']['state'] = 'held'
+    seat = world_target(context.spec, context.geometry, 'cap_seat')
+    from iiwa_experement.geometry import tcp_goal
+    from iiwa_llm_plan_executor.skills import _above
+    contact = tcp_goal(seat, context.geometry.cap_tcp_to_node)
+    safe = _above(contact, context.geometry.approach_gap)
+    context.mark_approach(safe, ('cap', 'hatch_seat'))
+    context.mark_contact(contact)
+    result = registry.execute(
+        node('t', 'transfer', 'cap', 'hatch_seat'), context,
+    )
+    assert result.event == 'success'
+    assert phases(motion) == ['retreat', 'transfer']
+    assert motion.calls[0][1] == 'retreat'
+
+
+def test_dispense_moves_along_the_segment_without_changing_ownership():
+    registry, context, motion, supervisor = make_registry()
+    context.mounted_tool = 'sealant_tool'
+    supervisor.state['mounted_tool'] = 'sealant_tool'
+    supervisor.state['tools']['sealant_tool']['state'] = 'mounted'
+    result = registry.execute(
+        node('d', 'dispense', 'side_1', params={
+            'start_fraction': 0.0, 'end_fraction': 1.0,
+        }),
+        context,
+    )
+    assert result.event == 'success'
+    assert phases(motion) == ['approach', 'lower', 'dispense', 'retreat']
+    assert context.mounted_tool == 'sealant_tool'
+    assert context.held_object is None
+
+
+def test_screw_handoff_delivers_to_the_operator_and_keeps_the_hole_empty():
+    registry, context, motion, supervisor = make_registry()
+    context.mounted_tool = 'magnetic_tool'
+    context.held_object = 'screw_1'
+    supervisor.state['mounted_tool'] = 'magnetic_tool'
+    supervisor.state['held_object'] = 'screw_1'
+    supervisor.state['screws']['screw_1']['state'] = 'held'
+    assert registry.execute(
+        node('t', 'transfer', 'screw_1', 'hole_1'), context,
+    ).event == 'success'
+    before = len(motion.calls)
+    result = registry.execute(
+        node('h', 'handoff_screw', 'screw_1', 'hole_1'), context,
+    )
+    assert result.event == 'success'
+    assert phases(motion)[before:] == ['lower', 'retreat']
+    assert supervisor.state['screws']['screw_1']['state'] == 'delivered'
+    assert supervisor.state['screws']['screw_1']['hole'] == 'hole_1'
+    assert supervisor.state['holes']['hole_1']['state'] == 'empty'
+    assert ('handoff_screw', 'screw_1') in registry.scene_adapter.events
+    assert result.details['installed'] is False
+    assert result.details['tightened'] is False
+
+
+def test_detect_withdraws_axially_then_uses_fresh_frames():
+    registry, context, motion, _ = make_registry()
+    context.mark_approach(
+        GEOMETRY.safe_poses['work'], ('cap', ''),
+    )
+    context.mark_contact(GEOMETRY.safe_poses['work'])
+    result = registry.execute(node('d', 'detect', 'cap'), context)
+    assert result.event == 'found'
+    assert phases(motion)[0] == 'retreat'
+    assert phases(motion)[1] == 'transfer'
+    assert context.phase == 'free'
+
+    context.frame_provider = lambda name: None
+    assert registry.execute(node('d', 'detect', 'cap'), context).event == 'failed'
+
+    context.frame_provider = lambda name: 5.0
+    context.close_inspection_objects = frozenset({'cap'})
+    context.frame_provider = lambda name: 5.0 if name == 'top' else None
+    assert registry.execute(node('d', 'detect', 'cap'), context).event == 'failed'
+
+
+def test_capture_requires_a_fresh_frame():
+    registry, context, _, _ = make_registry()
+    assert registry.execute(node('c', 'capture', 'top'), context).event == 'success'
+    context.frame_provider = lambda name: None
+    assert registry.execute(node('c', 'capture', 'top'), context).event == 'failed'
+
+
+def test_inspect_state_distinguishes_empty_holding_placed_unknown():
+    registry, context, _, supervisor = make_registry()
+
+    supervisor.state['cap']['state'] = 'free'
+    assert registry.execute(node('i', 'inspect_state', 'cap'), context).event == 'empty'
+
+    supervisor.state['held_object'] = 'cap'
+    supervisor.state['cap']['state'] = 'held'
+    assert registry.execute(node('i', 'inspect_state', 'cap'), context).event == 'holding'
+
+    supervisor.state['held_object'] = None
+    supervisor.state['cap']['state'] = 'installed'
+    assert registry.execute(node('i', 'inspect_state', 'cap'), context).event == 'placed'
+
+    supervisor.state['cap']['state'] = 'bogus'
+    assert registry.execute(node('i', 'inspect_state', 'cap'), context).event == 'unknown'
+
+
+def test_inspect_state_recovers_ownership_after_a_failed_grasp():
+    registry, context, _, supervisor = make_registry()
+    supervisor.state['held_object'] = None
+    supervisor.state['cap']['state'] = 'free'
+    context.ownership_unknown = True
+    result = registry.execute(node('i', 'inspect_state', 'cap'), context)
+    assert result.event == 'empty'
+    assert context.ownership_unknown is False
+
+
+def test_verify_measures_the_placed_hatch():
+    registry, context, _, supervisor = make_registry()
+    supervisor.state['cap']['state'] = 'installed'
+    supervisor.state['cap']['pose'] = transform_json(
+        world_target(context.spec, context.geometry, 'cap_seat')
+    )
+    result = registry.execute(
+        node('v', 'verify', 'cap', 'hatch_seat'), context,
+    )
+    assert result.event == 'ok'
+
+    shifted = world_target(context.spec, context.geometry, 'cap_seat')
+    shifted = type(shifted)(
+        xyz=(shifted.xyz[0] + 0.02, shifted.xyz[1], shifted.xyz[2]),
+        rotation=shifted.rotation,
+        parent=shifted.parent,
+        child=shifted.child,
+    )
+    supervisor.state['cap']['pose'] = transform_json(shifted)
+    assert registry.execute(
+        node('v', 'verify', 'cap', 'hatch_seat'), context,
+    ).event == 'not_ok'
+
+
+def test_stop_preserves_ownership_and_wait_scene_clear_reports_timeout():
+    registry, context, motion, _ = make_registry()
+    context.held_object = 'cap'
+    assert registry.execute(node('s', 'stop', 'robot'), context).event == 'success'
+    assert motion.stop_calls == ['graph stop node']
+    assert motion.reset_calls == 1
+    assert context.held_object == 'cap'
+    context.scene_clear_provider = lambda: False
+    assert registry.execute(
+        node('w', 'wait_scene_clear', 'workspace'), context,
+    ).event == 'timeout'
+    context.scene_clear_provider = lambda: True
+    assert registry.execute(
+        node('w', 'wait_scene_clear', 'workspace'), context,
+    ).event == 'clear'
+
+
+def test_unknown_skill_and_backend_failure_are_not_successes():
+    registry, context, motion, _ = make_registry(motion=FakeMotion(success=False))
+    assert registry.execute(node('x', 'tighten', 'hole_1'), context).event == 'failed'
+    assert registry.execute(node('a', 'approach', 'cap'), context).event == 'failed'

src/iiwa_llm_plan_executor/test/test_validator.py

@@ -0,0 +1,453 @@
+"""Graph admission: structure, safe branches and ownership."""
+from __future__ import annotations
+
+from pathlib import Path
+
+import pytest
+
+from iiwa_experement.geometry import load_geometry
+from iiwa_experement.scenario import load_scenario
+from iiwa_llm_plan_executor.schema import PlanSchemaError, parse_response
+from iiwa_llm_plan_executor.validator import validate_graph
+from iiwa_scene_context.catalog import DEFAULT_CATALOG
+from iiwa_scene_context.snapshot import build_envelope, build_scene
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+SCENES = PACKAGE.parent / 'iiwa_experement' / 'config' / 'scenes.yaml'
+GEOMETRY = load_geometry(PACKAGE.parent / 'iiwa_experement' / 'config' / 'geometry.yaml')
+SAFE_DEFAULTS = {
+    'collision': 'operator_required',
+    'grasp_failed': 'operator_required',
+    'human_in_scene': 'operator_required',
+    'not_found': 'replan_required',
+    'failed': 'replan_required',
+    'not_ok': 'operator_required',
+    'timeout': 'operator_required',
+    'unknown': 'operator_required',
+}
+PROGRESS = {'success', 'ok', 'found', 'clear', 'empty', 'holding', 'placed'}
+
+
+def spec_for(experiment_id: int = 1):
+    return load_scenario(experiment_id, SCENES)
+
+
+def scene_for(experiment_id: int = 1, *, mounted=None, held=None,
+              cap_state=None, revision=1):
+    del revision
+    spec = spec_for(experiment_id)
+    tools = {tool.name: {'state': 'docked'} for tool in spec.tools}
+    if mounted:
+        tools[mounted]['state'] = 'mounted'
+    scene = build_scene(
+        spec,
+        {
+            'version': 1,
+            'mounted_tool': mounted,
+            'held_object': held,
+            'ready': True,
+            'cap': {'state': cap_state or spec.cap_state},
+            'tools': tools,
+            'screws': {},
+            'holes': {},
+        },
+        DEFAULT_CATALOG,
+        GEOMETRY,
+        stamp=10.0,
+    )
+    return build_envelope(
+        scene,
+        state_version=1,
+        stamp=10.0,
+        experiment_id=experiment_id,
+    )
+
+
+def node(node_id, skill, obj='', target='', *, next_on=None, params=None,
+         outcomes=None):
+    events = outcomes or DEFAULT_CATALOG.outcomes(skill)
+    next_on = next_on or {}
+    transitions = []
+    for event in events:
+        if event in next_on:
+            destination = next_on[event]
+        elif event in PROGRESS:
+            destination = 'completed'
+        else:
+            destination = SAFE_DEFAULTS.get(event, 'operator_required')
+        transitions.append({'event': event, 'next': destination})
+    entry = {
+        'id': node_id,
+        'skill': skill,
+        'object': obj,
+        'target': target,
+        'transitions': transitions,
+    }
+    if params is not None:
+        entry['params'] = params
+    return entry
+
+
+def graph(entry_id, *nodes):
+    return parse_response({
+        'status': 'ready',
+        'entry': entry_id,
+        'plan': list(nodes),
+    })
+
+
+def expect(graph_value, scene):
+    return validate_graph(graph_value, scene, DEFAULT_CATALOG)
+
+
+def test_blocked_status_never_reaches_execution():
+    result = expect(
+        parse_response({'status': 'blocked', 'entry': '', 'plan': []}),
+        scene_for(),
+    )
+    assert result.ok is False
+    assert result.code == 'blocked'
+
+
+def test_cap_grab_without_vacuum_is_admitted_for_runtime_grasp_failed():
+    value = graph(
+        'grab_cap',
+        node('grab_cap', 'grab', 'cap', next_on={
+            'success': 'completed',
+            'grasp_failed': 'operator_required',
+        }),
+    )
+    result = expect(value, scene_for(1))
+    assert result.ok is True
+
+    # The success branch is kept in the graph but is not simulated as if the
+    # hatch were already held without a vacuum tool.
+    with_success_branch = graph(
+        'grab_cap',
+        node('grab_cap', 'grab', 'cap', next_on={
+            'success': 'transfer',
+            'grasp_failed': 'operator_required',
+        }),
+        node('transfer', 'transfer', 'cap', 'hatch_seat',
+             next_on={'success': 'completed'}),
+    )
+    assert expect(with_success_branch, scene_for(1)).ok is True
+
+
+def test_screw_grab_without_magnetic_tool_is_admitted():
+    value = graph(
+        'pick_screw',
+        node('pick_screw', 'pick', 'screw_1', next_on={
+            'success': 'completed',
+            'grasp_failed': 'operator_required',
+        }),
+    )
+    result = expect(value, scene_for(20))
+    assert result.ok is True
+
+
+def test_recovery_branch_that_assumes_a_held_hatch_has_its_own_reason():
+    value = graph(
+        'grab_cap',
+        node('grab_cap', 'grab', 'cap', next_on={
+            'success': 'completed',
+            'grasp_failed': 'inspect',
+        }),
+        node('inspect', 'inspect_state', 'cap', next_on={
+            'empty': 'transfer',
+            'holding': 'transfer',
+            'placed': 'completed',
+            'unknown': 'operator_required',
+        }),
+        node('transfer', 'transfer', 'cap', 'hatch_seat',
+             next_on={'success': 'completed'}),
+    )
+    result = expect(value, scene_for(1))
+    assert result.ok is False
+    assert result.code == 'replan_required'
+    assert 'hatch is not held' in result.reason
+    assert 'vacuum' not in result.reason
+
+
+def test_take_vacuum_and_leave_it_in_patron_is_accepted():
+    value = graph(
+        'grab_vacuum',
+        node('grab_vacuum', 'grab', 'vacuum',
+             next_on={'success': 'transfer'}),
+        node('transfer', 'transfer', 'vacuum', 'work',
+             next_on={'success': 'completed'}),
+    )
+    result = expect(value, scene_for(1))
+    assert result.ok is True
+
+
+def test_release_from_work_without_a_socket_is_rejected():
+    value = graph(
+        'grab_vacuum',
+        node('grab_vacuum', 'grab', 'vacuum',
+             next_on={'success': 'transfer'}),
+        node('transfer', 'transfer', 'vacuum', 'work',
+             next_on={'success': 'release'}),
+        node('release', 'release', 'vacuum', '',
+             next_on={'success': 'completed'}),
+    )
+    result = expect(value, scene_for(1))
+    assert result.ok is False
+    assert result.code == 'replan_required'
+
+
+def test_release_at_socket_requires_a_preceding_explicit_transfer():
+    value = graph(
+        'grab_vacuum',
+        node('grab_vacuum', 'grab', 'vacuum',
+             next_on={'success': 'transfer'}),
+        node('transfer', 'transfer', 'vacuum', 'work',
+             next_on={'success': 'release'}),
+        node('release', 'release', 'vacuum', 'vacuum_socket',
+             next_on={'success': 'completed'}),
+    )
+    result = expect(value, scene_for(1))
+    assert result.code == 'replan_required'
+    assert 'transfer' in result.reason
+
+
+def test_explicit_transfer_then_dock_or_release_returns_the_tool():
+    dock = graph(
+        'grab_vacuum',
+        node('grab_vacuum', 'grab', 'vacuum',
+             next_on={'success': 'transfer'}),
+        node('transfer', 'transfer', 'vacuum', 'vacuum_socket',
+             next_on={'success': 'dock'}),
+        node('dock', 'dock_tool', 'vacuum', 'vacuum_socket',
+             next_on={'success': 'completed'}),
+    )
+    assert expect(dock, scene_for(1)).ok is True
+
+    release = graph(
+        'grab_vacuum',
+        node('grab_vacuum', 'grab', 'vacuum',
+             next_on={'success': 'transfer'}),
+        node('transfer', 'transfer', 'vacuum', 'vacuum_socket',
+             next_on={'success': 'release'}),
+        node('release', 'release', 'vacuum', 'vacuum_socket',
+             next_on={'success': 'completed'}),
+    )
+    assert expect(release, scene_for(1)).ok is True
+
+
+def test_dock_tool_without_the_explicit_transfer_is_rejected():
+    value = graph(
+        'grab_vacuum',
+        node('grab_vacuum', 'grab', 'vacuum',
+             next_on={'success': 'dock'}),
+        node('dock', 'dock_tool', 'vacuum', 'vacuum_socket',
+             next_on={'success': 'completed'}),
+    )
+    assert expect(value, scene_for(1)).code == 'replan_required'
+
+
+def test_collision_must_not_continue_the_operation():
+    value = graph(
+        'approach',
+        node('approach', 'approach', 'cap',
+             next_on={'success': 'completed', 'collision': 'completed'}),
+    )
+    result = expect(value, scene_for(1))
+    assert result.ok is False
+    assert 'collision' in result.reason
+
+
+def test_grasp_failed_cannot_map_to_success():
+    value = graph(
+        'grab_vacuum',
+        node('grab_vacuum', 'grab', 'vacuum',
+             next_on={'success': 'completed', 'grasp_failed': 'completed'}),
+    )
+    result = expect(value, scene_for(1))
+    assert result.ok is False
+    assert 'grasp_failed' in result.reason
+
+
+def test_structural_errors_are_rejected():
+    scene = scene_for(1)
+    unknown_skill = {
+        'status': 'ready', 'entry': 'a',
+        'plan': [{
+            'id': 'a', 'skill': 'approval', 'object': 'cap', 'target': '',
+            'transitions': [
+                {'event': 'success', 'next': 'completed'},
+                {'event': 'failed', 'next': 'replan_required'},
+            ],
+        }],
+    }
+    assert expect(parse_response(unknown_skill), scene).ok is False
+    assert expect(
+        graph('a', node('a', 'detect', 'ghost')), scene,
+    ).ok is False
+    assert expect(
+        graph('a', node('a', 'detect', 'cap', 'ghost')), scene,
+    ).ok is False
+    assert expect(
+        graph('missing', node('a', 'detect', 'cap')), scene,
+    ).ok is False
+    unreachable = graph(
+        'a',
+        node('a', 'detect', 'cap'),
+        node('b', 'detect', 'cap'),
+    )
+    assert expect(unreachable, scene).ok is False
+    no_events = {
+        'status': 'ready', 'entry': 'a',
+        'plan': [{
+            'id': 'a', 'skill': 'detect', 'object': 'cap', 'target': '',
+            'transitions': [],
+        }],
+    }
+    assert expect(parse_response(no_events), scene).ok is False
+    missing_event = {
+        'status': 'ready', 'entry': 'a',
+        'plan': [{
+            'id': 'a', 'skill': 'detect', 'object': 'cap', 'target': '',
+            'transitions': [{'event': 'found', 'next': 'completed'}],
+        }],
+    }
+    assert expect(parse_response(missing_event), scene).ok is False
+    bad_next = {
+        'status': 'ready', 'entry': 'a',
+        'plan': [node('a', 'detect', 'cap',
+                      next_on={'found': 'ghost'})],
+    }
+    assert expect(parse_response(bad_next), scene).ok is False
+
+
+def test_unknown_tighten_is_rejected_before_motion():
+    payload = {
+        'status': 'ready', 'entry': 'a',
+        'plan': [{
+            'id': 'a', 'skill': 'tighten', 'object': 'hole_1', 'target': '',
+            'transitions': [
+                {'event': 'success', 'next': 'completed'},
+                {'event': 'failed', 'next': 'replan_required'},
+            ],
+        }],
+    }
+    result = expect(parse_response(payload), scene_for(1))
+    assert result.ok is False
+    assert 'unavailable skill' in result.reason
+
+
+def test_cycles_are_bounded_and_must_keep_a_recovery_branch():
+    dead_cycle = graph(
+        'a',
+        node('a', 'wait_scene_clear', 'workspace',
+             next_on={'clear': 'b', 'timeout': 'b', 'failed': 'b'}),
+        node('b', 'wait_scene_clear', 'workspace',
+             next_on={'clear': 'a', 'timeout': 'a', 'failed': 'a'}),
+    )
+    assert expect(dead_cycle, scene_for(1)).ok is False
+
+    recovering_cycle = graph(
+        'a',
+        node('a', 'wait_scene_clear', 'workspace',
+             next_on={'clear': 'b'}),
+        node('b', 'wait_scene_clear', 'workspace',
+             next_on={'clear': 'a'}),
+    )
+    assert expect(recovering_cycle, scene_for(1)).ok is True
+
+
+def test_aliases_for_pick_move_to_storage_and_inspect_are_supported():
+    value = graph(
+        'grab_vacuum',
+        node('grab_vacuum', 'grab', 'vacuum',
+             next_on={'success': 'pick_cap'}),
+        node('pick_cap', 'pick', 'cap', next_on={'success': 'transfer'}),
+        node('transfer', 'transfer', 'cap', 'hatch_seat',
+             next_on={'success': 'release'}),
+        node('release', 'release', 'cap', 'hatch_seat',
+             next_on={'success': 'inspect'}),
+        node('inspect', 'inspect', 'cap', 'hatch_seat',
+             next_on={'ok': 'completed'}),
+    )
+    assert expect(value, scene_for(1)).ok is True
+
+    bad = graph('move', node(
+        'move', 'move_to_storage', 'body', 'work',
+        next_on={'success': 'completed'},
+    ))
+    assert expect(bad, scene_for(1)).ok is False
+
+
+def test_screw_handoff_is_to_the_operator_and_leaves_the_hole_empty():
+    value = graph(
+        'grab_magnet',
+        node('grab_magnet', 'grab', 'magnetic_tool',
+             next_on={'success': 'pick_screw'}),
+        node('pick_screw', 'pick', 'screw_1',
+             next_on={'success': 'transfer'}),
+        node('transfer', 'transfer', 'screw_1', 'hole_1',
+             next_on={'success': 'handoff'}),
+        node('handoff', 'handoff_screw', 'screw_1', 'hole_1',
+             next_on={'success': 'completed'}),
+    )
+    result = expect(value, scene_for(20))
+    assert result.ok is True
+
+
+def test_dispense_requires_the_sealant_tool_at_runtime():
+    without = graph(
+        'dispense', node('dispense', 'dispense', 'side_1',
+                         next_on={'success': 'completed'}),
+    )
+    assert expect(without, scene_for(20)).ok is True
+    with_tool = graph(
+        'grab_sealant',
+        node('grab_sealant', 'grab', 'sealant_tool',
+             next_on={'success': 'dispense'}),
+        node('dispense', 'dispense', 'side_1',
+             next_on={'success': 'completed'}),
+    )
+    assert expect(with_tool, scene_for(20)).ok is True
+
+
+def test_params_limit_dispense_fractions():
+    value = graph(
+        'grab_sealant',
+        node('grab_sealant', 'grab', 'sealant_tool',
+             next_on={'success': 'dispense'}),
+        node('dispense', 'dispense', 'side_1',
+             next_on={'success': 'completed'},
+             params={'start_fraction': 0.0, 'end_fraction': 1.0}),
+    )
+    assert expect(value, scene_for(20)).ok is True
+
+    payload = {
+        'status': 'ready', 'entry': 'dispense',
+        'plan': [node('dispense', 'dispense', 'side_1',
+                      next_on={'success': 'completed'},
+                      params={'unknown': 1.0})],
+    }
+    result = expect(parse_response(payload), scene_for(20))
+    assert result.ok is False
+    assert 'params' in result.reason
+
+
+def test_unconfirmed_grasp_requires_inspect_state():
+    value = graph(
+        'grab_vacuum',
+        node('grab_vacuum', 'grab', 'vacuum',
+             next_on={'success': 'transfer', 'grasp_failed': 'stop'}),
+        node('stop', 'stop', 'robot',
+             next_on={'success': 'transfer'}),
+        node('transfer', 'transfer', 'vacuum', 'work',
+             next_on={'success': 'completed'}),
+    )
+    result = expect(value, scene_for(1))
+    assert result.ok is False
+    assert 'inspect_state' in result.reason
+
+
+def test_parse_errors_are_raised_not_smuggled_into_a_graph():
+    with pytest.raises(PlanSchemaError):
+        parse_response('[]')

src/iiwa_msgs/CMakeLists.txt

@@ -12,6 +12,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/MotionPriority.srv"
   "srv/MoveToPose.srv"
   "srv/MoveToNamedPose.srv"
+  "srv/GetRunStatus.srv"
+  "srv/SubmitPlan.srv"
   "action/MoveToPose.action"
   "action/MoveToJoints.action"
   DEPENDENCIES geometry_msgs action_msgs

src/iiwa_msgs/srv/GetRunStatus.srv

@@ -0,0 +1,4 @@
+string run_id
+---
+bool found
+string status_json

src/iiwa_msgs/srv/SubmitPlan.srv

@@ -0,0 +1,12 @@
+# Submit a ready-made plan graph without a user-supplied scene revision.
+string run_id
+# The untouched graph as a JSON string (object or string form).
+string response
+---
+bool accepted
+# accepted | duplicate | invalid | rejected | not_ready | busy | conflict
+string code
+# Human-readable reason for the admission decision.
+string reason
+# JSON status of an accepted or duplicate run; empty when nothing was registered.
+string status_json

src/iiwa_planning/include/iiwa_planning/start_state_monitor.hpp

@@ -37,7 +37,7 @@ public:
                   const sensor_msgs::msg::JointState &requested) const {
     // Explicit nonzero requested initial dynamics are never silently discarded.
     if (!requested.velocity.empty() && !velocities_ok(names, requested))
-      return false;
+      return diagnose(names, requested, "request start_state has moving joints");
     auto node = node_.lock();
     if (!node)
       return velocities_ok(names,
@@ -46,15 +46,39 @@ public:
     if (!latest_ || std::chrono::duration<double>(
                         std::chrono::steady_clock::now() - received_)
                             .count() > timeout_)
-      return false;
+      return diagnose(names, requested, "no fresh /joint_states (wall clock)");
     const double age =
         node->now().seconds() -
         (latest_->header.stamp.sec + latest_->header.stamp.nanosec * 1e-9);
-    return std::isfinite(age) && age >= 0 && age <= timeout_ &&
-           velocities_ok(names, *latest_);
+    if (!(std::isfinite(age) && age >= 0 && age <= timeout_))
+      return diagnose(names, requested, "stale /joint_states stamp");
+    return velocities_ok(names, *latest_)
+               ? true
+               : diagnose(names, *latest_, "joints still moving");
   }
 
 private:
+  bool diagnose(const std::vector<std::string> &names,
+                const sensor_msgs::msg::JointState &msg,
+                const char *why) const {
+    double worst = -1.0;
+    std::string worst_joint;
+    for (const auto &name : names) {
+      auto found = std::find(msg.name.begin(), msg.name.end(), name);
+      if (found == msg.name.end())
+        continue;
+      const size_t i = found - msg.name.begin();
+      if (i < msg.velocity.size() && std::isfinite(msg.velocity[i]) &&
+          std::abs(msg.velocity[i]) > worst) {
+        worst = std::abs(msg.velocity[i]);
+        worst_joint = name;
+      }
+    }
+    RCLCPP_WARN(rclcpp::get_logger("iiwa_spline_limits"),
+                "Start state rejected: %s; max |velocity| = %.4f rad/s at %s",
+                why, worst, worst_joint.c_str());
+    return false;
+  }
   bool velocities_ok(const std::vector<std::string> &names,
                      const sensor_msgs::msg::JointState &msg) const {
     for (const auto &name : names) {

src/iiwa_planning/scripts/move_to_pose_server.py

@@ -298,10 +298,22 @@ class IiwaMotionServer(Node):
         feedback.state = "executing"
         goal_handle.publish_feedback(feedback)
 
+        # Суставные цели — классический PTP (детерминированная интерполяция
+        # без случайных дуг RRTConnect); OMPL — запасной вариант.
         plan_params = self._make_plan_params(
-            "ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
+            "pilz_industrial_motion_planner", "PTP", 2.0, velocity_scale
         )
         ok, msg = self._plan_and_execute(plan_params, goal_handle)
+        if ok is False:
+            self.get_logger().warn(
+                f"PTP не справился ({msg}); переключаюсь на OMPL"
+            )
+            self._arm.set_start_state_to_current_state()
+            self._arm.set_goal_state(robot_state=goal_state)
+            plan_params = self._make_plan_params(
+                "ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
+            )
+            ok, msg = self._plan_and_execute(plan_params, goal_handle)
         return result, ok, msg
 
     def _handle_named(self, request: MoveToNamedPose.Request, response: MoveToNamedPose.Response):

src/iiwa_scene_context/docs/interfaces.md

@@ -0,0 +1,115 @@
+# Интерфейсы `iiwa_scene_context`
+
+Версия схемы сцены: **2**. Версия контракта: **2**. Версия каталога: **2**.
+Весь JSON — UTF-8, идентификаторы объектов стабильны и совпадают с Webots DEF.
+
+## ROS-интерфейсы
+
+| Направление | Имя | Тип | Примечание |
+| --- | --- | --- | --- |
+| сервис | `/iiwa_scene_context/get_scene` | `std_srvs/Trigger` | `success=true`, `message` = JSON конверта; иначе причина (сцена не готова / устарела / нет состояния). |
+| топик | `/iiwa_scene_context/scene_json` | `std_msgs/String` | Тот же конверт. QoS: reliable, transient-local, depth 1; поздний подписчик получает последний снимок. |
+| вход | `/iiwa_experement/object_state` | `std_msgs/String` | Живое состояние supervisor: владение, состояния и позы объектов. |
+| вход | `/iiwa_experement/scene_ready` | `std_srvs/Trigger` | Опрашивается узлом. До `success=true` снимок не публикуется. |
+
+Узел отвечает `success=false`, пока сцена не готова, пока не пришло первое
+состояние supervisor или пока последнее состояние старше `freshness_sec`
+(по умолчанию 2.0 с). Устаревший снимок нельзя трактовать как актуальный.
+
+## Конверт снимка
+
+```json
+{
+  "schema_version": 2,
+  "contract_version": 2,
+  "catalog_version": 2,
+  "experiment_id": 20,
+  "state_version": 7,
+  "timestamp": 42.0,
+  "scene_json": { "...": "см. ниже" }
+}
+```
+
+* `state_version` — дискретная версия supervisor. Она растёт при подтверждённом
+  изменении владения; обновление позы удерживаемого предмета её не меняет.
+* `scene_revision` **не входит** в публичный контракт: клиент не передаёт и не
+  получает его. Исполнитель сам хранит внутренний счётчик для защиты от
+  внешнего изменения сцены во время выполнения.
+* `timestamp` — время получения снимка. Потребитель сам проверяет свежесть.
+
+## `scene_json`
+
+Поля: `schema_version`, `contract_version`, `catalog_version`, `profile`,
+`experiment_id`, `domain`, `pose_conventions`, `components`, `destinations`,
+`tools`, `initial_state`, `cameras`, `available_skills`, `skill_contracts`,
+`action_rules`, `service_objects`, `recovery_attempts_per_component`.
+
+`components` содержит только реально созданные объекты выбранного
+эксперимента: всегда `fixture`, `body`; условно `cap`, `tray`, `screw_1`…
+`screw_10`. `tools` содержит только присутствующие инструменты.
+`destinations` содержит `work`, `hatch_seat`, `hole_1`…`hole_4`, гнездо
+`<tool>_socket` для каждого инструмента и только те участки герметика
+`side_i`, которые заданы в сцене. Для каждого участка публикуются начальная и
+конечная позы.
+
+### Позы
+
+Единые правила: `frame_id="world"`, метры, кватернион `xyzw`.
+
+```json
+{
+  "frame_id": "world",
+  "position": {"x": 0.3, "y": 0.12, "z": 0.823},
+  "orientation": {"x": 0.0, "y": 0.0, "z": 0.3826834, "w": 0.9238795},
+  "source": "webots_supervisor",
+  "stamp": 42.0
+}
+```
+
+* `source="webots_supervisor"` — живая поза из supervisor. Пока предмет
+  удерживается, публикуется его текущая поза, а не начальная.
+* `source="scenario_manifest"` — запасная начальная поза, если supervisor
+  ещё не прислал объект (например, в unit-тестах).
+* `source="executor_geometry"` — производная цель. Она вычисляется теми же
+  функциями `world_target`, `sealant_point` и `tcp_goal`, что использует
+  исполнитель, поэтому поза в `scene_json` и цель движения не расходятся.
+
+Позы есть у `components` и `tools` в поле `pose`. У мест назначения:
+`pose` (поза объекта/рабочей точки) и `tcp_pose` (цель TCP). Участок
+герметика содержит `start`, `end`, `start_tcp`, `end_tcp`. Числа проверяются
+на конечность, кадр — на `world`, кватернион — на нормировку.
+
+### Правила действий
+
+`action_rules` — компактные машиночитаемые правила для каждого доступного
+навыка:
+
+```json
+"grab": {
+  "object_types": ["hatch", "screw", "tool"],
+  "target_types": [],
+  "objects": ["cap", "screw_1", "vacuum", "..."],
+  "targets": [],
+  "required_tools": [],
+  "required_tool_by_kind": {"hatch": "vacuum", "screw": "magnetic_tool"},
+  "preconditions": ["gripper is empty"],
+  "result": "tool mounted or part attached to the mounted tool"
+}
+```
+
+Правила строятся из выбранной сцены, поэтому отсутствующие ID и навыки не
+рекламируются. В частности:
+
+* `grab(cap)` требует `vacuum`, `grab(screw_i)` — `magnetic_tool`;
+* `dispense` доступен только при наличии `sealant_tool` и участка `side_i`;
+* `dock_tool` требует предшествующий `transfer(<tool>, <tool>_socket)` в
+  собственное гнездо;
+* `handoff_screw` только передаёт винт виртуальному оператору у отверстия;
+  вставки и затяжки нет, отверстие остаётся пустым;
+* `tighten` не рекламируется, пока навык не реализован;
+* `move_to_storage` не применяется к инструменту и не ведёт `sealant_tool` в
+  `hatch_seat`.
+
+Сгенерированный пример для эксперимента 20:
+`docs/scene_json_experiment_20.json`. Он создаётся тем же `build_scene`, а не
+заменяет динамическую генерацию; тест сверяет пример с генератором.