grabardm/lightweight-cobot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

fix(experement): honest collision poses, mandatory lifts, rest-to-rest waits; 20/20 verified

Browse Source 
- 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)
This commit is contained in:
Даниил Грабарь
2026-11-10 15:11:06 +10:00
parent 6bd288dcaf
commit b26edc3cfc
15 changed files with 824 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
output/md/reference-experiments.md
+180
View File
@@ -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`