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

fix: move screw tray to the reachable table edge

Browse Source
This commit is contained in:
Даниил Грабарь
2026-11-08 12:02:37 +10:00
parent 4ba53082f4
commit 923770ebbb
6 changed files with 36 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/superpowers/specs/2026-11-08-grant-scenes.md
+8 -8
View File
@@ -15,7 +15,7 @@
| Посадочное место `hatch_seat` | Центр `(0.30, 0.12)` | Поза связана с корпусом; устанавливаемый люк имеет начало в `(0.30, 0.12, 0.82594)` | | Посадочное место `hatch_seat` | Центр `(0.30, 0.12)` | Поза связана с корпусом; устанавливаемый люк имеет начало в `(0.30, 0.12, 0.82594)` |
| Свободный люк, стандартная поза | `(0.35, −0.22, 0.790667)` | Поворот 45°; лежит отдельно на столе | | Свободный люк, стандартная поза | `(0.35, −0.22, 0.790667)` | Поворот 45°; лежит отдельно на столе |
| Стойка инструментов | `(0.12, −0.42, 0.793)` | Фиксированная стойка с отдельными инструментами в гнёздах | | Стойка инструментов | `(0.12, −0.42, 0.793)` | Фиксированная стойка с отдельными инструментами в гнёздах |
| Лоток винтов | `(−0.08, 0.34, 0.793)` | Длинная сторона вдоль мировой оси X; нижняя поверхность на столе | | Лоток винтов | `(0.20, 0.46, 0.793)` | Длинная сторона вдоль мировой оси X; нижняя поверхность на столе; у левого края стола, впереди слева от стойки |
Стойка и корпус присутствуют во всех сценах. Свободный люк, уже установленный люк, лоток, винты и рабочие инструменты появляются по таблице ниже. Установленный люк виртуально фиксируется после отпускания присоски. В опытах с крепежом он уже установлен и зафиксирован при старте. Стойка и корпус присутствуют во всех сценах. Свободный люк, уже установленный люк, лоток, винты и рабочие инструменты появляются по таблице ниже. Установленный люк виртуально фиксируется после отпускания присоски. В опытах с крепежом он уже установлен и зафиксирован при старте.
@@ -36,11 +36,11 @@
| Винт | `(X, Y, Z)` | Винт | `(X, Y, Z)` | | Винт | `(X, Y, Z)` | Винт | `(X, Y, Z)` |
| --- | --- | --- | --- | | --- | --- | --- | --- |
| `screw_1` | `(−0.120, 0.325, 0.7954)` | `screw_6` | `(−0.080, 0.325, 0.7954)` | | `screw_1` | `(0.160, 0.445, 0.7954)` | `screw_6` | `(0.200, 0.445, 0.7954)` |
| `screw_2` | `(−0.040, 0.325, 0.7954)` | `screw_7` | `(−0.060, 0.325, 0.7954)` | | `screw_2` | `(0.240, 0.445, 0.7954)` | `screw_7` | `(0.220, 0.445, 0.7954)` |
| `screw_3` | `(−0.120, 0.355, 0.7954)` | `screw_8` | `(−0.100, 0.355, 0.7954)` | | `screw_3` | `(0.160, 0.475, 0.7954)` | `screw_8` | `(0.180, 0.475, 0.7954)` |
| `screw_4` | `(−0.040, 0.355, 0.7954)` | `screw_9` | `(−0.080, 0.355, 0.7954)` | | `screw_4` | `(0.240, 0.475, 0.7954)` | `screw_9` | `(0.200, 0.475, 0.7954)` |
| `screw_5` | `(−0.100, 0.325, 0.7954)` | `screw_10` | `(−0.060, 0.355, 0.7954)` | | `screw_5` | `(0.180, 0.445, 0.7954)` | `screw_10` | `(0.220, 0.475, 0.7954)` |
Все десять винтов взаимозаменяемы. Для установки в четыре отверстия планировщик может выбрать **любые четыре разных доступных винта**. Оценка графа не должна требовать конкретные номера `screw_i`. В опыте 15 отказ задаётся третьей попытке захвата винта, который план выбрал третьим; отказ затяжки задаётся отверстию `hole_4`. Все десять винтов взаимозаменяемы. Для установки в четыре отверстия планировщик может выбрать **любые четыре разных доступных винта**. Оценка графа не должна требовать конкретные номера `screw_i`. В опыте 15 отказ задаётся третьей попытке захвата винта, который план выбрал третьим; отказ затяжки задаётся отверстию `hole_4`.
@@ -77,7 +77,7 @@
| 16 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Участок герметика, люк, один винт | | 16 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Участок герметика, люк, один винт |
| 17 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Полный контур, люк, два винта | | 17 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Полный контур, люк, два винта |
| 18 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Полная сборка | | 18 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Полная сборка |
| 19 | Стандартные | Свободный `(0.37, −0.20, 0.790667)`, **60°** | Лоток `(−0.10, 0.36, 0.793)`, **10 винтов** | Все четыре | Координаты всех десяти винтов смещены вместе с лотком на `(−0.02, +0.02)` | | 19 | Стандартные | Свободный `(0.37, −0.20, 0.790667)`, **60°** | Лоток `(0.18, 0.48, 0.793)`, **10 винтов** | Все четыре | Координаты всех десяти винтов смещены вместе с лотком на `(−0.02, +0.02)` |
| 20 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Исходная геометрия обычная; сбои вводятся во время выполнения | | 20 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Исходная геометрия обычная; сбои вводятся во время выполнения |
В опыте 3 смещается вся пара «стапель + корпус» без изменения взаимной высоты и поворота. Координаты `hatch_seat` и четырёх отверстий сдвигаются вместе с ними на `(−0.01, +0.02)`. В опыте 19 сдвигается лоток **со всеми десятью винтами**; ни один винт не остаётся в старой точке. В опыте 3 смещается вся пара «стапель + корпус» без изменения взаимной высоты и поворота. Координаты `hatch_seat` и четырёх отверстий сдвигаются вместе с ними на `(−0.01, +0.02)`. В опыте 19 сдвигается лоток **со всеми десятью винтами**; ни один винт не остаётся в старой точке.
@@ -86,6 +86,6 @@
## Геометрические и технические ограничения ## Геометрические и технические ограничения
По габаритам 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 мм. Это проверка начальной компоновки, не проверка траекторий инструмента. По габаритам 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.26`, `Y = 0.425…0.495`. Эти прямоугольники не пересекаются. Лоток перенесён к левому краю стола со стороны `+Y`, чтобы все винты были в достижимой зоне; зазор от лотка до стапеля по Y около 104 мм. Зазор от свободного люка до стойки по Y около 46 мм, до стапеля около 55 мм. Это проверка начальной компоновки, не проверка траекторий инструмента.
Перед реализацией контактных действий нужно проверить в Webots достижимость TCP и коллизии при подъёме люка, при извлечении инструмента из гнезда и при работе между десятью винтами. В `iiwa.wbt` трение `TableMaterial` равно нулю; лоток и винты понадобится удерживать до захвата или задать подходящую контактную модель. Действующий спавнер `object_spawner.py` создаёт случайные OBJ, поэтому для этой таблицы потребуется детерминированная загрузка по номеру эксперимента. Стойка должна содержать отдельно управляемые рабочие инструменты; удалённый `GrantToolRack.proto` для нового мира не используется. Перед реализацией контактных действий нужно проверить в Webots достижимость TCP и коллизии при подъёме люка, при извлечении инструмента из гнезда и при работе между десятью винтами. В `iiwa.wbt` трение `TableMaterial` равно нулю; лоток и винты понадобится удерживать до захвата или задать подходящую контактную модель. Действующий спавнер `object_spawner.py` создаёт случайные OBJ, поэтому для этой таблицы потребуется детерминированная загрузка по номеру эксперимента. Стойка должна содержать отдельно управляемые рабочие инструменты; удалённый `GrantToolRack.proto` для нового мира не используется.
src/iiwa_experement/config/scenes.yaml
+5 -3
View File
@@ -27,8 +27,10 @@ poses:
z: 0.793 z: 0.793
yaw_deg: 0 yaw_deg: 0
tray: tray:
x: -0.08 # Left/front edge of the reachable workspace, clear of the tool rack and
y: 0.34 # the fixture. Moving the whole tray also moves its ten screws.
x: 0.20
y: 0.46
z: 0.793 z: 0.793
yaw_deg: 0 yaw_deg: 0
cap_free: cap_free:
@@ -164,7 +166,7 @@ experiments:
cap: free cap: free
cap_pose: {x: 0.37, y: -0.20, z: 0.790667, yaw_deg: 60} cap_pose: {x: 0.37, y: -0.20, z: 0.790667, yaw_deg: 60}
tray: true tray: true
tray_pose: {x: -0.10, y: 0.36, z: 0.793, yaw_deg: 0} tray_pose: {x: 0.18, y: 0.48, z: 0.793, yaw_deg: 0}
tools: [vacuum, magnetic_tool, sealant_tool, torque_tool] tools: [vacuum, magnetic_tool, sealant_tool, torque_tool]
"20": "20":
cap: free cap: free
src/iiwa_experement/doc/README.md
+3 -2
View File
@@ -84,8 +84,9 @@ ros2 service call /iiwa_experement/scene_ready std_srvs/srv/Trigger {}
Координаты и наборы объектов заданы только в `config/scenes.yaml`; генератор Координаты и наборы объектов заданы только в `config/scenes.yaml`; генератор
не выбирает случайные позы. В опытах 11–20 всегда ровно десять винтов, даже не выбирает случайные позы. В опытах 11–20 всегда ровно десять винтов, даже
если сценарий использует один, два или четыре. В опыте 19 лоток и все десять если сценарий использует один, два или четыре. Лоток стоит у левого края стола
винтов смещены вместе. (`+Y`) впереди слева от стойки, в достижимой зоне; в опыте 19 лоток и все
десять винтов смещены вместе.
## Ресурсы ## Ресурсы
src/iiwa_experement/doc/experiment-scenes.md
+8 -8
View File
@@ -15,7 +15,7 @@
| Посадочное место `hatch_seat` | Центр `(0.30, 0.12)` | Поза связана с корпусом; устанавливаемый люк имеет начало в `(0.30, 0.12, 0.82594)` | | Посадочное место `hatch_seat` | Центр `(0.30, 0.12)` | Поза связана с корпусом; устанавливаемый люк имеет начало в `(0.30, 0.12, 0.82594)` |
| Свободный люк, стандартная поза | `(0.35, −0.22, 0.790667)` | Поворот 45°; лежит отдельно на столе | | Свободный люк, стандартная поза | `(0.35, −0.22, 0.790667)` | Поворот 45°; лежит отдельно на столе |
| Стойка инструментов | `(0.12, −0.42, 0.793)` | Фиксированная стойка с отдельными инструментами в гнёздах | | Стойка инструментов | `(0.12, −0.42, 0.793)` | Фиксированная стойка с отдельными инструментами в гнёздах |
| Лоток винтов | `(−0.08, 0.34, 0.793)` | Длинная сторона вдоль мировой оси X; нижняя поверхность на столе | | Лоток винтов | `(0.20, 0.46, 0.793)` | Длинная сторона вдоль мировой оси X; нижняя поверхность на столе; у левого края стола, впереди слева от стойки |
Стойка и корпус присутствуют во всех сценах. Свободный люк, уже установленный люк, лоток, винты и рабочие инструменты появляются по таблице ниже. Установленный люк виртуально фиксируется после отпускания присоски. В опытах с крепежом он уже установлен и зафиксирован при старте. Стойка и корпус присутствуют во всех сценах. Свободный люк, уже установленный люк, лоток, винты и рабочие инструменты появляются по таблице ниже. Установленный люк виртуально фиксируется после отпускания присоски. В опытах с крепежом он уже установлен и зафиксирован при старте.
@@ -36,11 +36,11 @@
| Винт | `(X, Y, Z)` | Винт | `(X, Y, Z)` | | Винт | `(X, Y, Z)` | Винт | `(X, Y, Z)` |
| --- | --- | --- | --- | | --- | --- | --- | --- |
| `screw_1` | `(−0.120, 0.325, 0.7954)` | `screw_6` | `(−0.080, 0.325, 0.7954)` | | `screw_1` | `(0.160, 0.445, 0.7954)` | `screw_6` | `(0.200, 0.445, 0.7954)` |
| `screw_2` | `(−0.040, 0.325, 0.7954)` | `screw_7` | `(−0.060, 0.325, 0.7954)` | | `screw_2` | `(0.240, 0.445, 0.7954)` | `screw_7` | `(0.220, 0.445, 0.7954)` |
| `screw_3` | `(−0.120, 0.355, 0.7954)` | `screw_8` | `(−0.100, 0.355, 0.7954)` | | `screw_3` | `(0.160, 0.475, 0.7954)` | `screw_8` | `(0.180, 0.475, 0.7954)` |
| `screw_4` | `(−0.040, 0.355, 0.7954)` | `screw_9` | `(−0.080, 0.355, 0.7954)` | | `screw_4` | `(0.240, 0.475, 0.7954)` | `screw_9` | `(0.200, 0.475, 0.7954)` |
| `screw_5` | `(−0.100, 0.325, 0.7954)` | `screw_10` | `(−0.060, 0.355, 0.7954)` | | `screw_5` | `(0.180, 0.445, 0.7954)` | `screw_10` | `(0.220, 0.475, 0.7954)` |
Все десять винтов взаимозаменяемы. Для установки в четыре отверстия планировщик может выбрать **любые четыре разных доступных винта**. Оценка графа не должна требовать конкретные номера `screw_i`. В опыте 15 отказ задаётся третьей попытке захвата винта, который план выбрал третьим; отказ затяжки задаётся отверстию `hole_4`. Все десять винтов взаимозаменяемы. Для установки в четыре отверстия планировщик может выбрать **любые четыре разных доступных винта**. Оценка графа не должна требовать конкретные номера `screw_i`. В опыте 15 отказ задаётся третьей попытке захвата винта, который план выбрал третьим; отказ затяжки задаётся отверстию `hole_4`.
@@ -77,7 +77,7 @@
| 16 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Участок герметика, люк, один винт | | 16 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Участок герметика, люк, один винт |
| 17 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Полный контур, люк, два винта | | 17 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Полный контур, люк, два винта |
| 18 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Полная сборка | | 18 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Полная сборка |
| 19 | Стандартные | Свободный `(0.37, −0.20, 0.790667)`, **60°** | Лоток `(−0.10, 0.36, 0.793)`, **10 винтов** | Все четыре | Координаты всех десяти винтов смещены вместе с лотком на `(−0.02, +0.02)` | | 19 | Стандартные | Свободный `(0.37, −0.20, 0.790667)`, **60°** | Лоток `(0.18, 0.48, 0.793)`, **10 винтов** | Все четыре | Координаты всех десяти винтов смещены вместе с лотком на `(−0.02, +0.02)` |
| 20 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Исходная геометрия обычная; сбои вводятся во время выполнения | | 20 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Все четыре | Исходная геометрия обычная; сбои вводятся во время выполнения |
В опыте 3 смещается вся пара «стапель + корпус» без изменения взаимной высоты и поворота. Координаты `hatch_seat` и четырёх отверстий сдвигаются вместе с ними на `(−0.01, +0.02)`. В опыте 19 сдвигается лоток **со всеми десятью винтами**; ни один винт не остаётся в старой точке. В опыте 3 смещается вся пара «стапель + корпус» без изменения взаимной высоты и поворота. Координаты `hatch_seat` и четырёх отверстий сдвигаются вместе с ними на `(−0.01, +0.02)`. В опыте 19 сдвигается лоток **со всеми десятью винтами**; ни один винт не остаётся в старой точке.
@@ -86,6 +86,6 @@
## Геометрические и технические ограничения ## Геометрические и технические ограничения
По габаритам 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 мм. Это проверка начальной компоновки, не проверка траекторий инструмента. По габаритам 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.26`, `Y = 0.425…0.495`. Эти прямоугольники не пересекаются. Лоток перенесён к левому краю стола со стороны `+Y`, чтобы все винты были в достижимой зоне; зазор от лотка до стапеля по Y около 104 мм. Зазор от свободного люка до стойки по Y около 46 мм, до стапеля около 55 мм. Это проверка начальной компоновки, не проверка траекторий инструмента.
Перед реализацией контактных действий нужно проверить в Webots достижимость TCP и коллизии при подъёме люка, при извлечении инструмента из гнезда и при работе между десятью винтами. В `iiwa.wbt` трение `TableMaterial` равно нулю; лоток и винты понадобится удерживать до захвата или задать подходящую контактную модель. Действующий спавнер `object_spawner.py` создаёт случайные OBJ, поэтому для этой таблицы потребуется детерминированная загрузка по номеру эксперимента. Стойка должна содержать отдельно управляемые рабочие инструменты; удалённый `GrantToolRack.proto` для нового мира не используется. Перед реализацией контактных действий нужно проверить в Webots достижимость TCP и коллизии при подъёме люка, при извлечении инструмента из гнезда и при работе между десятью винтами. В `iiwa.wbt` трение `TableMaterial` равно нулю; лоток и винты понадобится удерживать до захвата или задать подходящую контактную модель. Действующий спавнер `object_spawner.py` создаёт случайные OBJ, поэтому для этой таблицы потребуется детерминированная загрузка по номеру эксперимента. Стойка должна содержать отдельно управляемые рабочие инструменты; удалённый `GrantToolRack.proto` для нового мира не используется.
src/iiwa_experement/test/test_scenario.py
+11 -11
View File
@@ -19,16 +19,16 @@ SCENES = PACKAGE / 'config' / 'scenes.yaml'
STANDARD_TOOLS = set(TOOL_NAMES) STANDARD_TOOLS = set(TOOL_NAMES)
STANDARD_SCREWS = { STANDARD_SCREWS = {
'screw_1': (-0.120, 0.325), 'screw_1': (0.160, 0.445),
'screw_2': (-0.040, 0.325), 'screw_2': (0.240, 0.445),
'screw_3': (-0.120, 0.355), 'screw_3': (0.160, 0.475),
'screw_4': (-0.040, 0.355), 'screw_4': (0.240, 0.475),
'screw_5': (-0.100, 0.325), 'screw_5': (0.180, 0.445),
'screw_6': (-0.080, 0.325), 'screw_6': (0.200, 0.445),
'screw_7': (-0.060, 0.325), 'screw_7': (0.220, 0.445),
'screw_8': (-0.100, 0.355), 'screw_8': (0.180, 0.475),
'screw_9': (-0.080, 0.355), 'screw_9': (0.200, 0.475),
'screw_10': (-0.060, 0.355), 'screw_10': (0.220, 0.475),
} }
@@ -144,7 +144,7 @@ def test_experiment_nineteen_moves_hatch_and_tray_with_all_screws():
assert spec.cap_pose.xy == (0.37, -0.20) assert spec.cap_pose.xy == (0.37, -0.20)
assert spec.cap_pose.yaw_deg == 60 assert spec.cap_pose.yaw_deg == 60
assert spec.tray_pose is not None assert spec.tray_pose is not None
assert (spec.tray_pose.x, spec.tray_pose.y, spec.tray_pose.z) == (-0.10, 0.36, 0.793) assert (spec.tray_pose.x, spec.tray_pose.y, spec.tray_pose.z) == (0.18, 0.48, 0.793)
for screw in spec.screws: for screw in spec.screws:
expected = STANDARD_SCREWS[screw.name] expected = STANDARD_SCREWS[screw.name]
assert screw.pose.xy == pytest.approx( assert screw.pose.xy == pytest.approx(
src/iiwa_experement/test/test_webots_nodes.py
+1 -1
View File
@@ -158,7 +158,7 @@ def test_experiment_nineteen_shifts_the_tray_and_all_ten_screws():
standard = by_name(nodes(11)) standard = by_name(nodes(11))
shifted = by_name(nodes(19)) shifted = by_name(nodes(19))
tray = translation(shifted['grant_tray']) tray = translation(shifted['grant_tray'])
assert tray == pytest.approx((-0.10, 0.36, 0.793)) assert tray == pytest.approx((0.18, 0.48, 0.793))
for index in range(1, 11): for index in range(1, 11):
name = f'grant_screw_{index:02d}' name = f'grant_screw_{index:02d}'
old = translation(standard[name]) old = translation(standard[name])