Remove all R_comp: frozen head position + raw orientation deltas

Strip R_comp from wrist orientations too — only use frozen head
position (translation correction) for positions, and raw orientation
deltas for rotations. No head rotation corrections at all.

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

teleop/teleop_hand_and_arm.py

@@ -665,21 +665,11 @@ if __name__ == '__main__':
                 settle_elapsed = now - calibration_time
                 settle_alpha = min(1.0, settle_elapsed / max(args.settle_time, 0.01))
 
-                # Head rotation compensation: de-rotate wrist data by head's rotation
-                # since calibration. Converts world-frame positions/orientations back to
-                # a body-aligned frame, preventing arm swing during body rotation.
-                if head_R_at_cal is not None and args.head_rot_comp > 0.01:
-                    R_head_delta = tele_data.head_pose[:3, :3] @ head_R_at_cal.T
-                    R_comp = scale_rotation(R_head_delta.T, args.head_rot_comp)
-                else:
-                    R_head_delta = np.eye(3)
-                    R_comp = np.eye(3)
-
-                # Head orbit translation correction: when the head rotates, the
-                # Quest tracking point orbits around the neck, shifting the reported
-                # head position. Since tv_wrapper computes wrist - head_pos, this
-                # creates spurious drift. Cancel it by adding back the head position
-                # delta since calibration.
+                # Frozen head position: tv_wrapper computes wrist - head_pos,
+                # so head movement creates spurious wrist drift. Cancel it by
+                # adding back the head position change since calibration,
+                # effectively using the frozen calibration head position.
+                R_head_delta = tele_data.head_pose[:3, :3] @ head_R_at_cal.T if head_R_at_cal is not None else np.eye(3)
                 head_pos_delta = tele_data.head_pose[:3, 3] - head_pos_at_cal
                 left_pos_comp = tele_data.left_wrist_pose[:3, 3] + head_pos_delta
                 right_pos_comp = tele_data.right_wrist_pose[:3, 3] + head_pos_delta
@@ -691,9 +681,9 @@ if __name__ == '__main__':
                 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 (de-rotated), scale by rot_blend (and settle_alpha)
-                left_delta_R = (R_comp @ tele_data.left_wrist_pose[:3, :3]) @ vp_ref_left[:3, :3].T
-                right_delta_R = (R_comp @ tele_data.right_wrist_pose[:3, :3]) @ vp_ref_right[:3, :3].T
+                # Rotation: raw delta only, no head rotation compensation
+                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]