lightweight-cobot/output/md/reference-experiments.md

Эталонное выполнение 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 — полный прогон эталонного маршрута с журналом.

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
• Начальные позы объектов: 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