fix(experement): harden tool transfers and tightening at wrist limits

Exit the rack with a graded +Y move after taking or docking a tool, continue through home after a dock, and fall back from work to home when a joint plan is rejected. Wait for fresh zero-velocity joint states before planning and after joint motion, restore a screw to the planning scene before a pick retry, and twist only the flange joint while simulating tightening so the arm does not leave a reachable IK branch.

src/iiwa_experement/doc/README.md

@@ -90,7 +90,7 @@ Webots. В `execute` первое движение начинается не р
   инструментами и признаком `home_reached`.
 - Винты не вставляются. После подтверждённой передачи у отверстия винт
   убирается из сцены, отверстие остаётся пустым. В опытах 14–20 берётся
-  `torque_tool`, подходит к каждому отверстию, выполняет поворот на 60° и
+  `torque_tool`, подходит к каждому отверстию, выполняет поворот на 45° и
   возвращается в стойку. Это видимая имитация затяжки без установки винта.
 
 Управляемые ветви: опыт 4 — один неудачный захват люка; 5 — первая посадка

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

@@ -61,9 +61,10 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
 
 ## Политика движений
 
-- После захвата инструмент поднимается вертикально на 16 см, и только затем
+- После захвата инструмент поднимается на 8 см, затем смещается на 10 см по
-  выполняется боковое или суставное перемещение. Так корпус инструмента не
+  +Y от стойки, и только потом выполняется суставное перемещение. Полный
-  проходит сквозь текстуры стойки.
+  вертикальный подъём на 14–16 см упирается в предел запястья; боковой выход
+  даёт тот же зазор и не ломает достижимость `work`.
 - Между операциями используется ближняя нейтральная поза `work`. `home`
   остаётся финальной позой маршрута, позой перед установкой инструмента в
   стойку и аварийной реконфигурацией.
@@ -139,9 +140,11 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
 
 В опытах 14, 15 и 16–20 после передач винтов берётся `torque_tool`. Для
 каждого отверстия выполняется `tighten_simulated`: подъём над отверстием,
-осевое опускание, поворот на 60°, возврат и осевой отход. Если у дальнего
+осевое опускание и поворот последнего сустава на 45°. Вращается только
-отверстия запястье упирается в предел, поворот выполняется в противоположную
+фланец, поэтому инструмент поворачивается вокруг своей оси, не уходя в другую
-сторону. Отверстие остаётся пустым; винт не вставляется. Инструмент
+ветвь IK. Если положительное направление упирается в предел, используется
+отрицательное. После поворота робот уходит в `work`, как после передачи
+винта. Отверстие остаётся пустым; винт не вставляется. Инструмент
 возвращается в стойку, затем робот уходит домой.
 
 ## Ветви отказов
@@ -162,7 +165,7 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
 - Только симуляция Webots; FRI и физический робот не запускаются.
 - Винты не вставляются; после передачи отверстие остаётся пустым.
 - Затяжка имитационная: `torque_tool` подходит к отверстию, поворачивается на
-  60° вокруг оси инструмента и отходит. Винт к этому моменту уже передан
+  45° вокруг оси инструмента и отходит. Винт к этому моменту уже передан
   оператору и удалён из сцены.
 - Прикреплённые объекты MoveItPy в установленной версии приводят к ложным
   столкновениям. Переносимый объект ведёт supervisor; статическая сцена,

src/iiwa_experement/iiwa_experement/motion.py

@@ -157,6 +157,7 @@ class MotionAdapter:
         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._joint_velocities: tuple[float, ...] | None = None
         self.stopped = False
         self.stop_reason: str | None = None
         self._last_joint_time = None
@@ -168,6 +169,45 @@ class MotionAdapter:
         self._last_joint_time = self.node.get_clock().now()
         if len(message.position) == 7:
             self._joint_positions = tuple(float(value) for value in message.position)
