diff --git a/scripts/body_tracker.gd b/scripts/body_tracker.gd index 0a1dbdc..bdb6eff 100644 --- a/scripts/body_tracker.gd +++ b/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() diff --git a/server/retarget_bridge.py b/server/retarget_bridge.py index 8558707..1e0bc61 100644 --- a/server/retarget_bridge.py +++ b/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}")