From 9280e74b2414cae34a52c20fa7ef54c3c7854ca3 Mon Sep 17 00:00:00 2001 From: melancholytron Date: Sat, 28 Feb 2026 22:29:55 -0600 Subject: [PATCH] Fix hand joint mapping: skip PALM instead of WRIST for dex-retargeting The dex-retargeting target_link_human_indices expect WebXR convention (joint 0 = WRIST) but we were outputting OpenXR convention (joint 0 = PALM). This caused the 5 difference vectors referencing index 0 to compute palm-to-fingertip instead of wrist-to-fingertip, degrading tracking accuracy. Also updates Inspire hand ranges and normalization formula. Co-Authored-By: Claude Opus 4.6 --- scripts/body_tracker.gd | 10 ++++++---- server/retarget_bridge.py | 28 +++++++++++++++++++--------- 2 files changed, 25 insertions(+), 13 deletions(-) diff --git a/scripts/body_tracker.gd b/scripts/body_tracker.gd index bdb6eff..c8c4f88 100644 --- a/scripts/body_tracker.gd +++ b/scripts/body_tracker.gd @@ -276,9 +276,11 @@ func _xform_to_mat16(xform: Transform3D) -> Array: ## Get 25 hand joint positions from XRHandTracker, relative to wrist (75 floats). -## Joint order: PALM(0), THUMB_META..TIP(2-5), INDEX(6-10), MIDDLE(11-15), -## RING(16-20), LITTLE(21-25) — skipping WRIST(1). -## This matches the format expected by dex-retargeting / xr_teleoperate. +## Joint order matches WebXR / dex-retargeting convention: +## WRIST(0), THUMB_META..TIP(1-4), INDEX(5-9), MIDDLE(10-14), +## RING(15-19), LITTLE(20-24) — skipping PALM (OpenXR joint 0). +## OpenXR has 26 joints (0=PALM, 1=WRIST, 2-25=fingers). +## WebXR/dex-retargeting has 25 joints (0=WRIST, 1-24=fingers). func _get_hand_positions_ht(hand_tracker: XRHandTracker) -> Array: var wrist_xform := hand_tracker.get_hand_joint_transform(XRHandTracker.HAND_JOINT_WRIST) var wrist_inv := wrist_xform.affine_inverse() @@ -286,7 +288,7 @@ func _get_hand_positions_ht(hand_tracker: XRHandTracker) -> Array: positions.resize(HAND_JOINT_COUNT * 3) # 25 * 3 = 75 var out_idx := 0 for xr_joint in range(26): # OpenXR joints 0-25 - if xr_joint == 1: # Skip WRIST + if xr_joint == 0: # Skip PALM (not in WebXR/dex-retargeting convention) continue var joint_xform := hand_tracker.get_hand_joint_transform(xr_joint) var rel := wrist_inv * joint_xform diff --git a/server/retarget_bridge.py b/server/retarget_bridge.py index 1e0bc61..0a5002a 100644 --- a/server/retarget_bridge.py +++ b/server/retarget_bridge.py @@ -114,14 +114,16 @@ KD = [ 1, 1, 1, 1, 1, 1, 1, ] -# Inspire hand normalization ranges +# Inspire hand normalization ranges (min=closed fist, max=open hand) +# Inspire convention: 0.0=closed, 1.0=open +# Calibrated from dex-retargeting output with Quest 3 XRHandTracker INSPIRE_RANGES = [ - (0.0, 1.7), # finger flexion - (0.0, 1.7), - (0.0, 1.7), - (0.0, 1.7), - (0.0, 0.5), # thumb pitch - (-0.1, 1.3), # thumb yaw + (0.4, 1.7), # pinky flexion + (0.4, 1.7), # ring flexion + (0.4, 1.7), # middle flexion + (0.4, 1.7), # index flexion + (0.2, 0.5), # thumb pitch + (0.8, 1.3), # thumb yaw ] # Body joint indices in shared memory (8 joints x 7 floats = 56) @@ -567,17 +569,19 @@ def retarget_hands(left_retargeting, right_retargeting, ref_value = left_hand_pos[left_indices[1, :]] - left_hand_pos[left_indices[0, :]] q_rad = left_retargeting.retarget(ref_value) q_hw = q_rad[left_hw_map] + 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((max_val - q_hw[idx]) / (max_val - min_val), 0.0, 1.0) + hand_cmd[6 + idx] = np.clip((q_hw[idx] - min_val) / (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, :]] q_rad = right_retargeting.retarget(ref_value) q_hw = q_rad[right_hw_map] + 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((max_val - q_hw[idx]) / (max_val - min_val), 0.0, 1.0) + hand_cmd[idx] = np.clip((q_hw[idx] - min_val) / (max_val - min_val), 0.0, 1.0) return hand_cmd @@ -825,6 +829,12 @@ def main(): if step_count % 250 == 0: np.set_printoptions(precision=2, suppress=True) logger.info(f" [HAND OUT] R=[{current_hands[0]:.2f},{current_hands[1]:.2f},{current_hands[2]:.2f},{current_hands[3]:.2f},{current_hands[4]:.2f},{current_hands[5]:.2f}] L=[{current_hands[6]:.2f},{current_hands[7]:.2f},{current_hands[8]:.2f},{current_hands[9]:.2f},{current_hands[10]:.2f},{current_hands[11]:.2f}]") + if step_count % 250 == 0 and hasattr(retarget_hands, '_last_q_hw_r'): + q = retarget_hands._last_q_hw_r + logger.info(f" [HAND RAW] R_q_hw=[{q[0]:.3f},{q[1]:.3f},{q[2]:.3f},{q[3]:.3f},{q[4]:.3f},{q[5]:.3f}]") + if step_count % 250 == 0 and hasattr(retarget_hands, '_last_q_hw_l'): + q = retarget_hands._last_q_hw_l + logger.info(f" [HAND RAW] L_q_hw=[{q[0]:.3f},{q[1]:.3f},{q[2]:.3f},{q[3]:.3f},{q[4]:.3f},{q[5]:.3f}]") except Exception as e: logger.warning(f"Hand retarget failed: {e}")