+        if len(message.velocity) == 7:
+            self._joint_velocities = tuple(
+                float(value) for value in message.velocity
+            )
+
+    def joint_positions(self) -> tuple[float, ...] | None:
+        return self._joint_positions
+
+    def wait_stationary(
+        self, timeout_sec: float = 3.0, tolerance: float = 0.01,
+    ) -> bool:
+        """Wait for fresh, near-zero joint velocities before planning.
+
+        The planning pipeline rejects a motion whose recorded start state still
+        carries velocity.  The controller action can finish before the next
+        zero-velocity sample reaches the planning server, so gate the next
+        plan on a short stable window.
+        """
+        if self.node is None or self._freshness_check is not None:
+            return True
+        import rclpy
+
+        deadline = time.monotonic() + max(0.0, float(timeout_sec))
+        stable_since = None
+        while time.monotonic() < deadline:
+            rclpy.spin_once(self.node, timeout_sec=0.02)
+            velocities = self._joint_velocities
+            if velocities is None or len(velocities) != 7 or any(
+                abs(value) > tolerance for value in velocities
+            ):
+                stable_since = None
+                continue
+            stable_since = stable_since or time.monotonic()
+            if time.monotonic() - stable_since >= 0.20:
+                return True
+        self.node.get_logger().warning(
+            'joint_states did not settle before planning'
+        )
+        return False
 
     def _default_freshness(self) -> tuple[bool, str]:
         if self._last_joint_time is None:
@@ -338,6 +378,7 @@ class MotionAdapter:
 
     def move_named(self, name: str, speed: float = 0.2, label: str = '') -> MotionResult:
         label = label or f'move_named({name})'
+        self.wait_stationary()
         joints = self.geometry.named_joints.get(name)
         if self.joints_client is not None and joints is not None:
             result = self.move_joints(joints, speed, label)
@@ -353,7 +394,23 @@ class MotionAdapter:
             # A joint-space plan can be rejected in an unusual scene; retry
             # the named service once as the fallback path.
             self.reset_stop()
-        return self._move_named_service(name, speed, label)
+        result = self._move_named_service(name, speed, label)
+        if result.success or name != 'work':
+            return result
+        # The tool rack can leave the arm in a branch from which the close
+        # 'work' pose is unreachable.  Fall back to the globally safe home
+        # 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(
         self,

src/iiwa_experement/iiwa_experement/operations.py

@@ -304,6 +304,7 @@ class ReferenceOperations:
             # false contact with the target itself.
             self.bridge.remove(remove_before_contact)
             self._require_synchronized()
+        self.motion.wait_stationary()
         transfer = self._above(goal, transfer_gap)
         approach = self._above(goal, approach_gap)
         self._move(transfer, 'ompl', self.geometry.speeds['transfer'], f'{label}:transfer')
@@ -333,7 +334,9 @@ 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:
+    def _contact_with_home_retry(
+        self, goal: Transform, label: str, *, prepare=None, **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
@@ -348,6 +351,8 @@ class ReferenceOperations:
                 raise
             self._emit('reconfigure', label=label, target='home',
                        reason=str(error))
+            if prepare is not None:
+                prepare()
             self.motion.reset_stop()
             named = self.motion.move_named('home', 0.3)
             if not getattr(named, 'success', False):
@@ -392,30 +397,34 @@ class ReferenceOperations:
                 remove_before_contact=f'grant_{tool_name}',
             )
             self.supervisor.command('attach_tool', tool_name)
