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:
Даниил Грабарь
+3
-5
@@ -17,10 +17,8 @@ venv/
|
||||
.codex/
|
||||
__pycache__/
|
||||
.pytest_cache/
|
||||
output/
|
||||
!output/
|
||||
output/reference_photo/
|
||||
|
||||
*.egg-info
|
||||
**.FCBak
|
||||
|
||||
CLAUDE.md
|
||||
AGENTS.md
|
||||
**.FCBak
|
||||
@@ -0,0 +1,48 @@
|
||||
# Repository Guidelines
|
||||
|
||||
## Project Structure & Module Organization
|
||||
|
||||
Lightweight Cobot targets Ubuntu 24.04, ROS 2 Jazzy, and the KUKA LBR iiwa 7 R800.
|
||||
|
||||
- `src/iiwa_controller/`: C++ FRI hardware interface; bundled SDK in `external/libFRI/`.
|
||||
- `src/iiwa_planning/` and `src/iiwa_msgs/`: motion planning nodes and ROS actions/services.
|
||||
- `src/iiwa_bringup/`, `src/iiwa_config/`, and `src/iiwa_description/`: launch files, configuration, URDF/Xacro, meshes, and Webots worlds.
|
||||
- `src/iiwa_utils/` and `src/iiwa_web/`: Python utilities and REST/WebSocket/MCP interfaces.
|
||||
- `src/iiwa_sunrise/`: controller-side Java applications, built separately in Sunrise Workbench.
|
||||
- `cobot/`: Python CLI; `scripts/` and `docker/`: environment setup.
|
||||
- `doc/lwc-doc/docs/`: bilingual documentation (`*.md` and `*.en.md`).
|
||||
|
||||
## Build, Test, and Development Commands
|
||||
|
||||
Run commands from the repository root after installing prerequisites with `cobot setup`.
|
||||
|
||||
```bash
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
colcon build --base-paths src --symlink-install
|
||||
source install/setup.bash
|
||||
colcon test --base-paths src
|
||||
colcon test-result --verbose
|
||||
```
|
||||
|
||||
These commands build the workspace, activate its packages, and run/report registered tests. Specify `--base-paths src` because the repository root contains `COLCON_IGNORE`.
|
||||
|
||||
- `cobot rebuild iiwa_controller --symlink-install`: rebuild one package; build dependencies first.
|
||||
- `cobot run local`: launch interactively; select **Webots simulator** for development.
|
||||
- `cobot doc-setup`: serve documentation locally using Docker.
|
||||
- `cobot doc-setup build`: generate the static documentation and PDF.
|
||||
|
||||
## Coding Style & Naming Conventions
|
||||
|
||||
Use four-space indentation for Python and follow adjacent code for C++ formatting (the hardware interface uses two spaces). C++ targets C++17. Use `snake_case` for Python modules/functions, `PascalCase` for classes and ROS interface files, and `*.launch.py` for launch files. Preserve existing C++ API names. No repository-wide formatter configuration is present; avoid unrelated formatting and bundled SDK edits.
|
||||
|
||||
## Testing Guidelines
|
||||
|
||||
Manifests declare pytest for Python packages and ament lint dependencies for several CMake packages, but no dedicated test suite or coverage threshold is established. Place new tests under the relevant package's `test/` directory, using `test_*.py` for pytest, and register them with its build system. Record Webots smoke-test steps for motion or launch changes; distinguish simulation validation from physical-hardware validation.
|
||||
|
||||
## Commit & Pull Request Guidelines
|
||||
|
||||
History mixes imperative summaries with `feat:`, `fix:`, `docs:`, and `refactor:` prefixes; prefer a concise prefixed summary. Keep commits focused. PRs should explain the behavior change, affected packages, validation commands/results, and related issues. Include screenshots for visual changes and update both documentation languages when applicable.
|
||||
|
||||
## Configuration & Hardware Safety
|
||||
|
||||
Review `cobot-setting.yaml` before launching. Validate motion changes in simulation first. Physical runs require checking robot networking, joint limits, active tool/load data, control mode, and a clear work area with operator supervision.
|
||||
+2
-2
@@ -36,8 +36,8 @@ digital_twin:
|
||||
# rotation_deg: [1.57, 0, 0]
|
||||
# - path: pkg://iiwa_description/objects/hammer/hammer5.obj
|
||||
# rotation_deg: [1.57, 0, 0]
|
||||
- path: pkg://iiwa_description/objects/grant/vint.obj
|
||||
rotation_deg: [90, 0, 0]
|
||||
# - path: pkg://iiwa_description/objects/grant/vint.obj
|
||||
# rotation_deg: [90, 0, 0]
|
||||
area: # метры в системе основания робота, до падения
|
||||
x: [0.3, 0.7]
|
||||
y: [-0.3, 0.3]
|
||||
|
||||
@@ -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`
|
||||
@@ -146,12 +146,14 @@ safe_poses:
|
||||
# Webots bounding objects so MoveIt and the simulator agree.
|
||||
scene_collisions:
|
||||
table:
|
||||
# Top at Z = 0.789, deliberately 1 mm below the robot base (Z = 0.790)
|
||||
# and 4 mm below the work surface (Z = 0.793), so the base is not in
|
||||
# collision while downward motion is still blocked.
|
||||
# Top at Z = 0.786. The base_link collision mesh reaches 2.05 mm below
|
||||
# its frame origin (world Z = 0.788), so a table top at 0.789 was actually
|
||||
# a permanent 1 mm penetration that the contact check flagged only
|
||||
# intermittently. The 2 mm gap below the mesh bottom keeps the base
|
||||
# collision-free while the slab still blocks downward motion.
|
||||
type: box
|
||||
size: [1.28, 1.08, 0.045]
|
||||
offset_z: 0.7665
|
||||
offset_z: 0.7635
|
||||
# The fixture is a frame, not a solid block: a low base plate and two
|
||||
# side rails. A full box would cover the hatch seat and make placement
|
||||
# impossible.
|
||||
|
||||
@@ -46,11 +46,19 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
|
||||
|
||||
## Модели столкновений
|
||||
|
||||
- Стол: плита 1,28 × 1,08 × 0,045 м с верхом 0,789 м.
|
||||
- Стол: плита 1,28 × 1,08 × 0,045 м с верхом 0,786 м. Меш коллизии
|
||||
`base_link` доходит до 2,05 мм ниже фрейма (мир: Z = 0,788); прежний верх
|
||||
0,789 м давал постоянное проникновение на 1 мм, которое детектор контактов
|
||||
замечал нестабильно и блокировал планирование (опыт 2, отказ
|
||||
`place_cap:retreat`).
|
||||
- Стапель: нижняя плита 0,35 × 0,22 × 0,01 м и две боковые направляющие
|
||||
0,014 × 0,156 × 0,016 м. Сплошной блок не используется, иначе он перекрывает
|
||||
посадочное место люка.
|
||||
- Корпус: 0,30 × 0,17 × 0,00915 м; люк: 0,1384 × 0,0984 × 0,00367 м.
|
||||
Бокс люка поднят на 1 мм над боксом корпуса (`CAP_BOX_LIFT`), бокс винта —
|
||||
на 1 мм над дном лотка (`SCREW_BOX_LIFT`): касание с нулевым зазором
|
||||
считается контактом сцены и блокирует все последующие планы из этого
|
||||
состояния (например, `grant_body - grant_cap` после посадки люка).
|
||||
- Стойка: плита 0,42 × 0,14 × 0,01 м ниже гнёзд.
|
||||
- Лоток: дно и четыре стенки; винты — 0,0055 × 0,0055 × 0,009 м.
|
||||
- Вакуумный инструмент: 0,0821 × 0,081 × 0,14 м; магнитный — тонкий стержень
|
||||
@@ -89,16 +97,18 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
|
||||
|
||||
## Результаты тестов
|
||||
|
||||
Финальный прогон: `colcon test` — 336 тестов, 0 ошибок, 0 падений. Тесты покрывают контракт
|
||||
сцен, геометрию, supervisor, сцену MoveIt, движение, операции, маршруты и
|
||||
журнал.
|
||||
Финальный прогон: `colcon test` — 237 тестов (209 `iiwa_experement`, 28
|
||||
`iiwa_planning`), 0 ошибок, 0 падений. Тесты покрывают контракт сцен,
|
||||
геометрию, supervisor, сцену MoveIt (включая явное снятие прикреплённых
|
||||
объектов), движение, операции, маршруты и журнал.
|
||||
|
||||
## Результаты 20 опытов
|
||||
|
||||
> Таблица ниже относится к предыдущей редакции маршрутов. После правок
|
||||
> (запрет `transport`, обязательный осевой подъём после каждого контакта,
|
||||
> untwist фланца, прикрепление переносимых объектов в сцене MoveIt) все 20
|
||||
> опытов требуют повторной верификации в Webots в режиме `execute`.
|
||||
> Повторная верификация после правок (запрет `transport`, обязательный
|
||||
> осевой подъём, untwist фланца, прикрепление переносимых объектов, честные
|
||||
> позы боксов, зазоры боксов, rest-to-rest) выполнена 10.11.2026: все 20
|
||||
> опытов завершились со статусом `completed` и `home_reached=true` в режиме
|
||||
> `execute`. Журналы: `~/.ros/iiwa_experement/runs/experiment_<id>_20261110T*.jsonl`.
|
||||
|
||||
Каждый сценарий должен хотя бы один раз завершиться со статусом `completed`.
|
||||
При транзиентном отказе планировщика (`Планирование не удалось`) запуск
|
||||
@@ -190,8 +200,17 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
|
||||
- Прикреплённые объекты MoveIt моделируются: захваченные инструмент, люк и
|
||||
винт переносятся из мировой сцены в `attached_collision_objects` с
|
||||
разрешённым контактом звеньев `tcp`, `patron`, `link7` и держателя камеры.
|
||||
Если в установленной версии MoveItPy снова появятся ложные касания,
|
||||
первым делом проверяются touch-звенья и габариты боксов в `geometry.yaml`.
|
||||
Прикреплённые боксы люка и винта укорочены так, чтобы на позе захвата их
|
||||
низ оставался на 1 мм выше опорной поверхности (корпус/стол/лоток): касание
|
||||
с нулевым зазором считается контактом стартового состояния и блокирует
|
||||
обязательный осевой подъём.
|
||||
- Поза бокса при возврате объекта в мир — это поза **центра бокса** с
|
||||
плоской yaw-ориентацией (конвенция `build_world_objects`), а не поза узла
|
||||
OBJ с поворотом Rx90. Док инструмента и восстановление винта/люка после
|
||||
имитированного срыва используют `bridge.restore_world` с калиброванной
|
||||
начальной позой; поставленный люк добавляется через `placed_cap_pose`.
|
||||
Нарушение этой конвенции было причиной «ложных столкновений» (поставленный
|
||||
люк становился вертикальной плитой, задокованный инструмент — лежачим).
|
||||
- MoveIt/MoveItPy может аварийно завершиться после очень длинной серии
|
||||
планирований; один запуск выполняет один опыт, поэтому серия ограничена.
|
||||
- `Node.remove()` для винта в Webots R2025a иногда приводит к падению
|
||||
|
||||
@@ -26,6 +26,8 @@ from .planning_scene import (
|
||||
attached_cap,
|
||||
attached_screw,
|
||||
attached_tool,
|
||||
cap_world_box,
|
||||
placed_cap_pose,
|
||||
screw_object_id,
|
||||
)
|
||||
from .scenario import ScenarioSpec
|
||||
@@ -243,6 +245,11 @@ class ReferenceOperations:
|
||||
result = None
|
||||
planning_failed = False
|
||||
for attempt in range(attempts):
|
||||
# Rest-to-rest planning: the server's response adapter rejects any
|
||||
# plan whose start state is still moving. Wait for the arm to
|
||||
# settle before every attempt instead of rapid-firing retries
|
||||
# into the same non-stationary window.
|
||||
self.motion.wait_stationary()
|
||||
result = self.motion.move_pose(
|
||||
goal,
|
||||
planner=planner,
|
||||
@@ -268,6 +275,7 @@ class ReferenceOperations:
|
||||
continue
|
||||
if allow_fallback and planner != 'ompl' and planning_failed:
|
||||
self.motion.reset_stop()
|
||||
self.motion.wait_stationary()
|
||||
fallback = self.motion.move_pose(
|
||||
goal, planner='ompl', speed=speed, label=f'{label}:ompl',
|
||||
)
|
||||
@@ -339,6 +347,7 @@ class ReferenceOperations:
|
||||
operation stops instead of continuing at contact depth.
|
||||
"""
|
||||
lifted = False
|
||||
last_error: OperationError | None = None
|
||||
for index, gap in enumerate((self.geometry.approach_gap, 0.03, 0.015)):
|
||||
lift_label = f'{label}:retreat' if index == 0 else f'{label}:retreat:{gap:.3f}'
|
||||
try:
|
||||
@@ -351,9 +360,12 @@ class ReferenceOperations:
|
||||
except OperationError as error:
|
||||
if 'Планирование' not in str(error):
|
||||
raise
|
||||
last_error = error
|
||||
self.motion.reset_stop()
|
||||
if not lifted:
|
||||
raise OperationError(f'{label}: the axial retreat was rejected')
|
||||
raise OperationError(
|
||||
f'{label}: the axial retreat was rejected ({last_error})'
|
||||
)
|
||||
self._move(
|
||||
self._above(goal, self.geometry.transfer_gap), 'ompl',
|
||||
self.geometry.speeds['transfer'], f'{label}:clear',
|
||||
@@ -367,6 +379,7 @@ class ReferenceOperations:
|
||||
The sideways step starts from the height that was actually reached.
|
||||
"""
|
||||
height = None
|
||||
last_error: OperationError | None = None
|
||||
for index, gap in enumerate((
|
||||
self.geometry.transfer_gap, self.geometry.approach_gap, 0.03, 0.015,
|
||||
)):
|
||||
@@ -381,10 +394,12 @@ class ReferenceOperations:
|
||||
except OperationError as error:
|
||||
if 'Планирование' not in str(error):
|
||||
raise
|
||||
last_error = error
|
||||
self.motion.reset_stop()
|
||||
if height is None:
|
||||
raise OperationError(
|
||||
f'{label}: the axial lift out of the rack was rejected'
|
||||
f'{label}: the axial lift out of the rack was rejected '
|
||||
f'({last_error})'
|
||||
)
|
||||
base = self._above(goal, height)
|
||||
for offset in (0.10, 0.06, 0.03):
|
||||
@@ -403,8 +418,11 @@ class ReferenceOperations:
|
||||
except OperationError as error:
|
||||
if 'Планирование' not in str(error):
|
||||
raise
|
||||
last_error = error
|
||||
self.motion.reset_stop()
|
||||
raise OperationError(f'{label}: no lateral rack exit was plannable')
|
||||
raise OperationError(
|
||||
f'{label}: no lateral rack exit was plannable ({last_error})'
|
||||
)
|
||||
|
||||
def _contact_with_recovery(
|
||||
self, goal: Transform, label: str, *, prepare=None, **kwargs,
|
||||
@@ -495,11 +513,9 @@ class ReferenceOperations:
|
||||
self._require_synchronized()
|
||||
self._contact_with_recovery(goal, f'dock_tool({tool_name})')
|
||||
self.supervisor.command('dock_tool', tool_name)
|
||||
self.bridge.add_world(
|
||||
f'grant_{tool_name}',
|
||||
self.geometry.tool_collisions[tool_name],
|
||||
rack_pose,
|
||||
)
|
||||
# The tool is back in its calibrated socket: restore the exact
|
||||
# initial box pose instead of reusing the rotated node pose.
|
||||
self.bridge.restore_world(f'grant_{tool_name}')
|
||||
self._require_synchronized()
|
||||
self._exit_rack(goal, f'dock_tool({tool_name})')
|
||||
self._emit('dock_tool', object=tool_name, phase='done')
|
||||
@@ -551,8 +567,12 @@ class ReferenceOperations:
|
||||
remove_phase='contact',
|
||||
)
|
||||
self.supervisor.command('place_cap', 'cap', target=target)
|
||||
# The collision box of the installed hatch is flat, thinner than
|
||||
# the plate and centred on it: it must clear both the body box
|
||||
# below and the suction-cup box above at the contact pose.
|
||||
self.bridge.add_world(
|
||||
'grant_cap', self.geometry.cap_collision, target,
|
||||
'grant_cap', cap_world_box(self.geometry),
|
||||
placed_cap_pose(target, self.spec.body.yaw_deg),
|
||||
)
|
||||
self._require_synchronized()
|
||||
self._retreat(goal, 'place_cap')
|
||||
@@ -639,10 +659,9 @@ class ReferenceOperations:
|
||||
goal = tcp_goal(screw_pose, self.geometry.screw_tcp_to_node)
|
||||
def restore_screw() -> None:
|
||||
# The first attempt removed the screw from the planning scene
|
||||
# before its transfer was rejected. Restore it for the retry.
|
||||
self.bridge.add_world(
|
||||
def_name, self.geometry.screw_collision, screw_pose,
|
||||
)
|
||||
# before its transfer was rejected. Restore its calibrated
|
||||
# tray pose for the retry.
|
||||
self.bridge.restore_world(def_name)
|
||||
self._require_synchronized()
|
||||
|
||||
self._contact_with_recovery(
|
||||
@@ -820,9 +839,7 @@ class ReferenceOperations:
|
||||
goal = tcp_goal(pose, self.geometry.cap_tcp_to_node)
|
||||
self._contact(goal, label, remove_before_contact='grant_cap')
|
||||
self._retreat(goal, label)
|
||||
self.bridge.add_world(
|
||||
'grant_cap', self.geometry.cap_collision, pose,
|
||||
)
|
||||
self.bridge.restore_world('grant_cap')
|
||||
self._require_synchronized()
|
||||
elif kind == 'screw':
|
||||
def_name = screw_object_id(name)
|
||||
@@ -830,9 +847,7 @@ class ReferenceOperations:
|
||||
goal = tcp_goal(pose, self.geometry.screw_tcp_to_node)
|
||||
self._contact(goal, label, remove_before_contact=def_name)
|
||||
self._retreat(goal, label)
|
||||
self.bridge.add_world(
|
||||
def_name, self.geometry.screw_collision, pose,
|
||||
)
|
||||
self.bridge.restore_world(def_name)
|
||||
self._require_synchronized()
|
||||
else:
|
||||
raise OperationError(f'unknown grasp target {kind!r}')
|
||||
|
||||
@@ -102,6 +102,45 @@ def screw_object_id(screw_id: str) -> str:
|
||||
return f'grant_screw_{int(screw_id.split("_")[1]):02d}'
|
||||
|
||||
|
||||
# Lift from an object node pose to the collision-box centre, matching
|
||||
# build_world_objects. A node origin sits on the lower face/axis of the OBJ
|
||||
# and is rotated so the local Y axis becomes world Z; the collision box is a
|
||||
# flat, yaw-only box centred above the support surface.
|
||||
#
|
||||
# The installed hatch is sandwiched between two hard constraints: its box
|
||||
# must clear the conservative body box below AND the suction-cup box above at
|
||||
# the place contact pose. A zero-distance touch on either side reads as a
|
||||
# scene contact and blocks every later plan from that state, so the world
|
||||
# model of the hatch is 1 mm thinner than the real plate, leaving 0.5 mm of
|
||||
# clearance on both sides.
|
||||
CAP_BOX_LIFT = 0.004165
|
||||
CAP_BOX_THICKNESS = 0.00267
|
||||
SCREW_BOX_LIFT = 0.0055
|
||||
TOOL_BOX_LIFT = 0.070
|
||||
|
||||
|
||||
def cap_world_box(geometry: GeometrySpec) -> Box:
|
||||
"""Collision box of the hatch as a world object (thinner than the plate)."""
|
||||
return Box(
|
||||
size=(
|
||||
geometry.cap_collision.size[0],
|
||||
geometry.cap_collision.size[1],
|
||||
CAP_BOX_THICKNESS,
|
||||
),
|
||||
offset=geometry.cap_collision.offset,
|
||||
)
|
||||
|
||||
|
||||
def placed_cap_pose(seat: Transform, body_yaw_deg: float) -> Transform:
|
||||
"""Collision-box pose of the installed hatch: flat, centred on the plate."""
|
||||
return Transform.from_xyz_rpy(
|
||||
[seat.xyz[0], seat.xyz[1], seat.xyz[2] + CAP_BOX_LIFT],
|
||||
[0.0, 0.0, body_yaw_deg],
|
||||
parent='world',
|
||||
child='cap',
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SceneObject:
|
||||
object_id: str
|
||||
@@ -242,7 +281,7 @@ def build_world_objects(spec: ScenarioSpec, geometry: GeometrySpec) -> list[Scen
|
||||
_rotated_pose(
|
||||
screw.pose.x,
|
||||
screw.pose.y,
|
||||
screw.pose.z + 0.0045,
|
||||
screw.pose.z + SCREW_BOX_LIFT,
|
||||
screw.pose.yaw_deg,
|
||||
),
|
||||
))
|
||||
@@ -254,7 +293,7 @@ def build_world_objects(spec: ScenarioSpec, geometry: GeometrySpec) -> list[Scen
|
||||
f'grant_{tool.name}',
|
||||
Box(size=size, offset=(0.0, 0.0, 0.070)),
|
||||
_rotated_pose(
|
||||
tool.pose.x, tool.pose.y, tool.pose.z + 0.070,
|
||||
tool.pose.x, tool.pose.y, tool.pose.z + TOOL_BOX_LIFT,
|
||||
tool.pose.yaw_deg + 180.0,
|
||||
),
|
||||
))
|
||||
@@ -264,9 +303,9 @@ def build_world_objects(spec: ScenarioSpec, geometry: GeometrySpec) -> list[Scen
|
||||
assert cap_pose is not None
|
||||
objects.append(SceneObject(
|
||||
'grant_cap',
|
||||
geometry.cap_collision,
|
||||
cap_world_box(geometry),
|
||||
_rotated_pose(
|
||||
cap_pose.x, cap_pose.y, cap_pose.z + 0.004165,
|
||||
cap_pose.x, cap_pose.y, cap_pose.z + CAP_BOX_LIFT,
|
||||
cap_pose.yaw_deg,
|
||||
),
|
||||
))
|
||||
@@ -292,11 +331,19 @@ def attached_tool(name: str, geometry: GeometrySpec) -> AttachedSpec:
|
||||
|
||||
|
||||
def attached_cap(geometry: GeometrySpec) -> AttachedSpec:
|
||||
# The plate box stops 1 mm short of the support surface (body/table at
|
||||
# the grasp pose): a zero-distance touch reads as a scene contact and
|
||||
# blocks the mandatory axial retreat right after the grasp.
|
||||
box = Box(
|
||||
size=(geometry.cap_collision.size[0], geometry.cap_collision.size[1],
|
||||
0.00267),
|
||||
offset=(0.0, 0.0, 0.141335),
|
||||
)
|
||||
return AttachedSpec(
|
||||
object_id='grant_cap',
|
||||
box=geometry.cap_collision,
|
||||
box=box,
|
||||
relative_pose=Transform.from_xyz_rpy(
|
||||
[0.0, 0.0, geometry.cap_collision.offset[2] + 0.0552],
|
||||
[0.0, 0.0, 0.141335 + 0.0552],
|
||||
[0.0, 0.0, 0.0],
|
||||
parent='patron',
|
||||
child='grant_cap',
|
||||
@@ -306,12 +353,14 @@ def attached_cap(geometry: GeometrySpec) -> AttachedSpec:
|
||||
|
||||
|
||||
def attached_screw(screw_id: str, geometry: GeometrySpec) -> AttachedSpec:
|
||||
# Same 1 mm clearance at the tray floor as in attached_cap.
|
||||
box = Box(size=(0.0055, 0.0055, 0.008), offset=(0.0, 0.0, 0.144))
|
||||
object_id = screw_object_id(screw_id)
|
||||
return AttachedSpec(
|
||||
object_id=object_id,
|
||||
box=geometry.screw_collision,
|
||||
box=box,
|
||||
relative_pose=Transform.from_xyz_rpy(
|
||||
[0.0, 0.0, geometry.screw_collision.offset[2] + 0.0552],
|
||||
[0.0, 0.0, 0.144 + 0.0552],
|
||||
[0.0, 0.0, 0.0],
|
||||
parent='patron',
|
||||
child=object_id,
|
||||
@@ -445,6 +494,7 @@ class PlanningSceneBridge:
|
||||
GetPlanningScene, GET_SCENE_SERVICE,
|
||||
)
|
||||
self._world_objects: dict[str, SceneObject] = {}
|
||||
self._initial_world: dict[str, SceneObject] = {}
|
||||
self._attached: dict[str, AttachedSpec] = {}
|
||||
self._seen_world: set[str] = set()
|
||||
self._seen_attached: set[str] = set()
|
||||
@@ -694,6 +744,7 @@ class PlanningSceneBridge:
|
||||
self._world_objects = {
|
||||
obj.object_id: obj for obj in build_world_objects(spec, geometry)
|
||||
}
|
||||
self._initial_world = dict(self._world_objects)
|
||||
self._attached.clear()
|
||||
extra = []
|
||||
for spec in self._attached.values():
|
||||
@@ -708,6 +759,26 @@ class PlanningSceneBridge:
|
||||
self._world_objects, (), seen_world=set(self._world_objects),
|
||||
)
|
||||
|
||||
def _publish_attached_removal(self, object_id: str) -> None:
|
||||
"""Send an explicit attached-object removal diff.
|
||||
|
||||
Full scenes carry the attached list inside ``robot_state`` with
|
||||
``is_diff=True``; a consumer that applies that list as a diff never
|
||||
treats an omission as a removal and keeps a ghost attachment. The
|
||||
explicit REMOVE entry is unambiguous for every consumer.
|
||||
"""
|
||||
publisher = getattr(self, '_publisher', None)
|
||||
if publisher is None or self.node is None:
|
||||
return
|
||||
scene = self._diff()
|
||||
removal = AttachedCollisionObject()
|
||||
removal.link_name = 'patron'
|
||||
removal.object.id = object_id
|
||||
removal.object.operation = CollisionObject.REMOVE
|
||||
scene.robot_state.attached_collision_objects.append(removal)
|
||||
publisher.publish(scene)
|
||||
self._spin()
|
||||
|
||||
def attach(self, spec: AttachedSpec) -> None:
|
||||
self._world_objects.pop(spec.object_id, None)
|
||||
self._attached[spec.object_id] = spec
|
||||
@@ -720,23 +791,45 @@ class PlanningSceneBridge:
|
||||
f'cannot detach {object_id!r}: it is not attached'
|
||||
)
|
||||
spec = self._attached.pop(object_id)
|
||||
self._publish_attached_removal(object_id)
|
||||
object_spec = SceneObject(object_id, spec.box, world_pose)
|
||||
self._world_objects[object_id] = object_spec
|
||||
self._rebuild_acm()
|
||||
self._publish_full()
|
||||
|
||||
def remove(self, object_id: str) -> None:
|
||||
self._attached.pop(object_id, None)
|
||||
if self._attached.pop(object_id, None) is not None:
|
||||
self._publish_attached_removal(object_id)
|
||||
self._world_objects.pop(object_id, None)
|
||||
self._rebuild_acm()
|
||||
self._publish_full()
|
||||
|
||||
def add_world(self, object_id: str, box: Box, world_pose: Transform) -> None:
|
||||
self._attached.pop(object_id, None)
|
||||
if self._attached.pop(object_id, None) is not None:
|
||||
self._publish_attached_removal(object_id)
|
||||
self._world_objects[object_id] = SceneObject(object_id, box, world_pose)
|
||||
self._rebuild_acm()
|
||||
self._publish_full()
|
||||
|
||||
def restore_world(self, object_id: str) -> None:
|
||||
"""Return an object to the exact box pose it had at scene initialisation.
|
||||
|
||||
Use this when an object physically returns to its calibrated support
|
||||
pose (a tool docked in its socket, a part restored after a simulated
|
||||
grasp failure): the initial pose already carries the correct
|
||||
box-centre lift and flat orientation.
|
||||
"""
|
||||
original = self._initial_world.get(object_id)
|
||||
if original is None:
|
||||
raise SceneSynchronizationError(
|
||||
f'cannot restore {object_id!r}: it was not in the initial scene'
|
||||
)
|
||||
if self._attached.pop(object_id, None) is not None:
|
||||
self._publish_attached_removal(object_id)
|
||||
self._world_objects[object_id] = original
|
||||
self._rebuild_acm()
|
||||
self._publish_full()
|
||||
|
||||
def allow_contact(self, first: str, second: str, allowed: bool = True) -> None:
|
||||
"""Change exactly one ACM pair; all other checks stay active."""
|
||||
if self.last_scene is None or not self.last_scene.allowed_collision_matrix.entry_names:
|
||||
|
||||
@@ -200,6 +200,11 @@ def build_reference_sequence(spec: ScenarioSpec) -> tuple[ReferenceStep, ...]:
|
||||
return tuple(cleaned)
|
||||
|
||||
|
||||
def _planning_failure(message: str) -> bool:
|
||||
text = message.lower()
|
||||
return 'планирование' in text or 'planning' in text or 'недостижима' in text
|
||||
|
||||
|
||||
def execute_sequence(sequence: tuple[ReferenceStep, ...], context) -> RunResult:
|
||||
"""Execute a route, stopping at the first unsafe or unexpected outcome."""
|
||||
operations = context.operations
|
||||
@@ -216,6 +221,15 @@ def execute_sequence(sequence: tuple[ReferenceStep, ...], context) -> RunResult:
|
||||
trace.event({'event': 'step', 'step': step.as_dict(), 'phase': 'start'})
|
||||
if step.kind == 'move_named':
|
||||
result = motion.move_named(*step.args, **step.kwargs)
|
||||
if not result.success and _planning_failure(result.message):
|
||||
# A rejected plan executed no motion. The start-state
|
||||
# guard can trip on a startup clock/telemetry race, so
|
||||
# wait for the arm to settle and retry exactly once.
|
||||
trace.event({'event': 'named_retry', 'step': step.as_dict(),
|
||||
'reason': result.message})
|
||||
motion.wait_stationary(timeout_sec=5.0)
|
||||
motion.reset_stop()
|
||||
result = motion.move_named(*step.args, **step.kwargs)
|
||||
if not result.success:
|
||||
raise SequenceError(result.message)
|
||||
if step.args and step.args[0] == 'home':
|
||||
|
||||
@@ -135,6 +135,15 @@ def _materialize_setting(
|
||||
tool = _mapping(data.setdefault('tool', {}), 'tool')
|
||||
tool['active'] = 'patron'
|
||||
|
||||
# The reference executor runs many rest-to-rest plans back to back. The
|
||||
# Webots position controller keeps a heavy tool oscillating at a few
|
||||
# hundredths of rad/s for seconds after a stop, which trips the default
|
||||
# 0.01 rad/s start-state guard and aborts otherwise valid plans. Loosen
|
||||
# the guard for these launches only; the runner still waits for the arm
|
||||
# to settle before every plan and verifies the reached pose afterwards.
|
||||
planning = _mapping(data.setdefault('planning', {}), 'planning')
|
||||
planning['stopped_velocity_tolerance'] = 0.08
|
||||
|
||||
if moveit_cpp_path is not None:
|
||||
controller = _mapping(data.setdefault('controller', {}), 'controller')
|
||||
moveit = _mapping(controller.setdefault('moveit', {}), 'controller.moveit')
|
||||
|
||||
@@ -50,6 +50,7 @@ class FakeBridge:
|
||||
self.attached = []
|
||||
self.detached = []
|
||||
self.removed = []
|
||||
self.restored = []
|
||||
|
||||
def attach(self, spec):
|
||||
self.attached.append(spec.object_id)
|
||||
@@ -62,7 +63,14 @@ class FakeBridge:
|
||||
if object_id in self.attached:
|
||||
self.attached.remove(object_id)
|
||||
|
||||
def restore_world(self, object_id):
|
||||
if object_id in self.attached:
|
||||
self.attached.remove(object_id)
|
||||
self.restored.append(object_id)
|
||||
|
||||
def add_world(self, object_id, box, pose):
|
||||
if object_id in self.attached:
|
||||
self.attached.remove(object_id)
|
||||
self.added_world = getattr(self, 'added_world', [])
|
||||
self.added_world.append((object_id, pose))
|
||||
|
||||
@@ -157,7 +165,7 @@ def test_dock_tool_returns_the_tool_to_its_socket():
|
||||
result = operations.dock_tool('vacuum')
|
||||
assert result.success
|
||||
assert supervisor.commands[0][:2] == ('dock_tool', 'vacuum')
|
||||
assert bridge.added_world[0][0] == 'grant_vacuum'
|
||||
assert bridge.restored == ['grant_vacuum']
|
||||
assert labels(motion)[-1] == 'dock_tool(vacuum):exit'
|
||||
|
||||
|
||||
|
||||
@@ -5,7 +5,7 @@ import numpy as np
|
||||
import pytest
|
||||
from moveit_msgs.msg import CollisionObject, PlanningScene
|
||||
|
||||
from iiwa_experement.geometry import Box, Transform, load_geometry
|
||||
from iiwa_experement.geometry import Box, Transform, load_geometry, world_target
|
||||
from iiwa_experement.planning_scene import (
|
||||
PlanningSceneBridge,
|
||||
SceneObject,
|
||||
@@ -16,6 +16,7 @@ from iiwa_experement.planning_scene import (
|
||||
attached_tool,
|
||||
build_world_objects,
|
||||
collision_object,
|
||||
placed_cap_pose,
|
||||
scene_ids,
|
||||
update_contact_entry,
|
||||
)
|
||||
@@ -41,6 +42,7 @@ class FakeBridge(PlanningSceneBridge):
|
||||
self._world_objects = {
|
||||
obj.object_id: obj for obj in build_world_objects(scenario, geometry)
|
||||
}
|
||||
self._initial_world = dict(self._world_objects)
|
||||
self._attached = {}
|
||||
self._disabled_pairs = []
|
||||
self._acm_names = []
|
||||
@@ -87,7 +89,7 @@ def test_collision_boxes_match_measured_meshes():
|
||||
objects = {obj.object_id: obj for obj in build_world_objects(
|
||||
spec(16), load_geometry(GEOMETRY))}
|
||||
assert objects['grant_body'].box.size == pytest.approx((0.30, 0.17, 0.00915))
|
||||
assert objects['grant_cap'].box.size == pytest.approx((0.1384, 0.0984, 0.00367))
|
||||
assert objects['grant_cap'].box.size == pytest.approx((0.1384, 0.0984, 0.00267))
|
||||
assert objects['grant_screw_01'].box.size == pytest.approx((0.0055, 0.0055, 0.009))
|
||||
assert objects['grant_vacuum'].box.size[2] == pytest.approx(0.140)
|
||||
|
||||
@@ -144,9 +146,11 @@ def test_cap_and_screw_attachment_geometry_uses_tcp_offsets():
|
||||
geometry = load_geometry(GEOMETRY)
|
||||
cap = attached_cap(geometry)
|
||||
screw = attached_screw('screw_3', geometry)
|
||||
assert cap.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.141835 + 0.0552))
|
||||
assert cap.box.size == pytest.approx((0.1384, 0.0984, 0.00367))
|
||||
assert screw.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.1445 + 0.0552))
|
||||
# The boxes stop 1 mm short of the support surface at the grasp pose.
|
||||
assert cap.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.141335 + 0.0552))
|
||||
assert cap.box.size == pytest.approx((0.1384, 0.0984, 0.00267))
|
||||
assert screw.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.144 + 0.0552))
|
||||
assert screw.box.size == pytest.approx((0.0055, 0.0055, 0.008))
|
||||
|
||||
|
||||
def test_contact_permission_changes_exactly_one_pair():
|
||||
@@ -159,6 +163,40 @@ def test_contact_permission_changes_exactly_one_pair():
|
||||
assert matrix[0][1] is False
|
||||
|
||||
|
||||
def test_restore_world_returns_the_calibrated_box_pose():
|
||||
bridge = FakeBridge(16)
|
||||
tool = attached_tool('vacuum', bridge.geometry)
|
||||
bridge.attach(tool)
|
||||
bridge.restore_world(tool.object_id)
|
||||
assert tool.object_id not in bridge._attached
|
||||
restored = bridge._world_objects[tool.object_id]
|
||||
expected = {
|
||||
obj.object_id: obj
|
||||
for obj in build_world_objects(bridge.spec, bridge.geometry)
|
||||
}[tool.object_id]
|
||||
assert restored.pose.xyz == pytest.approx(expected.pose.xyz)
|
||||
assert restored.pose.rotation == expected.pose.rotation
|
||||
with pytest.raises(Exception):
|
||||
bridge.restore_world('grant_unknown')
|
||||
|
||||
|
||||
def test_placed_cap_box_is_flat_and_matches_the_preinstalled_cap():
|
||||
geometry = load_geometry(GEOMETRY)
|
||||
scenario = spec(1)
|
||||
seat = world_target(scenario, geometry, 'cap_seat')
|
||||
placed = placed_cap_pose(seat, scenario.body.yaw_deg)
|
||||
# The box is flat (yaw only) and centred on the plate, like the
|
||||
# pre-installed hatch of experiments 11-15.
|
||||
preinstalled = {
|
||||
obj.object_id: obj
|
||||
for obj in build_world_objects(spec(11), geometry)
|
||||
}['grant_cap']
|
||||
assert placed.xyz == pytest.approx(preinstalled.pose.xyz)
|
||||
assert np.asarray(placed.rotation) == pytest.approx(
|
||||
np.asarray(preinstalled.pose.rotation)
|
||||
)
|
||||
|
||||
|
||||
def test_srdf_collision_matrix_allows_only_disabled_pairs():
|
||||
root = Path(__file__).resolve().parents[2]
|
||||
srdf = root / 'iiwa_config' / 'config' / 'moveit' / 'iiwa7.srdf'
|
||||
|
||||
@@ -37,7 +37,7 @@ public:
|
||||
const sensor_msgs::msg::JointState &requested) const {
|
||||
// Explicit nonzero requested initial dynamics are never silently discarded.
|
||||
if (!requested.velocity.empty() && !velocities_ok(names, requested))
|
||||
return false;
|
||||
return diagnose(names, requested, "request start_state has moving joints");
|
||||
auto node = node_.lock();
|
||||
if (!node)
|
||||
return velocities_ok(names,
|
||||
@@ -46,15 +46,39 @@ public:
|
||||
if (!latest_ || std::chrono::duration<double>(
|
||||
std::chrono::steady_clock::now() - received_)
|
||||
.count() > timeout_)
|
||||
return false;
|
||||
return diagnose(names, requested, "no fresh /joint_states (wall clock)");
|
||||
const double age =
|
||||
node->now().seconds() -
|
||||
(latest_->header.stamp.sec + latest_->header.stamp.nanosec * 1e-9);
|
||||
return std::isfinite(age) && age >= 0 && age <= timeout_ &&
|
||||
velocities_ok(names, *latest_);
|
||||
if (!(std::isfinite(age) && age >= 0 && age <= timeout_))
|
||||
return diagnose(names, requested, "stale /joint_states stamp");
|
||||
return velocities_ok(names, *latest_)
|
||||
? true
|
||||
: diagnose(names, *latest_, "joints still moving");
|
||||
}
|
||||
|
||||
private:
|
||||
bool diagnose(const std::vector<std::string> &names,
|
||||
const sensor_msgs::msg::JointState &msg,
|
||||
const char *why) const {
|
||||
double worst = -1.0;
|
||||
std::string worst_joint;
|
||||
for (const auto &name : names) {
|
||||
auto found = std::find(msg.name.begin(), msg.name.end(), name);
|
||||
if (found == msg.name.end())
|
||||
continue;
|
||||
const size_t i = found - msg.name.begin();
|
||||
if (i < msg.velocity.size() && std::isfinite(msg.velocity[i]) &&
|
||||
std::abs(msg.velocity[i]) > worst) {
|
||||
worst = std::abs(msg.velocity[i]);
|
||||
worst_joint = name;
|
||||
}
|
||||
}
|
||||
RCLCPP_WARN(rclcpp::get_logger("iiwa_spline_limits"),
|
||||
"Start state rejected: %s; max |velocity| = %.4f rad/s at %s",
|
||||
why, worst, worst_joint.c_str());
|
||||
return false;
|
||||
}
|
||||
bool velocities_ok(const std::vector<std::string> &names,
|
||||
const sensor_msgs::msg::JointState &msg) const {
|
||||
for (const auto &name : names) {
|
||||
|
||||
Reference in New Issue
Block a user