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]