diff --git a/scripts/body_tracker.gd b/scripts/body_tracker.gd index 0458670..0a1dbdc 100644 --- a/scripts/body_tracker.gd +++ b/scripts/body_tracker.gd @@ -3,12 +3,12 @@ extends Node3D ## Computes chest-relative wrist positions and emits tracking data. ## Visualizes all body joints as colored spheres. ## -## Meta body tracking provides 70 joints. We use: -## CHEST (5) - torso orientation (solves body rotation problem) -## HEAD (7) - head pose -## LEFT_HAND_WRIST (12) - left wrist -## RIGHT_HAND_WRIST (17) - right wrist -## Hand joints (18-69) - finger positions +## Meta body tracking provides 70 joints (remapped to Godot enum). We use: +## CHEST (3) - torso orientation (solves body rotation problem) +## HEAD (6) - head pose +## LEFT_HAND (22) - left wrist +## RIGHT_HAND (49) - right wrist +## Hand joints (25-48, 52-75) - finger positions ## ## Output poses are in Godot's coordinate system (Y-up, -Z forward). ## The server handles conversion to robot conventions. @@ -16,37 +16,36 @@ extends Node3D ## Emitted every frame with tracking data dict ready to send. signal tracking_data_ready(data: Dictionary) -## Joint indices from XR_FB_body_tracking -## Reference: Meta OpenXR body tracking extension -const JOINT_ROOT := 0 -const JOINT_HIPS := 1 -const JOINT_SPINE_LOWER := 2 -const JOINT_SPINE_MIDDLE := 3 -const JOINT_SPINE_UPPER := 4 -const JOINT_CHEST := 5 -const JOINT_NECK := 6 -const JOINT_HEAD := 7 - -const JOINT_LEFT_SHOULDER := 8 -const JOINT_LEFT_SCAPULA := 9 -const JOINT_LEFT_ARM_UPPER := 10 -const JOINT_LEFT_ARM_LOWER := 11 -const JOINT_LEFT_HAND_WRIST := 12 - -const JOINT_RIGHT_SHOULDER := 13 -const JOINT_RIGHT_SCAPULA := 14 -const JOINT_RIGHT_ARM_UPPER := 15 -const JOINT_RIGHT_ARM_LOWER := 16 -const JOINT_RIGHT_HAND_WRIST := 17 - -# Hand joints start at index 18 for left hand, 43 for right hand -# 25 joints per hand (same layout as XR_EXT_hand_tracking) -const JOINT_LEFT_HAND_START := 18 -const JOINT_RIGHT_HAND_START := 43 +## Joint indices from Godot's XRBodyTracker.Joint enum +## NOTE: These are NOT the raw Meta XR_FB_body_tracking indices! +## The Godot OpenXR vendors plugin remaps Meta indices to Godot's enum. +## Reference: https://docs.godotengine.org/en/stable/classes/class_xrbodytracker.html +const JOINT_ROOT := 0 # XRBodyTracker.JOINT_ROOT +const JOINT_HIPS := 1 # XRBodyTracker.JOINT_HIPS +const JOINT_SPINE_LOWER := 2 # XRBodyTracker.JOINT_SPINE +const JOINT_CHEST := 3 # XRBodyTracker.JOINT_CHEST +const JOINT_UPPER_CHEST := 4 # XRBodyTracker.JOINT_UPPER_CHEST +const JOINT_NECK := 5 # XRBodyTracker.JOINT_NECK +const JOINT_HEAD := 6 # XRBodyTracker.JOINT_HEAD + +const JOINT_LEFT_SHOULDER := 8 # XRBodyTracker.JOINT_LEFT_SHOULDER +const JOINT_LEFT_ARM_UPPER := 9 # XRBodyTracker.JOINT_LEFT_UPPER_ARM +const JOINT_LEFT_ARM_LOWER := 10 # XRBodyTracker.JOINT_LEFT_LOWER_ARM +const JOINT_LEFT_HAND_WRIST := 22 # XRBodyTracker.JOINT_LEFT_HAND + +const JOINT_RIGHT_SHOULDER := 11 # XRBodyTracker.JOINT_RIGHT_SHOULDER +const JOINT_RIGHT_ARM_UPPER := 12 # XRBodyTracker.JOINT_RIGHT_UPPER_ARM +const JOINT_RIGHT_ARM_LOWER := 13 # XRBodyTracker.JOINT_RIGHT_LOWER_ARM +const JOINT_RIGHT_HAND_WRIST := 49 # XRBodyTracker.JOINT_RIGHT_HAND + +# Hand joints: Godot left hand fingers start at 25, right at 52 +# (25 joints per hand, same layout as XR_EXT_hand_tracking) +const JOINT_LEFT_HAND_START := 25 +const JOINT_RIGHT_HAND_START := 52 const HAND_JOINT_COUNT := 25 -## Total body joint count -const BODY_JOINT_COUNT := 70 +## Total body joint count (Godot XRBodyTracker.JOINT_MAX = 87) +const BODY_JOINT_COUNT := 87 ## Joint visualization colors const COLOR_BODY := Color(1.0, 0.7, 0.2, 1.0) # Orange - spine/torso @@ -87,11 +86,11 @@ func _create_joint_spheres() -> void: body_mesh.radius = 0.025 body_mesh.height = 0.05 - # Only create spheres for body joints (0-17), skip hand joints (18-69) + # Only create spheres for body/arm joints (0-21), skip hand joints (22+) # Hand joints are already visualized by vr_ui_pointer.gd _joint_spheres.resize(BODY_JOINT_COUNT) for i in range(BODY_JOINT_COUNT): - if i >= JOINT_LEFT_HAND_START: + if i >= JOINT_LEFT_HAND_WRIST: # 22+ = hand/finger joints _joint_spheres[i] = null continue var s := MeshInstance3D.new() @@ -106,17 +105,19 @@ func _create_joint_spheres() -> void: func _get_joint_color(joint_idx: int) -> Color: - if joint_idx == JOINT_HEAD: + if joint_idx == JOINT_HEAD: # 6 return COLOR_HEAD - elif joint_idx <= JOINT_NECK: + elif joint_idx <= JOINT_NECK: # 0-5 = body/spine return COLOR_BODY - elif joint_idx >= JOINT_LEFT_SHOULDER and joint_idx <= JOINT_LEFT_HAND_WRIST: + elif joint_idx >= JOINT_LEFT_SHOULDER and joint_idx <= JOINT_LEFT_ARM_LOWER: # 8-10 return COLOR_LEFT_ARM - elif joint_idx >= JOINT_RIGHT_SHOULDER and joint_idx <= JOINT_RIGHT_HAND_WRIST: + elif joint_idx >= JOINT_RIGHT_SHOULDER and joint_idx <= JOINT_RIGHT_ARM_LOWER: # 11-13 return COLOR_RIGHT_ARM - elif joint_idx >= JOINT_LEFT_HAND_START and joint_idx < JOINT_RIGHT_HAND_START: + elif joint_idx >= 14 and joint_idx <= 21: # legs + return COLOR_BODY + elif joint_idx >= JOINT_LEFT_HAND_WRIST and joint_idx < JOINT_RIGHT_HAND_WRIST: # 22-48 return COLOR_LEFT_HAND - elif joint_idx >= JOINT_RIGHT_HAND_START: + elif joint_idx >= JOINT_RIGHT_HAND_WRIST: # 49+ return COLOR_RIGHT_HAND return COLOR_BODY diff --git a/scripts/vr_ui_pointer.gd b/scripts/vr_ui_pointer.gd index 2022dee..ffaf628 100644 --- a/scripts/vr_ui_pointer.gd +++ b/scripts/vr_ui_pointer.gd @@ -41,9 +41,9 @@ const JOINT_COUNT := 26 const TIP_JOINTS := [5, 10, 15, 20, 25] const HAND_COLORS := [Color(0.3, 0.6, 1.0, 1.0), Color(0.3, 1.0, 0.6, 1.0)] -# XRBodyTracker fallback: hand joint start indices and count -const BODY_LEFT_HAND_START := 18 -const BODY_RIGHT_HAND_START := 43 +# XRBodyTracker fallback: hand joint start indices (Godot enum, NOT Meta indices) +const BODY_LEFT_HAND_START := 25 +const BODY_RIGHT_HAND_START := 52 const BODY_HAND_JOINT_COUNT := 25 # Fingertip indices within 25-joint body tracker hand block const BODY_TIP_JOINTS := [4, 9, 14, 19, 24] diff --git a/server/arm_ik_elbow.py b/server/arm_ik_elbow.py new file mode 100644 index 0000000..f0bafeb --- /dev/null +++ b/server/arm_ik_elbow.py @@ -0,0 +1,143 @@ +"""Extended G1 arm IK solver with elbow position constraints. + +Subclasses G1_29_ArmIK to add an elbow position cost term that resolves +the elbow ambiguity inherent in wrist-only IK. Uses l_lower/r_lower +positions from OpenXR body tracking as elbow targets. +""" + +import numpy as np +import casadi +import pinocchio as pin +from pinocchio import casadi as cpin + +from robot_arm_ik import G1_29_ArmIK + + +class G1_29_ArmIK_Elbow(G1_29_ArmIK): + """IK solver that constrains both wrist AND elbow positions. + + Cost function: + 50 * wrist_position² + rotation² + elbow_weight * elbow_position² + + 0.02 * regularization² + 0.1 * smoothing² + """ + + def __init__(self, elbow_weight=30.0, **kwargs): + super().__init__(**kwargs) + + # Find elbow frame IDs in the reduced model + self.L_elbow_id = self.reduced_robot.model.getFrameId("left_elbow_joint") + self.R_elbow_id = self.reduced_robot.model.getFrameId("right_elbow_joint") + + # Symbolic elbow target parameters + self.cElbow_l = casadi.SX.sym("elbow_l", 3, 1) + self.cElbow_r = casadi.SX.sym("elbow_r", 3, 1) + + # Elbow position error: FK(q) elbow pos - tracked elbow pos + self.elbow_error = casadi.Function( + "elbow_error", + [self.cq, self.cElbow_l, self.cElbow_r], + [casadi.vertcat( + self.cdata.oMf[self.L_elbow_id].translation - self.cElbow_l, + self.cdata.oMf[self.R_elbow_id].translation - self.cElbow_r, + )], + ) + + # Rebuild the optimization problem with elbow cost + self.opti = casadi.Opti() + self.var_q = self.opti.variable(self.reduced_robot.model.nq) + self.var_q_last = self.opti.parameter(self.reduced_robot.model.nq) + self.param_tf_l = self.opti.parameter(4, 4) + self.param_tf_r = self.opti.parameter(4, 4) + self.param_elbow_l = self.opti.parameter(3, 1) + self.param_elbow_r = self.opti.parameter(3, 1) + + # Re-evaluate cost functions with new opti variables + self.translational_cost = casadi.sumsqr( + self.translational_error(self.var_q, self.param_tf_l, self.param_tf_r)) + self.rotation_cost = casadi.sumsqr( + self.rotational_error(self.var_q, self.param_tf_l, self.param_tf_r)) + self.regularization_cost = casadi.sumsqr(self.var_q) + self.smooth_cost = casadi.sumsqr(self.var_q - self.var_q_last) + self.elbow_cost = casadi.sumsqr( + self.elbow_error(self.var_q, self.param_elbow_l, self.param_elbow_r)) + + # Joint limits + self.opti.subject_to(self.opti.bounded( + self.reduced_robot.model.lowerPositionLimit, + self.var_q, + self.reduced_robot.model.upperPositionLimit) + ) + + # Combined objective + self.opti.minimize( + 50 * self.translational_cost + + self.rotation_cost + + 0.02 * self.regularization_cost + + 0.1 * self.smooth_cost + + elbow_weight * self.elbow_cost + ) + + opts = { + 'expand': True, + 'detect_simple_bounds': True, + 'calc_lam_p': False, + 'print_time': False, + 'ipopt.sb': 'yes', + 'ipopt.print_level': 0, + 'ipopt.max_iter': 30, + 'ipopt.tol': 1e-4, + 'ipopt.acceptable_tol': 5e-4, + 'ipopt.acceptable_iter': 5, + 'ipopt.warm_start_init_point': 'yes', + 'ipopt.derivative_test': 'none', + 'ipopt.jacobian_approximation': 'exact', + } + self.opti.solver("ipopt", opts) + + def solve_ik(self, left_wrist, right_wrist, + current_lr_arm_motor_q=None, current_lr_arm_motor_dq=None, + left_elbow_pos=None, right_elbow_pos=None): + """Solve IK with wrist + elbow position targets. + + Args: + left_wrist: 4x4 SE(3) left wrist target + right_wrist: 4x4 SE(3) right wrist target + current_lr_arm_motor_q: current 14-DOF arm angles (warm start) + current_lr_arm_motor_dq: current velocities (API compat, unused) + left_elbow_pos: 3-vector left elbow position (waist frame) + right_elbow_pos: 3-vector right elbow position (waist frame) + + Returns: + (sol_q, sol_tauff) — 14-DOF joint angles and feedforward torques + """ + if current_lr_arm_motor_q is not None: + self.init_data = current_lr_arm_motor_q + self.opti.set_initial(self.var_q, self.init_data) + + self.opti.set_value(self.param_tf_l, left_wrist) + self.opti.set_value(self.param_tf_r, right_wrist) + self.opti.set_value(self.var_q_last, self.init_data) + + self.opti.set_value(self.param_elbow_l, + left_elbow_pos if left_elbow_pos is not None else np.zeros(3)) + self.opti.set_value(self.param_elbow_r, + right_elbow_pos if right_elbow_pos is not None else np.zeros(3)) + + try: + sol = self.opti.solve() + sol_q = self.opti.value(self.var_q) + except Exception as e: + sol_q = self.opti.debug.value(self.var_q) + + self.smooth_filter.add_data(sol_q) + sol_q = self.smooth_filter.filtered_data + + v = (current_lr_arm_motor_dq * 0.0 if current_lr_arm_motor_dq is not None + else (sol_q - self.init_data) * 0.0) + + self.init_data = sol_q + sol_tauff = pin.rnea( + self.reduced_robot.model, self.reduced_robot.data, + sol_q, v, np.zeros(self.reduced_robot.model.nv)) + + return sol_q, sol_tauff diff --git a/server/retarget_bridge.py b/server/retarget_bridge.py index 6547bad..8558707 100644 --- a/server/retarget_bridge.py +++ b/server/retarget_bridge.py @@ -53,10 +53,16 @@ if not hasattr(dex_retargeting, 'RetargetingConfig'): from dex_retargeting.retargeting_config import RetargetingConfig dex_retargeting.RetargetingConfig = RetargetingConfig +# IK solver from upstream xr_teleoperate +sys.path.insert(0, os.path.join(XR_TELEOP_DIR, "teleop", "robot_control")) +from arm_ik_elbow import G1_29_ArmIK_Elbow + # Local imports from teleop_server import TeleopServer from native_tv_wrapper import (T_ROBOT_OPENXR, T_OPENXR_ROBOT, T_TO_UNITREE_HAND, + T_TO_UNITREE_HUMANOID_LEFT_ARM, + T_TO_UNITREE_HUMANOID_RIGHT_ARM, NativeTeleWrapper) logger = logging.getLogger("retarget_bridge") @@ -142,23 +148,8 @@ R3_OPENXR_ROBOT = T_OPENXR_ROBOT[:3, :3] JOINT_SCALE = np.ones(17) JOINT_OFFSET = np.zeros(17) -# l_shoulder_pitch (index 3): XR range [-0.4, +2.3] -> robot range [-2.4, +0.8] -JOINT_SCALE[3] = 1.2 -JOINT_OFFSET[3] = -1.95 - -# l_shoulder_roll (index 4): XR Z range [+0.06, +1.05] -> robot range [0, +1.57] -JOINT_SCALE[4] = 1.6 -JOINT_OFFSET[4] = -0.1 - -# l_shoulder_yaw (index 5): XR -X range [-0.24, +0.93] -> robot range [-1.14, +1.14] -JOINT_SCALE[5] = 2.0 -JOINT_OFFSET[5] = -0.7 - -# l_elbow (index 6): XR Z range [-0.67, +0.97] -> robot range [-0.78, +1.31] -JOINT_SCALE[6] = 1.3 -JOINT_OFFSET[6] = 0.0 - -# TODO: calibrate remaining joints (wrist, right arm) +# All arm joints use IK solver output directly — no scaling needed +# JOINT_SCALE stays at 1.0 for all joints # Smoothing parameters EMA_ALPHA = 0.4 # Exponential moving average (0=no update, 1=no smoothing) @@ -203,6 +194,55 @@ def all_joints_valid(bj): 'r_upper', 'r_lower', 'r_wrist']) +def build_ik_targets(bj): + """Build IK-ready wrist SE(3) poses + elbow positions from body tracking. + + Uses hips as waist reference. Wrist rotation is preserved (from XRHandTracker) + to help the IK solver resolve elbow ambiguity. Elbow positions come from + XRBodyTracker l_lower/r_lower joints. + + Args: + bj: dict from parse_body_joints() — world-space 4x4 matrices + + Returns: + (left_wrist_4x4, right_wrist_4x4, left_elbow_pos, right_elbow_pos) + Wrists are 4x4 SE(3) in robot waist frame. + Elbows are 3-vectors (xyz) in robot waist frame. + """ + l_wrist_world = bj['l_wrist'] + r_wrist_world = bj['r_wrist'] + hips_world = bj['hips'] + + # Basis change: OpenXR → Robot convention + hips_robot = T_ROBOT_OPENXR @ hips_world @ T_OPENXR_ROBOT + left_Brobot_world = T_ROBOT_OPENXR @ l_wrist_world @ T_OPENXR_ROBOT + right_Brobot_world = T_ROBOT_OPENXR @ r_wrist_world @ T_OPENXR_ROBOT + + # Arm URDF convention transform (for wrists only — adjusts end-effector frame) + left_IPunitree = left_Brobot_world @ T_TO_UNITREE_HUMANOID_LEFT_ARM + right_IPunitree = right_Brobot_world @ T_TO_UNITREE_HUMANOID_RIGHT_ARM + + # Hips subtraction (translation only — makes wrist relative to waist) + hips_pos = hips_robot[0:3, 3] + + left_wrist_final = left_IPunitree.copy() + right_wrist_final = right_IPunitree.copy() + left_wrist_final[0:3, 3] -= hips_pos + right_wrist_final[0:3, 3] -= hips_pos + + # Elbow positions: basis change + hips subtraction (no arm convention transform) + l_elbow_pos = None + r_elbow_pos = None + if bj['l_lower'] is not None: + l_lower_robot = T_ROBOT_OPENXR @ bj['l_lower'] @ T_OPENXR_ROBOT + l_elbow_pos = l_lower_robot[0:3, 3] - hips_pos + if bj['r_lower'] is not None: + r_lower_robot = T_ROBOT_OPENXR @ bj['r_lower'] @ T_OPENXR_ROBOT + r_elbow_pos = r_lower_robot[0:3, 3] - hips_pos + + return left_wrist_final, right_wrist_final, l_elbow_pos, r_elbow_pos + + # --------------------------------------------------------------------------- # Calibration: captures neutral pose reference rotations # --------------------------------------------------------------------------- @@ -218,10 +258,10 @@ class CalibrationData: def __init__(self): self.calibrated = False - # Reference relative rotations (3x3 matrices) in XR parent-local frame - self.ref_waist = None # hips.T @ chest - self.ref_shoulder_L = None # chest.T @ l_upper - self.ref_shoulder_R = None # chest.T @ r_upper + self.ref_hips_rot = None # frozen hips rotation + self.ref_chest_rot = None # calibration-time chest rotation + self.ref_shoulder_L = None # chest.T @ l_upper (chest-relative) + self.ref_shoulder_R = None # chest.T @ r_upper (chest-relative) self.ref_elbow_L = None # l_upper.T @ l_lower self.ref_elbow_R = None # r_upper.T @ r_lower self.ref_wrist_L = None # l_lower.T @ l_wrist @@ -239,9 +279,10 @@ class CalibrationData: if not all_joints_valid(bj): return False - self.ref_waist = bj['hips'][:3, :3].T @ bj['chest'][:3, :3] - self.ref_shoulder_L = bj['hips'][:3, :3].T @ bj['l_upper'][:3, :3] - self.ref_shoulder_R = bj['hips'][:3, :3].T @ bj['r_upper'][:3, :3] + self.ref_hips_rot = bj['hips'][:3, :3].copy() + self.ref_chest_rot = bj['chest'][:3, :3].copy() + self.ref_shoulder_L = self.ref_hips_rot.T @ bj['l_upper'][:3, :3] + self.ref_shoulder_R = self.ref_hips_rot.T @ bj['r_upper'][:3, :3] self.ref_elbow_L = bj['l_upper'][:3, :3].T @ bj['l_lower'][:3, :3] self.ref_elbow_R = bj['r_upper'][:3, :3].T @ bj['r_lower'][:3, :3] self.ref_wrist_L = bj['l_lower'][:3, :3].T @ bj['l_wrist'][:3, :3] @@ -276,6 +317,7 @@ def _joint_delta_robot(R_ref, R_cur): return R3_ROBOT_OPENXR @ R_delta @ R3_ROBOT_OPENXR.T +_last_dbg = {} # Raw euler values for debug logging SOLO_JOINT = None # Set by --solo-joint arg; None = all joints active # Map solo-joint names to indices within the 17-element angles array @@ -310,40 +352,42 @@ def compute_all_angles(bj, cal): """ angles = np.zeros(17) # indices 0-16 map to joints 12-28 - # Axis mappings determined empirically via joint_calibrate.py: - # - # After R_basis @ R_delta @ R_basis.T, as_euler('YXZ') returns (Y, X, Z). - # The mapping to robot joints depends on the parent's orientation: - # - # Shoulder (parent=chest, upright): pitch=Y[0], roll=Z[2], yaw=-X[1] - # Elbow (parent=upper_arm, hanging): pitch=Z[2] - # Waist (parent=hips, upright): assumed same pattern as shoulder - # Wrist (parent=lower_arm, hanging): assumed same pattern as elbow - # --- Waist (0-2): DISABLED (not calibrated) --- # angles[0:3] = [0, 0, 0] - # --- Left shoulder (3-5): hips -> l_upper --- - # Using hips instead of chest to avoid head rotation contamination - # Parent (hips) is upright: pitch=Y, roll=Z, yaw=-X - R_sh_L_cur = bj['hips'][:3, :3].T @ bj['l_upper'][:3, :3] + # --- Left shoulder (3-5): frozen_hips -> l_upper --- + R_sh_L_cur = cal.ref_hips_rot.T @ bj['l_upper'][:3, :3] R_sd_L = _joint_delta_robot(cal.ref_shoulder_L, R_sh_L_cur) sy, sx, sz = Rotation.from_matrix(R_sd_L).as_euler('YXZ') - angles[3:6] = [sy, sz, -sx] # pitch=Y, roll=Z, yaw=-X # --- Left elbow (6): l_upper -> l_lower --- - # Parent (upper arm) hangs down: pitch=Z R_el_L_cur = bj['l_upper'][:3, :3].T @ bj['l_lower'][:3, :3] R_ed_L = _joint_delta_robot(cal.ref_elbow_L, R_el_L_cur) - _, _, ez_L = Rotation.from_matrix(R_ed_L).as_euler('YXZ') - angles[6] = ez_L # pitch = Z component + ey_L, ex_L, ez_L = Rotation.from_matrix(R_ed_L).as_euler('YXZ') + + # Full matrix mapping (from pose_calibrate.py 2026-02-28) + # Solves cross-axis coupling + scales in one step + # robot = M @ [sy, sx, sz, ez] + M_LEFT = np.array([ + [ +5.4026, -1.7709, +1.0419, -6.7338], # pitch + [ +4.4651, -0.0869, +2.8320, -5.4197], # roll + [ +3.1454, -1.6180, +2.3173, -3.9653], # yaw + [ +0.6962, +1.7176, +1.5304, +1.0564], # elbow + ]) + xr_vec = np.array([sy, sx, sz, ez_L]) + pitch, roll, yaw, elbow = M_LEFT @ xr_vec + angles[3:6] = [pitch, roll, yaw] + angles[6] = elbow + + _last_dbg['sh_YXZ'] = (sy, sx, sz) + _last_dbg['el_YXZ'] = (ey_L, ex_L, ez_L) # --- Left wrist (7-9): l_lower -> l_wrist --- # Parent (lower arm) hangs down: same pattern as elbow (Z-dominant) R_wr_L_cur = bj['l_lower'][:3, :3].T @ bj['l_wrist'][:3, :3] R_wrd_L = _joint_delta_robot(cal.ref_wrist_L, R_wr_L_cur) wy_wl, wx_wl, wz_wl = Rotation.from_matrix(R_wrd_L).as_euler('YXZ') - angles[7:10] = [wz_wl, wy_wl, -wx_wl] # roll=Z, pitch=Y, yaw=-X (tentative) + angles[7:10] = [0.0, 0.0, 0.0] # all wrist joints disabled for now (noisy) # --- Right arm (10-16): DISABLED (not calibrated, Quest tracking weak) --- # angles[10:17] = [0, 0, 0, 0, 0, 0, 0] @@ -591,6 +635,14 @@ def main(): if hand_data: l_retarget, r_retarget, l_indices, r_indices, l_hw_map, r_hw_map = hand_data + # --- Initialize IK solver --- + 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) + os.chdir(orig_cwd) + logger.info("IK solver ready (14 DOF: 7 left arm + 7 right arm, elbow constraints)") + # --- Control loop state --- interval = 1.0 / args.hz max_step = MAX_RATE * interval # rad per step @@ -598,7 +650,7 @@ def main(): current_angles = np.zeros(NUM_JOINTS) smoothed_upper = np.zeros(17) # joints 12-28 current_hands = np.ones(12) - tracking_timeout = 0.5 + tracking_timeout = 2.0 # seconds without data before considering tracking lost return_to_zero_rate = 0.02 last_valid_time = 0.0 startup_ramp = 0.0 @@ -619,11 +671,11 @@ def main(): signal.signal(signal.SIGINT, signal_handler) signal.signal(signal.SIGTERM, signal_handler) - logger.info(f"Bridge v4 running at {args.hz} Hz (DIRECT retargeting, no IK)") + logger.info(f"Bridge v5 running at {args.hz} Hz (IK retargeting via Pinocchio)") logger.info(f" Waist={'ON' if not args.no_waist else 'OFF'}, " f"Arms={'ON' if not args.no_arms else 'OFF'}, " f"Hands={'ON' if hand_data else 'OFF'}") - logger.info(f" Auto-calibration delay: {args.cal_delay}s after first tracking frame") + logger.info(f" Startup delay: {args.cal_delay}s after first tracking frame") logger.info("Waiting for Quest 3...") while running: @@ -650,12 +702,7 @@ def main(): # Detect reconnection: tracking resumed after being lost if not was_tracking and cal.calibrated: - logger.info("Tracking resumed after disconnect — RESETTING calibration") - cal = CalibrationData() - first_data_time = now - smoothed_upper = np.zeros(17) - startup_ramp = 0.0 - startup_time = None + logger.info("Tracking resumed — keeping existing calibration") was_tracking = True @@ -664,34 +711,61 @@ def main(): elapsed_startup = now - startup_time startup_ramp = min(elapsed_startup / startup_ramp_duration, 1.0) - # --- Parse body joints --- - if has_body_joints: - bj = parse_body_joints(bj_raw) - - # --- Auto-calibrate after delay --- - if not cal.calibrated and first_data_time is not None: - if (now - first_data_time) >= args.cal_delay: - if all_joints_valid(bj): - if cal.capture(bj): - logger.info("CALIBRATED — neutral pose captured. Starting retargeting.") - # Log calibration reference quaternions for debugging - l_up = bj_raw[BJ_L_UPPER*7:(BJ_L_UPPER+1)*7] - r_up = bj_raw[BJ_R_UPPER*7:(BJ_R_UPPER+1)*7] - logger.info(f" Cal ref L_upper quat=[{l_up[3]:.4f},{l_up[4]:.4f},{l_up[5]:.4f},{l_up[6]:.4f}]") - logger.info(f" Cal ref R_upper quat=[{r_up[3]:.4f},{r_up[4]:.4f},{r_up[5]:.4f},{r_up[6]:.4f}]") - startup_time = now - else: - logger.warning("Calibration capture failed, retrying next frame...") - else: - missing = [k for k in ['hips','chest','l_upper','l_lower','l_wrist', - 'r_upper','r_lower','r_wrist'] if bj[k] is None] - if step_count % 50 == 0: - logger.info(f"Waiting for all body joints (missing: {missing})") - - # --- Retarget body joints --- - if cal.calibrated and all_joints_valid(bj): - try: - raw_angles = compute_all_angles(bj, cal) + # --- Start IK retargeting after delay --- + if not cal.calibrated and first_data_time is not None: + if (now - first_data_time) >= args.cal_delay: + cal.calibrated = True # IK doesn't need body joint calibration + logger.info("Startup delay complete — IK retargeting active.") + startup_time = now + + # --- IK retarget: wrist poses -> joint angles --- + if cal.calibrated and has_body_joints: + try: + bj = parse_body_joints(bj_raw) + if (bj['l_wrist'] is not None and bj['r_wrist'] is not None + and bj['hips'] is not None): + # Build IK targets: wrist SE(3) + elbow positions + lw, rw, l_elbow, r_elbow = build_ik_targets(bj) + + # Debug: log IK targets every 250 steps + if step_count % 250 == 0: + l_elb_str = f"[{l_elbow[0]:+.3f},{l_elbow[1]:+.3f},{l_elbow[2]:+.3f}]" if l_elbow is not None else "None" + r_elb_str = f"[{r_elbow[0]:+.3f},{r_elbow[1]:+.3f},{r_elbow[2]:+.3f}]" if r_elbow is not None else "None" + # Raw OpenXR positions (Y-up, -Z forward) + lw_raw = bj['l_wrist'][0:3, 3] if bj['l_wrist'] is not None else np.zeros(3) + rw_raw = bj['r_wrist'][0:3, 3] if bj['r_wrist'] is not None else np.zeros(3) + le_raw = bj['l_lower'][0:3, 3] if bj['l_lower'] is not None else np.zeros(3) + re_raw = bj['r_lower'][0:3, 3] if bj['r_lower'] is not None else np.zeros(3) + hips_raw = bj['hips'][0:3, 3] if bj['hips'] is not None else np.zeros(3) + logger.info(f" [RAW OpenXR] hips=[{hips_raw[0]:+.3f},{hips_raw[1]:+.3f},{hips_raw[2]:+.3f}]") + logger.info(f" [RAW OpenXR] L_wrist=[{lw_raw[0]:+.3f},{lw_raw[1]:+.3f},{lw_raw[2]:+.3f}] " + f"L_elbow=[{le_raw[0]:+.3f},{le_raw[1]:+.3f},{le_raw[2]:+.3f}]") + logger.info(f" [RAW OpenXR] R_wrist=[{rw_raw[0]:+.3f},{rw_raw[1]:+.3f},{rw_raw[2]:+.3f}] " + f"R_elbow=[{re_raw[0]:+.3f},{re_raw[1]:+.3f},{re_raw[2]:+.3f}]") + logger.info(f" [IK INPUT] L_wrist pos=[{lw[0,3]:+.3f},{lw[1,3]:+.3f},{lw[2,3]:+.3f}] " + f"R_wrist pos=[{rw[0,3]:+.3f},{rw[1,3]:+.3f},{rw[2,3]:+.3f}]") + logger.info(f" [IK INPUT] L_elbow={l_elb_str} R_elbow={r_elb_str}") + + # Get current arm joint angles for warm-starting the IK solver + current_arm_q = np.concatenate([ + current_angles[15:22], # left arm (joints 15-21) + current_angles[22:29], # right arm (joints 22-28) + ]) + current_arm_dq = np.zeros(14) + + sol_q, sol_tauff = arm_ik.solve_ik( + lw, rw, + current_arm_q, + current_arm_dq, + left_elbow_pos=l_elbow, + right_elbow_pos=r_elbow, + ) + + # Map 14-DOF IK solution to 17-element upper body array + raw_angles = np.zeros(17) + raw_angles[3:10] = sol_q[:7] # left arm + raw_angles[10:17] = sol_q[7:] # right arm + raw_angles = apply_limits(raw_angles) # Zero out disabled groups @@ -706,10 +780,10 @@ def main(): # Apply startup ramp and write to output current_angles[12:29] = smoothed_upper * startup_ramp - except Exception as e: - retarget_error_count += 1 - if retarget_error_count <= 5 or retarget_error_count % 100 == 0: - logger.warning(f"Retarget failed ({retarget_error_count}x): {e}") + except Exception as e: + retarget_error_count += 1 + 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: @@ -765,6 +839,12 @@ def main(): f"waist=[{waist[0]:+.2f},{waist[1]:+.2f},{waist[2]:+.2f}] " f"L_sh=[{l_sh[0]:+.2f},{l_sh[1]:+.2f},{l_sh[2]:+.2f}] L_el={l_el:+.2f} " f"R_sh=[{r_sh[0]:+.2f},{r_sh[1]:+.2f},{r_sh[2]:+.2f}] R_el={r_el:+.2f}") + if tracking and cal.calibrated: + l_wr = current_angles[19:22] + r_wr = current_angles[26:29] + logger.info( + f" L_wr=[{l_wr[0]:+.2f},{l_wr[1]:+.2f},{l_wr[2]:+.2f}] " + f"R_wr=[{r_wr[0]:+.2f},{r_wr[1]:+.2f},{r_wr[2]:+.2f}]") if args.verbose and tracking and has_body_joints: logger.debug( f" Full upper body: {np.array2string(current_angles[12:29], precision=3, separator=',')}")