.

cobot-setting.yaml

@@ -9,7 +9,7 @@ robot:
 
 digital_twin:
   tcp_gizmo:
-    enabled: true              # TCP-гизма в Webots; только в режиме симуляции
+    enabled: false              # TCP-гизма в Webots; только в режиме симуляции
     publish_period: 0.032      # период управления [с]
     command_timeout: 0.3       # максимальный возраст цели при отпускании мыши [с]
     state_timeout: 0.5         # допустимый возраст состояния робота [с]

src/iiwa_experement/config/geometry.yaml

@@ -126,10 +126,13 @@ speeds:
 
 # Calibrated safe Cartesian tcp poses (world frame).  They are used as an
 # OMPL fallback when a short PTP named motion is rejected.
+# Only the two poses the reference executor is allowed to use are mirrored
+# here: 'work' is the neutral working pose, 'home' is the final pose of a
+# route.  The 'transport' pose from the global MoveIt config is deliberately
+# not listed: it is forbidden in the reference routes.
 named_joints:
   home: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
   work: [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0]
-  transport: [0.0, 0.436, 0.0, 1.57, 0.0, 0.0, 0.0]
 
 safe_poses:
   work:

src/iiwa_experement/doc/reference-control-qa.md

@@ -61,19 +61,28 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
 
 ## Политика движений
 
-- После захвата инструмент поднимается на 8 см, затем смещается на 10 см по
+- После захвата инструмент поднимается строго вертикально (8 см, при отказе
-  +Y от стойки, и только потом выполняется суставное перемещение. Полный
+  планировщика — 6/3/1,5 см), затем смещается по +Y от стойки с достигнутой
-  вертикальный подъём на 14–16 см упирается в предел запястья; боковой выход
+  высоты, и только потом выполняется суставное перемещение. Подъём
-  даёт тот же зазор и не ломает достижимость `work`.
+  обязателен: если все осевые участки отклонены, операция останавливается,
-- Между операциями используется ближняя нейтральная поза `work`. `home`
+  а не продолжается боковым движением на глубине контакта.
-  остаётся финальной позой маршрута, позой перед установкой инструмента в
+- Маршрут начинается с перехода в `work` и заканчивается `work` → `home`.
-  стойку и аварийной реконфигурацией.
+  `home` используется только как финальная поза. Между операциями робот
-- Если планировщик отклоняет подход из ветви `work` (дальние ячейки лотка,
+  планирует прямо из текущей рабочей позы; повторные `work` не вставляются.
-  отдельные гнёзда стойки), операция один раз переходит в `home` и повторяет
+- Если планировщик отклоняет подход из текущей ветви (дальние ячейки лотка,
-  подход. Событие `reconfigure` с причиной пишется в журнал; в успешных
+  отдельные гнёзда стойки), операция один раз переходит в `work` и повторяет
-  прогонах оно не появляется.
+  подход. Событие `reconfigure` с причиной пишется в журнал. Поза
+  `transport` в эталонных маршрутах запрещена и не используется нигде,
+  включая аварийные ветви.
+- После каждого контакта (передача винта, имитация затяжки, захват и посадка
+  люка, контур герметика) выполняется обязательный осевой подъём до
+  горизонтального перемещения. Захваченные инструмент, люк и винт
+  прикрепляются к роботу в сцене MoveIt (`bridge.attach`), поэтому перенос
+  планируется с учётом переносимого тела.
 - TCP при работе направлен вниз; затяжной инструмент вращается вокруг своей
-  оси, не меняя точку контакта.
+  оси, не меняя точку контакта. После поворота фланец возвращается в
+  исходную ориентацию (untwist), чтобы сустав 7 не накапливал закрутку
+  между отверстиями.
 - Положение после суставного перемещения проверяется по установившимся
   показаниям `/joint_states`, а не по первому отсчёту после ответа контроллера.
   При кратковременном отклонении цель повторяется один раз.
@@ -86,6 +95,11 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
 
 ## Результаты 20 опытов
 
+> Таблица ниже относится к предыдущей редакции маршрутов. После правок
+> (запрет `transport`, обязательный осевой подъём после каждого контакта,
+> untwist фланца, прикрепление переносимых объектов в сцене MoveIt) все 20
+> опытов требуют повторной верификации в Webots в режиме `execute`.
+
 Каждый сценарий должен хотя бы один раз завершиться со статусом `completed`.
 При транзиентном отказе планировщика (`Планирование не удалось`) запуск
 повторялся с чистой сценой; таблица показывает подтверждённый успешный прогон.
@@ -148,9 +162,10 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
 осевое опускание и поворот последнего сустава на 45°. Вращается только
 фланец, поэтому инструмент поворачивается вокруг своей оси, не уходя в другую
 ветвь IK. Если положительное направление упирается в предел, используется
