Fix head rotation comp: exclude head-to-waist offset from de-rotation

tv_wrapper adds a constant [0.15, 0, 0.45] offset to translate the
coordinate origin from HEAD to WAIST. This body-frame constant was
being de-rotated by R_comp along with the actual wrist position,
causing ~7.5cm arm drift per 30deg of head rotation.

Fix: strip the offset before de-rotation, re-add after.

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

teleop/teleop_hand_and_arm.py

@@ -640,9 +640,12 @@ if __name__ == '__main__':
                 else:
                     R_comp = np.eye(3)
 
-                # De-rotate wrist positions, then compute delta from calibration
-                left_pos_comp = R_comp @ tele_data.left_wrist_pose[:3, 3]
-                right_pos_comp = R_comp @ tele_data.right_wrist_pose[:3, 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
                 left_delta_pos = left_pos_comp - vp_ref_left[:3, 3]
                 right_delta_pos = right_pos_comp - vp_ref_right[:3, 3]