Browse Source
Add retargeting bridge for Isaac Sim teleoperation
Add retargeting bridge for Isaac Sim teleoperation
Adds a joint retargeting bridge that converts Quest 3 body tracking data into G1 robot joint commands for Isaac Sim via CycloneDDS. - Extend body_tracker.gd with 8 body joint transforms (56 floats) - Extend teleop_server.py with body_joints_shared memory - Add retarget_bridge.py: body retargeting, hand mapping, DDS publishing - Add launch_bridge.py: one-command launcher for sim + bridge - Update requirements.txt with unitree-sdk2py dependency Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>master
melancholytron
4 weeks ago
5 changed files with 1090 additions and 1 deletions
-
20scripts/body_tracker.gd
-
207server/launch_bridge.py
-
5server/requirements.txt
-
846server/retarget_bridge.py
-
13server/teleop_server.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() |
||||
@ -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() |
||||
Write
Preview
Loading…
Cancel
Save
Reference in new issue