feat(experement): simulate tightening and smooth tool transfers

Add a torque-tool tightening simulation for experiments 14-20: the tool approaches each empty hole, twists 60 degrees (reversing at the wrist limit), returns and retracts while the screw stays handed off. Lift attached tools vertically clear of the magazine before lateral motion, prefer the close 'work' pose between operations, and reconfigure through home only when planning fails. Settle measured joint positions before verifying joint moves.

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).

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:

src/iiwa_experement/doc/README.md

@@ -7,10 +7,11 @@
 этого `scene_spawner` добавляет объекты выбранного сценария.
 
 В режиме `execute` эталонный исполнитель перемещает робота, вставляет рабочие
-инструменты в постоянный `patron`, переносит люк, проводит сопло по контуру и
+инструменты в постоянный `patron`, переносит люк, проводит сопло по контуру,
-передаёт винты виртуальному оператору. Винты не вставляются и не затягиваются;
+передаёт винты виртуальному оператору и выполняет имитацию затяжки затяжным
-отверстия остаются пустыми. Режим `view` (по умолчанию) оставляет сцену
+инструментом. Винты не вставляются: после передачи отверстие остаётся пустым,
-неподвижной.
+а `torque_tool` подходит к отверстию, поворачивается вокруг своей оси и
+отходит. Режим `view` (по умолчанию) оставляет сцену неподвижной.
 
 ## Сборка
 
@@ -87,9 +88,10 @@ Webots. В `execute` первое движение начинается не р
   запись `event=result` со статусом `completed|stopped|failed`, доставленными
   парами винт-отверстие, позой люка, участками герметика, возвращёнными
   инструментами и признаком `home_reached`.
-- Винты не вставляются и не затягиваются. После подтверждённой передачи у
+- Винты не вставляются. После подтверждённой передачи у отверстия винт
-  отверстия винт убирается из сцены, отверстие остаётся пустым. Затяжной
+  убирается из сцены, отверстие остаётся пустым. В опытах 14–20 берётся
-  инструмент в маршрутах 1–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 винтов | магнитный |
+| 11–13 | установлен | 10 винтов | магнитный |
-| 16–20 | свободный | 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 отработали с одной разрешённой попыткой и без
+отказов 4, 5, 9, 15 и 20 отработали с одной разрешённой попыткой; винты не устанавливаются,
-установки/затяжки винтов. Режим `view` проверен для 1, 11, 16, 19 и 20: за
+затяжка выполняется имитационно. Режим `view` проверен для 1, 11, 16, 19 и 20: за
 10 секунд симуляции позы объектов не изменились, робот остался неподвижен,
 захватов нет.
 

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. Один винт
 

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` |
+| 14 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный, затяжной | Четыре передачи в порядке `hole_1 → hole_3 → hole_2 → hole_4`; имитация затяжки |
-| 15 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный | Те же четыре передачи; сбой первого захвата третьего винта |
+| 15 | Стандартные | Уже установлен | Стандартный лоток, **10 винтов** | Магнитный, затяжной | Те же четыре передачи и имитация затяжки; сбой первого захвата третьего винта |
-| 16 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик | Участок герметика, люк, одна передача винта |
+| 16 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик, затяжной | Участок герметика, люк, одна передача и имитация затяжки |
-| 17 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик | Полный контур, люк, две диагональные передачи |
+| 17 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик, затяжной | Полный контур, люк, две диагональные передачи и имитация затяжки |
-| 18 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик | Полная сборка, четыре передачи |
+| 18 | Стандартные | Стандартный свободный | Стандартный лоток, **10 винтов** | Присоска, магнитный, герметик, затяжной | Полная сборка, четыре передачи и имитация затяжки |
-| 19 | Стандартные | Свободный `(0.37, −0.20, 0.790667)`, **60°** | Лоток `(0.18, 0.48, 0.793)`, **10 винтов** | Присоска, магнитный, герметик | Полная сборка; координаты всех десяти винтов смещены вместе с лотком на `(−0.02, +0.02)` |
+| 19 | Стандартные | Свободный `(0.37, −0.20, 0.790667)`, **60°** | Лоток `(0.18, 0.48, 0.793)`, **10 винтов** | Присоска, магнитный, герметик, затяжной | Полная сборка; смещение лотка и всех винтов; имитация затяжки |
-| 20 | Стандартные | Стандартный свободный | Стандартный лоток, **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).

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; статическая сцена,
   контактные позы и безопасные высоты проверяются. Это ограничение описано

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
 
 

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)
+        last_error = 0.0
-        if not self._spin_until(future, self.command_timeout_sec):
+        for attempt in range(self.joint_correction_attempts):
-            return self._fail(label, 'joint goal response timed out', None, started)
+            future = self.joints_client.send_goal_async(request)
-        try:
+            if not self._spin_until(future, self.command_timeout_sec):
-            goal_handle = future.result()
+                return self._fail(label, 'joint goal response timed out', None, started)
-        except Exception as error:
+            try:
-            return self._fail(label, f'joint goal call failed: {error}', None, started)
+                goal_handle = future.result()
-        if goal_handle is None or not goal_handle.accepted:
+            except Exception as error:
-            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:
                 return self._fail(
-                    label,
+                    label, f'joint goal call failed: {error}', None, started,
-                    f'joint positions are {error:.4f} rad from the joint goal',
-                    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.
 

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:

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([
+    steps.append(_home())
-        _home(),
+    steps.append(ReferenceStep('dock_tool', ('magnetic_tool',)))
-        ReferenceStep('dock_tool', ('magnetic_tool',)),
+    if 'torque_tool' in spec.tool_names:
-        _home(),
+        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,

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:
+        if any(label.startswith(prefix) for prefix in self.fail_labels):
-            return MotionResult(False, 'injected failure', label)
+            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):lift',
-        'take_tool(vacuum):clear',
     ]
     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'

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 takes == ['sealant_tool', 'vacuum', 'magnetic_tool', 'torque_tool']
-        assert 'torque_tool' not in takes
+
+
+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():

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',)
+        if experiment_id in (14, 15):
-        assert 'torque_tool' not in spec.tool_names
+            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 set(spec.tool_names) == {
-        assert 'torque_tool' not in 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]
 
 

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 len(positions) == len(scenario.tool_names) == 4
-    assert 'grant_torque_tool' not in parsed
+    assert 'grant_torque_tool' in parsed
 
 
 def test_experiment_nineteen_shifts_the_tray_and_all_ten_screws():