Fix wrist rotation + enable finger tracking via XRHandTracker

- Compute R_CORR empirically from measured rotation matrices:
  R_CORR_L = Rz(+90°) @ Rx(180°), R_CORR_R = Rz(-90°) @ Rx(180°)
  Body tracking wrist frame differs from hand tracking frame used
  by the reference xr_teleoperate code.

- Switch finger joint data from XRBodyTracker to XRHandTracker.
  XRBodyTracker doesn't provide articulated finger data on Quest 3;
  XRHandTracker does via camera-based hand tracking.
  25 joints: PALM + 24 finger joints (skip WRIST), matching
  dex-retargeting's expected format.

- Re-enable hand retargeting in bridge main loop (was disabled
  during arm IK testing).

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

scripts/body_tracker.gd

@@ -178,9 +178,22 @@ func _process(_delta: float) -> void:
 	data["left_wrist"] = _xform_to_mat16(left_wrist_rel)
 	data["right_wrist"] = _xform_to_mat16(right_wrist_rel)
 
+	# Get XRHandTracker references (used for finger positions AND wrist rotation)
+	var left_ht = XRServer.get_tracker(&"/user/hand_tracker/left") as XRHandTracker
+	var right_ht = XRServer.get_tracker(&"/user/hand_tracker/right") as XRHandTracker
+
 	# Hand joint positions (25 joints per hand, relative to wrist)
-	var left_hand_pos := _get_hand_positions(tracker, JOINT_LEFT_HAND_START, left_wrist_xform)
-	var right_hand_pos := _get_hand_positions(tracker, JOINT_RIGHT_HAND_START, right_wrist_xform)
+	# Use XRHandTracker for finger articulation — XRBodyTracker doesn't provide it on Quest 3
+	var left_hand_pos: Array
+	var right_hand_pos: Array
+	if left_ht and left_ht.has_tracking_data:
+		left_hand_pos = _get_hand_positions_ht(left_ht)
+	else:
+		left_hand_pos = _get_hand_positions(tracker, JOINT_LEFT_HAND_START, left_wrist_xform)
+	if right_ht and right_ht.has_tracking_data:
+		right_hand_pos = _get_hand_positions_ht(right_ht)
+	else:
+		right_hand_pos = _get_hand_positions(tracker, JOINT_RIGHT_HAND_START, right_wrist_xform)
 	data["left_hand_pos"] = left_hand_pos
 	data["right_hand_pos"] = right_hand_pos
 
@@ -203,12 +216,10 @@ func _process(_delta: float) -> void:
 	# hand tracking has much better wrist rotation detection
 	var left_wrist_body := left_wrist_xform
 	var right_wrist_body := right_wrist_xform
-	var left_ht = XRServer.get_tracker(&"/user/hand_tracker/left") as XRHandTracker
 	if left_ht and left_ht.has_tracking_data:
 		var ht_wrist := left_ht.get_hand_joint_transform(XRHandTracker.HAND_JOINT_WRIST)
 		if ht_wrist.origin != Vector3.ZERO:
 			left_wrist_body = ht_wrist
-	var right_ht = XRServer.get_tracker(&"/user/hand_tracker/right") as XRHandTracker
 	if right_ht and right_ht.has_tracking_data:
 		var ht_wrist := right_ht.get_hand_joint_transform(XRHandTracker.HAND_JOINT_WRIST)
 		if ht_wrist.origin != Vector3.ZERO:
@@ -264,7 +275,29 @@ func _xform_to_mat16(xform: Transform3D) -> Array:
 	]
 
 
-## Get 25 hand joint positions relative to wrist, as flat array (75 floats)
+## 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.
+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()
+	var positions := []
+	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
+			continue
+		var joint_xform := hand_tracker.get_hand_joint_transform(xr_joint)
+		var rel := wrist_inv * joint_xform
+		positions[out_idx * 3 + 0] = rel.origin.x
+		positions[out_idx * 3 + 1] = rel.origin.y
+		positions[out_idx * 3 + 2] = rel.origin.z
+		out_idx += 1
+	return positions
+
+
+## Get 25 hand joint positions from XRBodyTracker (fallback), relative to wrist.
 ## Each joint: [x, y, z] relative to wrist
 func _get_hand_positions(tracker: XRBodyTracker, start_idx: int, wrist_xform: Transform3D) -> Array:
 	var wrist_inv := wrist_xform.affine_inverse()

server/retarget_bridge.py

@@ -230,6 +230,18 @@ def build_ik_targets(bj):
     left_wrist_final[0:3, 3] -= hips_pos
     right_wrist_final[0:3, 3] -= hips_pos
 
+    # Correction rotation: body tracking wrist frame → URDF end-effector frame
+    # Computed empirically from measured rotation matrices during palms-down pose.
+    # R_CORR_L = Rz(+90°) @ Rx(180°),  R_CORR_R = Rz(-90°) @ Rx(180°)
+    R_CORR_L = np.array([[ 0,  1,  0],
+                          [ 1,  0,  0],
+                          [ 0,  0, -1]], dtype=np.float64)
+    R_CORR_R = np.array([[ 0, -1,  0],
+                          [-1,  0,  0],
+                          [ 0,  0, -1]], dtype=np.float64)
+    left_wrist_final[0:3, 0:3] = left_wrist_final[0:3, 0:3] @ R_CORR_L
+    right_wrist_final[0:3, 0:3] = right_wrist_final[0:3, 0:3] @ R_CORR_R
+
     # Elbow positions: basis change + hips subtraction (no arm convention transform)
     l_elbow_pos = None
     r_elbow_pos = None
@@ -639,7 +651,7 @@ def main():
     logger.info("Initializing IK solver (Pinocchio + CasADi)...")
     orig_cwd = os.getcwd()
     os.chdir(os.path.join(XR_TELEOP_DIR, "teleop"))
-    arm_ik = G1_29_ArmIK_Elbow(Unit_Test=False, Visualization=False)
+    arm_ik = G1_29_ArmIK_Elbow(elbow_weight=5.0, Unit_Test=False, Visualization=False)
     os.chdir(orig_cwd)
     logger.info("IK solver ready (14 DOF: 7 left arm + 7 right arm, elbow constraints)")
 
@@ -785,11 +797,21 @@ def main():
                     if retarget_error_count <= 5 or retarget_error_count % 100 == 0:
                         logger.warning(f"IK solve failed ({retarget_error_count}x): {e}")
 
-            # --- Hand retargeting --- (DISABLED: testing left arm only)
-            if False and hand_data and cal.calibrated:
+            # --- Hand retargeting ---
+            if hand_data and cal.calibrated:
                 left_hand_pos = tele_data.left_hand_pos
                 right_hand_pos = tele_data.right_hand_pos
 
+                # Debug: log hand data validity periodically
+                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)
+                    else:
+                        l_range = 0
+                    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:
                     left_hand_pos, right_hand_pos = right_hand_pos, left_hand_pos
 
@@ -800,6 +822,9 @@ def main():
                         l_hw_map, r_hw_map,
                         left_hand_pos, right_hand_pos)
                     current_hands = raw_hands * startup_ramp + (1.0 - startup_ramp) * 1.0
+                    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}]")
                 except Exception as e:
                     logger.warning(f"Hand retarget failed: {e}")