-отрицательное. После поворота робот уходит в `work`, как после передачи
+отрицательное. После поворота фланец возвращается в исходную конфигурацию
-винта. Отверстие остаётся пустым; винт не вставляется. Инструмент
+(untwist), затем выполняется обязательный осевой подъём из отверстия, как
-возвращается в стойку, затем робот уходит домой.
+после передачи винта. Отверстие остаётся пустым; винт не вставляется.
+Инструмент возвращается в стойку, затем робот уходит домой.
 
 ## Ветви отказов
 
@@ -172,10 +187,11 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
 - Затяжка имитационная: `torque_tool` подходит к отверстию, поворачивается на
   45° вокруг оси инструмента и отходит. Винт к этому моменту уже передан
   оператору и удалён из сцены.
-- Прикреплённые объекты MoveItPy в установленной версии приводят к ложным
+- Прикреплённые объекты MoveIt моделируются: захваченные инструмент, люк и
-  столкновениям. Переносимый объект ведёт supervisor; статическая сцена,
+  винт переносятся из мировой сцены в `attached_collision_objects` с
-  контактные позы и безопасные высоты проверяются. Это ограничение описано
+  разрешённым контактом звеньев `tcp`, `patron`, `link7` и держателя камеры.
-  в README.
+  Если в установленной версии MoveItPy снова появятся ложные касания,
+  первым делом проверяются touch-звенья и габариты боксов в `geometry.yaml`.
 - MoveIt/MoveItPy может аварийно завершиться после очень длинной серии
   планирований; один запуск выполняет один опыт, поэтому серия ограничена.
 - `Node.remove()` для винта в Webots R2025a иногда приводит к падению

src/iiwa_experement/iiwa_experement/motion.py

@@ -395,21 +395,10 @@ class MotionAdapter:
             # the named service once as the fallback path.
             self.reset_stop()
         result = self._move_named_service(name, speed, label)
