diff --git a/scripts/body_tracker.gd b/scripts/body_tracker.gd index a702e3d..db1fe8f 100644 --- a/scripts/body_tracker.gd +++ b/scripts/body_tracker.gd @@ -189,6 +189,26 @@ func _process(_delta: float) -> void: data["left_hand_rot"] = left_hand_rot data["right_hand_rot"] = right_hand_rot + # Body joint transforms for retargeting (world-space pose7) + # Order: hips, chest, L_upper_arm, L_lower_arm, L_wrist, R_upper_arm, R_lower_arm, R_wrist + # Each joint: [x, y, z, qx, qy, qz, qw] = 7 floats, total 56 floats + var hips_xform := tracker.get_joint_transform(JOINT_HIPS) + var left_upper_arm_xform := tracker.get_joint_transform(JOINT_LEFT_ARM_UPPER) + var left_lower_arm_xform := tracker.get_joint_transform(JOINT_LEFT_ARM_LOWER) + 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) + + 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(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)) + data["body_joints"] = body_joints + # Debug logging every ~2 seconds if debug_log: _log_counter += 1 diff --git a/server/launch_bridge.py b/server/launch_bridge.py new file mode 100755 index 0000000..34f58c3 --- /dev/null +++ b/server/launch_bridge.py @@ -0,0 +1,207 @@ +#!/usr/bin/env python3 +""" +Launch Isaac Sim + retargeting bridge, and print the Quest 3 connection URL. + +Starts everything needed in one command: + 1. Isaac Sim (G1 with Inspire hands, DDS teleoperation) + 2. Retargeting bridge (WebSocket server + body retargeting + DDS publisher) + +Usage: + python launch_bridge.py + python launch_bridge.py --task Isaac-PickPlace-RedBlock-G129-Inspire-Joint + python launch_bridge.py --no-sim # bridge only (if sim is already running) + python launch_bridge.py --device cuda # use GPU for simulation +""" + +import socket +import subprocess +import sys +import os +import signal +import time +import threading + + +SIM_DIR = os.path.expanduser("~/git/unitree_sim_isaaclab") +BRIDGE_DIR = os.path.dirname(os.path.abspath(__file__)) + +# Default Isaac Sim arguments +DEFAULT_TASK = "Isaac-PickPlace-Cylinder-G129-Inspire-Joint" +DEFAULT_DEVICE = "cpu" + +# Message printed by sim when DDS is ready +SIM_READY_MARKER = "start controller success" + + +def get_local_ip(): + """Get local network IP via UDP socket (no traffic sent).""" + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + try: + s.connect(("8.8.8.8", 80)) + return s.getsockname()[0] + finally: + s.close() + + +def stream_and_watch(proc, marker, event): + """Read process stdout line-by-line, print it, and set event when marker seen.""" + for line in iter(proc.stdout.readline, ""): + sys.stdout.write(f" [sim] {line}") + sys.stdout.flush() + if marker in line: + event.set() + proc.stdout.close() + + +def stream_stderr(proc): + """Forward process stderr.""" + for line in iter(proc.stderr.readline, ""): + sys.stderr.write(f" [sim] {line}") + sys.stderr.flush() + proc.stderr.close() + + +def main(): + # --- Parse our args (separate from bridge args) --- + port = 8765 + task = DEFAULT_TASK + device = DEFAULT_DEVICE + no_sim = False + bridge_args = [] + + # Simple arg parser that pulls out our flags and passes the rest to the bridge + args = sys.argv[1:] + i = 0 + while i < len(args): + if args[i] == "--port" and i + 1 < len(args): + port = int(args[i + 1]) + bridge_args.extend(args[i:i+2]) + i += 2 + elif args[i] == "--task" and i + 1 < len(args): + task = args[i + 1] + i += 2 + elif args[i] == "--device" and i + 1 < len(args): + device = args[i + 1] + i += 2 + elif args[i] == "--no-sim": + no_sim = True + i += 1 + else: + bridge_args.append(args[i]) + i += 1 + + ip = get_local_ip() + + # --- Banner --- + print() + print("=" * 60) + print(" G1 Teleop -> Isaac Sim Retargeting Bridge") + print("=" * 60) + print() + print(f" Quest 3 connection:") + print(f" IP: {ip}") + print(f" Port: {port}") + print() + if not no_sim: + print(f" Isaac Sim:") + print(f" Task: {task}") + print(f" Device: {device}") + print() + print("=" * 60) + print() + sys.stdout.flush() + + sim_proc = None + bridge_proc = None + + def cleanup(sig=None, frame=None): + """Shut down both processes on exit.""" + print("\nShutting down...") + if bridge_proc and bridge_proc.poll() is None: + bridge_proc.send_signal(signal.SIGINT) + bridge_proc.wait(timeout=5) + if sim_proc and sim_proc.poll() is None: + sim_proc.send_signal(signal.SIGINT) + sim_proc.wait(timeout=10) + sys.exit(0) + + signal.signal(signal.SIGINT, cleanup) + signal.signal(signal.SIGTERM, cleanup) + + # --- Launch Isaac Sim --- + if not no_sim: + sim_cmd = [ + sys.executable, os.path.join(SIM_DIR, "sim_main.py"), + "--device", device, + "--enable_cameras", + "--task", task, + "--enable_inspire_dds", + "--robot_type", "g129", + ] + print(f"[1/2] Starting Isaac Sim...") + print(f" {' '.join(sim_cmd)}") + print() + sys.stdout.flush() + + sim_proc = subprocess.Popen( + sim_cmd, + cwd=SIM_DIR, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + bufsize=1, + ) + + # Stream sim output and watch for ready marker + sim_ready = threading.Event() + stdout_thread = threading.Thread( + target=stream_and_watch, args=(sim_proc, SIM_READY_MARKER, sim_ready), + daemon=True) + stderr_thread = threading.Thread( + target=stream_stderr, args=(sim_proc,), + daemon=True) + stdout_thread.start() + stderr_thread.start() + + # Wait for sim to be ready (timeout after 120s) + print(" Waiting for Isaac Sim to initialize (this may take a minute)...") + sys.stdout.flush() + if not sim_ready.wait(timeout=120): + if sim_proc.poll() is not None: + print("\n ERROR: Isaac Sim exited unexpectedly.") + sys.exit(1) + print("\n WARNING: Timed out waiting for sim ready marker.") + print(" Starting bridge anyway (sim may still be loading).") + else: + print(" Isaac Sim is ready!") + print() + sys.stdout.flush() + + # --- Launch bridge --- + bridge_cmd = [ + sys.executable, os.path.join(BRIDGE_DIR, "retarget_bridge.py"), + "--port", str(port), + ] + bridge_args + + step = "2/2" if not no_sim else "1/1" + print(f"[{step}] Starting retargeting bridge...") + print() + print("=" * 60) + print(f" READY -- connect Quest 3 to: {ip}:{port}") + print("=" * 60) + print() + sys.stdout.flush() + + bridge_proc = subprocess.Popen(bridge_cmd, cwd=BRIDGE_DIR) + + try: + # Wait for bridge to exit (or Ctrl-C) + bridge_proc.wait() + except KeyboardInterrupt: + pass + finally: + cleanup() + + +if __name__ == "__main__": + main() diff --git a/server/requirements.txt b/server/requirements.txt index 939c4d4..15f4dff 100644 --- a/server/requirements.txt +++ b/server/requirements.txt @@ -2,3 +2,8 @@ websockets>=12.0 numpy>=1.24 opencv-python>=4.8 + +# Retargeting bridge (DDS communication with Isaac Sim / real robot) +# Install from: /home/sparky/git/unitree_sim_deps/unitree_sdk2_python +# pip install -e /home/sparky/git/unitree_sim_deps/unitree_sdk2_python +unitree-sdk2py diff --git a/server/retarget_bridge.py b/server/retarget_bridge.py new file mode 100644 index 0000000..3ce7d6d --- /dev/null +++ b/server/retarget_bridge.py @@ -0,0 +1,846 @@ +#!/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] + +Architecture: + Quest 3 -> WebSocket -> TeleopServer (shared memory) + | + retarget_bridge.py + | + CycloneDDS -> Isaac Sim +""" + +import sys +import os +import time +import argparse +import logging +import signal +import numpy as np +from multiprocessing import Array + +# Local imports +from teleop_server import TeleopServer + +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) +# --------------------------------------------------------------------------- +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 + +# --------------------------------------------------------------------------- +# URDF joint limits (from g1_custom_collision_29dof.urdf) +# --------------------------------------------------------------------------- +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 +} + +SOFT_LIMIT_FACTOR = 0.9 + +# Default Kp/Kd from Unitree SDK example +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 +] +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, +] + +# 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) + +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) + +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) + + +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) + + +def euler_zxy(R): + """Decompose R = Rz(yaw) * Rx(roll) * Ry(pitch). Returns (yaw, roll, pitch). + + 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 + + +def euler_yxz(R): + """Decompose R = Ry(pitch) * Rx(roll) * Rz(yaw). Returns (pitch, roll, yaw). + + 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 + + +def euler_xyz(R): + """Decompose R = Rx(roll) * Ry(pitch) * Rz(yaw). Returns (roll, pitch, yaw). + + 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 extract_y_rotation(R): + """Extract the Y-axis rotation angle from a rotation matrix. + + Used for elbow (single-axis pitch). + """ + return np.arctan2(R[0, 2], R[0, 0]) + + +# 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 + + +# --------------------------------------------------------------------------- +# Joint smoother +# --------------------------------------------------------------------------- + +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 BodyRetargeter: + """Converts body tracking poses to G1 joint angles via orientation decomposition.""" + + 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. + + Args: + body_rotmats: dict mapping joint index (BJ_*) to 3x3 rotation matrix + in OpenXR world frame. + + Returns: + np.ndarray of shape (29,) with joint angles in radians. + """ + 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 self._calibrated: + self._ref_rotations['l_wrist'] = R_l_wrist_rel.copy() + + 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 + + # --- 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 + + 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") + + +# --------------------------------------------------------------------------- +# Hand retargeter +# --------------------------------------------------------------------------- + +class HandRetargeter: + """Maps Quest hand tracking to Inspire hand joint values.""" + + def retarget(self, left_hand_pos, right_hand_pos): + """Compute 12 Inspire hand joint values from Quest hand positions. + + 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] + + n1 = np.linalg.norm(v1) + n2 = np.linalg.norm(v2) + n3 = np.linalg.norm(v3) + + if n1 < 1e-6 or n2 < 1e-6 or n3 < 1e-6: + return 0.0 + + # 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) + + # Average curl angle: 0 (straight) to ~pi/2 (curled) + avg_curl = (angle1 + angle2) / 2.0 + + # 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 + + def _thumb_angles(self, positions): + """Compute thumb pitch and yaw from hand positions. + + 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 + + +# --------------------------------------------------------------------------- +# DDS publisher +# --------------------------------------------------------------------------- + +class DDSPublisher: + """Publishes G1 joint commands and Inspire hand commands via CycloneDDS.""" + + def __init__(self, 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 + from unitree_sdk2py.idl.unitree_go.msg.dds_ import MotorCmds_ + from unitree_sdk2py.idl.default import unitree_go_msg_dds__MotorCmd_ + + 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_ + + 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 + + for i in range(NUM_JOINTS): + cmd.motor_cmd[i].mode = 1 + cmd.motor_cmd[i].q = float(joint_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 out legs (not retargeted -- leave to standing controller) + 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 = self._crc.Crc(cmd) + 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): + mc = self._MotorCmd_() + mc.q = float(np.clip(hand_values[i], 0.0, 1.0)) + mc.dq = 0.0 + mc.tau = 0.0 + mc.kp = 5.0 + mc.kd = 0.5 + hand_cmd.cmds.append(mc) + self._hand_pub.Write(hand_cmd) + + +# --------------------------------------------------------------------------- +# Shared memory reader +# --------------------------------------------------------------------------- + +def read_body_joints(server): + """Read body joint rotation matrices from shared memory. + + 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[:]) + + # Check if any data has arrived (all zeros = no data yet) + if np.all(raw == 0): + return None + + 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) + + return rotmats + + +def read_hand_positions(server): + """Read hand joint positions from shared memory. + + 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 = 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 + + if np.any(raw_right != 0): + right_openxr = raw_right.reshape(25, 3) + right = (R_ROBOT_OPENXR @ right_openxr.T).T + + return left, right + + +# --------------------------------------------------------------------------- +# Main loop +# --------------------------------------------------------------------------- + +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") + args = parser.parse_args() + + logging.basicConfig( + level=logging.DEBUG if args.verbose else logging.INFO, + format="%(asctime)s [%(name)s] %(levelname)s: %(message)s" + ) + + # --- Start teleop server --- + 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() + + # --- 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 + last_valid_time = 0.0 + startup_ramp = 0.0 # 0.0 -> 1.0 over 2 seconds + startup_ramp_duration = 2.0 + startup_time = None + + # --- Control loop --- + interval = 1.0 / args.hz + running = True + + def signal_handler(sig, frame): + nonlocal running + running = False + + 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) + + 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 + + else: + # No data -- check timeout + now = time.time() + if last_valid_time > 0 and (now - last_valid_time) > tracking_timeout: + # Gradually return to zero + for i in range(12, NUM_JOINTS): + if abs(current_angles[i]) > return_to_zero_rate: + 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) + + # Publish + dds.publish_body(current_angles) + if not args.no_hands: + dds.publish_hands(current_hands) + + # 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" + 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 + if sleep_time > 0: + time.sleep(sleep_time) + + logger.info("Bridge shutting down") + + +if __name__ == "__main__": + main() diff --git a/server/teleop_server.py b/server/teleop_server.py index e4e4290..e23d4b2 100644 --- a/server/teleop_server.py +++ b/server/teleop_server.py @@ -68,7 +68,8 @@ class TeleopServer: left_hand_pinch_shared=None, left_hand_squeeze_shared=None, right_hand_pinch_shared=None, - right_hand_squeeze_shared=None): + right_hand_squeeze_shared=None, + body_joints_shared=None): self.host = host self.port = port @@ -91,6 +92,10 @@ class TeleopServer: self.right_hand_pinch_shared = right_hand_pinch_shared self.right_hand_squeeze_shared = right_hand_squeeze_shared + # Body joint transforms for retargeting (8 joints x 7 floats = 56) + # Order: hips, chest, L_upper_arm, L_lower_arm, L_wrist, R_upper_arm, R_lower_arm, R_wrist + self.body_joints_shared = body_joints_shared or Array('d', 56, lock=True) + # Initialize with identity matrices identity_flat = np.eye(4).flatten(order='F').tolist() with self.head_pose_shared.get_lock(): @@ -225,6 +230,12 @@ class TeleopServer: with self.right_hand_orientation_shared.get_lock(): self.right_hand_orientation_shared[:] = right_hand_rot + # Body joint transforms: 56 floats (8 joints × 7) + body_joints = data.get("body_joints") + if body_joints and len(body_joints) == 56: + with self.body_joints_shared.get_lock(): + self.body_joints_shared[:] = body_joints + async def _send_webcam_loop(self, websocket): """Send webcam JPEG frames to a client at ~15 fps.""" interval = 1.0 / 15.0