diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 4dcf90d..9cbc543 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -665,21 +665,11 @@ if __name__ == '__main__': settle_elapsed = now - calibration_time settle_alpha = min(1.0, settle_elapsed / max(args.settle_time, 0.01)) - # Head rotation compensation: de-rotate wrist data by head's rotation - # since calibration. Converts world-frame positions/orientations back to - # a body-aligned frame, preventing arm swing during body rotation. - if head_R_at_cal is not None and args.head_rot_comp > 0.01: - R_head_delta = tele_data.head_pose[:3, :3] @ head_R_at_cal.T - R_comp = scale_rotation(R_head_delta.T, args.head_rot_comp) - else: - R_head_delta = np.eye(3) - R_comp = np.eye(3) - - # 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. + # Frozen head position: tv_wrapper computes wrist - head_pos, + # so head movement creates spurious wrist drift. Cancel it by + # adding back the head position change since calibration, + # effectively using the frozen calibration head position. + R_head_delta = tele_data.head_pose[:3, :3] @ head_R_at_cal.T if head_R_at_cal is not None else np.eye(3) 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 @@ -691,9 +681,9 @@ if __name__ == '__main__': left_wrist_adjusted[:3, 3] = robot_ref_left[:3, 3] + settle_alpha * left_delta_pos right_wrist_adjusted[:3, 3] = robot_ref_right[:3, 3] + settle_alpha * right_delta_pos - # Rotation: compute delta (de-rotated), scale by rot_blend (and settle_alpha) - left_delta_R = (R_comp @ tele_data.left_wrist_pose[:3, :3]) @ vp_ref_left[:3, :3].T - right_delta_R = (R_comp @ tele_data.right_wrist_pose[:3, :3]) @ vp_ref_right[:3, :3].T + # Rotation: raw delta only, no head rotation compensation + left_delta_R = tele_data.left_wrist_pose[:3, :3] @ vp_ref_left[:3, :3].T + right_delta_R = tele_data.right_wrist_pose[:3, :3] @ vp_ref_right[:3, :3].T rot_alpha = args.rot_blend * settle_alpha if rot_alpha < 0.01: left_wrist_adjusted[:3, :3] = robot_ref_left[:3, :3]