Даниил Грабарь
b26edc3cfc
- forbid the transport pose; recovery reconfigures through work only - mandatory axial lift after every contact (handoff, tighten, dock, pick) - untwist the flange after simulated tightening so joint 7 never winds up - attach carried tool/cap/screw to the MoveIt scene; explicit REMOVE diffs - add_world receives box-centre poses (placed_cap_pose, restore_world) - table top lowered below the base_link collision mesh bottom (1 mm gap bug) - 1 mm clearances for cap/screw boxes; thinner cap world box (cup clearance) - wait for a stationary arm before every planning attempt (rest-to-rest) - loosen stopped_velocity_tolerance to 0.08 for reference launches only - retry a rejected named move once after settling (startup telemetry race) - adapter logs the exact reason and max joint velocity on start-state rejects - verified: all 20 experiments completed in Webots execute mode (10.11.2026)
181 lines
18 KiB
Markdown
181 lines
18 KiB
Markdown
# Эталонное выполнение 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`
|