Browse Source
Add native Quest 3 teleop support (--native flag)
Add native Quest 3 teleop support (--native flag)
- teleop_server.py: lightweight WebSocket server replacing Vuer (~250 lines) - native_tv_wrapper.py: drop-in TeleVuerWrapper replacement using chest-relative body tracking - teleop_hand_and_arm.py: --native/--native-port args, skip head pos compensation in native mode Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>main
3 changed files with 683 additions and 20 deletions
@ -0,0 +1,330 @@ |
|||||
|
""" |
||||
|
Native TeleVuerWrapper — drop-in replacement for TeleVuerWrapper that uses |
||||
|
the Quest 3 native app via TeleopServer instead of Vuer/browser. |
||||
|
|
||||
|
Returns TeleData in the same format as the original TeleVuerWrapper, but: |
||||
|
- Wrist poses arrive chest-relative from body tracking (no head subtraction needed) |
||||
|
- Chest orientation is available for body rotation decoupling |
||||
|
- No SSL, no browser, no Vuer dependency |
||||
|
|
||||
|
The coordinate transform pipeline is preserved: |
||||
|
1. Quest 3 sends poses in Godot's coordinate system (Y-up, -Z forward) |
||||
|
2. This wrapper converts to robot convention (X-forward, Y-left, Z-up) |
||||
|
3. Applies Unitree arm URDF convention transforms |
||||
|
4. Applies head-to-waist offset (same as original) |
||||
|
|
||||
|
Usage: |
||||
|
# Instead of: |
||||
|
# tv_wrapper = TeleVuerWrapper(use_hand_tracking=True, ...) |
||||
|
# Use: |
||||
|
# tv_wrapper = NativeTeleWrapper(port=8765) |
||||
|
# tv_wrapper.start() # Starts WebSocket server in background |
||||
|
""" |
||||
|
|
||||
|
import numpy as np |
||||
|
import time |
||||
|
import threading |
||||
|
from multiprocessing import Array, Value |
||||
|
from dataclasses import dataclass, field |
||||
|
from typing import Optional |
||||
|
|
||||
|
from teleop_server import TeleopServer |
||||
|
|
||||
|
|
||||
|
# --- Coordinate Transform Constants --- |
||||
|
# Godot coordinate system: Y-up, -Z forward, X-right |
||||
|
# This is equivalent to OpenXR convention. |
||||
|
# Robot convention: X-forward, Y-left, Z-up |
||||
|
|
||||
|
# Basis change: OpenXR (Y-up, -Z fwd) → Robot (Z-up, X fwd) |
||||
|
T_ROBOT_OPENXR = np.array([[ 0, 0,-1, 0], |
||||
|
[-1, 0, 0, 0], |
||||
|
[ 0, 1, 0, 0], |
||||
|
[ 0, 0, 0, 1]], dtype=np.float64) |
||||
|
|
||||
|
T_OPENXR_ROBOT = np.array([[ 0,-1, 0, 0], |
||||
|
[ 0, 0, 1, 0], |
||||
|
[-1, 0, 0, 0], |
||||
|
[ 0, 0, 0, 1]], dtype=np.float64) |
||||
|
|
||||
|
R_ROBOT_OPENXR = T_ROBOT_OPENXR[:3, :3] |
||||
|
R_OPENXR_ROBOT = T_OPENXR_ROBOT[:3, :3] |
||||
|
|
||||
|
# Arm initial pose convention transforms |
||||
|
T_TO_UNITREE_HUMANOID_LEFT_ARM = np.array([[1, 0, 0, 0], |
||||
|
[0, 0,-1, 0], |
||||
|
[0, 1, 0, 0], |
||||
|
[0, 0, 0, 1]], dtype=np.float64) |
||||
|
|
||||
|
T_TO_UNITREE_HUMANOID_RIGHT_ARM = np.array([[1, 0, 0, 0], |
||||
|
[0, 0, 1, 0], |
||||
|
[0,-1, 0, 0], |
||||
|
[0, 0, 0, 1]], dtype=np.float64) |
||||
|
|
||||
|
# Hand convention transform |
||||
|
T_TO_UNITREE_HAND = np.array([[0, 0, 1, 0], |
||||
|
[-1, 0, 0, 0], |
||||
|
[0, -1, 0, 0], |
||||
|
[0, 0, 0, 1]], dtype=np.float64) |
||||
|
|
||||
|
# Default poses (fallback when no data) |
||||
|
CONST_HEAD_POSE = np.array([[1, 0, 0, 0], |
||||
|
[0, 1, 0, 1.5], |
||||
|
[0, 0, 1, -0.2], |
||||
|
[0, 0, 0, 1]], dtype=np.float64) |
||||
|
|
||||
|
CONST_LEFT_ARM_POSE = np.array([[1, 0, 0, -0.15], |
||||
|
[0, 1, 0, 1.13], |
||||
|
[0, 0, 1, -0.3], |
||||
|
[0, 0, 0, 1]], dtype=np.float64) |
||||
|
|
||||
|
CONST_RIGHT_ARM_POSE = np.array([[1, 0, 0, 0.15], |
||||
|
[0, 1, 0, 1.13], |
||||
|
[0, 0, 1, -0.3], |
||||
|
[0, 0, 0, 1]], dtype=np.float64) |
||||
|
|
||||
|
|
||||
|
def fast_mat_inv(mat): |
||||
|
"""Fast SE(3) inverse using rotation transpose.""" |
||||
|
ret = np.eye(4) |
||||
|
ret[:3, :3] = mat[:3, :3].T |
||||
|
ret[:3, 3] = -mat[:3, :3].T @ mat[:3, 3] |
||||
|
return ret |
||||
|
|
||||
|
|
||||
|
def safe_mat_update(prev_mat, mat): |
||||
|
"""Return previous matrix if new matrix is singular.""" |
||||
|
det = np.linalg.det(mat) |
||||
|
if not np.isfinite(det) or np.isclose(det, 0.0, atol=1e-6): |
||||
|
return prev_mat, False |
||||
|
return mat, True |
||||
|
|
||||
|
|
||||
|
@dataclass |
||||
|
class TeleData: |
||||
|
"""Matches the TeleData from tv_wrapper.py exactly.""" |
||||
|
head_pose: np.ndarray # (4,4) SE(3) pose of head |
||||
|
left_wrist_pose: np.ndarray # (4,4) SE(3) pose of left wrist (IK frame) |
||||
|
right_wrist_pose: np.ndarray # (4,4) SE(3) pose of right wrist (IK frame) |
||||
|
chest_pose: np.ndarray = None # (4,4) SE(3) NEW: chest orientation |
||||
|
# Hand tracking |
||||
|
left_hand_pos: np.ndarray = None # (25,3) 3D positions of left hand joints |
||||
|
right_hand_pos: np.ndarray = None # (25,3) 3D positions of right hand joints |
||||
|
left_hand_rot: np.ndarray = None # (25,3,3) rotation matrices |
||||
|
right_hand_rot: np.ndarray = None # (25,3,3) rotation matrices |
||||
|
# Hand state (computed from finger positions) |
||||
|
left_hand_pinch: bool = False |
||||
|
left_hand_pinchValue: float = 10.0 |
||||
|
left_hand_squeeze: bool = False |
||||
|
left_hand_squeezeValue: float = 0.0 |
||||
|
right_hand_pinch: bool = False |
||||
|
right_hand_pinchValue: float = 10.0 |
||||
|
right_hand_squeeze: bool = False |
||||
|
right_hand_squeezeValue: float = 0.0 |
||||
|
|
||||
|
|
||||
|
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. |
||||
|
""" |
||||
|
|
||||
|
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): |
||||
|
""" |
||||
|
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. |
||||
|
""" |
||||
|
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 |
||||
|
|
||||
|
def start(self): |
||||
|
"""Start the WebSocket server in a background thread.""" |
||||
|
self._server_thread = self.server.start_in_thread() |
||||
|
|
||||
|
def get_tele_data(self) -> TeleData: |
||||
|
"""Get processed motion data, transformed to robot convention. |
||||
|
|
||||
|
Returns TeleData compatible with teleop_hand_and_arm.py. |
||||
|
""" |
||||
|
# Read raw poses from shared memory (column-major 4x4) |
||||
|
with self.server.head_pose_shared.get_lock(): |
||||
|
head_raw = np.array(self.server.head_pose_shared[:]).reshape(4, 4, order='F') |
||||
|
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') |
||||
|
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') |
||||
|
|
||||
|
# 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) |
||||
|
|
||||
|
# --- Transform from Godot/OpenXR convention to Robot convention --- |
||||
|
|
||||
|
# Head: basis change (world-space pose) |
||||
|
Brobot_world_head = T_ROBOT_OPENXR @ head_raw @ T_OPENXR_ROBOT |
||||
|
|
||||
|
# Chest: basis change (world-space pose) |
||||
|
Brobot_world_chest = T_ROBOT_OPENXR @ chest_raw @ 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)) |
||||
|
|
||||
|
# 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. |
||||
|
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 |
||||
|
|
||||
|
# --- Hand positions --- |
||||
|
left_hand_pos = np.zeros((25, 3)) |
||||
|
right_hand_pos = np.zeros((25, 3)) |
||||
|
|
||||
|
if left_valid and right_valid: |
||||
|
with self.server.left_hand_position_shared.get_lock(): |
||||
|
raw_left = np.array(self.server.left_hand_position_shared[:]) |
||||
|
with self.server.right_hand_position_shared.get_lock(): |
||||
|
raw_right = np.array(self.server.right_hand_position_shared[:]) |
||||
|
|
||||
|
if np.any(raw_left != 0): |
||||
|
# Hand positions are wrist-relative from Quest 3, in OpenXR coords. |
||||
|
# Must apply basis change (OpenXR→Robot) before hand convention transform. |
||||
|
# Derivation: original code does T_TO_UNITREE_HAND @ inv(arm_robot) @ (T_ROBOT_OPENXR @ world_pos) |
||||
|
# which simplifies to T_TO_UNITREE_HAND @ T_ROBOT_OPENXR @ wrist_relative_openxr |
||||
|
left_pos_25x3 = raw_left.reshape(25, 3) |
||||
|
left_hom = np.concatenate([left_pos_25x3.T, np.ones((1, 25))]) |
||||
|
left_hand_pos = (T_TO_UNITREE_HAND @ T_ROBOT_OPENXR @ left_hom)[0:3, :].T |
||||
|
|
||||
|
if np.any(raw_right != 0): |
||||
|
right_pos_25x3 = raw_right.reshape(25, 3) |
||||
|
right_hom = np.concatenate([right_pos_25x3.T, np.ones((1, 25))]) |
||||
|
right_hand_pos = (T_TO_UNITREE_HAND @ T_ROBOT_OPENXR @ right_hom)[0:3, :].T |
||||
|
|
||||
|
# --- Hand rotations --- |
||||
|
left_hand_rot = None |
||||
|
right_hand_rot = None |
||||
|
# TODO: Transform hand rotations if needed for dexterous hand control |
||||
|
|
||||
|
# --- Pinch/squeeze from finger distances --- |
||||
|
# Compute pinch from thumb-tip to index-tip distance |
||||
|
left_pinch_val, left_pinch = _compute_pinch(left_hand_pos) |
||||
|
right_pinch_val, right_pinch = _compute_pinch(right_hand_pos) |
||||
|
left_squeeze_val, left_squeeze = _compute_squeeze(left_hand_pos) |
||||
|
right_squeeze_val, right_squeeze = _compute_squeeze(right_hand_pos) |
||||
|
|
||||
|
return TeleData( |
||||
|
head_pose=Brobot_world_head, |
||||
|
left_wrist_pose=left_wrist_final, |
||||
|
right_wrist_pose=right_wrist_final, |
||||
|
chest_pose=Brobot_world_chest, |
||||
|
left_hand_pos=left_hand_pos, |
||||
|
right_hand_pos=right_hand_pos, |
||||
|
left_hand_rot=left_hand_rot, |
||||
|
right_hand_rot=right_hand_rot, |
||||
|
left_hand_pinch=left_pinch, |
||||
|
left_hand_pinchValue=left_pinch_val, |
||||
|
left_hand_squeeze=left_squeeze, |
||||
|
left_hand_squeezeValue=left_squeeze_val, |
||||
|
right_hand_pinch=right_pinch, |
||||
|
right_hand_pinchValue=right_pinch_val, |
||||
|
right_hand_squeeze=right_squeeze, |
||||
|
right_hand_squeezeValue=right_squeeze_val, |
||||
|
) |
||||
|
|
||||
|
def render_to_xr(self, img): |
||||
|
"""Send a webcam frame to the Quest 3 app. |
||||
|
|
||||
|
Args: |
||||
|
img: BGR numpy array (OpenCV format). Will be JPEG-encoded and sent. |
||||
|
""" |
||||
|
import cv2 |
||||
|
# Encode as JPEG |
||||
|
_, jpeg_buf = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 70]) |
||||
|
self.server.set_webcam_frame(jpeg_buf.tobytes()) |
||||
|
|
||||
|
def close(self): |
||||
|
"""Shutdown the server.""" |
||||
|
# The server thread is daemonic, it will stop when the process exits. |
||||
|
pass |
||||
|
|
||||
|
@property |
||||
|
def has_client(self) -> bool: |
||||
|
"""Check if a Quest 3 client is connected.""" |
||||
|
return len(self.server._clients) > 0 |
||||
|
|
||||
|
@property |
||||
|
def last_data_time(self) -> float: |
||||
|
"""Timestamp of last received tracking data.""" |
||||
|
with self.server.last_data_time_shared.get_lock(): |
||||
|
return self.server.last_data_time_shared.value |
||||
|
|
||||
|
|
||||
|
def _compute_pinch(hand_pos_25x3: np.ndarray) -> tuple: |
||||
|
"""Compute pinch state from thumb tip (4) and index tip (8) distance. |
||||
|
|
||||
|
Returns (pinchValue, is_pinching). |
||||
|
pinchValue: ~15.0 (open) → 0.0 (pinched), scaled to match Vuer convention * 100. |
||||
|
""" |
||||
|
if np.all(hand_pos_25x3 == 0): |
||||
|
return 10.0, False |
||||
|
|
||||
|
# OpenXR hand joint indices: thumb tip = 4, index tip = 8 |
||||
|
thumb_tip = hand_pos_25x3[4] |
||||
|
index_tip = hand_pos_25x3[8] |
||||
|
dist = np.linalg.norm(thumb_tip - index_tip) |
||||
|
|
||||
|
# Map distance to pinch value (meters → normalized) |
||||
|
# Typical range: 0.0 (touching) to 0.10 (fully open) |
||||
|
pinch_value = min(dist * 150.0, 15.0) # Scale to ~0-15 range |
||||
|
is_pinching = dist < 0.025 # 2.5cm threshold |
||||
|
|
||||
|
return pinch_value * 100.0, is_pinching # Match TeleVuerWrapper scaling |
||||
|
|
||||
|
|
||||
|
def _compute_squeeze(hand_pos_25x3: np.ndarray) -> tuple: |
||||
|
"""Compute squeeze (fist) state from average finger curl. |
||||
|
|
||||
|
Returns (squeezeValue, is_squeezing). |
||||
|
squeezeValue: 0.0 (open) → 1.0 (fist). |
||||
|
""" |
||||
|
if np.all(hand_pos_25x3 == 0): |
||||
|
return 0.0, False |
||||
|
|
||||
|
# Measure distance from fingertips to palm center |
||||
|
# Joint indices: palm=0, index_tip=8, middle_tip=12, ring_tip=16, pinky_tip=20 |
||||
|
palm = hand_pos_25x3[0] |
||||
|
tips = hand_pos_25x3[[8, 12, 16, 20]] |
||||
|
dists = np.linalg.norm(tips - palm, axis=1) |
||||
|
avg_dist = np.mean(dists) |
||||
|
|
||||
|
# Map: ~0.02m (fist) → 1.0, ~0.10m (open) → 0.0 |
||||
|
squeeze_value = np.clip(1.0 - (avg_dist - 0.02) / 0.08, 0.0, 1.0) |
||||
|
is_squeezing = squeeze_value > 0.7 |
||||
|
|
||||
|
return squeeze_value, is_squeezing |
||||
@ -0,0 +1,315 @@ |
|||||
|
#!/usr/bin/env python3 |
||||
|
""" |
||||
|
Lightweight WebSocket server for Quest 3 native teleop app. |
||||
|
Replaces Vuer (~900 lines) with ~200 lines. |
||||
|
|
||||
|
Receives body tracking data (JSON) from the Quest 3 native app, |
||||
|
writes poses to shared memory arrays compatible with teleop_hand_and_arm.py, |
||||
|
and sends webcam JPEG frames back to the app. |
||||
|
|
||||
|
Protocol: |
||||
|
Client → Server: JSON text messages with tracking data |
||||
|
Server → Client: Binary messages with JPEG webcam frames |
||||
|
Server → Client: JSON text messages for status/config |
||||
|
|
||||
|
Shared Memory Format (matches TeleVuer/TeleVuerWrapper interface): |
||||
|
head_pose_shared: Array('d', 16) — 4x4 SE(3), column-major ('F' order) |
||||
|
left_arm_pose_shared: Array('d', 16) — 4x4 SE(3), column-major ('F' order) |
||||
|
right_arm_pose_shared: Array('d', 16) — 4x4 SE(3), column-major ('F' order) |
||||
|
last_data_time_shared: Value('d') — Unix timestamp of last received data |
||||
|
chest_pose_shared: Array('d', 16) — 4x4 SE(3), NEW: chest orientation |
||||
|
|
||||
|
Hand tracking (if enabled): |
||||
|
left_hand_position_shared: Array('d', 75) — 25×3 positions |
||||
|
right_hand_position_shared: Array('d', 75) — 25×3 positions |
||||
|
left_hand_orientation_shared: Array('d', 225) — 25×3×3 rotations (col-major) |
||||
|
right_hand_orientation_shared: Array('d', 225) — 25×3×3 rotations (col-major) |
||||
|
|
||||
|
Usage: |
||||
|
# Standalone (for testing): |
||||
|
python teleop_server.py --port 8765 |
||||
|
|
||||
|
# Integrated with teleop_hand_and_arm.py: |
||||
|
# Import TeleopServer and pass shared memory arrays |
||||
|
""" |
||||
|
|
||||
|
import asyncio |
||||
|
import json |
||||
|
import time |
||||
|
import logging |
||||
|
import argparse |
||||
|
import threading |
||||
|
import numpy as np |
||||
|
from multiprocessing import Array, Value |
||||
|
|
||||
|
try: |
||||
|
import websockets |
||||
|
import websockets.asyncio.server |
||||
|
except ImportError: |
||||
|
print("ERROR: 'websockets' package required. Install with: pip install websockets") |
||||
|
raise |
||||
|
|
||||
|
logger = logging.getLogger("teleop_server") |
||||
|
|
||||
|
|
||||
|
class TeleopServer: |
||||
|
"""WebSocket server that bridges Quest 3 native app to teleop shared memory.""" |
||||
|
|
||||
|
def __init__(self, host="0.0.0.0", port=8765, |
||||
|
head_pose_shared=None, |
||||
|
left_arm_pose_shared=None, |
||||
|
right_arm_pose_shared=None, |
||||
|
last_data_time_shared=None, |
||||
|
chest_pose_shared=None, |
||||
|
left_hand_position_shared=None, |
||||
|
right_hand_position_shared=None, |
||||
|
left_hand_orientation_shared=None, |
||||
|
right_hand_orientation_shared=None, |
||||
|
left_hand_pinch_shared=None, |
||||
|
left_hand_squeeze_shared=None, |
||||
|
right_hand_pinch_shared=None, |
||||
|
right_hand_squeeze_shared=None): |
||||
|
self.host = host |
||||
|
self.port = port |
||||
|
|
||||
|
# Create shared memory if not provided (standalone mode) |
||||
|
self.head_pose_shared = head_pose_shared or Array('d', 16, lock=True) |
||||
|
self.left_arm_pose_shared = left_arm_pose_shared or Array('d', 16, lock=True) |
||||
|
self.right_arm_pose_shared = right_arm_pose_shared or Array('d', 16, lock=True) |
||||
|
self.last_data_time_shared = last_data_time_shared or Value('d', 0.0, lock=True) |
||||
|
self.chest_pose_shared = chest_pose_shared or Array('d', 16, lock=True) |
||||
|
|
||||
|
# Hand tracking arrays |
||||
|
self.left_hand_position_shared = left_hand_position_shared or Array('d', 75, lock=True) |
||||
|
self.right_hand_position_shared = right_hand_position_shared or Array('d', 75, lock=True) |
||||
|
self.left_hand_orientation_shared = left_hand_orientation_shared or Array('d', 225, lock=True) |
||||
|
self.right_hand_orientation_shared = right_hand_orientation_shared or Array('d', 225, lock=True) |
||||
|
|
||||
|
# Pinch/squeeze (computed from finger positions) |
||||
|
self.left_hand_pinch_shared = left_hand_pinch_shared |
||||
|
self.left_hand_squeeze_shared = left_hand_squeeze_shared |
||||
|
self.right_hand_pinch_shared = right_hand_pinch_shared |
||||
|
self.right_hand_squeeze_shared = right_hand_squeeze_shared |
||||
|
|
||||
|
# Initialize with identity matrices |
||||
|
identity_flat = np.eye(4).flatten(order='F').tolist() |
||||
|
with self.head_pose_shared.get_lock(): |
||||
|
self.head_pose_shared[:] = identity_flat |
||||
|
with self.left_arm_pose_shared.get_lock(): |
||||
|
self.left_arm_pose_shared[:] = identity_flat |
||||
|
with self.right_arm_pose_shared.get_lock(): |
||||
|
self.right_arm_pose_shared[:] = identity_flat |
||||
|
with self.chest_pose_shared.get_lock(): |
||||
|
self.chest_pose_shared[:] = identity_flat |
||||
|
|
||||
|
# Webcam frame (set by external code, sent to clients) |
||||
|
self._webcam_frame = None |
||||
|
self._webcam_lock = threading.Lock() |
||||
|
self._webcam_event = asyncio.Event() |
||||
|
|
||||
|
# Connection tracking |
||||
|
self._clients = set() |
||||
|
self._message_count = 0 |
||||
|
self._last_log_time = 0 |
||||
|
|
||||
|
def set_webcam_frame(self, jpeg_bytes: bytes): |
||||
|
"""Called by external code (e.g., ThreadedWebcam) to provide a new JPEG frame.""" |
||||
|
with self._webcam_lock: |
||||
|
self._webcam_frame = jpeg_bytes |
||||
|
# Signal the async send loop |
||||
|
try: |
||||
|
self._webcam_event.set() |
||||
|
except RuntimeError: |
||||
|
pass # Event loop not running yet |
||||
|
|
||||
|
async def _handle_client(self, websocket): |
||||
|
"""Handle a single WebSocket client connection.""" |
||||
|
remote = websocket.remote_address |
||||
|
logger.info(f"Client connected: {remote}") |
||||
|
self._clients.add(websocket) |
||||
|
|
||||
|
# Send initial config |
||||
|
await websocket.send(json.dumps({ |
||||
|
"type": "config", |
||||
|
"version": "1.0", |
||||
|
"body_tracking": True, |
||||
|
})) |
||||
|
|
||||
|
# Start webcam sender task for this client |
||||
|
webcam_task = asyncio.create_task(self._send_webcam_loop(websocket)) |
||||
|
|
||||
|
try: |
||||
|
async for message in websocket: |
||||
|
if isinstance(message, str): |
||||
|
self._handle_tracking_message(message) |
||||
|
# Binary messages from client are ignored |
||||
|
except websockets.exceptions.ConnectionClosed: |
||||
|
logger.info(f"Client disconnected: {remote}") |
||||
|
finally: |
||||
|
self._clients.discard(websocket) |
||||
|
webcam_task.cancel() |
||||
|
try: |
||||
|
await webcam_task |
||||
|
except asyncio.CancelledError: |
||||
|
pass |
||||
|
|
||||
|
def _handle_tracking_message(self, message: str): |
||||
|
"""Parse tracking JSON and write to shared memory.""" |
||||
|
try: |
||||
|
data = json.loads(message) |
||||
|
except json.JSONDecodeError: |
||||
|
logger.warning("Invalid JSON received") |
||||
|
return |
||||
|
|
||||
|
msg_type = data.get("type") |
||||
|
if msg_type != "tracking": |
||||
|
return |
||||
|
|
||||
|
self._message_count += 1 |
||||
|
now = time.time() |
||||
|
|
||||
|
# Rate-limited logging (every 5 seconds) |
||||
|
if now - self._last_log_time > 5.0: |
||||
|
logger.info(f"Tracking messages received: {self._message_count}") |
||||
|
self._last_log_time = now |
||||
|
|
||||
|
# Update timestamp |
||||
|
with self.last_data_time_shared.get_lock(): |
||||
|
self.last_data_time_shared.value = now |
||||
|
|
||||
|
# Head pose: 7 floats [x, y, z, qx, qy, qz, qw] → 4x4 column-major |
||||
|
head = data.get("head") |
||||
|
if head and len(head) == 7: |
||||
|
mat = _pose7_to_mat16(head) |
||||
|
with self.head_pose_shared.get_lock(): |
||||
|
self.head_pose_shared[:] = mat |
||||
|
|
||||
|
# Chest pose: 7 floats → 4x4 column-major |
||||
|
chest = data.get("chest") |
||||
|
if chest and len(chest) == 7: |
||||
|
mat = _pose7_to_mat16(chest) |
||||
|
with self.chest_pose_shared.get_lock(): |
||||
|
self.chest_pose_shared[:] = mat |
||||
|
|
||||
|
# Left wrist: 16 floats (already column-major 4x4) |
||||
|
left_wrist = data.get("left_wrist") |
||||
|
if left_wrist and len(left_wrist) == 16: |
||||
|
with self.left_arm_pose_shared.get_lock(): |
||||
|
self.left_arm_pose_shared[:] = left_wrist |
||||
|
|
||||
|
# Right wrist: 16 floats (already column-major 4x4) |
||||
|
right_wrist = data.get("right_wrist") |
||||
|
if right_wrist and len(right_wrist) == 16: |
||||
|
with self.right_arm_pose_shared.get_lock(): |
||||
|
self.right_arm_pose_shared[:] = right_wrist |
||||
|
|
||||
|
# Hand positions: 75 floats each (25 joints × 3) |
||||
|
left_hand_pos = data.get("left_hand_pos") |
||||
|
if left_hand_pos and len(left_hand_pos) == 75: |
||||
|
with self.left_hand_position_shared.get_lock(): |
||||
|
self.left_hand_position_shared[:] = left_hand_pos |
||||
|
|
||||
|
right_hand_pos = data.get("right_hand_pos") |
||||
|
if right_hand_pos and len(right_hand_pos) == 75: |
||||
|
with self.right_hand_position_shared.get_lock(): |
||||
|
self.right_hand_position_shared[:] = right_hand_pos |
||||
|
|
||||
|
# Hand rotations: 225 floats each (25 joints × 9) |
||||
|
left_hand_rot = data.get("left_hand_rot") |
||||
|
if left_hand_rot and len(left_hand_rot) == 225: |
||||
|
with self.left_hand_orientation_shared.get_lock(): |
||||
|
self.left_hand_orientation_shared[:] = left_hand_rot |
||||
|
|
||||
|
right_hand_rot = data.get("right_hand_rot") |
||||
|
if right_hand_rot and len(right_hand_rot) == 225: |
||||
|
with self.right_hand_orientation_shared.get_lock(): |
||||
|
self.right_hand_orientation_shared[:] = right_hand_rot |
||||
|
|
||||
|
async def _send_webcam_loop(self, websocket): |
||||
|
"""Send webcam JPEG frames to a client at ~15 fps.""" |
||||
|
interval = 1.0 / 15.0 |
||||
|
while True: |
||||
|
await asyncio.sleep(interval) |
||||
|
with self._webcam_lock: |
||||
|
frame = self._webcam_frame |
||||
|
if frame is not None: |
||||
|
try: |
||||
|
await websocket.send(frame) |
||||
|
except websockets.exceptions.ConnectionClosed: |
||||
|
break |
||||
|
|
||||
|
async def serve(self): |
||||
|
"""Start the WebSocket server.""" |
||||
|
logger.info(f"Starting teleop server on ws://{self.host}:{self.port}") |
||||
|
async with websockets.asyncio.server.serve( |
||||
|
self._handle_client, |
||||
|
self.host, |
||||
|
self.port, |
||||
|
max_size=2**20, # 1 MB max message size |
||||
|
ping_interval=20, |
||||
|
ping_timeout=20, |
||||
|
) as server: |
||||
|
logger.info(f"Teleop server running on ws://{self.host}:{self.port}") |
||||
|
await asyncio.Future() # Run forever |
||||
|
|
||||
|
def start_in_thread(self): |
||||
|
"""Start the server in a background thread. Returns the thread.""" |
||||
|
def _run(): |
||||
|
asyncio.run(self.serve()) |
||||
|
thread = threading.Thread(target=_run, daemon=True) |
||||
|
thread.start() |
||||
|
logger.info("Teleop server started in background thread") |
||||
|
return thread |
||||
|
|
||||
|
|
||||
|
def _pose7_to_mat16(pose7): |
||||
|
"""Convert [x, y, z, qx, qy, qz, qw] to 16-float column-major 4x4 matrix. |
||||
|
|
||||
|
Compatible with NumPy reshape(4,4, order='F'). |
||||
|
""" |
||||
|
x, y, z, qx, qy, qz, qw = pose7 |
||||
|
|
||||
|
# Quaternion to 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 |
||||
|
|
||||
|
r00 = 1 - 2 * (yy + zz) |
||||
|
r01 = 2 * (xy - wz) |
||||
|
r02 = 2 * (xz + wy) |
||||
|
r10 = 2 * (xy + wz) |
||||
|
r11 = 1 - 2 * (xx + zz) |
||||
|
r12 = 2 * (yz - wx) |
||||
|
r20 = 2 * (xz - wy) |
||||
|
r21 = 2 * (yz + wx) |
||||
|
r22 = 1 - 2 * (xx + yy) |
||||
|
|
||||
|
# Column-major (Fortran order): column 0, column 1, column 2, column 3 |
||||
|
return [ |
||||
|
r00, r10, r20, 0.0, # Column 0 |
||||
|
r01, r11, r21, 0.0, # Column 1 |
||||
|
r02, r12, r22, 0.0, # Column 2 |
||||
|
x, y, z, 1.0, # Column 3 |
||||
|
] |
||||
|
|
||||
|
|
||||
|
# --- Standalone mode --- |
||||
|
|
||||
|
def main(): |
||||
|
parser = argparse.ArgumentParser(description="Teleop WebSocket server for Quest 3 native app") |
||||
|
parser.add_argument("--host", default="0.0.0.0", help="Bind address (default: 0.0.0.0)") |
||||
|
parser.add_argument("--port", type=int, default=8765, help="WebSocket port (default: 8765)") |
||||
|
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" |
||||
|
) |
||||
|
|
||||
|
server = TeleopServer(host=args.host, port=args.port) |
||||
|
asyncio.run(server.serve()) |
||||
|
|
||||
|
|
||||
|
if __name__ == "__main__": |
||||
|
main() |
||||
Write
Preview
Loading…
Cancel
Save
Reference in new issue