Fix calibration jerk and wrist rotation drift

1. Settle ramp: after calibration, smoothly blend VP deltas from 0%
   to 100% over --settle-time (default 0.5s). Prevents the initial
   arm jerk caused by VP tracking jitter on the first frame.

2. Rotation blend: scale VP rotation deltas by --rot-blend (default
   0.3) to prevent wrist joints from drifting to odd positions.
   The VP-to-Unitree rotation convention may be wrong for Vision Pro
   (tuned for PICO), so dampening rotation tracking avoids the IK
   solver fighting a bad rotation target. Use --rot-blend 0 for
   position-only tracking, --rot-blend 1 for full rotation.

Both parameters ramp together during settle: rot_alpha = rot_blend * settle_alpha

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

teleop/teleop_hand_and_arm.py

@@ -66,6 +66,23 @@ def parse_r3_buttons(data):
         'Start': bool(btn1 & 0x04),
     }
 
+def scale_rotation(R, alpha):
+    """Scale a rotation matrix's angle by alpha (0=identity, 1=original). Pure numpy."""
+    cos_theta = np.clip((np.trace(R) - 1.0) / 2.0, -1.0, 1.0)
+    theta = np.arccos(cos_theta)
+    if theta < 1e-6:
+        return np.eye(3)
+    axis = np.array([
+        R[2, 1] - R[1, 2],
+        R[0, 2] - R[2, 0],
+        R[1, 0] - R[0, 1]
+    ]) / (2.0 * np.sin(theta))
+    scaled = theta * alpha
+    K = np.array([[0, -axis[2], axis[1]],
+                  [axis[2], 0, -axis[0]],
+                  [-axis[1], axis[0], 0]])
+    return np.eye(3) + np.sin(scaled) * K + (1.0 - np.cos(scaled)) * (K @ K)
+
 # Previous button state for edge detection
 _r3_prev_buttons = {}
 
@@ -120,6 +137,10 @@ if __name__ == '__main__':
     parser.add_argument('--i-decay', type=float, default=0.995, help='Per-frame decay factor for I term (default: 0.995)')
     parser.add_argument('--no-calibration', action='store_true',
                         help='Disable start-time calibration (use absolute VP poses)')
+    parser.add_argument('--settle-time', type=float, default=0.5,
+                        help='Seconds to ramp tracking after calibration (default: 0.5)')
+    parser.add_argument('--rot-blend', type=float, default=0.3,
+                        help='Rotation tracking blend (0=position-only, 1=full rotation, default: 0.3)')
     # record mode and task info
     parser.add_argument('--record', action = 'store_true', help = 'Enable data recording mode')
     parser.add_argument('--task-dir', type = str, default = './utils/data/', help = 'path to save data')
@@ -311,6 +332,7 @@ if __name__ == '__main__':
 
         # Relative-mode calibration state
         calibrated = False
+        calibration_time = 0.0   # When calibration was captured (for settle ramp)
         vp_ref_left = None       # 4x4: VP wrist pose at calibration moment
         vp_ref_right = None
         robot_ref_left = None    # 4x4: Robot FK pose at calibration moment
@@ -325,6 +347,7 @@ if __name__ == '__main__':
             vp_ref_left = _cal_tele.left_wrist_pose.copy()
             vp_ref_right = _cal_tele.right_wrist_pose.copy()
             calibrated = True
+            calibration_time = time.time()
             prev_start = True
             logger_mp.info(f"[calibration] Initial reference captured. "
                            f"Robot L={robot_ref_left[:3,3]}, R={robot_ref_right[:3,3]}")
@@ -382,6 +405,7 @@ if __name__ == '__main__':
                 vp_ref_left = _cal_tele.left_wrist_pose.copy()
                 vp_ref_right = _cal_tele.right_wrist_pose.copy()
                 calibrated = True
+                calibration_time = time.time()
                 i_error_left = np.zeros(3)
                 i_error_right = np.zeros(3)
                 logger_mp.info(f"[calibration] Re-calibrated on resume. "
@@ -470,20 +494,29 @@ if __name__ == '__main__':
 
             # --- Apply relative-mode calibration ---
             if calibrated:
-                # Position: robot_ref + (vp_current - vp_ref)
+                # Settle ramp: smoothly blend from 0% to 100% delta over settle_time
+                settle_elapsed = now - calibration_time
+                settle_alpha = min(1.0, settle_elapsed / max(args.settle_time, 0.01))
+
+                # Position: robot_ref + settle_alpha * (vp_current - vp_ref)
+                left_delta_pos = tele_data.left_wrist_pose[:3, 3] - vp_ref_left[:3, 3]
+                right_delta_pos = tele_data.right_wrist_pose[:3, 3] - vp_ref_right[:3, 3]
+
                 left_wrist_adjusted = np.eye(4)
                 right_wrist_adjusted = np.eye(4)
-                left_wrist_adjusted[:3, 3] = robot_ref_left[:3, 3] + (
-                    tele_data.left_wrist_pose[:3, 3] - vp_ref_left[:3, 3])
-                right_wrist_adjusted[:3, 3] = robot_ref_right[:3, 3] + (
-                    tele_data.right_wrist_pose[:3, 3] - vp_ref_right[:3, 3])
-                # Rotation: delta_R @ robot_ref_R  where delta_R = vp_R @ vp_ref_R^T
-                left_wrist_adjusted[:3, :3] = (
-                    tele_data.left_wrist_pose[:3, :3] @ vp_ref_left[:3, :3].T
-                ) @ robot_ref_left[:3, :3]
-                right_wrist_adjusted[:3, :3] = (
-                    tele_data.right_wrist_pose[:3, :3] @ vp_ref_right[:3, :3].T
-                ) @ robot_ref_right[:3, :3]
+                left_wrist_adjusted[:3, 3] = robot_ref_left[:3, 3] + settle_alpha * left_delta_pos
+                right_wrist_adjusted[:3, 3] = robot_ref_right[:3, 3] + settle_alpha * right_delta_pos
+
+                # Rotation: compute delta, scale by rot_blend (and settle_alpha)
+                left_delta_R = tele_data.left_wrist_pose[:3, :3] @ vp_ref_left[:3, :3].T
+                right_delta_R = tele_data.right_wrist_pose[:3, :3] @ vp_ref_right[:3, :3].T
+                rot_alpha = args.rot_blend * settle_alpha
+                if rot_alpha < 0.01:
+                    left_wrist_adjusted[:3, :3] = robot_ref_left[:3, :3]
+                    right_wrist_adjusted[:3, :3] = robot_ref_right[:3, :3]
+                else:
+                    left_wrist_adjusted[:3, :3] = scale_rotation(left_delta_R, rot_alpha) @ robot_ref_left[:3, :3]
+                    right_wrist_adjusted[:3, :3] = scale_rotation(right_delta_R, rot_alpha) @ robot_ref_right[:3, :3]
             else:
                 left_wrist_adjusted = tele_data.left_wrist_pose.copy()
                 right_wrist_adjusted = tele_data.right_wrist_pose.copy()