From c2107aced2d2a2cf08067fa3619dd4a915b82172 Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Thu, 19 Feb 2026 15:50:53 -0600 Subject: [PATCH] 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 --- teleop/teleop_hand_and_arm.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index e466018..c0b71cf 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/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]