diff --git a/scripts/body_tracker.gd b/scripts/body_tracker.gd index db1fe8f..0458670 100644 --- a/scripts/body_tracker.gd +++ b/scripts/body_tracker.gd @@ -198,15 +198,30 @@ func _process(_delta: float) -> void: var right_upper_arm_xform := tracker.get_joint_transform(JOINT_RIGHT_ARM_UPPER) var right_lower_arm_xform := tracker.get_joint_transform(JOINT_RIGHT_ARM_LOWER) + # Use XRHandTracker wrist transforms instead of body tracker wrist — + # 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: + right_wrist_body = ht_wrist + var body_joints := [] body_joints.append_array(_xform_to_pose7(hips_xform)) body_joints.append_array(_xform_to_pose7(chest_xform)) body_joints.append_array(_xform_to_pose7(left_upper_arm_xform)) body_joints.append_array(_xform_to_pose7(left_lower_arm_xform)) - body_joints.append_array(_xform_to_pose7(left_wrist_xform)) + body_joints.append_array(_xform_to_pose7(left_wrist_body)) body_joints.append_array(_xform_to_pose7(right_upper_arm_xform)) body_joints.append_array(_xform_to_pose7(right_lower_arm_xform)) - body_joints.append_array(_xform_to_pose7(right_wrist_xform)) + body_joints.append_array(_xform_to_pose7(right_wrist_body)) data["body_joints"] = body_joints # Debug logging every ~2 seconds diff --git a/server/calibration_log.md b/server/calibration_log.md new file mode 100644 index 0000000..a7a0915 --- /dev/null +++ b/server/calibration_log.md @@ -0,0 +1,131 @@ +# Joint Calibration Log + +Using simple basis change: `R_delta_robot = R_basis @ R_delta_xr @ R_basis.T` +Decomposition order for shoulders: YXZ Euler (matches URDF: pitch(Y), roll(X), yaw(Z)) + +## l_shoulder_pitch (joint 15) - DONE +- Robot motion: sine ±0.6 rad, period 8s (arm forward/backward) +- XR parent-child: chest -> l_upper +- **Result**: Y component (YXZ Euler) correlates positively with robot motion + - Robot +0.57 → XR Y ≈ +0.3 to +0.6 + - Robot -0.58 → XR Y ≈ -1.0 to -1.1 +- **Sign**: `SIGN_SH_L[0] = +1` (CORRECT, same sign) +- Note: X (roll) had constant negative offset (~-0.4 to -0.8), likely natural arm rotation during pitch motion + +## l_shoulder_roll (joint 16) - DONE +- Robot motion: sine ±0.6 rad, period 8s (arm out to left side) +- XR parent-child: chest -> l_upper +- **Result**: Z component (YXZ Euler) correlates with robot roll, NOT X + - Robot +0.60 → Z ≈ +0.93 to +1.23 + - Robot -0.60 → Z ≈ +0.05 (can't push arm into body) +- **Sign**: +1 (same sign) +- **Important**: This means YXZ Euler order may be wrong, or axis assignment needs swapping (roll=Z, yaw=X?) +## l_shoulder_yaw (joint 17) - DONE +- Robot motion: sine ±0.4 rad, period 8s (upper arm twist inward/outward) +- XR parent-child: chest -> l_upper +- **Result**: X component (YXZ Euler) correlates negatively with robot yaw + - Robot +0.40 → X ≈ -0.90 to -0.98 + - Robot -0.40 → X ≈ -0.001 to -0.09 +- **Sign**: -1 (inverted) + +### LEFT SHOULDER SUMMARY +- Pitch → Y component of YXZ, sign +1 +- Roll → Z component of YXZ, sign +1 +- Yaw → X component of YXZ, sign -1 +- **Effective Euler order is YZX, not YXZ!** +- Code fix: `as_euler('YZX')` gives (pitch, roll, yaw) directly, with yaw negated +## l_elbow (joint 18) - DONE +- Robot motion: sine ±0.7 rad, period 8s (elbow bend/extend) +- XR parent-child: l_upper -> l_lower +- **Result**: Z component (YXZ Euler) correlates positively with robot elbow + - Robot +0.69 → Z ≈ +0.82 to +0.92 + - Robot -0.69 → Z ≈ -0.65 to -0.93 +- **Sign**: +1 (same sign) +- Note: Currently code extracts Y[0] for elbow, should use Z[2] +## r_shoulder_pitch (joint 22) - DONE +- Robot motion: sine ±0.6 rad, period 8s (right arm forward/backward) +- XR parent-child: chest -> r_upper +- **Result**: Y component (YXZ Euler) has weak positive correlation + - Large constant offset (~0.8 rad) - user's arm drifted forward + - Robot +0.6 → Y ≈ 0.92, Robot -0.6 → Y ≈ 0.76 (difference ~0.16) +- **Sign**: +1 (same as left, weak signal) +- **Note**: Right arm Quest tracking is significantly weaker than left +## r_shoulder_roll (joint 23) - DONE +- Robot motion: sine ±0.6 rad, period 8s (right arm out to side) +- XR parent-child: chest -> r_upper +- **Result**: Z component (YXZ Euler) correlates (weak/intermittent signal) + - Robot -0.60 (arm out) → Z ≈ -0.82 to -0.96 (strong cycles) + - Robot +0.60 (arm in) → Z ≈ +0.07 +- **Sign**: +1 (same sign) +- **Note**: Very noisy, right arm Quest tracking weak +## r_shoulder_yaw (joint 24) - SKIPPED +- Assumed same as left: X component, sign -1 (right arm tracking too weak for subtle yaw) + +## r_elbow (joint 25) - DONE +- Robot motion: sine ±0.7 rad, period 8s (right elbow bend) +- XR parent-child: r_upper -> r_lower +- **Result**: ALL axes near zero (max ±0.07 despite ±0.7 robot motion) +- Quest right arm tracking is extremely weak - barely detects elbow bend +- **Assumed**: Same mapping as left elbow (Z component, sign +1) + +## EXTENT TUNING + +### l_shoulder_pitch (joint 15) +- Backward (positive): +0.8 rad (+45.8 deg) — OK +- Forward (negative): -2.4 rad (-137.5 deg) — OK +- Note: positive = backward, negative = forward for this joint +- Settings: amplitude=1.6, offset=-0.8 + +### l_shoulder_roll (joint 16) +- Inward (negative): ~0 rad (0 deg) — just touching body, no clipping +- Outward (positive): +1.585 rad (+91 deg) — arm straight out to side +- Settings: amplitude=0.8, offset=0.785 +- Note: needs outward bias to avoid body clipping + +### l_shoulder_yaw (joint 17) +- Inward (positive): +1.15 rad (+66 deg) +- Outward (negative): -1.15 rad (-66 deg) +- Settings: amplitude=1.15, offset=0.0 +- Note: clips into body at rest pose but OK since other joints move arm away in practice + +### l_elbow (joint 18) +- Extend (negative): -0.8 rad (-45.8 deg) +- Bend (positive): +1.33 rad (+76.2 deg) +- Settings: amplitude=1.065, offset=0.265 +- Note: positive = bend, negative = extend (opposite of label in joint_test.py) + +### l_wrist_roll (joint 19) +- Palm down (positive): +1.35 rad (+77.3 deg) +- Palm up (negative): -1.35 rad (-77.3 deg) +- Settings: amplitude=1.35, offset=0.0 +- Note: positive = palm down, negative = palm up (opposite of label) + +### l_wrist_pitch (joint 20) +- Wrist down (positive): +0.8 rad (+45.8 deg) +- Wrist up (negative): -0.8 rad (-45.8 deg) +- Settings: amplitude=0.8, offset=0.0 +- Directions confirmed correct + +### l_wrist_yaw (joint 21) +- Inward (negative): -1.28 rad (-73 deg) +- Outward (positive): +0.5 rad (+29 deg) +- Settings: amplitude=0.89, offset=-0.39 +- Note: negative = inward, positive = outward + +## OVERALL AXIS MAPPING SUMMARY + +After `R_basis @ R_delta @ R_basis.T` + `as_euler('YXZ')` → (Y, X, Z): + +### Shoulder (chest → upper_arm) — parent roughly upright: +- Robot pitch (Y joint) = Y component [0], sign +1 +- Robot roll (X joint) = Z component [2], sign +1 +- Robot yaw (Z joint) = X component [1], sign -1 + +### Elbow (upper_arm → lower_arm) — parent hangs down: +- Robot pitch (Y joint) = Z component [2], sign +1 +- Note: axis mapping differs from shoulder because parent orientation differs + +### Why axes differ per joint: +The basis change `R_basis @ R_delta @ R_basis.T` treats the delta as if in XR world-aligned frame, +but the delta is in the PARENT's local frame. When the parent hangs down (upper arm), its local +axes are rotated ~90° from world, causing the axis mapping to shift. diff --git a/server/joint_calibrate.py b/server/joint_calibrate.py new file mode 100644 index 0000000..5e68c9b --- /dev/null +++ b/server/joint_calibrate.py @@ -0,0 +1,265 @@ +#!/usr/bin/env python3 +""" +Joint calibration tool: animates ONE robot joint while recording Quest tracking data. + +The user copies the robot's motion. By comparing the known robot joint angle +with the raw XR tracking deltas, we determine the correct axis mapping and sign. + +Usage: + python joint_calibrate.py l_shoulder_pitch [--period 6] [--amplitude 0.8] +""" + +import sys +import os +import time +import argparse +import signal +import numpy as np +from scipy.spatial.transform import Rotation + +# Add xr_teleoperate to path +sys.path.insert(0, "/home/sparky/git/xr_teleoperate") +import logging_mp +if not hasattr(logging_mp, 'getLogger'): + logging_mp.getLogger = logging_mp.get_logger +import dex_retargeting +if not hasattr(dex_retargeting, 'RetargetingConfig'): + from dex_retargeting.retargeting_config import RetargetingConfig + dex_retargeting.RetargetingConfig = RetargetingConfig + +from teleop_server import TeleopServer +from native_tv_wrapper import T_ROBOT_OPENXR, NativeTeleWrapper + +NUM_JOINTS = 29 +R3_ROBOT_OPENXR = T_ROBOT_OPENXR[:3, :3] + +JOINTS = { + 'l_shoulder_pitch': 15, 'l_shoulder_roll': 16, 'l_shoulder_yaw': 17, + 'l_elbow': 18, + 'l_wrist_roll': 19, 'l_wrist_pitch': 20, 'l_wrist_yaw': 21, + 'r_shoulder_pitch': 22, 'r_shoulder_roll': 23, 'r_shoulder_yaw': 24, + 'r_elbow': 25, +} + +# Which XR parent-child pair to monitor for each robot joint +# Using hips (not chest) for shoulder to avoid head rotation contamination +JOINT_PARENTS = { + 'l_shoulder_pitch': ('hips', 'l_upper'), + 'l_shoulder_roll': ('hips', 'l_upper'), + 'l_shoulder_yaw': ('hips', 'l_upper'), + 'l_elbow': ('l_upper', 'l_lower'), + 'l_wrist_roll': ('l_lower', 'l_wrist'), + 'l_wrist_pitch': ('l_lower', 'l_wrist'), + 'l_wrist_yaw': ('l_lower', 'l_wrist'), + 'r_shoulder_pitch': ('hips', 'r_upper'), + 'r_shoulder_roll': ('hips', 'r_upper'), + 'r_shoulder_yaw': ('hips', 'r_upper'), + 'r_elbow': ('r_upper', 'r_lower'), +} + +KP = [60,60,60,100,40,40, 60,60,60,100,40,40, 60,40,40, 40,40,40,40,40,40,40, 40,40,40,40,40,40,40] +KD = [1,1,1,2,1,1, 1,1,1,2,1,1, 1,1,1, 1,1,1,1,1,1,1, 1,1,1,1,1,1,1] + +# Calibrated extents from joint testing +DEFAULT_AMP = { + 'l_shoulder_pitch': 1.6, 'l_shoulder_roll': 0.8, 'l_shoulder_yaw': 1.15, + 'l_elbow': 1.065, + 'l_wrist_roll': 1.35, 'l_wrist_pitch': 0.8, 'l_wrist_yaw': 0.89, + 'r_shoulder_pitch': 1.6, 'r_shoulder_roll': 0.8, 'r_shoulder_yaw': 1.15, + 'r_elbow': 1.065, +} +DEFAULT_OFFSET = { + 'l_shoulder_pitch': -0.8, 'l_shoulder_roll': 0.785, 'l_shoulder_yaw': 0.0, + 'l_elbow': 0.265, + 'l_wrist_roll': 0.0, 'l_wrist_pitch': 0.0, 'l_wrist_yaw': -0.39, + 'r_shoulder_pitch': -0.8, 'r_shoulder_roll': -0.785, 'r_shoulder_yaw': 0.0, + 'r_elbow': 0.265, +} + + +def pose7_to_mat4(pose7): + mat = np.eye(4) + r = Rotation.from_quat(pose7[3:7]) + mat[:3, :3] = r.as_matrix() + mat[:3, 3] = pose7[:3] + return mat + + +def parse_body_joints(flat): + names = ['hips', 'chest', 'l_upper', 'l_lower', 'l_wrist', + 'r_upper', 'r_lower', 'r_wrist'] + joints = {} + for i, name in enumerate(names): + p7 = flat[i*7:(i+1)*7] + if np.all(p7 == 0): + joints[name] = None + else: + joints[name] = pose7_to_mat4(p7) + return joints + + +def main(): + parser = argparse.ArgumentParser(description="Joint calibration with Quest tracking") + parser.add_argument("joint", choices=list(JOINTS.keys())) + parser.add_argument("--amplitude", "-a", type=float, default=None) + parser.add_argument("--offset", "-o", type=float, default=None) + parser.add_argument("--period", "-p", type=float, default=8.0) + parser.add_argument("--hz", type=float, default=50.0) + parser.add_argument("--port", type=int, default=8765) + parser.add_argument("--dds-domain", type=int, default=1) + parser.add_argument("--cal-delay", type=float, default=10.0, + help="Seconds to wait before calibrating XR (default: 10.0)") + args = parser.parse_args() + + joint_idx = JOINTS[args.joint] + amplitude = args.amplitude if args.amplitude is not None else DEFAULT_AMP.get(args.joint, 0.5) + offset = args.offset if args.offset is not None else DEFAULT_OFFSET.get(args.joint, 0.0) + parent_name, child_name = JOINT_PARENTS[args.joint] + + print(f"\n{'='*70}") + print(f"JOINT CALIBRATION: {args.joint} (index {joint_idx})") + print(f"Monitoring XR: {parent_name} -> {child_name}") + print(f"Animation: sine amp={amplitude:.2f} offset={offset:.2f} rad, period {args.period:.1f}s") + print(f"Range: [{offset-amplitude:.2f}, {offset+amplitude:.2f}] rad") + print(f"XR cal delay: {args.cal_delay}s") + print(f"{'='*70}") + print(f"\nStarting WebSocket server on port {args.port}...") + + # Start teleop server + tv = NativeTeleWrapper(port=args.port, host="0.0.0.0") + tv.start() + time.sleep(0.5) + + # Init DDS + from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize + from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ + from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ + from unitree_sdk2py.utils.crc import CRC + + ChannelFactoryInitialize(args.dds_domain) + pub = ChannelPublisher("rt/lowcmd", LowCmd_) + pub.Init() + cmd = unitree_hg_msg_dds__LowCmd_() + crc = CRC() + + running = True + def handler(sig, frame): + nonlocal running + running = False + signal.signal(signal.SIGINT, handler) + signal.signal(signal.SIGTERM, handler) + + # Wait for Quest connection + print("\nConnect Quest 3 and hold the ROBOT'S REST POSE for calibration...") + print("(Arms at sides, elbows bent 90 degrees, forearms forward)") + print(f"XR calibration will happen {args.cal_delay}s after first tracking frame.\n") + + cal_ref = None + first_tracking_time = None + interval = 1.0 / args.hz + t0 = time.time() # Start animation immediately + step = 0 + + while running: + t_start = time.perf_counter() + now = time.time() + step += 1 + + # 1) Compute robot animation value + elapsed = now - t0 + robot_val = offset + amplitude * np.sin(2 * np.pi * elapsed / args.period) + angles = np.zeros(NUM_JOINTS) + angles[joint_idx] = robot_val + + # 2) Read XR tracking and compute comparison (if available) + xr_info = None + with tv.server.body_joints_shared.get_lock(): + bj_raw = np.array(tv.server.body_joints_shared[:]) + has_data = np.any(bj_raw != 0) + data_time = tv.last_data_time + tracking = data_time > 0 and (now - data_time) < 0.5 + + if tracking and has_data: + if first_tracking_time is None: + first_tracking_time = now + print(f"\nFirst tracking data! Calibrating in {args.cal_delay}s — hold the rest pose...") + + bj = parse_body_joints(bj_raw) + parent = bj.get(parent_name) + child = bj.get(child_name) + + if parent is not None and child is not None: + R_parent = parent[:3, :3] + R_child = child[:3, :3] + R_rel = R_parent.T @ R_child + + # Calibrate after delay + if cal_ref is None and first_tracking_time is not None and (now - first_tracking_time) >= args.cal_delay: + cal_ref = R_rel.copy() + print("\nCALIBRATED! Now copy the robot's motion.\n") + print(f"{'Robot Value':>12s} | {'XR Delta (robot frame YXZ Euler)':>40s} | {'XR Delta (robot frame XYZ)':>35s}") + print("-" * 95) + + if cal_ref is not None: + # Simple basis change: XR frame -> robot frame + R_delta = cal_ref.T @ R_rel + R_delta_robot = R3_ROBOT_OPENXR @ R_delta @ R3_ROBOT_OPENXR.T + yxz = Rotation.from_matrix(R_delta_robot).as_euler('YXZ') + xyz = Rotation.from_matrix(R_delta_robot).as_euler('XYZ') + xr_info = (yxz, xyz) + + # 3) Publish DDS (once per frame, always with current angles) + cmd.mode_pr = 0 + cmd.mode_machine = 0 + for i in range(NUM_JOINTS): + cmd.motor_cmd[i].mode = 1 + cmd.motor_cmd[i].q = float(angles[i]) + cmd.motor_cmd[i].dq = 0.0 + cmd.motor_cmd[i].tau = 0.0 + cmd.motor_cmd[i].kp = float(KP[i]) + cmd.motor_cmd[i].kd = float(KD[i]) + for i in range(12): + cmd.motor_cmd[i].mode = 0 + cmd.motor_cmd[i].kp = 0.0 + cmd.motor_cmd[i].kd = 0.0 + cmd.crc = crc.Crc(cmd) + pub.Write(cmd) + + # 4) Log + if xr_info and step % int(args.hz) == 0: + yxz, xyz = xr_info + print(f" {robot_val:+.3f} rad | " + f"Y={yxz[0]:+.3f} X={yxz[1]:+.3f} Z={yxz[2]:+.3f} | " + f"X={xyz[0]:+.3f} Y={xyz[1]:+.3f} Z={xyz[2]:+.3f}") + elif not xr_info and step % int(args.hz * 2) == 0: + print(f" robot={robot_val:+.3f} (Waiting for Quest tracking data...)") + + sleep_time = interval - (time.perf_counter() - t_start) + if sleep_time > 0: + time.sleep(sleep_time) + + # Return to zero + print("\nReturning to zero...") + angles = np.zeros(NUM_JOINTS) + for _ in range(25): + cmd.mode_pr = 0 + cmd.mode_machine = 0 + for i in range(NUM_JOINTS): + cmd.motor_cmd[i].mode = 1 + cmd.motor_cmd[i].q = 0.0 + cmd.motor_cmd[i].dq = 0.0 + cmd.motor_cmd[i].tau = 0.0 + cmd.motor_cmd[i].kp = float(KP[i]) + cmd.motor_cmd[i].kd = float(KD[i]) + for i in range(12): + cmd.motor_cmd[i].mode = 0 + cmd.motor_cmd[i].kp = 0.0 + cmd.motor_cmd[i].kd = 0.0 + cmd.crc = crc.Crc(cmd) + pub.Write(cmd) + time.sleep(0.02) + print("Done.") + + +if __name__ == "__main__": + main() diff --git a/server/joint_test.py b/server/joint_test.py new file mode 100644 index 0000000..0ee5e14 --- /dev/null +++ b/server/joint_test.py @@ -0,0 +1,255 @@ +#!/usr/bin/env python3 +""" +Systematic joint-by-joint testing for sign calibration. + +Animates ONE joint at a time with a slow sine wave so the user can see +exactly what each joint does, then match the motion with their Quest 3. + +Usage: + python joint_test.py [--amplitude 0.5] [--period 4.0] + +Joint names: + waist_yaw (joint 12) - Z axis + waist_roll (joint 13) - X axis + waist_pitch (joint 14) - Y axis + l_shoulder_pitch (joint 15) - Y axis + l_shoulder_roll (joint 16) - X axis + l_shoulder_yaw (joint 17) - Z axis + l_elbow (joint 18) - Y axis + l_wrist_roll (joint 19) - X axis + l_wrist_pitch (joint 20) - Y axis + l_wrist_yaw (joint 21) - Z axis + r_shoulder_pitch (joint 22) - Y axis + r_shoulder_roll (joint 23) - X axis + r_shoulder_yaw (joint 24) - Z axis + r_elbow (joint 25) - Y axis + r_wrist_roll (joint 26) - X axis + r_wrist_pitch (joint 27) - Y axis + r_wrist_yaw (joint 28) - Z axis + + Also: "list" to show all joints, "all_shoulders" to cycle through shoulder joints +""" + +import sys +import os +import time +import argparse +import signal +import numpy as np + +NUM_JOINTS = 29 + +JOINTS = { + 'waist_yaw': 12, + 'waist_roll': 13, + 'waist_pitch': 14, + 'l_shoulder_pitch': 15, + 'l_shoulder_roll': 16, + 'l_shoulder_yaw': 17, + 'l_elbow': 18, + 'l_wrist_roll': 19, + 'l_wrist_pitch': 20, + 'l_wrist_yaw': 21, + 'r_shoulder_pitch': 22, + 'r_shoulder_roll': 23, + 'r_shoulder_yaw': 24, + 'r_elbow': 25, + 'r_wrist_roll': 26, + 'r_wrist_pitch': 27, + 'r_wrist_yaw': 28, +} + +# Joint descriptions - what POSITIVE values should do +DESCRIPTIONS = { + 'waist_yaw': 'Torso rotates LEFT (counterclockwise from above)', + 'waist_roll': 'Torso leans RIGHT', + 'waist_pitch': 'Torso leans FORWARD', + 'l_shoulder_pitch': 'Left arm swings FORWARD', + 'l_shoulder_roll': 'Left arm raises OUT to the LEFT side', + 'l_shoulder_yaw': 'Left upper arm rotates INWARD', + 'l_elbow': 'Left elbow BENDS (forearm toward shoulder)', + 'l_wrist_roll': 'Left wrist rotates (palm faces down→up)', + 'l_wrist_pitch': 'Left wrist bends DOWN (flexion)', + 'l_wrist_yaw': 'Left wrist bends toward THUMB side', + 'r_shoulder_pitch': 'Right arm swings FORWARD', + 'r_shoulder_roll': 'Right arm raises OUT to the RIGHT side (NEGATIVE values)', + 'r_shoulder_yaw': 'Right upper arm rotates INWARD', + 'r_elbow': 'Right elbow BENDS (forearm toward shoulder)', + 'r_wrist_roll': 'Right wrist rotates', + 'r_wrist_pitch': 'Right wrist bends DOWN (flexion)', + 'r_wrist_yaw': 'Right wrist bends toward THUMB side', +} + +# Default amplitudes (stay within safe range) +DEFAULT_AMPLITUDES = { + 'waist_yaw': 0.4, + 'waist_roll': 0.3, + 'waist_pitch': 0.3, + 'l_shoulder_pitch': 0.8, + 'l_shoulder_roll': 0.8, + 'l_shoulder_yaw': 0.5, + 'l_elbow': 0.8, + 'l_wrist_roll': 0.5, + 'l_wrist_pitch': 0.5, + 'l_wrist_yaw': 0.5, + 'r_shoulder_pitch': 0.8, + 'r_shoulder_roll': 0.8, + 'r_shoulder_yaw': 0.5, + 'r_elbow': 0.8, + 'r_wrist_roll': 0.5, + 'r_wrist_pitch': 0.5, + 'r_wrist_yaw': 0.5, +} + +# Default Kp/Kd gains +KP = [ + 60, 60, 60, 100, 40, 40, + 60, 60, 60, 100, 40, 40, + 60, 40, 40, + 40, 40, 40, 40, 40, 40, 40, + 40, 40, 40, 40, 40, 40, 40, +] +KD = [ + 1, 1, 1, 2, 1, 1, + 1, 1, 1, 2, 1, 1, + 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, +] + + +def init_dds(domain_id=1): + from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize + from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ + from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ + from unitree_sdk2py.utils.crc import CRC + + ChannelFactoryInitialize(domain_id) + pub = ChannelPublisher("rt/lowcmd", LowCmd_) + pub.Init() + cmd = unitree_hg_msg_dds__LowCmd_() + crc = CRC() + return pub, cmd, crc + + +def publish(pub, cmd, crc, angles): + cmd.mode_pr = 0 + cmd.mode_machine = 0 + for i in range(NUM_JOINTS): + cmd.motor_cmd[i].mode = 1 + cmd.motor_cmd[i].q = float(angles[i]) + cmd.motor_cmd[i].dq = 0.0 + cmd.motor_cmd[i].tau = 0.0 + cmd.motor_cmd[i].kp = float(KP[i]) + cmd.motor_cmd[i].kd = float(KD[i]) + # Zero legs + for i in range(12): + cmd.motor_cmd[i].mode = 0 + cmd.motor_cmd[i].kp = 0.0 + cmd.motor_cmd[i].kd = 0.0 + cmd.crc = crc.Crc(cmd) + pub.Write(cmd) + + +def main(): + parser = argparse.ArgumentParser(description="Joint-by-joint animation tester") + parser.add_argument("joint", help="Joint name (or 'list' to show all)") + parser.add_argument("--amplitude", "-a", type=float, default=None, + help="Sine wave amplitude in radians (default: joint-specific)") + parser.add_argument("--period", "-p", type=float, default=4.0, + help="Sine wave period in seconds (default: 4.0)") + parser.add_argument("--hz", type=float, default=50.0) + parser.add_argument("--offset", "-o", type=float, default=0.0, + help="Static offset added to sine wave") + parser.add_argument("--dds-domain", type=int, default=1) + parser.add_argument("--pause-at-peak", choices=["pos", "neg"], default=None, + help="Pause at peak each cycle: 'pos' or 'neg'") + parser.add_argument("--pause-duration", type=float, default=3.0, + help="How long to pause in seconds (default: 3.0)") + args = parser.parse_args() + + if args.joint == 'list': + print("\nAvailable joints:\n") + for name, idx in sorted(JOINTS.items(), key=lambda x: x[1]): + desc = DESCRIPTIONS.get(name, '') + amp = DEFAULT_AMPLITUDES.get(name, 0.5) + print(f" {name:20s} (joint {idx:2d}) +val: {desc}") + print() + return + + if args.joint not in JOINTS: + print(f"Unknown joint: {args.joint}") + print(f"Available: {', '.join(sorted(JOINTS.keys()))}") + return + + joint_idx = JOINTS[args.joint] + amplitude = args.amplitude if args.amplitude is not None else DEFAULT_AMPLITUDES.get(args.joint, 0.5) + desc = DESCRIPTIONS.get(args.joint, '') + + print(f"\n{'='*60}") + print(f"Testing: {args.joint} (joint index {joint_idx})") + print(f"Positive direction: {desc}") + print(f"Animation: sine wave, amplitude={amplitude:.2f} rad, period={args.period:.1f}s") + print(f"Offset: {args.offset:.2f} rad") + print(f"{'='*60}\n") + + pub, cmd, crc = init_dds(args.dds_domain) + + running = True + def handler(sig, frame): + nonlocal running + running = False + signal.signal(signal.SIGINT, handler) + signal.signal(signal.SIGTERM, handler) + + interval = 1.0 / args.hz + t0 = time.time() + step = 0 + + print("Animating... (Ctrl+C to stop)\n") + + last_pause_time = 0 + while running: + t_start = time.perf_counter() + elapsed = time.time() - t0 + angles = np.zeros(NUM_JOINTS) + + value = args.offset + amplitude * np.sin(2 * np.pi * elapsed / args.period) + angles[joint_idx] = value + + publish(pub, cmd, crc, angles) + + step += 1 + if step % int(args.hz * 1) == 0: + print(f" {args.joint} = {value:+.3f} rad ({np.degrees(value):+.1f} deg)") + + # Pause at peak (with cooldown to avoid re-triggering) + if args.pause_at_peak and (time.time() - last_pause_time) > args.period * 0.5: + if args.pause_at_peak == "pos": + peak_val = args.offset + amplitude + else: + peak_val = args.offset - amplitude + if abs(value - peak_val) < 0.05: + print(f"\n >>> PAUSED at {args.pause_at_peak} peak: {value:+.3f} rad ({np.degrees(value):+.1f} deg) <<<") + pause_end = time.time() + args.pause_duration + while time.time() < pause_end and running: + publish(pub, cmd, crc, angles) + time.sleep(interval) + last_pause_time = time.time() + print(f" >>> Resuming...\n") + + sleep_time = interval - (time.perf_counter() - t_start) + if sleep_time > 0: + time.sleep(sleep_time) + + # Return to zero + print("\nReturning to zero...") + angles = np.zeros(NUM_JOINTS) + for _ in range(25): + publish(pub, cmd, crc, angles) + time.sleep(0.02) + print("Done.") + + +if __name__ == "__main__": + main() diff --git a/server/native_tv_wrapper.py b/server/native_tv_wrapper.py index 10cf90f..5d7e546 100644 --- a/server/native_tv_wrapper.py +++ b/server/native_tv_wrapper.py @@ -126,29 +126,31 @@ class TeleData: class NativeTeleWrapper: """Drop-in replacement for TeleVuerWrapper using Quest 3 native app. - The key difference: wrist poses from body tracking are chest-relative, - so the head position subtraction in the original TeleVuerWrapper is - unnecessary. Instead, we use chest-relative data directly. + Matches the Unitree TeleVuerWrapper pipeline exactly: + 1. Wrist poses arrive chest-relative from body tracking (Godot computes chest_inv * wrist) + 2. We reconstruct world-space wrist poses: chest * chest_relative + 3. Apply basis change (OpenXR -> Robot convention) + 4. Apply Unitree arm URDF convention transforms + 5. Subtract HEAD position (translation only — rotation stays world-space) + 6. Add head-to-waist offset (matches Unitree: 0.15 x, 0.45 z) """ def __init__(self, port: int = 8765, host: str = "0.0.0.0", - chest_to_waist_x: float = 0.15, - chest_to_waist_z: float = 0.28): + head_to_waist_x: float = 0.15, + head_to_waist_z: float = 0.45): """ Args: port: WebSocket server port host: WebSocket bind address - chest_to_waist_x: Forward offset from chest to IK solver origin (meters). - Original head-to-waist was 0.15; chest is slightly forward - of head, so this may need tuning. - chest_to_waist_z: Vertical offset from chest to IK solver origin (meters). - Original head-to-waist was 0.45; chest-to-waist is shorter - (~0.25-0.30). Default 0.28 is a starting estimate. + head_to_waist_x: Forward offset from head to IK solver origin (meters). + Matches Unitree default: 0.15. + head_to_waist_z: Vertical offset from head to IK solver origin (meters). + Matches Unitree default: 0.45. """ self.server = TeleopServer(host=host, port=port) self._server_thread = None - self._chest_to_waist_x = chest_to_waist_x - self._chest_to_waist_z = chest_to_waist_z + self._head_to_waist_x = head_to_waist_x + self._head_to_waist_z = head_to_waist_z def start(self): """Start the WebSocket server in a background thread.""" @@ -157,7 +159,12 @@ class NativeTeleWrapper: def get_tele_data(self) -> TeleData: """Get processed motion data, transformed to robot convention. - Returns TeleData compatible with teleop_hand_and_arm.py. + Pipeline matches Unitree's TeleVuerWrapper (televuer/tv_wrapper.py): + 1. Reconstruct world-space wrist from chest-relative data + 2. Basis change (OpenXR -> Robot) + 3. Arm convention transform + 4. Head subtraction (translation only — rotation stays world-space) + 5. Head-to-waist offset """ # Read raw poses from shared memory (column-major 4x4) with self.server.head_pose_shared.get_lock(): @@ -165,42 +172,49 @@ class NativeTeleWrapper: with self.server.chest_pose_shared.get_lock(): chest_raw = np.array(self.server.chest_pose_shared[:]).reshape(4, 4, order='F') with self.server.left_arm_pose_shared.get_lock(): - left_wrist_raw = np.array(self.server.left_arm_pose_shared[:]).reshape(4, 4, order='F') + left_wrist_chest_rel = np.array(self.server.left_arm_pose_shared[:]).reshape(4, 4, order='F') with self.server.right_arm_pose_shared.get_lock(): - right_wrist_raw = np.array(self.server.right_arm_pose_shared[:]).reshape(4, 4, order='F') + right_wrist_chest_rel = np.array(self.server.right_arm_pose_shared[:]).reshape(4, 4, order='F') # Validate matrices head_raw, head_valid = safe_mat_update(CONST_HEAD_POSE, head_raw) - left_wrist_raw, left_valid = safe_mat_update(CONST_LEFT_ARM_POSE, left_wrist_raw) - right_wrist_raw, right_valid = safe_mat_update(CONST_RIGHT_ARM_POSE, right_wrist_raw) + chest_raw, chest_valid = safe_mat_update(np.eye(4), chest_raw) + left_wrist_chest_rel, left_valid = safe_mat_update(np.eye(4), left_wrist_chest_rel) + right_wrist_chest_rel, right_valid = safe_mat_update(np.eye(4), right_wrist_chest_rel) - # --- Transform from Godot/OpenXR convention to Robot convention --- + # --- Reconstruct world-space wrist poses --- + # body_tracker.gd sends: left_wrist_rel = chest_inv * left_wrist_world + # So: left_wrist_world = chest * left_wrist_rel + left_wrist_world = chest_raw @ left_wrist_chest_rel + right_wrist_world = chest_raw @ right_wrist_chest_rel - # Head: basis change (world-space pose) - Brobot_world_head = T_ROBOT_OPENXR @ head_raw @ T_OPENXR_ROBOT + # Validate reconstructed world-space poses + left_wrist_world, left_valid = safe_mat_update(CONST_LEFT_ARM_POSE, left_wrist_world) + right_wrist_world, right_valid = safe_mat_update(CONST_RIGHT_ARM_POSE, right_wrist_world) - # Chest: basis change (world-space pose) + # --- Basis change: OpenXR -> Robot convention --- + Brobot_world_head = T_ROBOT_OPENXR @ head_raw @ T_OPENXR_ROBOT Brobot_world_chest = T_ROBOT_OPENXR @ chest_raw @ T_OPENXR_ROBOT + left_Brobot_world = T_ROBOT_OPENXR @ left_wrist_world @ T_OPENXR_ROBOT + right_Brobot_world = T_ROBOT_OPENXR @ right_wrist_world @ T_OPENXR_ROBOT - # Wrist poses: these are CHEST-RELATIVE from body tracking. - # Apply basis change to chest-relative poses. - left_Brobot = T_ROBOT_OPENXR @ left_wrist_raw @ T_OPENXR_ROBOT - right_Brobot = T_ROBOT_OPENXR @ right_wrist_raw @ T_OPENXR_ROBOT - - # Apply Unitree arm URDF convention transforms - left_IPunitree = left_Brobot @ (T_TO_UNITREE_HUMANOID_LEFT_ARM if left_valid else np.eye(4)) - right_IPunitree = right_Brobot @ (T_TO_UNITREE_HUMANOID_RIGHT_ARM if right_valid else np.eye(4)) + # --- Arm URDF convention transform --- + left_IPunitree = left_Brobot_world @ (T_TO_UNITREE_HUMANOID_LEFT_ARM if left_valid else np.eye(4)) + right_IPunitree = right_Brobot_world @ (T_TO_UNITREE_HUMANOID_RIGHT_ARM if right_valid else np.eye(4)) - # CHEST-TO-WAIST offset: the IK solver origin is near the waist. - # Body tracking gives us chest-relative poses. The chest is lower - # than the head, so the vertical offset is smaller than the original - # head-to-waist offset (0.45). Configurable via constructor args. + # --- Head subtraction (TRANSLATION ONLY, matching Unitree tv_wrapper.py) --- + # The rotation stays in world-space, which is what the IK solver expects. + # Only the position is made head-relative. left_wrist_final = left_IPunitree.copy() right_wrist_final = right_IPunitree.copy() - left_wrist_final[0, 3] += self._chest_to_waist_x # x (forward) - right_wrist_final[0, 3] += self._chest_to_waist_x - left_wrist_final[2, 3] += self._chest_to_waist_z # z (up) - right_wrist_final[2, 3] += self._chest_to_waist_z + left_wrist_final[0:3, 3] -= Brobot_world_head[0:3, 3] + right_wrist_final[0:3, 3] -= Brobot_world_head[0:3, 3] + + # --- Head-to-waist offset (matching Unitree: x=0.15, z=0.45) --- + left_wrist_final[0, 3] += self._head_to_waist_x # x (forward) + right_wrist_final[0, 3] += self._head_to_waist_x + left_wrist_final[2, 3] += self._head_to_waist_z # z (up) + right_wrist_final[2, 3] += self._head_to_waist_z # --- Hand positions --- left_hand_pos = np.zeros((25, 3)) diff --git a/server/retarget_bridge.py b/server/retarget_bridge.py index 3ce7d6d..6547bad 100644 --- a/server/retarget_bridge.py +++ b/server/retarget_bridge.py @@ -1,20 +1,32 @@ #!/usr/bin/env python3 """ -Retargeting bridge: Quest 3 body tracking -> G1 robot joint commands via DDS. - -Reads body tracking data from the teleop server's shared memory, retargets -body joint orientations to G1 robot joint angles, and publishes commands -over CycloneDDS for Isaac Sim (or real robot). - -Usage: - python retarget_bridge.py [--port 8765] [--dds-domain 1] [--hz 50] +Retargeting bridge v4: Quest 3 full body tracking -> G1 robot joints via DIRECT mapping + DDS. + +Maps ALL tracked body joints directly to robot joint angles (no IK solver): +- Waist (3 DOF): hips->chest relative rotation -> ZXY Euler +- Shoulders (3 DOF x 2): chest->upper_arm -> YXZ Euler +- Elbows (1 DOF x 2): upper_arm->lower_arm -> Y rotation +- Wrists (3 DOF x 2): lower_arm->hand -> XYZ Euler +- Hands (12 DOF): dex-retargeting for Inspire hands +- Legs: zeroed (not tracked) + +Calibration: + On first tracking frame, captures the user's neutral pose as reference. + All subsequent angles are deltas from this reference, ensuring: + - User arms at sides -> robot arms at sides (zero angles) + - User arm movements -> proportional robot joint movements Architecture: Quest 3 -> WebSocket -> TeleopServer (shared memory) | retarget_bridge.py + - Direct joint retargeting (17 DOF) + - Inspire hand retargeting (12 DOF) | CycloneDDS -> Isaac Sim + +Usage: + python retarget_bridge.py [--port 8765] [--dds-domain 1] [--hz 50] [--verbose] """ import sys @@ -24,96 +36,69 @@ import argparse import logging import signal import numpy as np -from multiprocessing import Array +from scipy.spatial.transform import Rotation + +# Add xr_teleoperate to Python path for hand retargeting +XR_TELEOP_DIR = "/home/sparky/git/xr_teleoperate" +sys.path.insert(0, XR_TELEOP_DIR) + +# Monkey-patch logging_mp API compatibility +import logging_mp +if not hasattr(logging_mp, 'getLogger'): + logging_mp.getLogger = logging_mp.get_logger + +# Monkey-patch dex_retargeting +import dex_retargeting +if not hasattr(dex_retargeting, 'RetargetingConfig'): + from dex_retargeting.retargeting_config import RetargetingConfig + dex_retargeting.RetargetingConfig = RetargetingConfig # Local imports from teleop_server import TeleopServer +from native_tv_wrapper import (T_ROBOT_OPENXR, T_OPENXR_ROBOT, + T_TO_UNITREE_HAND, + NativeTeleWrapper) logger = logging.getLogger("retarget_bridge") # --------------------------------------------------------------------------- -# Coordinate transform constants -# --------------------------------------------------------------------------- -# OpenXR (Y-up, -Z forward, X-right) -> Robot (X-forward, Y-left, Z-up) -R_ROBOT_OPENXR = np.array([[ 0, 0,-1], - [-1, 0, 0], - [ 0, 1, 0]], dtype=np.float64) -R_OPENXR_ROBOT = R_ROBOT_OPENXR.T - -# --------------------------------------------------------------------------- -# G1 joint indices (29 DOF) +# G1 joint constants # --------------------------------------------------------------------------- NUM_JOINTS = 29 -# Waist -WAIST_YAW = 12 -WAIST_ROLL = 13 -WAIST_PITCH = 14 - -# Left arm -L_SHOULDER_PITCH = 15 -L_SHOULDER_ROLL = 16 -L_SHOULDER_YAW = 17 -L_ELBOW = 18 -L_WRIST_ROLL = 19 -L_WRIST_PITCH = 20 -L_WRIST_YAW = 21 - -# Right arm -R_SHOULDER_PITCH = 22 -R_SHOULDER_ROLL = 23 -R_SHOULDER_YAW = 24 -R_ELBOW = 25 -R_WRIST_ROLL = 26 -R_WRIST_PITCH = 27 -R_WRIST_YAW = 28 - -# --------------------------------------------------------------------------- -# Body joints shared memory layout (8 joints x 7 floats = 56) -# Each joint: [x, y, z, qx, qy, qz, qw] -# --------------------------------------------------------------------------- -BJ_HIPS = 0 # offset 0 -BJ_CHEST = 1 # offset 7 -BJ_L_UPPER_ARM = 2 # offset 14 -BJ_L_LOWER_ARM = 3 # offset 21 -BJ_L_WRIST = 4 # offset 28 -BJ_R_UPPER_ARM = 5 # offset 35 -BJ_R_LOWER_ARM = 6 # offset 42 -BJ_R_WRIST = 7 # offset 49 -NUM_BODY_JOINTS = 8 +# Joint index ranges in LowCmd +WAIST_START = 12 # indices 12-14: waist yaw, roll, pitch +LEFT_ARM_START = 15 # indices 15-21: 7 left arm joints +RIGHT_ARM_START = 22 # indices 22-28: 7 right arm joints -# --------------------------------------------------------------------------- -# URDF joint limits (from g1_custom_collision_29dof.urdf) -# --------------------------------------------------------------------------- +# Joint limits from URDF with 90% safety factor JOINT_LIMITS = { - 12: (-2.618, 2.618), # waist_yaw - 13: (-0.52, 0.52), # waist_roll - 14: (-0.52, 0.52), # waist_pitch - 15: (-3.0892, 2.6704), # left_shoulder_pitch - 16: (-1.5882, 2.2515), # left_shoulder_roll - 17: (-2.618, 2.618), # left_shoulder_yaw - 18: (-1.0472, 2.0944), # left_elbow - 19: (-1.972, 1.972), # left_wrist_roll - 20: (-1.614, 1.614), # left_wrist_pitch - 21: (-1.614, 1.614), # left_wrist_yaw - 22: (-3.0892, 2.6704), # right_shoulder_pitch - 23: (-2.2515, 1.5882), # right_shoulder_roll - 24: (-2.618, 2.618), # right_shoulder_yaw - 25: (-1.0472, 2.0944), # right_elbow - 26: (-1.972, 1.972), # right_wrist_roll - 27: (-1.614, 1.614), # right_wrist_pitch - 28: (-1.614, 1.614), # right_wrist_yaw + 12: (-2.356, 2.356), # waist_yaw + 13: (-0.468, 0.468), # waist_roll + 14: (-0.468, 0.468), # waist_pitch + 15: (-2.780, 2.403), # L shoulder_pitch + 16: (-1.429, 2.026), # L shoulder_roll + 17: (-2.356, 2.356), # L shoulder_yaw + 18: (-0.942, 1.885), # L elbow + 19: (-1.775, 1.775), # L wrist_roll + 20: (-1.453, 1.453), # L wrist_pitch + 21: (-1.453, 1.453), # L wrist_yaw + 22: (-2.780, 2.403), # R shoulder_pitch + 23: (-2.026, 1.429), # R shoulder_roll (mirrored) + 24: (-2.356, 2.356), # R shoulder_yaw + 25: (-0.942, 1.885), # R elbow + 26: (-1.775, 1.775), # R wrist_roll + 27: (-1.453, 1.453), # R wrist_pitch + 28: (-1.453, 1.453), # R wrist_yaw } -SOFT_LIMIT_FACTOR = 0.9 - -# Default Kp/Kd from Unitree SDK example +# Default Kp/Kd gains KP = [ - 60, 60, 60, 100, 40, 40, # Left leg - 60, 60, 60, 100, 40, 40, # Right leg - 60, 40, 40, # Waist - 40, 40, 40, 40, 40, 40, 40, # Left arm - 40, 40, 40, 40, 40, 40, 40, # Right arm + 60, 60, 60, 100, 40, 40, # Left leg (0-5) + 60, 60, 60, 100, 40, 40, # Right leg (6-11) + 60, 40, 40, # Waist (12-14) + 40, 40, 40, 40, 40, 40, 40, # Left arm (15-21) + 40, 40, 40, 40, 40, 40, 40, # Right arm (22-28) ] KD = [ 1, 1, 1, 2, 1, 1, @@ -123,453 +108,284 @@ KD = [ 1, 1, 1, 1, 1, 1, 1, ] -# Inspire hand joint ordering in DDS MotorCmds_ (12 total) -# Indices 0-5: right hand, 6-11: left hand -INSPIRE_R_PINKY = 0 -INSPIRE_R_RING = 1 -INSPIRE_R_MIDDLE = 2 -INSPIRE_R_INDEX = 3 -INSPIRE_R_THUMB_PITCH = 4 -INSPIRE_R_THUMB_YAW = 5 -INSPIRE_L_PINKY = 6 -INSPIRE_L_RING = 7 -INSPIRE_L_MIDDLE = 8 -INSPIRE_L_INDEX = 9 -INSPIRE_L_THUMB_PITCH = 10 -INSPIRE_L_THUMB_YAW = 11 - -# Quest hand joint indices (XR_EXT_hand_tracking, 25 per hand) -QH_PALM = 0 -QH_WRIST = 1 -QH_THUMB_METACARPAL = 2 -QH_THUMB_PROXIMAL = 3 -QH_THUMB_TIP = 4 -QH_INDEX_METACARPAL = 5 -QH_INDEX_PROXIMAL = 6 -QH_INDEX_INTERMEDIATE = 7 -QH_INDEX_TIP = 8 -QH_MIDDLE_METACARPAL = 9 -QH_MIDDLE_PROXIMAL = 10 -QH_MIDDLE_INTERMEDIATE = 11 -QH_MIDDLE_TIP = 12 -QH_RING_METACARPAL = 13 -QH_RING_PROXIMAL = 14 -QH_RING_INTERMEDIATE = 15 -QH_RING_TIP = 16 -QH_PINKY_METACARPAL = 17 -QH_PINKY_PROXIMAL = 18 -QH_PINKY_INTERMEDIATE = 19 -QH_PINKY_TIP = 20 - - -# --------------------------------------------------------------------------- -# Rotation math utilities -# --------------------------------------------------------------------------- - -def _rx(angle): - """Rotation matrix about X axis.""" - c, s = np.cos(angle), np.sin(angle) - return np.array([[1, 0, 0], [0, c, -s], [0, s, c]], dtype=np.float64) +# Inspire hand normalization ranges +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 +] -def _ry(angle): - """Rotation matrix about Y axis.""" - c, s = np.cos(angle), np.sin(angle) - return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]], dtype=np.float64) +# Body joint indices in shared memory (8 joints x 7 floats = 56) +BJ_HIPS = 0 +BJ_CHEST = 1 +BJ_L_UPPER = 2 +BJ_L_LOWER = 3 +BJ_L_WRIST = 4 +BJ_R_UPPER = 5 +BJ_R_LOWER = 6 +BJ_R_WRIST = 7 -def _rz(angle): - """Rotation matrix about Z axis.""" - c, s = np.cos(angle), np.sin(angle) - return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]], dtype=np.float64) +# 3x3 rotation submatrices of the basis change +R3_ROBOT_OPENXR = T_ROBOT_OPENXR[:3, :3] +R3_OPENXR_ROBOT = T_OPENXR_ROBOT[:3, :3] +# Signs are now baked into the axis mapping in compute_all_angles(). +# Determined empirically via joint_calibrate.py (see calibration_log.md). -def quat_to_rotmat(qx, qy, qz, qw): - """Quaternion to 3x3 rotation matrix.""" - xx = qx * qx; yy = qy * qy; zz = qz * qz - xy = qx * qy; xz = qx * qz; yz = qy * qz - wx = qw * qx; wy = qw * qy; wz = qw * qz - return np.array([ - [1 - 2*(yy + zz), 2*(xy - wz), 2*(xz + wy)], - [2*(xy + wz), 1 - 2*(xx + zz), 2*(yz - wx)], - [2*(xz - wy), 2*(yz + wx), 1 - 2*(xx + yy)], - ], dtype=np.float64) +# Per-joint linear mapping: robot_angle = scale * xr_angle + offset +# Determined empirically by comparing robot animation range with XR tracking range. +# Indices 0-16 map to joints 12-28 (waist 0-2, L arm 3-9, R arm 10-16) +# Default: scale=1.0, offset=0.0 (passthrough) +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 -def euler_zxy(R): - """Decompose R = Rz(yaw) * Rx(roll) * Ry(pitch). Returns (yaw, roll, pitch). +# 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 - Used for waist joints: yaw(Z) -> roll(X) -> pitch(Y). - """ - # R[2,1] = sin(roll) - roll = np.arcsin(np.clip(R[2, 1], -1.0, 1.0)) - if np.abs(R[2, 1]) < 0.9999: - yaw = np.arctan2(-R[0, 1], R[1, 1]) - pitch = np.arctan2(-R[2, 0], R[2, 2]) - else: - yaw = np.arctan2(R[1, 0], R[0, 0]) - pitch = 0.0 - return yaw, roll, pitch +# 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 -def euler_yxz(R): - """Decompose R = Ry(pitch) * Rx(roll) * Rz(yaw). Returns (pitch, roll, yaw). +# TODO: calibrate remaining joints (wrist, right arm) - Used for shoulder joints: pitch(Y) -> roll(X) -> yaw(Z). - """ - # R[1,2] = -sin(roll) - roll = np.arcsin(np.clip(-R[1, 2], -1.0, 1.0)) - if np.abs(R[1, 2]) < 0.9999: - pitch = np.arctan2(R[0, 2], R[2, 2]) - yaw = np.arctan2(R[1, 0], R[1, 1]) - else: - pitch = np.arctan2(-R[2, 0], R[0, 0]) - yaw = 0.0 - return pitch, roll, yaw +# Smoothing parameters +EMA_ALPHA = 0.4 # Exponential moving average (0=no update, 1=no smoothing) +MAX_RATE = 3.0 # Max joint velocity in rad/s -def euler_xyz(R): - """Decompose R = Rx(roll) * Ry(pitch) * Rz(yaw). Returns (roll, pitch, yaw). +# --------------------------------------------------------------------------- +# Body joint utilities +# --------------------------------------------------------------------------- - Used for wrist joints: roll(X) -> pitch(Y) -> yaw(Z). - """ - # R[0,2] = sin(pitch) - pitch = np.arcsin(np.clip(R[0, 2], -1.0, 1.0)) - if np.abs(R[0, 2]) < 0.9999: - roll = np.arctan2(-R[1, 2], R[2, 2]) - yaw = np.arctan2(-R[0, 1], R[0, 0]) - else: - roll = np.arctan2(R[2, 1], R[1, 1]) - yaw = 0.0 - return roll, pitch, yaw +def pose7_to_mat4(pose7): + """Convert [x,y,z,qx,qy,qz,qw] to 4x4 SE(3) matrix.""" + mat = np.eye(4) + r = Rotation.from_quat(pose7[3:7]) # scipy uses [qx,qy,qz,qw] + mat[:3, :3] = r.as_matrix() + mat[:3, 3] = pose7[:3] + return mat -def extract_y_rotation(R): - """Extract the Y-axis rotation angle from a rotation matrix. +def parse_body_joints(body_joints_flat): + """Parse the 56-float body joints array into 8 x 4x4 matrices. - Used for elbow (single-axis pitch). + Returns dict with keys: hips, chest, l_upper, l_lower, l_wrist, r_upper, r_lower, r_wrist. + All in OpenXR world space. """ - return np.arctan2(R[0, 2], R[0, 0]) + joints = {} + names = ['hips', 'chest', 'l_upper', 'l_lower', 'l_wrist', + 'r_upper', 'r_lower', 'r_wrist'] + for i, name in enumerate(names): + p7 = body_joints_flat[i*7:(i+1)*7] + if np.all(p7 == 0): + joints[name] = None + else: + joints[name] = pose7_to_mat4(p7) + return joints -# Pre-compute shoulder rest pose rotations -L_SHOULDER_REST = _ry(0.27931) @ _rx(-0.27925) -R_SHOULDER_REST = _ry(-0.27931) @ _rx(0.27925) -L_SHOULDER_REST_INV = L_SHOULDER_REST.T -R_SHOULDER_REST_INV = R_SHOULDER_REST.T +def all_joints_valid(bj): + """Check if all 8 body joints are valid (non-None).""" + return all(bj[k] is not None for k in + ['hips', 'chest', 'l_upper', 'l_lower', 'l_wrist', + 'r_upper', 'r_lower', 'r_wrist']) # --------------------------------------------------------------------------- -# Joint smoother +# Calibration: captures neutral pose reference rotations # --------------------------------------------------------------------------- -class JointSmoother: - """EMA smoothing with rate limiter for joint angles.""" - - def __init__(self, n_joints, alpha=0.3, max_delta=0.15): - """ - Args: - n_joints: Number of joints to smooth. - alpha: EMA factor (0=no change, 1=instant). Default 0.3. - max_delta: Max angle change per step in radians. At 50Hz, - 0.15 rad/step = 7.5 rad/s. - """ - self.prev = np.zeros(n_joints) - self.alpha = alpha - self.max_delta = max_delta - - def smooth(self, target): - """Apply EMA + rate limiting. Returns smoothed values.""" - # EMA - smoothed = self.alpha * target + (1.0 - self.alpha) * self.prev - # Rate limiter - delta = np.clip(smoothed - self.prev, -self.max_delta, self.max_delta) - result = self.prev + delta - self.prev = result.copy() - return result - - -# --------------------------------------------------------------------------- -# Body retargeter -# --------------------------------------------------------------------------- +class CalibrationData: + """Stores reference relative rotations for calibration. -class BodyRetargeter: - """Converts body tracking poses to G1 joint angles via orientation decomposition.""" + When the user stands with arms at sides (robot's zero pose), we capture + the relative rotation between each parent-child joint pair (R_ref). + All subsequent tracking computes deltas from these references and + transforms them to robot frame via a simple basis change. + """ def __init__(self): - # Pre-compute soft joint limits - self.soft_lo = np.zeros(NUM_JOINTS) - self.soft_hi = np.zeros(NUM_JOINTS) - for idx, (lo, hi) in JOINT_LIMITS.items(): - mid = (lo + hi) / 2.0 - half = (hi - lo) / 2.0 - self.soft_lo[idx] = mid - SOFT_LIMIT_FACTOR * half - self.soft_hi[idx] = mid + SOFT_LIMIT_FACTOR * half - - # Calibration: captured at first valid frame - self._calibrated = False - self._ref_rotations = {} # joint_group -> R_rel at calibration - - def retarget(self, body_rotmats): - """Compute 29 joint angles from body tracking rotation matrices. + 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_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 + self.ref_wrist_R = None # r_lower.T @ r_wrist + + def capture(self, bj): + """Capture reference rotations from current body joint data. Args: - body_rotmats: dict mapping joint index (BJ_*) to 3x3 rotation matrix - in OpenXR world frame. + bj: dict from parse_body_joints(), all entries must be non-None. Returns: - np.ndarray of shape (29,) with joint angles in radians. + True if calibration succeeded. """ - angles = np.zeros(NUM_JOINTS) - - # --- Waist: HIPS -> CHEST --- - R_hips = body_rotmats[BJ_HIPS] - R_chest = body_rotmats[BJ_CHEST] - R_waist_rel = self._to_robot_rel(R_hips, R_chest) - - if not self._calibrated: - self._ref_rotations['waist'] = R_waist_rel.copy() - - R_waist_delta = self._ref_rotations.get('waist', np.eye(3)).T @ R_waist_rel - yaw, roll, pitch = euler_zxy(R_waist_delta) - angles[WAIST_YAW] = yaw - angles[WAIST_ROLL] = roll - angles[WAIST_PITCH] = pitch - - # --- Left shoulder: CHEST -> LEFT_UPPER_ARM --- - R_l_upper = body_rotmats[BJ_L_UPPER_ARM] - R_l_shoulder_rel = self._to_robot_rel(R_chest, R_l_upper) - - if not self._calibrated: - self._ref_rotations['l_shoulder'] = R_l_shoulder_rel.copy() - - R_l_shoulder_delta = self._ref_rotations.get('l_shoulder', np.eye(3)).T @ R_l_shoulder_rel - l_sp, l_sr, l_sy = euler_yxz(R_l_shoulder_delta) - angles[L_SHOULDER_PITCH] = l_sp - angles[L_SHOULDER_ROLL] = l_sr - angles[L_SHOULDER_YAW] = l_sy - - # --- Left elbow: LEFT_UPPER_ARM -> LEFT_LOWER_ARM --- - R_l_lower = body_rotmats[BJ_L_LOWER_ARM] - R_l_elbow_rel = self._to_robot_rel(R_l_upper, R_l_lower) - - if not self._calibrated: - self._ref_rotations['l_elbow'] = R_l_elbow_rel.copy() - - R_l_elbow_delta = self._ref_rotations.get('l_elbow', np.eye(3)).T @ R_l_elbow_rel - angles[L_ELBOW] = extract_y_rotation(R_l_elbow_delta) - - # --- Left wrist: LEFT_LOWER_ARM -> LEFT_WRIST --- - R_l_wrist = body_rotmats[BJ_L_WRIST] - R_l_wrist_rel = self._to_robot_rel(R_l_lower, R_l_wrist) + if not all_joints_valid(bj): + return False - if not self._calibrated: - self._ref_rotations['l_wrist'] = R_l_wrist_rel.copy() + 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_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] + self.ref_wrist_R = bj['r_lower'][:3, :3].T @ bj['r_wrist'][:3, :3] - R_l_wrist_delta = self._ref_rotations.get('l_wrist', np.eye(3)).T @ R_l_wrist_rel - l_wr, l_wp, l_wy = euler_xyz(R_l_wrist_delta) - angles[L_WRIST_ROLL] = l_wr - angles[L_WRIST_PITCH] = l_wp - angles[L_WRIST_YAW] = l_wy + self.calibrated = True + return True - # --- Right shoulder: CHEST -> RIGHT_UPPER_ARM --- - R_r_upper = body_rotmats[BJ_R_UPPER_ARM] - R_r_shoulder_rel = self._to_robot_rel(R_chest, R_r_upper) - if not self._calibrated: - self._ref_rotations['r_shoulder'] = R_r_shoulder_rel.copy() - - R_r_shoulder_delta = self._ref_rotations.get('r_shoulder', np.eye(3)).T @ R_r_shoulder_rel - r_sp, r_sr, r_sy = euler_yxz(R_r_shoulder_delta) - angles[R_SHOULDER_PITCH] = r_sp - angles[R_SHOULDER_ROLL] = r_sr - angles[R_SHOULDER_YAW] = r_sy - - # --- Right elbow: RIGHT_UPPER_ARM -> RIGHT_LOWER_ARM --- - R_r_lower = body_rotmats[BJ_R_LOWER_ARM] - R_r_elbow_rel = self._to_robot_rel(R_r_upper, R_r_lower) - - if not self._calibrated: - self._ref_rotations['r_elbow'] = R_r_elbow_rel.copy() - - R_r_elbow_delta = self._ref_rotations.get('r_elbow', np.eye(3)).T @ R_r_elbow_rel - angles[R_ELBOW] = extract_y_rotation(R_r_elbow_delta) - - # --- Right wrist: RIGHT_LOWER_ARM -> RIGHT_WRIST --- - R_r_wrist = body_rotmats[BJ_R_WRIST] - R_r_wrist_rel = self._to_robot_rel(R_r_lower, R_r_wrist) - - if not self._calibrated: - self._ref_rotations['r_wrist'] = R_r_wrist_rel.copy() - - R_r_wrist_delta = self._ref_rotations.get('r_wrist', np.eye(3)).T @ R_r_wrist_rel - r_wr, r_wp, r_wy = euler_xyz(R_r_wrist_delta) - angles[R_WRIST_ROLL] = r_wr - angles[R_WRIST_PITCH] = r_wp - angles[R_WRIST_YAW] = r_wy - - # Mark calibration complete - if not self._calibrated: - self._calibrated = True - logger.info("Calibration captured (neutral pose reference)") - - # Clamp to soft joint limits - for idx in JOINT_LIMITS: - angles[idx] = np.clip(angles[idx], self.soft_lo[idx], self.soft_hi[idx]) - - return angles - - def _to_robot_rel(self, R_parent_openxr, R_child_openxr): - """Compute parent-relative rotation in robot frame. - - R_rel_openxr = R_parent^T @ R_child - R_rel_robot = R_ROBOT_OPENXR @ R_rel_openxr @ R_OPENXR_ROBOT - """ - R_rel = R_parent_openxr.T @ R_child_openxr - return R_ROBOT_OPENXR @ R_rel @ R_OPENXR_ROBOT +# --------------------------------------------------------------------------- +# Direct joint retargeting +# --------------------------------------------------------------------------- - def reset_calibration(self): - """Reset calibration so next frame becomes the new reference.""" - self._calibrated = False - self._ref_rotations.clear() - logger.info("Calibration reset -- next valid frame will be captured") +def _joint_delta_robot(R_ref, R_cur): + """Compute rotation delta from reference to current, in robot frame. + 1. R_delta = R_ref.T @ R_cur (delta in parent-local XR frame) + 2. R_delta_robot = R_basis @ R_delta @ R_basis.T (basis change to robot axes) -# --------------------------------------------------------------------------- -# Hand retargeter -# --------------------------------------------------------------------------- + The basis change transforms the rotation's axes from OpenXR (X-right, Y-up, + Z-back) to Robot (X-forward, Y-left, Z-up) without needing the parent's + world orientation. -class HandRetargeter: - """Maps Quest hand tracking to Inspire hand joint values.""" + Args: + R_ref: 3x3 reference relative rotation (from calibration) + R_cur: 3x3 current relative rotation - def retarget(self, left_hand_pos, right_hand_pos): - """Compute 12 Inspire hand joint values from Quest hand positions. + Returns: + 3x3 delta rotation matrix in robot coordinate frame + """ + R_delta = R_ref.T @ R_cur + return R3_ROBOT_OPENXR @ R_delta @ R3_ROBOT_OPENXR.T - Args: - left_hand_pos: (25, 3) wrist-relative hand positions (robot frame) - right_hand_pos: (25, 3) wrist-relative hand positions (robot frame) - Returns: - np.ndarray of shape (12,) with normalized hand values. - Convention: 1.0 = closed, 0.0 = open (matching Inspire DDS). - """ - hand_cmd = np.zeros(12) - - if np.any(left_hand_pos != 0): - hand_cmd[INSPIRE_L_INDEX] = self._finger_curl( - left_hand_pos, QH_INDEX_METACARPAL, QH_INDEX_PROXIMAL, - QH_INDEX_INTERMEDIATE, QH_INDEX_TIP) - hand_cmd[INSPIRE_L_MIDDLE] = self._finger_curl( - left_hand_pos, QH_MIDDLE_METACARPAL, QH_MIDDLE_PROXIMAL, - QH_MIDDLE_INTERMEDIATE, QH_MIDDLE_TIP) - hand_cmd[INSPIRE_L_RING] = self._finger_curl( - left_hand_pos, QH_RING_METACARPAL, QH_RING_PROXIMAL, - QH_RING_INTERMEDIATE, QH_RING_TIP) - hand_cmd[INSPIRE_L_PINKY] = self._finger_curl( - left_hand_pos, QH_PINKY_METACARPAL, QH_PINKY_PROXIMAL, - QH_PINKY_INTERMEDIATE, QH_PINKY_TIP) - t_pitch, t_yaw = self._thumb_angles(left_hand_pos) - hand_cmd[INSPIRE_L_THUMB_PITCH] = t_pitch - hand_cmd[INSPIRE_L_THUMB_YAW] = t_yaw - - if np.any(right_hand_pos != 0): - hand_cmd[INSPIRE_R_INDEX] = self._finger_curl( - right_hand_pos, QH_INDEX_METACARPAL, QH_INDEX_PROXIMAL, - QH_INDEX_INTERMEDIATE, QH_INDEX_TIP) - hand_cmd[INSPIRE_R_MIDDLE] = self._finger_curl( - right_hand_pos, QH_MIDDLE_METACARPAL, QH_MIDDLE_PROXIMAL, - QH_MIDDLE_INTERMEDIATE, QH_MIDDLE_TIP) - hand_cmd[INSPIRE_R_RING] = self._finger_curl( - right_hand_pos, QH_RING_METACARPAL, QH_RING_PROXIMAL, - QH_RING_INTERMEDIATE, QH_RING_TIP) - hand_cmd[INSPIRE_R_PINKY] = self._finger_curl( - right_hand_pos, QH_PINKY_METACARPAL, QH_PINKY_PROXIMAL, - QH_PINKY_INTERMEDIATE, QH_PINKY_TIP) - t_pitch, t_yaw = self._thumb_angles(right_hand_pos) - hand_cmd[INSPIRE_R_THUMB_PITCH] = t_pitch - hand_cmd[INSPIRE_R_THUMB_YAW] = t_yaw - - return hand_cmd - - def _finger_curl(self, positions, meta_idx, prox_idx, inter_idx, tip_idx): - """Compute normalized finger curl value. - - Returns: 0.0 (open) to 1.0 (closed) matching Inspire DDS convention. - """ - v1 = positions[prox_idx] - positions[meta_idx] - v2 = positions[inter_idx] - positions[prox_idx] - v3 = positions[tip_idx] - positions[inter_idx] +SOLO_JOINT = None # Set by --solo-joint arg; None = all joints active - n1 = np.linalg.norm(v1) - n2 = np.linalg.norm(v2) - n3 = np.linalg.norm(v3) +# Map solo-joint names to indices within the 17-element angles array +SOLO_JOINT_MAP = { + 'l_sh_pitch': 3, # joint 15 + 'l_sh_roll': 4, # joint 16 + 'l_sh_yaw': 5, # joint 17 + 'l_elbow': 6, # joint 18 + 'l_wr_roll': 7, # joint 19 + 'l_wr_pitch': 8, # joint 20 + 'l_wr_yaw': 9, # joint 21 +} - if n1 < 1e-6 or n2 < 1e-6 or n3 < 1e-6: - return 0.0 +def compute_all_angles(bj, cal): + """Compute all 17 upper body joint angles from body tracking data. - # Angle between consecutive bone segments - cos1 = np.clip(np.dot(v1 / n1, v2 / n2), -1.0, 1.0) - cos2 = np.clip(np.dot(v2 / n2, v3 / n3), -1.0, 1.0) - angle1 = np.arccos(cos1) - angle2 = np.arccos(cos2) + Uses calibrated references to compute delta rotations, then decomposes + into Euler angles matching the URDF joint chain axes. - # Average curl angle: 0 (straight) to ~pi/2 (curled) - avg_curl = (angle1 + angle2) / 2.0 + URDF joint chains: + Waist: yaw(Z) -> roll(X) -> pitch(Y) = ZXY Euler + Shoulder: pitch(Y) -> roll(X) -> yaw(Z) = YXZ Euler + Elbow: pitch(Y) = Y component + Wrist: roll(X) -> pitch(Y) -> yaw(Z) = XYZ Euler - # Normalize to [0, 1] where 1 = fully curled - # Max curl is approximately 1.5 rad - normalized = np.clip(avg_curl / 1.5, 0.0, 1.0) - return normalized + Args: + bj: dict from parse_body_joints() + cal: CalibrationData (must be calibrated) - def _thumb_angles(self, positions): - """Compute thumb pitch and yaw from hand positions. + Returns: + np.ndarray of shape (17,) for joint indices 12-28 + """ + 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] + 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 + + # --- 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) + + # --- Right arm (10-16): DISABLED (not calibrated, Quest tracking weak) --- + # angles[10:17] = [0, 0, 0, 0, 0, 0, 0] + + # Apply per-joint linear mapping: robot_angle = scale * xr_angle + offset + angles = JOINT_SCALE * angles + JOINT_OFFSET + + # Solo-joint mode: zero everything except the isolated joint + if SOLO_JOINT is not None and SOLO_JOINT in SOLO_JOINT_MAP: + solo_idx = SOLO_JOINT_MAP[SOLO_JOINT] + solo_val = angles[solo_idx] + angles[:] = 0.0 + angles[solo_idx] = solo_val + + return angles + + +def apply_limits(angles_17): + """Clamp 17 upper body angles to URDF joint limits.""" + for i in range(17): + joint_idx = 12 + i + lo, hi = JOINT_LIMITS[joint_idx] + angles_17[i] = np.clip(angles_17[i], lo, hi) + return angles_17 + + +def smooth_angles(current, target, alpha, max_step): + """Apply EMA smoothing + rate limiting. + + Args: + current: current angles (modified in place) + target: target angles + alpha: EMA alpha (higher = less smooth) + max_step: max change per step (rad) - Returns: (pitch_normalized, yaw_normalized) both in [0, 1]. - """ - meta = positions[QH_THUMB_METACARPAL] - prox = positions[QH_THUMB_PROXIMAL] - tip = positions[QH_THUMB_TIP] - - v1 = prox - meta - v2 = tip - prox - n1 = np.linalg.norm(v1) - n2 = np.linalg.norm(v2) - - if n1 < 1e-6 or n2 < 1e-6: - return 0.0, 0.5 - - v1n = v1 / n1 - v2n = v2 / n2 - - # Pitch: flexion angle between thumb bones - cos_pitch = np.clip(np.dot(v1n, v2n), -1.0, 1.0) - pitch_rad = np.arccos(cos_pitch) - # Inspire thumb pitch range: [0.0, 0.5] rad - # Normalize: 1.0 = max flexion (0.5 rad), 0.0 = straight (0 rad) - pitch_norm = np.clip(pitch_rad / 0.5, 0.0, 1.0) - - # Yaw: abduction (spread) of thumb - # Use the lateral component of the thumb direction relative to palm - palm = positions[QH_PALM] - index_meta = positions[QH_INDEX_METACARPAL] - palm_to_index = index_meta - palm - palm_to_index_n = np.linalg.norm(palm_to_index) - if palm_to_index_n < 1e-6: - return pitch_norm, 0.5 - - # Project thumb direction onto palm plane - palm_dir = palm_to_index / palm_to_index_n - thumb_dir = (tip - meta) - thumb_dir_n = np.linalg.norm(thumb_dir) - if thumb_dir_n < 1e-6: - return pitch_norm, 0.5 - thumb_dir = thumb_dir / thumb_dir_n - - # Yaw angle: how far the thumb points away from the index finger - cos_yaw = np.clip(np.dot(thumb_dir, palm_dir), -1.0, 1.0) - yaw_rad = np.arccos(cos_yaw) - # Inspire thumb yaw range: [-0.1, 1.3] rad. Normalize to [0, 1]. - yaw_norm = np.clip((yaw_rad - (-0.1)) / (1.3 - (-0.1)), 0.0, 1.0) - - return pitch_norm, yaw_norm + Returns: + smoothed angles + """ + blended = alpha * target + (1.0 - alpha) * current + delta = blended - current + clipped = np.clip(delta, -max_step, max_step) + return current + clipped # --------------------------------------------------------------------------- @@ -591,14 +407,12 @@ class DDSPublisher: logger.info(f"Initializing DDS (domain {domain_id})...") ChannelFactoryInitialize(domain_id) - # Body joint publisher self._body_pub = ChannelPublisher("rt/lowcmd", LowCmd_) self._body_pub.Init() self._low_cmd = unitree_hg_msg_dds__LowCmd_() self._crc = CRC() self._MotorCmd_ = unitree_go_msg_dds__MotorCmd_ - # Inspire hand publisher self._hand_pub = ChannelPublisher("rt/inspire/cmd", MotorCmds_) self._hand_pub.Init() self._MotorCmds_ = MotorCmds_ @@ -606,11 +420,6 @@ class DDSPublisher: logger.info("DDS publishers initialized (rt/lowcmd, rt/inspire/cmd)") def publish_body(self, joint_angles): - """Publish 29 joint angle commands via LowCmd_. - - Args: - joint_angles: np.ndarray of shape (29,) in radians. - """ cmd = self._low_cmd cmd.mode_pr = 0 cmd.mode_machine = 0 @@ -623,7 +432,7 @@ class DDSPublisher: cmd.motor_cmd[i].kp = float(KP[i]) cmd.motor_cmd[i].kd = float(KD[i]) - # Zero out legs (not retargeted -- leave to standing controller) + # Zero out legs (not retargeted) for i in range(12): cmd.motor_cmd[i].mode = 0 cmd.motor_cmd[i].kp = 0.0 @@ -633,11 +442,6 @@ class DDSPublisher: self._body_pub.Write(cmd) def publish_hands(self, hand_values): - """Publish 12 Inspire hand joint commands via MotorCmds_. - - Args: - hand_values: np.ndarray of shape (12,) with normalized [0,1] values. - """ hand_cmd = self._MotorCmds_() hand_cmd.cmds = [] for i in range(12): @@ -652,59 +456,74 @@ class DDSPublisher: # --------------------------------------------------------------------------- -# Shared memory reader +# Hand retargeting (dex-retargeting, unchanged) # --------------------------------------------------------------------------- -def read_body_joints(server): - """Read body joint rotation matrices from shared memory. +def init_hand_retargeting(): + from dex_retargeting.retargeting_config import RetargetingConfig + import yaml - Returns: - dict mapping BJ_* index to 3x3 rotation matrix, or None if no data. - """ - with server.body_joints_shared.get_lock(): - raw = np.array(server.body_joints_shared[:]) + urdf_dir = os.path.join(XR_TELEOP_DIR, "assets") + RetargetingConfig.set_default_urdf_dir(urdf_dir) - # Check if any data has arrived (all zeros = no data yet) - if np.all(raw == 0): - return None + yml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "inspire_hand.yml") + logger.info(f"Loading hand retargeting config from {yml_path}") + with open(yml_path) as f: + cfg = yaml.safe_load(f) - rotmats = {} - for i in range(NUM_BODY_JOINTS): - offset = i * 7 - qx, qy, qz, qw = raw[offset+3], raw[offset+4], raw[offset+5], raw[offset+6] - # Check for valid quaternion - qnorm = qx*qx + qy*qy + qz*qz + qw*qw - if qnorm < 0.5: - return None # Invalid data - rotmats[i] = quat_to_rotmat(qx, qy, qz, qw) + left_config = RetargetingConfig.from_dict(cfg['left']) + right_config = RetargetingConfig.from_dict(cfg['right']) + left_retargeting = left_config.build() + right_retargeting = right_config.build() - return rotmats + left_joint_names = left_retargeting.joint_names + right_joint_names = right_retargeting.joint_names + left_hw_names = ['L_pinky_proximal_joint', 'L_ring_proximal_joint', 'L_middle_proximal_joint', + 'L_index_proximal_joint', 'L_thumb_proximal_pitch_joint', 'L_thumb_proximal_yaw_joint'] + right_hw_names = ['R_pinky_proximal_joint', 'R_ring_proximal_joint', 'R_middle_proximal_joint', + 'R_index_proximal_joint', 'R_thumb_proximal_pitch_joint', 'R_thumb_proximal_yaw_joint'] -def read_hand_positions(server): - """Read hand joint positions from shared memory. + left_hw_map = [left_joint_names.index(n) for n in left_hw_names] + right_hw_map = [right_joint_names.index(n) for n in right_hw_names] - Returns: - (left_25x3, right_25x3) numpy arrays in robot frame. - """ - with server.left_hand_position_shared.get_lock(): - raw_left = np.array(server.left_hand_position_shared[:]) - with server.right_hand_position_shared.get_lock(): - raw_right = np.array(server.right_hand_position_shared[:]) + left_indices = left_retargeting.optimizer.target_link_human_indices + right_indices = right_retargeting.optimizer.target_link_human_indices + + logger.info("Hand retargeting initialized (dex-retargeting)") + + return (left_retargeting, right_retargeting, + left_indices, right_indices, + left_hw_map, right_hw_map) - left = np.zeros((25, 3)) - right = np.zeros((25, 3)) - if np.any(raw_left != 0): - left_openxr = raw_left.reshape(25, 3) - # Transform each position from OpenXR to robot frame - left = (R_ROBOT_OPENXR @ left_openxr.T).T +def retarget_hands(left_retargeting, right_retargeting, + left_indices, right_indices, + left_hw_map, right_hw_map, + left_hand_pos, right_hand_pos): + """Compute 12 normalized Inspire hand values from hand positions.""" + hand_cmd = np.ones(12) - if np.any(raw_right != 0): - right_openxr = raw_right.reshape(25, 3) - right = (R_ROBOT_OPENXR @ right_openxr.T).T + left_valid = not np.all(left_hand_pos == 0.0) + right_valid = not np.all(right_hand_pos == 0.0) - return left, right + if left_valid: + 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] + 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) + + 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] + 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) + + return hand_cmd # --------------------------------------------------------------------------- @@ -713,53 +532,85 @@ def read_hand_positions(server): def main(): parser = argparse.ArgumentParser( - description="Retargeting bridge: Quest 3 body tracking -> G1 DDS commands") - parser.add_argument("--port", type=int, default=8765, - help="WebSocket server port (default: 8765)") - parser.add_argument("--host", default="0.0.0.0", - help="WebSocket bind address (default: 0.0.0.0)") - parser.add_argument("--dds-domain", type=int, default=1, - help="DDS domain ID (default: 1 for sim)") - parser.add_argument("--hz", type=float, default=50.0, - help="Publishing frequency in Hz (default: 50)") - parser.add_argument("--alpha", type=float, default=0.3, - help="Smoothing factor, 0=slow 1=instant (default: 0.3)") - parser.add_argument("--no-hands", action="store_true", - help="Disable hand retargeting") - parser.add_argument("--verbose", action="store_true", - help="Enable debug logging") + description="Retargeting bridge v4: Quest 3 full body -> G1 direct joint mapping -> DDS") + parser.add_argument("--port", type=int, default=8765) + parser.add_argument("--host", default="0.0.0.0") + parser.add_argument("--dds-domain", type=int, default=1) + parser.add_argument("--hz", type=float, default=50.0) + parser.add_argument("--no-hands", action="store_true") + parser.add_argument("--no-waist", action="store_true", help="Disable waist tracking") + parser.add_argument("--no-arms", action="store_true", help="Disable arm tracking") + parser.add_argument("--mirror", action="store_true") + parser.add_argument("--verbose", action="store_true") + parser.add_argument("--cal-delay", type=float, default=10.0, + help="Seconds to wait before auto-calibrating (default: 10.0)") + parser.add_argument("--gain", type=float, default=None, + help="Joint angle gain multiplier (default: 1.4)") + parser.add_argument("--test", choices=["left", "right", "both", "wave"]) + parser.add_argument("--test-hands", action="store_true") + parser.add_argument("--solo-joint", type=str, default=None, + help="Isolate a single joint for testing. Options: " + "l_sh_pitch, l_sh_roll, l_sh_yaw, l_elbow, " + "l_wr_roll, l_wr_pitch, l_wr_yaw") args = parser.parse_args() + if args.solo_joint is not None: + global SOLO_JOINT + SOLO_JOINT = args.solo_joint + logging.basicConfig( level=logging.DEBUG if args.verbose else logging.INFO, format="%(asctime)s [%(name)s] %(levelname)s: %(message)s" ) - # --- Start teleop server --- + # --- Start teleop server (WebSocket + shared memory) --- logger.info(f"Starting teleop server on ws://{args.host}:{args.port}") - server = TeleopServer(host=args.host, port=args.port) - server_thread = server.start_in_thread() + tv_wrapper = NativeTeleWrapper(port=args.port, host=args.host) + tv_wrapper.start() # --- Initialize DDS --- - dds = DDSPublisher(domain_id=args.dds_domain) - - # --- Initialize retargeters --- - body_retargeter = BodyRetargeter() - hand_retargeter = HandRetargeter() - body_smoother = JointSmoother(NUM_JOINTS, alpha=args.alpha, max_delta=0.15) - hand_smoother = JointSmoother(12, alpha=0.4, max_delta=0.2) - - # --- Deadman / timeout state --- - tracking_timeout = 0.5 # seconds - return_to_zero_rate = 0.02 # rad/step + dds = None + for attempt in range(60): + try: + dds = DDSPublisher(domain_id=args.dds_domain) + break + except Exception as e: + logger.warning(f"DDS init attempt {attempt+1} failed ({e}), retrying in 2s...") + time.sleep(2.0) + if dds is None: + logger.error("Failed to initialize DDS after 60 attempts, exiting.") + sys.exit(1) + + # --- Test mode --- + if args.test: + _run_test_mode(args, dds) + return + + # --- Initialize hand retargeting --- + hand_data = init_hand_retargeting() if not args.no_hands else None + if hand_data: + l_retarget, r_retarget, l_indices, r_indices, l_hw_map, r_hw_map = hand_data + + # --- Control loop state --- + interval = 1.0 / args.hz + max_step = MAX_RATE * interval # rad per step + running = True + current_angles = np.zeros(NUM_JOINTS) + smoothed_upper = np.zeros(17) # joints 12-28 + current_hands = np.ones(12) + tracking_timeout = 0.5 + return_to_zero_rate = 0.02 last_valid_time = 0.0 - startup_ramp = 0.0 # 0.0 -> 1.0 over 2 seconds + startup_ramp = 0.0 startup_ramp_duration = 2.0 startup_time = None + step_count = 0 + retarget_error_count = 0 - # --- Control loop --- - interval = 1.0 / args.hz - running = True + # Calibration state + cal = CalibrationData() + first_data_time = None + was_tracking = False # track connection state for re-calibration def signal_handler(sig, frame): nonlocal running @@ -768,46 +619,119 @@ def main(): signal.signal(signal.SIGINT, signal_handler) signal.signal(signal.SIGTERM, signal_handler) - logger.info(f"Bridge running at {args.hz} Hz. Waiting for Quest 3 connection...") - step_count = 0 - current_angles = np.zeros(NUM_JOINTS) - current_hands = np.zeros(12) + logger.info(f"Bridge v4 running at {args.hz} Hz (DIRECT retargeting, no IK)") + 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("Waiting for Quest 3...") while running: t_start = time.perf_counter() - # Read body tracking data - body_rotmats = read_body_joints(server) - - if body_rotmats is not None: - now = time.time() - with server.last_data_time_shared.get_lock(): - data_time = server.last_data_time_shared.value - - if data_time > 0: - last_valid_time = now - - # Retarget body - raw_angles = body_retargeter.retarget(body_rotmats) - smoothed_angles = body_smoother.smooth(raw_angles) - - # Startup ramp - if startup_time is None: - startup_time = now - logger.info("First tracking data received -- starting ramp-up") - elapsed = now - startup_time - startup_ramp = min(elapsed / startup_ramp_duration, 1.0) - current_angles = smoothed_angles * startup_ramp - - # Retarget hands - if not args.no_hands: - left_hand, right_hand = read_hand_positions(server) - raw_hands = hand_retargeter.retarget(left_hand, right_hand) - current_hands = hand_smoother.smooth(raw_hands) * startup_ramp + # Read tracking data (for hand positions) + tele_data = tv_wrapper.get_tele_data() + data_time = tv_wrapper.last_data_time + now = time.time() + + # Read body joints directly from shared memory (world-space pose7) + with tv_wrapper.server.body_joints_shared.get_lock(): + bj_raw = np.array(tv_wrapper.server.body_joints_shared[:]) + has_body_joints = np.any(bj_raw != 0) + + if data_time > 0 and (now - data_time) < tracking_timeout: + last_valid_time = now + is_tracking = True + + # Track when first data arrives + if first_data_time is None: + first_data_time = now + logger.info("First tracking data received") + + # 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 + + was_tracking = True + + # Startup ramp (starts after calibration) + if startup_time is not None: + 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) + raw_angles = apply_limits(raw_angles) + + # Zero out disabled groups + if args.no_waist: + raw_angles[0:3] = 0.0 + if args.no_arms: + raw_angles[3:17] = 0.0 + + # Smooth + smoothed_upper = smooth_angles(smoothed_upper, raw_angles, EMA_ALPHA, max_step) + + # 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}") + + # --- Hand retargeting --- (DISABLED: testing left arm only) + if False and hand_data and cal.calibrated: + left_hand_pos = tele_data.left_hand_pos + right_hand_pos = tele_data.right_hand_pos + + if args.mirror: + left_hand_pos, right_hand_pos = right_hand_pos, left_hand_pos + + try: + raw_hands = retarget_hands( + l_retarget, r_retarget, + l_indices, r_indices, + l_hw_map, r_hw_map, + left_hand_pos, right_hand_pos) + current_hands = raw_hands * startup_ramp + (1.0 - startup_ramp) * 1.0 + except Exception as e: + logger.warning(f"Hand retarget failed: {e}") else: - # No data -- check timeout - now = time.time() + # No tracking data or stale + was_tracking = False if last_valid_time > 0 and (now - last_valid_time) > tracking_timeout: # Gradually return to zero for i in range(12, NUM_JOINTS): @@ -815,32 +739,112 @@ def main(): current_angles[i] -= np.sign(current_angles[i]) * return_to_zero_rate else: current_angles[i] = 0.0 - # Return hands to open - current_hands = np.maximum(current_hands - 0.02, 0.0) + smoothed_upper *= 0.95 # decay smoothed state too + current_hands = np.minimum(current_hands + 0.02, 1.0) - # Publish + # --- Publish --- dds.publish_body(current_angles) - if not args.no_hands: + if hand_data: dds.publish_hands(current_hands) - # Status logging + # --- Status logging --- step_count += 1 - if step_count % int(args.hz * 5) == 0: # Every 5 seconds - status = "TRACKING" if body_rotmats is not None else "WAITING" + log_interval = int(args.hz * (1 if args.verbose else 5)) + if log_interval > 0 and step_count % log_interval == 0: + tracking = data_time > 0 and (now - data_time) < tracking_timeout + status = "TRACKING" if (tracking and cal.calibrated) else ( + "CALIBRATING" if (tracking and not cal.calibrated) else "WAITING") ramp_pct = int(startup_ramp * 100) - logger.info(f"[{status}] step={step_count} ramp={ramp_pct}% " - f"waist=[{current_angles[12]:.2f},{current_angles[13]:.2f},{current_angles[14]:.2f}] " - f"L_shoulder=[{current_angles[15]:.2f},{current_angles[16]:.2f},{current_angles[17]:.2f}] " - f"R_shoulder=[{current_angles[22]:.2f},{current_angles[23]:.2f},{current_angles[24]:.2f}]") - - # Sleep to maintain target frequency - elapsed = time.perf_counter() - t_start - sleep_time = interval - elapsed + waist = current_angles[12:15] + l_sh = current_angles[15:18] + l_el = current_angles[18] + r_sh = current_angles[22:25] + r_el = current_angles[25] + logger.info( + f"[{status}] step={step_count} ramp={ramp_pct}% " + 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 args.verbose and tracking and has_body_joints: + logger.debug( + f" Full upper body: {np.array2string(current_angles[12:29], precision=3, separator=',')}") + # Log raw quaternions for L/R upper arms to diagnose tracking issues + l_upper_q = bj_raw[BJ_L_UPPER*7:(BJ_L_UPPER+1)*7] + r_upper_q = bj_raw[BJ_R_UPPER*7:(BJ_R_UPPER+1)*7] + l_lower_q = bj_raw[BJ_L_LOWER*7:(BJ_L_LOWER+1)*7] + r_lower_q = bj_raw[BJ_R_LOWER*7:(BJ_R_LOWER+1)*7] + logger.debug( + f" L_upper_q=[{l_upper_q[3]:+.4f},{l_upper_q[4]:+.4f},{l_upper_q[5]:+.4f},{l_upper_q[6]:+.4f}] " + f"R_upper_q=[{r_upper_q[3]:+.4f},{r_upper_q[4]:+.4f},{r_upper_q[5]:+.4f},{r_upper_q[6]:+.4f}]") + logger.debug( + f" L_lower_q=[{l_lower_q[3]:+.4f},{l_lower_q[4]:+.4f},{l_lower_q[5]:+.4f},{l_lower_q[6]:+.4f}] " + f"R_lower_q=[{r_lower_q[3]:+.4f},{r_lower_q[4]:+.4f},{r_lower_q[5]:+.4f},{r_lower_q[6]:+.4f}]") + if retarget_error_count > 0: + logger.debug(f" Retarget errors: {retarget_error_count}") + + sleep_time = interval - (time.perf_counter() - t_start) if sleep_time > 0: time.sleep(sleep_time) logger.info("Bridge shutting down") +def _run_test_mode(args, dds): + """Run test mode: send known poses without Quest.""" + logger.info(f"TEST MODE: '{args.test}' — sending fixed poses (no Quest needed)") + running = True + + def signal_handler(sig, frame): + nonlocal running + running = False + + signal.signal(signal.SIGINT, signal_handler) + signal.signal(signal.SIGTERM, signal_handler) + + interval = 1.0 / args.hz + step_count = 0 + t0 = time.time() + + while running: + t_start = time.perf_counter() + angles = np.zeros(NUM_JOINTS) + elapsed = time.time() - t0 + + if args.test == "left": + angles[15] = 0.5; angles[16] = 0.3; angles[18] = 0.4 + elif args.test == "right": + angles[22] = 0.5; angles[23] = -0.3; angles[25] = 0.4 + elif args.test == "both": + angles[15] = 0.5; angles[16] = 0.3; angles[18] = 0.4 + angles[22] = -0.5; angles[23] = -0.3; angles[25] = 0.8 + elif args.test == "wave": + wave = np.sin(elapsed * 1.5) + angles[15] = 0.8 * wave; angles[18] = 0.3 + 0.3 * wave + angles[22] = -0.8 * wave; angles[25] = 0.3 - 0.3 * wave + + dds.publish_body(angles) + + if args.test_hands: + hand_vals = np.ones(12) + cycle = (elapsed % 4.0) / 4.0 + curl = 1.0 - np.sin(cycle * np.pi) + for i in [0, 1, 2, 3, 6, 7, 8, 9]: + hand_vals[i] = curl + hand_vals[4] = 0.5 + 0.5 * curl + hand_vals[10] = 0.5 + 0.5 * curl + hand_vals[5] = 0.5; hand_vals[11] = 0.5 + dds.publish_hands(hand_vals) + + step_count += 1 + if step_count % int(args.hz * 2) == 0: + logger.info(f"[TEST:{args.test}] step={step_count}") + + sleep_time = interval - (time.perf_counter() - t_start) + if sleep_time > 0: + time.sleep(sleep_time) + + logger.info("Test mode shutting down") + + if __name__ == "__main__": main() diff --git a/server/teleop_server.py b/server/teleop_server.py index e23d4b2..61599be 100644 --- a/server/teleop_server.py +++ b/server/teleop_server.py @@ -257,8 +257,8 @@ class TeleopServer: self.host, self.port, max_size=2**20, # 1 MB max message size - ping_interval=20, - ping_timeout=20, + ping_interval=30, + ping_timeout=60, ) as server: logger.info(f"Teleop server running on ws://{self.host}:{self.port}") await asyncio.Future() # Run forever