Fix head compensation: use translation correction instead of rotation

Data analysis revealed that R_comp rotation was making arm drift 2-3x
WORSE. Root cause: when the head rotates, the Quest tracking point
orbits around the neck, creating a translational shift in head position.
Since tv_wrapper subtracts head_pos from wrist positions, this shift
causes spurious wrist drift (r=-0.96 correlation, slope=-1.0).

Replace rotation-based position compensation with translation
correction: store head_pos_at_cal, add head_pos_delta each frame to
cancel the head orbit artifact. Keep R_comp for wrist orientations.

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

teleop/teleop_hand_and_arm.py

@@ -460,6 +460,7 @@ if __name__ == '__main__':
         robot_ref_left = None    # 4x4: Robot FK pose at calibration moment
         robot_ref_right = None
         head_R_at_cal = None         # 3x3: head rotation at calibration (for body rotation compensation)
+        head_pos_at_cal = None       # 3-vec: head position at calibration (for head orbit correction)
         prev_start = False
 
         # Initial calibration: capture reference poses on first START
@@ -470,6 +471,7 @@ if __name__ == '__main__':
             vp_ref_left = _cal_tele.left_wrist_pose.copy()
             vp_ref_right = _cal_tele.right_wrist_pose.copy()
             head_R_at_cal = _cal_tele.head_pose[:3, :3].copy()
+            head_pos_at_cal = _cal_tele.head_pose[:3, 3].copy()
             calibrated = True
             calibration_time = time.time()
             prev_start = True
@@ -555,6 +557,7 @@ if __name__ == '__main__':
                 vp_ref_left = _cal_tele.left_wrist_pose.copy()
                 vp_ref_right = _cal_tele.right_wrist_pose.copy()
                 head_R_at_cal = _cal_tele.head_pose[:3, :3].copy()
+                head_pos_at_cal = _cal_tele.head_pose[:3, 3].copy()
                 calibrated = True
                 calibration_time = time.time()
                 i_error_left = np.zeros(3)
@@ -672,12 +675,14 @@ if __name__ == '__main__':
                     R_head_delta = np.eye(3)
                     R_comp = np.eye(3)
 
-                # De-rotate wrist positions, then compute delta from calibration.
-                # The head-to-waist offset [0.15, 0, 0.45] added by tv_wrapper is a
-                # body-frame constant — strip it before de-rotation, re-add after.
-                _H2W = np.array([0.15, 0.0, 0.45])
-                left_pos_comp = R_comp @ (tele_data.left_wrist_pose[:3, 3] - _H2W) + _H2W
-                right_pos_comp = R_comp @ (tele_data.right_wrist_pose[:3, 3] - _H2W) + _H2W
+                # 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.
+                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
                 left_delta_pos = left_pos_comp - vp_ref_left[:3, 3]
                 right_delta_pos = right_pos_comp - vp_ref_right[:3, 3]