-        if result.success or name != 'work':
+        # 'home' and 'work' are the only named poses the reference executor
-            return result
+        # may use; the forbidden 'transport' pose is never a fallback.  A
-        # The tool rack can leave the arm in a branch from which the close
+        # rejected 'work' already tried its calibrated safe Cartesian pose
-        # 'work' pose is unreachable.  Fall back to the globally safe home
+        # inside the service path, so a failure here is reported as-is.
-        # configuration rather than aborting the route.
-        home = self.geometry.named_joints.get('home')
-        if home is not None:
-            self.reset_stop()
-            fallback = self.move_joints(home, speed, f'{label}:home')
-            if fallback.success:
-                if self.node is not None:
-                    self.node.get_logger().warning(
-                        f'{label}: work unreachable; reconfigured to home'
-                    )
-                return fallback
         return result
 
     def move_joints(

src/iiwa_experement/iiwa_experement/operations.py

@@ -331,36 +331,106 @@ class ReferenceOperations:
         self._move(goal, 'lin', self.geometry.speeds['contact'], f'{label}:contact')
 
     def _retreat(self, goal: Transform, label: str) -> None:
-        approach = self._above(goal, self.geometry.approach_gap)
+        """Lift axially away from a contact pose before any lateral motion.
-        transfer = self._above(goal, self.geometry.transfer_gap)
-        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(
+        On real hardware a sideways move straight out of a contact pose drags
+        the tool or the part across the fixture, so the lift is mandatory:
+        the gaps only shrink, and if every axial segment is rejected the
+        operation stops instead of continuing at contact depth.
+        """
+        lifted = False
+        for index, gap in enumerate((self.geometry.approach_gap, 0.03, 0.015)):
+            lift_label = f'{label}:retreat' if index == 0 else f'{label}:retreat:{gap:.3f}'
+            try:
+                self._move(
+                    self._above(goal, gap), 'lin',
+                    self.geometry.speeds['approach'], lift_label,
+                )
+                lifted = True
+                break
+            except OperationError as error:
+                if 'Планирование' not in str(error):
+                    raise
+                self.motion.reset_stop()
+        if not lifted:
+            raise OperationError(f'{label}: the axial retreat was rejected')
+        self._move(
+            self._above(goal, self.geometry.transfer_gap), 'ompl',
+            self.geometry.speeds['transfer'], f'{label}:clear',
+        )
+
+    def _exit_rack(self, goal: Transform, label: str) -> None:
+        """Lift straight up out of the rack, then step sideways out of it.
+
+        The lift is mandatory: a lateral move at socket depth would drag the
+        tool (or the empty patron) through the magazine on real hardware.
+        The sideways step starts from the height that was actually reached.
+        """
+        height = None
+        for index, gap in enumerate((
+            self.geometry.transfer_gap, self.geometry.approach_gap, 0.03, 0.015,
+        )):
+            lift_label = f'{label}:lift' if index == 0 else f'{label}:lift:{gap:.3f}'
+            try:
+                self._move(
+                    self._above(goal, gap), 'lin',
+                    self.geometry.speeds['approach'], lift_label,
+                )
+                height = gap
+                break
+            except OperationError as error:
+                if 'Планирование' not in str(error):
+                    raise
+                self.motion.reset_stop()
+        if height is None:
+            raise OperationError(
+                f'{label}: the axial lift out of the rack was rejected'
+            )
+        base = self._above(goal, height)
+        for offset in (0.10, 0.06, 0.03):
+            exit_pose = Transform.from_xyz_rpy(
+                [0.0, offset, 0.0], [0.0, 0.0, 0.0],
+                parent='world', child='rack_exit_offset',
+            ).compose_relaxed(base)
+            exit_label = (
+                f'{label}:exit' if offset == 0.10 else f'{label}:exit:{offset:.2f}'
+            )
+            try:
+                self._move(
+                    exit_pose, 'lin', self.geometry.speeds['transfer'], exit_label,
+                )
+                return
+            except OperationError as error:
+                if 'Планирование' not in str(error):
+                    raise
+                self.motion.reset_stop()
+        raise OperationError(f'{label}: no lateral rack exit was plannable')
+
+    def _contact_with_recovery(
         self, goal: Transform, label: str, *, prepare=None, **kwargs,
     ) -> None:
-        """Contact a target, reconfiguring through home only if planning fails.
+        """Contact a target, reconfiguring through 'work' if planning fails.
 
-        The routes use the closer 'work' pose between tasks.  A few tray and
+        'work' is the only neutral pose the routes are allowed to use:
-        rack cells are nevertheless only reachable from the home branch, so
+        'transport' is forbidden entirely and 'home' is reserved for the end
-        the fallback is applied on demand instead of forcing a home detour on
+        of the route.  A single retry from 'work' switches the IK branch
-        every transition.
+        without leaving the working area.
         """
         try:
             self._contact(goal, label, **kwargs)
         except OperationError as error:
             if 'Планирование' not in str(error):
                 raise
-            self._emit('reconfigure', label=label, target='home',
+            self._emit('reconfigure', label=label, target='work',
                        reason=str(error))
             if prepare is not None:
                 prepare()
             self.motion.reset_stop()
-            named = self.motion.move_named('home', 0.3)
+            named = self.motion.move_named('work', 0.3)
             if not getattr(named, 'success', False):
                 raise error
             self.motion.reset_stop()
-            self._contact(goal, f'{label}:home_retry', **kwargs)
+            self._contact(goal, f'{label}:recovery', **kwargs)
 
     def _require_synchronized(self) -> None:
         try:
@@ -394,49 +464,17 @@ 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_with_home_retry(
+            self._contact_with_recovery(
                 goal, f'take_tool({tool_name})',
                 remove_before_contact=f'grant_{tool_name}',
             )
             self.supervisor.command('attach_tool', tool_name)
-            # Lift a little, then translate along +Y away from the magazine
+            # The held tool becomes part of the robot in the planning scene,
-            # before any large joint-space move.  A full-height vertical lift
+            # so the lift, the rack exit and every later transfer are planned
-            # runs into the wrist limit; the lateral exit is what actually
+            # with the tool body instead of dragging it through the magazine.
-            # keeps the tool body out of the rack textures.
+            self.bridge.attach(attached_tool(tool_name, self.geometry))
-            transfer = self._above(goal, self.geometry.transfer_gap)
+            self._require_synchronized()
-            try:
+            self._exit_rack(goal, f'take_tool({tool_name})')
-                self._move(
-                    transfer, 'lin', self.geometry.speeds['approach'],
-                    f'take_tool({tool_name}):lift',
-                )
-            except OperationError as error:
-                # The tool is attached.  Continue with the lateral exit (or
-                # let the route's 'work' move recover) rather than aborting
-                # with the tool in the rack.
-                if 'Планирование' not in str(error):
-                    raise
-                self._emit('lift_skipped', label=f'take_tool({tool_name})',
-                           reason=str(error))
-                self.motion.reset_stop()
-            for offset in (0.10, 0.06, 0.03):
-                exit_pose = Transform.from_xyz_rpy(
-                    [0.0, offset, 0.0], [0.0, 0.0, 0.0],
-                    parent='world', child='rack_exit_offset',
-                ).compose_relaxed(transfer)
-                label = (
-                    f'take_tool({tool_name}):exit' if offset == 0.10
-                    else f'take_tool({tool_name}):exit:{offset:.2f}'
-                )
-                try:
-                    self._move(
-                        exit_pose, 'lin', self.geometry.speeds['transfer'],
-                        label,
-                    )
-                    break
-                except OperationError as error:
-                    if 'Планирование' not in str(error):
-                        raise
-                    self.motion.reset_stop()
             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))
@@ -455,7 +493,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_with_home_retry(goal, f'dock_tool({tool_name})')
+            self._contact_with_recovery(goal, f'dock_tool({tool_name})')
             self.supervisor.command('dock_tool', tool_name)
             self.bridge.add_world(
                 f'grant_{tool_name}',
@@ -463,44 +501,7 @@ class ReferenceOperations:
                 rack_pose,
             )
             self._require_synchronized()
-            try:
+            self._exit_rack(goal, f'dock_tool({tool_name})')
-                self._retreat(goal, f'dock_tool({tool_name})')
-            except OperationError as error:
-                # The axial retreat can be rejected from the socket branch.
-                # The tool is already docked, so stepping out is still safe.
-                if 'Планирование' not in str(error):
-                    raise
-                self.motion.reset_stop()
-            # Step out of the rack along +Y before any joint-space move; the
-            # axial retreat alone can leave the wrist against its limit.  Use a
-            # lower height if the full retreat was rejected.
-            for gap in (
-                self.geometry.transfer_gap, 0.05, 0.02,
-            ):
-                base = self._above(goal, gap)
-                reached = False
-                for offset in (0.10, 0.06, 0.03):
-                    exit_pose = Transform.from_xyz_rpy(
-                        [0.0, offset, 0.0], [0.0, 0.0, 0.0],
-                        parent='world', child='rack_exit_offset',
-                    ).compose_relaxed(base)
-                    label = (
-                        f'dock_tool({tool_name}):exit' if offset == 0.10
-                        else f'dock_tool({tool_name}):exit:{offset:.2f}'
-                    )
-                    try:
-                        self._move(
-                            exit_pose, 'lin',
-                            self.geometry.speeds['transfer'], label,
-                        )
-                        reached = True
-                        break
-                    except OperationError as error:
-                        if 'Планирование' not in str(error):
-                            raise
-                        self.motion.reset_stop()
-                if reached:
-                    break
             self._emit('dock_tool', object=tool_name, phase='done')
             return OperationResult(True, 'dock_tool', f'{tool_name} docked', tuple(self.events))
         except Exception as error:
@@ -526,6 +527,10 @@ class ReferenceOperations:
                 goal, 'pick_cap', remove_before_contact='grant_cap',
             )
             self.supervisor.command('grab_cap', 'cap')
+            # Plan the retreat and the transfer with the hatch attached to the
+            # robot, not as a blind spot removed from the scene.
+            self.bridge.attach(attached_cap(self.geometry))
+            self._require_synchronized()
             self._retreat(goal, 'pick_cap')
             self._emit('pick_cap', object='cap', phase='done', tcp=goal.xyz)
             return OperationResult(True, 'pick_cap', 'hatch grasped', tuple(self.events))
@@ -607,9 +612,8 @@ class ReferenceOperations:
             first = sealant_point(self.geometry, self.spec, segments[0], 0.0)
             first_goal = tcp_goal(first, tip)
             self._contact(first_goal, 'dispense_contour:start')
-            for index, segment in enumerate(segments):
+            for segment in segments:
-                end_fraction = 1.0 if index < len(segments) - 1 else 1.0
+                end = sealant_point(self.geometry, self.spec, segment, 1.0)
-                end = sealant_point(self.geometry, self.spec, segment, end_fraction)
                 end_goal = tcp_goal(end, tip)
                 self._move(
                     end_goal, 'lin', self.geometry.speeds['contour'],
@@ -641,24 +645,18 @@ class ReferenceOperations:
                 )
                 self._require_synchronized()
 
-            self._contact_with_home_retry(
+            self._contact_with_recovery(
                 goal, f'pick_screw({screw_id})',
                 remove_before_contact=def_name,
                 prepare=restore_screw,
             )
             self.supervisor.command('grab_screw', screw_id)
-            try:
+            self.bridge.attach(attached_screw(screw_id, self.geometry))
+            self._require_synchronized()
+            # The axial lift is mandatory even at far tray cells: continuing
+            # laterally with the screw at tray depth would drag it across the
+            # tray walls on real hardware.
             self._retreat(goal, f'pick_screw({screw_id})')
-            except OperationError as error:
-                # The screw is already attached.  A rejected axial lift at a
-                # far tray cell can be recovered by the route's next 'work'
-                # move instead of aborting with the screw in hand.
-                if 'Планирование' not in str(error):
-                    raise
-                self._emit('retreat_skipped',
-                           label=f'pick_screw({screw_id})',
-                           reason=str(error))
-                self.motion.reset_stop()
             self._emit('pick_screw', object=screw_id, phase='done')
             return OperationResult(True, 'pick_screw', f'{screw_id} grasped', tuple(self.events))
         except Exception as error:
@@ -672,7 +670,7 @@ class ReferenceOperations:
             self._require_synchronized()
             hole = world_target(self.spec, self.geometry, f'hole:{hole_id}')
             goal = tcp_goal(hole, self.geometry.screw_tcp_to_node)
-            self._contact(
+            self._contact_with_recovery(
                 goal, f'handoff_screw({screw_id},{hole_id})',
                 remove_before_contact=def_name,
                 remove_phase='contact',
@@ -681,9 +679,10 @@ class ReferenceOperations:
                 'handoff_screw', screw_id, target=hole, target_name=hole_id,
             )
             self._require_synchronized()
-            # A Cartesian retreat from the hole can leave the wrist in a
+            # Always lift axially out of the handoff pose before the route
-            # singular branch.  The caller moves to 'work' in joint space
+            # continues: a joint-space or lateral move at hole height would
-            # immediately afterwards; that is the verified clearance move.
+            # drag the magnet across the body on real hardware.
+            self._retreat(goal, f'handoff_screw({screw_id},{hole_id})')
             self._emit('delivered', object=screw_id, target=hole_id,
                        phase='done', hole=hole_id)
             return OperationResult(
@@ -717,10 +716,10 @@ class ReferenceOperations:
             )
             tip = self.geometry.tool_tcp_to_nodes['torque_tool']
             goal = tcp_goal(target, tip)
-            self._contact(goal, f'tighten_hole({hole_id})')
+            self._contact_with_recovery(goal, f'tighten_hole({hole_id})')
             twist_error = None
-            twisted = goal
             twist_done = False
+            joint_twist = False
             # Prefer a pure flange rotation: it twists the tool about its own
             # axis without moving the arm to another IK branch.  A Cartesian
             # twist can push joint 6 into its limit, after which 'work' is no
@@ -730,6 +729,7 @@ class ReferenceOperations:
             if provider is not None:
                 joints = provider()
             if joints is not None and len(joints) == 7:
+                joints = tuple(float(value) for value in joints)
                 for sign in (1.0, -1.0):
                     target_joints = list(joints)
                     target_joints[6] += math.radians(sign * twist_deg)
@@ -740,6 +740,7 @@ class ReferenceOperations:
                     )
                     if getattr(result, 'success', False):
                         twist_done = True
+                        joint_twist = True
                         self._emit('tighten_twist', object=hole_id,
                                    joint=7, delta_deg=sign * twist_deg)
                         break
@@ -761,7 +762,6 @@ class ReferenceOperations:
                             candidate, 'lin', self.geometry.speeds['contour'],
                             f'tighten_hole({hole_id}):{suffix}',
                         )
-                        twisted = candidate
                         twist_done = True
                         break
                     except OperationError as error:
@@ -774,9 +774,29 @@ class ReferenceOperations:
                         twist_error = error
                 if not twist_done:
                     raise twist_error
-            # The caller's following 'work' move is the verified clearance
+            # Return the flange to the contact orientation before leaving.
-            # move, as after a screw handoff.  A separate Cartesian retreat
+            # Without the untwist, joint 7 accumulates 45 deg per hole and the
-            # from the twisted pose can be rejected by the wrist limits.
+            # wrist winds up until every later plan starts from a twisted
+            # branch.
+            if joint_twist:
+                self.motion.wait_stationary()
+                untwist = self.motion.move_joints(
+                    joints, self.geometry.speeds['contour'],
+                    f'tighten_hole({hole_id}):joint_untwist',
+                )
+                if not getattr(untwist, 'success', False):
+                    raise OperationError(
+                        f'tighten_hole({hole_id}): the joint untwist failed: '
+                        f'{untwist.message}'
+                    )
+            else:
+                self._move(
+                    goal, 'lin', self.geometry.speeds['contour'],
+                    f'tighten_hole({hole_id}):untwist',
+                )
+            # Lift axially out of the hole before the route continues, exactly
+            # as after a screw handoff.
+            self._retreat(goal, f'tighten_hole({hole_id})')
             self._emit('tighten_simulated', object=hole_id, phase='done',
                        twist_deg=twist_deg)
             return OperationResult(

src/iiwa_experement/iiwa_experement/planning_scene.py

@@ -40,8 +40,11 @@ SCENE_TOPIC = '/planning_scene'
 MONITORED_TOPIC = '/monitored_planning_scene'
 GET_SCENE_SERVICE = '/get_planning_scene'
 SCENE_FRAME = 'base_link'
+# The tool is rigidly bolted to the flange, so contact between its collision
+# box and the wrist/flange links is expected and allowed.  Without 'link7'
+# the wide vacuum box reads as a false collision against the wrist housing.
 TOUCH_LINKS = (
-    'tcp', 'patron', 'camera_holder', 'camera_corner',
+    'tcp', 'patron', 'link7', 'camera_holder', 'camera_corner',
     'camera_hand', 'camera_optical_frame',
 )
 ROBOT_LINKS = (

src/iiwa_experement/iiwa_experement/reference_scenarios.py

@@ -31,15 +31,13 @@ def _work(speed: float = 0.3) -> ReferenceStep:
 
 
 def _take(tool: str, *, first: bool = False) -> list[ReferenceStep]:
+    # The route starts at 'work'; later tool changes reconfigure there first
+    # so the approach starts from a known branch.  The task itself follows
+    # directly from the tool exit.
     steps: list[ReferenceStep] = []
     if not first:
-        # '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([
+    steps.append(ReferenceStep('take_tool', (tool,)))
-        ReferenceStep('take_tool', (tool,)),
-        _work(),
-    ])
     return steps
 
 
@@ -49,7 +47,6 @@ 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:
@@ -57,7 +54,7 @@ def _cap_steps(
             ReferenceStep('simulate_grasp_failure', ('cap',)),
             ReferenceStep('inspect_state'),
         ])
-    steps.extend([ReferenceStep('pick_cap'), _work()])
+    steps.append(ReferenceStep('pick_cap'))
     if verify_failure:
         shift = 0.003
         for failure in spec.failures:
@@ -68,7 +65,6 @@ def _cap_steps(
             ReferenceStep('place_cap_shifted', (), {'dx': shift}),
             ReferenceStep('inspect_cap', (), {'expect': 'not_ok', 'tolerance': 0.002}),
             ReferenceStep('pick_cap'),
-            _work(),
             ReferenceStep('place_cap'),
             ReferenceStep('inspect_cap', (), {'expect': 'ok'}),
         ])
@@ -77,13 +73,7 @@ def _cap_steps(
             ReferenceStep('place_cap'),
             ReferenceStep('inspect_cap', (), {'expect': 'ok'}),
         ])
-    steps.extend([
+    steps.append(ReferenceStep('dock_tool', ('vacuum',)))
-        _home(),
-        ReferenceStep('dock_tool', ('vacuum',)),
-    ])
-    # Always leave the rack through home: the dock retreat can sit against a
-    # wrist limit from which 'work' is not reachable.
-    steps.append(_home())
     return steps
 
 
@@ -92,7 +82,6 @@ def _sealant_steps(
     *,
     interrupted: bool = False,
     first_tool: bool = True,
-    finish_home: bool = True,
 ) -> list[ReferenceStep]:
     sides = list(spec.sealant_sides)
     steps = _take('sealant_tool', first=first_tool)
@@ -111,10 +100,8 @@ def _sealant_steps(
         steps.append(ReferenceStep('dispense_contour', (tuple(sides),)))
     steps.extend([
         ReferenceStep('inspect_sealant', (), {'expect': 'ok'}),
-        _home(),
         ReferenceStep('dock_tool', ('sealant_tool',)),
     ])
-    steps.append(_home())
     return steps
 
 
@@ -123,7 +110,6 @@ 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
@@ -140,31 +126,23 @@ def _screw_steps(
             ReferenceStep('pick_screw', (screw,)),
             _work(),
             ReferenceStep('handoff_screw', (screw, hole)),
-            _work(),
         ])
-    steps.append(_home())
+    steps.append(_work())
     steps.append(ReferenceStep('dock_tool', ('magnetic_tool',)))
-    steps.append(_home())
     if 'torque_tool' in spec.tool_names:
-        steps.extend([
+        steps.append(_work())
-            ReferenceStep('take_tool', ('torque_tool',)),
+        steps.append(ReferenceStep('take_tool', ('torque_tool',)))
-            _work(),
-        ])
         for hole in spec.screw_holes:
-            steps.extend([
+            steps.append(ReferenceStep('tighten_hole', (hole,)))
-                ReferenceStep('tighten_hole', (hole,)),
+        steps.append(_work())
-                _work(),
-            ])
-        steps.append(_home())
         steps.append(ReferenceStep('dock_tool', ('torque_tool',)))
-        steps.append(_home())
     return steps
 
 
 def build_reference_sequence(spec: ScenarioSpec) -> tuple[ReferenceStep, ...]:
     """Return the deterministic route for one experiment."""
     experiment = spec.experiment_id
-    steps: list[ReferenceStep] = []
+    steps: list[ReferenceStep] = [_work()]
 
     if experiment in (1, 2, 3, 4, 5):
         steps.extend(_cap_steps(
@@ -179,7 +157,7 @@ def build_reference_sequence(spec: ScenarioSpec) -> tuple[ReferenceStep, ...]:
         ))
     elif experiment == 10:
         steps.extend(_sealant_steps(
-            spec, interrupted=False, first_tool=True, finish_home=False,
+            spec, interrupted=False, first_tool=True,
         ))
         steps.extend(_cap_steps(spec, first_tool=False))
     elif 11 <= experiment <= 15:
@@ -191,20 +169,35 @@ def build_reference_sequence(spec: ScenarioSpec) -> tuple[ReferenceStep, ...]:
     else:
         interrupted = experiment == 20
         steps.extend(_sealant_steps(
-            spec, interrupted=interrupted, first_tool=True, finish_home=False,
+            spec, interrupted=interrupted, first_tool=True,
         ))
         steps.extend(_cap_steps(
             spec,
             verify_failure=(experiment == 20),
             first_tool=False,
-            finish_home=False,
         ))
         steps.extend(_screw_steps(
             spec,
             failure_index=0 if experiment == 20 else None,
             first_tool=False,
         ))
-    return tuple(steps)
+
+    # The technical process always ends at 'work'; only the very last move is
+    # the final return to 'home'.
+    steps.append(_work())
+    steps.append(_home())
+    cleaned: list[ReferenceStep] = []
+    for step in steps:
+        if (
+            cleaned
+            and step.kind == 'move_named'
+            and step.args == ('work',)
+            and cleaned[-1].kind == 'move_named'
+            and cleaned[-1].args == ('work',)
+        ):
+            continue
+        cleaned.append(step)
+    return tuple(cleaned)
 
 
 def execute_sequence(sequence: tuple[ReferenceStep, ...], context) -> RunResult:

src/iiwa_experement/test/test_motion.py

@@ -3,6 +3,8 @@ from pathlib import Path
 
 import pytest
 
+from scipy.spatial.transform import Rotation
+
 from iiwa_experement.geometry import Transform, load_geometry
 from iiwa_experement.motion import MotionAdapter
 
@@ -245,3 +247,45 @@ def test_unavailable_server_is_reported_without_motion():
     assert 'unavailable' in result.message
     assert action.requests == []
     assert adapter.stopped is False
+
+
+def test_work_fallback_uses_the_safe_pose_and_never_transport():
+    planning_error = 'Планирование не удалось: injected failure'
+    joints = FakeActionClient(result=FakeResult(False, planning_error))
+    named = FakeService(success=False, message=planning_error)
+    action = FakeActionClient()
+
+    def echo_pose():
+        request = action.requests[-1]
+        matrix = Rotation.from_euler(
+            'ZYX', [request.a, request.b, request.c],
+        ).as_matrix()
+        return Transform(
+            xyz=(request.x, request.y, request.z),
+            rotation=tuple(tuple(float(v) for v in row) for row in matrix),
+            parent='base_link', child='tcp',
+        )
+
+    g = geometry()
+    # The forbidden pose is not mirrored into the executor at all.
+    assert 'transport' not in g.named_joints
+    adapter = MotionAdapter(
+        node=None,
+        geometry=g,
+        action_client=action,
+        joints_client=joints,
+        named_client=named,
+        stop_client=StopService(),
+        tf_buffer=object(),
+        scene_check=lambda: (True, 'ok'),
+        freshness_check=lambda: (True, 'ok'),
+        pose_provider=echo_pose,
+        spin_until=lambda future, timeout: future.done(),
+    )
+    result = adapter.move_named('work', speed=0.2)
+    assert result.success, result.message
+    # Both named paths were rejected, so the calibrated safe Cartesian pose
+    # was planned with OMPL instead of reconfiguring through 'transport'.
+    assert action.requests[-1].planner == 'ompl'
+    for request in joints.requests:
+        assert list(request.joints) == list(g.named_joints['work'])

src/iiwa_experement/test/test_operations.py

@@ -59,6 +59,8 @@ class FakeBridge:
 
     def remove(self, object_id):
         self.removed.append(object_id)
+        if object_id in self.attached:
+            self.attached.remove(object_id)
 
     def add_world(self, object_id, box, pose):
         self.added_world = getattr(self, 'added_world', [])
@@ -142,7 +144,7 @@ def test_take_tool_follows_the_visible_axial_route():
         'take_tool(vacuum):exit',
     ]
     assert supervisor.commands[0][:2] == ('attach_tool', 'vacuum')
-    assert bridge.attached == []
+    assert bridge.attached == ['grant_vacuum']
     contact_goal = motion.moves[2][3]
     assert contact_goal.xyz == pytest.approx(expected.xyz, abs=1e-9)
     assert motion.moves[0][1] == 'ompl'
@@ -162,7 +164,7 @@ def test_dock_tool_returns_the_tool_to_its_socket():
 def test_cap_pick_and_place_use_the_hatch_node_transform():
     operations, supervisor, bridge, motion, geometry = make_operations(1)
     assert operations.pick_cap().success
-    assert bridge.attached == []
+    assert bridge.attached == ['grant_cap']
     assert supervisor.commands[0][:2] == ('grab_cap', 'cap')
     assert operations.place_cap().success
     assert bridge.added_world[0][0] == 'grant_cap'
@@ -213,7 +215,7 @@ def test_contour_does_not_lift_between_adjacent_sides():
 def test_screw_pick_and_handoff_are_one_motion_and_one_removal():
     operations, supervisor, bridge, motion, geometry = make_operations(11)
     assert operations.pick_screw('screw_1').success
-    assert bridge.attached == []
+    assert bridge.attached == ['grant_screw_01']
     assert supervisor.commands[0][:2] == ('grab_screw', 'screw_1')
     assert operations.handoff_screw('screw_1', 'hole_1').success
     assert supervisor.commands[1][0] == 'handoff_screw'
@@ -236,33 +238,64 @@ def test_tighten_hole_is_a_visible_revolution_without_a_screw():
     assert label == 'tighten_hole(hole_1):joint_twist'
     assert joints[:6] == (0.0,) * 6
     assert joints[6] == pytest.approx(math.radians(45.0))
+    # The flange is untwisted before the axial retreat, so joint 7 does not
+    # accumulate twist across holes.
+    untwist_label, untwist_joints = motion.joint_moves[1]
+    assert untwist_label == 'tighten_hole(hole_1):joint_untwist'
+    assert untwist_joints == (0.0,) * 7
+    assert names[-2:] == [
+        'tighten_hole(hole_1):retreat', 'tighten_hole(hole_1):clear',
+    ]
     # 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():
+def test_screw_pick_reconfigures_through_work_when_a_cell_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 motion.named[-1][0] == 'work'
-    assert 'pick_screw(screw_1):home_retry:transfer' in labels(motion)
+    assert 'pick_screw(screw_1):recovery:transfer' in labels(motion)
     assert supervisor.commands[0][:2] == ('grab_screw', 'screw_1')
 
 
+def test_screw_pick_stops_instead_of_dragging_when_the_lift_is_rejected():
+    operations, supervisor, bridge, motion, geometry = make_operations(11)
+    motion.fail_labels.add('pick_screw(screw_1):retreat')
+    with pytest.raises(OperationError):
+        operations.pick_screw('screw_1')
+    assert operations.events[-1]['phase'] == 'failed'
+
+
+def test_handoff_screw_lifts_axially_before_the_next_move():
+    operations, supervisor, bridge, motion, geometry = make_operations(11)
+    assert operations.pick_screw('screw_1').success
+    motion.moves.clear()
+    assert operations.handoff_screw('screw_1', 'hole_1').success
+    names = labels(motion)
+    assert names[-2:] == [
+        'handoff_screw(screw_1,hole_1):retreat',
+        'handoff_screw(screw_1,hole_1):clear',
+    ]
+
+
 def test_tighten_hole_reverses_when_the_flange_limit_blocks_one_direction():
     operations, supervisor, bridge, motion, geometry = make_operations(14)
     motion.fail_positive_joint = True
     assert operations.tighten_hole('hole_1').success
-    assert len(motion.joint_moves) == 2
+    assert len(motion.joint_moves) == 3
     assert motion.joint_moves[1][1][6] == pytest.approx(math.radians(-45.0))
+    # The untwist returns the flange to the contact configuration.
+    assert motion.joint_moves[2][0] == 'tighten_hole(hole_1):joint_untwist'
+    assert motion.joint_moves[2][1][6] == pytest.approx(0.0)
 
 
 def test_failed_motion_stops_the_operation_and_emits_failure():
     operations, supervisor, bridge, motion, geometry = make_operations(1)
     motion.fail_labels.update({
         'take_tool(vacuum):contact',
-        'take_tool(vacuum):home_retry:contact',
+        'take_tool(vacuum):recovery:contact',
     })
     with pytest.raises(OperationError):
         operations.take_tool('vacuum')

src/iiwa_experement/test/test_reference_scenarios.py

@@ -40,6 +40,27 @@ def test_every_route_is_nonempty_and_returns_home():
         assert names[-2:] == ['dock_tool', 'move_named'] or names[-1] == 'move_named' or 'dock_tool' in names
 
 
+def test_routes_start_at_work_and_only_end_at_home():
+    for experiment_id in range(1, 21):
+        steps = route(experiment_id)
+        assert steps[0].kind == 'move_named'
+        assert steps[0].args == ('work',)
+        assert steps[-2].kind == 'move_named'
+        assert steps[-2].args == ('work',)
+        assert steps[-1].kind == 'move_named'
+        assert steps[-1].args == ('home',)
+        homes = [
+            index for index, step in enumerate(steps)
+            if step.kind == 'move_named' and step.args == ('home',)
+        ]
+        assert homes == [len(steps) - 1]
+        # The 'transport' pose is forbidden in the reference routes.
+        assert not any(
+            step.kind == 'move_named' and step.args == ('transport',)
+            for step in steps
+        )
+
+
 def test_cap_experiments_use_the_vacuum_tool():
     for experiment_id in range(1, 6):
         names = kinds(experiment_id)