-            # Lift straight up, clear of the magazine, before any lateral or
+            # Lift a little, then translate along +Y away from the magazine
-            # joint-space move.  Without this the tool body sweeps through the
+            # before any large joint-space move.  A full-height vertical lift
-            # rack obstacles while the arm travels to 'work'.  The sockets sit
+            # runs into the wrist limit; the lateral exit is what actually
-            # at slightly different reachable heights, so accept the highest
+            # keeps the tool body out of the rack textures.
-            # clearance that plans.
+            transfer = self._above(goal, self.geometry.transfer_gap)
-            lift_error = None
+            self._move(
-            for gap in (0.16, 0.14, 0.12):
+                transfer, 'lin', self.geometry.speeds['approach'],
+                f'take_tool({tool_name}):lift',
+            )
+            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}):lift' if gap == 0.16
+                    f'take_tool({tool_name}):exit' if offset == 0.10
-                    else f'take_tool({tool_name}):lift:{gap:.2f}'
+                    else f'take_tool({tool_name}):exit:{offset:.2f}'
                 )
                 try:
                     self._move(
-                        self._above(goal, gap), 'lin',
+                        exit_pose, 'lin', self.geometry.speeds['transfer'],
-                        self.geometry.speeds['approach'], label,
+                        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))
@@ -443,6 +452,28 @@ class ReferenceOperations:
             )
             self._require_synchronized()
             self._retreat(goal, f'dock_tool({tool_name})')
+            # Step out of the rack along +Y before any joint-space move; the
+            # axial retreat alone can leave the wrist against its limit.
+            transfer = self._above(goal, self.geometry.transfer_gap)
+            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'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,
+                    )
+                    break
+                except OperationError as error:
+                    if 'Планирование' not in str(error):
+                        raise
+                    self.motion.reset_stop()
             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:
@@ -575,9 +606,18 @@ class ReferenceOperations:
             self._require_synchronized()
             screw_pose = self.supervisor.object_pose(def_name)
             goal = tcp_goal(screw_pose, self.geometry.screw_tcp_to_node)
+            def restore_screw() -> None:
+                # The first attempt removed the screw from the planning scene
+                # before its transfer was rejected.  Restore it for the retry.
+                self.bridge.add_world(
+                    def_name, self.geometry.screw_collision, screw_pose,
+                )
+                self._require_synchronized()
+
             self._contact_with_home_retry(
                 goal, f'pick_screw({screw_id})',
                 remove_before_contact=def_name,
+                prepare=restore_screw,
             )
             self.supervisor.command('grab_screw', screw_id)
             self._retreat(goal, f'pick_screw({screw_id})')
@@ -618,7 +658,7 @@ class ReferenceOperations:
             raise
 
     # ------------------------------------------------------------------ tightening
