fix(experement): untwist after the lift, wait out controller lag, no gizmo in runs

- tighten_hole: lift out of the hole first, then return joint 7 (undoing the
  twist at the contact point visibly unscrewed the simulated tightening)
- motion: wait up to 8 s for the measured tcp to converge to the held goal
  instead of re-issuing it at once (the Webots controller reports success on
  the desired state while the arm still creeps; the re-issue looked like a
  violent 5-10 cm snap at dock and handoff transfers)
- reference launches disable the tcp gizmo in the temporary setting.yaml
  (cobot-setting.yaml only applies to 'cobot run')
- verified in Webots execute mode after these changes: 5, 11, 14, 15, 20

src/iiwa_config/config/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         # допустимый возраст состояния робота [с]
@@ -23,7 +23,7 @@ digital_twin:
     controller_timer: "50"
 
     objects:
-      enabled: true
+      enabled: false
       count: 5                    # точное общее количество; модели могут повторяться
       models:
         - path: pkg://iiwa_description/objects/hammer/hammer1.obj

src/iiwa_experement/iiwa_experement/motion.py

@@ -357,6 +357,21 @@ class MotionAdapter:
                     True, 'motion completed', label, True, base_goal, actual,
                     time.monotonic() - started,
                 )
+            # The Webots trajectory controller reports success on the desired
+            # trajectory state while the measured arm is still creeping to the
+            # held final setpoint.  Give it a bounded window to arrive before
+            # re-issuing the goal; re-issuing at once looks like a violent
+            # sideways snap of 5-10 cm.
+            converged = self._wait_for_converged_pose(base_goal, timeout_sec=8.0)
+            if converged is not None:
+                if self.node is not None:
+                    self.node.get_logger().info(
+                        f'{label}: tcp converged to the goal after a short wait'
+                    )
+                return MotionResult(
+                    True, 'motion completed', label, True, base_goal, converged,
+                    time.monotonic() - started,
+                )
             if attempt + 1 < self.pose_correction_attempts:
                 # The action succeeded but the controller stopped outside the
                 # pose tolerance.  Re-issue the same verified goal once; this
@@ -526,6 +541,30 @@ class MotionAdapter:
             True, None, self.actual_tcp(), time.monotonic() - started,
         )
 
+    def _wait_for_converged_pose(
+        self, goal: Transform, timeout_sec: float,
+    ) -> Transform | None:
+        """Wait for the measured tcp to reach the goal within tolerances.
+
+        Returns the converged pose, or None when the window elapsed.
+        """
+        if self.node is None:
+            return None
+        import rclpy
+
+        deadline = time.monotonic() + max(0.0, float(timeout_sec))
+        while time.monotonic() < deadline:
+            rclpy.spin_once(self.node, timeout_sec=0.05)
+            current = self.actual_tcp()
+            if current is not None:
+                distance, angle = _pose_distance(current, goal)
+                if (
+                    distance <= self.position_tolerance
+                    and angle <= self.orientation_tolerance
+                ):
+                    return current
+        return None
+
     def _settle(self) -> None:
         if self.settle_time_sec <= 0.0 or self.node is None:
             return

src/iiwa_experement/iiwa_experement/operations.py

@@ -739,6 +739,7 @@ class ReferenceOperations:
             twist_error = None
             twist_done = False
             joint_twist = False
+            twisted_goal = goal
             # 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
@@ -782,6 +783,7 @@ class ReferenceOperations:
                             f'tighten_hole({hole_id}):{suffix}',
                         )
                         twist_done = True
+                        twisted_goal = candidate
                         break
                     except OperationError as error:
                         # Holes on the far side of the body can sit against
@@ -793,14 +795,24 @@ class ReferenceOperations:
                         twist_error = error
                 if not twist_done:
                     raise twist_error
-            # Return the flange to the contact orientation before leaving.
+            # Lift axially out of the hole BEFORE undoing the twist: rotating
-            # Without the untwist, joint 7 accumulates 45 deg per hole and the
+            # the flange back at the contact point would visibly unscrew the
-            # wrist winds up until every later plan starts from a twisted
+            # simulated tightening.
-            # branch.
+            self._retreat(twisted_goal, f'tighten_hole({hole_id})')
             if joint_twist:
+                # Return only joint 7 to its pre-twist value; the other
+                # joints stay where the retreat left them.
+                current = provider() if provider is not None else None
+                self._require(
+                    current is not None and len(current) == 7,
+                    f'tighten_hole({hole_id}): joint state unavailable for '
+                    'the untwist',
+                )
+                untwist_joints = list(current)
+                untwist_joints[6] = joints[6]
                 self.motion.wait_stationary()
                 untwist = self.motion.move_joints(
-                    joints, self.geometry.speeds['contour'],
+                    untwist_joints, self.geometry.speeds['contour'],
                     f'tighten_hole({hole_id}):joint_untwist',
                 )
                 if not getattr(untwist, 'success', False):
@@ -809,13 +821,13 @@ class ReferenceOperations:
                         f'{untwist.message}'
                     )
             else:
+                # Pure rotation about the tool axis at the safe height; the
+                # raised untwisted pose shares the retreat position.
                 self._move(
-                    goal, 'lin', self.geometry.speeds['contour'],
+                    self._above(goal, self.geometry.transfer_gap), '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/runtime_config.py

@@ -131,6 +131,13 @@ def _materialize_setting(
     objects['enabled'] = False
     objects['count'] = 0
     webots['world'] = str(world_path.resolve())
+    # The interactive TCP gizmo must not exist in reference runs: it draws an
+    # overlay ball and can preempt the reference route.  cobot-setting.yaml
+    # only applies to `cobot run`, not to this launch.
+    gizmo = _mapping(
+        digital_twin.setdefault('tcp_gizmo', {}), 'digital_twin.tcp_gizmo',
+    )
+    gizmo['enabled'] = False
 
     tool = _mapping(data.setdefault('tool', {}), 'tool')
     tool['active'] = 'patron'

src/iiwa_experement/worlds/grant/.iiwa-grant.wbproj

@@ -0,0 +1,9 @@
+Webots Project File version R2025a
+perspectives: 000000ff00000000fd0000000300000000000001ca0000039ffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000000000010000023a000002effc0200000001fb0000001400540065007800740045006400690074006f00720000000014000002ef0000003f00ffffff0000000300000780000000dafc0100000002fb0000000e0043006f006e0073006f006c006501000001d0000005b00000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c010000000000000780000000000000000000000780000003cb00000004000000040000000100000008fc00000000
+simulationViewPerspectives: 000000ff0000000100000002000000000000077e0100000002010000000101
+sceneTreePerspectives: 000000ff00000001000000030000001c000000c0000000000100000002010000000201
+maximizedDockId: -1
+centralWidgetVisible: 1
+renderingMode: PLAIN
+orthographicViewHeight: 1
+textFiles: -1