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 <noreply@anthropic.com>

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

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: