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

# Эталонное управление экспериментами 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`: в нём робот и все объекты остаются неподвижными. Настоящее оборудование не применяется.