Decouple arm tracking from head movement

Captures head position at calibration time and compensates for head
drift each frame. tv_wrapper subtracts live head position from wrist
poses, which causes robot arms to move when operator looks around or
walks. Head decoupling adds back (current_head - ref_head) to cancel
this coupling, so only actual hand movement drives the robot arms.

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

teleop/teleop_hand_and_arm.py

@@ -354,6 +354,7 @@ if __name__ == '__main__':
         vp_ref_right = None
         robot_ref_left = None    # 4x4: Robot FK pose at calibration moment
         robot_ref_right = None
+        head_ref = None          # 3-vec: Head position at calibration (for decoupling)
         prev_start = False
 
         # Initial calibration: capture reference poses on first START
@@ -363,11 +364,13 @@ if __name__ == '__main__':
             robot_ref_left, robot_ref_right = arm_ik.compute_fk_pose(_cal_arm_q)
             vp_ref_left = _cal_tele.left_wrist_pose.copy()
             vp_ref_right = _cal_tele.right_wrist_pose.copy()
+            head_ref = _cal_tele.head_pose[:3, 3].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]}")
+                           f"Robot L={robot_ref_left[:3,3]}, R={robot_ref_right[:3,3]} "
+                           f"Head={head_ref}")
 
         # main loop. robot start to follow VR user's motion
         while not STOP:
@@ -423,6 +426,7 @@ if __name__ == '__main__':
                 robot_ref_left, robot_ref_right = arm_ik.compute_fk_pose(_cal_arm_q)
                 vp_ref_left = _cal_tele.left_wrist_pose.copy()
                 vp_ref_right = _cal_tele.right_wrist_pose.copy()
+                head_ref = _cal_tele.head_pose[:3, 3].copy()
                 calibrated = True
                 calibration_time = time.time()
                 i_error_left = np.zeros(3)
@@ -521,9 +525,14 @@ if __name__ == '__main__':
                 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]
+                # Head decoupling: tv_wrapper subtracts live head position from wrists.
+                # Add back head drift so only hand movement (not head movement) drives arms.
+                head_drift = tele_data.head_pose[:3, 3] - head_ref
+
+                # Position: robot_ref + settle_alpha * (vp_current + head_drift - vp_ref)
+                # head_drift cancels tv_wrapper's live head subtraction, replacing it with fixed ref
+                left_delta_pos = (tele_data.left_wrist_pose[:3, 3] + head_drift) - vp_ref_left[:3, 3]
+                right_delta_pos = (tele_data.right_wrist_pose[:3, 3] + head_drift) - vp_ref_right[:3, 3]
 
                 left_wrist_adjusted = np.eye(4)
                 right_wrist_adjusted = np.eye(4)