# Эталонное выполнение 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`