-    def tighten_hole(self, hole_id: str, twist_deg: float = 60.0) -> OperationResult:
+    def tighten_hole(self, hole_id: str, twist_deg: float = 45.0) -> OperationResult:
         """Simulate tightening at an empty hole with the torque tool.
 
         The screw has already been handed to the virtual operator and removed
@@ -641,35 +681,64 @@ class ReferenceOperations:
             goal = tcp_goal(target, tip)
             self._contact(goal, f'tighten_hole({hole_id})')
             twist_error = None
-            for suffix, offset in (
+            twisted = goal
-                ('twist:positive', twist_deg),
+            twist_done = False
-                ('twist:negative', -twist_deg),
+            # Prefer a pure flange rotation: it twists the tool about its own
-            ):
+            # axis without moving the arm to another IK branch.  A Cartesian
-                twisted = goal.compose_relaxed(Transform.from_xyz_rpy(
+            # twist can push joint 6 into its limit, after which 'work' is no
-                    [0.0, 0.0, 0.0], [0.0, 0.0, offset],
+            # longer reachable.
-                    parent='tcp', child='tighten_twist',
+            joints = None
-                ))
+            provider = getattr(self.motion, 'joint_positions', None)
-                try:
+            if provider is not None:
-                    self._move(
+                joints = provider()
-                        twisted, 'lin', self.geometry.speeds['contour'],
+            if joints is not None and len(joints) == 7:
-                        f'tighten_hole({hole_id}):{suffix}',
+                for sign in (1.0, -1.0):
+                    target_joints = list(joints)
+                    target_joints[6] += math.radians(sign * twist_deg)
+                    self.motion.wait_stationary()
+                    result = self.motion.move_joints(
+                        target_joints, self.geometry.speeds['contour'],
+                        f'tighten_hole({hole_id}):joint_twist',
                     )
-                    break
+                    if getattr(result, 'success', False):
-                except OperationError as error:
+                        twist_done = True
-                    # Holes on the far side of the body can sit against the
+                        self._emit('tighten_twist', object=hole_id,
-                    # wrist limit in one direction.  A failed plan executes no
+                                   joint=7, delta_deg=sign * twist_deg)
-                    # motion, so the opposite twist is safe to try.
+                        break
-                    if 'Планирование' not in str(error):
+                    reset = getattr(self.motion, 'reset_stop', None)
-                        raise
+                    if reset is not None:
-                    self.motion.reset_stop()
+                        reset()
-                    twist_error = error
+            if not twist_done:
-            else:
+                for suffix, offset in (
-                raise twist_error
+                    ('twist:positive', twist_deg),
-            self._move(
+                    ('twist:negative', -twist_deg),
-                goal, 'lin', self.geometry.speeds['contour'],
+                ):
-                f'tighten_hole({hole_id}):return',
+                    candidate = goal.compose_relaxed(Transform.from_xyz_rpy(
-            )
+                        [0.0, 0.0, 0.0], [0.0, 0.0, offset],
-            self._retreat(goal, f'tighten_hole({hole_id})')
+                        parent='tcp', child='tighten_twist',
+                    ))
+                    self.motion.wait_stationary()
+                    try:
+                        self._move(
+                            candidate, 'lin', self.geometry.speeds['contour'],
+                            f'tighten_hole({hole_id}):{suffix}',
+                        )
+                        twisted = candidate
+                        twist_done = True
+                        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.
+                        if 'Планирование' not in str(error):
+                            raise
+                        self.motion.reset_stop()
+                        twist_error = error
+                if not twist_done:
+                    raise twist_error
+            # The caller's following 'work' move is the verified clearance
+            # move, as after a screw handoff.  A separate Cartesian retreat
+            # from the twisted pose can be rejected by the wrist limits.
             self._emit('tighten_simulated', object=hole_id, phase='done',
                        twist_deg=twist_deg)
             return OperationResult(

src/iiwa_experement/iiwa_experement/reference_scenarios.py

@@ -81,8 +81,9 @@ def _cap_steps(
         _home(),
         ReferenceStep('dock_tool', ('vacuum',)),
     ])
-    if finish_home:
+    # Always leave the rack through home: the dock retreat can sit against a
-        steps.append(_home())
+    # wrist limit from which 'work' is not reachable.
+    steps.append(_home())
     return steps
 
 
@@ -113,8 +114,7 @@ def _sealant_steps(
         _home(),
         ReferenceStep('dock_tool', ('sealant_tool',)),
     ])
-    if finish_home:
+    steps.append(_home())
-        steps.append(_home())
     return steps
 
 
@@ -144,9 +144,9 @@ def _screw_steps(
         ])
     steps.append(_home())
     steps.append(ReferenceStep('dock_tool', ('magnetic_tool',)))
+    steps.append(_home())
     if 'torque_tool' in spec.tool_names:
         steps.extend([
-            _work(),
             ReferenceStep('take_tool', ('torque_tool',)),
             _work(),
         ])
@@ -157,7 +157,6 @@ def _screw_steps(
             ])
         steps.append(_home())
         steps.append(ReferenceStep('dock_tool', ('torque_tool',)))
-    if finish_home:
         steps.append(_home())
     return steps
 

src/iiwa_experement/test/test_operations.py

@@ -1,4 +1,5 @@
 """Operation sequences are visible, ordered and fail closed."""
+import math
 from pathlib import Path
 
 import pytest
@@ -75,6 +76,9 @@ class FakeMotion:
         self.moves = []
         self.named = []
         self.fail_labels = set()
+        self.joint_moves = []
+        self.joint_values = (0.0,) * 7
+        self.fail_positive_joint = False
 
     def move_pose(self, goal, planner='ptp', speed=0.2, label=''):
         self.moves.append((label, planner, speed, goal))
@@ -86,9 +90,22 @@ class FakeMotion:
         self.named.append((name, speed))
         return MotionResult(True, 'ok', f'move_named({name})')
 
+    def joint_positions(self):
+        return self.joint_values
+
+    def move_joints(self, joints, speed=0.2, label=''):
+        self.joint_moves.append((label, tuple(joints)))
+        if self.fail_positive_joint and joints[6] > 0:
+            return MotionResult(False, 'injected joint failure', label)
+        self.joint_values = tuple(joints)
+        return MotionResult(True, 'ok', label)
+
     def reset_stop(self):
         return None
 
+    def wait_stationary(self, *args, **kwargs):
+        return True
+
 
 def make_operations(experiment_id):
     spec = load_scenario(experiment_id, SCENES)
@@ -122,6 +139,7 @@ def test_take_tool_follows_the_visible_axial_route():
         'take_tool(vacuum):approach',
         'take_tool(vacuum):contact',
         'take_tool(vacuum):lift',
+        'take_tool(vacuum):exit',
     ]
     assert supervisor.commands[0][:2] == ('attach_tool', 'vacuum')
     assert bridge.attached == []
@@ -138,7 +156,7 @@ def test_dock_tool_returns_the_tool_to_its_socket():
     assert result.success
     assert supervisor.commands[0][:2] == ('dock_tool', 'vacuum')
     assert bridge.added_world[0][0] == 'grant_vacuum'
-    assert labels(motion)[-1] == 'dock_tool(vacuum):clear'
+    assert labels(motion)[-1] == 'dock_tool(vacuum):exit'
 
 
 def test_cap_pick_and_place_use_the_hatch_node_transform():
@@ -213,11 +231,11 @@ def test_tighten_hole_is_a_visible_revolution_without_a_screw():
         'tighten_hole(hole_1):approach',
         'tighten_hole(hole_1):contact',
     ]
-    twist = motion.moves[names.index('tighten_hole(hole_1):twist:positive')][3]
+    assert motion.joint_moves
-    contact = motion.moves[names.index('tighten_hole(hole_1):contact')][3]
+    label, joints = motion.joint_moves[0]
-    assert twist.xyz == pytest.approx(contact.xyz)
+    assert label == 'tighten_hole(hole_1):joint_twist'
-    relative = contact.inverse().compose_relaxed(twist)
+    assert joints[:6] == (0.0,) * 6
-    assert abs(relative.rotation[2][2] - 1.0) < 1e-9
+    assert joints[6] == pytest.approx(math.radians(45.0))
     # The operation is a simulation only: no supervisor command changes state.
     assert supervisor.commands == []
     assert bridge.attached == []
@@ -232,13 +250,12 @@ def test_screw_pick_reconfigures_to_home_when_work_is_blocked():
     assert supervisor.commands[0][:2] == ('grab_screw', 'screw_1')
 
 
-def test_tighten_hole_reverses_the_twist_when_one_direction_is_blocked():
+def test_tighten_hole_reverses_when_the_flange_limit_blocks_one_direction():
     operations, supervisor, bridge, motion, geometry = make_operations(14)
-    motion.fail_labels.add('tighten_hole(hole_1):twist:positive')
+    motion.fail_positive_joint = True
     assert operations.tighten_hole('hole_1').success
-    names = labels(motion)
+    assert len(motion.joint_moves) == 2
-    assert 'tighten_hole(hole_1):twist:positive' in names
+    assert motion.joint_moves[1][1][6] == pytest.approx(math.radians(-45.0))
-    assert 'tighten_hole(hole_1):twist:negative' in names
 
 
 def test_failed_motion_stops_the_operation_and_emits_failure():