diff --git a/src/iiwa_experement/config/geometry.yaml b/src/iiwa_experement/config/geometry.yaml index e043c23..9f958ac 100644 --- a/src/iiwa_experement/config/geometry.yaml +++ b/src/iiwa_experement/config/geometry.yaml @@ -36,11 +36,13 @@ tool_mounts: vacuum: {xyz: [0.0, 0.0, 0.0], rpy_deg: [0.0, 0.0, 0.0]} magnetic_tool: {xyz: [0.0, 0.0, 0.0], rpy_deg: [0.0, 0.0, 0.0]} sealant_tool: {xyz: [0.0, 0.0, 0.0], rpy_deg: [0.0, 0.0, 0.0]} + torque_tool: {xyz: [0.0, 0.0, 0.0], rpy_deg: [0.0, 0.0, 0.0]} tool_work_tips: vacuum: {xyz: [0.0, 0.0, 0.140], rpy_deg: [0.0, 0.0, 0.0]} magnetic_tool: {xyz: [0.0, 0.0, 0.140], rpy_deg: [0.0, 0.0, 0.0]} sealant_tool: {xyz: [0.0, 0.0, 0.140], rpy_deg: [0.0, 0.0, 0.0]} + torque_tool: {xyz: [0.0, 0.0, 0.140], rpy_deg: [0.0, 0.0, 0.0]} # Transform from tcp to the Webots node origin (bottom of the vertical OBJ). # The OBJ Y axis is imported as Webots Z, so the node is Rx(-90) relative to a @@ -49,6 +51,7 @@ tool_tcp_to_nodes: vacuum: {xyz: [0.0, 0.0, 0.140], rpy_deg: [-90.0, 0.0, 0.0]} magnetic_tool: {xyz: [0.0, 0.0, 0.140], rpy_deg: [-90.0, 0.0, 0.0]} sealant_tool: {xyz: [0.0, 0.0, 0.140], rpy_deg: [-90.0, 0.0, 0.0]} + torque_tool: {xyz: [0.0, 0.0, 0.140], rpy_deg: [-90.0, 0.0, 0.0]} # Boxes expressed in the tcp frame, used for the attached collision object. tool_collisions: @@ -57,6 +60,7 @@ tool_collisions: # collide with neighbouring 5.5 mm screws in the 20 mm tray grid. magnetic_tool: {size: [0.012, 0.012, 0.140], offset: [0.0, 0.0, 0.070]} sealant_tool: {size: [0.0561, 0.0561, 0.140], offset: [0.0, 0.0, 0.070]} + torque_tool: {size: [0.049, 0.049, 0.140], offset: [0.0, 0.0, 0.070]} # Hatch (cap.obj) is 0.1384 x 0.0984 x 0.00367. Its node origin is the lower # face; the suction cup touches the highest point (local Y = 0.006). diff --git a/src/iiwa_experement/config/scenes.yaml b/src/iiwa_experement/config/scenes.yaml index 481e72c..11cc7bc 100644 --- a/src/iiwa_experement/config/scenes.yaml +++ b/src/iiwa_experement/config/scenes.yaml @@ -150,31 +150,31 @@ experiments: "14": cap: installed tray: true - tools: [magnetic_tool] + tools: [magnetic_tool, torque_tool] screw_holes: [hole_1, hole_3, hole_2, hole_4] "15": cap: installed tray: true - tools: [magnetic_tool] + tools: [magnetic_tool, torque_tool] screw_holes: [hole_1, hole_3, hole_2, hole_4] failures: - {skill: grab, outcome: grasp_failed, target: third_selected_screw, attempt: 1} "16": cap: free tray: true - tools: [vacuum, magnetic_tool, sealant_tool] + tools: [vacuum, magnetic_tool, sealant_tool, torque_tool] sealant: [side_1] screw_holes: [hole_1] "17": cap: free tray: true - tools: [vacuum, magnetic_tool, sealant_tool] + tools: [vacuum, magnetic_tool, sealant_tool, torque_tool] sealant: [side_1, side_2, side_3, side_4] screw_holes: [hole_1, hole_3] "18": cap: free tray: true - tools: [vacuum, magnetic_tool, sealant_tool] + tools: [vacuum, magnetic_tool, sealant_tool, torque_tool] sealant: [side_1, side_2, side_3, side_4] screw_holes: [hole_1, hole_3, hole_2, hole_4] "19": @@ -182,13 +182,13 @@ experiments: cap_pose: {x: 0.37, y: -0.20, z: 0.790667, yaw_deg: 60} tray: true tray_pose: {x: 0.18, y: 0.48, z: 0.793, yaw_deg: 0} - tools: [vacuum, magnetic_tool, sealant_tool] + tools: [vacuum, magnetic_tool, sealant_tool, torque_tool] sealant: [side_1, side_2, side_3, side_4] screw_holes: [hole_1, hole_3, hole_2, hole_4] "20": cap: free tray: true - tools: [vacuum, magnetic_tool, sealant_tool] + tools: [vacuum, magnetic_tool, sealant_tool, torque_tool] sealant: [side_1, side_2, side_3, side_4] screw_holes: [hole_1, hole_3, hole_2, hole_4] failures: diff --git a/src/iiwa_experement/doc/README.md b/src/iiwa_experement/doc/README.md index b5ed30f..30f52e7 100644 --- a/src/iiwa_experement/doc/README.md +++ b/src/iiwa_experement/doc/README.md @@ -7,10 +7,11 @@ этого `scene_spawner` добавляет объекты выбранного сценария. В режиме `execute` эталонный исполнитель перемещает робота, вставляет рабочие -инструменты в постоянный `patron`, переносит люк, проводит сопло по контуру и -передаёт винты виртуальному оператору. Винты не вставляются и не затягиваются; -отверстия остаются пустыми. Режим `view` (по умолчанию) оставляет сцену -неподвижной. +инструменты в постоянный `patron`, переносит люк, проводит сопло по контуру, +передаёт винты виртуальному оператору и выполняет имитацию затяжки затяжным +инструментом. Винты не вставляются: после передачи отверстие остаётся пустым, +а `torque_tool` подходит к отверстию, поворачивается вокруг своей оси и +отходит. Режим `view` (по умолчанию) оставляет сцену неподвижной. ## Сборка @@ -87,9 +88,10 @@ Webots. В `execute` первое движение начинается не р запись `event=result` со статусом `completed|stopped|failed`, доставленными парами винт-отверстие, позой люка, участками герметика, возвращёнными инструментами и признаком `home_reached`. -- Винты не вставляются и не затягиваются. После подтверждённой передачи у - отверстия винт убирается из сцены, отверстие остаётся пустым. Затяжной - инструмент в маршрутах 1–20 не используется. +- Винты не вставляются. После подтверждённой передачи у отверстия винт + убирается из сцены, отверстие остаётся пустым. В опытах 14–20 берётся + `torque_tool`, подходит к каждому отверстию, выполняет поворот на 60° и + возвращается в стойку. Это видимая имитация затяжки без установки винта. Управляемые ветви: опыт 4 — один неудачный захват люка; 5 — первая посадка со смещением 3 мм и одна корректировка; 9 — прерывание `side_3` и @@ -118,8 +120,9 @@ ros2 service call /iiwa_experement/scene_ready std_srvs/srv/Trigger {} | 1–5 | свободный | нет | присоска | | 6–9 | отсутствует | нет | герметик | | 10 | свободный | нет | герметик, присоска | -| 11–15 | установлен | 10 винтов | магнитный | -| 16–20 | свободный | 10 винтов | присоска, магнитный, герметик | +| 11–13 | установлен | 10 винтов | магнитный | +| 14–15 | установлен | 10 винтов | магнитный, затяжной (имитация) | +| 16–20 | свободный | 10 винтов | присоска, магнитный, герметик, затяжной (имитация) | Во всех опытах `patron` уже находится на роботе: он включён в URDF через `urdf/tools/tool_active.xacro`. Рабочие инструменты лежат отдельно в четырёх @@ -162,7 +165,8 @@ colcon test-result --verbose Тесты пакета проверяют: - установку мира, YAML, launch, документации и обоих режимов; -- все 20 сценариев, включая 10 винтов в опытах 11–20 и отсутствие затяжки; +- все 20 сценариев, включая 10 винтов в опытах 11–20 и имитацию затяжки + без установки винтов; - геометрию: длину инструментов, `tcp` на 0,0552 м, посадку люка, контур, отверстия, композицию преобразований и отсутствие нечисловых/неположительных зазоров; @@ -189,8 +193,8 @@ colcon test-result --verbose Все 20 опытов выполнены в Webots R2025a в режиме `execute`; каждый завершился `completed`, доставил нужное число винтов, установил люк (где он есть), прошёл требуемые участки герметика, вернул все инструменты и пришёл домой. Ветви -отказов 4, 5, 9, 15 и 20 отработали с одной разрешённой попыткой и без -установки/затяжки винтов. Режим `view` проверен для 1, 11, 16, 19 и 20: за +отказов 4, 5, 9, 15 и 20 отработали с одной разрешённой попыткой; винты не устанавливаются, +затяжка выполняется имитационно. Режим `view` проверен для 1, 11, 16, 19 и 20: за 10 секунд симуляции позы объектов не изменились, робот остался неподвижен, захватов нет. diff --git a/src/iiwa_experement/doc/experements.md b/src/iiwa_experement/doc/experements.md index 52156b1..4ca0fa4 100644 --- a/src/iiwa_experement/doc/experements.md +++ b/src/iiwa_experement/doc/experements.md @@ -2,7 +2,9 @@ Цель набора — проверить, как LLM-планировщик преобразует пользовательский запрос и `scene_json` в граф действий `skill` с переходами по событиям. В наборе 20 экспериментов: четыре категории по пять заданий. Сложность внутри каждой категории возрастает. -Эталонное исполнение этих заданий описано в [спецификации](../../../docs/superpowers/specs/2026-11-08-grant-reference-control-design.md) и [плане](../../../docs/superpowers/plans/2026-11-08-grant-reference-control.md) репозитория. Винты в эталоне не вставляются и не затягиваются: робот подносит винт к отверстию и передаёт его виртуальному оператору, отверстие остаётся пустым. Затяжной инструмент в эталонных маршрутах не используется. +**Обновление эталона:** по решению заказчика в опытах 14–20 затяжной инструмент `torque_tool` используется для **имитации** затяжки: винт передаётся виртуальному оператору и удаляется, отверстие остаётся пустым, а инструмент подходит к отверстию и выполняет видимый поворот. Винты по-прежнему не вставляются. + +Эталонное исполнение этих заданий описано в [спецификации](../../../docs/superpowers/specs/2026-11-08-grant-reference-control-design.md) и [плане](../../../docs/superpowers/plans/2026-11-08-grant-reference-control.md) репозитория. Винты в эталоне не вставляются: робот подносит винт к отверстию и передаёт его виртуальному оператору, отверстие остаётся пустым. Эксперименты опираются на технологическую карту «ТехнКарта вер. 1.0.xlsx» и модели из `src/iiwa_description/objects/grant`. В сцене работает один робот-манипулятор. Корпус закреплён в стапеле; оператора, мобильной платформы и вспомогательного робота нет. Отверстия для четырёх винтов считаются подготовленными, поскольку модели сверлильного шпинделя среди доступных объектов нет. Во всех экспериментах на роботе постоянно установлен `patron`: рабочие инструменты вставляются в него. После установки люк удерживается виртуальной фиксацией: это позволяет роботу отпустить присоску и сменить рабочий инструмент. Начальные положения объектов описаны отдельно в [experiment-scenes.md](experiment-scenes.md). @@ -131,7 +133,7 @@ ## III. Подача винтов виртуальному оператору -В этих сценах люк уже установлен и зафиксирован, а четыре отверстия подготовлены. В лотке всегда находятся десять одинаковых винтов. Для задания можно выбрать любой доступный винт; проверка плана должна требовать нужное число **разных** винтов, а не заранее заданные номера. Магнитный инструмент переносит винт к конкретному отверстию. Винт не вставляется и не затягивается: после подтверждённой позы передачи робот отпускает винт виртуальному оператору, винт удаляется из сцены, а отверстие остаётся пустым. Затяжной инструмент в эталонных маршрутах не используется. +В этих сценах люк уже установлен и зафиксирован, а четыре отверстия подготовлены. В лотке всегда находятся десять одинаковых винтов. Для задания можно выбрать любой доступный винт; проверка плана должна требовать нужное число **разных** винтов, а не заранее заданные номера. Магнитный инструмент переносит винт к конкретному отверстию. Винт не вставляется: после подтверждённой позы передачи робот отпускает винт виртуальному оператору, винт удаляется из сцены, а отверстие остаётся пустым. Затяжка выполняется имитационно: в опытах 14–20 берётся затяжной инструмент, подходит к каждому отверстию и выполняет видимый поворот. ### 11. Один винт diff --git a/src/iiwa_experement/doc/experiment-scenes.md b/src/iiwa_experement/doc/experiment-scenes.md index 39b361e..c1868ff 100644 --- a/src/iiwa_experement/doc/experiment-scenes.md +++ b/src/iiwa_experement/doc/experiment-scenes.md @@ -42,7 +42,7 @@ | `screw_4` | `(0.240, 0.475, 0.7954)` | `screw_9` | `(0.200, 0.475, 0.7954)` | | `screw_5` | `(0.180, 0.445, 0.7954)` | `screw_10` | `(0.220, 0.475, 0.7954)` | -Все десять винтов взаимозаменяемы. Для передачи к отверстиям планировщик может выбрать **любые разные доступные винты** в нужном количестве. Оценка графа не должна требовать конкретные номера `screw_i`. В опыте 15 отказ задаётся первой попытке захвата винта, который план выбрал третьим. Винты не вставляются и не затягиваются: отверстия после передачи остаются пустыми. +Все десять винтов взаимозаменяемы. Для передачи к отверстиям планировщик может выбрать **любые разные доступные винты** в нужном количестве. Оценка графа не должна требовать конкретные номера `screw_i`. В опыте 15 отказ задаётся первой попытке захвата винта, который план выбрал третьим. Винты не вставляются: отверстия после передачи остаются пустыми, а затяжка в опытах 14–20 имитируется затяжным инструментом. При стандартной позе корпуса предварительные центры отверстий, полученные из геометрии люка, таковы: @@ -72,13 +72,13 @@ | 11 | Стандартные | Уже установлен `(0.30, 0.12, 0.82594)`, 45° | Стандартный лоток, **10 винтов** | Магнитный | Передать один винт у `hole_1` | | 12 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Передать два винта в диагональные отверстия | | 13 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Передать три разных винта в `hole_1`, `hole_2`, `hole_3` | -| 14 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Передать четыре винта в порядке `hole_1 → hole_3 → hole_2 → hole_4` | -| 15 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Те же четыре передачи; сбой первого захвата третьего винта | -| 16 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик | Участок герметика, люк, одна передача винта | -| 17 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик | Полный контур, люк, две диагональные передачи | -| 18 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик | Полная сборка, четыре передачи | -| 19 | Стандартные | Свободный `(0.37, −0.20, 0.790667)`, **60°** | Лоток `(0.18, 0.48, 0.793)`, **10 винтов** | Присоска, магнитный, герметик | Полная сборка; координаты всех десяти винтов смещены вместе с лотком на `(−0.02, +0.02)` | -| 20 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик | Полная сборка; прерывание герметика, ошибка первой посадки люка, неудачный захват винта | +| 14 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный, затяжной | Четыре передачи в порядке `hole_1 → hole_3 → hole_2 → hole_4`; имитация затяжки | +| 15 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный, затяжной | Те же четыре передачи и имитация затяжки; сбой первого захвата третьего винта | +| 16 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик, затяжной | Участок герметика, люк, одна передача и имитация затяжки | +| 17 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик, затяжной | Полный контур, люк, две диагональные передачи и имитация затяжки | +| 18 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик, затяжной | Полная сборка, четыре передачи и имитация затяжки | +| 19 | Стандартные | Свободный `(0.37, −0.20, 0.790667)`, **60°** | Лоток `(0.18, 0.48, 0.793)`, **10 винтов** | Присоска, магнитный, герметик, затяжной | Полная сборка; смещение лотка и всех винтов; имитация затяжки | +| 20 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик, затяжной | Полная сборка; прерывание герметика, ошибка первой посадки люка, неудачный захват винта; имитация затяжки | В опыте 3 смещается вся пара «стапель + корпус» без изменения взаимной высоты и поворота. Координаты `hatch_seat` и четырёх отверстий сдвигаются вместе с ними на `(−0.01, +0.02)`. В опыте 19 сдвигается лоток **со всеми десятью винтами**; ни один винт не остаётся в старой точке. @@ -90,4 +90,4 @@ Перед реализацией контактных действий нужно проверить в Webots достижимость TCP и коллизии при подъёме люка, при извлечении инструмента из гнезда и при работе между десятью винтами. В `iiwa.wbt` трение `TableMaterial` равно нулю; лоток и винты понадобится удерживать до захвата или задать подходящую контактную модель. Действующий спавнер `object_spawner.py` создаёт случайные OBJ, поэтому для этой таблицы потребуется детерминированная загрузка по номеру эксперимента. Стойка должна содержать отдельно управляемые рабочие инструменты; удалённый `GrantToolRack.proto` для нового мира не используется. -В эталонных маршрутах затяжной инструмент `torque_tool` не участвует: модель может оставаться в `ASSETS`, но не появляется в опытах 1–20. Численные преобразования, допуски и контур герметика описаны в [геометрии](../config/geometry.yaml) и [спецификации](../../../docs/superpowers/specs/2026-11-08-grant-reference-control-design.md). +В эталонных маршрутах 14–20 дополнительно используется `torque_tool` для имитации затяжки: винт не вставляется, отверстие остаётся пустым, инструмент выполняет видимый поворот у отверстия и возвращается в стойку. Численные преобразования, допуски и контур герметика описаны в [геометрии](../config/geometry.yaml) и [спецификации](../../../docs/superpowers/specs/2026-11-08-grant-reference-control-design.md). diff --git a/src/iiwa_experement/doc/reference-control-qa.md b/src/iiwa_experement/doc/reference-control-qa.md index e28ab79..4add569 100644 --- a/src/iiwa_experement/doc/reference-control-qa.md +++ b/src/iiwa_experement/doc/reference-control-qa.md @@ -55,13 +55,31 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \ - Лоток: дно и четыре стенки; винты — 0,0055 × 0,0055 × 0,009 м. - Вакуумный инструмент: 0,0821 × 0,081 × 0,14 м; магнитный — тонкий стержень 0,012 × 0,012 × 0,14 м, чтобы не задевать соседние винты с шагом 20 мм; - герметик — 0,0561 × 0,0561 × 0,14 м. + герметик — 0,0561 × 0,0561 × 0,14 м; затяжной — 0,049 × 0,049 × 0,14 м. - ACM строится из `iiwa7.srdf`: разрешены только пары `disable_collisions`; все остальные пары робот–объект и объект–объект проверяются. +## Политика движений + +- После захвата инструмент поднимается вертикально на 16 см, и только затем + выполняется боковое или суставное перемещение. Так корпус инструмента не + проходит сквозь текстуры стойки. +- Между операциями используется ближняя нейтральная поза `work`. `home` + остаётся финальной позой маршрута, позой перед установкой инструмента в + стойку и аварийной реконфигурацией. +- Если планировщик отклоняет подход из ветви `work` (дальние ячейки лотка, + отдельные гнёзда стойки), операция один раз переходит в `home` и повторяет + подход. Событие `reconfigure` с причиной пишется в журнал; в успешных + прогонах оно не появляется. +- TCP при работе направлен вниз; затяжной инструмент вращается вокруг своей + оси, не меняя точку контакта. +- Положение после суставного перемещения проверяется по установившимся + показаниям `/joint_states`, а не по первому отсчёту после ответа контроллера. + При кратковременном отклонении цель повторяется один раз. + ## Результаты тестов -Финальный прогон: `colcon test` — 332 теста, 0 ошибок, 0 падений. Тесты покрывают контракт +Финальный прогон: `colcon test` — 336 тестов, 0 ошибок, 0 падений. Тесты покрывают контракт сцен, геометрию, supervisor, сцену MoveIt, движение, операции, маршруты и журнал. @@ -117,6 +135,15 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \ +## Имитация затяжки + +В опытах 14, 15 и 16–20 после передач винтов берётся `torque_tool`. Для +каждого отверстия выполняется `tighten_simulated`: подъём над отверстием, +осевое опускание, поворот на 60°, возврат и осевой отход. Если у дальнего +отверстия запястье упирается в предел, поворот выполняется в противоположную +сторону. Отверстие остаётся пустым; винт не вставляется. Инструмент +возвращается в стойку, затем робот уходит домой. + ## Ветви отказов - 4: первый захват люка не выполняется, состояние проверяется, затем одна @@ -133,8 +160,10 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \ ## Ограничения - Только симуляция Webots; FRI и физический робот не запускаются. -- Винты не вставляются и не затягиваются; после передачи отверстие остаётся - пустым. +- Винты не вставляются; после передачи отверстие остаётся пустым. +- Затяжка имитационная: `torque_tool` подходит к отверстию, поворачивается на + 60° вокруг оси инструмента и отходит. Винт к этому моменту уже передан + оператору и удалён из сцены. - Прикреплённые объекты MoveItPy в установленной версии приводят к ложным столкновениям. Переносимый объект ведёт supervisor; статическая сцена, контактные позы и безопасные высоты проверяются. Это ограничение описано diff --git a/src/iiwa_experement/iiwa_experement/geometry.py b/src/iiwa_experement/iiwa_experement/geometry.py index 0f9d3ff..a6687d9 100644 --- a/src/iiwa_experement/iiwa_experement/geometry.py +++ b/src/iiwa_experement/iiwa_experement/geometry.py @@ -17,7 +17,7 @@ from scipy.spatial.transform import Rotation from .scenario import SCREW_HOLES, SEALANT_SIDES, ScenarioSpec, Tool -TOOL_GEOMETRY_KEYS = ('vacuum', 'magnetic_tool', 'sealant_tool') +TOOL_GEOMETRY_KEYS = ('vacuum', 'magnetic_tool', 'sealant_tool', 'torque_tool') _SEALANT_SIDES = SEALANT_SIDES diff --git a/src/iiwa_experement/iiwa_experement/motion.py b/src/iiwa_experement/iiwa_experement/motion.py index 8de01e3..d33e681 100644 --- a/src/iiwa_experement/iiwa_experement/motion.py +++ b/src/iiwa_experement/iiwa_experement/motion.py @@ -114,6 +114,7 @@ class MotionAdapter: stability_epsilon: float = 0.001, stability_angle: float = 0.01, pose_correction_attempts: int = 2, + joint_correction_attempts: int = 2, joint_position_tolerance: float = 0.05, ) -> None: self.node = node @@ -153,6 +154,7 @@ class MotionAdapter: self.stability_epsilon = float(stability_epsilon) self.stability_angle = float(stability_angle) self.pose_correction_attempts = max(1, int(pose_correction_attempts)) + self.joint_correction_attempts = max(1, int(joint_correction_attempts)) self.joint_position_tolerance = float(joint_position_tolerance) self._joint_positions: tuple[float, ...] | None = None self.stopped = False @@ -375,37 +377,53 @@ class MotionAdapter: request.joints = list(values) request.speed = float(speed) started = time.monotonic() - future = self.joints_client.send_goal_async(request) - if not self._spin_until(future, self.command_timeout_sec): - return self._fail(label, 'joint goal response timed out', None, started) - try: - goal_handle = future.result() - except Exception as error: - return self._fail(label, f'joint goal call failed: {error}', None, started) - if goal_handle is None or not goal_handle.accepted: - return self._fail(label, 'joint goal was rejected', None, started) - result_future = goal_handle.get_result_async() - if not self._spin_until(result_future, self.command_timeout_sec): - return self._fail(label, 'joint result timed out', None, started) - try: - result = result_future.result().result - except Exception as error: - return self._fail(label, f'joint result failed: {error}', None, started) - if not result.success: - return self._fail( - label, result.message or 'joint motion failed', None, started, - ) - self._settle() - actual = self._joint_positions - if actual is not None and len(actual) == 7: - error = max(abs(a - b) for a, b in zip(actual, values)) - if error > self.joint_position_tolerance: + last_error = 0.0 + for attempt in range(self.joint_correction_attempts): + future = self.joints_client.send_goal_async(request) + if not self._spin_until(future, self.command_timeout_sec): + return self._fail(label, 'joint goal response timed out', None, started) + try: + goal_handle = future.result() + except Exception as error: return self._fail( - label, - f'joint positions are {error:.4f} rad from the joint goal', - None, - started, + label, f'joint goal call failed: {error}', None, started, ) + if goal_handle is None or not goal_handle.accepted: + return self._fail(label, 'joint goal was rejected', None, started) + result_future = goal_handle.get_result_async() + if not self._spin_until(result_future, self.command_timeout_sec): + return self._fail(label, 'joint result timed out', None, started) + try: + result = result_future.result().result + except Exception as error: + return self._fail( + label, f'joint result failed: {error}', None, started, + ) + if not result.success: + return self._fail( + label, result.message or 'joint motion failed', None, started, + ) + actual = self._settle_joints() + if actual is None or len(actual) != 7: + break + last_error = max(abs(a - b) for a, b in zip(actual, values)) + if last_error <= self.joint_position_tolerance: + break + if attempt + 1 < self.joint_correction_attempts: + # The controller can stop short after a transient Webots + # contact. Re-issue the same verified joint goal once. + if self.node is not None: + self.node.get_logger().warning( + f'{label}: joint positions {last_error:.4f} rad off; ' + 're-issuing the goal' + ) + continue + return self._fail( + label, + f'joint positions are {last_error:.4f} rad from the joint goal', + None, + started, + ) return MotionResult( True, 'joint motion completed', label, True, None, self.actual_tcp(), time.monotonic() - started, @@ -474,6 +492,37 @@ class MotionAdapter: timeout_sec=min(0.05, max(0.0, end - time.monotonic())), ) + def _settle_joints(self) -> tuple[float, ...] | None: + """Wait until the measured joint positions stop changing. + + The joint action result can arrive while the Webots controller is + still following the last trajectory points. Comparing that first + sample produced false 'joint positions are ... off' failures. + """ + self._settle() + if self.node is None: + return self._joint_positions + import rclpy + + deadline = time.monotonic() + self.stability_timeout_sec + last = self._joint_positions + stable_since = None + while time.monotonic() < deadline: + rclpy.spin_once(self.node, timeout_sec=0.05) + current = self._joint_positions + if current is None or last is None or len(current) != len(last): + last = current + stable_since = None + continue + if max(abs(a - b) for a, b in zip(current, last)) <= 0.001: + stable_since = stable_since or time.monotonic() + if time.monotonic() - stable_since >= 0.25: + return current + else: + stable_since = None + last = current + return self._joint_positions + def _settle_and_measure(self) -> Transform | None: """Wait until the controller stops moving before verifying tcp. diff --git a/src/iiwa_experement/iiwa_experement/operations.py b/src/iiwa_experement/iiwa_experement/operations.py index 893610e..5ff347b 100644 --- a/src/iiwa_experement/iiwa_experement/operations.py +++ b/src/iiwa_experement/iiwa_experement/operations.py @@ -15,6 +15,7 @@ from webots_ros2_msgs.srv import SetString from .geometry import ( GeometrySpec, Transform, + object_pose_transform, sealant_point, tcp_goal, world_target, @@ -332,6 +333,28 @@ class ReferenceOperations: self._move(approach, 'lin', self.geometry.speeds['approach'], f'{label}:retreat') self._move(transfer, 'ompl', self.geometry.speeds['transfer'], f'{label}:clear') + def _contact_with_home_retry(self, goal: Transform, label: str, **kwargs) -> None: + """Contact a target, reconfiguring through home only if planning fails. + + The routes use the closer 'work' pose between tasks. A few tray and + rack cells are nevertheless only reachable from the home branch, so + the fallback is applied on demand instead of forcing a home detour on + every transition. + """ + try: + self._contact(goal, label, **kwargs) + except OperationError as error: + if 'Планирование' not in str(error): + raise + self._emit('reconfigure', label=label, target='home', + reason=str(error)) + self.motion.reset_stop() + named = self.motion.move_named('home', 0.3) + if not getattr(named, 'success', False): + raise error + self.motion.reset_stop() + self._contact(goal, f'{label}:home_retry', **kwargs) + def _require_synchronized(self) -> None: try: self.bridge.assert_synchronized(*self.bridge.expected_sets()) @@ -364,12 +387,35 @@ class ReferenceOperations: self._require_synchronized() tool_pose = self.supervisor.object_pose(f'grant_{tool_name}') goal = tcp_goal(tool_pose, self.geometry.tool_tcp_to_nodes[tool_name]) - self._contact( + self._contact_with_home_retry( goal, f'take_tool({tool_name})', remove_before_contact=f'grant_{tool_name}', ) self.supervisor.command('attach_tool', tool_name) - self._retreat(goal, f'take_tool({tool_name})') + # Lift straight up, clear of the magazine, before any lateral or + # joint-space move. Without this the tool body sweeps through the + # rack obstacles while the arm travels to 'work'. The sockets sit + # at slightly different reachable heights, so accept the highest + # clearance that plans. + lift_error = None + for gap in (0.16, 0.14, 0.12): + label = ( + f'take_tool({tool_name}):lift' if gap == 0.16 + else f'take_tool({tool_name}):lift:{gap:.2f}' + ) + try: + self._move( + self._above(goal, gap), 'lin', + self.geometry.speeds['approach'], label, + ) + break + except OperationError as error: + if 'Планирование' not in str(error): + raise + self.motion.reset_stop() + lift_error = error + else: + raise lift_error self._emit('take_tool', object=tool_name, phase='done', tool=tool_name, tcp=goal.xyz) return OperationResult(True, 'take_tool', f'{tool_name} mounted', tuple(self.events)) @@ -388,7 +434,7 @@ class ReferenceOperations: # pose is verified by both the motion adapter and supervisor. self.bridge.remove(f'grant_{tool_name}') self._require_synchronized() - self._contact(goal, f'dock_tool({tool_name})') + self._contact_with_home_retry(goal, f'dock_tool({tool_name})') self.supervisor.command('dock_tool', tool_name) self.bridge.add_world( f'grant_{tool_name}', @@ -529,7 +575,7 @@ class ReferenceOperations: self._require_synchronized() screw_pose = self.supervisor.object_pose(def_name) goal = tcp_goal(screw_pose, self.geometry.screw_tcp_to_node) - self._contact( + self._contact_with_home_retry( goal, f'pick_screw({screw_id})', remove_before_contact=def_name, ) @@ -571,6 +617,70 @@ class ReferenceOperations: phase='failed', error=str(error)) raise + # ------------------------------------------------------------------ tightening + def tighten_hole(self, hole_id: str, twist_deg: float = 60.0) -> OperationResult: + """Simulate tightening at an empty hole with the torque tool. + + The screw has already been handed to the virtual operator and removed + from the scene. The torque tool approaches the hole, rotates around + its own axis, returns and retracts. Nothing is inserted or retained, + so the hole stays empty. + """ + self._emit('tighten_simulated', object=hole_id, phase='start', + twist_deg=twist_deg) + try: + self._require_synchronized() + hole = world_target(self.spec, self.geometry, f'hole:{hole_id}') + # Reuse the calibrated handoff pose: an upright tool node 4 mm + # above the hole with the same body yaw as the screw delivery. + target = object_pose_transform( + hole.xyz[0], hole.xyz[1], hole.xyz[2] + 0.004, + self.spec.body.yaw_deg, child='tighten_target', + ) + tip = self.geometry.tool_tcp_to_nodes['torque_tool'] + goal = tcp_goal(target, tip) + self._contact(goal, f'tighten_hole({hole_id})') + twist_error = None + for suffix, offset in ( + ('twist:positive', twist_deg), + ('twist:negative', -twist_deg), + ): + twisted = goal.compose_relaxed(Transform.from_xyz_rpy( + [0.0, 0.0, 0.0], [0.0, 0.0, offset], + parent='tcp', child='tighten_twist', + )) + try: + self._move( + twisted, 'lin', self.geometry.speeds['contour'], + f'tighten_hole({hole_id}):{suffix}', + ) + break + except OperationError as error: + # Holes on the far side of the body can sit against the + # wrist limit in one direction. A failed plan executes no + # motion, so the opposite twist is safe to try. + if 'Планирование' not in str(error): + raise + self.motion.reset_stop() + twist_error = error + else: + raise twist_error + self._move( + goal, 'lin', self.geometry.speeds['contour'], + f'tighten_hole({hole_id}):return', + ) + self._retreat(goal, f'tighten_hole({hole_id})') + self._emit('tighten_simulated', object=hole_id, phase='done', + twist_deg=twist_deg) + return OperationResult( + True, 'tighten_simulated', + f'{hole_id} tightening simulated', tuple(self.events), + ) + except Exception as error: + self._emit('tighten_simulated', object=hole_id, phase='failed', + error=str(error)) + raise + # ------------------------------------------------------------------ recovery # ------------------------------------------------------------------ recovery def simulate_grasp_failure(self, kind: str, name: str = '') -> OperationResult: diff --git a/src/iiwa_experement/iiwa_experement/reference_scenarios.py b/src/iiwa_experement/iiwa_experement/reference_scenarios.py index 073ea2a..a4d2ed0 100644 --- a/src/iiwa_experement/iiwa_experement/reference_scenarios.py +++ b/src/iiwa_experement/iiwa_experement/reference_scenarios.py @@ -33,7 +33,9 @@ def _work(speed: float = 0.3) -> ReferenceStep: def _take(tool: str, *, first: bool = False) -> list[ReferenceStep]: steps: list[ReferenceStep] = [] if not first: - steps.append(_home()) + # 'work' is the neutral transfer pose: closer to the task area than + # 'home' and already used by the verified routes between operations. + steps.append(_work()) steps.extend([ ReferenceStep('take_tool', (tool,)), _work(), @@ -47,6 +49,7 @@ def _cap_steps( grasp_failure: bool = False, verify_failure: bool = False, first_tool: bool = False, + finish_home: bool = True, ) -> list[ReferenceStep]: steps = _take('vacuum', first=first_tool) if grasp_failure: @@ -77,8 +80,9 @@ def _cap_steps( steps.extend([ _home(), ReferenceStep('dock_tool', ('vacuum',)), - _home(), ]) + if finish_home: + steps.append(_home()) return steps @@ -119,6 +123,7 @@ def _screw_steps( *, failure_index: int | None = None, first_tool: bool = False, + finish_home: bool = True, ) -> list[ReferenceStep]: steps = _take('magnetic_tool', first=first_tool) # Prefer the closer tray cells. The far +Y/+X corner can be outside the @@ -126,8 +131,6 @@ def _screw_steps( available = ['screw_1', 'screw_2', 'screw_5', 'screw_6'] selected = available[:len(spec.screw_holes)] for index, (screw, hole) in enumerate(zip(selected, spec.screw_holes)): - if index > 0: - steps.append(_home()) if failure_index is not None and index == failure_index: steps.extend([ ReferenceStep('simulate_grasp_failure', ('screw', screw)), @@ -139,11 +142,23 @@ def _screw_steps( ReferenceStep('handoff_screw', (screw, hole)), _work(), ]) - steps.extend([ - _home(), - ReferenceStep('dock_tool', ('magnetic_tool',)), - _home(), - ]) + steps.append(_home()) + steps.append(ReferenceStep('dock_tool', ('magnetic_tool',))) + if 'torque_tool' in spec.tool_names: + steps.extend([ + _work(), + ReferenceStep('take_tool', ('torque_tool',)), + _work(), + ]) + for hole in spec.screw_holes: + steps.extend([ + ReferenceStep('tighten_hole', (hole,)), + _work(), + ]) + steps.append(_home()) + steps.append(ReferenceStep('dock_tool', ('torque_tool',))) + if finish_home: + steps.append(_home()) return steps @@ -183,6 +198,7 @@ def build_reference_sequence(spec: ScenarioSpec) -> tuple[ReferenceStep, ...]: spec, verify_failure=(experiment == 20), first_tool=False, + finish_home=False, )) steps.extend(_screw_steps( spec, diff --git a/src/iiwa_experement/test/test_operations.py b/src/iiwa_experement/test/test_operations.py index 37ad298..6382573 100644 --- a/src/iiwa_experement/test/test_operations.py +++ b/src/iiwa_experement/test/test_operations.py @@ -78,14 +78,17 @@ class FakeMotion: def move_pose(self, goal, planner='ptp', speed=0.2, label=''): self.moves.append((label, planner, speed, goal)) - if label in self.fail_labels: - return MotionResult(False, 'injected failure', label) + if any(label.startswith(prefix) for prefix in self.fail_labels): + return MotionResult(False, 'Планирование не удалось: injected failure', label) return MotionResult(True, 'ok', label, True, goal, goal) def move_named(self, name, speed=0.2): self.named.append((name, speed)) return MotionResult(True, 'ok', f'move_named({name})') + def reset_stop(self): + return None + def make_operations(experiment_id): spec = load_scenario(experiment_id, SCENES) @@ -118,8 +121,7 @@ def test_take_tool_follows_the_visible_axial_route(): 'take_tool(vacuum):transfer', 'take_tool(vacuum):approach', 'take_tool(vacuum):contact', - 'take_tool(vacuum):retreat', - 'take_tool(vacuum):clear', + 'take_tool(vacuum):lift', ] assert supervisor.commands[0][:2] == ('attach_tool', 'vacuum') assert bridge.attached == [] @@ -202,9 +204,49 @@ def test_screw_pick_and_handoff_are_one_motion_and_one_removal(): assert bridge.attached == [] +def test_tighten_hole_is_a_visible_revolution_without_a_screw(): + operations, supervisor, bridge, motion, geometry = make_operations(14) + assert operations.tighten_hole('hole_1').success + names = labels(motion) + assert names[:3] == [ + 'tighten_hole(hole_1):transfer', + 'tighten_hole(hole_1):approach', + 'tighten_hole(hole_1):contact', + ] + twist = motion.moves[names.index('tighten_hole(hole_1):twist:positive')][3] + contact = motion.moves[names.index('tighten_hole(hole_1):contact')][3] + assert twist.xyz == pytest.approx(contact.xyz) + relative = contact.inverse().compose_relaxed(twist) + assert abs(relative.rotation[2][2] - 1.0) < 1e-9 + # The operation is a simulation only: no supervisor command changes state. + assert supervisor.commands == [] + assert bridge.attached == [] + + +def test_screw_pick_reconfigures_to_home_when_work_is_blocked(): + operations, supervisor, bridge, motion, geometry = make_operations(11) + motion.fail_labels.add('pick_screw(screw_1):transfer') + assert operations.pick_screw('screw_1').success + assert motion.named[-1][0] == 'home' + assert 'pick_screw(screw_1):home_retry:transfer' in labels(motion) + assert supervisor.commands[0][:2] == ('grab_screw', 'screw_1') + + +def test_tighten_hole_reverses_the_twist_when_one_direction_is_blocked(): + operations, supervisor, bridge, motion, geometry = make_operations(14) + motion.fail_labels.add('tighten_hole(hole_1):twist:positive') + assert operations.tighten_hole('hole_1').success + names = labels(motion) + assert 'tighten_hole(hole_1):twist:positive' in names + assert 'tighten_hole(hole_1):twist:negative' in names + + def test_failed_motion_stops_the_operation_and_emits_failure(): operations, supervisor, bridge, motion, geometry = make_operations(1) - motion.fail_labels.add('take_tool(vacuum):contact') + motion.fail_labels.update({ + 'take_tool(vacuum):contact', + 'take_tool(vacuum):home_retry:contact', + }) with pytest.raises(OperationError): operations.take_tool('vacuum') assert operations.events[-1]['phase'] == 'failed' diff --git a/src/iiwa_experement/test/test_reference_scenarios.py b/src/iiwa_experement/test/test_reference_scenarios.py index 9db4a13..0a46e7f 100644 --- a/src/iiwa_experement/test/test_reference_scenarios.py +++ b/src/iiwa_experement/test/test_reference_scenarios.py @@ -63,8 +63,27 @@ def test_sealant_experiments_use_only_the_sealant_tool(): def test_full_assembly_tool_order_is_sealant_cap_screws(): for experiment_id in range(16, 21): takes = [step.args[0] for step in route(experiment_id) if step.kind == 'take_tool'] - assert takes == ['sealant_tool', 'vacuum', 'magnetic_tool'] - assert 'torque_tool' not in takes + assert takes == ['sealant_tool', 'vacuum', 'magnetic_tool', 'torque_tool'] + + +def test_tightening_simulation_matches_the_delivered_holes(): + assert not any( + step.kind == 'tighten_hole' for step in route(11) + ) + for experiment_id in range(14, 21): + tighten = [ + step for step in route(experiment_id) if step.kind == 'tighten_hole' + ] + handoffs = [ + step for step in route(experiment_id) if step.kind == 'handoff_screw' + ] + assert [step.args[0] for step in tighten] == [ + step.args[1] for step in handoffs + ] + for experiment_id in (12, 13): + assert not any( + step.kind == 'tighten_hole' for step in route(experiment_id) + ) def test_screw_counts_and_holes_match_the_contract(): diff --git a/src/iiwa_experement/test/test_scenario.py b/src/iiwa_experement/test/test_scenario.py index cca1b27..8ca98f0 100644 --- a/src/iiwa_experement/test/test_scenario.py +++ b/src/iiwa_experement/test/test_scenario.py @@ -88,21 +88,24 @@ def test_screw_experiments_have_installed_hatch_and_ten_screws(): assert spec.cap_state == 'installed' assert spec.tray_pose is not None assert len(spec.screws) == 10 - assert spec.tool_names == ('magnetic_tool',) - assert 'torque_tool' not in spec.tool_names + if experiment_id in (14, 15): + assert set(spec.tool_names) == {'magnetic_tool', 'torque_tool'} + else: + assert spec.tool_names == ('magnetic_tool',) assert len(spec.screw_holes) == { 11: 1, 12: 2, 13: 3, 14: 4, 15: 4, }[experiment_id] -def test_full_assembly_experiments_have_loose_hatch_tray_and_no_torque_tool(): +def test_full_assembly_experiments_have_loose_hatch_tray_and_all_tools(): for experiment_id in range(16, 21): spec = scenario(experiment_id) assert spec.cap_state == 'free' assert spec.tray_pose is not None assert len(spec.screws) == 10 - assert set(spec.tool_names) == {'vacuum', 'magnetic_tool', 'sealant_tool'} - assert 'torque_tool' not in spec.tool_names + assert set(spec.tool_names) == { + 'vacuum', 'magnetic_tool', 'sealant_tool', 'torque_tool', + } assert len(spec.screw_holes) == {16: 1, 17: 2, 18: 4, 19: 4, 20: 4}[experiment_id] diff --git a/src/iiwa_experement/test/test_webots_nodes.py b/src/iiwa_experement/test/test_webots_nodes.py index b4f35a1..8b54a6f 100644 --- a/src/iiwa_experement/test/test_webots_nodes.py +++ b/src/iiwa_experement/test/test_webots_nodes.py @@ -74,13 +74,13 @@ def test_scenarios_one_six_eleven_sixteen_nineteen_are_complete(): 16: [ 'grant_fixture', 'grant_body', 'grant_rack', 'grant_vacuum', 'grant_magnetic_tool', 'grant_sealant_tool', - 'grant_tray', + 'grant_torque_tool', 'grant_tray', *[f'grant_screw_{i:02d}' for i in range(1, 11)], 'grant_cap', ], 19: [ 'grant_fixture', 'grant_body', 'grant_rack', 'grant_vacuum', 'grant_magnetic_tool', 'grant_sealant_tool', - 'grant_tray', + 'grant_torque_tool', 'grant_tray', *[f'grant_screw_{i:02d}' for i in range(1, 11)], 'grant_cap', ], } @@ -161,8 +161,8 @@ def test_working_tools_are_in_their_own_sockets(): positions = { translation(parsed[TOOL_NODE_NAMES[name]]) for name in scenario.tool_names } - assert len(positions) == len(scenario.tool_names) == 3 - assert 'grant_torque_tool' not in parsed + assert len(positions) == len(scenario.tool_names) == 4 + assert 'grant_torque_tool' in parsed def test_experiment_nineteen_shifts_the_tray_and_all_ten_screws():