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