From 714ba7079273d39e579835d23da7b3b89836ffc7 Mon Sep 17 00:00:00 2001 From: melancholytron Date: Sun, 1 Mar 2026 10:45:00 -0600 Subject: [PATCH] Fix hand coordinate frame + invert finger direction for Inspire hands R_FIX_HAND rotation corrects hand positions from body tracker frame (+X fingers) to Inspire URDF frame (-Y fingers). Inverts normalization formula so open hand maps to open robot hand. Adjusts inspire_hand.yml scaling/alpha for smoother tracking. Co-Authored-By: Claude Opus 4.6 --- server/inspire_hand.yml | 35 +++++++++++++++++++++++++++++++++++ server/retarget_bridge.py | 32 ++++++++++++++++++++++++++++---- 2 files changed, 63 insertions(+), 4 deletions(-) create mode 100644 server/inspire_hand.yml diff --git a/server/inspire_hand.yml b/server/inspire_hand.yml new file mode 100644 index 0000000..c06c8ea --- /dev/null +++ b/server/inspire_hand.yml @@ -0,0 +1,35 @@ +left: + type: DexPilot + urdf_path: inspire_hand/inspire_hand_left.urdf + target_joint_names: + [ + 'L_thumb_proximal_yaw_joint', + 'L_thumb_proximal_pitch_joint', + 'L_index_proximal_joint', + 'L_middle_proximal_joint', + 'L_ring_proximal_joint', + 'L_pinky_proximal_joint' + ] + wrist_link_name: "L_hand_base_link" + finger_tip_link_names: [ "L_thumb_tip", "L_index_tip", "L_middle_tip", "L_ring_tip", "L_pinky_tip" ] + target_link_human_indices: [[ 9, 14, 19, 24, 14, 19, 24, 19, 24, 24, 0, 0, 0, 0, 0], [ 4, 4, 4, 4, 9, 9, 9, 14, 14, 19, 4, 9, 14, 19, 24]] + scaling_factor: 1.0 + low_pass_alpha: 0.6 + +right: + type: DexPilot + urdf_path: inspire_hand/inspire_hand_right.urdf + target_joint_names: + [ + 'R_thumb_proximal_yaw_joint', + 'R_thumb_proximal_pitch_joint', + 'R_index_proximal_joint', + 'R_middle_proximal_joint', + 'R_ring_proximal_joint', + 'R_pinky_proximal_joint' + ] + wrist_link_name: "R_hand_base_link" + finger_tip_link_names: [ "R_thumb_tip", "R_index_tip", "R_middle_tip", "R_ring_tip", "R_pinky_tip" ] + target_link_human_indices: [[ 9, 14, 19, 24, 14, 19, 24, 19, 24, 24, 0, 0, 0, 0, 0], [ 4, 4, 4, 4, 9, 9, 9, 14, 14, 19, 4, 9, 14, 19, 24]] + scaling_factor: 1.0 + low_pass_alpha: 0.6 diff --git a/server/retarget_bridge.py b/server/retarget_bridge.py index 0a5002a..9bfa608 100644 --- a/server/retarget_bridge.py +++ b/server/retarget_bridge.py @@ -555,6 +555,17 @@ def init_hand_retargeting(): left_hw_map, right_hw_map) + +# Correction rotation: transforms hand positions from our frame (after T_TO_UNITREE_HAND @ T_ROBOT_OPENXR) +# into the Inspire URDF wrist frame where fingers extend in -Y. +# Our frame has fingers in +X, palm-normal in +Y, pinky-direction in +Z. +# URDF frame has fingers in -Y, palm-normal in -X, pinky-direction in -Z. +# R_FIX = Rx(180°) @ Rz(90°), verified against URDF joint positions. +R_FIX_HAND = np.array([[ 0, -1, 0], + [-1, 0, 0], + [ 0, 0, -1]], dtype=np.float64) + + def retarget_hands(left_retargeting, right_retargeting, left_indices, right_indices, left_hw_map, right_hw_map, @@ -562,6 +573,10 @@ def retarget_hands(left_retargeting, right_retargeting, """Compute 12 normalized Inspire hand values from hand positions.""" hand_cmd = np.ones(12) + # Rotate hand positions into URDF frame before retargeting + left_hand_pos = left_hand_pos @ R_FIX_HAND.T + right_hand_pos = right_hand_pos @ R_FIX_HAND.T + left_valid = not np.all(left_hand_pos == 0.0) right_valid = not np.all(right_hand_pos == 0.0) @@ -572,7 +587,7 @@ def retarget_hands(left_retargeting, right_retargeting, retarget_hands._last_q_hw_l = q_hw.copy() for idx in range(6): min_val, max_val = INSPIRE_RANGES[idx] - hand_cmd[6 + idx] = np.clip((q_hw[idx] - min_val) / (max_val - min_val), 0.0, 1.0) + hand_cmd[6 + idx] = np.clip((max_val - q_hw[idx]) / (max_val - min_val), 0.0, 1.0) if right_valid: ref_value = right_hand_pos[right_indices[1, :]] - right_hand_pos[right_indices[0, :]] @@ -581,7 +596,7 @@ def retarget_hands(left_retargeting, right_retargeting, retarget_hands._last_q_hw_r = q_hw.copy() for idx in range(6): min_val, max_val = INSPIRE_RANGES[idx] - hand_cmd[idx] = np.clip((q_hw[idx] - min_val) / (max_val - min_val), 0.0, 1.0) + hand_cmd[idx] = np.clip((max_val - q_hw[idx]) / (max_val - min_val), 0.0, 1.0) return hand_cmd @@ -810,10 +825,19 @@ def main(): if step_count % 250 == 0: l_valid = left_hand_pos is not None and not np.all(left_hand_pos == 0.0) r_valid = right_hand_pos is not None and not np.all(right_hand_pos == 0.0) - if left_hand_pos is not None: - l_range = np.ptp(left_hand_pos, axis=0) if left_hand_pos.ndim == 2 else np.ptp(left_hand_pos) + if left_hand_pos is not None and left_hand_pos.ndim == 2: + l_range = np.ptp(left_hand_pos, axis=0) + # Log key joint positions: wrist(0), thumb_tip(4), index_tip(9), middle_tip(14), pinky_tip(24) + logger.info(f" [HAND POS L] wrist={left_hand_pos[0]} thumb_tip={left_hand_pos[4]} idx_tip={left_hand_pos[9]} mid_tip={left_hand_pos[14]} pinky_tip={left_hand_pos[24]}") else: l_range = 0 + if right_hand_pos is not None and right_hand_pos.ndim == 2: + logger.info(f" [HAND POS R] wrist={right_hand_pos[0]} thumb_tip={right_hand_pos[4]} idx_tip={right_hand_pos[9]} mid_tip={right_hand_pos[14]} pinky_tip={right_hand_pos[24]}") + # Also log raw shared memory data to see if Godot data changes + with tv_wrapper.server.left_hand_position_shared.get_lock(): + raw_l = np.array(tv_wrapper.server.left_hand_position_shared[:]) + raw_l_25x3 = raw_l.reshape(25, 3) + logger.info(f" [HAND RAW POS L] wrist={raw_l_25x3[0]} thumb_tip={raw_l_25x3[4]} idx_tip={raw_l_25x3[9]} pinky_tip={raw_l_25x3[24]}") logger.info(f" [HAND DEBUG] L_valid={l_valid} R_valid={r_valid} L_shape={left_hand_pos.shape if left_hand_pos is not None else None} L_range={l_range}") if args.mirror: