fix(experement): honest collision poses, mandatory lifts, rest-to-rest waits; 20/20 verified
- 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:
Даниил Грабарь
@@ -0,0 +1,225 @@
|
||||
# Эксперименты для LLM-планировщика сборки люка
|
||||
|
||||
Цель набора — проверить, как LLM-планировщик преобразует пользовательский запрос и `scene_json` в граф действий `skill` с переходами по событиям. В наборе 20 экспериментов: четыре категории по пять заданий. Сложность внутри каждой категории возрастает.
|
||||
|
||||
Эксперименты опираются на технологическую карту «ТехнКарта вер. 1.0.xlsx» и модели из `src/iiwa_description/objects/grant`. В сцене работает один робот-манипулятор. Корпус закреплён в стапеле; оператора, мобильной платформы и вспомогательного робота нет. Отверстия для четырёх винтов считаются подготовленными, поскольку модели сверлильного шпинделя среди доступных объектов нет. Во всех экспериментах на роботе постоянно установлен `patron`: рабочие инструменты вставляются в него. После установки люк удерживается виртуальной фиксацией: это позволяет роботу отпустить присоску и сменить рабочий инструмент. Начальные положения объектов описаны отдельно в [experiment-scenes.md](experiment-scenes.md).
|
||||
|
||||
## Контракт для экспериментов
|
||||
|
||||
В исходном примере доступны `detect`, `approach`, `lower`, `grab`, `lift`, `transfer`, `align`, `release`, `retreat`, `capture`, `verify`, `stop`, `wait_scene_clear`, `inspect_state`, `notify_operator`. План должен вставлять присоску из стойки в постоянный `patron`, если начальное состояние содержит `tool_docked`; перед `grab` требуется контактное `lower`, а перед отпусканием люка в посадочном месте — его опускание до контакта. После отпускания нужен безопасный отвод и возврат. `dock_tool` извлекает рабочий инструмент из `patron`; сам `patron` остаётся на роботе.
|
||||
|
||||
Для остальных операций соответствующий `scene_json` расширяется следующими навыками:
|
||||
|
||||
| Навык | Действие | Основные исходы |
|
||||
| --- | --- | --- |
|
||||
| `dispense` | Пройти соплом именованный участок контура с условной подачей герметика | `completed`, `interrupted`, `failed` |
|
||||
| `press` | Прижать установленный люк в заданной зоне | `success`, `failed` |
|
||||
| `seat_screw` | Подтвердить, что винт после осевого опускания остался в отверстии | `seated`, `not_seated`, `failed` |
|
||||
| `tighten` | Затянуть конкретный винт до момента, заданного в сцене | `success`, `torque_not_reached`, `failed` |
|
||||
| `dock_tool` | Вернуть инструмент в его гнездо и отсоединить от робота | `success`, `failed` |
|
||||
|
||||
Контракты `verify` и `inspect_state` нужно расширить на контур герметика и винты. Для тестов достаточно состояния симулятора; `capture` не входит в основные маршруты. Предлагаемые идентификаторы: `cap`, `hatch_seat`, `body`, `sealant_path`, `side_1`…`side_4`, `hole_1`…`hole_4`, `screw_1`…`screw_10`, `screw_tray`, `patron`, `vacuum`, `sealant_tool`, `magnetic_tool`, `torque_tool`.
|
||||
|
||||
Каждый эксперимент начинается в домашней позиции. К объекту робот идёт через безопасную точку над ним; после контакта сначала отходит по оси инструмента. В конце он возвращает инструмент в стойку и идёт домой. Ветви движения при `collision` должны вести к остановке; при неопределённом состоянии нельзя продолжать перенос или отпускание детали. Число повторных попыток задаёт `recovery_attempts_per_component`.
|
||||
|
||||
## I. Захват и установка люка
|
||||
|
||||
### 1. Базовая установка
|
||||
|
||||
**Запрос модели:** «Возьми люк присоской, перенеси на корпус, установи, отпусти и проверь итоговое положение».
|
||||
|
||||
**Условия:** люк лежит в известной области; присоска в стойке; посадочное место свободно.
|
||||
|
||||
**Действия робота:** найти присоску → подойти и опуститься к гнезду → захватить инструмент → подняться → найти люк → подойти над ним и опуститься до контакта → включить захват → поднять люк → найти посадочное место → перенести люк над ним → совместить → опустить до посадки → отпустить → отвести присоску → проверить положение люка → вернуть присоску в стойку → домой.
|
||||
|
||||
**Проверка плана:** работа с деталью не начинается раньше подтверждённого захвата инструмента; `verify` следует за `release`.
|
||||
|
||||
### 2. Люк с изменённой исходной ориентацией
|
||||
|
||||
**Запрос:** «Установи люк на корпус, даже если он лежит повёрнутым относительно посадочного места».
|
||||
|
||||
**Условия:** положение люка обнаруживается во время выполнения; его начальный угол отличается от целевого.
|
||||
|
||||
**Действия робота:** взять присоску → обнаружить фактическую позу люка → подойти и захватить его → поднять выше стапеля → перенести в безопасную точку над корпусом → повернуть и совместить с `hatch_seat` через `align` → опустить → отпустить → отвести инструмент → проверить установку → вернуть инструмент → домой.
|
||||
|
||||
**Проверка плана:** план использует результат `detect`, а не предполагает исходную ориентацию детали из текста запроса.
|
||||
|
||||
### 3. Люк и посадочное место обнаруживаются отдельно
|
||||
|
||||
**Запрос:** «Найди люк и его посадочное место, затем установи люк и проверь результат».
|
||||
|
||||
**Условия:** обе позы уточняются при выполнении.
|
||||
|
||||
**Действия робота:** взять присоску → `detect(cap)` → подойти, опуститься, захватить и поднять люк → `detect(hatch_seat)` → перенести над найденной точкой → совместить → опустить → отпустить → отвести инструмент → проверить → вернуть инструмент → домой.
|
||||
|
||||
**Ветви и проверка:** `not_found` у любого обнаружения ведёт к `replan_required`; перенос к посадочному месту не начинается, пока оно не найдено.
|
||||
|
||||
### 4. Неудачный первый захват
|
||||
|
||||
**Запрос:** «Установи люк; если присоска не удержала его с первого раза, безопасно повтори попытку».
|
||||
|
||||
**Условия:** исполнитель один раз возвращает `grasp_failed` при захвате люка; `recovery_attempts_per_component = 1`.
|
||||
|
||||
**Действия робота:** взять присоску → найти люк → подойти и опуститься → попытаться захватить. При `grasp_failed`: остановиться → проверить, пуст ли захват и где находится люк → заново обнаружить люк → повторить подход, опускание и захват один раз → поднять, перенести, совместить, установить, отпустить и проверить → вернуть инструмент → домой.
|
||||
|
||||
**Проверка плана:** после `grasp_failed` робот не выполняет `lift` вслепую; второй отказ заканчивается остановкой и уведомлением.
|
||||
|
||||
### 5. Исправление положения после установки
|
||||
|
||||
**Запрос:** «Установи люк и проверь положение. Если он оказался вне допуска, поправь его одной повторной установкой».
|
||||
|
||||
**Условия:** первая посадка даёт `verify:not_ok`; люк остаётся доступным для повторного захвата.
|
||||
|
||||
**Действия робота:** выполнить установку как в эксперименте 1 → получить `not_ok` → обнаружить фактическое положение люка → подойти присоской, опуститься, снова захватить и поднять → повторно обнаружить посадочное место → перенести, совместить, опустить и отпустить → повторить `verify` → вернуть инструмент → домой при `ok`.
|
||||
|
||||
**Проверка плана:** не более одной повторной установки; при неизвестном состоянии детали или второй неуспешной проверке граф заканчивается `operator_required`.
|
||||
|
||||
## II. Работа с герметиком
|
||||
|
||||
Для этих экспериментов `sealant_path` разбит на именованные участки `side_1`…`side_4`. Герметик условный: проверяются маршрут робота и изменение состояния контура в симуляторе.
|
||||
|
||||
### 6. Один прямой участок
|
||||
|
||||
**Запрос:** «Нанеси герметик на первый прямой участок посадочного контура корпуса».
|
||||
|
||||
**Условия:** инструмент в стойке; корпус закреплён.
|
||||
|
||||
**Действия робота:** найти инструмент → подойти, опуститься и взять его → подняться → найти `side_1` → подойти над началом участка → опустить сопло → выполнить `dispense(side_1)` → отвести сопло вверх → вернуть инструмент → домой → проверить, что обработан именно `side_1`.
|
||||
|
||||
**Проверка плана:** нет подачи при перемещении от стойки к корпусу.
|
||||
|
||||
### 7. Два смежных участка
|
||||
|
||||
**Запрос:** «Нанеси непрерывную линию герметика на первую и вторую стороны посадочного контура».
|
||||
|
||||
**Условия:** стороны соединяются одним углом.
|
||||
|
||||
**Действия робота:** взять инструмент → обнаружить контур → подойти к началу `side_1` → опустить сопло → пройти `side_1` → перейти через угол без отвода от рабочей высоты → пройти `side_2` → остановить подачу → поднять инструмент → вернуть в стойку → домой → проверить оба участка.
|
||||
|
||||
**Проверка плана:** план сохраняет порядок смежных участков и не поднимает сопло посреди непрерывной линии.
|
||||
|
||||
### 8. Замкнутый контур
|
||||
|
||||
**Запрос:** «Нанеси герметик по всему замкнутому контуру посадочного места люка».
|
||||
|
||||
**Условия:** четыре стороны доступны инструменту.
|
||||
|
||||
**Действия робота:** взять инструмент → найти контур → подойти к выбранному началу → опустить сопло → последовательно выполнить `dispense` для четырёх сторон и углов → завершить контур в точке начала → поднять сопло → вернуть инструмент → домой → проверить полноту контура.
|
||||
|
||||
**Проверка плана:** перечислены все четыре участка ровно по одному разу; конечная проверка относится ко всему контуру.
|
||||
|
||||
### 9. Прерывание подачи
|
||||
|
||||
**Запрос:** «Нанеси герметик по контуру; если подача прервётся, возобнови её с места разрыва и закончи контур».
|
||||
|
||||
**Условия:** на третьем участке один раз приходит `dispense:interrupted`.
|
||||
|
||||
**Действия робота:** взять инструмент → обработать первые два участка → начать третий → при прерывании остановить действие и отвести сопло → проверить состояние обработанных участков и последнюю подтверждённую точку → снова подойти к началу недостающего отрезка → опустить сопло → закончить третий и четвёртый участки → проверить контур → вернуть инструмент → домой.
|
||||
|
||||
**Проверка плана:** повторно не проходится весь контур; неизвестная точка разрыва ведёт к `replan_required`.
|
||||
|
||||
### 10. Герметик и установка люка
|
||||
|
||||
**Запрос:** «Нанеси герметик по всему посадочному контуру, затем возьми люк присоской и установи его на корпус».
|
||||
|
||||
**Условия:** люк лежит отдельно; для его установки после отпускания предусмотрена виртуальная фиксация.
|
||||
|
||||
**Действия робота:** взять инструмент герметика → обработать четыре участка → проверить контур → вернуть инструмент → взять присоску → найти и захватить люк → поднять и перенести его над корпусом → совместить с посадочным местом → опустить и прижать → отпустить → проверить положение люка → вернуть присоску → домой.
|
||||
|
||||
**Проверка плана:** присоска не берётся, пока инструмент герметика не возвращён; люк не опускается на неполный контур.
|
||||
|
||||
## III. Подача винтов и затяжка
|
||||
|
||||
В этих сценах люк уже установлен и зафиксирован, а четыре отверстия подготовлены. В лотке всегда находятся десять одинаковых винтов. Для задания можно выбрать любой доступный винт; проверка плана должна требовать нужное число **разных** винтов, а не заранее заданные номера. Магнитный инструмент переносит винт; `seat_screw` подтверждает, что винт остаётся в отверстии после отвода магнита.
|
||||
|
||||
### 11. Один винт
|
||||
|
||||
**Запрос:** «Возьми винт из лотка и установи его в первое отверстие люка».
|
||||
|
||||
**Действия робота:** взять магнитный инструмент → найти один доступный винт в лотке → подойти и опуститься к нему → захватить и поднять → найти `hole_1` → перенести винт над отверстием → совместить оси → опустить → выполнить `seat_screw` → отпустить винт → отвести инструмент → проверить наличие винта в отверстии → вернуть инструмент → домой.
|
||||
|
||||
**Проверка плана:** `release` невозможен до подтверждённой посадки винта.
|
||||
|
||||
### 12. Два винта по диагонали
|
||||
|
||||
**Запрос:** «Установи два винта в диагонально противоположные отверстия люка».
|
||||
|
||||
**Действия робота:** взять магнитный инструмент → установить первый винт по цепочке «найти → захватить → поднять → перенести → совместить → опустить → посадить → отпустить» → подняться над люком → вернуться к лотку → повторить для диагонально противоположного отверстия → проверить обе точки → вернуть инструмент → домой.
|
||||
|
||||
**Проверка плана:** второму винту соответствуют другая ячейка лотка и другое отверстие.
|
||||
|
||||
### 13. Четыре винта крестовым порядком
|
||||
|
||||
**Запрос:** «Установи четыре винта в отверстия люка крестовым порядком».
|
||||
|
||||
**Действия робота:** взять магнитный инструмент → последовательно взять винты из четырёх ячеек и установить их в порядке `hole_1 → hole_3 → hole_2 → hole_4`; перед каждым переносом подтвердить захват, перед отпусканием — посадку → проверить наличие четырёх винтов → вернуть инструмент → домой.
|
||||
|
||||
**Проверка плана:** нет повторного использования ячейки или отверстия; после каждой установки робот возвращается к лотку с пустым магнитным захватом.
|
||||
|
||||
### 14. Установка и затяжка четырёх винтов
|
||||
|
||||
**Запрос:** «Установи четыре винта крестовым порядком, затем затяни каждый до заданного момента».
|
||||
|
||||
**Действия робота:** выполнить четыре установки как в эксперименте 13 → вернуть магнитный инструмент → взять инструмент затяжки → для каждого из четырёх винтов подойти сверху, совместить инструмент с осью, опуститься, выполнить `tighten`, отвести инструмент → проверить четыре результата затяжки → вернуть инструмент → домой.
|
||||
|
||||
**Проверка плана:** `tighten` не вызывается для винта без подтверждённого `seat_screw`.
|
||||
|
||||
### 15. Ошибка захвата и недостигнутый момент
|
||||
|
||||
**Запрос:** «Установи и затяни четыре винта. Если один винт не захватится или затяжка не достигнет нужного момента, повтори соответствующее действие один раз».
|
||||
|
||||
**Условия:** первый захват третьего выбранного винта даёт `grasp_failed`; первая затяжка в `hole_4` даёт `torque_not_reached`.
|
||||
|
||||
**Действия робота:** установить первые два винта → на третьем остановиться, проверить состояние захвата и повторно обнаружить винт → повторить его захват один раз → установить третий и четвёртый → сменить магнитный инструмент на затяжной → затянуть четыре винта; для четвёртого после проверки состояния повторить затяжку один раз → проверить все четыре точки → вернуть инструмент → домой.
|
||||
|
||||
**Проверка плана:** ошибки обрабатываются у соответствующего винта; уже установленные винты не вынимаются и не устанавливаются повторно.
|
||||
|
||||
## IV. Полная сборка и перепланирование
|
||||
|
||||
### 16. Герметик, люк и один винт
|
||||
|
||||
**Запрос:** «Нанеси герметик на заданный участок, установи люк и закрепи его одним винтом».
|
||||
|
||||
**Действия робота:** взять инструмент герметика → обработать участок → вернуть инструмент → взять присоску → захватить, перенести, совместить, опустить и отпустить люк → проверить его положение → вернуть присоску → взять магнитный инструмент → установить один винт → вернуть магнит → взять инструмент затяжки → затянуть винт → вернуть инструмент → домой.
|
||||
|
||||
**Проверка плана:** зависимости «герметик → люк → винт → затяжка» соблюдены.
|
||||
|
||||
### 17. Полный контур и два крепления
|
||||
|
||||
**Запрос:** «Нанеси полный контур герметика, установи люк и закрепи его двумя диагональными винтами».
|
||||
|
||||
**Действия робота:** пройти четыре участка герметика и проверить контур → вернуть инструмент → присоской установить и проверить люк → вернуть присоску → магнитным инструментом установить два диагональных винта → вернуть магнит → затянуть оба винта → проверить две затяжки → вернуть инструмент → домой.
|
||||
|
||||
**Проверка плана:** если контур не подтверждён, план не переходит к установке люка.
|
||||
|
||||
### 18. Полная сборка
|
||||
|
||||
**Запрос:** «Нанеси герметик по контуру, установи люк, поставь четыре винта крестовым порядком и затяни их».
|
||||
|
||||
**Действия робота:** полный контур герметика → проверка → смена на присоску → захват и установка люка → проверка → смена на магнит → четыре установки винтов → смена на инструмент затяжки → четыре затяжки → итоговая проверка люка и крепежа → вернуть инструмент → домой.
|
||||
|
||||
**Проверка плана:** у каждого из четырёх винтов есть собственные узлы установки и затяжки; итоговая проверка охватывает всю сборку.
|
||||
|
||||
### 19. Полная сборка при смещённых деталях
|
||||
|
||||
**Запрос:** «Собери узел полностью. Люк и лоток могут лежать не в номинальных местах — используй их фактическое положение».
|
||||
|
||||
**Условия:** симулятор смещает люк и лоток в допустимой рабочей области; `detect` возвращает их актуальные позы.
|
||||
|
||||
**Действия робота:** нанести и проверить герметик → обнаружить фактическую позу люка, захватить и установить его → проверить → обнаружить фактическую позу лотка и по очереди найти четыре винта → установить их → затянуть → проверить сборку → вернуть инструмент → домой.
|
||||
|
||||
**Проверка плана:** план не подставляет прежние координаты люка и винтов после нового `detect`; зависимости операций сохраняются.
|
||||
|
||||
### 20. Полная сборка с несколькими сбоями
|
||||
|
||||
**Запрос:** «Выполни полную сборку. При прерывании подачи герметика, неудачном захвате винта или ошибке установки люка восстанови работу, если состояние известно и разрешена одна повторная попытка».
|
||||
|
||||
**Условия:** один раз прерывается `dispense`, первый захват одного винта неудачен, первая проверка положения люка возвращает `not_ok`.
|
||||
|
||||
**Действия робота:** начать контур → определить необработанный отрезок и завершить его → проверить контур → установить люк → при `not_ok` заново обнаружить и один раз переставить его → проверить → устанавливать винты; при `grasp_failed` остановиться, проверить состояние и один раз повторить захват нужного винта → затянуть четыре винта → проверить четыре установки, четыре затяжки и положение люка → вернуть инструмент → домой.
|
||||
|
||||
**Проверка плана:** восстановление начинается от подтверждённого состояния; лимит попыток соблюдён; любое `unknown` или повторный отказ ведёт к остановке и уведомлению.
|
||||
|
||||
## Общие критерии оценки графа
|
||||
|
||||
Для каждого ответа планировщика следует проверить существование всех узлов, указанных в `next`; соблюдение зависимостей операций; отсутствие физически невозможных переходов, например `transfer` без подтверждённого захвата; корректное завершение аварийных ветвей; соблюдение лимита повторных попыток. Семантическая проверка графа полезнее посимвольного сравнения с одним образцом JSON.
|
||||
@@ -0,0 +1,91 @@
|
||||
# Начальная сцена для каждого эксперимента
|
||||
|
||||
Этот документ задаёт положения объектов для 20 экспериментов из [experements.md](experements.md). Единицы измерения — метры; координаты даны в мировой системе Webots. Это проект начальной компоновки для будущего `src/iiwa_description/worlds/grant/iiwa-grant.wbt`, а не описание уже созданного мира.
|
||||
|
||||
## Основа мира и постоянные элементы
|
||||
|
||||
Базовый мир — `src/iiwa_description/worlds/iiwa.wbt`. В нём есть стол, ограждения и освещение. Робот появляется через запуск в точке `(-0.25, 0, 0.79)`, с нулевым поворотом вокруг Z. Верх столешницы находится на `Z = 0.793`; её рабочая область по столешнице приблизительно `X = −0.64…0.64`, `Y = −0.54…0.54`. Для точки в мировой системе: `X_base = X_world + 0.25`, `Y_base = Y_world`, `Z_base = Z_world − 0.79`.
|
||||
|
||||
`patron` **всегда остаётся на роботе**. В начале каждого эксперимента в него не вставлен ни один из четырёх рабочих инструментов. Робот берёт нужный инструмент из стойки и вставляет в `patron`; при возврате извлекает только рабочий инструмент. Отсутствие рабочего инструмента не означает отсутствие `patron`.
|
||||
|
||||
| Объект | Центр `(X, Y, Z)` | Ориентация и состояние |
|
||||
| --- | --- | --- |
|
||||
| Стапель | `(0.30, 0.12, 0.803)` | Вертикальная ось OBJ переведена в Z Webots, затем поворот 45° вокруг Z; закреплён |
|
||||
| Корпус | `(0.30, 0.12, 0.823)` | Та же ориентация; закреплён на опорах стапеля |
|
||||
| Посадочное место `hatch_seat` | Центр `(0.30, 0.12)` | Поза связана с корпусом; устанавливаемый люк имеет начало в `(0.30, 0.12, 0.82594)` |
|
||||
| Свободный люк, стандартная поза | `(0.35, −0.22, 0.790667)` | Поворот 45°; лежит отдельно на столе |
|
||||
| Стойка инструментов | `(0.12, −0.42, 0.793)` | Фиксированная стойка с отдельными инструментами в гнёздах |
|
||||
| Лоток винтов | `(−0.08, 0.34, 0.793)` | Длинная сторона вдоль мировой оси X; нижняя поверхность на столе |
|
||||
|
||||
Стойка и корпус присутствуют во всех сценах. Свободный люк, уже установленный люк, лоток, винты и рабочие инструменты появляются по таблице ниже. Установленный люк виртуально фиксируется после отпускания присоски. В опытах с крепежом он уже установлен и зафиксирован при старте.
|
||||
|
||||
### Гнёзда инструментов
|
||||
|
||||
Положения соответствуют четырём гнёздам модели `tool-magizine.obj` при указанной позе стойки. Нужные для опыта инструменты появляются как отдельные объекты, доступные для извлечения и вставки в `patron`.
|
||||
|
||||
| Инструмент | Центр гнезда `(X, Y, Z)` |
|
||||
| --- | --- |
|
||||
| Присоска `vacuum` | `(0.255, −0.42, 0.803)` |
|
||||
| Магнитный `magnetic_tool` | `(0.165, −0.42, 0.803)` |
|
||||
| Герметик `sealant_tool` | `(0.075, −0.42, 0.803)` |
|
||||
| Затяжка `torque_tool` | `(−0.015, −0.42, 0.803)` |
|
||||
|
||||
### Десять винтов в лотке
|
||||
|
||||
Во **всех** экспериментах с лотком при старте в нём находятся десять одинаковых винтов. Даже если задача требует установить только один, два или четыре, остальные остаются на месте и усложняют поиск и выбор. Винты стоят вертикально; их нижняя точка запланирована на `Z = 0.7954`, у внутреннего дна лотка. Центры соседних винтов разнесены на 20 мм вдоль X и 30 мм между рядами Y. Четыре первых номера размещены по углам сетки, остальные заполняют промежутки.
|
||||
|
||||
| Винт | `(X, Y, Z)` | Винт | `(X, Y, Z)` |
|
||||
| --- | --- | --- | --- |
|
||||
| `screw_1` | `(−0.120, 0.325, 0.7954)` | `screw_6` | `(−0.080, 0.325, 0.7954)` |
|
||||
| `screw_2` | `(−0.040, 0.325, 0.7954)` | `screw_7` | `(−0.060, 0.325, 0.7954)` |
|
||||
| `screw_3` | `(−0.120, 0.355, 0.7954)` | `screw_8` | `(−0.100, 0.355, 0.7954)` |
|
||||
| `screw_4` | `(−0.040, 0.355, 0.7954)` | `screw_9` | `(−0.080, 0.355, 0.7954)` |
|
||||
| `screw_5` | `(−0.100, 0.325, 0.7954)` | `screw_10` | `(−0.060, 0.355, 0.7954)` |
|
||||
|
||||
Все десять винтов взаимозаменяемы. Для установки в четыре отверстия планировщик может выбрать **любые четыре разных доступных винта**. Оценка графа не должна требовать конкретные номера `screw_i`. В опыте 15 отказ задаётся третьей попытке захвата винта, который план выбрал третьим; отказ затяжки задаётся отверстию `hole_4`.
|
||||
|
||||
При стандартной позе корпуса предварительные центры отверстий, полученные из геометрии люка, таковы:
|
||||
|
||||
| Отверстие | `(X, Y)` | Отверстие | `(X, Y)` |
|
||||
| --- | --- | --- | --- |
|
||||
| `hole_1` | `(0.2243, 0.1052)` | `hole_3` | `(0.3757, 0.1348)` |
|
||||
| `hole_2` | `(0.2852, 0.0443)` | `hole_4` | `(0.3148, 0.1957)` |
|
||||
|
||||
Противоположные пары — `hole_1`/`hole_3` и `hole_2`/`hole_4`; крестовой порядок: `hole_1 → hole_3 → hole_2 → hole_4`. Точную ось посадки винта и высоту головки нужно проверить в Webots перед построением контактной траектории.
|
||||
|
||||
## Размещение по экспериментам
|
||||
|
||||
В таблице «стандартный корпус» означает стапель `(0.30, 0.12, 0.803)` и корпус `(0.30, 0.12, 0.823)` из раздела выше. Указанные инструменты находятся в своих гнёздах; `patron` на роботе во всех строках. Если есть лоток, в нём **всегда десять винтов** в точках из предыдущей таблицы. Герметик до начала опыта не нанесён.
|
||||
|
||||
| № | Стапель и корпус | Люк при старте | Лоток и винты | Рабочие инструменты в стойке | Отличие сценария |
|
||||
| ---: | --- | --- | --- | --- | --- |
|
||||
| 1 | Стандартные | Свободный `(0.35, −0.22, 0.790667)`, 45° | Нет | Присоска | Базовая установка |
|
||||
| 2 | Стандартные | Свободный `(0.35, −0.22, 0.790667)`, **135°** | Нет | Присоска | Люк нужно повернуть на 90° при переносе |
|
||||
| 3 | Стапель `(0.29, 0.14, 0.803)`, корпус `(0.29, 0.14, 0.823)` | Свободный `(0.33, −0.22, 0.790667)`, 45° | Нет | Присоска | Посадочное место смещено вместе с корпусом; целевой люк `(0.29, 0.14, 0.82594)` |
|
||||
| 4 | Стандартные | Стандартный свободный | Нет | Присоска | Первая попытка захвата даст `grasp_failed` |
|
||||
| 5 | Стандартные | Стандартный свободный | Нет | Присоска | После первой установки люк окажется в `(0.303, 0.12, 0.82594)` и проверка даст `not_ok` |
|
||||
| 6 | Стандартные | Нет | Нет | Герметик | Обрабатывается `side_1` |
|
||||
| 7 | Стандартные | Нет | Нет | Герметик | Обрабатываются смежные `side_1`, `side_2` |
|
||||
| 8 | Стандартные | Нет | Нет | Герметик | Обрабатывается полный `sealant_path` |
|
||||
| 9 | Стандартные | Нет | Нет | Герметик | На `side_3` при выполнении прервётся подача |
|
||||
| 10 | Стандартные | Стандартный свободный | Нет | Герметик, присоска | Герметик, затем установка люка |
|
||||
| 11 | Стандартные | Уже установлен `(0.30, 0.12, 0.82594)`, 45° | Стандартный лоток, **10 винтов** | Магнитный | Установить один из десяти винтов |
|
||||
| 12 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Установить два разных винта |
|
||||
| 13 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Установить четыре разных винта |
|
||||
| 14 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный, затяжной | Установить и затянуть четыре винта |
|
||||
| 15 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный, затяжной | Сбой третьего захвата и первой затяжки `hole_4` |
|
||||
| 16 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Участок герметика, люк, один винт |
|
||||
| 17 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Полный контур, люк, два винта |
|
||||
| 18 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Полная сборка |
|
||||
| 19 | Стандартные | Свободный `(0.37, −0.20, 0.790667)`, **60°** | Лоток `(−0.10, 0.36, 0.793)`, **10 винтов** | Все четыре | Координаты всех десяти винтов смещены вместе с лотком на `(−0.02, +0.02)` |
|
||||
| 20 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Исходная геометрия обычная; сбои вводятся во время выполнения |
|
||||
|
||||
В опыте 3 смещается вся пара «стапель + корпус» без изменения взаимной высоты и поворота. Координаты `hatch_seat` и четырёх отверстий сдвигаются вместе с ними на `(−0.01, +0.02)`. В опыте 19 сдвигается лоток **со всеми десятью винтами**; ни один винт не остаётся в старой точке.
|
||||
|
||||
Опыты 4, 5, 9, 15 и 20 требуют также управляемых событий исполнителя. Эти события и последующие изменения положения детали не являются дополнительными объектами начальной сцены. Герметик удобнее учитывать как состояние именованных участков контура, а не как заранее лежащую на корпусе модель.
|
||||
|
||||
## Геометрические и технические ограничения
|
||||
|
||||
По габаритам OBJ стапель занимает в плане примерно `X = 0.0985…0.5015`, `Y = −0.0815…0.3215`; стандартный свободный люк — `X = 0.2663…0.4337`, `Y = −0.3037…−0.1363`; стойка — `X = −0.09…0.33`, `Y = −0.49…−0.35`; лоток — `X = −0.14…−0.02`, `Y = 0.305…0.375`. Эти прямоугольники не пересекаются. Зазор от свободного люка до стойки по Y около 46 мм, до стапеля около 55 мм; от лотка до стапеля по X около 118 мм. Это проверка начальной компоновки, не проверка траекторий инструмента.
|
||||
|
||||
Перед реализацией контактных действий нужно проверить в Webots достижимость TCP и коллизии при подъёме люка, при извлечении инструмента из гнезда и при работе между десятью винтами. В `iiwa.wbt` трение `TableMaterial` равно нулю; лоток и винты понадобится удерживать до захвата или задать подходящую контактную модель. Действующий спавнер `object_spawner.py` создаёт случайные OBJ, поэтому для этой таблицы потребуется детерминированная загрузка по номеру эксперимента. Стойка должна содержать отдельно управляемые рабочие инструменты; удалённый `GrantToolRack.proto` для нового мира не используется.
|
||||
@@ -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`
|
||||
Reference in New Issue
Block a user