From a1971636d24ed8607dfd2a92f0ee265ad91b7bd6 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 16:52:37 +0000 Subject: [PATCH 01/72] Initial Commit: Added teleop module from quest_teleop_dev Former-commit-id: d7f37e8c4708f6934241efe446a88feb5f7c66f9 Former-commit-id: 74838dbec9abab151c72c4f535aa62075d74aafa --- dimos/teleop/quest3/README.md | 335 +++++++++++ dimos/teleop/quest3/__init__.py | 8 + dimos/teleop/quest3/control/fastapi_server.py | 419 +++++++++++++ .../quest3/control/tracking_processor.py | 170 ++++++ dimos/teleop/quest3/static/index.html | 446 ++++++++++++++ dimos/teleop/quest3/teleop_module.py | 561 ++++++++++++++++++ dimos/teleop/teleop_arm_controller.py | 535 +++++++++++++++++ dimos/teleop/teleop_blueprints.py | 144 +++++ 8 files changed, 2618 insertions(+) create mode 100644 dimos/teleop/quest3/README.md create mode 100644 dimos/teleop/quest3/__init__.py create mode 100644 dimos/teleop/quest3/control/fastapi_server.py create mode 100644 dimos/teleop/quest3/control/tracking_processor.py create mode 100644 dimos/teleop/quest3/static/index.html create mode 100644 dimos/teleop/quest3/teleop_module.py create mode 100644 dimos/teleop/teleop_arm_controller.py create mode 100644 dimos/teleop/teleop_blueprints.py diff --git a/dimos/teleop/quest3/README.md b/dimos/teleop/quest3/README.md new file mode 100644 index 0000000000..401d23b3f4 --- /dev/null +++ b/dimos/teleop/quest3/README.md @@ -0,0 +1,335 @@ +# Quest3 VR Teleoperation + +**NPM-Free** VR teleoperation system for Quest 3 headset with robot manipulation. + +## Quick Start + +### 1. Start Teleoperation + +```python +from dimos.teleop.quest3 import Quest3TeleopModule + +# Create and start +teleop = Quest3TeleopModule() +teleop.start() + +# Output: +# โœ… Quest3 Teleoperation Module started on https://0.0.0.0:8443 +# ๐Ÿ“ฑ Open this URL on Quest 3: https://:8443/ +``` + +### 2. Connect Quest 3 + +1. **Find your server IP**: + ```bash + hostname -I + # Example: 192.168.1.100 + ``` + +2. **Open Quest 3 browser** and go to: + ``` + https://192.168.1.100:8443/ + ``` + +3. **Accept security warning** (self-signed certificate is safe) + +4. **Click "Connect"** button and put on headset + +5. **Press X button** on controller to start teleoperation + - First press: Calibrates (captures initial poses) and starts control + - Press again: Stops control (calibration preserved) + - Press again: Resumes control + +## Architecture + +``` +Quest 3 Browser + โ†“ Opens: https://your-ip:8443/ + โ†“ Loads: Standalone HTML/JS VR client + โ†“ Connects: wss://your-ip:8443/ws + โ†“ X button pressed โ†’ sends start_teleop command + โ†“ +FastAPI Server (Python, single process) + โ†“ Serves HTML + WebSocket + โ†“ Auto-generates SSL certificates + โ†“ Processes tracking data (20Hz) + โ†“ Routes X button commands to TeleopArmController + โ†“ +Quest3TeleopModule + โ†“ Publishes controller pose topics (PoseStamped) + โ†“ +TeleopArmController + โ†“ Receives X button command โ†’ start_control() + โ†“ Calibrates (captures initial controller + robot poses) + โ†“ Activates control loop (runs continuously, gated by flags) + โ†“ Calculates relative control: target = initial_robot + (current_controller - initial_controller) + โ†“ Publishes cartesian commands (Pose) + โ†“ +XArmDriver / Robot Driver + โ†“ Executes cartesian commands + โ†“ +Robot +``` + +## Key Features + +โœ… **Zero npm/Node.js dependencies** - Pure Python + vanilla JS +โœ… **Single HTML file** - No build process, instant updates +โœ… **Auto-generated SSL** - Certificates created automatically +โœ… **One Python process** - Everything in FastAPI +โœ… **X button control** - Press X to start/stop teleoperation +โœ… **Auto-calibration** - Captures initial poses on first X press +โœ… **Relative control** - Robot follows controller movement relative to initial pose +โœ… **20Hz tracking rate** - Low latency controller data +โœ… **WebXR passthrough** - AR mode support + +## Configuration + +```python +from dimos.teleop.quest3 import Quest3TeleopModule, Quest3TeleopConfig + +config = Quest3TeleopConfig( + signaling_host="0.0.0.0", + signaling_port=8443, # HTTPS port + use_https=True, # Required for WebXR + driver_module_name="XArmDriver", + position_scale=1.0, + enable_left_arm=True, + enable_right_arm=False, + max_velocity=0.5, + workspace_limits={ + "x": (0.1, 1.0), + "y": (-0.8, 0.8), + "z": (0.1, 0.7), + }, +) + +module = Quest3TeleopModule(config=config) +module.start() +``` + +## Available Blueprints + +### Basic Teleoperation (No Camera) + +```bash +# XArm6 +dimos run quest3-xarm6 + +# XArm7 +dimos run quest3-xarm7 + +# Piper Robot +dimos run quest3-piper +``` + +### With Camera Streaming + +```bash +# XArm6 + RealSense D435i +dimos run quest3-xarm6-camera + +# XArm7 + RealSense +dimos run quest3-xarm7-camera + +# Piper + RealSense +dimos run quest3-piper-camera +``` + +## Files Structure + +``` +dimos/teleop/quest3/ +โ”œโ”€โ”€ static/ +โ”‚ โ””โ”€โ”€ index.html # Standalone VR client (vanilla JS) +โ”œโ”€โ”€ certs/ # Auto-generated SSL certificates +โ”‚ โ”œโ”€โ”€ cert.pem +โ”‚ โ””โ”€โ”€ key.pem +โ”œโ”€โ”€ control/ +โ”‚ โ”œโ”€โ”€ fastapi_server.py # FastAPI server with HTTPS + WebSocket +โ”‚ โ””โ”€โ”€ tracking_processor.py # VR tracking data processor +โ”œโ”€โ”€ teleop_module.py # Main teleoperation module +โ”œโ”€โ”€ README.md # This file +โ””โ”€โ”€ README_NO_NPM.md # Detailed setup guide +``` + +## How It Works + +1. **Control Loop**: Runs continuously from `start()`, processing controller poses at 20Hz +2. **X Button Press**: + - Sends `start_teleop` command via WebSocket + - Triggers `TeleopArmController.start_control()` + - If not calibrated, automatically calibrates first + - Then activates control (sets `control_active = True`) +3. **Calibration**: Captures initial controller pose and robot end-effector pose +4. **Relative Control**: Calculates target pose as: `target = initial_robot + (current_controller - initial_controller)` +5. **Safety Gates**: Control loop only sends commands when both `calibrated` and `control_active` are True + +## RPC Methods + +**Quest3TeleopModule**: +- `start()` โ†’ Start module and FastAPI server +- `stop()` โ†’ Stop module and server +- `get_status()` โ†’ Get current status + +**TeleopArmController**: +- `start_control()` โ†’ Calibrate (if needed) and activate control +- `stop_control()` โ†’ Stop sending commands (preserves calibration) +- `calibrate()` โ†’ Manually calibrate +- `is_control_active()` โ†’ Check if control is active + +## WebSocket Protocol + +### Handshake +```json +// Client โ†’ Server +{ + "role": "teleop", + "robot_ip": "" +} + +// Server โ†’ Client +{ + "type": "handshake_ack", + "status": "connected" +} +``` + +### Tracking Data (20Hz) +```json +{ + "type": "controller", + "left": { + "targetLocation": [16 floats], // 4x4 transformation matrix + "trigger": 0.0, // 0.0 - 1.0 + "grip": 0.0, + "joystickX": 0.0, + "joystickY": 0.0, + "buttons": [true, false, ...] + }, + "right": { /* same structure */ } +} +``` + +### X Button Commands +```json +// Client โ†’ Server (when X button pressed) +{ + "type": "start_teleop" // or "stop_teleop" +} + +// Server โ†’ Client (response) +{ + "type": "teleop_started", // or "teleop_stopped" + "message": "Control active - move controllers to control robot" +} +``` + +## Output Topics + +The module publishes these topics: + +- `left_controller_pose` (PoseStamped) - Left controller pose +- `right_controller_pose` (PoseStamped) - Right controller pose +- `left_trigger` (Bool) - Left trigger button state +- `right_trigger` (Bool) - Right trigger button state + +## Troubleshooting + +### WebXR Not Available +**Solution**: Make sure you're using HTTPS (required by Quest 3): +```python +config = Quest3TeleopConfig(use_https=True) # Must be True! +``` + +### Certificate Warning on Quest 3 +**Solution**: Accept the self-signed certificate: +1. Click "Advanced" on warning page +2. Click "Proceed to (unsafe)" +3. This is safe for local development + +### Port Already in Use +```bash +# Check what's using the port +sudo lsof -i :8443 + +# Kill the process +sudo kill -9 + +# Or use a different port +config = Quest3TeleopConfig(signaling_port=9443) +``` + +### Can't Generate SSL Certificates +```bash +# Install OpenSSL +sudo apt-get install openssl + +# Verify +openssl version +``` + +### WebSocket Connection Fails +**Check**: +1. Server is running: `curl https://your-ip:8443/health` +2. Firewall allows port: `sudo ufw allow 8443` +3. Same network (Quest 3 and server) +4. Correct IP (not localhost) + +### No Tracking Data / Robot Not Moving +1. **Press X button** to start teleoperation (calibrates and activates) +2. Verify WebSocket connection in browser console +3. Check server logs for calibration/control status +4. Ensure controllers are being tracked in VR +5. Check that `control_active` and `calibrated` are both True in logs + +## Development + +### Edit VR Client +Simply edit `static/index.html` and refresh browser (no build needed!): + +```html + +
Custom Info
+ + +``` + +### Standalone Server Testing +```bash +cd dimos/teleop/quest3/control +python fastapi_server.py + +# Server starts on https://0.0.0.0:8443 +# Test: https://localhost:8443/health +``` + +### Custom Integration + +```python +from dimos.core.blueprints import autoconnect +from dimos.teleop.quest3 import quest3_teleop_module +from your_robot import your_robot_blueprint + +custom_stack = autoconnect( + your_robot_blueprint, + quest3_teleop_module( + driver_module_name="YourDriver", + position_scale=1.0, + ), +) + +coordinator = custom_stack.build() +coordinator.loop() +``` + +## Performance (Need to be updated) + +- **Latency**: ~50-70ms end-to-end (Quest 3 โ†’ Python โ†’ Robot) +- **Tracking Rate**: 20Hz (50ms interval) +- **CPU Usage**: <5% (FastAPI server) +- **Memory**: ~50MB (Python process) +- **Network**: ~2KB/s (tracking data) diff --git a/dimos/teleop/quest3/__init__.py b/dimos/teleop/quest3/__init__.py new file mode 100644 index 0000000000..cb7ac90b3e --- /dev/null +++ b/dimos/teleop/quest3/__init__.py @@ -0,0 +1,8 @@ +"""Quest3 VR Teleoperation.""" + +from dimos.teleop.quest3.teleop_module import Quest3TeleopConfig, Quest3TeleopModule + +__all__ = [ + "Quest3TeleopConfig", + "Quest3TeleopModule", +] diff --git a/dimos/teleop/quest3/control/fastapi_server.py b/dimos/teleop/quest3/control/fastapi_server.py new file mode 100644 index 0000000000..176d59277c --- /dev/null +++ b/dimos/teleop/quest3/control/fastapi_server.py @@ -0,0 +1,419 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +FastAPI WebSocket server for Quest3 VR teleoperation. + +Handles WebSocket connections and processes VR controller tracking data +using FastAPI for better performance and features. + +Now includes HTTPS support and serves the standalone HTML VR client. +""" + +from __future__ import annotations + +import asyncio +import json +import os +from pathlib import Path +import subprocess +from typing import TYPE_CHECKING, Optional + +from fastapi import FastAPI, WebSocket, WebSocketDisconnect +from fastapi.middleware.cors import CORSMiddleware +from fastapi.responses import FileResponse +from fastapi.staticfiles import StaticFiles +import uvicorn + +from dimos.teleop.quest3.control.tracking_processor import TrackingProcessor +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from collections.abc import Callable + + import numpy as np + from numpy.typing import NDArray + +logger = setup_logger() + + +class ConnectionManager: + """Manages active WebSocket connections for teleoperation.""" + + def __init__(self): + self.active_connections: list[WebSocket] = [] + self.data_callback: Callable | None = None + self.command_callback: Callable | None = None + self.connection_count = 0 + + async def connect(self, websocket: WebSocket) -> None: + """Accept and register a new WebSocket connection. + + Args: + websocket: The WebSocket connection to register + """ + await websocket.accept() + self.active_connections.append(websocket) + self.connection_count += 1 + logger.info(f"Client connected (total: {len(self.active_connections)})") + + def disconnect(self, websocket: WebSocket) -> None: + """Remove a WebSocket connection from active connections. + + Args: + websocket: The WebSocket connection to remove + """ + if websocket in self.active_connections: + self.active_connections.remove(websocket) + logger.info(f"Client disconnected (remaining: {len(self.active_connections)})") + + def set_callback(self, callback: Callable) -> None: + """Set callback function to send controller data to teleop module. + + Args: + callback: Function that takes (left_pose, right_pose, left_gripper, right_gripper) + """ + self.data_callback = callback + logger.info("Controller data callback registered") + + def set_command_callback(self, callback: Callable) -> None: + """Set callback function to handle teleop commands (start_teleop/stop_teleop). + + Args: + callback: Async function that takes (command_type: str, websocket: WebSocket) -> dict + """ + self.command_callback = callback + logger.info("Command callback registered") + + async def broadcast(self, message: dict) -> None: + """Broadcast a message to all connected clients. + + Args: + message: Dictionary to send as JSON to all clients + """ + disconnected = [] + for connection in self.active_connections: + try: + await connection.send_json(message) + except Exception as e: + logger.error(f"Failed to broadcast to client: {e}") + disconnected.append(connection) + + # Clean up disconnected clients + for conn in disconnected: + self.disconnect(conn) + + def get_connection_count(self) -> int: + """Get the number of active connections. + + Returns: + Number of active WebSocket connections + """ + return len(self.active_connections) + + +class TeleopFastAPIServer: + """FastAPI-based WebSocket server for Quest3 teleoperation.""" + + def __init__( + self, + host: str = "0.0.0.0", + port: int = 8443, # Changed default to 8443 for HTTPS + use_https: bool = True, + ): + """Initialize the FastAPI server. + + Args: + host: Host address to bind to (default: 0.0.0.0) + port: Port to bind to (default: 8443 for HTTPS) + use_https: Whether to use HTTPS (required for WebXR on Quest 3) + """ + self.host = host + self.port = port + self.use_https = use_https + self.app = FastAPI(title="Quest3 Teleoperation Server") + self.manager = ConnectionManager() + self.server: uvicorn.Server | None = None + + # SSL certificate paths + self.cert_dir = Path(__file__).parent.parent / "certs" + self.cert_file = self.cert_dir / "cert.pem" + self.key_file = self.cert_dir / "key.pem" + + # Static files directory + self.static_dir = Path(__file__).parent.parent / "static" + + # Enable CORS for Quest 3 browser + self.app.add_middleware( + CORSMiddleware, + allow_origins=["*"], # Allow all origins for VR headset + allow_credentials=True, + allow_methods=["*"], + allow_headers=["*"], + ) + + # Register routes + self._setup_routes() + + # Setup SSL certificates if needed + if self.use_https: + self._ensure_ssl_certificates() + + def _ensure_ssl_certificates(self) -> None: + """Generate self-signed SSL certificates if they don't exist.""" + self.cert_dir.mkdir(parents=True, exist_ok=True) + + if self.cert_file.exists() and self.key_file.exists(): + logger.info("SSL certificates found") + return + + logger.info("Generating self-signed SSL certificates...") + try: + subprocess.run( + [ + "openssl", + "req", + "-x509", + "-newkey", + "rsa:2048", + "-nodes", + "-sha256", + "-subj", + "/CN=localhost", + "-keyout", + str(self.key_file), + "-out", + str(self.cert_file), + "-days", + "365", + ], + check=True, + capture_output=True, + ) + logger.info("โœ“ SSL certificates generated successfully") + logger.warning( + "โš  These are self-signed certificates. Quest 3 will show a security warning." + ) + except subprocess.CalledProcessError as e: + logger.error(f"Failed to generate SSL certificates: {e}") + logger.error("Make sure openssl is installed: sudo apt-get install openssl") + raise + except FileNotFoundError: + logger.error("openssl command not found. Please install: sudo apt-get install openssl") + raise + + def _setup_routes(self) -> None: + """Setup FastAPI routes and WebSocket endpoints.""" + + @self.app.get("/") + async def root(): + """Serve the VR client HTML page.""" + html_file = self.static_dir / "index.html" + if html_file.exists(): + return FileResponse(html_file) + else: + return { + "service": "Quest3 Teleoperation Server", + "status": "running", + "active_connections": self.manager.get_connection_count(), + "error": "VR client HTML not found. Check static/index.html", + } + + @self.app.get("/health") + async def health(): + """Health check endpoint.""" + return { + "status": "healthy", + "connections": self.manager.get_connection_count(), + } + + @self.app.websocket("/ws") + async def websocket_endpoint(websocket: WebSocket): + """WebSocket endpoint for VR teleoperation tracking data. + + Args: + websocket: The WebSocket connection from Quest 3 client + """ + await self.manager.connect(websocket) + processor = TrackingProcessor() + message_count = 0 + + try: + # Wait for initial handshake message + initial_data = await websocket.receive_json() + role = initial_data.get("role") + logger.info(f"Client handshake: {initial_data}") + + if role != "teleop": + await websocket.send_json( + {"type": "error", "error": f"Invalid role: {role}. Expected 'teleop'."} + ) + await websocket.close() + return + + # Send acknowledgment + await websocket.send_json({"type": "handshake_ack", "status": "connected"}) + + # Main tracking loop + while True: + # Receive message + message = await websocket.receive_json() + message_count += 1 + + # Check if this is a command message (start_teleop/stop_teleop) + message_type = message.get("type") + if message_type in ("start_teleop", "stop_teleop"): + logger.info(f"Received command: {message_type}") + # Route to command handler + if self.manager.command_callback is not None: + try: + response = await self.manager.command_callback( + message_type, websocket + ) + # Send response back to client + await websocket.send_json(response) + except Exception as e: + logger.error( + f"Error handling command {message_type}: {e}", exc_info=True + ) + await websocket.send_json( + {"type": "error", "error": f"Command failed: {e!s}"} + ) + else: + logger.warning(f"Command callback not set, ignoring {message_type}") + await websocket.send_json( + {"type": "error", "error": "Command handler not available"} + ) + continue + + # Otherwise, process as tracking data + result = processor.process_tracking_message(message) + + if result is not None: + left_pose, right_pose, left_gripper, right_gripper = result + + # Send to callback (teleop module) + if self.manager.data_callback is not None: + self.manager.data_callback( + left_pose, right_pose, left_gripper, right_gripper + ) + + # Log periodically + if message_count % 100 == 0: + logger.debug(f"Processed {message_count} tracking messages") + + except WebSocketDisconnect: + logger.info("Client disconnected normally") + except Exception as e: + logger.error(f"Error in WebSocket handler: {e}", exc_info=True) + finally: + self.manager.disconnect(websocket) + logger.info(f"Connection closed after {message_count} messages") + + def set_callback(self, callback: Callable) -> None: + """Set callback function for controller data. + + Args: + callback: Function that takes (left_pose, right_pose, left_gripper, right_gripper) + """ + self.manager.set_callback(callback) + + def set_command_callback(self, callback: Callable) -> None: + """Set callback function for teleop commands. + + Args: + callback: Async function that takes (command_type: str, websocket: WebSocket) -> dict + """ + self.manager.set_command_callback(callback) + + async def start_async(self) -> None: + """Start the FastAPI server asynchronously.""" + config_kwargs = { + "app": self.app, + "host": self.host, + "port": self.port, + "log_level": "info", + "access_log": False, # Disable access logs for performance + } + + # Add SSL if enabled + if self.use_https: + config_kwargs["ssl_keyfile"] = str(self.key_file) + config_kwargs["ssl_certfile"] = str(self.cert_file) + protocol = "https" + ws_protocol = "wss" + else: + protocol = "http" + ws_protocol = "ws" + + config = uvicorn.Config(**config_kwargs) + self.server = uvicorn.Server(config) + + logger.info(f"FastAPI server starting on {protocol}://{self.host}:{self.port}") + logger.info(f"VR Client: {protocol}://{self.host}:{self.port}/") + logger.info(f"WebSocket: {ws_protocol}://{self.host}:{self.port}/ws") + + await self.server.serve() + + def run(self) -> None: + """Run the FastAPI server (blocking). + + This is for standalone testing only. + """ + run_kwargs = { + "app": self.app, + "host": self.host, + "port": self.port, + "log_level": "info", + } + + if self.use_https: + run_kwargs["ssl_keyfile"] = str(self.key_file) + run_kwargs["ssl_certfile"] = str(self.cert_file) + + uvicorn.run(**run_kwargs) + + async def stop_async(self) -> None: + """Stop the FastAPI server asynchronously.""" + if self.server: + logger.info("Stopping FastAPI server...") + self.server.should_exit = True + await asyncio.sleep(0.1) # Give time for graceful shutdown + + async def broadcast_robot_state(self, state: dict) -> None: + """Broadcast robot state to all connected clients. + + Args: + state: Dictionary containing robot state information + """ + await self.manager.broadcast(state) + + +# Standalone testing +if __name__ == "__main__": + import logging + + logging.basicConfig( + level=logging.INFO, format="%(asctime)s - %(name)s - %(levelname)s - %(message)s" + ) + + def test_callback(left_pose, right_pose, left_gripper, right_gripper): + """Test callback for tracking data.""" + print(f"Left pose: {left_pose[:3, 3]}, gripper: {left_gripper}") + print(f"Right pose: {right_pose[:3, 3]}, gripper: {right_gripper}") + + server = TeleopFastAPIServer() + server.set_callback(test_callback) + server.run() diff --git a/dimos/teleop/quest3/control/tracking_processor.py b/dimos/teleop/quest3/control/tracking_processor.py new file mode 100644 index 0000000000..ca8cf0ade5 --- /dev/null +++ b/dimos/teleop/quest3/control/tracking_processor.py @@ -0,0 +1,170 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +VR controller tracking processor for Quest3 teleoperation. + +Processes VR controller tracking data, applies coordinate transformations, +and returns robot-space poses and gripper values. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Any + +import numpy as np +from scipy.spatial.transform import Rotation + +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from numpy.typing import NDArray + +logger = setup_logger() + +# Coordinate frame transformation from VR to robot +# IMPORTANT: Adjust this matrix to match YOUR robot's coordinate system +VR_TO_ROBOT_FRAME = np.array( + [ + [0, 0, -1, 0], # Robot X-axis = -VR Z-axis (flip forward/back) + [-1, 0, 0, 0], # Robot Y-axis = -VR X-axis (flip left/right) + [0, 1, 0, 0], # Robot Z-axis = +VR Y-axis (keep up direction) + [0, 0, 0, 1], # Homogeneous coordinate + ], + dtype=np.float32, +) + + +class TrackingProcessor: + """Processes VR controller tracking data to robot-space poses. + + Handles: + - VR to robot coordinate frame transformation + - Controller orientation alignment + - Safety constraints (Z position clamping) + """ + + def __init__(self) -> None: + """Initialize tracking processor.""" + # Current poses (4x4 transformation matrices) + self.left_wrist_pose = np.eye(4, dtype=np.float32) + self.right_wrist_pose = np.eye(4, dtype=np.float32) + + # Current gripper values (0.0 = open, 1.0 = closed) + self.left_gripper_value = 0.0 + self.right_gripper_value = 0.0 + + def process_tracking_message( + self, event: dict[str, Any] + ) -> tuple[NDArray[np.float32], NDArray[np.float32], float, float] | None: + """Process VR tracking message and return robot-space poses. + + Args: + event: Dictionary with 'type', 'left', and 'right' controller data + + Returns: + Tuple of (left_pose, right_pose, left_gripper, right_gripper) or None if invalid + """ + tracking_type = event.get("type") + if tracking_type != "controller": + logger.debug("Ignoring non-controller tracking type: %s", tracking_type) + return None + + # Process both controllers + for side in ["left", "right"]: + tracking_data = event.get(side) + if tracking_data is not None: + if not isinstance(tracking_data, dict): + logger.warning("Invalid tracking data format for %s", side) + continue + + self._process_controller(tracking_data, side, tracking_type) + + # Get controller poses + left_pose = self.left_wrist_pose.copy() + right_pose = self.right_wrist_pose.copy() + + return left_pose, right_pose, self.left_gripper_value, self.right_gripper_value + + def _process_controller( + self, tracking_data: dict[str, Any], side: str, tracking_type: str + ) -> None: + """Process single controller's tracking data. + + Args: + tracking_data: Controller data with 'targetLocation' and 'trigger' + side: 'left' or 'right' + tracking_type: 'controller' or 'hand' + """ + # Process target location (pose) + if "targetLocation" in tracking_data: + self._process_target_location(tracking_data["targetLocation"], side, tracking_type) + + # Process gripper (trigger) + if side == "left": + self.left_gripper_value = self._extract_gripper_value(tracking_data) + else: + self.right_gripper_value = self._extract_gripper_value(tracking_data) + + def _process_target_location( + self, target_location: list[float], side: str, tracking_type: str + ) -> None: + """Process controller pose from VR space to robot space. + + Args: + target_location: Flat 16-element transformation matrix (column-major) + side: 'left' or 'right' + tracking_type: 'controller' or 'hand' + """ + # Convert to 4x4 matrix + target_matrix_flat = np.array(target_location, dtype=np.float32) + if target_matrix_flat.size != 16: + logger.error("Invalid targetLocation size: %d, expected 16", target_matrix_flat.size) + return + + # Reshape and transpose (column-major from JS to row-major) + target_matrix = target_matrix_flat.reshape(4, 4).T + + # Rotate controller 90ยฐ around Z-axis for gripper alignment + if tracking_type == "controller": + direction = -1 if side == "right" else 1 + rotation = Rotation.from_euler("z", 90 * direction, degrees=True) + target_matrix[:3, :3] = target_matrix[:3, :3] @ rotation.as_matrix() + + # Apply VR to robot frame transformation + wrist_mat = VR_TO_ROBOT_FRAME @ target_matrix + + # Store the pose + if side == "left": + self.left_wrist_pose = wrist_mat + else: + self.right_wrist_pose = wrist_mat + + def _extract_gripper_value(self, tracking_data: dict[str, Any]) -> float: + """Extract and validate gripper value from tracking data. + + Args: + tracking_data: Controller data containing 'trigger' + + Returns: + Gripper value clamped to [0.0, 1.0] + """ + gripper_value = tracking_data.get("trigger", 0.0) + try: + gripper_value = float(gripper_value) + return max(0.0, min(1.0, gripper_value)) + except (ValueError, TypeError): + logger.warning("Invalid trigger value: %s", gripper_value) + return 0.0 diff --git a/dimos/teleop/quest3/static/index.html b/dimos/teleop/quest3/static/index.html new file mode 100644 index 0000000000..faa70a3a57 --- /dev/null +++ b/dimos/teleop/quest3/static/index.html @@ -0,0 +1,446 @@ + + + + + + Quest 3 VR Teleop + + + +
+

Quest 3 VR Teleop

+
Ready to connect
+ + +
+ + + + + diff --git a/dimos/teleop/quest3/teleop_module.py b/dimos/teleop/quest3/teleop_module.py new file mode 100644 index 0000000000..c0f2b3e2bd --- /dev/null +++ b/dimos/teleop/quest3/teleop_module.py @@ -0,0 +1,561 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +Quest3 Teleoperation Module + +A dimos Module that runs the WebSocket signaling server for Quest3 VR teleoperation, +receives controller tracking data, calibrates VR poses, and streams DELTA poses to +TeleopArmController. + +Architecture: +- X button pressed โ†’ calibrate VR (capture initial controller poses) +- Computes: delta = current_controller - initial_controller +- Publishes: delta poses (Pose) to TeleopArmController +- TeleopArmController auto-calibrates robot on first delta received +""" + +from __future__ import annotations + +import asyncio +from dataclasses import dataclass, field +import threading +import time +from typing import TYPE_CHECKING, Any + +from dimos.core import Module, Out, rpc +from dimos.core.module import ModuleConfig +from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Vector3 +from dimos.msgs.std_msgs import Bool +from dimos.teleop.quest3.control.fastapi_server import TeleopFastAPIServer +from dimos.utils.logging_config import setup_logger +from dimos.utils.transform_utils import matrix_to_pose + +if TYPE_CHECKING: + import numpy as np + from numpy.typing import NDArray + from websockets.server import WebSocketServerProtocol + +logger = setup_logger() + + +@dataclass +class Quest3TeleopConfig(ModuleConfig): + """Configuration for Quest3 Teleoperation Module.""" + + # WebSocket server settings + signaling_host: str = "0.0.0.0" + signaling_port: int = 8443 # HTTPS port (was 8013 for old setup) + use_https: bool = True # Enable HTTPS for WebXR (required by Quest 3) + + # Driver module settings + driver_module_name: str = "XArmDriver" # Can be "XArmDriver", "PiperDriver", etc. + + # Control settings + position_scale: float = 1.0 # Scale factor for positions + enable_left_arm: bool = True + enable_right_arm: bool = False + + # Safety limits + max_velocity: float = 0.5 # m/s + workspace_limits: dict[str, tuple[float, float]] = field( + default_factory=lambda: { + "x": (0.1, 1), + "y": (-0.8, 0.8), + "z": (0.1, 0.7), + } + ) + + +class Quest3TeleopModule(Module): + """Quest3 VR Teleoperation Module. + + This module: + 1. Runs a WebSocket signaling server for VR connections + 2. Receives controller tracking data from Quest3 + 3. Calibrates VR poses when X button is pressed + 4. Computes and streams DELTA poses (current - initial) to TeleopArmController + 5. TeleopArmController receives deltas and auto-calibrates robot on first delta + + ## Output Topics: + - left_controller_delta: PoseStamped - Left controller delta pose (position + orientation delta) + - right_controller_delta: PoseStamped - Right controller delta pose + - left_trigger: Bool - Left trigger button state + - right_trigger: Bool - Right trigger button state + + ## RPC Methods: + - start() -> None: Start the module and signaling server + - stop() -> None: Stop the module and signaling server + - calibrate_vr() -> dict: Calibrate VR (capture initial controller poses) + - is_vr_calibrated() -> bool: Check if VR is calibrated + - get_status() -> dict: Get current teleoperation status + """ + + default_config = Quest3TeleopConfig + + # Output topics - publishing DELTA poses as PoseStamped + left_controller_delta: Out[PoseStamped] = None # type: ignore[assignment] + right_controller_delta: Out[PoseStamped] = None # type: ignore[assignment] + left_trigger: Out[Bool] = None # type: ignore[assignment] + right_trigger: Out[Bool] = None # type: ignore[assignment] + + def __init__(self, *args, **kwargs) -> None: + super().__init__(*args, **kwargs) + + # No RPC dependencies - data-driven activation + self.rpc_calls = [] + + # FastAPI WebSocket server + self._fastapi_server: TeleopFastAPIServer | None = None + self._server_thread: threading.Thread | None = None + self._event_loop: asyncio.AbstractEventLoop | None = None + + # VR Calibration state + self._vr_calibrated = False + self._left_controller_initial: Pose | None = None + self._right_controller_initial: Pose | None = None + + # Latest controller data (absolute poses from VR) + self._left_pose: NDArray[np.float32] | None = None + self._right_pose: NDArray[np.float32] | None = None + self._left_trigger_pressed: bool = False + self._right_trigger_pressed: bool = False + + # Connection state + self._connected_clients = 0 + + # Rate limiting: streaming frequency + self._last_stream_time: float = 0.0 + self._stream_frequency = 20 # Hz - stream controller data at 20Hz + self._stream_period = 1.0 / self._stream_frequency + + logger.info("Quest3TeleopModule initialized") + + # ========================================================================= + # Module Lifecycle + # ========================================================================= + + @rpc + def start(self) -> None: + """Start the Quest3 teleoperation module and signaling server.""" + logger.info("๐Ÿš€ Starting Quest3TeleopModule...") + super().start() + + # Start signaling server in background thread + self._start_signaling_server() + + protocol = "https" if self.config.use_https else "http" + logger.info( + f"โœ… Quest3 Teleoperation Module started on {protocol}://{self.config.signaling_host}:{self.config.signaling_port}" + ) + logger.info( + f"๐Ÿ“ฑ Open this URL on Quest 3: {protocol}://:{self.config.signaling_port}/" + ) + logger.info("โธ๏ธ Press X button in VR to calibrate and start teleoperation") + + @rpc + def stop(self) -> None: + """Stop the Quest3 teleoperation module and signaling server.""" + logger.info("Stopping Quest3TeleopModule") + + # Stop signaling server + self._stop_signaling_server() + + super().stop() + + def _start_signaling_server(self) -> None: + """Start the FastAPI WebSocket server in a background thread.""" + + def run_server(): + # Create new event loop for this thread + loop = asyncio.new_event_loop() + asyncio.set_event_loop(loop) + self._event_loop = loop + + # Create FastAPI server instance + self._fastapi_server = TeleopFastAPIServer( + host=self.config.signaling_host, + port=self.config.signaling_port, + use_https=self.config.use_https, + ) + + # Register callbacks + self._fastapi_server.set_callback(self._on_tracking_data) + self._fastapi_server.set_command_callback(self._handle_x_button) + logger.info("FastAPI server initialized with callbacks") + + try: + # Run FastAPI server + loop.run_until_complete(self._fastapi_server.start_async()) + except Exception as e: + logger.error(f"FastAPI server error: {e}", exc_info=True) + finally: + loop.close() + + # Start server thread + self._server_thread = threading.Thread(target=run_server, daemon=True, name="FastAPIServer") + self._server_thread.start() + logger.info("FastAPI server thread started") + + def _stop_signaling_server(self) -> None: + """Stop the FastAPI WebSocket server.""" + if self._fastapi_server and self._event_loop: + # Schedule stop on the event loop + asyncio.run_coroutine_threadsafe(self._fastapi_server.stop_async(), self._event_loop) + + if self._event_loop and self._event_loop.is_running(): + # Stop the event loop + self._event_loop.call_soon_threadsafe(self._event_loop.stop) + + # Wait for thread to finish + if self._server_thread and self._server_thread.is_alive(): + self._server_thread.join(timeout=2.0) + + logger.info("FastAPI server stopped") + + # ========================================================================= + # VR Calibration + # ========================================================================= + + @rpc + def calibrate_vr(self) -> dict[str, Any]: + """Calibrate VR by capturing initial controller poses. + + This is called when X button is pressed in VR. + Captures the current controller poses as the "zero" reference. + After calibration, delta poses are published. + + Returns: + Dict with 'success' and optional 'message' or 'error' + """ + logger.info("๐Ÿ“ Calibrating VR controllers...") + + try: + # Check if we have controller data + if self._left_pose is None and self._right_pose is None: + return { + "success": False, + "error": "No controller data received yet. Move controllers and try again.", + } + + # Capture left controller initial pose + if self.config.enable_left_arm and self._left_pose is not None: + left_pose_obj = matrix_to_pose(self._left_pose) + + # Check if pose is valid (not all zeros) + pose_magnitude = ( + left_pose_obj.x**2 + left_pose_obj.y**2 + left_pose_obj.z**2 + ) ** 0.5 + if pose_magnitude < 0.001: + return { + "success": False, + "error": "Left controller pose is invalid (all zeros)", + } + + self._left_controller_initial = left_pose_obj + logger.info( + f"โœ… Captured left controller initial: " + f"pos=[{left_pose_obj.x:.3f}, {left_pose_obj.y:.3f}, {left_pose_obj.z:.3f}], " + f"rpy=[{left_pose_obj.roll:.3f}, {left_pose_obj.pitch:.3f}, {left_pose_obj.yaw:.3f}]" + ) + + # Capture right controller initial pose + if self.config.enable_right_arm and self._right_pose is not None: + right_pose_obj = matrix_to_pose(self._right_pose) + + # Check if pose is valid + pose_magnitude = ( + right_pose_obj.x**2 + right_pose_obj.y**2 + right_pose_obj.z**2 + ) ** 0.5 + if pose_magnitude < 0.001: + return { + "success": False, + "error": "Right controller pose is invalid (all zeros)", + } + + self._right_controller_initial = right_pose_obj + logger.info( + f"โœ… Captured right controller initial: " + f"pos=[{right_pose_obj.x:.3f}, {right_pose_obj.y:.3f}, {right_pose_obj.z:.3f}], " + f"rpy=[{right_pose_obj.roll:.3f}, {right_pose_obj.pitch:.3f}, {right_pose_obj.yaw:.3f}]" + ) + + self._vr_calibrated = True + self._last_stream_time = 0.0 # Reset to start streaming immediately + + logger.info("โœ… VR calibration complete - now streaming delta poses") + return {"success": True, "message": "VR calibrated - move controllers to control robot"} + + except Exception as e: + logger.error(f"VR calibration failed: {e}", exc_info=True) + return {"success": False, "error": str(e)} + + @rpc + def reset_calibration(self) -> dict[str, Any]: + """Reset VR calibration. Stops streaming until recalibrated. + + Returns: + Dict with 'success' and 'message' + """ + self._vr_calibrated = False + self._left_controller_initial = None + self._right_controller_initial = None + + logger.info("โธ๏ธ VR calibration reset - press X to recalibrate") + return {"success": True, "message": "Calibration reset - press X to recalibrate"} + + @rpc + def is_vr_calibrated(self) -> bool: + """Check if VR is calibrated. + + Returns: + True if VR is calibrated and streaming deltas + """ + return self._vr_calibrated + + @rpc + def get_status(self) -> dict: + """Get current teleoperation status. + + Returns: + Dictionary with status information + """ + return { + "vr_calibrated": self._vr_calibrated, + "connected_clients": self._connected_clients, + "server_running": self._server_thread is not None and self._server_thread.is_alive(), + "left_arm_enabled": self.config.enable_left_arm, + "right_arm_enabled": self.config.enable_right_arm, + "has_left_data": self._left_pose is not None, + "has_right_data": self._right_pose is not None, + "left_trigger_pressed": self._left_trigger_pressed, + "right_trigger_pressed": self._right_trigger_pressed, + } + + # ========================================================================= + # X Button Handler + # ========================================================================= + + async def _handle_x_button(self, command_type: str, websocket) -> dict: + """Handle X button press from VR client. + + X button toggles calibration: + - If not calibrated: calibrate VR + - If calibrated: reset calibration (stop streaming) + + Args: + command_type: 'start_teleop' or 'stop_teleop' + websocket: WebSocket connection to send responses + + Returns: + Response dictionary to send back to client + """ + try: + if command_type == "start_teleop": + logger.info("๐ŸŽฎ X button pressed - calibrating VR...") + result = self.calibrate_vr() + + if result.get("success"): + return { + "type": "teleop_started", + "message": result.get("message", "VR calibrated"), + } + else: + return { + "type": "calibration_failed", + "error": result.get("error", "Calibration failed"), + } + + elif command_type == "stop_teleop": + logger.info("โธ๏ธ X button pressed - stopping teleop...") + result = self.reset_calibration() + + return { + "type": "teleop_stopped", + "message": result.get("message", "Teleop stopped"), + } + + except Exception as e: + logger.error(f"Error handling X button: {e}", exc_info=True) + return {"type": "error", "error": f"Command failed: {e!s}"} + + return {"type": "error", "error": f"Unknown command: {command_type}"} + + # ========================================================================= + # Controller Data Processing + # ========================================================================= + + def _on_tracking_data( + self, + left_pose: NDArray[np.float32], + right_pose: NDArray[np.float32], + left_gripper: float, + right_gripper: float, + ) -> None: + """Receive tracking data from VR. + + Called by the signaling server when new tracking data arrives. + Stores absolute poses and computes/streams deltas if calibrated. + + Args: + left_pose: 4x4 transformation matrix for left controller + right_pose: 4x4 transformation matrix for right controller + left_gripper: Left gripper value (0.0-1.0) + right_gripper: Right gripper value (0.0-1.0) + """ + # Store absolute poses + self._left_pose = left_pose + self._right_pose = right_pose + + # Convert gripper values to trigger booleans (threshold at 0.5) + self._left_trigger_pressed = left_gripper > 0.5 + self._right_trigger_pressed = right_gripper > 0.5 + + # Only stream deltas if VR is calibrated + if not self._vr_calibrated: + return + + # Rate limit streaming + current_time = time.time() + time_since_last_stream = current_time - self._last_stream_time + if time_since_last_stream < self._stream_period: + return + + self._last_stream_time = current_time + self._stream_delta_poses(left_pose, right_pose) + + def _stream_delta_poses( + self, + left_pose: NDArray[np.float32], + right_pose: NDArray[np.float32], + ) -> None: + """Compute and stream delta poses (current - initial). + + Args: + left_pose: 4x4 transformation matrix for left controller (absolute) + right_pose: 4x4 transformation matrix for right controller (absolute) + """ + # Track publish count for logging + if not hasattr(self, "_publish_count"): + self._publish_count = 0 + self._publish_count += 1 + + try: + current_time = time.time() + + # Left controller delta + if self.config.enable_left_arm and self._left_controller_initial is not None: + if self.left_controller_delta and hasattr(self.left_controller_delta, "publish"): + left_pose_obj = matrix_to_pose(left_pose) + delta_pose_stamped = self._compute_delta( + left_pose_obj, + self._left_controller_initial, + current_time, + "quest3_left_controller_delta", + ) + + try: + self.left_controller_delta.publish(delta_pose_stamped) + + # Log periodically + if self._publish_count <= 5 or self._publish_count % 100 == 0: + logger.info( + f"๐Ÿ“ค Published left delta #{self._publish_count}: " + f"pos=[{delta_pose_stamped.position.x:.3f}, {delta_pose_stamped.position.y:.3f}, {delta_pose_stamped.position.z:.3f}], " + f"rpy=[{delta_pose_stamped.roll:.3f}, {delta_pose_stamped.pitch:.3f}, {delta_pose_stamped.yaw:.3f}], " + f"frame_id={delta_pose_stamped.frame_id}" + ) + except Exception as e: + logger.error(f"Failed to publish left delta: {e}") + + # Right controller delta + if self.config.enable_right_arm and self._right_controller_initial is not None: + if self.right_controller_delta and hasattr(self.right_controller_delta, "publish"): + right_pose_obj = matrix_to_pose(right_pose) + delta_pose_stamped = self._compute_delta( + right_pose_obj, + self._right_controller_initial, + current_time, + "quest3_right_controller_delta", + ) + + try: + self.right_controller_delta.publish(delta_pose_stamped) + + if self._publish_count <= 5 or self._publish_count % 100 == 0: + logger.info( + f"๐Ÿ“ค Published right delta #{self._publish_count}: " + f"pos=[{delta_pose_stamped.position.x:.3f}, {delta_pose_stamped.position.y:.3f}, {delta_pose_stamped.position.z:.3f}], " + f"frame_id={delta_pose_stamped.frame_id}" + ) + except Exception as e: + logger.error(f"Failed to publish right delta: {e}") + + # Publish trigger states + if self.left_trigger and hasattr(self.left_trigger, "publish"): + try: + self.left_trigger.publish(Bool(data=self._left_trigger_pressed)) + except Exception as e: + logger.debug(f"Failed to publish left trigger: {e}") + + if self.right_trigger and hasattr(self.right_trigger, "publish"): + try: + self.right_trigger.publish(Bool(data=self._right_trigger_pressed)) + except Exception as e: + logger.debug(f"Failed to publish right trigger: {e}") + + except Exception as e: + logger.error(f"Error streaming delta poses: {e}") + + def _compute_delta( + self, current: Pose, initial: Pose, timestamp: float, frame_id: str + ) -> PoseStamped: + """Compute delta pose: current - initial. + + For position: simple subtraction + For orientation: delta_quat = current * inverse(initial) + + Args: + current: Current controller pose + initial: Initial controller pose (reference) + timestamp: Timestamp for the delta pose + frame_id: Frame ID for the delta pose + + Returns: + Delta pose as PoseStamped (position delta + orientation delta) + """ + # Position delta + delta_x = current.x - initial.x + delta_y = current.y - initial.y + delta_z = current.z - initial.z + + # Orientation delta: delta_quat = current * inverse(initial) + delta_quat = current.orientation * initial.orientation.inverse() + + delta_pose = Pose( + position=Vector3(delta_x, delta_y, delta_z), + orientation=delta_quat, + ) + + return PoseStamped( + ts=timestamp, + frame_id=frame_id, + position=delta_pose.position, + orientation=delta_pose.orientation, + ) + + +# Expose blueprint for declarative composition +quest3_teleop_module = Quest3TeleopModule.blueprint diff --git a/dimos/teleop/teleop_arm_controller.py b/dimos/teleop/teleop_arm_controller.py new file mode 100644 index 0000000000..19e3335b78 --- /dev/null +++ b/dimos/teleop/teleop_arm_controller.py @@ -0,0 +1,535 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +Teleop Arm Controller + +Receives DELTA poses from Quest3TeleopModule and applies them to the robot. +Auto-calibrates robot on first delta received. + +Architecture: +- Subscribes to left_controller_delta and right_controller_delta from Quest3TeleopModule +- On first delta received: gets robot's initial end-effector pose via RPC +- Calculates: target_pose = initial_robot_pose + delta +- Publishes cartesian_command (Pose) to driver +""" + +from dataclasses import dataclass +import threading +import time +import traceback +from typing import Any + +import numpy as np + +from dimos.core import In, Module, Out, rpc +from dimos.core.module import ModuleConfig +from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Vector3 +from dimos.msgs.std_msgs import Bool +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +@dataclass +class TeleopArmControllerConfig(ModuleConfig): + """Configuration for Teleop Arm Controller.""" + + # Driver settings + driver_module_name: str = "XArmDriver" # Name of the driver module to get robot pose from + + # Control settings + control_frequency: float = 50.0 # Hz - control loop frequency + enable_left_arm: bool = True + enable_right_arm: bool = False + + # Safety settings + workspace_limits: dict[str, tuple[float, float]] | None = None # Optional workspace limits + + +class TeleopArmController(Module): + """Teleop Arm Controller - applies delta poses to robot. + + This controller: + 1. Receives DELTA poses (PoseStamped) from Quest3TeleopModule + 2. On first delta: gets initial robot end-effector pose via RPC (auto-calibration) + 3. Applies delta to initial robot pose: target = initial_robot + delta + 4. Publishes cartesian commands (Pose) to manipulator driver + 5. Optionally applies workspace limits + + Works with any manipulator driver that provides: + - get_cartesian_state RPC method + - cartesian_command input topic (Pose) + """ + + default_config = TeleopArmControllerConfig + config: TeleopArmControllerConfig + + # Input topics - receiving DELTA poses as PoseStamped + left_controller_delta: In[PoseStamped] = None # type: ignore[assignment] + right_controller_delta: In[PoseStamped] = None # type: ignore[assignment] + left_trigger: In[Bool] = None # type: ignore[assignment] + right_trigger: In[Bool] = None # type: ignore[assignment] + + # Output topics (Pose for commands) + left_cartesian_command: Out[Pose] = None # type: ignore[assignment] + right_cartesian_command: Out[Pose] = None # type: ignore[assignment] + + # RPC dependencies (dynamically set based on config) + rpc_calls: list[str] = [] + + def __init__(self, *args: Any, **kwargs: Any) -> None: + """Initialize the teleop arm controller.""" + super().__init__(*args, **kwargs) + + # Set RPC calls dynamically based on driver name + driver_name = self.config.driver_module_name + self.rpc_calls = [ + f"{driver_name}.get_cartesian_state", + ] + + # Latest delta data + self._left_delta: PoseStamped | None = None + self._right_delta: PoseStamped | None = None + self._left_trigger_pressed: bool = False + self._right_trigger_pressed: bool = False + + # Robot initial state (auto-calibrated on first delta) + self._left_robot_initial_position: Vector3 | None = None + self._left_robot_initial_rpy: Vector3 | None = None + self._right_robot_initial_position: Vector3 | None = None + self._right_robot_initial_rpy: Vector3 | None = None + self._robot_calibrated = False + + # State for unwrapped logging + self._last_logged_rpy = {} + + # Control loop + self._control_thread: threading.Thread | None = None + self._stop_event = threading.Event() + + # State lock + self._state_lock = threading.Lock() + + logger.info("TeleopArmController initialized - waiting for delta poses") + + # ========================================================================= + # Module Lifecycle + # ========================================================================= + + @rpc + def start(self) -> None: + """Start the teleop arm controller.""" + logger.info("๐Ÿš€ Starting TeleopArmController...") + super().start() + + # Subscribe to input topics (delta poses) + if self.left_controller_delta and hasattr(self.left_controller_delta, "subscribe"): + self.left_controller_delta.subscribe(self._on_left_delta) + logger.debug("Subscribed to left_controller_delta") + + if self.right_controller_delta and hasattr(self.right_controller_delta, "subscribe"): + self.right_controller_delta.subscribe(self._on_right_delta) + logger.debug("Subscribed to right_controller_delta") + + if self.left_trigger and hasattr(self.left_trigger, "subscribe"): + self.left_trigger.subscribe(self._on_left_trigger) + logger.debug("Subscribed to left_trigger") + + if self.right_trigger and hasattr(self.right_trigger, "subscribe"): + self.right_trigger.subscribe(self._on_right_trigger) + logger.debug("Subscribed to right_trigger") + + # Start control loop + self._stop_event.clear() + self._control_thread = threading.Thread( + target=self._control_loop, daemon=True, name="TeleopArmController" + ) + self._control_thread.start() + + logger.info("TeleopArmController started - waiting for delta poses to auto-calibrate") + + @rpc + def stop(self) -> None: + """Stop the teleop arm controller.""" + logger.info("Stopping TeleopArmController") + + # Stop control loop + self._stop_event.set() + if self._control_thread: + self._control_thread.join(timeout=2.0) + + super().stop() + logger.info("TeleopArmController stopped") + + # ========================================================================= + # Input Callbacks + # ========================================================================= + + def _on_left_delta(self, delta: PoseStamped) -> None: + """Callback for left controller delta pose. + + On first delta, auto-calibrates robot. + """ + if not self.config.enable_left_arm: + return + + # Auto-calibrate robot on first delta received + if not self._robot_calibrated: + logger.info("๐Ÿ“ฅ First delta received - auto-calibrating robot...") + self._calibrate_robot() + + with self._state_lock: + self._left_delta = delta + + # Log first few deltas + if not hasattr(self, "_left_delta_count"): + self._left_delta_count = 0 + self._left_delta_count += 1 + if self._left_delta_count <= 5: + logger.info( + f"๐Ÿ“ฅ Received left delta #{self._left_delta_count}: " + f"pos=[{delta.position.x:.3f}, {delta.position.y:.3f}, {delta.position.z:.3f}], " + f"frame_id={delta.frame_id}" + ) + + def _on_right_delta(self, delta: PoseStamped) -> None: + """Callback for right controller delta pose. + + On first delta, auto-calibrates robot. + """ + if not self.config.enable_right_arm: + return + + # Auto-calibrate robot on first delta received + if not self._robot_calibrated: + logger.info("๐Ÿ“ฅ First delta received - auto-calibrating robot...") + self._calibrate_robot() + + with self._state_lock: + self._right_delta = delta + + if not hasattr(self, "_right_delta_count"): + self._right_delta_count = 0 + self._right_delta_count += 1 + if self._right_delta_count <= 5: + logger.info( + f"๐Ÿ“ฅ Received right delta #{self._right_delta_count}: " + f"pos=[{delta.position.x:.3f}, {delta.position.y:.3f}, {delta.position.z:.3f}], " + f"frame_id={delta.frame_id}" + ) + + def _on_left_trigger(self, trigger: Bool) -> None: + """Callback for left trigger.""" + with self._state_lock: + self._left_trigger_pressed = trigger.data + + def _on_right_trigger(self, trigger: Bool) -> None: + """Callback for right trigger.""" + with self._state_lock: + self._right_trigger_pressed = trigger.data + + # ========================================================================= + # Robot Calibration (Auto-triggered on first delta) + # ========================================================================= + + def _calibrate_robot(self) -> bool: + """Calibrate robot by getting its initial pose via RPC. + + Called automatically when first delta pose is received. + + Returns: + True if calibration successful, False otherwise + """ + logger.info("๐Ÿค– Calibrating robot (getting initial pose)...") + + try: + driver_name = self.config.driver_module_name + rpc_method_name = f"{driver_name}.get_cartesian_state" + + get_cartesian_state = self.get_rpc_calls(rpc_method_name) + + if get_cartesian_state is None: + logger.error("RPC callable is None - check blueprint wiring") + return False + + result = get_cartesian_state() + + if result and result.get("success"): + pose_data = result.get("pose", {}) + + # Store robot initial state + if self.config.enable_left_arm: + self._left_robot_initial_position = Vector3( + pose_data.get("x", 0.0), pose_data.get("y", 0.0), pose_data.get("z", 0.0) + ) + self._left_robot_initial_rpy = Vector3( + pose_data.get("roll", 0.0), + pose_data.get("pitch", 0.0), + pose_data.get("yaw", 0.0), + ) + logger.info( + f"โœ… Robot initial pose: " + f"pos=[{self._left_robot_initial_position.x:.3f}, " + f"{self._left_robot_initial_position.y:.3f}, " + f"{self._left_robot_initial_position.z:.3f}], " + f"rpy=[{self._left_robot_initial_rpy.x:.3f}, " + f"{self._left_robot_initial_rpy.y:.3f}, " + f"{self._left_robot_initial_rpy.z:.3f}]" + ) + + if self.config.enable_right_arm: + self._right_robot_initial_position = Vector3( + pose_data.get("x", 0.0), pose_data.get("y", 0.0), pose_data.get("z", 0.0) + ) + self._right_robot_initial_rpy = Vector3( + pose_data.get("roll", 0.0), + pose_data.get("pitch", 0.0), + pose_data.get("yaw", 0.0), + ) + + self._robot_calibrated = True + logger.info("โœ… Robot calibration complete - control active!") + return True + else: + error_msg = f"Failed to get robot cartesian state: {result}" + logger.error(error_msg) + return False + + except Exception as e: + logger.error(f"Robot calibration failed: {e}", exc_info=True) + traceback.print_exc() + return False + + @rpc + def recalibrate_robot(self) -> dict[str, Any]: + """Manually recalibrate robot (get new initial pose). + + Returns: + Dict with 'success' and optional 'message' or 'error' + """ + self._robot_calibrated = False + success = self._calibrate_robot() + + if success: + return {"success": True, "message": "Robot recalibrated"} + else: + return {"success": False, "error": "Recalibration failed"} + + @rpc + def is_robot_calibrated(self) -> bool: + """Check if robot is calibrated. + + Returns: + True if calibrated, False otherwise + """ + return self._robot_calibrated + + @rpc + def get_status(self) -> dict: + """Get controller status. + + Returns: + Dict with status information + """ + return { + "robot_calibrated": self._robot_calibrated, + "has_left_delta": self._left_delta is not None, + "has_right_delta": self._right_delta is not None, + "left_arm_enabled": self.config.enable_left_arm, + "right_arm_enabled": self.config.enable_right_arm, + } + + # ========================================================================= + # Control Loop + # ========================================================================= + + def _control_loop(self) -> None: + """Main control loop - applies deltas to robot initial pose.""" + logger.info("Control loop started") + period = 1.0 / self.config.control_frequency + next_time = time.perf_counter() + period + + loop_count = 0 + while not self._stop_event.is_set(): + try: + loop_count += 1 + + # Get latest state + with self._state_lock: + left_delta = self._left_delta + right_delta = self._right_delta + robot_calibrated = self._robot_calibrated + + # Log state periodically + if loop_count <= 10 or loop_count % 100 == 0: + logger.debug( + f"Control loop #{loop_count}: robot_calibrated={robot_calibrated}, " + f"left_delta={'โœ“' if left_delta else 'โœ—'}, right_delta={'โœ“' if right_delta else 'โœ—'}" + ) + + # Only process if robot is calibrated + if robot_calibrated: + # Process left arm + if self.config.enable_left_arm and left_delta is not None: + self._apply_delta(left_delta, "left") + + # Process right arm + if self.config.enable_right_arm and right_delta is not None: + self._apply_delta(right_delta, "right") + + except Exception as e: + logger.error(f"Error in control loop: {e}", exc_info=True) + + # Rate control + next_time += period + sleep_time = next_time - time.perf_counter() + if sleep_time > 0: + time.sleep(sleep_time) + else: + next_time = time.perf_counter() + period + + logger.info("Control loop stopped") + + def _apply_delta(self, delta: PoseStamped, arm_side: str) -> None: + """Apply delta pose to robot initial pose and publish command. + + Calculates: target_pose = initial_robot_pose + delta + + Args: + delta: Delta pose (PoseStamped) from Quest3TeleopModule + arm_side: "left" or "right" + """ + try: + # Track command count for logging + if not hasattr(self, "_command_count"): + self._command_count = 0 + self._command_count += 1 + + # Get robot initial poses for this arm + if arm_side == "left": + robot_initial_pos = self._left_robot_initial_position + robot_initial_rpy = self._left_robot_initial_rpy + else: + robot_initial_pos = self._right_robot_initial_position + robot_initial_rpy = self._right_robot_initial_rpy + + if robot_initial_pos is None or robot_initial_rpy is None: + logger.debug(f"{arm_side.capitalize()} arm not calibrated, skipping") + return + + # Calculate target position: initial_robot + delta + target_x = robot_initial_pos.x + delta.position.x + target_y = robot_initial_pos.y + delta.position.y + target_z = robot_initial_pos.z + delta.position.z + + # Calculate target orientation using RPY addition: + # Convert delta quaternion to Euler, then add to robot initial RPY + delta_euler = delta.orientation.to_euler() + + target_roll = robot_initial_rpy.x + delta_euler.x + target_pitch = robot_initial_rpy.y + delta_euler.y + target_yaw = robot_initial_rpy.z + delta_euler.z + + # Wrap angles to [-ฯ€, ฯ€] + def wrap_angle(angle): + return np.arctan2(np.sin(angle), np.cos(angle)) + + target_roll = wrap_angle(target_roll) + target_pitch = wrap_angle(target_pitch) + target_yaw = wrap_angle(target_yaw) + + # Create target orientation quaternion from final RPY + target_rpy = Vector3(target_roll, target_pitch, target_yaw) + target_orientation = Quaternion.from_euler(target_rpy) + + # Apply workspace limits if configured + if self.config.workspace_limits: + target_x = max( + self.config.workspace_limits["x"][0], + min(self.config.workspace_limits["x"][1], target_x), + ) + target_y = max( + self.config.workspace_limits["y"][0], + min(self.config.workspace_limits["y"][1], target_y), + ) + target_z = max( + self.config.workspace_limits["z"][0], + min(self.config.workspace_limits["z"][1], target_z), + ) + + # Create target pose + target_pose = Pose( + position=Vector3(target_x, target_y, target_z), + orientation=target_orientation, + ) + + # Log and publish + if arm_side == "left": + quat = target_pose.orientation + + # Unwrap Euler angles for smooth logging + current_rpy = np.array([target_pose.roll, target_pose.pitch, target_pose.yaw]) + if arm_side in self._last_logged_rpy: + prev_rpy = self._last_logged_rpy[arm_side] + diff = current_rpy - prev_rpy + diff = (diff + np.pi) % (2 * np.pi) - np.pi + current_rpy = prev_rpy + diff + self._last_logged_rpy[arm_side] = current_rpy + + print( + f"Target Pose: pos=[{target_pose.x:.3f}, {target_pose.y:.3f}, {target_pose.z:.3f}], " + f"rpy=[{current_rpy[0]:.3f}, {current_rpy[1]:.3f}, {current_rpy[2]:.3f}], " + f"quat=[{quat.x:.4f}, {quat.y:.4f}, {quat.z:.4f}, {quat.w:.4f}]", + flush=True, + ) + + # Publish to robot + # if self.left_cartesian_command and hasattr(self.left_cartesian_command, "publish"): + # try: + # self.left_cartesian_command.publish(target_pose) + # except Exception as e: + # logger.error(f"Failed to publish left cartesian command: {e}") + + elif arm_side == "right": + quat = target_pose.orientation + + current_rpy = np.array([target_pose.roll, target_pose.pitch, target_pose.yaw]) + if arm_side in self._last_logged_rpy: + prev_rpy = self._last_logged_rpy[arm_side] + diff = current_rpy - prev_rpy + diff = (diff + np.pi) % (2 * np.pi) - np.pi + current_rpy = prev_rpy + diff + self._last_logged_rpy[arm_side] = current_rpy + + print( + f"Target Pose: pos=[{target_pose.x:.3f}, {target_pose.y:.3f}, {target_pose.z:.3f}], " + f"rpy=[{current_rpy[0]:.3f}, {current_rpy[1]:.3f}, {current_rpy[2]:.3f}], " + f"quat=[{quat.x:.4f}, {quat.y:.4f}, {quat.z:.4f}, {quat.w:.4f}]", + flush=True, + ) + + # Publish to robot + # if self.right_cartesian_command and hasattr(self.right_cartesian_command, "publish"): + # try: + # self.right_cartesian_command.publish(target_pose) + # except Exception as e: + # logger.error(f"Failed to publish right cartesian command: {e}") + + except Exception as e: + logger.error(f"Error applying {arm_side} delta: {e}", exc_info=True) + + +# Expose blueprint for declarative composition +teleop_arm_controller = TeleopArmController.blueprint diff --git a/dimos/teleop/teleop_blueprints.py b/dimos/teleop/teleop_blueprints.py new file mode 100644 index 0000000000..c63c21ba29 --- /dev/null +++ b/dimos/teleop/teleop_blueprints.py @@ -0,0 +1,144 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +Blueprints for Quest3 teleoperation with XArm. + +This module provides declarative blueprints for combining Quest3TeleopModule, +TeleopArmController, and XArmDriver for VR teleoperation. + +Architecture: + Quest3TeleopModule (VR calibration, delta computation) + โ†“ X button โ†’ calibrate VR + โ†“ Computes: delta = current_controller - initial_controller + โ†“ Publishes: delta poses (Pose) + โ†“ + TeleopArmController (Robot calibration, delta application) + โ†“ First delta โ†’ auto-calibrate robot + โ†“ Computes: target = initial_robot + delta + โ†“ Publishes: cartesian commands (Pose) + โ†“ + XArmDriver (Robot control) + +Usage: + # Programmatically: + from dimos.teleop.teleop_blueprints import quest3_xarm6_teleop + coordinator = quest3_xarm6_teleop.build() + coordinator.loop() + + # Or build your own composition: + from dimos.teleop.quest3.teleop_module import quest3_teleop_module + from dimos.teleop.teleop_arm_controller import teleop_arm_controller + from dimos.hardware.manipulators.xarm.xarm_driver import xarm_driver + from dimos.core.blueprints import autoconnect + + my_system = autoconnect( + quest3_teleop_module( + driver_module_name="XArmDriver", + signaling_port=8443, + ), + teleop_arm_controller( + driver_module_name="XArmDriver", + enable_left_arm=True, + ), + xarm_driver( + ip="192.168.1.210", + dof=6, + connection_type="hardware", + ), + ) +""" + +from dimos.core.blueprints import autoconnect +from dimos.core.transport import LCMTransport +from dimos.hardware.manipulators.xarm.xarm_driver import XArmDriver, xarm_driver +from dimos.msgs.geometry_msgs import Pose, PoseStamped +from dimos.msgs.std_msgs import Bool +from dimos.teleop.quest3.teleop_module import Quest3TeleopModule, quest3_teleop_module +from dimos.teleop.teleop_arm_controller import TeleopArmController, teleop_arm_controller + +# ============================================================================= +# Quest3 + XArm6 Teleoperation Blueprint +# ============================================================================= +# Combines: +# - Quest3TeleopModule: VR calibration + delta computation +# - TeleopArmController: Robot calibration + delta application +# - XArmDriver: Hardware/sim interface +# +# Data flow: +# Quest3TeleopModule.left_controller_delta โ”€โ”€โ–บ TeleopArmController.left_controller_delta +# Quest3TeleopModule.right_controller_delta โ”€โ”€โ–บ TeleopArmController.right_controller_delta +# Quest3TeleopModule.left_trigger โ”€โ”€โ–บ TeleopArmController.left_trigger +# Quest3TeleopModule.right_trigger โ”€โ”€โ–บ TeleopArmController.right_trigger +# TeleopArmController.left_cartesian_command โ”€โ”€โ–บ XArmDriver.cartesian_command +# TeleopArmController (RPC) โ”€โ”€โ–บ XArmDriver.get_cartesian_state (auto-calibration) +# ============================================================================= + +quest3_xarm6_teleop = ( + autoconnect( + quest3_teleop_module( + driver_module_name="XArmDriver", + signaling_host="0.0.0.0", + signaling_port=8443, # HTTPS port (required for WebXR) + use_https=True, # Enable HTTPS for Quest 3 WebXR + enable_left_arm=True, + enable_right_arm=False, + ), + teleop_arm_controller( + driver_module_name="XArmDriver", + control_frequency=20.0, # 20 Hz for consistent control + enable_left_arm=True, + enable_right_arm=False, + ), + xarm_driver( + ip="192.168.1.210", + dof=6, + has_gripper=False, + has_force_torque=False, + has_cartesian_control=True, # Enable Cartesian control for teleop + control_rate=20, # 20 Hz to match teleop frequency + monitor_rate=10, + connection_type="hardware", # Change to "hardware" for real robot + ), + ) + .remappings( + [ + # Map TeleopArmController's left_cartesian_command to XArmDriver's cartesian_command + (TeleopArmController, "left_cartesian_command", "cartesian_command"), + ] + ) + .transports( + { + # Delta poses from Quest3TeleopModule to TeleopArmController + ("left_controller_delta", PoseStamped): LCMTransport( + "/quest3/left_controller_delta", PoseStamped + ), + ("right_controller_delta", PoseStamped): LCMTransport( + "/quest3/right_controller_delta", PoseStamped + ), + # Trigger states + ("left_trigger", Bool): LCMTransport("/quest3/left_trigger", Bool), + ("right_trigger", Bool): LCMTransport("/quest3/right_trigger", Bool), + # Cartesian commands to robot + ("left_cartesian_command", Pose): LCMTransport("/xarm/cartesian_command", Pose), + ("right_cartesian_command", Pose): LCMTransport("/xarm/right_cartesian_command", Pose), + ("cartesian_command", Pose): LCMTransport("/xarm/cartesian_command", Pose), + } + ) +) + + +__all__ = [ + "quest3_xarm6_teleop", +] From f0894f2d7ade4ce7d66dafce57656a9cbb51774b Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 20:01:55 +0000 Subject: [PATCH 02/72] Fis: Generalizing controller module Former-commit-id: d713c7d4052d9844ed2b31abcc85571db60a2384 Former-commit-id: 1529848b746b85ed08e80223625fd80001ba150c --- dimos/teleop/teleop_arm_controller.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/dimos/teleop/teleop_arm_controller.py b/dimos/teleop/teleop_arm_controller.py index 19e3335b78..0cc1872b99 100644 --- a/dimos/teleop/teleop_arm_controller.py +++ b/dimos/teleop/teleop_arm_controller.py @@ -47,12 +47,12 @@ class TeleopArmControllerConfig(ModuleConfig): """Configuration for Teleop Arm Controller.""" # Driver settings - driver_module_name: str = "XArmDriver" # Name of the driver module to get robot pose from + driver_module_name: str = "RobotDriver" # Name of the driver module to get robot pose from # Control settings control_frequency: float = 50.0 # Hz - control loop frequency enable_left_arm: bool = True - enable_right_arm: bool = False + enable_right_arm: bool = True # Safety settings workspace_limits: dict[str, tuple[float, float]] | None = None # Optional workspace limits @@ -187,7 +187,7 @@ def _on_left_delta(self, delta: PoseStamped) -> None: # Auto-calibrate robot on first delta received if not self._robot_calibrated: - logger.info("๐Ÿ“ฅ First delta received - auto-calibrating robot...") + logger.info("First delta received - auto-calibrating robot...") self._calibrate_robot() with self._state_lock: @@ -199,7 +199,7 @@ def _on_left_delta(self, delta: PoseStamped) -> None: self._left_delta_count += 1 if self._left_delta_count <= 5: logger.info( - f"๐Ÿ“ฅ Received left delta #{self._left_delta_count}: " + f"Received left delta #{self._left_delta_count}: " f"pos=[{delta.position.x:.3f}, {delta.position.y:.3f}, {delta.position.z:.3f}], " f"frame_id={delta.frame_id}" ) @@ -214,7 +214,7 @@ def _on_right_delta(self, delta: PoseStamped) -> None: # Auto-calibrate robot on first delta received if not self._robot_calibrated: - logger.info("๐Ÿ“ฅ First delta received - auto-calibrating robot...") + logger.info("First delta received - auto-calibrating robot...") self._calibrate_robot() with self._state_lock: @@ -225,7 +225,7 @@ def _on_right_delta(self, delta: PoseStamped) -> None: self._right_delta_count += 1 if self._right_delta_count <= 5: logger.info( - f"๐Ÿ“ฅ Received right delta #{self._right_delta_count}: " + f"Received right delta #{self._right_delta_count}: " f"pos=[{delta.position.x:.3f}, {delta.position.y:.3f}, {delta.position.z:.3f}], " f"frame_id={delta.frame_id}" ) @@ -252,7 +252,7 @@ def _calibrate_robot(self) -> bool: Returns: True if calibration successful, False otherwise """ - logger.info("๐Ÿค– Calibrating robot (getting initial pose)...") + logger.info("Calibrating robot (getting initial pose)...") try: driver_name = self.config.driver_module_name @@ -280,7 +280,7 @@ def _calibrate_robot(self) -> bool: pose_data.get("yaw", 0.0), ) logger.info( - f"โœ… Robot initial pose: " + f"Robot initial pose: " f"pos=[{self._left_robot_initial_position.x:.3f}, " f"{self._left_robot_initial_position.y:.3f}, " f"{self._left_robot_initial_position.z:.3f}], " From edf508fc0bb3ec6ca9b7ec944ab5e7c9f71dfa13 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 20:02:08 +0000 Subject: [PATCH 03/72] Fix: debug statements, Feat: uses both right and left controllers Former-commit-id: b5264de291605bf100d57093ab6e05e71d3311c6 Former-commit-id: b3f8b0a7b635e9854dd58cf458deced7f21d5071 --- dimos/teleop/quest3/teleop_module.py | 72 ++++++++++++++-------------- 1 file changed, 35 insertions(+), 37 deletions(-) diff --git a/dimos/teleop/quest3/teleop_module.py b/dimos/teleop/quest3/teleop_module.py index c0f2b3e2bd..7891b9b5a9 100644 --- a/dimos/teleop/quest3/teleop_module.py +++ b/dimos/teleop/quest3/teleop_module.py @@ -60,16 +60,14 @@ class Quest3TeleopConfig(ModuleConfig): signaling_port: int = 8443 # HTTPS port (was 8013 for old setup) use_https: bool = True # Enable HTTPS for WebXR (required by Quest 3) - # Driver module settings - driver_module_name: str = "XArmDriver" # Can be "XArmDriver", "PiperDriver", etc. - # Control settings position_scale: float = 1.0 # Scale factor for positions enable_left_arm: bool = True - enable_right_arm: bool = False + enable_right_arm: bool = True # Safety limits - max_velocity: float = 0.5 # m/s + safety_limits: bool = True + max_: float = 0.5 # m/s workspace_limits: dict[str, tuple[float, float]] = field( default_factory=lambda: { "x": (0.1, 1), @@ -86,26 +84,24 @@ class Quest3TeleopModule(Module): 1. Runs a WebSocket signaling server for VR connections 2. Receives controller tracking data from Quest3 3. Calibrates VR poses when X button is pressed - 4. Computes and streams DELTA poses (current - initial) to TeleopArmController - 5. TeleopArmController receives deltas and auto-calibrates robot on first delta + 4. Computes and streams DELTA poses (current - initial) out - ## Output Topics: - - left_controller_delta: PoseStamped - Left controller delta pose (position + orientation delta) - - right_controller_delta: PoseStamped - Right controller delta pose - - left_trigger: Bool - Left trigger button state - - right_trigger: Bool - Right trigger button state + ## LCM Topics: + left_controller_delta: Out[PoseStamped] Left controller delta pose (position + orientation delta) = current - initial + right_controller_delta: Out[PoseStamped] Right controller delta pose (position + orientation delta) = current - initial + left_trigger: Out[Bool] Left trigger button state + right_trigger: Out[Bool] Right trigger button state ## RPC Methods: - start() -> None: Start the module and signaling server - stop() -> None: Stop the module and signaling server - calibrate_vr() -> dict: Calibrate VR (capture initial controller poses) - - is_vr_calibrated() -> bool: Check if VR is calibrated + - _is_calibrated() -> bool: Check if VR is calibrated - get_status() -> dict: Get current teleoperation status """ default_config = Quest3TeleopConfig - # Output topics - publishing DELTA poses as PoseStamped left_controller_delta: Out[PoseStamped] = None # type: ignore[assignment] right_controller_delta: Out[PoseStamped] = None # type: ignore[assignment] left_trigger: Out[Bool] = None # type: ignore[assignment] @@ -123,7 +119,7 @@ def __init__(self, *args, **kwargs) -> None: self._event_loop: asyncio.AbstractEventLoop | None = None # VR Calibration state - self._vr_calibrated = False + self._is_calibrated = False self._left_controller_initial: Pose | None = None self._right_controller_initial: Pose | None = None @@ -133,7 +129,7 @@ def __init__(self, *args, **kwargs) -> None: self._left_trigger_pressed: bool = False self._right_trigger_pressed: bool = False - # Connection state + # Connection state [for future use] self._connected_clients = 0 # Rate limiting: streaming frequency @@ -150,7 +146,7 @@ def __init__(self, *args, **kwargs) -> None: @rpc def start(self) -> None: """Start the Quest3 teleoperation module and signaling server.""" - logger.info("๐Ÿš€ Starting Quest3TeleopModule...") + logger.info("Starting Quest3 Teleoperation Module...") super().start() # Start signaling server in background thread @@ -158,17 +154,17 @@ def start(self) -> None: protocol = "https" if self.config.use_https else "http" logger.info( - f"โœ… Quest3 Teleoperation Module started on {protocol}://{self.config.signaling_host}:{self.config.signaling_port}" + f"Quest3 Teleoperation Module started on {protocol}://{self.config.signaling_host}:{self.config.signaling_port}" ) logger.info( - f"๐Ÿ“ฑ Open this URL on Quest 3: {protocol}://:{self.config.signaling_port}/" + f"Open this URL on Quest 3: {protocol}://:{self.config.signaling_port}/" ) - logger.info("โธ๏ธ Press X button in VR to calibrate and start teleoperation") + logger.info("Press X button in VR to calibrate and start teleoperation") @rpc def stop(self) -> None: """Stop the Quest3 teleoperation module and signaling server.""" - logger.info("Stopping Quest3TeleopModule") + logger.info("Stopping Quest3 Teleoperation Module...") # Stop signaling server self._stop_signaling_server() @@ -240,7 +236,7 @@ def calibrate_vr(self) -> dict[str, Any]: Returns: Dict with 'success' and optional 'message' or 'error' """ - logger.info("๐Ÿ“ Calibrating VR controllers...") + logger.info("Calibrating VR controllers...") try: # Check if we have controller data @@ -255,6 +251,7 @@ def calibrate_vr(self) -> dict[str, Any]: left_pose_obj = matrix_to_pose(self._left_pose) # Check if pose is valid (not all zeros) + # TODO [Can remove if the first pose received after pressing X button is valid] pose_magnitude = ( left_pose_obj.x**2 + left_pose_obj.y**2 + left_pose_obj.z**2 ) ** 0.5 @@ -266,7 +263,7 @@ def calibrate_vr(self) -> dict[str, Any]: self._left_controller_initial = left_pose_obj logger.info( - f"โœ… Captured left controller initial: " + f"Captured left controller initial: " f"pos=[{left_pose_obj.x:.3f}, {left_pose_obj.y:.3f}, {left_pose_obj.z:.3f}], " f"rpy=[{left_pose_obj.roll:.3f}, {left_pose_obj.pitch:.3f}, {left_pose_obj.yaw:.3f}]" ) @@ -275,7 +272,8 @@ def calibrate_vr(self) -> dict[str, Any]: if self.config.enable_right_arm and self._right_pose is not None: right_pose_obj = matrix_to_pose(self._right_pose) - # Check if pose is valid + # Check if pose is valid (not all zeros) + # TODO [Can remove if the first pose received after pressing X button is valid] pose_magnitude = ( right_pose_obj.x**2 + right_pose_obj.y**2 + right_pose_obj.z**2 ) ** 0.5 @@ -292,10 +290,10 @@ def calibrate_vr(self) -> dict[str, Any]: f"rpy=[{right_pose_obj.roll:.3f}, {right_pose_obj.pitch:.3f}, {right_pose_obj.yaw:.3f}]" ) - self._vr_calibrated = True + self._is_calibrated = True self._last_stream_time = 0.0 # Reset to start streaming immediately - logger.info("โœ… VR calibration complete - now streaming delta poses") + logger.info("VR calibration complete. Now streaming delta poses...") return {"success": True, "message": "VR calibrated - move controllers to control robot"} except Exception as e: @@ -309,11 +307,11 @@ def reset_calibration(self) -> dict[str, Any]: Returns: Dict with 'success' and 'message' """ - self._vr_calibrated = False + self._is_calibrated = False self._left_controller_initial = None self._right_controller_initial = None - logger.info("โธ๏ธ VR calibration reset - press X to recalibrate") + logger.info("VR calibration reset. Press X to recalibrate...") return {"success": True, "message": "Calibration reset - press X to recalibrate"} @rpc @@ -323,7 +321,7 @@ def is_vr_calibrated(self) -> bool: Returns: True if VR is calibrated and streaming deltas """ - return self._vr_calibrated + return self._is_calibrated @rpc def get_status(self) -> dict: @@ -333,7 +331,7 @@ def get_status(self) -> dict: Dictionary with status information """ return { - "vr_calibrated": self._vr_calibrated, + "is_calibrated": self._is_calibrated, "connected_clients": self._connected_clients, "server_running": self._server_thread is not None and self._server_thread.is_alive(), "left_arm_enabled": self.config.enable_left_arm, @@ -364,7 +362,7 @@ async def _handle_x_button(self, command_type: str, websocket) -> dict: """ try: if command_type == "start_teleop": - logger.info("๐ŸŽฎ X button pressed - calibrating VR...") + logger.info("X button pressed - calibrating VR...") result = self.calibrate_vr() if result.get("success"): @@ -379,7 +377,7 @@ async def _handle_x_button(self, command_type: str, websocket) -> dict: } elif command_type == "stop_teleop": - logger.info("โธ๏ธ X button pressed - stopping teleop...") + logger.info("X button pressed - stopping teleop...") result = self.reset_calibration() return { @@ -424,13 +422,13 @@ def _on_tracking_data( self._right_trigger_pressed = right_gripper > 0.5 # Only stream deltas if VR is calibrated - if not self._vr_calibrated: + if not self._is_calibrated: + logger.warning("VR is not calibrated. Not streaming delta poses...") return # Rate limit streaming current_time = time.time() - time_since_last_stream = current_time - self._last_stream_time - if time_since_last_stream < self._stream_period: + if current_time - self._last_stream_time < self._stream_period: return self._last_stream_time = current_time @@ -472,7 +470,7 @@ def _stream_delta_poses( # Log periodically if self._publish_count <= 5 or self._publish_count % 100 == 0: logger.info( - f"๐Ÿ“ค Published left delta #{self._publish_count}: " + f"Published left delta #{self._publish_count}: " f"pos=[{delta_pose_stamped.position.x:.3f}, {delta_pose_stamped.position.y:.3f}, {delta_pose_stamped.position.z:.3f}], " f"rpy=[{delta_pose_stamped.roll:.3f}, {delta_pose_stamped.pitch:.3f}, {delta_pose_stamped.yaw:.3f}], " f"frame_id={delta_pose_stamped.frame_id}" @@ -496,7 +494,7 @@ def _stream_delta_poses( if self._publish_count <= 5 or self._publish_count % 100 == 0: logger.info( - f"๐Ÿ“ค Published right delta #{self._publish_count}: " + f"Published right delta #{self._publish_count}: " f"pos=[{delta_pose_stamped.position.x:.3f}, {delta_pose_stamped.position.y:.3f}, {delta_pose_stamped.position.z:.3f}], " f"frame_id={delta_pose_stamped.frame_id}" ) From 2a0098764051e279480b7fba194130b6a103a108 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 20:02:28 +0000 Subject: [PATCH 04/72] Fix: Added I/O types, lock for threading, X button press safety Former-commit-id: 8eba738e2a69cdc62ea80863ee52dafc025934f0 Former-commit-id: cda02365bbdf844dd4b367ab50f16f55baaa2de8 --- dimos/teleop/quest3/control/fastapi_server.py | 62 ++++++++++++------- 1 file changed, 39 insertions(+), 23 deletions(-) diff --git a/dimos/teleop/quest3/control/fastapi_server.py b/dimos/teleop/quest3/control/fastapi_server.py index 176d59277c..f836cfc91f 100644 --- a/dimos/teleop/quest3/control/fastapi_server.py +++ b/dimos/teleop/quest3/control/fastapi_server.py @@ -25,38 +25,37 @@ from __future__ import annotations import asyncio -import json -import os +import time from pathlib import Path import subprocess -from typing import TYPE_CHECKING, Optional +from typing import TYPE_CHECKING from fastapi import FastAPI, WebSocket, WebSocketDisconnect from fastapi.middleware.cors import CORSMiddleware from fastapi.responses import FileResponse -from fastapi.staticfiles import StaticFiles import uvicorn +from dimos.msgs.geometry_msgs import Pose from dimos.teleop.quest3.control.tracking_processor import TrackingProcessor from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: from collections.abc import Callable - import numpy as np - from numpy.typing import NDArray - logger = setup_logger() +# Rate limiting for command messages (in seconds) +COMMAND_DEBOUNCE_SECONDS = 2 + class ConnectionManager: """Manages active WebSocket connections for teleoperation.""" def __init__(self): self.active_connections: list[WebSocket] = [] - self.data_callback: Callable | None = None - self.command_callback: Callable | None = None - self.connection_count = 0 + self.active_connections_lock = asyncio.Lock() + self.data_callback: Callable[[Pose, Pose, float, float]] | None = None + self.command_callback: Callable[[str, WebSocket]] | None = None async def connect(self, websocket: WebSocket) -> None: """Accept and register a new WebSocket connection. @@ -65,8 +64,8 @@ async def connect(self, websocket: WebSocket) -> None: websocket: The WebSocket connection to register """ await websocket.accept() - self.active_connections.append(websocket) - self.connection_count += 1 + with self.active_connections_lock: + self.active_connections.append(websocket) logger.info(f"Client connected (total: {len(self.active_connections)})") def disconnect(self, websocket: WebSocket) -> None: @@ -75,9 +74,10 @@ def disconnect(self, websocket: WebSocket) -> None: Args: websocket: The WebSocket connection to remove """ - if websocket in self.active_connections: - self.active_connections.remove(websocket) - logger.info(f"Client disconnected (remaining: {len(self.active_connections)})") + with self.active_connections_lock: + if websocket in self.active_connections: + self.active_connections.remove(websocket) + logger.info(f"Client disconnected (remaining: {len(self.active_connections)})") def set_callback(self, callback: Callable) -> None: """Set callback function to send controller data to teleop module. @@ -104,12 +104,13 @@ async def broadcast(self, message: dict) -> None: message: Dictionary to send as JSON to all clients """ disconnected = [] - for connection in self.active_connections: - try: - await connection.send_json(message) - except Exception as e: - logger.error(f"Failed to broadcast to client: {e}") - disconnected.append(connection) + with self.active_connections_lock: + for connection in self.active_connections: + try: + await connection.send_json(message) + except Exception as e: + logger.error(f"Failed to broadcast to client: {e}") + disconnected.append(connection) # Clean up disconnected clients for conn in disconnected: @@ -121,7 +122,8 @@ def get_connection_count(self) -> int: Returns: Number of active WebSocket connections """ - return len(self.active_connections) + with self.active_connections_lock: + return len(self.active_connections) class TeleopFastAPIServer: @@ -131,6 +133,7 @@ def __init__( self, host: str = "0.0.0.0", port: int = 8443, # Changed default to 8443 for HTTPS + # why 8443 ? use_https: bool = True, ): """Initialize the FastAPI server. @@ -148,7 +151,7 @@ def __init__( self.server: uvicorn.Server | None = None # SSL certificate paths - self.cert_dir = Path(__file__).parent.parent / "certs" + self.cert_dir = Path(__file__).parent.parent / "certs" # any better way of writing this? self.cert_file = self.cert_dir / "cert.pem" self.key_file = self.cert_dir / "key.pem" @@ -266,6 +269,9 @@ async def websocket_endpoint(websocket: WebSocket): # Send acknowledgment await websocket.send_json({"type": "handshake_ack", "status": "connected"}) + # Track command timestamps for rate limiting + last_command_time: dict[int, float] = {} + # Main tracking loop while True: # Receive message @@ -275,6 +281,13 @@ async def websocket_endpoint(websocket: WebSocket): # Check if this is a command message (start_teleop/stop_teleop) message_type = message.get("type") if message_type in ("start_teleop", "stop_teleop"): + # Rate limit commands to prevent double-triggers + now = time.time() + if now - last_command_time.get(id(websocket), 0) < COMMAND_DEBOUNCE_SECONDS: + logger.debug(f"Rate limiting command: {message_type}") + continue + last_command_time[id(websocket)] = now + logger.info(f"Received command: {message_type}") # Route to command handler if self.manager.command_callback is not None: @@ -386,6 +399,9 @@ def run(self) -> None: uvicorn.run(**run_kwargs) async def stop_async(self) -> None: + # close all connections + await self.manager.disconnect() + """Stop the FastAPI server asynchronously.""" if self.server: logger.info("Stopping FastAPI server...") From 2abef27253c84ccd3503913e8657781c39f04550 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 20:02:37 +0000 Subject: [PATCH 05/72] Misc: Fix debug statements, Former-commit-id: f1097f838b2e0db960cc3855f8d30dd8723fc0f7 Former-commit-id: f146f111be19949bc4154213c7c3c8f2f2004cfa --- .../quest3/control/tracking_processor.py | 22 ++++++++----------- 1 file changed, 9 insertions(+), 13 deletions(-) diff --git a/dimos/teleop/quest3/control/tracking_processor.py b/dimos/teleop/quest3/control/tracking_processor.py index ca8cf0ade5..2e28e542dc 100644 --- a/dimos/teleop/quest3/control/tracking_processor.py +++ b/dimos/teleop/quest3/control/tracking_processor.py @@ -35,7 +35,6 @@ logger = setup_logger() # Coordinate frame transformation from VR to robot -# IMPORTANT: Adjust this matrix to match YOUR robot's coordinate system VR_TO_ROBOT_FRAME = np.array( [ [0, 0, -1, 0], # Robot X-axis = -VR Z-axis (flip forward/back) @@ -53,7 +52,7 @@ class TrackingProcessor: Handles: - VR to robot coordinate frame transformation - Controller orientation alignment - - Safety constraints (Z position clamping) + - Safety constraints (Z position clamping) (optional) """ def __init__(self) -> None: @@ -78,7 +77,7 @@ def process_tracking_message( Tuple of (left_pose, right_pose, left_gripper, right_gripper) or None if invalid """ tracking_type = event.get("type") - if tracking_type != "controller": + if tracking_type != "controller": # TODO: handle hand tracking type logger.debug("Ignoring non-controller tracking type: %s", tracking_type) return None @@ -90,7 +89,7 @@ def process_tracking_message( logger.warning("Invalid tracking data format for %s", side) continue - self._process_controller(tracking_data, side, tracking_type) + self._process_controller(tracking_data, side) # Get controller poses left_pose = self.left_wrist_pose.copy() @@ -99,18 +98,17 @@ def process_tracking_message( return left_pose, right_pose, self.left_gripper_value, self.right_gripper_value def _process_controller( - self, tracking_data: dict[str, Any], side: str, tracking_type: str + self, tracking_data: dict[str, Any], side: str ) -> None: """Process single controller's tracking data. Args: tracking_data: Controller data with 'targetLocation' and 'trigger' side: 'left' or 'right' - tracking_type: 'controller' or 'hand' """ # Process target location (pose) if "targetLocation" in tracking_data: - self._process_target_location(tracking_data["targetLocation"], side, tracking_type) + self._process_target_location(tracking_data["targetLocation"], side) # Process gripper (trigger) if side == "left": @@ -119,14 +117,13 @@ def _process_controller( self.right_gripper_value = self._extract_gripper_value(tracking_data) def _process_target_location( - self, target_location: list[float], side: str, tracking_type: str + self, target_location: list[float], side: str ) -> None: """Process controller pose from VR space to robot space. Args: target_location: Flat 16-element transformation matrix (column-major) side: 'left' or 'right' - tracking_type: 'controller' or 'hand' """ # Convert to 4x4 matrix target_matrix_flat = np.array(target_location, dtype=np.float32) @@ -138,10 +135,9 @@ def _process_target_location( target_matrix = target_matrix_flat.reshape(4, 4).T # Rotate controller 90ยฐ around Z-axis for gripper alignment - if tracking_type == "controller": - direction = -1 if side == "right" else 1 - rotation = Rotation.from_euler("z", 90 * direction, degrees=True) - target_matrix[:3, :3] = target_matrix[:3, :3] @ rotation.as_matrix() + direction = -1 if side == "right" else 1 + rotation = Rotation.from_euler("z", 90 * direction, degrees=True) + target_matrix[:3, :3] = target_matrix[:3, :3] @ rotation.as_matrix() # Apply VR to robot frame transformation wrist_mat = VR_TO_ROBOT_FRAME @ target_matrix From f6e28c34cf3ef341e6979acbc8804f09ab02edc3 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam <63036454+ruthwikdasyam@users.noreply.github.com> Date: Thu, 8 Jan 2026 20:03:36 +0000 Subject: [PATCH 06/72] CI code cleanup Former-commit-id: c347a33af439fe057461e6e00b5f54b3b0ed8230 Former-commit-id: 189e969dcc3a9aa7b3119eaa0db01691c446cadb --- dimos/teleop/quest3/control/fastapi_server.py | 7 ++++--- dimos/teleop/quest3/control/tracking_processor.py | 10 +++------- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/dimos/teleop/quest3/control/fastapi_server.py b/dimos/teleop/quest3/control/fastapi_server.py index f836cfc91f..d47c75f7dd 100644 --- a/dimos/teleop/quest3/control/fastapi_server.py +++ b/dimos/teleop/quest3/control/fastapi_server.py @@ -25,9 +25,9 @@ from __future__ import annotations import asyncio -import time from pathlib import Path import subprocess +import time from typing import TYPE_CHECKING from fastapi import FastAPI, WebSocket, WebSocketDisconnect @@ -35,13 +35,14 @@ from fastapi.responses import FileResponse import uvicorn -from dimos.msgs.geometry_msgs import Pose from dimos.teleop.quest3.control.tracking_processor import TrackingProcessor from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: from collections.abc import Callable + from dimos.msgs.geometry_msgs import Pose + logger = setup_logger() # Rate limiting for command messages (in seconds) @@ -151,7 +152,7 @@ def __init__( self.server: uvicorn.Server | None = None # SSL certificate paths - self.cert_dir = Path(__file__).parent.parent / "certs" # any better way of writing this? + self.cert_dir = Path(__file__).parent.parent / "certs" # any better way of writing this? self.cert_file = self.cert_dir / "cert.pem" self.key_file = self.cert_dir / "key.pem" diff --git a/dimos/teleop/quest3/control/tracking_processor.py b/dimos/teleop/quest3/control/tracking_processor.py index 2e28e542dc..b87de7e1ec 100644 --- a/dimos/teleop/quest3/control/tracking_processor.py +++ b/dimos/teleop/quest3/control/tracking_processor.py @@ -77,7 +77,7 @@ def process_tracking_message( Tuple of (left_pose, right_pose, left_gripper, right_gripper) or None if invalid """ tracking_type = event.get("type") - if tracking_type != "controller": # TODO: handle hand tracking type + if tracking_type != "controller": # TODO: handle hand tracking type logger.debug("Ignoring non-controller tracking type: %s", tracking_type) return None @@ -97,9 +97,7 @@ def process_tracking_message( return left_pose, right_pose, self.left_gripper_value, self.right_gripper_value - def _process_controller( - self, tracking_data: dict[str, Any], side: str - ) -> None: + def _process_controller(self, tracking_data: dict[str, Any], side: str) -> None: """Process single controller's tracking data. Args: @@ -116,9 +114,7 @@ def _process_controller( else: self.right_gripper_value = self._extract_gripper_value(tracking_data) - def _process_target_location( - self, target_location: list[float], side: str - ) -> None: + def _process_target_location(self, target_location: list[float], side: str) -> None: """Process controller pose from VR space to robot space. Args: From d7010ba6a5d649caa29144fb10375f5b0f89fe70 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 21:47:27 +0000 Subject: [PATCH 07/72] Feat: Added pose visualization with rerun Former-commit-id: 4203caa58941fee8c9be25718730f0a2c16a80b2 Former-commit-id: c6fb9454d817a9631d387ea9ab4ea75da09105c0 --- dimos/teleop/quest3/teleop_module.py | 98 ++++++++++++++++++++++++---- 1 file changed, 85 insertions(+), 13 deletions(-) diff --git a/dimos/teleop/quest3/teleop_module.py b/dimos/teleop/quest3/teleop_module.py index 7891b9b5a9..fa35411ee8 100644 --- a/dimos/teleop/quest3/teleop_module.py +++ b/dimos/teleop/quest3/teleop_module.py @@ -37,6 +37,7 @@ from dimos.core import Module, Out, rpc from dimos.core.module import ModuleConfig +from dimos.core.global_config import GlobalConfig from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Vector3 from dimos.msgs.std_msgs import Bool from dimos.teleop.quest3.control.fastapi_server import TeleopFastAPIServer @@ -50,6 +51,13 @@ logger = setup_logger() +try: + import rerun as rr + from dimos.dashboard.rerun_init import connect_rerun + RERUN_AVAILABLE = True +except ImportError: + RERUN_AVAILABLE = False + @dataclass class Quest3TeleopConfig(ModuleConfig): @@ -65,6 +73,9 @@ class Quest3TeleopConfig(ModuleConfig): enable_left_arm: bool = True enable_right_arm: bool = True + # Visualization settings + visualize_in_rerun: bool = True # Visualize VR controller poses in Rerun + # Safety limits safety_limits: bool = True max_: float = 0.5 # m/s @@ -107,9 +118,14 @@ class Quest3TeleopModule(Module): left_trigger: Out[Bool] = None # type: ignore[assignment] right_trigger: Out[Bool] = None # type: ignore[assignment] - def __init__(self, *args, **kwargs) -> None: + def __init__( + self, global_config: GlobalConfig | None = None, *args, **kwargs + ) -> None: super().__init__(*args, **kwargs) + # Get global config for Rerun connection + self._global_config = global_config or GlobalConfig() + # No RPC dependencies - data-driven activation self.rpc_calls = [] @@ -132,10 +148,7 @@ def __init__(self, *args, **kwargs) -> None: # Connection state [for future use] self._connected_clients = 0 - # Rate limiting: streaming frequency - self._last_stream_time: float = 0.0 - self._stream_frequency = 20 # Hz - stream controller data at 20Hz - self._stream_period = 1.0 / self._stream_frequency + # Note: Rate limiting is done client-side (VR headset), not server-side logger.info("Quest3TeleopModule initialized") @@ -149,6 +162,15 @@ def start(self) -> None: logger.info("Starting Quest3 Teleoperation Module...") super().start() + # Connect to Rerun for visualization if enabled + if ( + self.config.visualize_in_rerun + and RERUN_AVAILABLE + and self._global_config.viewer_backend.startswith("rerun") + ): + connect_rerun(global_config=self._global_config) + logger.info("Connected to Rerun for VR controller visualization") + # Start signaling server in background thread self._start_signaling_server() @@ -291,7 +313,6 @@ def calibrate_vr(self) -> dict[str, Any]: ) self._is_calibrated = True - self._last_stream_time = 0.0 # Reset to start streaming immediately logger.info("VR calibration complete. Now streaming delta poses...") return {"success": True, "message": "VR calibrated - move controllers to control robot"} @@ -363,7 +384,9 @@ async def _handle_x_button(self, command_type: str, websocket) -> dict: try: if command_type == "start_teleop": logger.info("X button pressed - calibrating VR...") + logger.info(f"Current state: left_pose={self._left_pose is not None}, right_pose={self._right_pose is not None}") result = self.calibrate_vr() + logger.info(f"Calibration result: {result}") if result.get("success"): return { @@ -413,6 +436,15 @@ def _on_tracking_data( left_gripper: Left gripper value (0.0-1.0) right_gripper: Right gripper value (0.0-1.0) """ + # Track how many tracking messages we've received + if not hasattr(self, "_tracking_msg_count"): + self._tracking_msg_count = 0 + self._tracking_msg_count += 1 + + # Log first few tracking messages to confirm data is arriving + if self._tracking_msg_count <= 3: + logger.info(f"Received tracking data #{self._tracking_msg_count}") + # Store absolute poses self._left_pose = left_pose self._right_pose = right_pose @@ -423,15 +455,12 @@ def _on_tracking_data( # Only stream deltas if VR is calibrated if not self._is_calibrated: - logger.warning("VR is not calibrated. Not streaming delta poses...") + # Only log this warning once every 100 messages to avoid spam + if self._tracking_msg_count <= 1 or self._tracking_msg_count % 100 == 0: + logger.warning("VR is not calibrated. Press X button to calibrate.") return - # Rate limit streaming - current_time = time.time() - if current_time - self._last_stream_time < self._stream_period: - return - - self._last_stream_time = current_time + # Stream delta poses (client already rate-limits, so no server-side limiting needed) self._stream_delta_poses(left_pose, right_pose) def _stream_delta_poses( @@ -467,6 +496,9 @@ def _stream_delta_poses( try: self.left_controller_delta.publish(delta_pose_stamped) + # Visualize in Rerun (absolute pose + delta) + self._visualize_controller_in_rerun(left_pose_obj, "left") + # Log periodically if self._publish_count <= 5 or self._publish_count % 100 == 0: logger.info( @@ -492,6 +524,9 @@ def _stream_delta_poses( try: self.right_controller_delta.publish(delta_pose_stamped) + # Visualize in Rerun (absolute pose + delta) + self._visualize_controller_in_rerun(right_pose_obj, "right") + if self._publish_count <= 5 or self._publish_count % 100 == 0: logger.info( f"Published right delta #{self._publish_count}: " @@ -517,6 +552,43 @@ def _stream_delta_poses( except Exception as e: logger.error(f"Error streaming delta poses: {e}") + def _visualize_controller_in_rerun( + self, controller_pose: Pose, arm_side: str + ) -> None: + """Visualize VR controller absolute pose and delta in Rerun. + + Args: + controller_pose: Absolute controller pose in robot space + arm_side: "left" or "right" + """ + if not ( + self.config.visualize_in_rerun + and RERUN_AVAILABLE + and self._global_config.viewer_backend.startswith("rerun") + ): + return + + try: + # Convert to PoseStamped for Rerun visualization + controller_pose_stamped = PoseStamped( + ts=time.time(), + frame_id=f"world/teleop/{arm_side}_controller", + position=controller_pose.position, + orientation=controller_pose.orientation, + ) + # Log absolute controller pose transform + rr.log( + f"world/teleop/{arm_side}_controller", + controller_pose_stamped.to_rerun(), + ) + # Log coordinate frame axes to visualize the transform + rr.log( + f"world/teleop/{arm_side}_controller", + rr.TransformAxes3D(length=0.15), # type: ignore[attr-defined] + ) + except Exception as e: + logger.debug(f"Failed to log {arm_side} controller to Rerun: {e}") + def _compute_delta( self, current: Pose, initial: Pose, timestamp: float, frame_id: str ) -> PoseStamped: From 0032c8748461750c2f556d108d1f154f9b99111c Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 21:47:36 +0000 Subject: [PATCH 08/72] Feat: Generalized it to work with all drivers Former-commit-id: 8451bd61b82fc097989eebcfc9314f22cda7f57d Former-commit-id: 866c9b84caf37c832eae84344f5a1ac70619ee99 --- dimos/teleop/teleop_arm_controller.py | 75 +++++++++++++++++---------- 1 file changed, 47 insertions(+), 28 deletions(-) diff --git a/dimos/teleop/teleop_arm_controller.py b/dimos/teleop/teleop_arm_controller.py index 0cc1872b99..7d8f70b5b4 100644 --- a/dimos/teleop/teleop_arm_controller.py +++ b/dimos/teleop/teleop_arm_controller.py @@ -48,6 +48,7 @@ class TeleopArmControllerConfig(ModuleConfig): # Driver settings driver_module_name: str = "RobotDriver" # Name of the driver module to get robot pose from + dummy_driver: bool = False # If True, skip RPC calls and use zeros for initial pose # Control settings control_frequency: float = 50.0 # Hz - control loop frequency @@ -96,7 +97,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: # Set RPC calls dynamically based on driver name driver_name = self.config.driver_module_name self.rpc_calls = [ - f"{driver_name}.get_cartesian_state", + f"{driver_name}.get_state", ] # Latest delta data @@ -248,23 +249,41 @@ def _calibrate_robot(self) -> bool: """Calibrate robot by getting its initial pose via RPC. Called automatically when first delta pose is received. + If dummy_driver=True, uses zeros instead of making RPC calls. Returns: True if calibration successful, False otherwise """ logger.info("Calibrating robot (getting initial pose)...") + # If dummy_driver is enabled, use zeros for initial pose + if self.config.dummy_driver: + logger.info("Dummy driver mode - using zeros for initial pose") + if self.config.enable_left_arm: + self._left_robot_initial_position = Vector3(0.0, 0.0, 0.0) + self._left_robot_initial_rpy = Vector3(0.0, 0.0, 0.0) + logger.info("Left arm initial pose: pos=[0, 0, 0], rpy=[0, 0, 0]") + + if self.config.enable_right_arm: + self._right_robot_initial_position = Vector3(0.0, 0.0, 0.0) + self._right_robot_initial_rpy = Vector3(0.0, 0.0, 0.0) + logger.info("Right arm initial pose: pos=[0, 0, 0], rpy=[0, 0, 0]") + + self._robot_calibrated = True + logger.info("Robot calibration complete (dummy mode) - control active!") + return True + try: driver_name = self.config.driver_module_name - rpc_method_name = f"{driver_name}.get_cartesian_state" + rpc_method_name = f"{driver_name}.get_state" - get_cartesian_state = self.get_rpc_calls(rpc_method_name) + get_state = self.get_rpc_calls(rpc_method_name) - if get_cartesian_state is None: + if get_state is None: logger.error("RPC callable is None - check blueprint wiring") return False - result = get_cartesian_state() + result = get_state() if result and result.get("success"): pose_data = result.get("pose", {}) @@ -454,7 +473,7 @@ def wrap_angle(angle): target_rpy = Vector3(target_roll, target_pitch, target_yaw) target_orientation = Quaternion.from_euler(target_rpy) - # Apply workspace limits if configured + # Apply workspace limits if configured # TODO: move to a method if self.config.workspace_limits: target_x = max( self.config.workspace_limits["x"][0], @@ -488,19 +507,19 @@ def wrap_angle(angle): current_rpy = prev_rpy + diff self._last_logged_rpy[arm_side] = current_rpy - print( - f"Target Pose: pos=[{target_pose.x:.3f}, {target_pose.y:.3f}, {target_pose.z:.3f}], " - f"rpy=[{current_rpy[0]:.3f}, {current_rpy[1]:.3f}, {current_rpy[2]:.3f}], " - f"quat=[{quat.x:.4f}, {quat.y:.4f}, {quat.z:.4f}, {quat.w:.4f}]", - flush=True, - ) + # print( + # f"Target Pose: pos=[{target_pose.x:.3f}, {target_pose.y:.3f}, {target_pose.z:.3f}], " + # f"rpy=[{current_rpy[0]:.3f}, {current_rpy[1]:.3f}, {current_rpy[2]:.3f}], " + # f"quat=[{quat.x:.4f}, {quat.y:.4f}, {quat.z:.4f}, {quat.w:.4f}]", + # flush=True, + # ) # Publish to robot - # if self.left_cartesian_command and hasattr(self.left_cartesian_command, "publish"): - # try: - # self.left_cartesian_command.publish(target_pose) - # except Exception as e: - # logger.error(f"Failed to publish left cartesian command: {e}") + if self.left_cartesian_command and hasattr(self.left_cartesian_command, "publish"): + try: + self.left_cartesian_command.publish(target_pose) + except Exception as e: + logger.error(f"Failed to publish left cartesian command: {e}") elif arm_side == "right": quat = target_pose.orientation @@ -513,19 +532,19 @@ def wrap_angle(angle): current_rpy = prev_rpy + diff self._last_logged_rpy[arm_side] = current_rpy - print( - f"Target Pose: pos=[{target_pose.x:.3f}, {target_pose.y:.3f}, {target_pose.z:.3f}], " - f"rpy=[{current_rpy[0]:.3f}, {current_rpy[1]:.3f}, {current_rpy[2]:.3f}], " - f"quat=[{quat.x:.4f}, {quat.y:.4f}, {quat.z:.4f}, {quat.w:.4f}]", - flush=True, - ) + # print( + # f"Target Pose: pos=[{target_pose.x:.3f}, {target_pose.y:.3f}, {target_pose.z:.3f}], " + # f"rpy=[{current_rpy[0]:.3f}, {current_rpy[1]:.3f}, {current_rpy[2]:.3f}], " + # f"quat=[{quat.x:.4f}, {quat.y:.4f}, {quat.z:.4f}, {quat.w:.4f}]", + # flush=True, + # ) # Publish to robot - # if self.right_cartesian_command and hasattr(self.right_cartesian_command, "publish"): - # try: - # self.right_cartesian_command.publish(target_pose) - # except Exception as e: - # logger.error(f"Failed to publish right cartesian command: {e}") + if self.right_cartesian_command and hasattr(self.right_cartesian_command, "publish"): + try: + self.right_cartesian_command.publish(target_pose) + except Exception as e: + logger.error(f"Failed to publish right cartesian command: {e}") except Exception as e: logger.error(f"Error applying {arm_side} delta: {e}", exc_info=True) From 0bf2e420ccdf148ab2018d52ec820bf64f8d423b Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 21:47:48 +0000 Subject: [PATCH 09/72] Misc: Added quest blueprint, and Modules Former-commit-id: fed1dfdaf032bb2e1b35a5a89219d1130925c011 Former-commit-id: 5171a03d874182c84f187c5b715dba363000a200 --- dimos/robot/all_blueprints.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index f989098f05..95cb9948ed 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -54,6 +54,8 @@ "demo-remapping": "dimos.robot.unitree_webrtc.demo_remapping:remapping", "demo-remapping-transport": "dimos.robot.unitree_webrtc.demo_remapping:remapping_and_transport", "demo-error-on-name-conflicts": "dimos.robot.unitree_webrtc.demo_error_on_name_conflicts:blueprint", + # Teleop blueprints + "quest3-teleop": "dimos.teleop.teleop_blueprints:quest3_teleop", } @@ -87,6 +89,9 @@ "xarm_driver": "dimos.hardware.manipulators.xarm.xarm_driver", "cartesian_motion_controller": "dimos.manipulation.control.servo_control.cartesian_motion_controller", "joint_trajectory_controller": "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller", + # Teleop modules + "quest3_teleop_module": "dimos.teleop.quest3.teleop_module", + "teleop_arm_controller": "dimos.teleop.teleop_arm_controller", } From 69286b03fc7675c384b4ef7279f81bfeb83106c4 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 21:47:54 +0000 Subject: [PATCH 10/72] Feat: Removed xarm dependency - generalized it over drivers Former-commit-id: 2b7b6c9bbe0f7f11b62b065cf19f78628be49ce2 Former-commit-id: 01836653ec6344304802510586bff1617014b2f9 --- dimos/teleop/teleop_blueprints.py | 76 ++++++++++--------------------- 1 file changed, 24 insertions(+), 52 deletions(-) diff --git a/dimos/teleop/teleop_blueprints.py b/dimos/teleop/teleop_blueprints.py index c63c21ba29..e5f80fcdc6 100644 --- a/dimos/teleop/teleop_blueprints.py +++ b/dimos/teleop/teleop_blueprints.py @@ -13,111 +13,87 @@ # limitations under the License. """ -Blueprints for Quest3 teleoperation with XArm. +Blueprints for Quest3 teleoperation. -This module provides declarative blueprints for combining Quest3TeleopModule, -TeleopArmController, and XArmDriver for VR teleoperation. +This module provides declarative blueprints for combining Quest3TeleopModule +and TeleopArmController for VR teleoperation with any robot arm. Architecture: Quest3TeleopModule (VR calibration, delta computation) โ†“ X button โ†’ calibrate VR โ†“ Computes: delta = current_controller - initial_controller - โ†“ Publishes: delta poses (Pose) + โ†“ Publishes: delta poses (PoseStamped) โ†“ TeleopArmController (Robot calibration, delta application) โ†“ First delta โ†’ auto-calibrate robot โ†“ Computes: target = initial_robot + delta - โ†“ Publishes: cartesian commands (Pose) + โ†“ Publishes: cartesian commands (PoseStamped) โ†“ - XArmDriver (Robot control) + Robot Driver (any manipulator driver) Usage: # Programmatically: - from dimos.teleop.teleop_blueprints import quest3_xarm6_teleop - coordinator = quest3_xarm6_teleop.build() + from dimos.teleop.teleop_blueprints import quest3_teleop + coordinator = quest3_teleop.build() coordinator.loop() - # Or build your own composition: + # Or build your own composition with a specific driver: from dimos.teleop.quest3.teleop_module import quest3_teleop_module from dimos.teleop.teleop_arm_controller import teleop_arm_controller - from dimos.hardware.manipulators.xarm.xarm_driver import xarm_driver from dimos.core.blueprints import autoconnect my_system = autoconnect( quest3_teleop_module( - driver_module_name="XArmDriver", signaling_port=8443, ), teleop_arm_controller( - driver_module_name="XArmDriver", + driver_module_name="MyRobotDriver", enable_left_arm=True, ), - xarm_driver( - ip="192.168.1.210", - dof=6, - connection_type="hardware", - ), + my_robot_driver(...), ) """ from dimos.core.blueprints import autoconnect from dimos.core.transport import LCMTransport -from dimos.hardware.manipulators.xarm.xarm_driver import XArmDriver, xarm_driver -from dimos.msgs.geometry_msgs import Pose, PoseStamped +from dimos.msgs.geometry_msgs import PoseStamped from dimos.msgs.std_msgs import Bool -from dimos.teleop.quest3.teleop_module import Quest3TeleopModule, quest3_teleop_module -from dimos.teleop.teleop_arm_controller import TeleopArmController, teleop_arm_controller +from dimos.teleop.quest3.teleop_module import quest3_teleop_module +from dimos.teleop.teleop_arm_controller import teleop_arm_controller # ============================================================================= -# Quest3 + XArm6 Teleoperation Blueprint +# Quest3 Teleoperation Blueprint # ============================================================================= # Combines: # - Quest3TeleopModule: VR calibration + delta computation # - TeleopArmController: Robot calibration + delta application -# - XArmDriver: Hardware/sim interface # # Data flow: # Quest3TeleopModule.left_controller_delta โ”€โ”€โ–บ TeleopArmController.left_controller_delta # Quest3TeleopModule.right_controller_delta โ”€โ”€โ–บ TeleopArmController.right_controller_delta # Quest3TeleopModule.left_trigger โ”€โ”€โ–บ TeleopArmController.left_trigger # Quest3TeleopModule.right_trigger โ”€โ”€โ–บ TeleopArmController.right_trigger -# TeleopArmController.left_cartesian_command โ”€โ”€โ–บ XArmDriver.cartesian_command -# TeleopArmController (RPC) โ”€โ”€โ–บ XArmDriver.get_cartesian_state (auto-calibration) +# TeleopArmController.left_cartesian_command โ”€โ”€โ–บ Robot Driver (via LCM) +# TeleopArmController.right_cartesian_command โ”€โ”€โ–บ Robot Driver (via LCM) # ============================================================================= -quest3_xarm6_teleop = ( +quest3_teleop = ( autoconnect( quest3_teleop_module( - driver_module_name="XArmDriver", signaling_host="0.0.0.0", signaling_port=8443, # HTTPS port (required for WebXR) use_https=True, # Enable HTTPS for Quest 3 WebXR enable_left_arm=True, - enable_right_arm=False, + enable_right_arm=True, ), teleop_arm_controller( - driver_module_name="XArmDriver", - control_frequency=20.0, # 20 Hz for consistent control + driver_module_name="DummyDriver", + dummy_driver=True, # Skip RPC calls, use zeros for initial pose + control_frequency=50.0, # Hz - control loop frequency enable_left_arm=True, - enable_right_arm=False, - ), - xarm_driver( - ip="192.168.1.210", - dof=6, - has_gripper=False, - has_force_torque=False, - has_cartesian_control=True, # Enable Cartesian control for teleop - control_rate=20, # 20 Hz to match teleop frequency - monitor_rate=10, - connection_type="hardware", # Change to "hardware" for real robot + enable_right_arm=True, ), ) - .remappings( - [ - # Map TeleopArmController's left_cartesian_command to XArmDriver's cartesian_command - (TeleopArmController, "left_cartesian_command", "cartesian_command"), - ] - ) .transports( { # Delta poses from Quest3TeleopModule to TeleopArmController @@ -130,15 +106,11 @@ # Trigger states ("left_trigger", Bool): LCMTransport("/quest3/left_trigger", Bool), ("right_trigger", Bool): LCMTransport("/quest3/right_trigger", Bool), - # Cartesian commands to robot - ("left_cartesian_command", Pose): LCMTransport("/xarm/cartesian_command", Pose), - ("right_cartesian_command", Pose): LCMTransport("/xarm/right_cartesian_command", Pose), - ("cartesian_command", Pose): LCMTransport("/xarm/cartesian_command", Pose), } ) ) __all__ = [ - "quest3_xarm6_teleop", + "quest3_teleop", ] From 54264915005b36ff703e7999205c944d5e55b1f5 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 21:47:59 +0000 Subject: [PATCH 11/72] Fix: threading Former-commit-id: b1ffc5e2d6685c6d27a2f1a81c54149f1fb47c32 Former-commit-id: ef0c1a9879ab990d2412d9f47444342aab987268 --- dimos/teleop/quest3/control/fastapi_server.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dimos/teleop/quest3/control/fastapi_server.py b/dimos/teleop/quest3/control/fastapi_server.py index d47c75f7dd..ecab7c9be3 100644 --- a/dimos/teleop/quest3/control/fastapi_server.py +++ b/dimos/teleop/quest3/control/fastapi_server.py @@ -25,6 +25,7 @@ from __future__ import annotations import asyncio +import threading from pathlib import Path import subprocess import time @@ -54,7 +55,7 @@ class ConnectionManager: def __init__(self): self.active_connections: list[WebSocket] = [] - self.active_connections_lock = asyncio.Lock() + self.active_connections_lock = threading.Lock() self.data_callback: Callable[[Pose, Pose, float, float]] | None = None self.command_callback: Callable[[str, WebSocket]] | None = None From 3e47fa266d2d4b04eab6e5e0df8f7aa423cbbef1 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 21:48:05 +0000 Subject: [PATCH 12/72] Feat: Rate control - 90Hz max Former-commit-id: cf5725256d25f4e52979395743c4ba4d9ced575c Former-commit-id: 577b386c1cb2b9d13d72e2d4cac0d85860e5af32 --- dimos/teleop/quest3/static/index.html | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/dimos/teleop/quest3/static/index.html b/dimos/teleop/quest3/static/index.html index faa70a3a57..9659e2fe23 100644 --- a/dimos/teleop/quest3/static/index.html +++ b/dimos/teleop/quest3/static/index.html @@ -116,7 +116,8 @@

Quest 3 VR Teleop

let xrRefSpace = null; let gl = null; let lastSendTime = 0; - const sendInterval = 1000 / 20; // 20Hz + const sendInterval = 0; // ~90Hz -- running at full speed + // const sendInterval = 1000 / 50; // ~50Hz target to achieve // Teleop state let teleopActive = false; @@ -256,11 +257,7 @@

Quest 3 VR Teleop

// Always check for button presses checkButtonPresses(frame); - // Only send tracking data if teleop is active - if (!teleopActive) { - return; - } - + // Rate limit tracking data const now = performance.now(); if (now - lastSendTime < sendInterval) { return; From 91354ac19164d63a1900f4fdc66229a958cc3ea5 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 22:12:02 +0000 Subject: [PATCH 13/72] Fix: changed file name Former-commit-id: e4e96eee86701211bb2be985c841073e42895540 Former-commit-id: b973af98108d6b076d1e893f8cdcd9f0df966241 --- ...ntroller.py => teleop_robot_controller.py} | 73 ++++++------------- 1 file changed, 21 insertions(+), 52 deletions(-) rename dimos/teleop/{teleop_arm_controller.py => teleop_robot_controller.py} (88%) diff --git a/dimos/teleop/teleop_arm_controller.py b/dimos/teleop/teleop_robot_controller.py similarity index 88% rename from dimos/teleop/teleop_arm_controller.py rename to dimos/teleop/teleop_robot_controller.py index 7d8f70b5b4..00b6491242 100644 --- a/dimos/teleop/teleop_arm_controller.py +++ b/dimos/teleop/teleop_robot_controller.py @@ -13,7 +13,7 @@ # limitations under the License. """ -Teleop Arm Controller +Teleop Robot Controller Receives DELTA poses from Quest3TeleopModule and applies them to the robot. Auto-calibrates robot on first delta received. @@ -43,8 +43,8 @@ @dataclass -class TeleopArmControllerConfig(ModuleConfig): - """Configuration for Teleop Arm Controller.""" +class TeleopRobotControllerConfig(ModuleConfig): + """Configuration for Teleop Robot Controller.""" # Driver settings driver_module_name: str = "RobotDriver" # Name of the driver module to get robot pose from @@ -55,12 +55,9 @@ class TeleopArmControllerConfig(ModuleConfig): enable_left_arm: bool = True enable_right_arm: bool = True - # Safety settings - workspace_limits: dict[str, tuple[float, float]] | None = None # Optional workspace limits - -class TeleopArmController(Module): - """Teleop Arm Controller - applies delta poses to robot. +class TeleopRobotController(Module): + """Teleop Robot Controller - applies delta poses to robot. This controller: 1. Receives DELTA poses (PoseStamped) from Quest3TeleopModule @@ -74,8 +71,8 @@ class TeleopArmController(Module): - cartesian_command input topic (Pose) """ - default_config = TeleopArmControllerConfig - config: TeleopArmControllerConfig + default_config = TeleopRobotControllerConfig + config: TeleopRobotControllerConfig # Input topics - receiving DELTA poses as PoseStamped left_controller_delta: In[PoseStamped] = None # type: ignore[assignment] @@ -91,7 +88,7 @@ class TeleopArmController(Module): rpc_calls: list[str] = [] def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize the teleop arm controller.""" + """Initialize the teleop robot controller.""" super().__init__(*args, **kwargs) # Set RPC calls dynamically based on driver name @@ -123,7 +120,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: # State lock self._state_lock = threading.Lock() - logger.info("TeleopArmController initialized - waiting for delta poses") + logger.info("TeleopRobotController initialized - waiting for delta poses") # ========================================================================= # Module Lifecycle @@ -131,8 +128,8 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: @rpc def start(self) -> None: - """Start the teleop arm controller.""" - logger.info("๐Ÿš€ Starting TeleopArmController...") + """Start the teleop robot controller.""" + logger.info("Starting TeleopRobotController...") super().start() # Subscribe to input topics (delta poses) @@ -155,16 +152,16 @@ def start(self) -> None: # Start control loop self._stop_event.clear() self._control_thread = threading.Thread( - target=self._control_loop, daemon=True, name="TeleopArmController" + target=self._control_loop, daemon=True, name="TeleopRobotController" ) self._control_thread.start() - logger.info("TeleopArmController started - waiting for delta poses to auto-calibrate") + logger.info("TeleopRobotController started - waiting for delta poses to auto-calibrate") @rpc def stop(self) -> None: - """Stop the teleop arm controller.""" - logger.info("Stopping TeleopArmController") + """Stop the teleop Robot controller.""" + logger.info("Stopping TeleopRobotController") # Stop control loop self._stop_event.set() @@ -172,7 +169,7 @@ def stop(self) -> None: self._control_thread.join(timeout=2.0) super().stop() - logger.info("TeleopArmController stopped") + logger.info("TeleopRobotController stopped") # ========================================================================= # Input Callbacks @@ -319,7 +316,7 @@ def _calibrate_robot(self) -> bool: ) self._robot_calibrated = True - logger.info("โœ… Robot calibration complete - control active!") + logger.info("Robot calibration complete - control active!") return True else: error_msg = f"Failed to get robot cartesian state: {result}" @@ -395,7 +392,7 @@ def _control_loop(self) -> None: if loop_count <= 10 or loop_count % 100 == 0: logger.debug( f"Control loop #{loop_count}: robot_calibrated={robot_calibrated}, " - f"left_delta={'โœ“' if left_delta else 'โœ—'}, right_delta={'โœ“' if right_delta else 'โœ—'}" + f"left_delta={left_delta is not None}, right_delta={right_delta is not None}" ) # Only process if robot is calibrated @@ -473,21 +470,6 @@ def wrap_angle(angle): target_rpy = Vector3(target_roll, target_pitch, target_yaw) target_orientation = Quaternion.from_euler(target_rpy) - # Apply workspace limits if configured # TODO: move to a method - if self.config.workspace_limits: - target_x = max( - self.config.workspace_limits["x"][0], - min(self.config.workspace_limits["x"][1], target_x), - ) - target_y = max( - self.config.workspace_limits["y"][0], - min(self.config.workspace_limits["y"][1], target_y), - ) - target_z = max( - self.config.workspace_limits["z"][0], - min(self.config.workspace_limits["z"][1], target_z), - ) - # Create target pose target_pose = Pose( position=Vector3(target_x, target_y, target_z), @@ -498,7 +480,7 @@ def wrap_angle(angle): if arm_side == "left": quat = target_pose.orientation - # Unwrap Euler angles for smooth logging + # Unwrap Euler angles for smooth logging (handles +pi and -pi discontinuities) current_rpy = np.array([target_pose.roll, target_pose.pitch, target_pose.yaw]) if arm_side in self._last_logged_rpy: prev_rpy = self._last_logged_rpy[arm_side] @@ -507,13 +489,6 @@ def wrap_angle(angle): current_rpy = prev_rpy + diff self._last_logged_rpy[arm_side] = current_rpy - # print( - # f"Target Pose: pos=[{target_pose.x:.3f}, {target_pose.y:.3f}, {target_pose.z:.3f}], " - # f"rpy=[{current_rpy[0]:.3f}, {current_rpy[1]:.3f}, {current_rpy[2]:.3f}], " - # f"quat=[{quat.x:.4f}, {quat.y:.4f}, {quat.z:.4f}, {quat.w:.4f}]", - # flush=True, - # ) - # Publish to robot if self.left_cartesian_command and hasattr(self.left_cartesian_command, "publish"): try: @@ -524,6 +499,7 @@ def wrap_angle(angle): elif arm_side == "right": quat = target_pose.orientation + # Unwrap Euler angles for smooth logging (handles +pi and -pi discontinuities) current_rpy = np.array([target_pose.roll, target_pose.pitch, target_pose.yaw]) if arm_side in self._last_logged_rpy: prev_rpy = self._last_logged_rpy[arm_side] @@ -532,13 +508,6 @@ def wrap_angle(angle): current_rpy = prev_rpy + diff self._last_logged_rpy[arm_side] = current_rpy - # print( - # f"Target Pose: pos=[{target_pose.x:.3f}, {target_pose.y:.3f}, {target_pose.z:.3f}], " - # f"rpy=[{current_rpy[0]:.3f}, {current_rpy[1]:.3f}, {current_rpy[2]:.3f}], " - # f"quat=[{quat.x:.4f}, {quat.y:.4f}, {quat.z:.4f}, {quat.w:.4f}]", - # flush=True, - # ) - # Publish to robot if self.right_cartesian_command and hasattr(self.right_cartesian_command, "publish"): try: @@ -551,4 +520,4 @@ def wrap_angle(angle): # Expose blueprint for declarative composition -teleop_arm_controller = TeleopArmController.blueprint +teleop_robot_controller = TeleopRobotController.blueprint From 57fd58b7acf145df9cdf57911941c8d4ff8c1db4 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 22:12:06 +0000 Subject: [PATCH 14/72] Updates with base_module addition Former-commit-id: 2d597b88d28d7f5df4ab9fe12fef4935aaac7694 Former-commit-id: c126af846526fc95f20d27e8d34623ec133e5dbf --- dimos/teleop/teleop_blueprints.py | 34 ++++++++++++++++--------------- 1 file changed, 18 insertions(+), 16 deletions(-) diff --git a/dimos/teleop/teleop_blueprints.py b/dimos/teleop/teleop_blueprints.py index e5f80fcdc6..a74a811d79 100644 --- a/dimos/teleop/teleop_blueprints.py +++ b/dimos/teleop/teleop_blueprints.py @@ -16,7 +16,7 @@ Blueprints for Quest3 teleoperation. This module provides declarative blueprints for combining Quest3TeleopModule -and TeleopArmController for VR teleoperation with any robot arm. +and TeleopRobotController for VR teleoperation with any robot arm. Architecture: Quest3TeleopModule (VR calibration, delta computation) @@ -24,7 +24,7 @@ โ†“ Computes: delta = current_controller - initial_controller โ†“ Publishes: delta poses (PoseStamped) โ†“ - TeleopArmController (Robot calibration, delta application) + TeleopRobotController (Robot calibration, delta application) โ†“ First delta โ†’ auto-calibrate robot โ†“ Computes: target = initial_robot + delta โ†“ Publishes: cartesian commands (PoseStamped) @@ -38,15 +38,15 @@ coordinator.loop() # Or build your own composition with a specific driver: - from dimos.teleop.quest3.teleop_module import quest3_teleop_module - from dimos.teleop.teleop_arm_controller import teleop_arm_controller + from dimos.teleop.quest3.quest3_teleop_module import quest3_teleop_module + from dimos.teleop.teleop_robot_controller import teleop_robot_controller from dimos.core.blueprints import autoconnect my_system = autoconnect( quest3_teleop_module( signaling_port=8443, ), - teleop_arm_controller( + teleop_robot_controller( driver_module_name="MyRobotDriver", enable_left_arm=True, ), @@ -58,23 +58,23 @@ from dimos.core.transport import LCMTransport from dimos.msgs.geometry_msgs import PoseStamped from dimos.msgs.std_msgs import Bool -from dimos.teleop.quest3.teleop_module import quest3_teleop_module -from dimos.teleop.teleop_arm_controller import teleop_arm_controller +from dimos.teleop.quest3.quest3_teleop_module import quest3_teleop_module +from dimos.teleop.teleop_robot_controller import teleop_robot_controller # ============================================================================= # Quest3 Teleoperation Blueprint # ============================================================================= # Combines: # - Quest3TeleopModule: VR calibration + delta computation -# - TeleopArmController: Robot calibration + delta application +# - TeleopRobotController: Robot calibration + delta application # # Data flow: -# Quest3TeleopModule.left_controller_delta โ”€โ”€โ–บ TeleopArmController.left_controller_delta -# Quest3TeleopModule.right_controller_delta โ”€โ”€โ–บ TeleopArmController.right_controller_delta -# Quest3TeleopModule.left_trigger โ”€โ”€โ–บ TeleopArmController.left_trigger -# Quest3TeleopModule.right_trigger โ”€โ”€โ–บ TeleopArmController.right_trigger -# TeleopArmController.left_cartesian_command โ”€โ”€โ–บ Robot Driver (via LCM) -# TeleopArmController.right_cartesian_command โ”€โ”€โ–บ Robot Driver (via LCM) +# Quest3TeleopModule.left_controller_delta โ”€โ”€โ–บ TeleopRobotController.left_controller_delta +# Quest3TeleopModule.right_controller_delta โ”€โ”€โ–บ TeleopRobotController.right_controller_delta +# Quest3TeleopModule.left_trigger โ”€โ”€โ–บ TeleopRobotController.left_trigger +# Quest3TeleopModule.right_trigger โ”€โ”€โ–บ TeleopRobotController.right_trigger +# TeleopRobotController.left_cartesian_command โ”€โ”€โ–บ Robot Driver (via LCM) +# TeleopRobotController.right_cartesian_command โ”€โ”€โ–บ Robot Driver (via LCM) # ============================================================================= quest3_teleop = ( @@ -85,8 +85,10 @@ use_https=True, # Enable HTTPS for Quest 3 WebXR enable_left_arm=True, enable_right_arm=True, + visualize_in_rerun=True, + safety_limits=True, ), - teleop_arm_controller( + teleop_robot_controller( driver_module_name="DummyDriver", dummy_driver=True, # Skip RPC calls, use zeros for initial pose control_frequency=50.0, # Hz - control loop frequency @@ -96,7 +98,7 @@ ) .transports( { - # Delta poses from Quest3TeleopModule to TeleopArmController + # Delta poses from Quest3TeleopModule to TeleopRobotController ("left_controller_delta", PoseStamped): LCMTransport( "/quest3/left_controller_delta", PoseStamped ), From 81aa3cabc31fe3fdd00abc281858def250f44977 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 22:12:15 +0000 Subject: [PATCH 15/72] Feat: Added base module: can be used to make device specific teleop modules Former-commit-id: fc35dab1becd11a051de7810a710b5664e6844a2 Former-commit-id: 84f00ce8d86ea4389d34c65a44896a42ec7948c0 --- dimos/teleop/base/base_teleop_module.py | 488 ++++++++++++++++++ dimos/teleop/quest3/teleop_module.py | 631 ------------------------ 2 files changed, 488 insertions(+), 631 deletions(-) create mode 100644 dimos/teleop/base/base_teleop_module.py delete mode 100644 dimos/teleop/quest3/teleop_module.py diff --git a/dimos/teleop/base/base_teleop_module.py b/dimos/teleop/base/base_teleop_module.py new file mode 100644 index 0000000000..1d89f668d7 --- /dev/null +++ b/dimos/teleop/base/base_teleop_module.py @@ -0,0 +1,488 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +Base Teleoperation Module + +Abstract base class for all teleoperation devices that provides: +- Dual-arm controller calibration (left/right) +- Delta pose computation (current - initial) +- LCM topic publishing (delta poses + trigger states) +- Rerun visualization +- Standard RPC interface + +Device-specific modules inherit from this and implement their connection logic. +""" + +from __future__ import annotations + +from abc import ABC +from dataclasses import dataclass, field +import time +from typing import TYPE_CHECKING, Any + +from dimos.core import Module, Out, rpc +from dimos.core.global_config import GlobalConfig +from dimos.core.module import ModuleConfig +from dimos.msgs.geometry_msgs import Pose, PoseStamped, Vector3 +from dimos.msgs.std_msgs import Bool +from dimos.utils.logging_config import setup_logger +from dimos.utils.transform_utils import matrix_to_pose + +if TYPE_CHECKING: + import numpy as np + from numpy.typing import NDArray + +logger = setup_logger() + +try: + import rerun as rr + + from dimos.dashboard.rerun_init import connect_rerun + + RERUN_AVAILABLE = True +except ImportError: + RERUN_AVAILABLE = False + + +@dataclass +class BaseTeleopConfig(ModuleConfig): + """Base configuration for teleoperation modules.""" + + # Control settings + position_scale: float = 1.0 # Scale factor for positions + enable_left_arm: bool = True + enable_right_arm: bool = True + + # Visualization settings + visualize_in_rerun: bool = True # Visualize controller poses in Rerun + + # Safety limits + safety_limits: bool = True + max_velocity: float = 0.5 # m/s + workspace_limits: dict[str, tuple[float, float]] = field( + default_factory=lambda: { + "x": (-1.0, 1.0), + "y": (-0.8, 0.8), + "z": (0.1, 2.0), + } + ) + + +class BaseTeleopModule(Module, ABC): + """Base class for teleoperation modules. + + Provides common functionality for dual-arm teleoperation: + - Calibration: capture initial controller poses + - Delta computation: current - initial + - Publishing: delta poses and trigger states to LCM + - Visualization: Rerun integration + - RPC interface: standard methods for all devices + + Subclasses implement device-specific connection logic and call + `update_controller_poses()` when new tracking data arrives. + + ## LCM Topics (Output): + - left_controller_delta: Out[PoseStamped] - Left controller delta pose + - right_controller_delta: Out[PoseStamped] - Right controller delta pose + - left_trigger: Out[Bool] - Left trigger button state + - right_trigger: Out[Bool] - Right trigger button state + + ## RPC Methods: + - calibrate_vr() -> dict: Calibrate by capturing initial poses + - reset_calibration() -> dict: Reset calibration + - is_vr_calibrated() -> bool: Check calibration status + - get_status() -> dict: Get teleoperation status + """ + + default_config = BaseTeleopConfig + + # LCM Output topics + left_controller_delta: Out[PoseStamped] = None # type: ignore[assignment] + right_controller_delta: Out[PoseStamped] = None # type: ignore[assignment] + left_trigger: Out[Bool] = None # type: ignore[assignment] + right_trigger: Out[Bool] = None # type: ignore[assignment] + + def __init__(self, global_config: GlobalConfig | None = None, *args, **kwargs) -> None: + super().__init__(*args, **kwargs) + + # Get global config for Rerun connection + self._global_config = global_config or GlobalConfig() + + # No RPC dependencies - data-driven activation + self.rpc_calls = [] + + # Calibration state + self._is_calibrated = False + self._left_controller_initial: Pose | None = None + self._right_controller_initial: Pose | None = None + + # Latest controller data (absolute poses) + self._left_pose: NDArray[np.float32] | None = None + self._right_pose: NDArray[np.float32] | None = None + self._left_trigger_pressed: bool = False + self._right_trigger_pressed: bool = False + + # Tracking counters + self._tracking_msg_count = 0 + self._publish_count = 0 + + # Connection state (for subclasses to update) + self._connected_clients = 0 + + logger.info(f"{self.__class__.__name__} base initialized") + + # ========================================================================= + # Module Lifecycle + # ========================================================================= + + @rpc + def start(self) -> None: + """Start the teleoperation module.""" + logger.info(f"Starting {self.__class__.__name__}...") + super().start() + + # Connect to Rerun for visualization if enabled + if ( + self.config.visualize_in_rerun + and RERUN_AVAILABLE + and self._global_config.viewer_backend.startswith("rerun") + ): + connect_rerun(global_config=self._global_config) + logger.info("Connected to Rerun for controller visualization") + + # ========================================================================= + # Calibration + # ========================================================================= + + @rpc + def calibrate_vr(self) -> dict[str, Any]: + """Calibrate by capturing initial controller poses. + + This is typically called when a calibration button is pressed. + Captures the current controller poses as the "zero" reference. + After calibration, delta poses are published. + + Returns: + Dict with 'success' and optional 'message' or 'error' + """ + logger.info("Calibrating controllers...") + + try: + # Check if we have controller data + if self._left_pose is None and self._right_pose is None: + return { + "success": False, + "error": "No controller data received yet. Move controllers and try again.", + } + + # Capture left controller initial pose + if self.config.enable_left_arm and self._left_pose is not None: + left_pose_obj = matrix_to_pose(self._left_pose) + + # Check if pose is valid (not all zeros) + pose_magnitude = ( + left_pose_obj.x**2 + left_pose_obj.y**2 + left_pose_obj.z**2 + ) ** 0.5 + if pose_magnitude < 0.001: + return { + "success": False, + "error": "Left controller pose is invalid (all zeros)", + } + + self._left_controller_initial = left_pose_obj + logger.info( + f"Captured left controller initial: " + f"pos=[{left_pose_obj.x:.3f}, {left_pose_obj.y:.3f}, {left_pose_obj.z:.3f}], " + f"rpy=[{left_pose_obj.roll:.3f}, {left_pose_obj.pitch:.3f}, {left_pose_obj.yaw:.3f}]" + ) + + # Capture right controller initial pose + if self.config.enable_right_arm and self._right_pose is not None: + right_pose_obj = matrix_to_pose(self._right_pose) + + # Check if pose is valid (not all zeros) + pose_magnitude = ( + right_pose_obj.x**2 + right_pose_obj.y**2 + right_pose_obj.z**2 + ) ** 0.5 + if pose_magnitude < 0.001: + return { + "success": False, + "error": "Right controller pose is invalid (all zeros)", + } + + self._right_controller_initial = right_pose_obj + logger.info( + f"Captured right controller initial: " + f"pos=[{right_pose_obj.x:.3f}, {right_pose_obj.y:.3f}, {right_pose_obj.z:.3f}], " + f"rpy=[{right_pose_obj.roll:.3f}, {right_pose_obj.pitch:.3f}, {right_pose_obj.yaw:.3f}]" + ) + + self._is_calibrated = True + + logger.info("Calibration complete. Now streaming delta poses...") + return {"success": True, "message": "Calibrated - move controllers to control robot"} + + except Exception as e: + logger.error(f"Calibration failed: {e}", exc_info=True) + return {"success": False, "error": str(e)} + + @rpc + def reset_calibration(self) -> dict[str, Any]: + """Reset calibration. Stops streaming until recalibrated. + + Returns: + Dict with 'success' and 'message' + """ + self._is_calibrated = False + self._left_controller_initial = None + self._right_controller_initial = None + + logger.info("Calibration reset. Recalibrate to resume teleoperation...") + return {"success": True, "message": "Calibration reset - recalibrate to resume"} + + @rpc + def is_vr_calibrated(self) -> bool: + """Check if calibrated. + + Returns: + True if calibrated and streaming deltas + """ + return self._is_calibrated + + @rpc + def get_status(self) -> dict: + """Get current teleoperation status. + + Returns: + Dictionary with status information + """ + return { + "is_calibrated": self._is_calibrated, + "connected_clients": self._connected_clients, + "left_arm_enabled": self.config.enable_left_arm, + "right_arm_enabled": self.config.enable_right_arm, + "has_left_data": self._left_pose is not None, + "has_right_data": self._right_pose is not None, + "left_trigger_pressed": self._left_trigger_pressed, + "right_trigger_pressed": self._right_trigger_pressed, + } + + # ========================================================================= + # Controller Data Processing (called by subclasses) + # ========================================================================= + + def update_controller_poses( + self, + left_pose: NDArray[np.float32], + right_pose: NDArray[np.float32], + left_gripper: float, + right_gripper: float, + ) -> None: + """Update controller poses and publish deltas if calibrated. + + This method should be called by subclasses when new tracking data arrives + from the device-specific connection. + + Args: + left_pose: 4x4 transformation matrix for left controller + right_pose: 4x4 transformation matrix for right controller + left_gripper: Left gripper value (0.0-1.0) + right_gripper: Right gripper value (0.0-1.0) + """ + # Track how many tracking messages we've received + self._tracking_msg_count += 1 + + # Log first few tracking messages to confirm data is arriving + if self._tracking_msg_count <= 3: + logger.info(f"Received tracking data #{self._tracking_msg_count}") + + # Store absolute poses + self._left_pose = left_pose + self._right_pose = right_pose + + # Convert gripper values to trigger booleans (threshold at 0.5) + self._left_trigger_pressed = left_gripper > 0.5 + self._right_trigger_pressed = right_gripper > 0.5 + + # Only stream deltas if calibrated + if not self._is_calibrated: + # Only log this warning once every 100 messages to avoid spam + if self._tracking_msg_count <= 1 or self._tracking_msg_count % 100 == 0: + logger.warning("Not calibrated. Calibrate to start teleoperation.") + return + + # Compute and publish deltas + self._compute_and_publish_deltas() + + def _compute_and_publish_deltas(self) -> None: + """Compute and publish delta poses (current - initial).""" + # Track publish count for logging + self._publish_count += 1 + + try: + current_time = time.time() + + # Left controller delta + if self.config.enable_left_arm and self._left_controller_initial is not None: + if self.left_controller_delta and hasattr(self.left_controller_delta, "publish"): + if self._left_pose is not None: + left_pose_obj = matrix_to_pose(self._left_pose) + delta_pose_stamped = self._compute_delta( + left_pose_obj, + self._left_controller_initial, + current_time, + f"{self.__class__.__name__.lower()}_left_controller_delta", + ) + + try: + self.left_controller_delta.publish(delta_pose_stamped) + + # Visualize in Rerun (absolute pose) + self._visualize_controller_in_rerun(left_pose_obj, "left") + + # Log periodically + if self._publish_count <= 5 or self._publish_count % 100 == 0: + logger.info( + f"Published left delta #{self._publish_count}: " + f"pos=[{delta_pose_stamped.position.x:.3f}, {delta_pose_stamped.position.y:.3f}, {delta_pose_stamped.position.z:.3f}], " + f"rpy=[{delta_pose_stamped.roll:.3f}, {delta_pose_stamped.pitch:.3f}, {delta_pose_stamped.yaw:.3f}], " + f"frame_id={delta_pose_stamped.frame_id}" + ) + except Exception as e: + logger.error(f"Failed to publish left delta: {e}") + + # Right controller delta + if self.config.enable_right_arm and self._right_controller_initial is not None: + if self.right_controller_delta and hasattr(self.right_controller_delta, "publish"): + if self._right_pose is not None: + right_pose_obj = matrix_to_pose(self._right_pose) + delta_pose_stamped = self._compute_delta( + right_pose_obj, + self._right_controller_initial, + current_time, + f"{self.__class__.__name__.lower()}_right_controller_delta", + ) + + try: + self.right_controller_delta.publish(delta_pose_stamped) + + # Visualize in Rerun (absolute pose) + self._visualize_controller_in_rerun(right_pose_obj, "right") + + if self._publish_count <= 5 or self._publish_count % 100 == 0: + logger.info( + f"Published right delta #{self._publish_count}: " + f"pos=[{delta_pose_stamped.position.x:.3f}, {delta_pose_stamped.position.y:.3f}, {delta_pose_stamped.position.z:.3f}], " + f"frame_id={delta_pose_stamped.frame_id}" + ) + except Exception as e: + logger.error(f"Failed to publish right delta: {e}") + + # Publish trigger states + if self.left_trigger and hasattr(self.left_trigger, "publish"): + try: + self.left_trigger.publish(Bool(data=self._left_trigger_pressed)) + except Exception as e: + logger.debug(f"Failed to publish left trigger: {e}") + + if self.right_trigger and hasattr(self.right_trigger, "publish"): + try: + self.right_trigger.publish(Bool(data=self._right_trigger_pressed)) + except Exception as e: + logger.debug(f"Failed to publish right trigger: {e}") + + except Exception as e: + logger.error(f"Error computing/publishing delta poses: {e}") + + def _compute_delta( + self, current: Pose, initial: Pose, timestamp: float, frame_id: str + ) -> PoseStamped: + """Compute delta pose: current - initial. + + For position: simple subtraction + For orientation: delta_quat = current * inverse(initial) + + Args: + current: Current controller pose + initial: Initial controller pose (reference) + timestamp: Timestamp for the delta pose + frame_id: Frame ID for the delta pose + + Returns: + Delta pose as PoseStamped (position delta + orientation delta) + """ + # Position delta + delta_x = current.x - initial.x + delta_y = current.y - initial.y + delta_z = current.z - initial.z + + # Orientation delta: delta_quat = current * inverse(initial) + delta_quat = current.orientation * initial.orientation.inverse() + + delta_pose = Pose( + position=Vector3(delta_x, delta_y, delta_z), + orientation=delta_quat, + ) + + return PoseStamped( + ts=timestamp, + frame_id=frame_id, + position=delta_pose.position, + orientation=delta_pose.orientation, + ) + + # ========================================================================= + # Rerun Visualization + # ========================================================================= + + def _visualize_controller_in_rerun(self, controller_pose: Pose, arm_side: str) -> None: + """Visualize controller absolute pose in Rerun. + + Args: + controller_pose: Absolute controller pose in robot space + arm_side: "left" or "right" + """ + if not ( + self.config.visualize_in_rerun + and RERUN_AVAILABLE + and self._global_config.viewer_backend.startswith("rerun") + ): + return + + try: + # Convert to PoseStamped for Rerun visualization + controller_pose_stamped = PoseStamped( + ts=time.time(), + frame_id=f"world/teleop/{arm_side}_controller", + position=controller_pose.position, + orientation=controller_pose.orientation, + ) + # Log absolute controller pose transform + rr.log( + f"world/teleop/{arm_side}_controller", + controller_pose_stamped.to_rerun(), + ) + # Log coordinate frame axes to visualize the transform + rr.log( + f"world/teleop/{arm_side}_controller", + rr.Arrows3D( # type: ignore[attr-defined] + vectors=[[0.15, 0, 0], [0, 0.15, 0], [0, 0, 0.15]], + origins=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], + colors=[[255, 0, 0], [0, 255, 0], [0, 0, 255]], + ), + ) + except Exception as e: + logger.debug(f"Failed to log {arm_side} controller to Rerun: {e}") diff --git a/dimos/teleop/quest3/teleop_module.py b/dimos/teleop/quest3/teleop_module.py deleted file mode 100644 index fa35411ee8..0000000000 --- a/dimos/teleop/quest3/teleop_module.py +++ /dev/null @@ -1,631 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -""" -Quest3 Teleoperation Module - -A dimos Module that runs the WebSocket signaling server for Quest3 VR teleoperation, -receives controller tracking data, calibrates VR poses, and streams DELTA poses to -TeleopArmController. - -Architecture: -- X button pressed โ†’ calibrate VR (capture initial controller poses) -- Computes: delta = current_controller - initial_controller -- Publishes: delta poses (Pose) to TeleopArmController -- TeleopArmController auto-calibrates robot on first delta received -""" - -from __future__ import annotations - -import asyncio -from dataclasses import dataclass, field -import threading -import time -from typing import TYPE_CHECKING, Any - -from dimos.core import Module, Out, rpc -from dimos.core.module import ModuleConfig -from dimos.core.global_config import GlobalConfig -from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Vector3 -from dimos.msgs.std_msgs import Bool -from dimos.teleop.quest3.control.fastapi_server import TeleopFastAPIServer -from dimos.utils.logging_config import setup_logger -from dimos.utils.transform_utils import matrix_to_pose - -if TYPE_CHECKING: - import numpy as np - from numpy.typing import NDArray - from websockets.server import WebSocketServerProtocol - -logger = setup_logger() - -try: - import rerun as rr - from dimos.dashboard.rerun_init import connect_rerun - RERUN_AVAILABLE = True -except ImportError: - RERUN_AVAILABLE = False - - -@dataclass -class Quest3TeleopConfig(ModuleConfig): - """Configuration for Quest3 Teleoperation Module.""" - - # WebSocket server settings - signaling_host: str = "0.0.0.0" - signaling_port: int = 8443 # HTTPS port (was 8013 for old setup) - use_https: bool = True # Enable HTTPS for WebXR (required by Quest 3) - - # Control settings - position_scale: float = 1.0 # Scale factor for positions - enable_left_arm: bool = True - enable_right_arm: bool = True - - # Visualization settings - visualize_in_rerun: bool = True # Visualize VR controller poses in Rerun - - # Safety limits - safety_limits: bool = True - max_: float = 0.5 # m/s - workspace_limits: dict[str, tuple[float, float]] = field( - default_factory=lambda: { - "x": (0.1, 1), - "y": (-0.8, 0.8), - "z": (0.1, 0.7), - } - ) - - -class Quest3TeleopModule(Module): - """Quest3 VR Teleoperation Module. - - This module: - 1. Runs a WebSocket signaling server for VR connections - 2. Receives controller tracking data from Quest3 - 3. Calibrates VR poses when X button is pressed - 4. Computes and streams DELTA poses (current - initial) out - - ## LCM Topics: - left_controller_delta: Out[PoseStamped] Left controller delta pose (position + orientation delta) = current - initial - right_controller_delta: Out[PoseStamped] Right controller delta pose (position + orientation delta) = current - initial - left_trigger: Out[Bool] Left trigger button state - right_trigger: Out[Bool] Right trigger button state - - ## RPC Methods: - - start() -> None: Start the module and signaling server - - stop() -> None: Stop the module and signaling server - - calibrate_vr() -> dict: Calibrate VR (capture initial controller poses) - - _is_calibrated() -> bool: Check if VR is calibrated - - get_status() -> dict: Get current teleoperation status - """ - - default_config = Quest3TeleopConfig - - left_controller_delta: Out[PoseStamped] = None # type: ignore[assignment] - right_controller_delta: Out[PoseStamped] = None # type: ignore[assignment] - left_trigger: Out[Bool] = None # type: ignore[assignment] - right_trigger: Out[Bool] = None # type: ignore[assignment] - - def __init__( - self, global_config: GlobalConfig | None = None, *args, **kwargs - ) -> None: - super().__init__(*args, **kwargs) - - # Get global config for Rerun connection - self._global_config = global_config or GlobalConfig() - - # No RPC dependencies - data-driven activation - self.rpc_calls = [] - - # FastAPI WebSocket server - self._fastapi_server: TeleopFastAPIServer | None = None - self._server_thread: threading.Thread | None = None - self._event_loop: asyncio.AbstractEventLoop | None = None - - # VR Calibration state - self._is_calibrated = False - self._left_controller_initial: Pose | None = None - self._right_controller_initial: Pose | None = None - - # Latest controller data (absolute poses from VR) - self._left_pose: NDArray[np.float32] | None = None - self._right_pose: NDArray[np.float32] | None = None - self._left_trigger_pressed: bool = False - self._right_trigger_pressed: bool = False - - # Connection state [for future use] - self._connected_clients = 0 - - # Note: Rate limiting is done client-side (VR headset), not server-side - - logger.info("Quest3TeleopModule initialized") - - # ========================================================================= - # Module Lifecycle - # ========================================================================= - - @rpc - def start(self) -> None: - """Start the Quest3 teleoperation module and signaling server.""" - logger.info("Starting Quest3 Teleoperation Module...") - super().start() - - # Connect to Rerun for visualization if enabled - if ( - self.config.visualize_in_rerun - and RERUN_AVAILABLE - and self._global_config.viewer_backend.startswith("rerun") - ): - connect_rerun(global_config=self._global_config) - logger.info("Connected to Rerun for VR controller visualization") - - # Start signaling server in background thread - self._start_signaling_server() - - protocol = "https" if self.config.use_https else "http" - logger.info( - f"Quest3 Teleoperation Module started on {protocol}://{self.config.signaling_host}:{self.config.signaling_port}" - ) - logger.info( - f"Open this URL on Quest 3: {protocol}://:{self.config.signaling_port}/" - ) - logger.info("Press X button in VR to calibrate and start teleoperation") - - @rpc - def stop(self) -> None: - """Stop the Quest3 teleoperation module and signaling server.""" - logger.info("Stopping Quest3 Teleoperation Module...") - - # Stop signaling server - self._stop_signaling_server() - - super().stop() - - def _start_signaling_server(self) -> None: - """Start the FastAPI WebSocket server in a background thread.""" - - def run_server(): - # Create new event loop for this thread - loop = asyncio.new_event_loop() - asyncio.set_event_loop(loop) - self._event_loop = loop - - # Create FastAPI server instance - self._fastapi_server = TeleopFastAPIServer( - host=self.config.signaling_host, - port=self.config.signaling_port, - use_https=self.config.use_https, - ) - - # Register callbacks - self._fastapi_server.set_callback(self._on_tracking_data) - self._fastapi_server.set_command_callback(self._handle_x_button) - logger.info("FastAPI server initialized with callbacks") - - try: - # Run FastAPI server - loop.run_until_complete(self._fastapi_server.start_async()) - except Exception as e: - logger.error(f"FastAPI server error: {e}", exc_info=True) - finally: - loop.close() - - # Start server thread - self._server_thread = threading.Thread(target=run_server, daemon=True, name="FastAPIServer") - self._server_thread.start() - logger.info("FastAPI server thread started") - - def _stop_signaling_server(self) -> None: - """Stop the FastAPI WebSocket server.""" - if self._fastapi_server and self._event_loop: - # Schedule stop on the event loop - asyncio.run_coroutine_threadsafe(self._fastapi_server.stop_async(), self._event_loop) - - if self._event_loop and self._event_loop.is_running(): - # Stop the event loop - self._event_loop.call_soon_threadsafe(self._event_loop.stop) - - # Wait for thread to finish - if self._server_thread and self._server_thread.is_alive(): - self._server_thread.join(timeout=2.0) - - logger.info("FastAPI server stopped") - - # ========================================================================= - # VR Calibration - # ========================================================================= - - @rpc - def calibrate_vr(self) -> dict[str, Any]: - """Calibrate VR by capturing initial controller poses. - - This is called when X button is pressed in VR. - Captures the current controller poses as the "zero" reference. - After calibration, delta poses are published. - - Returns: - Dict with 'success' and optional 'message' or 'error' - """ - logger.info("Calibrating VR controllers...") - - try: - # Check if we have controller data - if self._left_pose is None and self._right_pose is None: - return { - "success": False, - "error": "No controller data received yet. Move controllers and try again.", - } - - # Capture left controller initial pose - if self.config.enable_left_arm and self._left_pose is not None: - left_pose_obj = matrix_to_pose(self._left_pose) - - # Check if pose is valid (not all zeros) - # TODO [Can remove if the first pose received after pressing X button is valid] - pose_magnitude = ( - left_pose_obj.x**2 + left_pose_obj.y**2 + left_pose_obj.z**2 - ) ** 0.5 - if pose_magnitude < 0.001: - return { - "success": False, - "error": "Left controller pose is invalid (all zeros)", - } - - self._left_controller_initial = left_pose_obj - logger.info( - f"Captured left controller initial: " - f"pos=[{left_pose_obj.x:.3f}, {left_pose_obj.y:.3f}, {left_pose_obj.z:.3f}], " - f"rpy=[{left_pose_obj.roll:.3f}, {left_pose_obj.pitch:.3f}, {left_pose_obj.yaw:.3f}]" - ) - - # Capture right controller initial pose - if self.config.enable_right_arm and self._right_pose is not None: - right_pose_obj = matrix_to_pose(self._right_pose) - - # Check if pose is valid (not all zeros) - # TODO [Can remove if the first pose received after pressing X button is valid] - pose_magnitude = ( - right_pose_obj.x**2 + right_pose_obj.y**2 + right_pose_obj.z**2 - ) ** 0.5 - if pose_magnitude < 0.001: - return { - "success": False, - "error": "Right controller pose is invalid (all zeros)", - } - - self._right_controller_initial = right_pose_obj - logger.info( - f"โœ… Captured right controller initial: " - f"pos=[{right_pose_obj.x:.3f}, {right_pose_obj.y:.3f}, {right_pose_obj.z:.3f}], " - f"rpy=[{right_pose_obj.roll:.3f}, {right_pose_obj.pitch:.3f}, {right_pose_obj.yaw:.3f}]" - ) - - self._is_calibrated = True - - logger.info("VR calibration complete. Now streaming delta poses...") - return {"success": True, "message": "VR calibrated - move controllers to control robot"} - - except Exception as e: - logger.error(f"VR calibration failed: {e}", exc_info=True) - return {"success": False, "error": str(e)} - - @rpc - def reset_calibration(self) -> dict[str, Any]: - """Reset VR calibration. Stops streaming until recalibrated. - - Returns: - Dict with 'success' and 'message' - """ - self._is_calibrated = False - self._left_controller_initial = None - self._right_controller_initial = None - - logger.info("VR calibration reset. Press X to recalibrate...") - return {"success": True, "message": "Calibration reset - press X to recalibrate"} - - @rpc - def is_vr_calibrated(self) -> bool: - """Check if VR is calibrated. - - Returns: - True if VR is calibrated and streaming deltas - """ - return self._is_calibrated - - @rpc - def get_status(self) -> dict: - """Get current teleoperation status. - - Returns: - Dictionary with status information - """ - return { - "is_calibrated": self._is_calibrated, - "connected_clients": self._connected_clients, - "server_running": self._server_thread is not None and self._server_thread.is_alive(), - "left_arm_enabled": self.config.enable_left_arm, - "right_arm_enabled": self.config.enable_right_arm, - "has_left_data": self._left_pose is not None, - "has_right_data": self._right_pose is not None, - "left_trigger_pressed": self._left_trigger_pressed, - "right_trigger_pressed": self._right_trigger_pressed, - } - - # ========================================================================= - # X Button Handler - # ========================================================================= - - async def _handle_x_button(self, command_type: str, websocket) -> dict: - """Handle X button press from VR client. - - X button toggles calibration: - - If not calibrated: calibrate VR - - If calibrated: reset calibration (stop streaming) - - Args: - command_type: 'start_teleop' or 'stop_teleop' - websocket: WebSocket connection to send responses - - Returns: - Response dictionary to send back to client - """ - try: - if command_type == "start_teleop": - logger.info("X button pressed - calibrating VR...") - logger.info(f"Current state: left_pose={self._left_pose is not None}, right_pose={self._right_pose is not None}") - result = self.calibrate_vr() - logger.info(f"Calibration result: {result}") - - if result.get("success"): - return { - "type": "teleop_started", - "message": result.get("message", "VR calibrated"), - } - else: - return { - "type": "calibration_failed", - "error": result.get("error", "Calibration failed"), - } - - elif command_type == "stop_teleop": - logger.info("X button pressed - stopping teleop...") - result = self.reset_calibration() - - return { - "type": "teleop_stopped", - "message": result.get("message", "Teleop stopped"), - } - - except Exception as e: - logger.error(f"Error handling X button: {e}", exc_info=True) - return {"type": "error", "error": f"Command failed: {e!s}"} - - return {"type": "error", "error": f"Unknown command: {command_type}"} - - # ========================================================================= - # Controller Data Processing - # ========================================================================= - - def _on_tracking_data( - self, - left_pose: NDArray[np.float32], - right_pose: NDArray[np.float32], - left_gripper: float, - right_gripper: float, - ) -> None: - """Receive tracking data from VR. - - Called by the signaling server when new tracking data arrives. - Stores absolute poses and computes/streams deltas if calibrated. - - Args: - left_pose: 4x4 transformation matrix for left controller - right_pose: 4x4 transformation matrix for right controller - left_gripper: Left gripper value (0.0-1.0) - right_gripper: Right gripper value (0.0-1.0) - """ - # Track how many tracking messages we've received - if not hasattr(self, "_tracking_msg_count"): - self._tracking_msg_count = 0 - self._tracking_msg_count += 1 - - # Log first few tracking messages to confirm data is arriving - if self._tracking_msg_count <= 3: - logger.info(f"Received tracking data #{self._tracking_msg_count}") - - # Store absolute poses - self._left_pose = left_pose - self._right_pose = right_pose - - # Convert gripper values to trigger booleans (threshold at 0.5) - self._left_trigger_pressed = left_gripper > 0.5 - self._right_trigger_pressed = right_gripper > 0.5 - - # Only stream deltas if VR is calibrated - if not self._is_calibrated: - # Only log this warning once every 100 messages to avoid spam - if self._tracking_msg_count <= 1 or self._tracking_msg_count % 100 == 0: - logger.warning("VR is not calibrated. Press X button to calibrate.") - return - - # Stream delta poses (client already rate-limits, so no server-side limiting needed) - self._stream_delta_poses(left_pose, right_pose) - - def _stream_delta_poses( - self, - left_pose: NDArray[np.float32], - right_pose: NDArray[np.float32], - ) -> None: - """Compute and stream delta poses (current - initial). - - Args: - left_pose: 4x4 transformation matrix for left controller (absolute) - right_pose: 4x4 transformation matrix for right controller (absolute) - """ - # Track publish count for logging - if not hasattr(self, "_publish_count"): - self._publish_count = 0 - self._publish_count += 1 - - try: - current_time = time.time() - - # Left controller delta - if self.config.enable_left_arm and self._left_controller_initial is not None: - if self.left_controller_delta and hasattr(self.left_controller_delta, "publish"): - left_pose_obj = matrix_to_pose(left_pose) - delta_pose_stamped = self._compute_delta( - left_pose_obj, - self._left_controller_initial, - current_time, - "quest3_left_controller_delta", - ) - - try: - self.left_controller_delta.publish(delta_pose_stamped) - - # Visualize in Rerun (absolute pose + delta) - self._visualize_controller_in_rerun(left_pose_obj, "left") - - # Log periodically - if self._publish_count <= 5 or self._publish_count % 100 == 0: - logger.info( - f"Published left delta #{self._publish_count}: " - f"pos=[{delta_pose_stamped.position.x:.3f}, {delta_pose_stamped.position.y:.3f}, {delta_pose_stamped.position.z:.3f}], " - f"rpy=[{delta_pose_stamped.roll:.3f}, {delta_pose_stamped.pitch:.3f}, {delta_pose_stamped.yaw:.3f}], " - f"frame_id={delta_pose_stamped.frame_id}" - ) - except Exception as e: - logger.error(f"Failed to publish left delta: {e}") - - # Right controller delta - if self.config.enable_right_arm and self._right_controller_initial is not None: - if self.right_controller_delta and hasattr(self.right_controller_delta, "publish"): - right_pose_obj = matrix_to_pose(right_pose) - delta_pose_stamped = self._compute_delta( - right_pose_obj, - self._right_controller_initial, - current_time, - "quest3_right_controller_delta", - ) - - try: - self.right_controller_delta.publish(delta_pose_stamped) - - # Visualize in Rerun (absolute pose + delta) - self._visualize_controller_in_rerun(right_pose_obj, "right") - - if self._publish_count <= 5 or self._publish_count % 100 == 0: - logger.info( - f"Published right delta #{self._publish_count}: " - f"pos=[{delta_pose_stamped.position.x:.3f}, {delta_pose_stamped.position.y:.3f}, {delta_pose_stamped.position.z:.3f}], " - f"frame_id={delta_pose_stamped.frame_id}" - ) - except Exception as e: - logger.error(f"Failed to publish right delta: {e}") - - # Publish trigger states - if self.left_trigger and hasattr(self.left_trigger, "publish"): - try: - self.left_trigger.publish(Bool(data=self._left_trigger_pressed)) - except Exception as e: - logger.debug(f"Failed to publish left trigger: {e}") - - if self.right_trigger and hasattr(self.right_trigger, "publish"): - try: - self.right_trigger.publish(Bool(data=self._right_trigger_pressed)) - except Exception as e: - logger.debug(f"Failed to publish right trigger: {e}") - - except Exception as e: - logger.error(f"Error streaming delta poses: {e}") - - def _visualize_controller_in_rerun( - self, controller_pose: Pose, arm_side: str - ) -> None: - """Visualize VR controller absolute pose and delta in Rerun. - - Args: - controller_pose: Absolute controller pose in robot space - arm_side: "left" or "right" - """ - if not ( - self.config.visualize_in_rerun - and RERUN_AVAILABLE - and self._global_config.viewer_backend.startswith("rerun") - ): - return - - try: - # Convert to PoseStamped for Rerun visualization - controller_pose_stamped = PoseStamped( - ts=time.time(), - frame_id=f"world/teleop/{arm_side}_controller", - position=controller_pose.position, - orientation=controller_pose.orientation, - ) - # Log absolute controller pose transform - rr.log( - f"world/teleop/{arm_side}_controller", - controller_pose_stamped.to_rerun(), - ) - # Log coordinate frame axes to visualize the transform - rr.log( - f"world/teleop/{arm_side}_controller", - rr.TransformAxes3D(length=0.15), # type: ignore[attr-defined] - ) - except Exception as e: - logger.debug(f"Failed to log {arm_side} controller to Rerun: {e}") - - def _compute_delta( - self, current: Pose, initial: Pose, timestamp: float, frame_id: str - ) -> PoseStamped: - """Compute delta pose: current - initial. - - For position: simple subtraction - For orientation: delta_quat = current * inverse(initial) - - Args: - current: Current controller pose - initial: Initial controller pose (reference) - timestamp: Timestamp for the delta pose - frame_id: Frame ID for the delta pose - - Returns: - Delta pose as PoseStamped (position delta + orientation delta) - """ - # Position delta - delta_x = current.x - initial.x - delta_y = current.y - initial.y - delta_z = current.z - initial.z - - # Orientation delta: delta_quat = current * inverse(initial) - delta_quat = current.orientation * initial.orientation.inverse() - - delta_pose = Pose( - position=Vector3(delta_x, delta_y, delta_z), - orientation=delta_quat, - ) - - return PoseStamped( - ts=timestamp, - frame_id=frame_id, - position=delta_pose.position, - orientation=delta_pose.orientation, - ) - - -# Expose blueprint for declarative composition -quest3_teleop_module = Quest3TeleopModule.blueprint From 118bc6e81ebd1193dfba7a24ab91d2a14b894f9b Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 22:12:21 +0000 Subject: [PATCH 16/72] Feat: Module updated to inherit form base class Former-commit-id: c36fd87ddf3d019117ae0456edec2b7d2049969e Former-commit-id: 4db422e0918189d89dc0ce6ce31c5beea0f90a72 --- dimos/teleop/quest3/quest3_teleop_module.py | 278 ++++++++++++++++++++ 1 file changed, 278 insertions(+) create mode 100644 dimos/teleop/quest3/quest3_teleop_module.py diff --git a/dimos/teleop/quest3/quest3_teleop_module.py b/dimos/teleop/quest3/quest3_teleop_module.py new file mode 100644 index 0000000000..582039d596 --- /dev/null +++ b/dimos/teleop/quest3/quest3_teleop_module.py @@ -0,0 +1,278 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +Quest3 Teleoperation Module + +A dimos Module that runs the WebSocket signaling server for Quest3 VR teleoperation, +receives controller tracking data, and streams DELTA poses to TeleopRobotController. + +This module inherits from BaseTeleopModule which handles: +- Calibration (capture initial controller poses) +- Delta computation (current - initial) +- Publishing delta poses to LCM +- Rerun visualization +- Standard RPC interface + +This module implements Quest3-specific: +- FastAPI/WebSocket server for Quest3 VR headset +- X button handler for calibration trigger +""" + +from __future__ import annotations + +import asyncio +from dataclasses import dataclass +import threading +from typing import TYPE_CHECKING, Any + +from dimos.core import rpc +from dimos.teleop.base import BaseTeleopConfig, BaseTeleopModule +from dimos.teleop.quest3.control.fastapi_server import TeleopFastAPIServer +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + import numpy as np + from numpy.typing import NDArray + + from dimos.core.global_config import GlobalConfig + +logger = setup_logger() + + +@dataclass +class Quest3TeleopConfig(BaseTeleopConfig): + """Configuration for Quest3 Teleoperation Module.""" + + # Quest3-specific WebSocket server settings + signaling_host: str = "0.0.0.0" + signaling_port: int = 8443 # HTTPS port for WebXR + use_https: bool = True # Enable HTTPS for WebXR (required by Quest 3) + # Exposed from BaseTeleopConfig for blueprint configuration + enable_left_arm: bool = True # Enable left arm teleoperation + enable_right_arm: bool = True # Enable right arm teleoperation + visualize_in_rerun: bool = True # Visualize VR controller poses in Rerun + safety_limits: bool = True # Enable safety limits + + # Other inherited from BaseTeleopConfig (with defaults): + # - position_scale: float = 1.0 + # - max_velocity: float = 0.5 + # - workspace_limits: dict[str, tuple[float, float]] + + +class Quest3TeleopModule(BaseTeleopModule): + """Quest3 VR Teleoperation Module. + + Inherits calibration, delta computation, and visualization from BaseTeleopModule. + Implements Quest3-specific WebSocket server and VR connection logic. + + ## Device-Specific Features: + - FastAPI/WebSocket server for Quest3 VR headset communication + - HTTPS support for WebXR (required by Quest 3) + - X button handler for calibration trigger + + ## LCM Topics (inherited from base): + - left_controller_delta: Out[PoseStamped] - Left controller delta pose + - right_controller_delta: Out[PoseStamped] - Right controller delta pose + - left_trigger: Out[Bool] - Left trigger button state + - right_trigger: Out[Bool] - Right trigger button state + + ## RPC Methods (inherited from base): + - start() -> None: Start the module and signaling server + - stop() -> None: Stop the module and signaling server + - calibrate_vr() -> dict: Calibrate VR (capture initial controller poses) + - reset_calibration() -> dict: Reset calibration + - is_vr_calibrated() -> bool: Check if VR is calibrated + - get_status() -> dict: Get current teleoperation status + """ + + default_config = Quest3TeleopConfig + + def __init__(self, global_config: GlobalConfig | None = None, *args, **kwargs) -> None: + super().__init__(global_config=global_config, *args, **kwargs) + + # Quest3-specific: FastAPI WebSocket server + self._fastapi_server: TeleopFastAPIServer | None = None + self._server_thread: threading.Thread | None = None + self._event_loop: asyncio.AbstractEventLoop | None = None + + logger.info("Quest3TeleopModule initialized") + + # ========================================================================= + # Module Lifecycle (Quest3-specific) + # ========================================================================= + + @rpc + def start(self) -> None: + """Start the Quest3 teleoperation module and signaling server.""" + logger.info("Starting Quest3 Teleoperation Module...") + + # Call base class start (handles Rerun connection, etc.) + super().start() + + # Start Quest3-specific signaling server + self._start_signaling_server() + + protocol = "https" if self.config.use_https else "http" + logger.info( + f"Quest3 Teleoperation Module started on {protocol}://{self.config.signaling_host}:{self.config.signaling_port}" + ) + logger.info( + f"Open this URL on Quest 3: {protocol}://:{self.config.signaling_port}/" + ) + logger.info("Press X button in VR to calibrate and start teleoperation") + + @rpc + def stop(self) -> None: + """Stop the Quest3 teleoperation module and signaling server.""" + logger.info("Stopping Quest3 Teleoperation Module...") + + # Stop signaling server + self._stop_signaling_server() + + super().stop() + + def _start_signaling_server(self) -> None: + """Start the FastAPI WebSocket server in a background thread.""" + + def run_server(): + # Create new event loop for this thread + loop = asyncio.new_event_loop() + asyncio.set_event_loop(loop) + self._event_loop = loop + + # Create FastAPI server instance + self._fastapi_server = TeleopFastAPIServer( + host=self.config.signaling_host, + port=self.config.signaling_port, + use_https=self.config.use_https, + ) + + # Register callbacks + self._fastapi_server.set_callback(self._on_tracking_data) + self._fastapi_server.set_command_callback(self._handle_x_button) + logger.info("FastAPI server initialized with callbacks") + + try: + # Run FastAPI server + loop.run_until_complete(self._fastapi_server.start_async()) + except Exception as e: + logger.error(f"FastAPI server error: {e}", exc_info=True) + finally: + loop.close() + + # Start server thread + self._server_thread = threading.Thread(target=run_server, daemon=True, name="FastAPIServer") + self._server_thread.start() + logger.info("FastAPI server thread started") + + def _stop_signaling_server(self) -> None: + """Stop the FastAPI WebSocket server.""" + if self._fastapi_server and self._event_loop: + # Schedule stop on the event loop + asyncio.run_coroutine_threadsafe(self._fastapi_server.stop_async(), self._event_loop) + + if self._event_loop and self._event_loop.is_running(): + # Stop the event loop + self._event_loop.call_soon_threadsafe(self._event_loop.stop) + + # Wait for thread to finish + if self._server_thread and self._server_thread.is_alive(): + self._server_thread.join(timeout=2.0) + + logger.info("FastAPI server stopped") + + # ========================================================================= + # Quest3-Specific: X Button Handler + # ========================================================================= + + async def _handle_x_button(self, command_type: str, websocket) -> dict: + """Handle X button press from VR client. + + X button toggles calibration: + - If not calibrated: calibrate VR + - If calibrated: reset calibration (stop streaming) + + Args: + command_type: 'start_teleop' or 'stop_teleop' + websocket: WebSocket connection to send responses + + Returns: + Response dictionary to send back to client + """ + try: + if command_type == "start_teleop": + logger.info("X button pressed - calibrating VR...") + logger.info( + f"Current state: left_pose={self._left_pose is not None}, right_pose={self._right_pose is not None}" + ) + result = self.calibrate_vr() + logger.info(f"Calibration result: {result}") + + if result.get("success"): + return { + "type": "teleop_started", + "message": result.get("message", "VR calibrated"), + } + else: + return { + "type": "calibration_failed", + "error": result.get("error", "Calibration failed"), + } + + elif command_type == "stop_teleop": + logger.info("X button pressed - stopping teleop...") + result = self.reset_calibration() + + return { + "type": "teleop_stopped", + "message": result.get("message", "Teleop stopped"), + } + + except Exception as e: + logger.error(f"Error handling X button: {e}", exc_info=True) + return {"type": "error", "error": f"Command failed: {e!s}"} + + return {"type": "error", "error": f"Unknown command: {command_type}"} + + # ========================================================================= + # Quest3-Specific: Controller Data Processing + # ========================================================================= + + def _on_tracking_data( + self, + left_pose: NDArray[np.float32], + right_pose: NDArray[np.float32], + left_gripper: float, + right_gripper: float, + ) -> None: + """Receive tracking data from Quest3 VR. + + Called by the FastAPI server when new tracking data arrives from Quest3. + Delegates to base class method which handles calibration, delta computation, + and publishing. + + Args: + left_pose: 4x4 transformation matrix for left controller + right_pose: 4x4 transformation matrix for right controller + left_gripper: Left gripper value (0.0-1.0) + right_gripper: Right gripper value (0.0-1.0) + """ + # Call base class method to handle everything + self.update_controller_poses(left_pose, right_pose, left_gripper, right_gripper) + + +# Expose blueprint for declarative composition +quest3_teleop_module = Quest3TeleopModule.blueprint From 7e21067964ee7e9fad87d25f6bad30e2aa393e66 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 22:12:26 +0000 Subject: [PATCH 17/72] docs: Readme Updated Former-commit-id: ebda886c0f934ebafb7fdb50525a33b420f4628d Former-commit-id: e08b862867b5f21d1b066f09302fdcf69713c301 --- dimos/teleop/README.md | 210 +++++++++++++++++++++++++++ dimos/teleop/base/README.md | 111 +++++++++++++++ dimos/teleop/quest3/README.md | 258 +++++++++++----------------------- 3 files changed, 400 insertions(+), 179 deletions(-) create mode 100644 dimos/teleop/README.md create mode 100644 dimos/teleop/base/README.md diff --git a/dimos/teleop/README.md b/dimos/teleop/README.md new file mode 100644 index 0000000000..df38b72405 --- /dev/null +++ b/dimos/teleop/README.md @@ -0,0 +1,210 @@ +# Teleoperation + +Teleoperation modules for controlling robots with various input devices. + +## Overview + +The teleoperation system consists of: +- **Base module** ([`base/`](base/)) - Common functionality for all devices +- **Device modules** (e.g., [`quest3/`](quest3/)) - Device-specific implementations +- **Robot controller** ([`teleop_robot_controller.py`](teleop_robot_controller.py)) - Applies deltas to robot + +## Architecture + +``` +Device Module (Quest3, SpaceMouse, etc.) + โ”œโ”€โ”€ Inherits from BaseTeleopModule + โ”œโ”€โ”€ Device-specific connection (WebSocket, USB, etc.) + โ””โ”€โ”€ Calls: update_controller_poses() + โ†“ +BaseTeleopModule + โ”œโ”€โ”€ Calibration (capture initial poses) + โ”œโ”€โ”€ Delta computation (current - initial) + โ”œโ”€โ”€ Publishes delta poses (LCM) + โ””โ”€โ”€ Rerun visualization + โ†“ +TeleopRobotController + โ”œโ”€โ”€ Receives delta poses + โ”œโ”€โ”€ Auto-calibrates robot + โ””โ”€โ”€ Applies: target = initial_robot + delta + โ†“ +Robot Driver โ†’ Robot +``` + +## Supported Devices + +### Quest 3 VR Headset [Any VR headset] +**Module**: [`quest3/`](quest3/) +**Type**: VR controllers (6DOF dual-arm) +**Connection**: WebSocket/HTTPS (WebXR) +**Features**: AR mode, dual-arm control, trigger buttons + +```python +from dimos.teleop.quest3 import Quest3TeleopModule + +teleop = Quest3TeleopModule() +teleop.start() +# Open https://your-ip:8443 on Quest 3 +``` + +See [Quest3 README](quest3/README.md) for details. + +## Quick Start + +### 1. Choose Your Device + +```python +# Quest 3 VR +from dimos.teleop.quest3 import Quest3TeleopModule +device = Quest3TeleopModule() + +# Future: SpaceMouse, other VR headsets, etc. +``` + +### 2. Start Teleoperation + +```python +device.start() +# Press calibration button (X button for Quest3) +# Move controllers to control robot +``` + +### 3. Use with Blueprint + +```python +from dimos.core.blueprints import autoconnect +from dimos.teleop.quest3 import quest3_teleop_module +from dimos.teleop import teleop_robot_controller + +system = autoconnect( + quest3_teleop_module( + enable_left_arm=True, + enable_right_arm=True, + ), + teleop_robot_controller( + driver_module_name="YourDriver", + ), + your_robot_blueprint, +) + +coordinator = system.build() +coordinator.loop() +``` + +## How It Works + +### Two-Stage Calibration + +1. **Device Calibration** (press calibration button): + - Captures initial controller poses + - Starts publishing **delta poses**: `delta = current - initial` + +2. **Robot Calibration** (automatic on first delta): + - TeleopRobotController captures robot's initial pose + - Applies deltas: `target_pose = robot_initial + delta` + +### Data Flow + +``` +Controllers โ†’ Device Module โ†’ Delta Poses โ†’ Arm Controller โ†’ Robot + (absolute) (relative) (absolute) +``` + +**Key insight**: Device publishes deltas, not absolute poses. This makes the system: +- โœ… Device-agnostic (any coordinate frame works) +- โœ… Robot-agnostic (works with any robot) +- โœ… Recalibrable (just press button again) + +## Creating New Devices + +See [`base/README.md`](base/README.md) for instructions on creating new teleoperation devices. + +Example devices you could add: +- SpaceMouse (USB 6DOF controller) +- Other VR headsets (HTC Vive, Valve Index) +- Haptic devices (Force Dimension Omega) +- Game controllers with custom mapping +- Mobile phone IMU-based control + + + +## LCM Topics + +All devices publish: + +- `left_controller_delta: Out[PoseStamped]` - Left delta pose +- `right_controller_delta: Out[PoseStamped]` - Right delta pose +- `left_trigger: Out[Bool]` - Left trigger state +- `right_trigger: Out[Bool]` - Right trigger state + +## Components + +- **[`base/`](base/)** - Base teleoperation module (abstract) +- **[`quest3/`](quest3/)** - Quest 3 VR implementation +- **[`teleop_robot_controller.py`](teleop_robot_controller.py)** - Applies deltas to robot +- **[`teleop_blueprints.py`](teleop_blueprints.py)** - Pre-built system blueprints + +## Benefits of This Architecture + +1. **Reusability**: Common code shared across all devices +2. **Consistency**: Same behavior and interface for all devices +3. **Maintainability**: Bug fixes in base class benefit all devices +4. **Rapid Development**: New devices in ~50-100 lines of code +5. **Device Agnostic**: Robot doesn't know/care what device is used + +## Example: Full System + +```python +from dimos.teleop.teleop_blueprints import quest3_teleop + +# Pre-built blueprint with Quest3 + TeleopRobotController +coordinator = quest3_teleop.build() +coordinator.loop() +``` + +Or build custom: + +```python +from dimos.core.blueprints import autoconnect +from dimos.core.transport import LCMTransport +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.teleop.quest3 import quest3_teleop_module +from dimos.teleop import teleop_robot_controller + +my_system = ( + autoconnect( + quest3_teleop_module(signaling_port=8443), + teleop_robot_controller(driver_module_name="XArmDriver"), + your_robot, + ) + .transports({ + ("left_controller_delta", PoseStamped): LCMTransport("/teleop/left", PoseStamped), + ("right_controller_delta", PoseStamped): LCMTransport("/teleop/right", PoseStamped), + }) +) + +coordinator = my_system.build() +coordinator.loop() +``` + +## Troubleshooting + +### Device Not Publishing Deltas +1. Press calibration button to capture initial poses +2. Check `is_vr_calibrated()` returns True +3. Move controllers and check LCM topics + +### Robot Not Moving +1. Ensure TeleopRobotController received first delta (auto-calibrates) +2. Check robot driver is running +3. Verify LCM topic connections in blueprint + +### Want to Recalibrate +Just call `reset_calibration()` and press calibration button again. + +## Related Documentation + +- [Base Module](base/README.md) - Creating new devices +- [Quest3 Module](quest3/README.md) - Quest 3 VR setup +- [Blueprints](../../docs/api/blueprints.md) - System composition + diff --git a/dimos/teleop/base/README.md b/dimos/teleop/base/README.md new file mode 100644 index 0000000000..d7a6037940 --- /dev/null +++ b/dimos/teleop/base/README.md @@ -0,0 +1,111 @@ +# Base Teleoperation Module + +Abstract base class for all teleoperation devices in dimos. + +## Overview + +`BaseTeleopModule` provides common functionality for teleoperation devices: +- **Calibration**: Capture initial controller poses as reference +- **Delta computation**: Calculate `current - initial` poses +- **Publishing**: Stream delta poses and trigger states to LCM +- **Visualization**: Rerun integration for controller poses +- **RPC interface**: Standard methods across all devices + +## Architecture + +``` +BaseTeleopModule (base class) +โ”œโ”€โ”€ Calibration logic +โ”œโ”€โ”€ Delta pose computation +โ”œโ”€โ”€ LCM publishing +โ”œโ”€โ”€ Rerun visualization +โ””โ”€โ”€ update_controller_poses() โ† Device-specific code calls this + +Device-Specific Module (e.g., Quest3TeleopModule) +โ”œโ”€โ”€ Connection logic (WebSocket, USB, etc.) +โ”œโ”€โ”€ Data parsing +โ””โ”€โ”€ Calls: update_controller_poses(left_pose, right_pose, left_gripper, right_gripper) +``` + +## Creating a New Teleoperation Module + +1. **Inherit from `BaseTeleopModule`** +2. **Define device-specific config** (inherit from `BaseTeleopConfig`) +3. **Implement connection logic** in `start()` method +4. **Call `update_controller_poses()`** when new tracking data arrives + +### Example: SpaceMouse Device + +```python +from dimos.teleop.base import BaseTeleopConfig, BaseTeleopModule + +@dataclass +class SpaceMouseTeleopConfig(BaseTeleopConfig): + usb_device_id: str = "/dev/input/spacemouse" + # Any additional params + +class SpaceMouseTeleopModule(BaseTeleopModule): + default_config = SpaceMouseTeleopConfig + + @rpc + def start(self): + super().start() + # Start USB reader thread + self._usb_thread = threading.Thread(target=self._read_usb_loop) + self._usb_thread.start() + + def _read_usb_loop(self): + while self._running: + # Parse USB data to 4x4 transformation matrices + left_pose, right_pose = parse_spacemouse_data(...) + + # Base class handles everything else! + self.update_controller_poses( + left_pose, right_pose, + left_gripper=0.0, right_gripper=0.0 + ) +``` + +## Configuration + +### BaseTeleopConfig + +```python +@dataclass +class BaseTeleopConfig(ModuleConfig): + position_scale: float = 1.0 # Scale factor for positions + enable_left_arm: bool = True # Enable left arm + enable_right_arm: bool = True # Enable right arm + visualize_in_rerun: bool = True # Visualize in Rerun + safety_limits: bool = True # Enable safety limits + max_velocity: float = 0.5 # m/s + workspace_limits: dict[str, tuple[float, float]] # x, y, z limits +``` + +## RPC Methods (Inherited) + +All devices get these methods automatically: + +- `calibrate_vr()` โ†’ Capture initial controller poses +- `reset_calibration()` โ†’ Reset calibration state +- `is_vr_calibrated()` โ†’ Check if calibrated +- `get_status()` โ†’ Get teleoperation status + +## LCM Topics (Inherited) + +All devices publish these topics: + +- `left_controller_delta: Out[PoseStamped]` - Left controller delta pose +- `right_controller_delta: Out[PoseStamped]` - Right controller delta pose +- `left_trigger: Out[Bool]` - Left trigger state +- `right_trigger: Out[Bool]` - Right trigger state + +## Benefits + +- **Reusability**: Write device connection once, inherit everything else +- **Consistency**: Same RPC interface and behavior across all devices +- **Easy testing**: Base class tested independently + +## Existing Implementations + +- **Quest3TeleopModule** (`dimos/teleop/quest3/`) - Meta Quest 3 VR headset via WebXR diff --git a/dimos/teleop/quest3/README.md b/dimos/teleop/quest3/README.md index 401d23b3f4..f6b0476f3d 100644 --- a/dimos/teleop/quest3/README.md +++ b/dimos/teleop/quest3/README.md @@ -1,6 +1,6 @@ # Quest3 VR Teleoperation -**NPM-Free** VR teleoperation system for Quest 3 headset with robot manipulation. +VR teleoperation for Quest 3 headset with robot manipulation. ## Quick Start @@ -14,8 +14,8 @@ teleop = Quest3TeleopModule() teleop.start() # Output: -# โœ… Quest3 Teleoperation Module started on https://0.0.0.0:8443 -# ๐Ÿ“ฑ Open this URL on Quest 3: https://:8443/ +# Quest3 Teleoperation Module started on https://0.0.0.0:8443 +# Open this URL on Quest 3: https://:8443/ ``` ### 2. Connect Quest 3 @@ -23,12 +23,12 @@ teleop.start() 1. **Find your server IP**: ```bash hostname -I - # Example: 192.168.1.100 + # Example: 10.0.0.217 ``` 2. **Open Quest 3 browser** and go to: ``` - https://192.168.1.100:8443/ + https://10.0.0.217:8443/ ``` 3. **Accept security warning** (self-signed certificate is safe) @@ -45,43 +45,54 @@ teleop.start() ``` Quest 3 Browser โ†“ Opens: https://your-ip:8443/ - โ†“ Loads: Standalone HTML/JS VR client + โ†“ Loads: Standalone HTML/JS VR client (no build step!) โ†“ Connects: wss://your-ip:8443/ws - โ†“ X button pressed โ†’ sends start_teleop command + โ†“ X button pressed โ†’ calibrate_vr() via WebSocket โ†“ -FastAPI Server (Python, single process) - โ†“ Serves HTML + WebSocket +FastAPI Server (Quest3-specific) + โ†“ Serves static HTML + WebSocket โ†“ Auto-generates SSL certificates - โ†“ Processes tracking data (20Hz) - โ†“ Routes X button commands to TeleopArmController + โ†“ Receives tracking data (left/right controller poses) + โ†“ Calls: update_controller_poses() โ†“ -Quest3TeleopModule - โ†“ Publishes controller pose topics (PoseStamped) - โ†“ -TeleopArmController - โ†“ Receives X button command โ†’ start_control() - โ†“ Calibrates (captures initial controller + robot poses) - โ†“ Activates control loop (runs continuously, gated by flags) - โ†“ Calculates relative control: target = initial_robot + (current_controller - initial_controller) +Quest3TeleopModule (inherits BaseTeleopModule) + โ”œโ”€โ”€ FastAPI/WebSocket server (Quest3-specific) + โ”œโ”€โ”€ X button handler (Quest3-specific) + โ””โ”€โ”€ Calibration, delta computation, publishing (from base) + โ†“ Publishes delta pose topics (PoseStamped) + โ†“ +TeleopRobotController + โ†“ Receives delta poses from Quest3TeleopModule + โ†“ Auto-calibrates robot on first delta + โ†“ Calculates: target = initial_robot + delta โ†“ Publishes cartesian commands (Pose) โ†“ -XArmDriver / Robot Driver +Robot Driver โ†“ Executes cartesian commands - โ†“ -Robot ``` +### What's Inherited from BaseTeleopModule + +Quest3TeleopModule inherits these features: +- โœ… Calibration logic (capture initial controller poses) +- โœ… Delta pose computation (current - initial) +- โœ… LCM publishing (delta poses + trigger states) +- โœ… Rerun visualization +- โœ… Standard RPC interface + +Quest3-specific implementation: +- ๐Ÿ”ง FastAPI/WebSocket server for VR headset +- ๐Ÿ”ง HTTPS certificate generation +- ๐Ÿ”ง X button handler + ## Key Features -โœ… **Zero npm/Node.js dependencies** - Pure Python + vanilla JS -โœ… **Single HTML file** - No build process, instant updates -โœ… **Auto-generated SSL** - Certificates created automatically -โœ… **One Python process** - Everything in FastAPI -โœ… **X button control** - Press X to start/stop teleoperation -โœ… **Auto-calibration** - Captures initial poses on first X press -โœ… **Relative control** - Robot follows controller movement relative to initial pose -โœ… **20Hz tracking rate** - Low latency controller data -โœ… **WebXR passthrough** - AR mode support +โœ… Pure Python + vanilla JS +โœ… Press X to start/stop teleoperation +โœ… Captures initial poses on first X press +โœ… Robot follows controller movement relative to initial pose +โœ… Low latency controller data +โœ… AR mode ## Configuration @@ -89,18 +100,21 @@ Robot from dimos.teleop.quest3 import Quest3TeleopModule, Quest3TeleopConfig config = Quest3TeleopConfig( + # Quest3-specific settings signaling_host="0.0.0.0", - signaling_port=8443, # HTTPS port - use_https=True, # Required for WebXR - driver_module_name="XArmDriver", - position_scale=1.0, + signaling_port=8443, # HTTPS port (required for WebXR) + use_https=True, # Required for Quest 3 WebXR + + # Inherited from BaseTeleopConfig enable_left_arm=True, - enable_right_arm=False, + enable_right_arm=True, + visualize_in_rerun=True, # Visualize controllers in Rerun + position_scale=1.0, max_velocity=0.5, workspace_limits={ - "x": (0.1, 1.0), + "x": (-1.0, 1.0), "y": (-0.8, 0.8), - "z": (0.1, 0.7), + "z": (0.1, 2.0), }, ) @@ -108,75 +122,16 @@ module = Quest3TeleopModule(config=config) module.start() ``` -## Available Blueprints - -### Basic Teleoperation (No Camera) - -```bash -# XArm6 -dimos run quest3-xarm6 - -# XArm7 -dimos run quest3-xarm7 - -# Piper Robot -dimos run quest3-piper -``` - -### With Camera Streaming - -```bash -# XArm6 + RealSense D435i -dimos run quest3-xarm6-camera - -# XArm7 + RealSense -dimos run quest3-xarm7-camera - -# Piper + RealSense -dimos run quest3-piper-camera -``` - -## Files Structure - -``` -dimos/teleop/quest3/ -โ”œโ”€โ”€ static/ -โ”‚ โ””โ”€โ”€ index.html # Standalone VR client (vanilla JS) -โ”œโ”€โ”€ certs/ # Auto-generated SSL certificates -โ”‚ โ”œโ”€โ”€ cert.pem -โ”‚ โ””โ”€โ”€ key.pem -โ”œโ”€โ”€ control/ -โ”‚ โ”œโ”€โ”€ fastapi_server.py # FastAPI server with HTTPS + WebSocket -โ”‚ โ””โ”€โ”€ tracking_processor.py # VR tracking data processor -โ”œโ”€โ”€ teleop_module.py # Main teleoperation module -โ”œโ”€โ”€ README.md # This file -โ””โ”€โ”€ README_NO_NPM.md # Detailed setup guide -``` - -## How It Works - -1. **Control Loop**: Runs continuously from `start()`, processing controller poses at 20Hz -2. **X Button Press**: - - Sends `start_teleop` command via WebSocket - - Triggers `TeleopArmController.start_control()` - - If not calibrated, automatically calibrates first - - Then activates control (sets `control_active = True`) -3. **Calibration**: Captures initial controller pose and robot end-effector pose -4. **Relative Control**: Calculates target pose as: `target = initial_robot + (current_controller - initial_controller)` -5. **Safety Gates**: Control loop only sends commands when both `calibrated` and `control_active` are True ## RPC Methods -**Quest3TeleopModule**: +All inherited from `BaseTeleopModule`: - `start()` โ†’ Start module and FastAPI server - `stop()` โ†’ Stop module and server -- `get_status()` โ†’ Get current status - -**TeleopArmController**: -- `start_control()` โ†’ Calibrate (if needed) and activate control -- `stop_control()` โ†’ Stop sending commands (preserves calibration) -- `calibrate()` โ†’ Manually calibrate -- `is_control_active()` โ†’ Check if control is active +- `calibrate_vr()` โ†’ Calibrate VR (capture initial controller poses) +- `reset_calibration()` โ†’ Reset VR calibration +- `is_vr_calibrated()` โ†’ Check if VR is calibrated +- `get_status()` โ†’ Get teleoperation status ## WebSocket Protocol @@ -195,22 +150,6 @@ dimos/teleop/quest3/ } ``` -### Tracking Data (20Hz) -```json -{ - "type": "controller", - "left": { - "targetLocation": [16 floats], // 4x4 transformation matrix - "trigger": 0.0, // 0.0 - 1.0 - "grip": 0.0, - "joystickX": 0.0, - "joystickY": 0.0, - "buttons": [true, false, ...] - }, - "right": { /* same structure */ } -} -``` - ### X Button Commands ```json // Client โ†’ Server (when X button pressed) @@ -227,47 +166,15 @@ dimos/teleop/quest3/ ## Output Topics -The module publishes these topics: +Inherited from `BaseTeleopModule`: -- `left_controller_pose` (PoseStamped) - Left controller pose -- `right_controller_pose` (PoseStamped) - Right controller pose +- `left_controller_delta` (PoseStamped) - Left controller **delta** pose (current - initial) +- `right_controller_delta` (PoseStamped) - Right controller **delta** pose (current - initial) - `left_trigger` (Bool) - Left trigger button state - `right_trigger` (Bool) - Right trigger button state -## Troubleshooting - -### WebXR Not Available -**Solution**: Make sure you're using HTTPS (required by Quest 3): -```python -config = Quest3TeleopConfig(use_https=True) # Must be True! -``` - -### Certificate Warning on Quest 3 -**Solution**: Accept the self-signed certificate: -1. Click "Advanced" on warning page -2. Click "Proceed to (unsafe)" -3. This is safe for local development - -### Port Already in Use -```bash -# Check what's using the port -sudo lsof -i :8443 +**Note**: These are **delta poses**, not absolute poses. TeleopRobotController applies these deltas to the robot's initial pose. -# Kill the process -sudo kill -9 - -# Or use a different port -config = Quest3TeleopConfig(signaling_port=9443) -``` - -### Can't Generate SSL Certificates -```bash -# Install OpenSSL -sudo apt-get install openssl - -# Verify -openssl version -``` ### WebSocket Connection Fails **Check**: @@ -277,26 +184,15 @@ openssl version 4. Correct IP (not localhost) ### No Tracking Data / Robot Not Moving -1. **Press X button** to start teleoperation (calibrates and activates) -2. Verify WebSocket connection in browser console -3. Check server logs for calibration/control status -4. Ensure controllers are being tracked in VR -5. Check that `control_active` and `calibrated` are both True in logs +1. **Press X button** to calibrate VR (captures initial controller poses) +2. **Move controllers** - deltas are published automatically after calibration +3. Verify WebSocket connection in browser console +4. Check server logs: `is_vr_calibrated()` should return True +5. TeleopRobotController auto-calibrates robot on first delta +6. Ensure controllers are being tracked in VR ## Development -### Edit VR Client -Simply edit `static/index.html` and refresh browser (no build needed!): - -```html - -
Custom Info
- - -``` ### Standalone Server Testing ```bash @@ -312,24 +208,28 @@ python fastapi_server.py ```python from dimos.core.blueprints import autoconnect from dimos.teleop.quest3 import quest3_teleop_module +from dimos.teleop import teleop_robot_controller from your_robot import your_robot_blueprint custom_stack = autoconnect( - your_robot_blueprint, quest3_teleop_module( + signaling_port=8443, + enable_left_arm=True, + enable_right_arm=False, + ), + teleop_robot_controller( driver_module_name="YourDriver", - position_scale=1.0, + enable_left_arm=True, + enable_right_arm=False, ), + your_robot_blueprint, ) coordinator = custom_stack.build() coordinator.loop() ``` -## Performance (Need to be updated) +## Related -- **Latency**: ~50-70ms end-to-end (Quest 3 โ†’ Python โ†’ Robot) -- **Tracking Rate**: 20Hz (50ms interval) -- **CPU Usage**: <5% (FastAPI server) -- **Memory**: ~50MB (Python process) -- **Network**: ~2KB/s (tracking data) +- [`BaseTeleopModule`](../base/) - Base class for all teleoperation devices +- [`TeleopRobotController`](../teleop_robot_controller.py) - Applies deltas to robot From 97c4e0b6a7a78104412e9075b8979647b6f2ffa1 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 22:12:28 +0000 Subject: [PATCH 18/72] Misc: Init updates Former-commit-id: 965c54b934da0280300f0bd21f7e47bbfa76f789 Former-commit-id: abb568d0904d5b24e29f0a5f6c71ffc14b4f8daf --- dimos/teleop/__init__.py | 23 +++++++++++++++++++++++ dimos/teleop/base/__init__.py | 22 ++++++++++++++++++++++ dimos/teleop/quest3/__init__.py | 2 +- 3 files changed, 46 insertions(+), 1 deletion(-) create mode 100644 dimos/teleop/__init__.py create mode 100644 dimos/teleop/base/__init__.py diff --git a/dimos/teleop/__init__.py b/dimos/teleop/__init__.py new file mode 100644 index 0000000000..4df4ca1044 --- /dev/null +++ b/dimos/teleop/__init__.py @@ -0,0 +1,23 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Teleoperation modules for dimos.""" + +from dimos.teleop.base import BaseTeleopConfig, BaseTeleopModule + +__all__ = [ + "BaseTeleopConfig", + "BaseTeleopModule", +] + diff --git a/dimos/teleop/base/__init__.py b/dimos/teleop/base/__init__.py new file mode 100644 index 0000000000..c5c9f896ae --- /dev/null +++ b/dimos/teleop/base/__init__.py @@ -0,0 +1,22 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Base teleoperation module exports.""" + +from dimos.teleop.base.base_teleop_module import ( + BaseTeleopConfig, + BaseTeleopModule, +) + +__all__ = ["BaseTeleopConfig", "BaseTeleopModule"] diff --git a/dimos/teleop/quest3/__init__.py b/dimos/teleop/quest3/__init__.py index cb7ac90b3e..488e40e8be 100644 --- a/dimos/teleop/quest3/__init__.py +++ b/dimos/teleop/quest3/__init__.py @@ -1,6 +1,6 @@ """Quest3 VR Teleoperation.""" -from dimos.teleop.quest3.teleop_module import Quest3TeleopConfig, Quest3TeleopModule +from dimos.teleop.quest3.quest3_teleop_module import Quest3TeleopConfig, Quest3TeleopModule __all__ = [ "Quest3TeleopConfig", From 8e55eba84bbc9f64f56bdd4277bd7e0673fd0522 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 22:13:05 +0000 Subject: [PATCH 19/72] Fix: Pre-commit fixes Former-commit-id: e946e67e972d3021f8f7d3ccb3c67c30adb0187d Former-commit-id: dc5219f3116a8743870bc343236513d6c178ccd3 --- dimos/teleop/README.md | 9 ++- dimos/teleop/__init__.py | 1 - dimos/teleop/quest3/control/fastapi_server.py | 2 +- dimos/teleop/teleop_blueprints.py | 59 +++++++++---------- dimos/teleop/teleop_robot_controller.py | 8 +-- 5 files changed, 36 insertions(+), 43 deletions(-) diff --git a/dimos/teleop/README.md b/dimos/teleop/README.md index df38b72405..87f71a4545 100644 --- a/dimos/teleop/README.md +++ b/dimos/teleop/README.md @@ -34,9 +34,9 @@ Robot Driver โ†’ Robot ## Supported Devices ### Quest 3 VR Headset [Any VR headset] -**Module**: [`quest3/`](quest3/) -**Type**: VR controllers (6DOF dual-arm) -**Connection**: WebSocket/HTTPS (WebXR) +**Module**: [`quest3/`](quest3/) +**Type**: VR controllers (6DOF dual-arm) +**Connection**: WebSocket/HTTPS (WebXR) **Features**: AR mode, dual-arm control, trigger buttons ```python @@ -133,7 +133,7 @@ Example devices you could add: All devices publish: - `left_controller_delta: Out[PoseStamped]` - Left delta pose -- `right_controller_delta: Out[PoseStamped]` - Right delta pose +- `right_controller_delta: Out[PoseStamped]` - Right delta pose - `left_trigger: Out[Bool]` - Left trigger state - `right_trigger: Out[Bool]` - Right trigger state @@ -207,4 +207,3 @@ Just call `reset_calibration()` and press calibration button again. - [Base Module](base/README.md) - Creating new devices - [Quest3 Module](quest3/README.md) - Quest 3 VR setup - [Blueprints](../../docs/api/blueprints.md) - System composition - diff --git a/dimos/teleop/__init__.py b/dimos/teleop/__init__.py index 4df4ca1044..089ff68c99 100644 --- a/dimos/teleop/__init__.py +++ b/dimos/teleop/__init__.py @@ -20,4 +20,3 @@ "BaseTeleopConfig", "BaseTeleopModule", ] - diff --git a/dimos/teleop/quest3/control/fastapi_server.py b/dimos/teleop/quest3/control/fastapi_server.py index ecab7c9be3..466100cb82 100644 --- a/dimos/teleop/quest3/control/fastapi_server.py +++ b/dimos/teleop/quest3/control/fastapi_server.py @@ -25,9 +25,9 @@ from __future__ import annotations import asyncio -import threading from pathlib import Path import subprocess +import threading import time from typing import TYPE_CHECKING diff --git a/dimos/teleop/teleop_blueprints.py b/dimos/teleop/teleop_blueprints.py index a74a811d79..4523ce3141 100644 --- a/dimos/teleop/teleop_blueprints.py +++ b/dimos/teleop/teleop_blueprints.py @@ -77,39 +77,36 @@ # TeleopRobotController.right_cartesian_command โ”€โ”€โ–บ Robot Driver (via LCM) # ============================================================================= -quest3_teleop = ( - autoconnect( - quest3_teleop_module( - signaling_host="0.0.0.0", - signaling_port=8443, # HTTPS port (required for WebXR) - use_https=True, # Enable HTTPS for Quest 3 WebXR - enable_left_arm=True, - enable_right_arm=True, - visualize_in_rerun=True, - safety_limits=True, +quest3_teleop = autoconnect( + quest3_teleop_module( + signaling_host="0.0.0.0", + signaling_port=8443, # HTTPS port (required for WebXR) + use_https=True, # Enable HTTPS for Quest 3 WebXR + enable_left_arm=True, + enable_right_arm=True, + visualize_in_rerun=True, + safety_limits=True, + ), + teleop_robot_controller( + driver_module_name="DummyDriver", + dummy_driver=True, # Skip RPC calls, use zeros for initial pose + control_frequency=50.0, # Hz - control loop frequency + enable_left_arm=True, + enable_right_arm=True, + ), +).transports( + { + # Delta poses from Quest3TeleopModule to TeleopRobotController + ("left_controller_delta", PoseStamped): LCMTransport( + "/quest3/left_controller_delta", PoseStamped ), - teleop_robot_controller( - driver_module_name="DummyDriver", - dummy_driver=True, # Skip RPC calls, use zeros for initial pose - control_frequency=50.0, # Hz - control loop frequency - enable_left_arm=True, - enable_right_arm=True, + ("right_controller_delta", PoseStamped): LCMTransport( + "/quest3/right_controller_delta", PoseStamped ), - ) - .transports( - { - # Delta poses from Quest3TeleopModule to TeleopRobotController - ("left_controller_delta", PoseStamped): LCMTransport( - "/quest3/left_controller_delta", PoseStamped - ), - ("right_controller_delta", PoseStamped): LCMTransport( - "/quest3/right_controller_delta", PoseStamped - ), - # Trigger states - ("left_trigger", Bool): LCMTransport("/quest3/left_trigger", Bool), - ("right_trigger", Bool): LCMTransport("/quest3/right_trigger", Bool), - } - ) + # Trigger states + ("left_trigger", Bool): LCMTransport("/quest3/left_trigger", Bool), + ("right_trigger", Bool): LCMTransport("/quest3/right_trigger", Bool), + } ) diff --git a/dimos/teleop/teleop_robot_controller.py b/dimos/teleop/teleop_robot_controller.py index 00b6491242..3b6ba3f68f 100644 --- a/dimos/teleop/teleop_robot_controller.py +++ b/dimos/teleop/teleop_robot_controller.py @@ -478,8 +478,6 @@ def wrap_angle(angle): # Log and publish if arm_side == "left": - quat = target_pose.orientation - # Unwrap Euler angles for smooth logging (handles +pi and -pi discontinuities) current_rpy = np.array([target_pose.roll, target_pose.pitch, target_pose.yaw]) if arm_side in self._last_logged_rpy: @@ -497,8 +495,6 @@ def wrap_angle(angle): logger.error(f"Failed to publish left cartesian command: {e}") elif arm_side == "right": - quat = target_pose.orientation - # Unwrap Euler angles for smooth logging (handles +pi and -pi discontinuities) current_rpy = np.array([target_pose.roll, target_pose.pitch, target_pose.yaw]) if arm_side in self._last_logged_rpy: @@ -509,7 +505,9 @@ def wrap_angle(angle): self._last_logged_rpy[arm_side] = current_rpy # Publish to robot - if self.right_cartesian_command and hasattr(self.right_cartesian_command, "publish"): + if self.right_cartesian_command and hasattr( + self.right_cartesian_command, "publish" + ): try: self.right_cartesian_command.publish(target_pose) except Exception as e: From e527f2204d9c44526e3d1177b9e1c6f88252b600 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 22:24:24 +0000 Subject: [PATCH 20/72] Feat: Added Float32 to std_msgs Former-commit-id: 7a08cc744be6eb7f72168a47d440b69998c7ec99 Former-commit-id: 1fba88a74a25d17eef50ef04b99a71502dc49cff --- dimos/msgs/std_msgs/Float32.py | 59 +++++++++++++++++++++++++++++++++ dimos/msgs/std_msgs/__init__.py | 3 +- 2 files changed, 61 insertions(+), 1 deletion(-) create mode 100644 dimos/msgs/std_msgs/Float32.py diff --git a/dimos/msgs/std_msgs/Float32.py b/dimos/msgs/std_msgs/Float32.py new file mode 100644 index 0000000000..94a2ca6551 --- /dev/null +++ b/dimos/msgs/std_msgs/Float32.py @@ -0,0 +1,59 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Float32 message type.""" + +from typing import ClassVar + +from dimos_lcm.std_msgs import Float32 as LCMFloat32 + +try: + from std_msgs.msg import Float32 as ROSFloat32 # type: ignore[attr-defined] +except ImportError: + ROSFloat32 = None # type: ignore[assignment, misc] + + +class Float32(LCMFloat32): # type: ignore[misc] + """ROS-compatible Float32 message.""" + + msg_name: ClassVar[str] = "std_msgs.Float32" + + def __init__(self, data: float = 0.0) -> None: + """Initialize Float32 with data value.""" + self.data = data + + @classmethod + def from_ros_msg(cls, ros_msg: ROSFloat32) -> "Float32": + """Create a Float32 from a ROS std_msgs/Float32 message. + + Args: + ros_msg: ROS Float32 message + + Returns: + Float32 instance + """ + return cls(data=ros_msg.data) + + def to_ros_msg(self) -> ROSFloat32: + """Convert to a ROS std_msgs/Float32 message. + + Returns: + ROS Float32 message + """ + if ROSFloat32 is None: + raise ImportError("ROS std_msgs not available") + ros_msg = ROSFloat32() # type: ignore[no-untyped-call] + ros_msg.data = float(self.data) + return ros_msg + diff --git a/dimos/msgs/std_msgs/__init__.py b/dimos/msgs/std_msgs/__init__.py index 9002b8c4ef..1404d547ee 100644 --- a/dimos/msgs/std_msgs/__init__.py +++ b/dimos/msgs/std_msgs/__init__.py @@ -13,8 +13,9 @@ # limitations under the License. from .Bool import Bool +from .Float32 import Float32 from .Header import Header from .Int8 import Int8 from .Int32 import Int32 -__all__ = ["Bool", "Header", "Int8", "Int32"] +__all__ = ["Bool", "Float32", "Header", "Int8", "Int32"] From 2f1c182df0216bb99c113135b34e6441bc8b82a9 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 22:24:30 +0000 Subject: [PATCH 21/72] Feat: Trigger value published in float Former-commit-id: 82254913efcde0d8d941b53a65df9ee737b74ad5 Former-commit-id: d7a935096d7fd57ac9ad524ed1910294fb973a44 --- dimos/teleop/quest3/control/tracking_processor.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/dimos/teleop/quest3/control/tracking_processor.py b/dimos/teleop/quest3/control/tracking_processor.py index b87de7e1ec..0af1dd40d6 100644 --- a/dimos/teleop/quest3/control/tracking_processor.py +++ b/dimos/teleop/quest3/control/tracking_processor.py @@ -145,18 +145,18 @@ def _process_target_location(self, target_location: list[float], side: str) -> N self.right_wrist_pose = wrist_mat def _extract_gripper_value(self, tracking_data: dict[str, Any]) -> float: - """Extract and validate gripper value from tracking data. + """Extract gripper value from tracking data. Args: tracking_data: Controller data containing 'trigger' Returns: - Gripper value clamped to [0.0, 1.0] + Gripper value (0.0-1.0) from VR controller trigger """ gripper_value = tracking_data.get("trigger", 0.0) try: - gripper_value = float(gripper_value) - return max(0.0, min(1.0, gripper_value)) + # VR trigger value is already 0.0-1.0, just convert to float + return float(gripper_value) except (ValueError, TypeError): - logger.warning("Invalid trigger value: %s", gripper_value) + logger.warning("Invalid trigger value: %s, using 0.0", gripper_value) return 0.0 From 52708250a335f20a43d7b79904f9044d2193d43e Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 22:24:45 +0000 Subject: [PATCH 22/72] Feat: Publishes in float32 + triggers viz in rerun Former-commit-id: 2cf6c7710d3be297f09e1d9ee2eaef7a7caefc26 Former-commit-id: bfc94ce1bc2566d12a6b2dfd8b84dbb40d9f55bc --- dimos/teleop/base/base_teleop_module.py | 61 ++++++++++++++++++------- 1 file changed, 44 insertions(+), 17 deletions(-) diff --git a/dimos/teleop/base/base_teleop_module.py b/dimos/teleop/base/base_teleop_module.py index 1d89f668d7..62e92db966 100644 --- a/dimos/teleop/base/base_teleop_module.py +++ b/dimos/teleop/base/base_teleop_module.py @@ -37,7 +37,7 @@ from dimos.core.global_config import GlobalConfig from dimos.core.module import ModuleConfig from dimos.msgs.geometry_msgs import Pose, PoseStamped, Vector3 -from dimos.msgs.std_msgs import Bool +from dimos.msgs.std_msgs import Bool, Float32 from dimos.utils.logging_config import setup_logger from dimos.utils.transform_utils import matrix_to_pose @@ -97,8 +97,8 @@ class BaseTeleopModule(Module, ABC): ## LCM Topics (Output): - left_controller_delta: Out[PoseStamped] - Left controller delta pose - right_controller_delta: Out[PoseStamped] - Right controller delta pose - - left_trigger: Out[Bool] - Left trigger button state - - right_trigger: Out[Bool] - Right trigger button state + - left_trigger: Out[Float32] - Left trigger/gripper value (0.0-1.0) + - right_trigger: Out[Float32] - Right trigger/gripper value (0.0-1.0) ## RPC Methods: - calibrate_vr() -> dict: Calibrate by capturing initial poses @@ -112,8 +112,8 @@ class BaseTeleopModule(Module, ABC): # LCM Output topics left_controller_delta: Out[PoseStamped] = None # type: ignore[assignment] right_controller_delta: Out[PoseStamped] = None # type: ignore[assignment] - left_trigger: Out[Bool] = None # type: ignore[assignment] - right_trigger: Out[Bool] = None # type: ignore[assignment] + left_trigger: Out[Float32] = None # type: ignore[assignment] + right_trigger: Out[Float32] = None # type: ignore[assignment] def __init__(self, global_config: GlobalConfig | None = None, *args, **kwargs) -> None: super().__init__(*args, **kwargs) @@ -132,8 +132,8 @@ def __init__(self, global_config: GlobalConfig | None = None, *args, **kwargs) - # Latest controller data (absolute poses) self._left_pose: NDArray[np.float32] | None = None self._right_pose: NDArray[np.float32] | None = None - self._left_trigger_pressed: bool = False - self._right_trigger_pressed: bool = False + self._left_gripper_value: float = 0.0 # 0.0 to 1.0 + self._right_gripper_value: float = 0.0 # 0.0 to 1.0 # Tracking counters self._tracking_msg_count = 0 @@ -276,8 +276,8 @@ def get_status(self) -> dict: "right_arm_enabled": self.config.enable_right_arm, "has_left_data": self._left_pose is not None, "has_right_data": self._right_pose is not None, - "left_trigger_pressed": self._left_trigger_pressed, - "right_trigger_pressed": self._right_trigger_pressed, + "left_gripper_value": self._left_gripper_value, + "right_gripper_value": self._right_gripper_value, } # ========================================================================= @@ -313,9 +313,9 @@ def update_controller_poses( self._left_pose = left_pose self._right_pose = right_pose - # Convert gripper values to trigger booleans (threshold at 0.5) - self._left_trigger_pressed = left_gripper > 0.5 - self._right_trigger_pressed = right_gripper > 0.5 + # Store gripper values (0.0 to 1.0) + self._left_gripper_value = float(left_gripper) + self._right_gripper_value = float(right_gripper) # Only stream deltas if calibrated if not self._is_calibrated: @@ -391,18 +391,22 @@ def _compute_and_publish_deltas(self) -> None: except Exception as e: logger.error(f"Failed to publish right delta: {e}") - # Publish trigger states + # Publish gripper values (0.0 to 1.0) if self.left_trigger and hasattr(self.left_trigger, "publish"): try: - self.left_trigger.publish(Bool(data=self._left_trigger_pressed)) + self.left_trigger.publish(Float32(data=self._left_gripper_value)) + # Visualize gripper value in Rerun + self._visualize_gripper_in_rerun(self._left_gripper_value, "left") except Exception as e: - logger.debug(f"Failed to publish left trigger: {e}") + logger.debug(f"Failed to publish left gripper: {e}") if self.right_trigger and hasattr(self.right_trigger, "publish"): try: - self.right_trigger.publish(Bool(data=self._right_trigger_pressed)) + self.right_trigger.publish(Float32(data=self._right_gripper_value)) + # Visualize gripper value in Rerun + self._visualize_gripper_in_rerun(self._right_gripper_value, "right") except Exception as e: - logger.debug(f"Failed to publish right trigger: {e}") + logger.debug(f"Failed to publish right gripper: {e}") except Exception as e: logger.error(f"Error computing/publishing delta poses: {e}") @@ -486,3 +490,26 @@ def _visualize_controller_in_rerun(self, controller_pose: Pose, arm_side: str) - ) except Exception as e: logger.debug(f"Failed to log {arm_side} controller to Rerun: {e}") + + def _visualize_gripper_in_rerun(self, gripper_value: float, arm_side: str) -> None: + """Visualize gripper/trigger value in Rerun as a scalar time series. + + Args: + gripper_value: Gripper value (0.0-1.0) from VR controller + arm_side: "left" or "right" + """ + if not ( + self.config.visualize_in_rerun + and RERUN_AVAILABLE + and self._global_config.viewer_backend.startswith("rerun") + ): + return + + try: + # Log gripper value as scalar time series + rr.log( + f"world/teleop/{arm_side}_controller/gripper", + rr.Scalars(gripper_value), # type: ignore[attr-defined] + ) + except Exception as e: + logger.debug(f"Failed to log {arm_side} gripper to Rerun: {e}") From c0f9edfabf51a276dcc03a02cac7481476ab3244 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 22:24:53 +0000 Subject: [PATCH 23/72] Bool -> Float32: change update Former-commit-id: 3892e96e9a90f51f037aabb1dd8b022c0524ad7d Former-commit-id: 1cf6bdb10c2a70120d208bc1e9c41cff55cee950 --- dimos/teleop/teleop_blueprints.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/dimos/teleop/teleop_blueprints.py b/dimos/teleop/teleop_blueprints.py index 4523ce3141..0a1a1b1719 100644 --- a/dimos/teleop/teleop_blueprints.py +++ b/dimos/teleop/teleop_blueprints.py @@ -57,7 +57,7 @@ from dimos.core.blueprints import autoconnect from dimos.core.transport import LCMTransport from dimos.msgs.geometry_msgs import PoseStamped -from dimos.msgs.std_msgs import Bool +from dimos.msgs.std_msgs import Bool, Float32 from dimos.teleop.quest3.quest3_teleop_module import quest3_teleop_module from dimos.teleop.teleop_robot_controller import teleop_robot_controller @@ -103,9 +103,9 @@ ("right_controller_delta", PoseStamped): LCMTransport( "/quest3/right_controller_delta", PoseStamped ), - # Trigger states - ("left_trigger", Bool): LCMTransport("/quest3/left_trigger", Bool), - ("right_trigger", Bool): LCMTransport("/quest3/right_trigger", Bool), + # Gripper values (0.0-1.0) + ("left_trigger", Float32): LCMTransport("/quest3/left_trigger", Float32), + ("right_trigger", Float32): LCMTransport("/quest3/right_trigger", Float32), } ) From 495e66989d6ebdb108d0b36a37b6e9383e34f42d Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 22:25:02 +0000 Subject: [PATCH 24/72] Fix: No publishing before starting (pressing x to start) or after stopping (pressing x to end) Former-commit-id: a695f2a755749761e9a472157b1129883e7a1eed Former-commit-id: 8b60fd653ac57913d08b8fa4d8e225cdd43fa565 --- dimos/teleop/teleop_robot_controller.py | 80 +++++++++++++++++++++---- 1 file changed, 69 insertions(+), 11 deletions(-) diff --git a/dimos/teleop/teleop_robot_controller.py b/dimos/teleop/teleop_robot_controller.py index 3b6ba3f68f..3359183b56 100644 --- a/dimos/teleop/teleop_robot_controller.py +++ b/dimos/teleop/teleop_robot_controller.py @@ -36,7 +36,7 @@ from dimos.core import In, Module, Out, rpc from dimos.core.module import ModuleConfig from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Vector3 -from dimos.msgs.std_msgs import Bool +from dimos.msgs.std_msgs import Bool, Float32 from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -55,6 +55,9 @@ class TeleopRobotControllerConfig(ModuleConfig): enable_left_arm: bool = True enable_right_arm: bool = True + # Safety settings + delta_timeout: float = 1 # Seconds - stop publishing if no new delta received + class TeleopRobotController(Module): """Teleop Robot Controller - applies delta poses to robot. @@ -77,12 +80,14 @@ class TeleopRobotController(Module): # Input topics - receiving DELTA poses as PoseStamped left_controller_delta: In[PoseStamped] = None # type: ignore[assignment] right_controller_delta: In[PoseStamped] = None # type: ignore[assignment] - left_trigger: In[Bool] = None # type: ignore[assignment] - right_trigger: In[Bool] = None # type: ignore[assignment] + left_trigger: In[Float32] = None # type: ignore[assignment] # Gripper value 0.0-1.0 + right_trigger: In[Float32] = None # type: ignore[assignment] # Gripper value 0.0-1.0 # Output topics (Pose for commands) left_cartesian_command: Out[Pose] = None # type: ignore[assignment] right_cartesian_command: Out[Pose] = None # type: ignore[assignment] + left_gripper_command: Out[Bool] = None # type: ignore[assignment] + right_gripper_command: Out[Bool] = None # type: ignore[assignment] # RPC dependencies (dynamically set based on config) rpc_calls: list[str] = [] @@ -100,8 +105,12 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: # Latest delta data self._left_delta: PoseStamped | None = None self._right_delta: PoseStamped | None = None - self._left_trigger_pressed: bool = False - self._right_trigger_pressed: bool = False + self._left_gripper_value: float = 0.0 # 0.0 to 1.0 + self._right_gripper_value: float = 0.0 # 0.0 to 1.0 + + # Timestamps for delta timeout detection + self._left_delta_timestamp: float | None = None + self._right_delta_timestamp: float | None = None # Robot initial state (auto-calibrated on first delta) self._left_robot_initial_position: Vector3 | None = None @@ -190,6 +199,7 @@ def _on_left_delta(self, delta: PoseStamped) -> None: with self._state_lock: self._left_delta = delta + self._left_delta_timestamp = time.time() # Log first few deltas if not hasattr(self, "_left_delta_count"): @@ -217,6 +227,7 @@ def _on_right_delta(self, delta: PoseStamped) -> None: with self._state_lock: self._right_delta = delta + self._right_delta_timestamp = time.time() if not hasattr(self, "_right_delta_count"): self._right_delta_count = 0 @@ -228,15 +239,15 @@ def _on_right_delta(self, delta: PoseStamped) -> None: f"frame_id={delta.frame_id}" ) - def _on_left_trigger(self, trigger: Bool) -> None: - """Callback for left trigger.""" + def _on_left_trigger(self, gripper: Float32) -> None: + """Callback for left gripper value (0.0-1.0).""" with self._state_lock: - self._left_trigger_pressed = trigger.data + self._left_gripper_value = float(gripper.data) - def _on_right_trigger(self, trigger: Bool) -> None: - """Callback for right trigger.""" + def _on_right_trigger(self, gripper: Float32) -> None: + """Callback for right gripper value (0.0-1.0).""" with self._state_lock: - self._right_trigger_pressed = trigger.data + self._right_gripper_value = float(gripper.data) # ========================================================================= # Robot Calibration (Auto-triggered on first delta) @@ -383,10 +394,33 @@ def _control_loop(self) -> None: loop_count += 1 # Get latest state + current_time = time.time() with self._state_lock: left_delta = self._left_delta right_delta = self._right_delta robot_calibrated = self._robot_calibrated + left_delta_time = self._left_delta_timestamp + right_delta_time = self._right_delta_timestamp + + # Check for stale deltas (timeout) + delta_timeout = self.config.delta_timeout + if left_delta_time is not None: + if current_time - left_delta_time > delta_timeout: + logger.debug("Left delta timed out - clearing") + with self._state_lock: + self._left_delta = None + self._left_delta_timestamp = None + self._left_gripper_value = 0.0 # Clear gripper value too + left_delta = None + + if right_delta_time is not None: + if current_time - right_delta_time > delta_timeout: + logger.debug("Right delta timed out - clearing") + with self._state_lock: + self._right_delta = None + self._right_delta_timestamp = None + self._right_gripper_value = 0.0 # Clear gripper value too + right_delta = None # Log state periodically if loop_count <= 10 or loop_count % 100 == 0: @@ -405,6 +439,30 @@ def _control_loop(self) -> None: if self.config.enable_right_arm and right_delta is not None: self._apply_delta(right_delta, "right") + # Publish gripper commands (only when calibrated) + # Convert float (0.0-1.0) to Bool (threshold at 0.5) + with self._state_lock: + left_gripper_val = self._left_gripper_value + right_gripper_val = self._right_gripper_value + + if self.config.enable_left_arm: + if self.left_gripper_command and hasattr(self.left_gripper_command, "publish"): + try: + # Convert float to bool: > 0.5 = closed + left_gripper_closed = left_gripper_val > 0.5 + self.left_gripper_command.publish(Bool(data=left_gripper_closed)) + except Exception as e: + logger.debug(f"Failed to publish left gripper command: {e}") + + if self.config.enable_right_arm: + if self.right_gripper_command and hasattr(self.right_gripper_command, "publish"): + try: + # Convert float to bool: > 0.5 = closed + right_gripper_closed = right_gripper_val > 0.5 + self.right_gripper_command.publish(Bool(data=right_gripper_closed)) + except Exception as e: + logger.debug(f"Failed to publish right gripper command: {e}") + except Exception as e: logger.error(f"Error in control loop: {e}", exc_info=True) From 7a860bcd5d4448b525d84952b55999e38218800e Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 22:25:05 +0000 Subject: [PATCH 25/72] Added folder structure Former-commit-id: 6d4b98b6e385b30c62d51fa46d5815e44d49f484 Former-commit-id: a7c616a6dc0deb810293f6fa9d7e81570ed77eac --- dimos/teleop/README.md | 39 ++++++++++++++++++++++++++++++++------- 1 file changed, 32 insertions(+), 7 deletions(-) diff --git a/dimos/teleop/README.md b/dimos/teleop/README.md index 87f71a4545..c572236652 100644 --- a/dimos/teleop/README.md +++ b/dimos/teleop/README.md @@ -9,19 +9,44 @@ The teleoperation system consists of: - **Device modules** (e.g., [`quest3/`](quest3/)) - Device-specific implementations - **Robot controller** ([`teleop_robot_controller.py`](teleop_robot_controller.py)) - Applies deltas to robot +## Folder Structure + +``` +teleop/ +โ”œโ”€โ”€ __init__.py # Package exports +โ”œโ”€โ”€ README.md # This file +โ”œโ”€โ”€ base/ # Base teleoperation module +โ”‚ โ”œโ”€โ”€ __init__.py +โ”‚ โ”œโ”€โ”€ base_teleop_module.py # BaseTeleopModule (calibration, deltas, publishing) +โ”‚ โ””โ”€โ”€ README.md # Guide for creating new devices +โ”œโ”€โ”€ quest3/ # Quest 3 VR implementation +โ”‚ โ”œโ”€โ”€ __init__.py +โ”‚ โ”œโ”€โ”€ quest3_teleop_module.py # Quest3TeleopModule (WebSocket server) +โ”‚ โ”œโ”€โ”€ README.md # Quest3 setup and usage +โ”‚ โ”œโ”€โ”€ control/ +โ”‚ โ”‚ โ”œโ”€โ”€ fastapi_server.py # FastAPI/WebSocket server +โ”‚ โ”‚ โ””โ”€โ”€ tracking_processor.py # VR tracking data processing +โ”‚ โ”œโ”€โ”€ static/ +โ”‚ โ”‚ โ””โ”€โ”€ index.html # VR client (HTML/JS) +โ”‚ โ””โ”€โ”€ certs/ # SSL certificates (auto-generated) +โ”œโ”€โ”€ teleop_robot_controller.py # Applies deltas to robot +โ””โ”€โ”€ teleop_blueprints.py # Pre-built system blueprints +``` + ## Architecture ``` -Device Module (Quest3, SpaceMouse, etc.) - โ”œโ”€โ”€ Inherits from BaseTeleopModule - โ”œโ”€โ”€ Device-specific connection (WebSocket, USB, etc.) - โ””โ”€โ”€ Calls: update_controller_poses() - โ†“ -BaseTeleopModule +BaseTeleopModule (base class) โ”œโ”€โ”€ Calibration (capture initial poses) โ”œโ”€โ”€ Delta computation (current - initial) โ”œโ”€โ”€ Publishes delta poses (LCM) - โ””โ”€โ”€ Rerun visualization + โ”œโ”€โ”€ Rerun visualization + โ””โ”€โ”€ update_controller_poses() โ† Device modules call this + โ†‘ +Device Module (Quest3, SpaceMouse, etc.) + โ”œโ”€โ”€ Inherits from BaseTeleopModule + โ”œโ”€โ”€ Device-specific connection (WebSocket, USB, etc.) + โ””โ”€โ”€ Receives tracking data โ†’ calls update_controller_poses() โ†“ TeleopRobotController โ”œโ”€โ”€ Receives delta poses From aaef8203e6ddfb544a797cc2bc9c2ed284cd8946 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam <63036454+ruthwikdasyam@users.noreply.github.com> Date: Thu, 8 Jan 2026 22:25:18 +0000 Subject: [PATCH 26/72] CI code cleanup Former-commit-id: d486cf741d971f363aa73d9c678c6c23ac4fea5d Former-commit-id: da0a3bad11b3dab5211cdacfbf49f5568b03d333 --- dimos/msgs/std_msgs/Float32.py | 1 - dimos/teleop/teleop_blueprints.py | 6 +++--- dimos/teleop/teleop_robot_controller.py | 8 ++++++-- 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/dimos/msgs/std_msgs/Float32.py b/dimos/msgs/std_msgs/Float32.py index 94a2ca6551..cb6d71e08f 100644 --- a/dimos/msgs/std_msgs/Float32.py +++ b/dimos/msgs/std_msgs/Float32.py @@ -56,4 +56,3 @@ def to_ros_msg(self) -> ROSFloat32: ros_msg = ROSFloat32() # type: ignore[no-untyped-call] ros_msg.data = float(self.data) return ros_msg - diff --git a/dimos/teleop/teleop_blueprints.py b/dimos/teleop/teleop_blueprints.py index 0a1a1b1719..c60fa6743a 100644 --- a/dimos/teleop/teleop_blueprints.py +++ b/dimos/teleop/teleop_blueprints.py @@ -103,9 +103,9 @@ ("right_controller_delta", PoseStamped): LCMTransport( "/quest3/right_controller_delta", PoseStamped ), - # Gripper values (0.0-1.0) - ("left_trigger", Float32): LCMTransport("/quest3/left_trigger", Float32), - ("right_trigger", Float32): LCMTransport("/quest3/right_trigger", Float32), + # Gripper values (0.0-1.0) + ("left_trigger", Float32): LCMTransport("/quest3/left_trigger", Float32), + ("right_trigger", Float32): LCMTransport("/quest3/right_trigger", Float32), } ) diff --git a/dimos/teleop/teleop_robot_controller.py b/dimos/teleop/teleop_robot_controller.py index 3359183b56..4d3e2ce00f 100644 --- a/dimos/teleop/teleop_robot_controller.py +++ b/dimos/teleop/teleop_robot_controller.py @@ -446,7 +446,9 @@ def _control_loop(self) -> None: right_gripper_val = self._right_gripper_value if self.config.enable_left_arm: - if self.left_gripper_command and hasattr(self.left_gripper_command, "publish"): + if self.left_gripper_command and hasattr( + self.left_gripper_command, "publish" + ): try: # Convert float to bool: > 0.5 = closed left_gripper_closed = left_gripper_val > 0.5 @@ -455,7 +457,9 @@ def _control_loop(self) -> None: logger.debug(f"Failed to publish left gripper command: {e}") if self.config.enable_right_arm: - if self.right_gripper_command and hasattr(self.right_gripper_command, "publish"): + if self.right_gripper_command and hasattr( + self.right_gripper_command, "publish" + ): try: # Convert float to bool: > 0.5 = closed right_gripper_closed = right_gripper_val > 0.5 From 7214540a2fbb5ddfaa98575759a9ea7dd3ee522e Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 22:28:42 +0000 Subject: [PATCH 27/72] docs: updated Readme Former-commit-id: d83158a01986df8deafc067182b2e841a5f311ec Former-commit-id: d1de48c3d9562582b757b45a75e61deb73f7b71d --- dimos/teleop/README.md | 101 +++++++--------------------------- dimos/teleop/quest3/README.md | 75 +++++-------------------- 2 files changed, 32 insertions(+), 144 deletions(-) diff --git a/dimos/teleop/README.md b/dimos/teleop/README.md index c572236652..ef8b77c61a 100644 --- a/dimos/teleop/README.md +++ b/dimos/teleop/README.md @@ -65,56 +65,34 @@ Robot Driver โ†’ Robot **Features**: AR mode, dual-arm control, trigger buttons ```python -from dimos.teleop.quest3 import Quest3TeleopModule +from dimos.teleop.teleop_blueprints import quest3_teleop -teleop = Quest3TeleopModule() -teleop.start() +coordinator = quest3_teleop.build() +coordinator.loop() # Open https://your-ip:8443 on Quest 3 ``` -See [Quest3 README](quest3/README.md) for details. - ## Quick Start -### 1. Choose Your Device +**Use the pre-built blueprint** - it's ready to run: ```python -# Quest 3 VR -from dimos.teleop.quest3 import Quest3TeleopModule -device = Quest3TeleopModule() - -# Future: SpaceMouse, other VR headsets, etc. -``` - -### 2. Start Teleoperation +from dimos.teleop.teleop_blueprints import quest3_teleop -```python -device.start() -# Press calibration button (X button for Quest3) -# Move controllers to control robot +# Pre-built blueprint with Quest3 + TeleopRobotController +coordinator = quest3_teleop.build() +coordinator.loop() ``` -### 3. Use with Blueprint - -```python -from dimos.core.blueprints import autoconnect -from dimos.teleop.quest3 import quest3_teleop_module -from dimos.teleop import teleop_robot_controller - -system = autoconnect( - quest3_teleop_module( - enable_left_arm=True, - enable_right_arm=True, - ), - teleop_robot_controller( - driver_module_name="YourDriver", - ), - your_robot_blueprint, -) +That's it! The blueprint includes: +- Quest3TeleopModule (VR calibration, delta computation) +- TeleopRobotController (robot calibration, delta application) +- All LCM topic connections configured -coordinator = system.build() -coordinator.loop() -``` +Then: +1. Open `https://your-ip:8443/` on Quest 3 +2. Press X button to calibrate and start teleoperation +3. Move controllers to control robot ## How It Works @@ -140,18 +118,6 @@ Controllers โ†’ Device Module โ†’ Delta Poses โ†’ Arm Controller โ†’ Robot - โœ… Robot-agnostic (works with any robot) - โœ… Recalibrable (just press button again) -## Creating New Devices - -See [`base/README.md`](base/README.md) for instructions on creating new teleoperation devices. - -Example devices you could add: -- SpaceMouse (USB 6DOF controller) -- Other VR headsets (HTC Vive, Valve Index) -- Haptic devices (Force Dimension Omega) -- Game controllers with custom mapping -- Mobile phone IMU-based control - - ## LCM Topics @@ -159,23 +125,9 @@ All devices publish: - `left_controller_delta: Out[PoseStamped]` - Left delta pose - `right_controller_delta: Out[PoseStamped]` - Right delta pose -- `left_trigger: Out[Bool]` - Left trigger state -- `right_trigger: Out[Bool]` - Right trigger state - -## Components - -- **[`base/`](base/)** - Base teleoperation module (abstract) -- **[`quest3/`](quest3/)** - Quest 3 VR implementation -- **[`teleop_robot_controller.py`](teleop_robot_controller.py)** - Applies deltas to robot -- **[`teleop_blueprints.py`](teleop_blueprints.py)** - Pre-built system blueprints - -## Benefits of This Architecture +- `left_trigger: Out[Float32]` - Left trigger/gripper value (0.0-1.0) +- `right_trigger: Out[Float32]` - Right trigger/gripper value (0.0-1.0) -1. **Reusability**: Common code shared across all devices -2. **Consistency**: Same behavior and interface for all devices -3. **Maintainability**: Bug fixes in base class benefit all devices -4. **Rapid Development**: New devices in ~50-100 lines of code -5. **Device Agnostic**: Robot doesn't know/care what device is used ## Example: Full System @@ -199,7 +151,7 @@ from dimos.teleop import teleop_robot_controller my_system = ( autoconnect( quest3_teleop_module(signaling_port=8443), - teleop_robot_controller(driver_module_name="XArmDriver"), + teleop_robot_controller(driver_module_name="RobotDriver"), your_robot, ) .transports({ @@ -212,21 +164,6 @@ coordinator = my_system.build() coordinator.loop() ``` -## Troubleshooting - -### Device Not Publishing Deltas -1. Press calibration button to capture initial poses -2. Check `is_vr_calibrated()` returns True -3. Move controllers and check LCM topics - -### Robot Not Moving -1. Ensure TeleopRobotController received first delta (auto-calibrates) -2. Check robot driver is running -3. Verify LCM topic connections in blueprint - -### Want to Recalibrate -Just call `reset_calibration()` and press calibration button again. - ## Related Documentation - [Base Module](base/README.md) - Creating new devices diff --git a/dimos/teleop/quest3/README.md b/dimos/teleop/quest3/README.md index f6b0476f3d..c226574d97 100644 --- a/dimos/teleop/quest3/README.md +++ b/dimos/teleop/quest3/README.md @@ -44,55 +44,32 @@ teleop.start() ``` Quest 3 Browser - โ†“ Opens: https://your-ip:8443/ - โ†“ Loads: Standalone HTML/JS VR client (no build step!) - โ†“ Connects: wss://your-ip:8443/ws - โ†“ X button pressed โ†’ calibrate_vr() via WebSocket + โ†“ Opens https://your-ip:8443/ (WebXR) + โ†“ X button โ†’ calibrate_vr() via WebSocket โ†“ -FastAPI Server (Quest3-specific) - โ†“ Serves static HTML + WebSocket - โ†“ Auto-generates SSL certificates - โ†“ Receives tracking data (left/right controller poses) - โ†“ Calls: update_controller_poses() +FastAPI Server + โ†“ Receives tracking data (controller poses) โ†“ Quest3TeleopModule (inherits BaseTeleopModule) - โ”œโ”€โ”€ FastAPI/WebSocket server (Quest3-specific) - โ”œโ”€โ”€ X button handler (Quest3-specific) - โ””โ”€โ”€ Calibration, delta computation, publishing (from base) - โ†“ Publishes delta pose topics (PoseStamped) - โ†“ + โ†“ Computes delta poses (current - initial) + โ†“ Publishes delta poses (PoseStamped) + โ†“ TeleopRobotController - โ†“ Receives delta poses from Quest3TeleopModule โ†“ Auto-calibrates robot on first delta - โ†“ Calculates: target = initial_robot + delta + โ†“ Applies: target = initial_robot + delta โ†“ Publishes cartesian commands (Pose) โ†“ Robot Driver - โ†“ Executes cartesian commands + โ†“ Executes commands ``` -### What's Inherited from BaseTeleopModule - -Quest3TeleopModule inherits these features: -- โœ… Calibration logic (capture initial controller poses) -- โœ… Delta pose computation (current - initial) -- โœ… LCM publishing (delta poses + trigger states) -- โœ… Rerun visualization -- โœ… Standard RPC interface - -Quest3-specific implementation: -- ๐Ÿ”ง FastAPI/WebSocket server for VR headset -- ๐Ÿ”ง HTTPS certificate generation -- ๐Ÿ”ง X button handler ## Key Features -โœ… Pure Python + vanilla JS -โœ… Press X to start/stop teleoperation -โœ… Captures initial poses on first X press -โœ… Robot follows controller movement relative to initial pose -โœ… Low latency controller data -โœ… AR mode +Press X to start/stop teleoperation +Captures initial poses on first X press +Robot follows controller movement relative to initial pose +AR mode ## Configuration @@ -123,16 +100,6 @@ module.start() ``` -## RPC Methods - -All inherited from `BaseTeleopModule`: -- `start()` โ†’ Start module and FastAPI server -- `stop()` โ†’ Stop module and server -- `calibrate_vr()` โ†’ Calibrate VR (capture initial controller poses) -- `reset_calibration()` โ†’ Reset VR calibration -- `is_vr_calibrated()` โ†’ Check if VR is calibrated -- `get_status()` โ†’ Get teleoperation status - ## WebSocket Protocol ### Handshake @@ -176,24 +143,8 @@ Inherited from `BaseTeleopModule`: **Note**: These are **delta poses**, not absolute poses. TeleopRobotController applies these deltas to the robot's initial pose. -### WebSocket Connection Fails -**Check**: -1. Server is running: `curl https://your-ip:8443/health` -2. Firewall allows port: `sudo ufw allow 8443` -3. Same network (Quest 3 and server) -4. Correct IP (not localhost) - -### No Tracking Data / Robot Not Moving -1. **Press X button** to calibrate VR (captures initial controller poses) -2. **Move controllers** - deltas are published automatically after calibration -3. Verify WebSocket connection in browser console -4. Check server logs: `is_vr_calibrated()` should return True -5. TeleopRobotController auto-calibrates robot on first delta -6. Ensure controllers are being tracked in VR - ## Development - ### Standalone Server Testing ```bash cd dimos/teleop/quest3/control From 9f724c891e2f3e3179d2cec3692cb26471234df8 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 22:31:15 +0000 Subject: [PATCH 28/72] Misc: init updates Former-commit-id: 14dbb66064cd8b1f00641efb96526d791365e554 Former-commit-id: 0b5e5c35307cedd788b25c32000d838ac83db025 --- dimos/teleop/__init__.py | 3 +++ dimos/teleop/quest3/__init__.py | 14 ++++++++++++++ 2 files changed, 17 insertions(+) diff --git a/dimos/teleop/__init__.py b/dimos/teleop/__init__.py index 089ff68c99..a098d2cce7 100644 --- a/dimos/teleop/__init__.py +++ b/dimos/teleop/__init__.py @@ -15,8 +15,11 @@ """Teleoperation modules for dimos.""" from dimos.teleop.base import BaseTeleopConfig, BaseTeleopModule +from dimos.teleop.quest3 import Quest3TeleopConfig, Quest3TeleopModule __all__ = [ "BaseTeleopConfig", "BaseTeleopModule", + "Quest3TeleopConfig", + "Quest3TeleopModule", ] diff --git a/dimos/teleop/quest3/__init__.py b/dimos/teleop/quest3/__init__.py index 488e40e8be..181ff28410 100644 --- a/dimos/teleop/quest3/__init__.py +++ b/dimos/teleop/quest3/__init__.py @@ -1,3 +1,17 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + """Quest3 VR Teleoperation.""" from dimos.teleop.quest3.quest3_teleop_module import Quest3TeleopConfig, Quest3TeleopModule From de6ea0f8abbabc09c8bfa1f2c28da05e37ddd53c Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 22:35:08 +0000 Subject: [PATCH 29/72] Fix: comments update Former-commit-id: 5a51daad3845a8ef2363e9dfa66d44da5ccfbfb6 Former-commit-id: 6bd40f0edcc98b0c6f8e0f2fd73e6f7c45058f22 --- dimos/teleop/quest3/control/fastapi_server.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/dimos/teleop/quest3/control/fastapi_server.py b/dimos/teleop/quest3/control/fastapi_server.py index 466100cb82..b06e63c8ac 100644 --- a/dimos/teleop/quest3/control/fastapi_server.py +++ b/dimos/teleop/quest3/control/fastapi_server.py @@ -134,8 +134,7 @@ class TeleopFastAPIServer: def __init__( self, host: str = "0.0.0.0", - port: int = 8443, # Changed default to 8443 for HTTPS - # why 8443 ? + port: int = 8443, # Standard HTTPS port (alternative to 443). Any port works with HTTPS. use_https: bool = True, ): """Initialize the FastAPI server. From 7045b3c23f3bc9c3faf7f8b8043475575b5b0a6e Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 22:43:57 +0000 Subject: [PATCH 30/72] Fix: disconnect requires websocket input Former-commit-id: 99c8c9c8e5aeaa18df2bdbc75a4a9c0405df7587 Former-commit-id: 2556574793ee1d071e4e1bc87f4fbeefa095461e --- dimos/teleop/quest3/control/fastapi_server.py | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/dimos/teleop/quest3/control/fastapi_server.py b/dimos/teleop/quest3/control/fastapi_server.py index b06e63c8ac..cc47fbafbd 100644 --- a/dimos/teleop/quest3/control/fastapi_server.py +++ b/dimos/teleop/quest3/control/fastapi_server.py @@ -400,10 +400,17 @@ def run(self) -> None: uvicorn.run(**run_kwargs) async def stop_async(self) -> None: - # close all connections - await self.manager.disconnect() - """Stop the FastAPI server asynchronously.""" + # Close all connections + with self.manager.active_connections_lock: + connections_to_close = list(self.manager.active_connections) + for websocket in connections_to_close: + self.manager.disconnect(websocket) + try: + await websocket.close() + except Exception as e: + logger.debug(f"Error closing websocket: {e}") + if self.server: logger.info("Stopping FastAPI server...") self.server.should_exit = True From 9e8cc390e44b5e506e545202684021b8e92e74ff Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 23:01:07 +0000 Subject: [PATCH 31/72] Fix: update path for modules + File names Former-commit-id: 2eb04dba57a8cdf1f6674ee61668958355ac0175 Former-commit-id: 8d2663594f6f8e56b82461589952fda32cd5d37e --- dimos/robot/all_blueprints.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 95cb9948ed..ccae6c5348 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -90,8 +90,8 @@ "cartesian_motion_controller": "dimos.manipulation.control.servo_control.cartesian_motion_controller", "joint_trajectory_controller": "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller", # Teleop modules - "quest3_teleop_module": "dimos.teleop.quest3.teleop_module", - "teleop_arm_controller": "dimos.teleop.teleop_arm_controller", + "quest3_teleop_module": "dimos.teleop.quest3.quest3_teleop_module", + "teleop_robot_controller": "dimos.teleop.teleop_robot_controller", } From 5fd45b31ca49fd7ef3f7c9692820ee260c56fb87 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 8 Jan 2026 23:01:09 +0000 Subject: [PATCH 32/72] Fix: mypy errors Former-commit-id: 24678f220ecab68bbcfff924f828e48b121a1b7e Former-commit-id: 5020a7fea579b73072a2f2266926b36dc984941d --- dimos/teleop/base/base_teleop_module.py | 15 ++++- dimos/teleop/quest3/control/fastapi_server.py | 55 ++++++++++++------- .../quest3/control/tracking_processor.py | 4 +- dimos/teleop/quest3/quest3_teleop_module.py | 16 ++++-- dimos/teleop/teleop_robot_controller.py | 13 +++-- 5 files changed, 69 insertions(+), 34 deletions(-) diff --git a/dimos/teleop/base/base_teleop_module.py b/dimos/teleop/base/base_teleop_module.py index 62e92db966..50d57c0b72 100644 --- a/dimos/teleop/base/base_teleop_module.py +++ b/dimos/teleop/base/base_teleop_module.py @@ -108,6 +108,7 @@ class BaseTeleopModule(Module, ABC): """ default_config = BaseTeleopConfig + config: BaseTeleopConfig # LCM Output topics left_controller_delta: Out[PoseStamped] = None # type: ignore[assignment] @@ -115,7 +116,9 @@ class BaseTeleopModule(Module, ABC): left_trigger: Out[Float32] = None # type: ignore[assignment] right_trigger: Out[Float32] = None # type: ignore[assignment] - def __init__(self, global_config: GlobalConfig | None = None, *args, **kwargs) -> None: + def __init__( + self, global_config: GlobalConfig | None = None, *args: Any, **kwargs: Any + ) -> None: super().__init__(*args, **kwargs) # Get global config for Rerun connection @@ -163,6 +166,12 @@ def start(self) -> None: connect_rerun(global_config=self._global_config) logger.info("Connected to Rerun for controller visualization") + @rpc + def stop(self) -> None: + """Stop the teleoperation module.""" + logger.info(f"Stopping {self.__class__.__name__}...") + super().stop() + # ========================================================================= # Calibration # ========================================================================= @@ -263,7 +272,7 @@ def is_vr_calibrated(self) -> bool: return self._is_calibrated @rpc - def get_status(self) -> dict: + def get_status(self) -> dict[str, Any]: """Get current teleoperation status. Returns: @@ -477,7 +486,7 @@ def _visualize_controller_in_rerun(self, controller_pose: Pose, arm_side: str) - # Log absolute controller pose transform rr.log( f"world/teleop/{arm_side}_controller", - controller_pose_stamped.to_rerun(), + controller_pose_stamped.to_rerun(), # type: ignore[no-untyped-call] ) # Log coordinate frame axes to visualize the transform rr.log( diff --git a/dimos/teleop/quest3/control/fastapi_server.py b/dimos/teleop/quest3/control/fastapi_server.py index cc47fbafbd..6ef9b5f000 100644 --- a/dimos/teleop/quest3/control/fastapi_server.py +++ b/dimos/teleop/quest3/control/fastapi_server.py @@ -29,7 +29,7 @@ import subprocess import threading import time -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Any from fastapi import FastAPI, WebSocket, WebSocketDisconnect from fastapi.middleware.cors import CORSMiddleware @@ -40,9 +40,10 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from collections.abc import Callable + from collections.abc import Awaitable, Callable - from dimos.msgs.geometry_msgs import Pose + import numpy as np + from numpy.typing import NDArray logger = setup_logger() @@ -53,11 +54,13 @@ class ConnectionManager: """Manages active WebSocket connections for teleoperation.""" - def __init__(self): + def __init__(self) -> None: self.active_connections: list[WebSocket] = [] self.active_connections_lock = threading.Lock() - self.data_callback: Callable[[Pose, Pose, float, float]] | None = None - self.command_callback: Callable[[str, WebSocket]] | None = None + self.data_callback: Callable[ + [NDArray[np.float32], NDArray[np.float32], float, float], None + ] | None = None + self.command_callback: Callable[[str, WebSocket], Awaitable[dict[str, Any]]] | None = None async def connect(self, websocket: WebSocket) -> None: """Accept and register a new WebSocket connection. @@ -81,7 +84,10 @@ def disconnect(self, websocket: WebSocket) -> None: self.active_connections.remove(websocket) logger.info(f"Client disconnected (remaining: {len(self.active_connections)})") - def set_callback(self, callback: Callable) -> None: + def set_callback( + self, + callback: Callable[[NDArray[np.float32], NDArray[np.float32], float, float], None], + ) -> None: """Set callback function to send controller data to teleop module. Args: @@ -90,7 +96,9 @@ def set_callback(self, callback: Callable) -> None: self.data_callback = callback logger.info("Controller data callback registered") - def set_command_callback(self, callback: Callable) -> None: + def set_command_callback( + self, callback: Callable[[str, WebSocket], Awaitable[dict[str, Any]]] + ) -> None: """Set callback function to handle teleop commands (start_teleop/stop_teleop). Args: @@ -99,7 +107,7 @@ def set_command_callback(self, callback: Callable) -> None: self.command_callback = callback logger.info("Command callback registered") - async def broadcast(self, message: dict) -> None: + async def broadcast(self, message: dict[str, Any]) -> None: """Broadcast a message to all connected clients. Args: @@ -222,7 +230,7 @@ def _setup_routes(self) -> None: """Setup FastAPI routes and WebSocket endpoints.""" @self.app.get("/") - async def root(): + async def root() -> FileResponse | dict[str, Any]: """Serve the VR client HTML page.""" html_file = self.static_dir / "index.html" if html_file.exists(): @@ -236,7 +244,7 @@ async def root(): } @self.app.get("/health") - async def health(): + async def health() -> dict[str, Any]: """Health check endpoint.""" return { "status": "healthy", @@ -244,7 +252,7 @@ async def health(): } @self.app.websocket("/ws") - async def websocket_endpoint(websocket: WebSocket): + async def websocket_endpoint(websocket: WebSocket) -> None: """WebSocket endpoint for VR teleoperation tracking data. Args: @@ -336,7 +344,10 @@ async def websocket_endpoint(websocket: WebSocket): self.manager.disconnect(websocket) logger.info(f"Connection closed after {message_count} messages") - def set_callback(self, callback: Callable) -> None: + def set_callback( + self, + callback: Callable[[NDArray[np.float32], NDArray[np.float32], float, float], None], + ) -> None: """Set callback function for controller data. Args: @@ -344,7 +355,9 @@ def set_callback(self, callback: Callable) -> None: """ self.manager.set_callback(callback) - def set_command_callback(self, callback: Callable) -> None: + def set_command_callback( + self, callback: Callable[[str, WebSocket], Awaitable[dict[str, Any]]] + ) -> None: """Set callback function for teleop commands. Args: @@ -354,7 +367,7 @@ def set_command_callback(self, callback: Callable) -> None: async def start_async(self) -> None: """Start the FastAPI server asynchronously.""" - config_kwargs = { + config_kwargs: dict[str, Any] = { "app": self.app, "host": self.host, "port": self.port, @@ -372,7 +385,7 @@ async def start_async(self) -> None: protocol = "http" ws_protocol = "ws" - config = uvicorn.Config(**config_kwargs) + config = uvicorn.Config(**config_kwargs) # type: ignore[misc] self.server = uvicorn.Server(config) logger.info(f"FastAPI server starting on {protocol}://{self.host}:{self.port}") @@ -386,7 +399,7 @@ def run(self) -> None: This is for standalone testing only. """ - run_kwargs = { + run_kwargs: dict[str, Any] = { "app": self.app, "host": self.host, "port": self.port, @@ -397,7 +410,7 @@ def run(self) -> None: run_kwargs["ssl_keyfile"] = str(self.key_file) run_kwargs["ssl_certfile"] = str(self.cert_file) - uvicorn.run(**run_kwargs) + uvicorn.run(**run_kwargs) # type: ignore[misc] async def stop_async(self) -> None: """Stop the FastAPI server asynchronously.""" @@ -416,7 +429,7 @@ async def stop_async(self) -> None: self.server.should_exit = True await asyncio.sleep(0.1) # Give time for graceful shutdown - async def broadcast_robot_state(self, state: dict) -> None: + async def broadcast_robot_state(self, state: dict[str, Any]) -> None: """Broadcast robot state to all connected clients. Args: @@ -433,7 +446,9 @@ async def broadcast_robot_state(self, state: dict) -> None: level=logging.INFO, format="%(asctime)s - %(name)s - %(levelname)s - %(message)s" ) - def test_callback(left_pose, right_pose, left_gripper, right_gripper): + def test_callback( + left_pose: Any, right_pose: Any, left_gripper: float, right_gripper: float + ) -> None: """Test callback for tracking data.""" print(f"Left pose: {left_pose[:3, 3]}, gripper: {left_gripper}") print(f"Right pose: {right_pose[:3, 3]}, gripper: {right_gripper}") diff --git a/dimos/teleop/quest3/control/tracking_processor.py b/dimos/teleop/quest3/control/tracking_processor.py index 0af1dd40d6..111eaa08ff 100644 --- a/dimos/teleop/quest3/control/tracking_processor.py +++ b/dimos/teleop/quest3/control/tracking_processor.py @@ -25,7 +25,7 @@ from typing import TYPE_CHECKING, Any import numpy as np -from scipy.spatial.transform import Rotation +from scipy.spatial.transform import Rotation # type: ignore[import-untyped] from dimos.utils.logging_config import setup_logger @@ -158,5 +158,5 @@ def _extract_gripper_value(self, tracking_data: dict[str, Any]) -> float: # VR trigger value is already 0.0-1.0, just convert to float return float(gripper_value) except (ValueError, TypeError): - logger.warning("Invalid trigger value: %s, using 0.0", gripper_value) + logger.warning("Invalid trigger value: %s", gripper_value) return 0.0 diff --git a/dimos/teleop/quest3/quest3_teleop_module.py b/dimos/teleop/quest3/quest3_teleop_module.py index 582039d596..13f2280c9f 100644 --- a/dimos/teleop/quest3/quest3_teleop_module.py +++ b/dimos/teleop/quest3/quest3_teleop_module.py @@ -99,9 +99,15 @@ class Quest3TeleopModule(BaseTeleopModule): """ default_config = Quest3TeleopConfig + config: Quest3TeleopConfig - def __init__(self, global_config: GlobalConfig | None = None, *args, **kwargs) -> None: - super().__init__(global_config=global_config, *args, **kwargs) + def __init__( + self, global_config: GlobalConfig | None = None, *args: Any, **kwargs: Any + ) -> None: + # Remove global_config from kwargs to avoid passing it twice + kwargs.pop("global_config", None) + # Pass global_config as positional argument to match base class signature + super().__init__(global_config, *args, **kwargs) # Quest3-specific: FastAPI WebSocket server self._fastapi_server: TeleopFastAPIServer | None = None @@ -147,7 +153,7 @@ def stop(self) -> None: def _start_signaling_server(self) -> None: """Start the FastAPI WebSocket server in a background thread.""" - def run_server(): + def run_server() -> None: # Create new event loop for this thread loop = asyncio.new_event_loop() asyncio.set_event_loop(loop) @@ -198,7 +204,9 @@ def _stop_signaling_server(self) -> None: # Quest3-Specific: X Button Handler # ========================================================================= - async def _handle_x_button(self, command_type: str, websocket) -> dict: + async def _handle_x_button( + self, command_type: str, websocket: Any + ) -> dict[str, Any]: """Handle X button press from VR client. X button toggles calibration: diff --git a/dimos/teleop/teleop_robot_controller.py b/dimos/teleop/teleop_robot_controller.py index 4d3e2ce00f..70b7ea48f9 100644 --- a/dimos/teleop/teleop_robot_controller.py +++ b/dimos/teleop/teleop_robot_controller.py @@ -29,10 +29,13 @@ import threading import time import traceback -from typing import Any +from typing import TYPE_CHECKING, Any import numpy as np +if TYPE_CHECKING: + from numpy.typing import NDArray + from dimos.core import In, Module, Out, rpc from dimos.core.module import ModuleConfig from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Vector3 @@ -120,7 +123,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: self._robot_calibrated = False # State for unwrapped logging - self._last_logged_rpy = {} + self._last_logged_rpy: dict[str, NDArray[np.float64]] = {} # Control loop self._control_thread: threading.Thread | None = None @@ -364,7 +367,7 @@ def is_robot_calibrated(self) -> bool: return self._robot_calibrated @rpc - def get_status(self) -> dict: + def get_status(self) -> dict[str, Any]: """Get controller status. Returns: @@ -521,8 +524,8 @@ def _apply_delta(self, delta: PoseStamped, arm_side: str) -> None: target_yaw = robot_initial_rpy.z + delta_euler.z # Wrap angles to [-ฯ€, ฯ€] - def wrap_angle(angle): - return np.arctan2(np.sin(angle), np.cos(angle)) + def wrap_angle(angle: float) -> float: + return float(np.arctan2(np.sin(angle), np.cos(angle))) target_roll = wrap_angle(target_roll) target_pitch = wrap_angle(target_pitch) From 3b741a904d1c2ab6c849061e340a6912f39a90a3 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam <63036454+ruthwikdasyam@users.noreply.github.com> Date: Thu, 8 Jan 2026 23:01:19 +0000 Subject: [PATCH 33/72] CI code cleanup Former-commit-id: 456828350f557e5eb9bde9bf316242c3bd44cdf5 Former-commit-id: 6e2d4e40fbcb3e8c81176fdeb843bb5bef6a9fbe --- dimos/teleop/quest3/control/fastapi_server.py | 6 +++--- dimos/teleop/quest3/quest3_teleop_module.py | 4 +--- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/dimos/teleop/quest3/control/fastapi_server.py b/dimos/teleop/quest3/control/fastapi_server.py index 6ef9b5f000..ec9caf557b 100644 --- a/dimos/teleop/quest3/control/fastapi_server.py +++ b/dimos/teleop/quest3/control/fastapi_server.py @@ -57,9 +57,9 @@ class ConnectionManager: def __init__(self) -> None: self.active_connections: list[WebSocket] = [] self.active_connections_lock = threading.Lock() - self.data_callback: Callable[ - [NDArray[np.float32], NDArray[np.float32], float, float], None - ] | None = None + self.data_callback: ( + Callable[[NDArray[np.float32], NDArray[np.float32], float, float], None] | None + ) = None self.command_callback: Callable[[str, WebSocket], Awaitable[dict[str, Any]]] | None = None async def connect(self, websocket: WebSocket) -> None: diff --git a/dimos/teleop/quest3/quest3_teleop_module.py b/dimos/teleop/quest3/quest3_teleop_module.py index 13f2280c9f..9511c4b2d0 100644 --- a/dimos/teleop/quest3/quest3_teleop_module.py +++ b/dimos/teleop/quest3/quest3_teleop_module.py @@ -204,9 +204,7 @@ def _stop_signaling_server(self) -> None: # Quest3-Specific: X Button Handler # ========================================================================= - async def _handle_x_button( - self, command_type: str, websocket: Any - ) -> dict[str, Any]: + async def _handle_x_button(self, command_type: str, websocket: Any) -> dict[str, Any]: """Handle X button press from VR client. X button toggles calibration: From d1fdc101d00a7d24012518c0f0b457b08663cb3b Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Fri, 9 Jan 2026 17:59:59 -0800 Subject: [PATCH 34/72] Feat: Add sub: quaternions need subract method --- dimos/msgs/geometry_msgs/Pose.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/dimos/msgs/geometry_msgs/Pose.py b/dimos/msgs/geometry_msgs/Pose.py index bf6a821cc8..458b2d8633 100644 --- a/dimos/msgs/geometry_msgs/Pose.py +++ b/dimos/msgs/geometry_msgs/Pose.py @@ -222,6 +222,21 @@ def __add__(self, other: Pose | PoseConvertable | LCMTransform | Transform) -> P return Pose(new_position, new_orientation) + def __sub__(self, other: Pose | PoseConvertable) -> Pose: + """Compute the delta pose: self - other. + + For position: simple subtraction. + For orientation: delta_quat = self.orientation * inverse(other.orientation) + """ + if isinstance(other, Pose): + other_pose = other + else: + other_pose = Pose(other) + + delta_position = self.position - other_pose.position + delta_orientation = self.orientation * other_pose.orientation.inverse() + return Pose(delta_position, delta_orientation) + @classmethod def from_ros_msg(cls, ros_msg: ROSPose) -> Pose: """Create a Pose from a ROS geometry_msgs/Pose message. From 968fbf999221d53692b8ea3f1313e6efab894ff9 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Fri, 9 Jan 2026 18:02:49 -0800 Subject: [PATCH 35/72] Fix: Modules are more generalized --- dimos/teleop/base/base_teleop_module.py | 363 +++++++------------ dimos/teleop/quest3/quest3_teleop_module.py | 62 ++-- dimos/teleop/teleop_robot_controller.py | 382 ++++---------------- 3 files changed, 250 insertions(+), 557 deletions(-) diff --git a/dimos/teleop/base/base_teleop_module.py b/dimos/teleop/base/base_teleop_module.py index 50d57c0b72..459fe0d5e9 100644 --- a/dimos/teleop/base/base_teleop_module.py +++ b/dimos/teleop/base/base_teleop_module.py @@ -17,7 +17,7 @@ Base Teleoperation Module Abstract base class for all teleoperation devices that provides: -- Dual-arm controller calibration (left/right) +- Multi-controller calibration - Delta pose computation (current - initial) - LCM topic publishing (delta poses + trigger states) - Rerun visualization @@ -30,14 +30,15 @@ from abc import ABC from dataclasses import dataclass, field +from enum import Enum import time from typing import TYPE_CHECKING, Any from dimos.core import Module, Out, rpc from dimos.core.global_config import GlobalConfig from dimos.core.module import ModuleConfig -from dimos.msgs.geometry_msgs import Pose, PoseStamped, Vector3 -from dimos.msgs.std_msgs import Bool, Float32 +from dimos.msgs.geometry_msgs import Pose, PoseStamped +from dimos.msgs.std_msgs import Float32 from dimos.utils.logging_config import setup_logger from dimos.utils.transform_utils import matrix_to_pose @@ -49,9 +50,7 @@ try: import rerun as rr - from dimos.dashboard.rerun_init import connect_rerun - RERUN_AVAILABLE = True except ImportError: RERUN_AVAILABLE = False @@ -62,15 +61,18 @@ class BaseTeleopConfig(ModuleConfig): """Base configuration for teleoperation modules.""" # Control settings - position_scale: float = 1.0 # Scale factor for positions - enable_left_arm: bool = True - enable_right_arm: bool = True + num_inputs: int = 1 # Number of inputs (controllers) + enable_inputs: list[bool] = field(default_factory=list) + input_labels: list[str] = field(default_factory=list) # Visualization settings visualize_in_rerun: bool = True # Visualize controller poses in Rerun + log_input_data: bool = False # Log input pose/gripper data periodically + log_input_data_interval: int = 100 # Log every N publishes when enabled # Safety limits safety_limits: bool = True + position_scale: float = 1.0 # Scale factor for positions TODO: Implement proportional scaling max_velocity: float = 0.5 # m/s workspace_limits: dict[str, tuple[float, float]] = field( default_factory=lambda: { @@ -81,10 +83,19 @@ class BaseTeleopConfig(ModuleConfig): ) +class TeleopStatusKey(str, Enum): + """Status dictionary keys (controller_* entries are indexed by controller number).""" + + IS_CALIBRATED = "is_calibrated" + CONTROLLER_ENABLED = "controller_{index}_enabled" + CONTROLLER_HAS_DATA = "controller_{index}_has_data" + CONTROLLER_GRIPPER_VALUE = "controller_{index}_gripper_value" + CONTROLLER_LABEL = "controller_{index}_label" + class BaseTeleopModule(Module, ABC): """Base class for teleoperation modules. - Provides common functionality for dual-arm teleoperation: + Provides common functionality for multi-controller teleoperation: - Calibration: capture initial controller poses - Delta computation: current - initial - Publishing: delta poses and trigger states to LCM @@ -95,15 +106,13 @@ class BaseTeleopModule(Module, ABC): `update_controller_poses()` when new tracking data arrives. ## LCM Topics (Output): - - left_controller_delta: Out[PoseStamped] - Left controller delta pose - - right_controller_delta: Out[PoseStamped] - Right controller delta pose - - left_trigger: Out[Float32] - Left trigger/gripper value (0.0-1.0) - - right_trigger: Out[Float32] - Right trigger/gripper value (0.0-1.0) + - controller_delta_{i}: Out[PoseStamped] - Controller i delta pose + - trigger_value_{i}: Out[Float32] - Controller i trigger/gripper value (0.0-1.0) ## RPC Methods: - - calibrate_vr() -> dict: Calibrate by capturing initial poses + - calibrate() -> dict: Calibrate by capturing initial poses - reset_calibration() -> dict: Reset calibration - - is_vr_calibrated() -> bool: Check calibration status + - is_calibrated() -> bool: Check if calibrated - get_status() -> dict: Get teleoperation status """ @@ -111,10 +120,8 @@ class BaseTeleopModule(Module, ABC): config: BaseTeleopConfig # LCM Output topics - left_controller_delta: Out[PoseStamped] = None # type: ignore[assignment] - right_controller_delta: Out[PoseStamped] = None # type: ignore[assignment] - left_trigger: Out[Float32] = None # type: ignore[assignment] - right_trigger: Out[Float32] = None # type: ignore[assignment] + controller_delta_0: Out[PoseStamped] = None # type: ignore + trigger_value_0: Out[Float32] = None # type: ignore def __init__( self, global_config: GlobalConfig | None = None, *args: Any, **kwargs: Any @@ -124,26 +131,27 @@ def __init__( # Get global config for Rerun connection self._global_config = global_config or GlobalConfig() - # No RPC dependencies - data-driven activation - self.rpc_calls = [] - # Calibration state self._is_calibrated = False - self._left_controller_initial: Pose | None = None - self._right_controller_initial: Pose | None = None + self._initial_poses: list[Pose | None] = [None] * self.config.num_inputs # Latest controller data (absolute poses) - self._left_pose: NDArray[np.float32] | None = None - self._right_pose: NDArray[np.float32] | None = None - self._left_gripper_value: float = 0.0 # 0.0 to 1.0 - self._right_gripper_value: float = 0.0 # 0.0 to 1.0 - - # Tracking counters + self._all_poses: list[NDArray[np.float32] | None] = [None] * self.config.num_inputs + self._all_gripper_values: list[float] = [0.0] * self.config.num_inputs self._tracking_msg_count = 0 self._publish_count = 0 - # Connection state (for subclasses to update) - self._connected_clients = 0 + # Set default values for enable_inputs and input_labels if not provided + if not self.config.enable_inputs: + self.config.enable_inputs = [True] * self.config.num_inputs + if not self.config.input_labels: + self.config.input_labels = [f"controller_{i}" for i in range(self.config.num_inputs)] + + # check for mismatches between num_inputs and enable_inputs or input_labels + if len(self.config.enable_inputs) != self.config.num_inputs: + raise ValueError(f"Number of enable_inputs ({len(self.config.enable_inputs)}) does not match num_inputs ({self.config.num_inputs})") + if len(self.config.input_labels) != self.config.num_inputs: + raise ValueError(f"Number of input_labels ({len(self.config.input_labels)}) does not match num_inputs ({self.config.num_inputs})") logger.info(f"{self.__class__.__name__} base initialized") @@ -177,7 +185,7 @@ def stop(self) -> None: # ========================================================================= @rpc - def calibrate_vr(self) -> dict[str, Any]: + def calibrate(self) -> dict[str, Any]: """Calibrate by capturing initial controller poses. This is typically called when a calibration button is pressed. @@ -190,55 +198,30 @@ def calibrate_vr(self) -> dict[str, Any]: logger.info("Calibrating controllers...") try: - # Check if we have controller data - if self._left_pose is None and self._right_pose is None: + # Check if we have controller data for enabled inputs + enabled_indices = [i for i, enabled in enumerate(self.config.enable_inputs) if enabled] + if not enabled_indices: return { "success": False, - "error": "No controller data received yet. Move controllers and try again.", + "error": "No controllers are enabled. Enable at least one controller and try again.", } - # Capture left controller initial pose - if self.config.enable_left_arm and self._left_pose is not None: - left_pose_obj = matrix_to_pose(self._left_pose) - - # Check if pose is valid (not all zeros) - pose_magnitude = ( - left_pose_obj.x**2 + left_pose_obj.y**2 + left_pose_obj.z**2 - ) ** 0.5 - if pose_magnitude < 0.001: - return { - "success": False, - "error": "Left controller pose is invalid (all zeros)", - } - - self._left_controller_initial = left_pose_obj - logger.info( - f"Captured left controller initial: " - f"pos=[{left_pose_obj.x:.3f}, {left_pose_obj.y:.3f}, {left_pose_obj.z:.3f}], " - f"rpy=[{left_pose_obj.roll:.3f}, {left_pose_obj.pitch:.3f}, {left_pose_obj.yaw:.3f}]" - ) - - # Capture right controller initial pose - if self.config.enable_right_arm and self._right_pose is not None: - right_pose_obj = matrix_to_pose(self._right_pose) - - # Check if pose is valid (not all zeros) - pose_magnitude = ( - right_pose_obj.x**2 + right_pose_obj.y**2 + right_pose_obj.z**2 - ) ** 0.5 - if pose_magnitude < 0.001: + # Capture controller initial poses + for i in enabled_indices: + if self._all_poses[i] is not None: + pose_obj = matrix_to_pose(self._all_poses[i]) + self._initial_poses[i] = pose_obj + logger.info( + f"Captured controller initial: " + f"pos=[{pose_obj.x:.3f}, {pose_obj.y:.3f}, {pose_obj.z:.3f}], " + f"rpy=[{pose_obj.roll:.3f}, {pose_obj.pitch:.3f}, {pose_obj.yaw:.3f}]" + ) + else: return { "success": False, - "error": "Right controller pose is invalid (all zeros)", + "error": f"Controller {self.config.input_labels[i]} data is None. Move controller and try again.", } - self._right_controller_initial = right_pose_obj - logger.info( - f"Captured right controller initial: " - f"pos=[{right_pose_obj.x:.3f}, {right_pose_obj.y:.3f}, {right_pose_obj.z:.3f}], " - f"rpy=[{right_pose_obj.roll:.3f}, {right_pose_obj.pitch:.3f}, {right_pose_obj.yaw:.3f}]" - ) - self._is_calibrated = True logger.info("Calibration complete. Now streaming delta poses...") @@ -256,14 +239,13 @@ def reset_calibration(self) -> dict[str, Any]: Dict with 'success' and 'message' """ self._is_calibrated = False - self._left_controller_initial = None - self._right_controller_initial = None + self._initial_poses = [None] * self.config.num_inputs logger.info("Calibration reset. Recalibrate to resume teleoperation...") return {"success": True, "message": "Calibration reset - recalibrate to resume"} @rpc - def is_vr_calibrated(self) -> bool: + def is_calibrated(self) -> bool: """Check if calibrated. Returns: @@ -276,18 +258,18 @@ def get_status(self) -> dict[str, Any]: """Get current teleoperation status. Returns: - Dictionary with status information + Dictionary with status information keyed by TeleopStatusKey templates. """ - return { - "is_calibrated": self._is_calibrated, - "connected_clients": self._connected_clients, - "left_arm_enabled": self.config.enable_left_arm, - "right_arm_enabled": self.config.enable_right_arm, - "has_left_data": self._left_pose is not None, - "has_right_data": self._right_pose is not None, - "left_gripper_value": self._left_gripper_value, - "right_gripper_value": self._right_gripper_value, + status: dict[str, Any] = { + TeleopStatusKey.IS_CALIBRATED.value: self._is_calibrated, } + for i in range(self.config.num_inputs): + status[TeleopStatusKey.CONTROLLER_ENABLED.value.format(index=i)] = self.config.enable_inputs[i] + status[TeleopStatusKey.CONTROLLER_HAS_DATA.value.format(index=i)] = self._all_poses[i] is not None + status[TeleopStatusKey.CONTROLLER_GRIPPER_VALUE.value.format(index=i)] = self._all_gripper_values[i] + status[TeleopStatusKey.CONTROLLER_LABEL.value.format(index=i)] = self.config.input_labels[i] + + return status # ========================================================================= # Controller Data Processing (called by subclasses) @@ -295,10 +277,8 @@ def get_status(self) -> dict[str, Any]: def update_controller_poses( self, - left_pose: NDArray[np.float32], - right_pose: NDArray[np.float32], - left_gripper: float, - right_gripper: float, + controller_poses: list[NDArray[np.float32]], + controller_gripper_values: list[float], ) -> None: """Update controller poses and publish deltas if calibrated. @@ -306,10 +286,8 @@ def update_controller_poses( from the device-specific connection. Args: - left_pose: 4x4 transformation matrix for left controller - right_pose: 4x4 transformation matrix for right controller - left_gripper: Left gripper value (0.0-1.0) - right_gripper: Right gripper value (0.0-1.0) + controller_poses: List of 4x4 transformation matrices for all controllers + controller_gripper_values: List of gripper values (0.0-1.0) for all controllers """ # Track how many tracking messages we've received self._tracking_msg_count += 1 @@ -319,12 +297,8 @@ def update_controller_poses( logger.info(f"Received tracking data #{self._tracking_msg_count}") # Store absolute poses - self._left_pose = left_pose - self._right_pose = right_pose - - # Store gripper values (0.0 to 1.0) - self._left_gripper_value = float(left_gripper) - self._right_gripper_value = float(right_gripper) + self._all_poses = controller_poses + self._all_gripper_values = controller_gripper_values # Only stream deltas if calibrated if not self._is_calibrated: @@ -336,137 +310,72 @@ def update_controller_poses( # Compute and publish deltas self._compute_and_publish_deltas() + def _compute_and_publish_deltas(self) -> None: """Compute and publish delta poses (current - initial).""" # Track publish count for logging self._publish_count += 1 try: - current_time = time.time() - - # Left controller delta - if self.config.enable_left_arm and self._left_controller_initial is not None: - if self.left_controller_delta and hasattr(self.left_controller_delta, "publish"): - if self._left_pose is not None: - left_pose_obj = matrix_to_pose(self._left_pose) - delta_pose_stamped = self._compute_delta( - left_pose_obj, - self._left_controller_initial, - current_time, - f"{self.__class__.__name__.lower()}_left_controller_delta", - ) - - try: - self.left_controller_delta.publish(delta_pose_stamped) - - # Visualize in Rerun (absolute pose) - self._visualize_controller_in_rerun(left_pose_obj, "left") - - # Log periodically - if self._publish_count <= 5 or self._publish_count % 100 == 0: - logger.info( - f"Published left delta #{self._publish_count}: " - f"pos=[{delta_pose_stamped.position.x:.3f}, {delta_pose_stamped.position.y:.3f}, {delta_pose_stamped.position.z:.3f}], " - f"rpy=[{delta_pose_stamped.roll:.3f}, {delta_pose_stamped.pitch:.3f}, {delta_pose_stamped.yaw:.3f}], " - f"frame_id={delta_pose_stamped.frame_id}" - ) - except Exception as e: - logger.error(f"Failed to publish left delta: {e}") - - # Right controller delta - if self.config.enable_right_arm and self._right_controller_initial is not None: - if self.right_controller_delta and hasattr(self.right_controller_delta, "publish"): - if self._right_pose is not None: - right_pose_obj = matrix_to_pose(self._right_pose) - delta_pose_stamped = self._compute_delta( - right_pose_obj, - self._right_controller_initial, - current_time, - f"{self.__class__.__name__.lower()}_right_controller_delta", - ) - - try: - self.right_controller_delta.publish(delta_pose_stamped) - - # Visualize in Rerun (absolute pose) - self._visualize_controller_in_rerun(right_pose_obj, "right") - - if self._publish_count <= 5 or self._publish_count % 100 == 0: - logger.info( - f"Published right delta #{self._publish_count}: " - f"pos=[{delta_pose_stamped.position.x:.3f}, {delta_pose_stamped.position.y:.3f}, {delta_pose_stamped.position.z:.3f}], " - f"frame_id={delta_pose_stamped.frame_id}" - ) - except Exception as e: - logger.error(f"Failed to publish right delta: {e}") - - # Publish gripper values (0.0 to 1.0) - if self.left_trigger and hasattr(self.left_trigger, "publish"): - try: - self.left_trigger.publish(Float32(data=self._left_gripper_value)) - # Visualize gripper value in Rerun - self._visualize_gripper_in_rerun(self._left_gripper_value, "left") - except Exception as e: - logger.debug(f"Failed to publish left gripper: {e}") - - if self.right_trigger and hasattr(self.right_trigger, "publish"): - try: - self.right_trigger.publish(Float32(data=self._right_gripper_value)) - # Visualize gripper value in Rerun - self._visualize_gripper_in_rerun(self._right_gripper_value, "right") - except Exception as e: - logger.debug(f"Failed to publish right gripper: {e}") + for i in range(self.config.num_inputs): + if not self.config.enable_inputs[i]: + continue + + current_time = time.time() + pose_obj = matrix_to_pose(self._all_poses[i]) + input_label = self.config.input_labels[i] + delta_pose = pose_obj - self._initial_poses[i] + delta_pose_stamped = PoseStamped( + ts=current_time, + frame_id=f"{self.__class__.__name__.lower()}_{input_label}_controller_delta", + position=delta_pose.position, + orientation=delta_pose.orientation, + ) + + controller_output = getattr(self, f"controller_delta_{i}", None) + if controller_output and hasattr(controller_output, "publish"): + try: + controller_output.publish(delta_pose_stamped) + self._visualize_controller_in_rerun(pose_obj, input_label) + except Exception as e: + logger.error(f"Failed to publish {input_label} delta: {e}") + + trigger_output = getattr(self, f"trigger_value_{i}", None) + if trigger_output and hasattr(trigger_output, "publish"): + try: + trigger_output.publish(Float32(data=self._all_gripper_values[i])) + self._visualize_trigger_in_rerun(self._all_gripper_values[i], input_label) + except Exception as e: + logger.debug(f"Failed to publish {input_label} gripper: {e}") + + + if self.config.log_input_data and ( + self._publish_count <= 5 + or self._publish_count % self.config.log_input_data_interval == 0 + ): + logger.info( + f"Published {input_label} delta #{self._publish_count}: " + f"pos=[{delta_pose_stamped.position.x:.3f}, {delta_pose_stamped.position.y:.3f}, {delta_pose_stamped.position.z:.3f}], " + f"rpy=[{delta_pose_stamped.roll:.3f}, {delta_pose_stamped.pitch:.3f}, {delta_pose_stamped.yaw:.3f}], " + f"frame_id={delta_pose_stamped.frame_id}, " + f"trigger_value={self._all_gripper_values[i]:.3f}" + ) except Exception as e: logger.error(f"Error computing/publishing delta poses: {e}") - def _compute_delta( - self, current: Pose, initial: Pose, timestamp: float, frame_id: str - ) -> PoseStamped: - """Compute delta pose: current - initial. - - For position: simple subtraction - For orientation: delta_quat = current * inverse(initial) - - Args: - current: Current controller pose - initial: Initial controller pose (reference) - timestamp: Timestamp for the delta pose - frame_id: Frame ID for the delta pose - - Returns: - Delta pose as PoseStamped (position delta + orientation delta) - """ - # Position delta - delta_x = current.x - initial.x - delta_y = current.y - initial.y - delta_z = current.z - initial.z - - # Orientation delta: delta_quat = current * inverse(initial) - delta_quat = current.orientation * initial.orientation.inverse() - - delta_pose = Pose( - position=Vector3(delta_x, delta_y, delta_z), - orientation=delta_quat, - ) - - return PoseStamped( - ts=timestamp, - frame_id=frame_id, - position=delta_pose.position, - orientation=delta_pose.orientation, - ) - # ========================================================================= # Rerun Visualization # ========================================================================= - def _visualize_controller_in_rerun(self, controller_pose: Pose, arm_side: str) -> None: + def _visualize_controller_in_rerun( + self, controller_pose: Pose, controller_label: str + ) -> None: """Visualize controller absolute pose in Rerun. Args: controller_pose: Absolute controller pose in robot space - arm_side: "left" or "right" + controller_label: Controller label from input_labels """ if not ( self.config.visualize_in_rerun @@ -479,33 +388,33 @@ def _visualize_controller_in_rerun(self, controller_pose: Pose, arm_side: str) - # Convert to PoseStamped for Rerun visualization controller_pose_stamped = PoseStamped( ts=time.time(), - frame_id=f"world/teleop/{arm_side}_controller", + frame_id=f"world/teleop/{controller_label}_controller", position=controller_pose.position, orientation=controller_pose.orientation, ) # Log absolute controller pose transform rr.log( - f"world/teleop/{arm_side}_controller", + f"world/teleop/{controller_label}_controller", controller_pose_stamped.to_rerun(), # type: ignore[no-untyped-call] ) # Log coordinate frame axes to visualize the transform rr.log( - f"world/teleop/{arm_side}_controller", + f"world/teleop/{controller_label}_controller", rr.Arrows3D( # type: ignore[attr-defined] - vectors=[[0.15, 0, 0], [0, 0.15, 0], [0, 0, 0.15]], + vectors=[[0.30, 0, 0], [0, 0.30, 0], [0, 0, 0.30]], origins=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], colors=[[255, 0, 0], [0, 255, 0], [0, 0, 255]], ), ) except Exception as e: - logger.debug(f"Failed to log {arm_side} controller to Rerun: {e}") + logger.debug(f"Failed to log {controller_label} controller to Rerun: {e}") - def _visualize_gripper_in_rerun(self, gripper_value: float, arm_side: str) -> None: - """Visualize gripper/trigger value in Rerun as a scalar time series. + def _visualize_trigger_in_rerun(self, trigger_value: float, controller_label: str) -> None: + """Visualize trigger value in Rerun as a scalar time series. Args: - gripper_value: Gripper value (0.0-1.0) from VR controller - arm_side: "left" or "right" + trigger_value: Trigger value (0.0-1.0) from controller + controller_label: Controller label from input_labels """ if not ( self.config.visualize_in_rerun @@ -515,10 +424,10 @@ def _visualize_gripper_in_rerun(self, gripper_value: float, arm_side: str) -> No return try: - # Log gripper value as scalar time series + # Log trigger value as scalar time series rr.log( - f"world/teleop/{arm_side}_controller/gripper", - rr.Scalars(gripper_value), # type: ignore[attr-defined] + f"world/teleop/{controller_label}_controller/trigger", + rr.Scalars(trigger_value), # type: ignore[attr-defined] ) except Exception as e: - logger.debug(f"Failed to log {arm_side} gripper to Rerun: {e}") + logger.debug(f"Failed to log {controller_label} trigger to Rerun: {e}") diff --git a/dimos/teleop/quest3/quest3_teleop_module.py b/dimos/teleop/quest3/quest3_teleop_module.py index 9511c4b2d0..30370d90dc 100644 --- a/dimos/teleop/quest3/quest3_teleop_module.py +++ b/dimos/teleop/quest3/quest3_teleop_module.py @@ -34,13 +34,16 @@ from __future__ import annotations import asyncio -from dataclasses import dataclass +from dataclasses import dataclass, field import threading +import time from typing import TYPE_CHECKING, Any -from dimos.core import rpc +from dimos.core import Out, rpc from dimos.teleop.base import BaseTeleopConfig, BaseTeleopModule from dimos.teleop.quest3.control.fastapi_server import TeleopFastAPIServer +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.std_msgs import Float32 from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -60,13 +63,19 @@ class Quest3TeleopConfig(BaseTeleopConfig): signaling_host: str = "0.0.0.0" signaling_port: int = 8443 # HTTPS port for WebXR use_https: bool = True # Enable HTTPS for WebXR (required by Quest 3) - # Exposed from BaseTeleopConfig for blueprint configuration - enable_left_arm: bool = True # Enable left arm teleoperation - enable_right_arm: bool = True # Enable right arm teleoperation - visualize_in_rerun: bool = True # Visualize VR controller poses in Rerun - safety_limits: bool = True # Enable safety limits + + # Control settings + num_inputs: int = 2 # Number of inputs (controllers) + enable_inputs: list[bool] = field(default_factory=lambda: [True, True]) + input_labels: list[str] = field(default_factory=lambda: ["left_vr", "right_vr"]) + + # Visualization settings + visualize_in_rerun: bool = True # Visualize controller poses in Rerun + log_input_data: bool = False # Log input pose/gripper data periodically + log_input_data_interval: int = 100 # Log every N publishes when enabled # Other inherited from BaseTeleopConfig (with defaults): + # safety_limits: bool = True # Enable safety limits # - position_scale: float = 1.0 # - max_velocity: float = 0.5 # - workspace_limits: dict[str, tuple[float, float]] @@ -84,23 +93,27 @@ class Quest3TeleopModule(BaseTeleopModule): - X button handler for calibration trigger ## LCM Topics (inherited from base): - - left_controller_delta: Out[PoseStamped] - Left controller delta pose - - right_controller_delta: Out[PoseStamped] - Right controller delta pose - - left_trigger: Out[Bool] - Left trigger button state - - right_trigger: Out[Bool] - Right trigger button state + - controller_delta_0: Out[PoseStamped] - Controller 0 delta pose + - controller_delta_1: Out[PoseStamped] - Controller 1 delta pose + - trigger_value_0: Out[Float32] - Controller 0 trigger/gripper value (0.0-1.0) + - trigger_value_1: Out[Float32] - Controller 1 trigger/gripper value (0.0-1.0) ## RPC Methods (inherited from base): - start() -> None: Start the module and signaling server - stop() -> None: Stop the module and signaling server - - calibrate_vr() -> dict: Calibrate VR (capture initial controller poses) + - calibrate() -> dict: Calibrate (capture initial controller poses) - reset_calibration() -> dict: Reset calibration - - is_vr_calibrated() -> bool: Check if VR is calibrated + - is_calibrated() -> bool: Check if calibrated - get_status() -> dict: Get current teleoperation status """ default_config = Quest3TeleopConfig config: Quest3TeleopConfig + # LCM output topics for VR (controller_0 is inherited from base) + controller_delta_1: Out[PoseStamped] = None # type: ignore[assignment] + trigger_value_1: Out[Float32] = None # type: ignore[assignment] + def __init__( self, global_config: GlobalConfig | None = None, *args: Any, **kwargs: Any ) -> None: @@ -221,10 +234,9 @@ async def _handle_x_button(self, command_type: str, websocket: Any) -> dict[str, try: if command_type == "start_teleop": logger.info("X button pressed - calibrating VR...") - logger.info( - f"Current state: left_pose={self._left_pose is not None}, right_pose={self._right_pose is not None}" - ) - result = self.calibrate_vr() + pose_states = [pose is not None for pose in self._all_poses] + logger.info(f"Current state: controller_poses={pose_states}") + result = self.calibrate() logger.info(f"Calibration result: {result}") if result.get("success"): @@ -259,10 +271,8 @@ async def _handle_x_button(self, command_type: str, websocket: Any) -> dict[str, def _on_tracking_data( self, - left_pose: NDArray[np.float32], - right_pose: NDArray[np.float32], - left_gripper: float, - right_gripper: float, + controller_poses: list[NDArray[np.float32]], + controller_gripper_values: list[float], ) -> None: """Receive tracking data from Quest3 VR. @@ -271,13 +281,13 @@ def _on_tracking_data( and publishing. Args: - left_pose: 4x4 transformation matrix for left controller - right_pose: 4x4 transformation matrix for right controller - left_gripper: Left gripper value (0.0-1.0) - right_gripper: Right gripper value (0.0-1.0) + controller_poses: List of 4x4 transformation matrices for controllers + controller_gripper_values: List of gripper values (0.0-1.0) """ # Call base class method to handle everything - self.update_controller_poses(left_pose, right_pose, left_gripper, right_gripper) + self.update_controller_poses(controller_poses, controller_gripper_values) + + # Expose blueprint for declarative composition diff --git a/dimos/teleop/teleop_robot_controller.py b/dimos/teleop/teleop_robot_controller.py index 70b7ea48f9..95de3d1dd1 100644 --- a/dimos/teleop/teleop_robot_controller.py +++ b/dimos/teleop/teleop_robot_controller.py @@ -15,11 +15,11 @@ """ Teleop Robot Controller -Receives DELTA poses from Quest3TeleopModule and applies them to the robot. +Receives controller delta poses from Quest3TeleopModule and applies them to the robot. Auto-calibrates robot on first delta received. Architecture: -- Subscribes to left_controller_delta and right_controller_delta from Quest3TeleopModule +- Subscribes to controller_delta from Quest3TeleopModule - On first delta received: gets robot's initial end-effector pose via RPC - Calculates: target_pose = initial_robot_pose + delta - Publishes cartesian_command (Pose) to driver @@ -51,12 +51,11 @@ class TeleopRobotControllerConfig(ModuleConfig): # Driver settings driver_module_name: str = "RobotDriver" # Name of the driver module to get robot pose from + driver_method_name: str = "get_state" dummy_driver: bool = False # If True, skip RPC calls and use zeros for initial pose # Control settings control_frequency: float = 50.0 # Hz - control loop frequency - enable_left_arm: bool = True - enable_right_arm: bool = True # Safety settings delta_timeout: float = 1 # Seconds - stop publishing if no new delta received @@ -66,14 +65,14 @@ class TeleopRobotController(Module): """Teleop Robot Controller - applies delta poses to robot. This controller: - 1. Receives DELTA poses (PoseStamped) from Quest3TeleopModule + 1. Receives DELTA poses (PoseStamped) from Quest3TeleopModule for a single controller 2. On first delta: gets initial robot end-effector pose via RPC (auto-calibration) 3. Applies delta to initial robot pose: target = initial_robot + delta 4. Publishes cartesian commands (Pose) to manipulator driver 5. Optionally applies workspace limits Works with any manipulator driver that provides: - - get_cartesian_state RPC method + - state RPC method (configurable via driver_method_name) - cartesian_command input topic (Pose) """ @@ -81,16 +80,12 @@ class TeleopRobotController(Module): config: TeleopRobotControllerConfig # Input topics - receiving DELTA poses as PoseStamped - left_controller_delta: In[PoseStamped] = None # type: ignore[assignment] - right_controller_delta: In[PoseStamped] = None # type: ignore[assignment] - left_trigger: In[Float32] = None # type: ignore[assignment] # Gripper value 0.0-1.0 - right_trigger: In[Float32] = None # type: ignore[assignment] # Gripper value 0.0-1.0 + controller_delta: In[PoseStamped] = None # type: ignore[assignment] + trigger_value: In[Float32] = None # type: ignore[assignment] # Gripper value 0.0-1.0 # Output topics (Pose for commands) - left_cartesian_command: Out[Pose] = None # type: ignore[assignment] - right_cartesian_command: Out[Pose] = None # type: ignore[assignment] - left_gripper_command: Out[Bool] = None # type: ignore[assignment] - right_gripper_command: Out[Bool] = None # type: ignore[assignment] + cartesian_command: Out[Pose] = None # type: ignore[assignment] + gripper_command: Out[Bool] = None # type: ignore[assignment] # RPC dependencies (dynamically set based on config) rpc_calls: list[str] = [] @@ -100,31 +95,21 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) # Set RPC calls dynamically based on driver name - driver_name = self.config.driver_module_name self.rpc_calls = [ - f"{driver_name}.get_state", + f"{self.config.driver_module_name}.{self.config.driver_method_name}", ] # Latest delta data - self._left_delta: PoseStamped | None = None - self._right_delta: PoseStamped | None = None - self._left_gripper_value: float = 0.0 # 0.0 to 1.0 - self._right_gripper_value: float = 0.0 # 0.0 to 1.0 + self._delta_pose: Pose | None = None + self._gripper_value: float = 0.0 # 0.0 to 1.0 # Timestamps for delta timeout detection - self._left_delta_timestamp: float | None = None - self._right_delta_timestamp: float | None = None + self._delta_timestamp: float | None = None # Robot initial state (auto-calibrated on first delta) - self._left_robot_initial_position: Vector3 | None = None - self._left_robot_initial_rpy: Vector3 | None = None - self._right_robot_initial_position: Vector3 | None = None - self._right_robot_initial_rpy: Vector3 | None = None + self._robot_initial_pose: Pose | None = None self._robot_calibrated = False - # State for unwrapped logging - self._last_logged_rpy: dict[str, NDArray[np.float64]] = {} - # Control loop self._control_thread: threading.Thread | None = None self._stop_event = threading.Event() @@ -145,21 +130,17 @@ def start(self) -> None: super().start() # Subscribe to input topics (delta poses) - if self.left_controller_delta and hasattr(self.left_controller_delta, "subscribe"): - self.left_controller_delta.subscribe(self._on_left_delta) - logger.debug("Subscribed to left_controller_delta") - - if self.right_controller_delta and hasattr(self.right_controller_delta, "subscribe"): - self.right_controller_delta.subscribe(self._on_right_delta) - logger.debug("Subscribed to right_controller_delta") - - if self.left_trigger and hasattr(self.left_trigger, "subscribe"): - self.left_trigger.subscribe(self._on_left_trigger) - logger.debug("Subscribed to left_trigger") + if self.controller_delta and hasattr(self.controller_delta, "subscribe"): + self.controller_delta.subscribe(self._on_controller_delta) + logger.debug("Subscribed to controller_delta") + else: + logger.warning("controller_delta not wired; no delta subscription") - if self.right_trigger and hasattr(self.right_trigger, "subscribe"): - self.right_trigger.subscribe(self._on_right_trigger) - logger.debug("Subscribed to right_trigger") + if self.trigger_value and hasattr(self.trigger_value, "subscribe"): + self.trigger_value.subscribe(self._on_trigger_value) + logger.debug("Subscribed to trigger_value") + else: + logger.warning("trigger_value not wired; no trigger subscription") # Start control loop self._stop_event.clear() @@ -187,70 +168,34 @@ def stop(self) -> None: # Input Callbacks # ========================================================================= - def _on_left_delta(self, delta: PoseStamped) -> None: - """Callback for left controller delta pose. + def _on_controller_delta(self, delta: PoseStamped) -> None: + """Callback for controller delta pose. On first delta, auto-calibrates robot. """ - if not self.config.enable_left_arm: - return - # Auto-calibrate robot on first delta received if not self._robot_calibrated: logger.info("First delta received - auto-calibrating robot...") self._calibrate_robot() with self._state_lock: - self._left_delta = delta - self._left_delta_timestamp = time.time() + self._delta_pose = Pose(position=delta.position, orientation=delta.orientation) # Log first few deltas - if not hasattr(self, "_left_delta_count"): - self._left_delta_count = 0 - self._left_delta_count += 1 - if self._left_delta_count <= 5: - logger.info( - f"Received left delta #{self._left_delta_count}: " - f"pos=[{delta.position.x:.3f}, {delta.position.y:.3f}, {delta.position.z:.3f}], " - f"frame_id={delta.frame_id}" - ) - - def _on_right_delta(self, delta: PoseStamped) -> None: - """Callback for right controller delta pose. - - On first delta, auto-calibrates robot. - """ - if not self.config.enable_right_arm: - return - - # Auto-calibrate robot on first delta received - if not self._robot_calibrated: - logger.info("First delta received - auto-calibrating robot...") - self._calibrate_robot() - - with self._state_lock: - self._right_delta = delta - self._right_delta_timestamp = time.time() - - if not hasattr(self, "_right_delta_count"): - self._right_delta_count = 0 - self._right_delta_count += 1 - if self._right_delta_count <= 5: + if not hasattr(self, "_delta_count"): + self._delta_count = 0 + self._delta_count += 1 + if self._delta_count <= 5: logger.info( - f"Received right delta #{self._right_delta_count}: " + f"Received controller delta #{self._delta_count}: " f"pos=[{delta.position.x:.3f}, {delta.position.y:.3f}, {delta.position.z:.3f}], " f"frame_id={delta.frame_id}" ) - def _on_left_trigger(self, gripper: Float32) -> None: - """Callback for left gripper value (0.0-1.0).""" + def _on_trigger_value(self, gripper: Float32) -> None: + """Callback for controller gripper value (0.0-1.0).""" with self._state_lock: - self._left_gripper_value = float(gripper.data) - - def _on_right_trigger(self, gripper: Float32) -> None: - """Callback for right gripper value (0.0-1.0).""" - with self._state_lock: - self._right_gripper_value = float(gripper.data) + self._gripper_value = float(gripper.data) # ========================================================================= # Robot Calibration (Auto-triggered on first delta) @@ -270,24 +215,18 @@ def _calibrate_robot(self) -> bool: # If dummy_driver is enabled, use zeros for initial pose if self.config.dummy_driver: logger.info("Dummy driver mode - using zeros for initial pose") - if self.config.enable_left_arm: - self._left_robot_initial_position = Vector3(0.0, 0.0, 0.0) - self._left_robot_initial_rpy = Vector3(0.0, 0.0, 0.0) - logger.info("Left arm initial pose: pos=[0, 0, 0], rpy=[0, 0, 0]") - - if self.config.enable_right_arm: - self._right_robot_initial_position = Vector3(0.0, 0.0, 0.0) - self._right_robot_initial_rpy = Vector3(0.0, 0.0, 0.0) - logger.info("Right arm initial pose: pos=[0, 0, 0], rpy=[0, 0, 0]") + self._robot_initial_pose = Pose( + position=Vector3(0.0, 0.0, 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, 0.0)), + ) + logger.info("Initial pose: pos=[0, 0, 0], rpy=[0, 0, 0]") self._robot_calibrated = True logger.info("Robot calibration complete (dummy mode) - control active!") return True try: - driver_name = self.config.driver_module_name - rpc_method_name = f"{driver_name}.get_state" - + rpc_method_name = f"{self.config.driver_module_name}.{self.config.driver_method_name}" get_state = self.get_rpc_calls(rpc_method_name) if get_state is None: @@ -300,42 +239,20 @@ def _calibrate_robot(self) -> bool: pose_data = result.get("pose", {}) # Store robot initial state - if self.config.enable_left_arm: - self._left_robot_initial_position = Vector3( - pose_data.get("x", 0.0), pose_data.get("y", 0.0), pose_data.get("z", 0.0) - ) - self._left_robot_initial_rpy = Vector3( - pose_data.get("roll", 0.0), - pose_data.get("pitch", 0.0), - pose_data.get("yaw", 0.0), - ) - logger.info( - f"Robot initial pose: " - f"pos=[{self._left_robot_initial_position.x:.3f}, " - f"{self._left_robot_initial_position.y:.3f}, " - f"{self._left_robot_initial_position.z:.3f}], " - f"rpy=[{self._left_robot_initial_rpy.x:.3f}, " - f"{self._left_robot_initial_rpy.y:.3f}, " - f"{self._left_robot_initial_rpy.z:.3f}]" - ) - - if self.config.enable_right_arm: - self._right_robot_initial_position = Vector3( - pose_data.get("x", 0.0), pose_data.get("y", 0.0), pose_data.get("z", 0.0) - ) - self._right_robot_initial_rpy = Vector3( - pose_data.get("roll", 0.0), - pose_data.get("pitch", 0.0), - pose_data.get("yaw", 0.0), - ) - + position = Vector3(pose_data.get("x", 0.0), pose_data.get("y", 0.0), pose_data.get("z", 0.0)) + rpy = Vector3(pose_data.get("roll", 0.0), pose_data.get("pitch", 0.0), pose_data.get("yaw", 0.0)) + self._robot_initial_pose = Pose( + position=position, + orientation=Quaternion.from_euler(rpy), + ) + logger.info(f"Robot initial pose: {pose_data}") self._robot_calibrated = True logger.info("Robot calibration complete - control active!") return True - else: - error_msg = f"Failed to get robot cartesian state: {result}" - logger.error(error_msg) - return False + + error_msg = f"Failed to get robot cartesian state: {result}" + logger.error(error_msg) + return False except Exception as e: logger.error(f"Robot calibration failed: {e}", exc_info=True) @@ -375,10 +292,7 @@ def get_status(self) -> dict[str, Any]: """ return { "robot_calibrated": self._robot_calibrated, - "has_left_delta": self._left_delta is not None, - "has_right_delta": self._right_delta is not None, - "left_arm_enabled": self.config.enable_left_arm, - "right_arm_enabled": self.config.enable_right_arm, + "has_delta": self._delta_pose is not None, } # ========================================================================= @@ -396,80 +310,37 @@ def _control_loop(self) -> None: try: loop_count += 1 - # Get latest state - current_time = time.time() with self._state_lock: - left_delta = self._left_delta - right_delta = self._right_delta robot_calibrated = self._robot_calibrated - left_delta_time = self._left_delta_timestamp - right_delta_time = self._right_delta_timestamp - - # Check for stale deltas (timeout) - delta_timeout = self.config.delta_timeout - if left_delta_time is not None: - if current_time - left_delta_time > delta_timeout: - logger.debug("Left delta timed out - clearing") - with self._state_lock: - self._left_delta = None - self._left_delta_timestamp = None - self._left_gripper_value = 0.0 # Clear gripper value too - left_delta = None - - if right_delta_time is not None: - if current_time - right_delta_time > delta_timeout: - logger.debug("Right delta timed out - clearing") - with self._state_lock: - self._right_delta = None - self._right_delta_timestamp = None - self._right_gripper_value = 0.0 # Clear gripper value too - right_delta = None + delta_pose = self._delta_pose + gripper_val = self._gripper_value + robot_initial_pose = self._robot_initial_pose + if robot_calibrated: + # Publishing Pose + if robot_initial_pose is not None and delta_pose is not None: + target_pose = robot_initial_pose + delta_pose + if self.cartesian_command and hasattr(self.cartesian_command, "publish"): + try: + self.cartesian_command.publish(target_pose) + except Exception as e: + logger.error(f"Failed to publish cartesian command: {e}") + + # Publishing Gripper + if self.gripper_command and hasattr(self.gripper_command, "publish"): + try: + self.gripper_command.publish(Bool(data=gripper_val > 0.5)) + except Exception as e: + logger.debug(f"Failed to publish gripper command: {e}") + # Log state periodically if loop_count <= 10 or loop_count % 100 == 0: logger.debug( - f"Control loop #{loop_count}: robot_calibrated={robot_calibrated}, " - f"left_delta={left_delta is not None}, right_delta={right_delta is not None}" + f"Control loop #{loop_count}, " + f"robot_calibrated={robot_calibrated}, " + f"has_delta={delta_pose is not None}" ) - # Only process if robot is calibrated - if robot_calibrated: - # Process left arm - if self.config.enable_left_arm and left_delta is not None: - self._apply_delta(left_delta, "left") - - # Process right arm - if self.config.enable_right_arm and right_delta is not None: - self._apply_delta(right_delta, "right") - - # Publish gripper commands (only when calibrated) - # Convert float (0.0-1.0) to Bool (threshold at 0.5) - with self._state_lock: - left_gripper_val = self._left_gripper_value - right_gripper_val = self._right_gripper_value - - if self.config.enable_left_arm: - if self.left_gripper_command and hasattr( - self.left_gripper_command, "publish" - ): - try: - # Convert float to bool: > 0.5 = closed - left_gripper_closed = left_gripper_val > 0.5 - self.left_gripper_command.publish(Bool(data=left_gripper_closed)) - except Exception as e: - logger.debug(f"Failed to publish left gripper command: {e}") - - if self.config.enable_right_arm: - if self.right_gripper_command and hasattr( - self.right_gripper_command, "publish" - ): - try: - # Convert float to bool: > 0.5 = closed - right_gripper_closed = right_gripper_val > 0.5 - self.right_gripper_command.publish(Bool(data=right_gripper_closed)) - except Exception as e: - logger.debug(f"Failed to publish right gripper command: {e}") - except Exception as e: logger.error(f"Error in control loop: {e}", exc_info=True) @@ -479,108 +350,11 @@ def _control_loop(self) -> None: if sleep_time > 0: time.sleep(sleep_time) else: + if loop_count % 100 == 0: + logger.warning("Control loop overrun by %.4fs", -sleep_time) next_time = time.perf_counter() + period logger.info("Control loop stopped") - def _apply_delta(self, delta: PoseStamped, arm_side: str) -> None: - """Apply delta pose to robot initial pose and publish command. - - Calculates: target_pose = initial_robot_pose + delta - - Args: - delta: Delta pose (PoseStamped) from Quest3TeleopModule - arm_side: "left" or "right" - """ - try: - # Track command count for logging - if not hasattr(self, "_command_count"): - self._command_count = 0 - self._command_count += 1 - - # Get robot initial poses for this arm - if arm_side == "left": - robot_initial_pos = self._left_robot_initial_position - robot_initial_rpy = self._left_robot_initial_rpy - else: - robot_initial_pos = self._right_robot_initial_position - robot_initial_rpy = self._right_robot_initial_rpy - - if robot_initial_pos is None or robot_initial_rpy is None: - logger.debug(f"{arm_side.capitalize()} arm not calibrated, skipping") - return - - # Calculate target position: initial_robot + delta - target_x = robot_initial_pos.x + delta.position.x - target_y = robot_initial_pos.y + delta.position.y - target_z = robot_initial_pos.z + delta.position.z - - # Calculate target orientation using RPY addition: - # Convert delta quaternion to Euler, then add to robot initial RPY - delta_euler = delta.orientation.to_euler() - - target_roll = robot_initial_rpy.x + delta_euler.x - target_pitch = robot_initial_rpy.y + delta_euler.y - target_yaw = robot_initial_rpy.z + delta_euler.z - - # Wrap angles to [-ฯ€, ฯ€] - def wrap_angle(angle: float) -> float: - return float(np.arctan2(np.sin(angle), np.cos(angle))) - - target_roll = wrap_angle(target_roll) - target_pitch = wrap_angle(target_pitch) - target_yaw = wrap_angle(target_yaw) - - # Create target orientation quaternion from final RPY - target_rpy = Vector3(target_roll, target_pitch, target_yaw) - target_orientation = Quaternion.from_euler(target_rpy) - - # Create target pose - target_pose = Pose( - position=Vector3(target_x, target_y, target_z), - orientation=target_orientation, - ) - - # Log and publish - if arm_side == "left": - # Unwrap Euler angles for smooth logging (handles +pi and -pi discontinuities) - current_rpy = np.array([target_pose.roll, target_pose.pitch, target_pose.yaw]) - if arm_side in self._last_logged_rpy: - prev_rpy = self._last_logged_rpy[arm_side] - diff = current_rpy - prev_rpy - diff = (diff + np.pi) % (2 * np.pi) - np.pi - current_rpy = prev_rpy + diff - self._last_logged_rpy[arm_side] = current_rpy - - # Publish to robot - if self.left_cartesian_command and hasattr(self.left_cartesian_command, "publish"): - try: - self.left_cartesian_command.publish(target_pose) - except Exception as e: - logger.error(f"Failed to publish left cartesian command: {e}") - - elif arm_side == "right": - # Unwrap Euler angles for smooth logging (handles +pi and -pi discontinuities) - current_rpy = np.array([target_pose.roll, target_pose.pitch, target_pose.yaw]) - if arm_side in self._last_logged_rpy: - prev_rpy = self._last_logged_rpy[arm_side] - diff = current_rpy - prev_rpy - diff = (diff + np.pi) % (2 * np.pi) - np.pi - current_rpy = prev_rpy + diff - self._last_logged_rpy[arm_side] = current_rpy - - # Publish to robot - if self.right_cartesian_command and hasattr( - self.right_cartesian_command, "publish" - ): - try: - self.right_cartesian_command.publish(target_pose) - except Exception as e: - logger.error(f"Failed to publish right cartesian command: {e}") - - except Exception as e: - logger.error(f"Error applying {arm_side} delta: {e}", exc_info=True) - - # Expose blueprint for declarative composition teleop_robot_controller = TeleopRobotController.blueprint From 84b9bc0083e7077472e4753706fdf107af4ad492 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Fri, 9 Jan 2026 18:09:44 -0800 Subject: [PATCH 36/72] Fix: updates with new method changes/renaming --- dimos/teleop/quest3/control/fastapi_server.py | 25 ++++---- .../quest3/control/tracking_processor.py | 57 ++++++++----------- dimos/teleop/teleop_blueprints.py | 46 +++++++-------- 3 files changed, 58 insertions(+), 70 deletions(-) diff --git a/dimos/teleop/quest3/control/fastapi_server.py b/dimos/teleop/quest3/control/fastapi_server.py index ec9caf557b..68a8081669 100644 --- a/dimos/teleop/quest3/control/fastapi_server.py +++ b/dimos/teleop/quest3/control/fastapi_server.py @@ -58,7 +58,7 @@ def __init__(self) -> None: self.active_connections: list[WebSocket] = [] self.active_connections_lock = threading.Lock() self.data_callback: ( - Callable[[NDArray[np.float32], NDArray[np.float32], float, float], None] | None + Callable[[list[NDArray[np.float32]], list[float]], None] | None ) = None self.command_callback: Callable[[str, WebSocket], Awaitable[dict[str, Any]]] | None = None @@ -86,12 +86,12 @@ def disconnect(self, websocket: WebSocket) -> None: def set_callback( self, - callback: Callable[[NDArray[np.float32], NDArray[np.float32], float, float], None], + callback: Callable[[list[NDArray[np.float32]], list[float]], None], ) -> None: """Set callback function to send controller data to teleop module. Args: - callback: Function that takes (left_pose, right_pose, left_gripper, right_gripper) + callback: Function that takes (controller_poses, controller_gripper_values) """ self.data_callback = callback logger.info("Controller data callback registered") @@ -229,7 +229,7 @@ def _ensure_ssl_certificates(self) -> None: def _setup_routes(self) -> None: """Setup FastAPI routes and WebSocket endpoints.""" - @self.app.get("/") + @self.app.get("/", response_model=None) async def root() -> FileResponse | dict[str, Any]: """Serve the VR client HTML page.""" html_file = self.static_dir / "index.html" @@ -324,12 +324,12 @@ async def websocket_endpoint(websocket: WebSocket) -> None: result = processor.process_tracking_message(message) if result is not None: - left_pose, right_pose, left_gripper, right_gripper = result + controller_poses, controller_gripper_values = result # Send to callback (teleop module) if self.manager.data_callback is not None: self.manager.data_callback( - left_pose, right_pose, left_gripper, right_gripper + controller_poses, controller_gripper_values ) # Log periodically @@ -346,12 +346,12 @@ async def websocket_endpoint(websocket: WebSocket) -> None: def set_callback( self, - callback: Callable[[NDArray[np.float32], NDArray[np.float32], float, float], None], + callback: Callable[[list[NDArray[np.float32]], list[float]], None], ) -> None: """Set callback function for controller data. Args: - callback: Function that takes (left_pose, right_pose, left_gripper, right_gripper) + callback: Function that takes (controller_poses, controller_gripper_values) """ self.manager.set_callback(callback) @@ -446,12 +446,11 @@ async def broadcast_robot_state(self, state: dict[str, Any]) -> None: level=logging.INFO, format="%(asctime)s - %(name)s - %(levelname)s - %(message)s" ) - def test_callback( - left_pose: Any, right_pose: Any, left_gripper: float, right_gripper: float - ) -> None: + def test_callback(controller_poses: Any, controller_gripper_values: Any) -> None: """Test callback for tracking data.""" - print(f"Left pose: {left_pose[:3, 3]}, gripper: {left_gripper}") - print(f"Right pose: {right_pose[:3, 3]}, gripper: {right_gripper}") + if len(controller_poses) >= 2 and len(controller_gripper_values) >= 2: + print(f"Left pose: {controller_poses[0][:3, 3]}, gripper: {controller_gripper_values[0]}") + print(f"Right pose: {controller_poses[1][:3, 3]}, gripper: {controller_gripper_values[1]}") server = TeleopFastAPIServer() server.set_callback(test_callback) diff --git a/dimos/teleop/quest3/control/tracking_processor.py b/dimos/teleop/quest3/control/tracking_processor.py index 111eaa08ff..548b975e1e 100644 --- a/dimos/teleop/quest3/control/tracking_processor.py +++ b/dimos/teleop/quest3/control/tracking_processor.py @@ -17,7 +17,7 @@ VR controller tracking processor for Quest3 teleoperation. Processes VR controller tracking data, applies coordinate transformations, -and returns robot-space poses and gripper values. +and returns robot-space poses and gripper values per controller. """ from __future__ import annotations @@ -67,52 +67,43 @@ def __init__(self) -> None: def process_tracking_message( self, event: dict[str, Any] - ) -> tuple[NDArray[np.float32], NDArray[np.float32], float, float] | None: + ) -> tuple[list[NDArray[np.float32]], list[float]] | None: """Process VR tracking message and return robot-space poses. Args: event: Dictionary with 'type', 'left', and 'right' controller data Returns: - Tuple of (left_pose, right_pose, left_gripper, right_gripper) or None if invalid + Tuple of (controller_poses, controller_gripper_values) or None if invalid """ tracking_type = event.get("type") - if tracking_type != "controller": # TODO: handle hand tracking type + if tracking_type != "controller": logger.debug("Ignoring non-controller tracking type: %s", tracking_type) return None # Process both controllers for side in ["left", "right"]: tracking_data = event.get(side) - if tracking_data is not None: - if not isinstance(tracking_data, dict): - logger.warning("Invalid tracking data format for %s", side) - continue - - self._process_controller(tracking_data, side) - - # Get controller poses - left_pose = self.left_wrist_pose.copy() - right_pose = self.right_wrist_pose.copy() - - return left_pose, right_pose, self.left_gripper_value, self.right_gripper_value - - def _process_controller(self, tracking_data: dict[str, Any], side: str) -> None: - """Process single controller's tracking data. - - Args: - tracking_data: Controller data with 'targetLocation' and 'trigger' - side: 'left' or 'right' - """ - # Process target location (pose) - if "targetLocation" in tracking_data: - self._process_target_location(tracking_data["targetLocation"], side) - - # Process gripper (trigger) - if side == "left": - self.left_gripper_value = self._extract_gripper_value(tracking_data) - else: - self.right_gripper_value = self._extract_gripper_value(tracking_data) + if tracking_data is None: + continue + if not isinstance(tracking_data, dict): + logger.debug("Invalid %s tracking payload: %s", side, type(tracking_data)) + continue + + # Process target location (pose) + if "targetLocation" in tracking_data: + self._process_target_location(tracking_data["targetLocation"], side) + + # Process gripper (trigger) + if side == "left": + self.left_gripper_value = self._extract_gripper_value(tracking_data) + else: + self.right_gripper_value = self._extract_gripper_value(tracking_data) + + return ( + [self.left_wrist_pose, self.right_wrist_pose], + [self.left_gripper_value, self.right_gripper_value], + ) def _process_target_location(self, target_location: list[float], side: str) -> None: """Process controller pose from VR space to robot space. diff --git a/dimos/teleop/teleop_blueprints.py b/dimos/teleop/teleop_blueprints.py index c60fa6743a..c0ec259d6a 100644 --- a/dimos/teleop/teleop_blueprints.py +++ b/dimos/teleop/teleop_blueprints.py @@ -27,7 +27,7 @@ TeleopRobotController (Robot calibration, delta application) โ†“ First delta โ†’ auto-calibrate robot โ†“ Computes: target = initial_robot + delta - โ†“ Publishes: cartesian commands (PoseStamped) + โ†“ Publishes: cartesian commands (Pose) โ†“ Robot Driver (any manipulator driver) @@ -45,10 +45,12 @@ my_system = autoconnect( quest3_teleop_module( signaling_port=8443, + num_inputs=2, + enable_inputs=[True, True], + input_labels=["left_vr", "right_vr"], ), teleop_robot_controller( driver_module_name="MyRobotDriver", - enable_left_arm=True, ), my_robot_driver(...), ) @@ -57,24 +59,22 @@ from dimos.core.blueprints import autoconnect from dimos.core.transport import LCMTransport from dimos.msgs.geometry_msgs import PoseStamped -from dimos.msgs.std_msgs import Bool, Float32 +from dimos.msgs.std_msgs import Float32 from dimos.teleop.quest3.quest3_teleop_module import quest3_teleop_module -from dimos.teleop.teleop_robot_controller import teleop_robot_controller +from dimos.teleop.teleop_robot_controller import TeleopRobotController, teleop_robot_controller # ============================================================================= # Quest3 Teleoperation Blueprint # ============================================================================= # Combines: # - Quest3TeleopModule: VR calibration + delta computation -# - TeleopRobotController: Robot calibration + delta application +# - TeleopRobotController: Robot calibration + delta application (single controller) # # Data flow: -# Quest3TeleopModule.left_controller_delta โ”€โ”€โ–บ TeleopRobotController.left_controller_delta -# Quest3TeleopModule.right_controller_delta โ”€โ”€โ–บ TeleopRobotController.right_controller_delta -# Quest3TeleopModule.left_trigger โ”€โ”€โ–บ TeleopRobotController.left_trigger -# Quest3TeleopModule.right_trigger โ”€โ”€โ–บ TeleopRobotController.right_trigger -# TeleopRobotController.left_cartesian_command โ”€โ”€โ–บ Robot Driver (via LCM) -# TeleopRobotController.right_cartesian_command โ”€โ”€โ–บ Robot Driver (via LCM) +# Quest3TeleopModule.controller_delta_0 -- TeleopRobotController.controller_delta +# Quest3TeleopModule.trigger_value_0 -- TeleopRobotController.trigger_value +# TeleopRobotController.cartesian_command -- Robot Driver (via LCM) +# TeleopRobotController.gripper_command -- Robot Driver (via LCM) # ============================================================================= quest3_teleop = autoconnect( @@ -82,8 +82,9 @@ signaling_host="0.0.0.0", signaling_port=8443, # HTTPS port (required for WebXR) use_https=True, # Enable HTTPS for Quest 3 WebXR - enable_left_arm=True, - enable_right_arm=True, + num_inputs=2, + enable_inputs=[True, False], + input_labels=["left_vr", "right_vr"], visualize_in_rerun=True, safety_limits=True, ), @@ -91,25 +92,22 @@ driver_module_name="DummyDriver", dummy_driver=True, # Skip RPC calls, use zeros for initial pose control_frequency=50.0, # Hz - control loop frequency - enable_left_arm=True, - enable_right_arm=True, ), +).remappings( + [ + (TeleopRobotController, "controller_delta", "controller_delta_0"), + (TeleopRobotController, "trigger_value", "trigger_value_0"), + ] ).transports( { # Delta poses from Quest3TeleopModule to TeleopRobotController - ("left_controller_delta", PoseStamped): LCMTransport( - "/quest3/left_controller_delta", PoseStamped + ("controller_delta_0", PoseStamped): LCMTransport( + "/quest3/controller_delta_0", PoseStamped ), - ("right_controller_delta", PoseStamped): LCMTransport( - "/quest3/right_controller_delta", PoseStamped - ), - # Gripper values (0.0-1.0) - ("left_trigger", Float32): LCMTransport("/quest3/left_trigger", Float32), - ("right_trigger", Float32): LCMTransport("/quest3/right_trigger", Float32), + ("trigger_value_0", Float32): LCMTransport("/quest3/trigger_value_0", Float32), } ) - __all__ = [ "quest3_teleop", ] From 5ea31512577f2ac8a907f4c3b67f35e2ec70b898 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Fri, 9 Jan 2026 18:33:33 -0800 Subject: [PATCH 37/72] Fix: pre-commit errors --- dimos/teleop/base/base_teleop_module.py | 57 +++++++++++------- dimos/teleop/quest3/control/fastapi_server.py | 16 ++--- dimos/teleop/quest3/quest3_teleop_module.py | 6 +- dimos/teleop/teleop_blueprints.py | 60 ++++++++++--------- dimos/teleop/teleop_robot_controller.py | 13 +++- 5 files changed, 88 insertions(+), 64 deletions(-) diff --git a/dimos/teleop/base/base_teleop_module.py b/dimos/teleop/base/base_teleop_module.py index 459fe0d5e9..4ae2c956ea 100644 --- a/dimos/teleop/base/base_teleop_module.py +++ b/dimos/teleop/base/base_teleop_module.py @@ -32,7 +32,7 @@ from dataclasses import dataclass, field from enum import Enum import time -from typing import TYPE_CHECKING, Any +from typing import TYPE_CHECKING, Any, Sequence from dimos.core import Module, Out, rpc from dimos.core.global_config import GlobalConfig @@ -50,7 +50,9 @@ try: import rerun as rr + from dimos.dashboard.rerun_init import connect_rerun + RERUN_AVAILABLE = True except ImportError: RERUN_AVAILABLE = False @@ -92,6 +94,7 @@ class TeleopStatusKey(str, Enum): CONTROLLER_GRIPPER_VALUE = "controller_{index}_gripper_value" CONTROLLER_LABEL = "controller_{index}_label" + class BaseTeleopModule(Module, ABC): """Base class for teleoperation modules. @@ -149,9 +152,13 @@ def __init__( # check for mismatches between num_inputs and enable_inputs or input_labels if len(self.config.enable_inputs) != self.config.num_inputs: - raise ValueError(f"Number of enable_inputs ({len(self.config.enable_inputs)}) does not match num_inputs ({self.config.num_inputs})") + raise ValueError( + f"Number of enable_inputs ({len(self.config.enable_inputs)}) does not match num_inputs ({self.config.num_inputs})" + ) if len(self.config.input_labels) != self.config.num_inputs: - raise ValueError(f"Number of input_labels ({len(self.config.input_labels)}) does not match num_inputs ({self.config.num_inputs})") + raise ValueError( + f"Number of input_labels ({len(self.config.input_labels)}) does not match num_inputs ({self.config.num_inputs})" + ) logger.info(f"{self.__class__.__name__} base initialized") @@ -208,8 +215,9 @@ def calibrate(self) -> dict[str, Any]: # Capture controller initial poses for i in enabled_indices: - if self._all_poses[i] is not None: - pose_obj = matrix_to_pose(self._all_poses[i]) + pose_matrix = self._all_poses[i] + if pose_matrix is not None: + pose_obj = matrix_to_pose(pose_matrix) self._initial_poses[i] = pose_obj logger.info( f"Captured controller initial: " @@ -264,10 +272,18 @@ def get_status(self) -> dict[str, Any]: TeleopStatusKey.IS_CALIBRATED.value: self._is_calibrated, } for i in range(self.config.num_inputs): - status[TeleopStatusKey.CONTROLLER_ENABLED.value.format(index=i)] = self.config.enable_inputs[i] - status[TeleopStatusKey.CONTROLLER_HAS_DATA.value.format(index=i)] = self._all_poses[i] is not None - status[TeleopStatusKey.CONTROLLER_GRIPPER_VALUE.value.format(index=i)] = self._all_gripper_values[i] - status[TeleopStatusKey.CONTROLLER_LABEL.value.format(index=i)] = self.config.input_labels[i] + status[TeleopStatusKey.CONTROLLER_ENABLED.value.format(index=i)] = ( + self.config.enable_inputs[i] + ) + status[TeleopStatusKey.CONTROLLER_HAS_DATA.value.format(index=i)] = ( + self._all_poses[i] is not None + ) + status[TeleopStatusKey.CONTROLLER_GRIPPER_VALUE.value.format(index=i)] = ( + self._all_gripper_values[i] + ) + status[TeleopStatusKey.CONTROLLER_LABEL.value.format(index=i)] = ( + self.config.input_labels[i] + ) return status @@ -277,8 +293,8 @@ def get_status(self) -> dict[str, Any]: def update_controller_poses( self, - controller_poses: list[NDArray[np.float32]], - controller_gripper_values: list[float], + controller_poses: Sequence[NDArray[np.float32] | None], + controller_gripper_values: Sequence[float], ) -> None: """Update controller poses and publish deltas if calibrated. @@ -297,8 +313,8 @@ def update_controller_poses( logger.info(f"Received tracking data #{self._tracking_msg_count}") # Store absolute poses - self._all_poses = controller_poses - self._all_gripper_values = controller_gripper_values + self._all_poses = list(controller_poses) + self._all_gripper_values = list(controller_gripper_values) # Only stream deltas if calibrated if not self._is_calibrated: @@ -310,22 +326,24 @@ def update_controller_poses( # Compute and publish deltas self._compute_and_publish_deltas() - def _compute_and_publish_deltas(self) -> None: """Compute and publish delta poses (current - initial).""" # Track publish count for logging self._publish_count += 1 try: - for i in range(self.config.num_inputs): if not self.config.enable_inputs[i]: continue current_time = time.time() - pose_obj = matrix_to_pose(self._all_poses[i]) + pose_matrix = self._all_poses[i] + if pose_matrix is None: + continue + initial_pose = self._initial_poses[i] + pose_obj = matrix_to_pose(pose_matrix) input_label = self.config.input_labels[i] - delta_pose = pose_obj - self._initial_poses[i] + delta_pose = pose_obj - initial_pose delta_pose_stamped = PoseStamped( ts=current_time, frame_id=f"{self.__class__.__name__.lower()}_{input_label}_controller_delta", @@ -349,7 +367,6 @@ def _compute_and_publish_deltas(self) -> None: except Exception as e: logger.debug(f"Failed to publish {input_label} gripper: {e}") - if self.config.log_input_data and ( self._publish_count <= 5 or self._publish_count % self.config.log_input_data_interval == 0 @@ -368,9 +385,7 @@ def _compute_and_publish_deltas(self) -> None: # Rerun Visualization # ========================================================================= - def _visualize_controller_in_rerun( - self, controller_pose: Pose, controller_label: str - ) -> None: + def _visualize_controller_in_rerun(self, controller_pose: Pose, controller_label: str) -> None: """Visualize controller absolute pose in Rerun. Args: diff --git a/dimos/teleop/quest3/control/fastapi_server.py b/dimos/teleop/quest3/control/fastapi_server.py index 68a8081669..7ef36794b6 100644 --- a/dimos/teleop/quest3/control/fastapi_server.py +++ b/dimos/teleop/quest3/control/fastapi_server.py @@ -57,9 +57,7 @@ class ConnectionManager: def __init__(self) -> None: self.active_connections: list[WebSocket] = [] self.active_connections_lock = threading.Lock() - self.data_callback: ( - Callable[[list[NDArray[np.float32]], list[float]], None] | None - ) = None + self.data_callback: Callable[[list[NDArray[np.float32]], list[float]], None] | None = None self.command_callback: Callable[[str, WebSocket], Awaitable[dict[str, Any]]] | None = None async def connect(self, websocket: WebSocket) -> None: @@ -328,9 +326,7 @@ async def websocket_endpoint(websocket: WebSocket) -> None: # Send to callback (teleop module) if self.manager.data_callback is not None: - self.manager.data_callback( - controller_poses, controller_gripper_values - ) + self.manager.data_callback(controller_poses, controller_gripper_values) # Log periodically if message_count % 100 == 0: @@ -449,8 +445,12 @@ async def broadcast_robot_state(self, state: dict[str, Any]) -> None: def test_callback(controller_poses: Any, controller_gripper_values: Any) -> None: """Test callback for tracking data.""" if len(controller_poses) >= 2 and len(controller_gripper_values) >= 2: - print(f"Left pose: {controller_poses[0][:3, 3]}, gripper: {controller_gripper_values[0]}") - print(f"Right pose: {controller_poses[1][:3, 3]}, gripper: {controller_gripper_values[1]}") + print( + f"Left pose: {controller_poses[0][:3, 3]}, gripper: {controller_gripper_values[0]}" + ) + print( + f"Right pose: {controller_poses[1][:3, 3]}, gripper: {controller_gripper_values[1]}" + ) server = TeleopFastAPIServer() server.set_callback(test_callback) diff --git a/dimos/teleop/quest3/quest3_teleop_module.py b/dimos/teleop/quest3/quest3_teleop_module.py index 30370d90dc..7365180817 100644 --- a/dimos/teleop/quest3/quest3_teleop_module.py +++ b/dimos/teleop/quest3/quest3_teleop_module.py @@ -42,8 +42,6 @@ from dimos.core import Out, rpc from dimos.teleop.base import BaseTeleopConfig, BaseTeleopModule from dimos.teleop.quest3.control.fastapi_server import TeleopFastAPIServer -from dimos.msgs.geometry_msgs import PoseStamped -from dimos.msgs.std_msgs import Float32 from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -51,6 +49,8 @@ from numpy.typing import NDArray from dimos.core.global_config import GlobalConfig + from dimos.msgs.geometry_msgs import PoseStamped + from dimos.msgs.std_msgs import Float32 logger = setup_logger() @@ -288,7 +288,5 @@ def _on_tracking_data( self.update_controller_poses(controller_poses, controller_gripper_values) - - # Expose blueprint for declarative composition quest3_teleop_module = Quest3TeleopModule.blueprint diff --git a/dimos/teleop/teleop_blueprints.py b/dimos/teleop/teleop_blueprints.py index c0ec259d6a..f90d98f0f2 100644 --- a/dimos/teleop/teleop_blueprints.py +++ b/dimos/teleop/teleop_blueprints.py @@ -77,35 +77,39 @@ # TeleopRobotController.gripper_command -- Robot Driver (via LCM) # ============================================================================= -quest3_teleop = autoconnect( - quest3_teleop_module( - signaling_host="0.0.0.0", - signaling_port=8443, # HTTPS port (required for WebXR) - use_https=True, # Enable HTTPS for Quest 3 WebXR - num_inputs=2, - enable_inputs=[True, False], - input_labels=["left_vr", "right_vr"], - visualize_in_rerun=True, - safety_limits=True, - ), - teleop_robot_controller( - driver_module_name="DummyDriver", - dummy_driver=True, # Skip RPC calls, use zeros for initial pose - control_frequency=50.0, # Hz - control loop frequency - ), -).remappings( - [ - (TeleopRobotController, "controller_delta", "controller_delta_0"), - (TeleopRobotController, "trigger_value", "trigger_value_0"), - ] -).transports( - { - # Delta poses from Quest3TeleopModule to TeleopRobotController - ("controller_delta_0", PoseStamped): LCMTransport( - "/quest3/controller_delta_0", PoseStamped +quest3_teleop = ( + autoconnect( + quest3_teleop_module( + signaling_host="0.0.0.0", + signaling_port=8443, # HTTPS port (required for WebXR) + use_https=True, # Enable HTTPS for Quest 3 WebXR + num_inputs=2, + enable_inputs=[True, False], + input_labels=["left_vr", "right_vr"], + visualize_in_rerun=True, + safety_limits=True, + ), + teleop_robot_controller( + driver_module_name="DummyDriver", + dummy_driver=True, # Skip RPC calls, use zeros for initial pose + control_frequency=50.0, # Hz - control loop frequency ), - ("trigger_value_0", Float32): LCMTransport("/quest3/trigger_value_0", Float32), - } + ) + .remappings( + [ + (TeleopRobotController, "controller_delta", "controller_delta_0"), + (TeleopRobotController, "trigger_value", "trigger_value_0"), + ] + ) + .transports( + { + # Delta poses from Quest3TeleopModule to TeleopRobotController + ("controller_delta_0", PoseStamped): LCMTransport( + "/quest3/controller_delta_0", PoseStamped + ), + ("trigger_value_0", Float32): LCMTransport("/quest3/trigger_value_0", Float32), + } + ) ) __all__ = [ diff --git a/dimos/teleop/teleop_robot_controller.py b/dimos/teleop/teleop_robot_controller.py index 95de3d1dd1..1068a8fd47 100644 --- a/dimos/teleop/teleop_robot_controller.py +++ b/dimos/teleop/teleop_robot_controller.py @@ -239,8 +239,14 @@ def _calibrate_robot(self) -> bool: pose_data = result.get("pose", {}) # Store robot initial state - position = Vector3(pose_data.get("x", 0.0), pose_data.get("y", 0.0), pose_data.get("z", 0.0)) - rpy = Vector3(pose_data.get("roll", 0.0), pose_data.get("pitch", 0.0), pose_data.get("yaw", 0.0)) + position = Vector3( + pose_data.get("x", 0.0), pose_data.get("y", 0.0), pose_data.get("z", 0.0) + ) + rpy = Vector3( + pose_data.get("roll", 0.0), + pose_data.get("pitch", 0.0), + pose_data.get("yaw", 0.0), + ) self._robot_initial_pose = Pose( position=position, orientation=Quaternion.from_euler(rpy), @@ -332,7 +338,7 @@ def _control_loop(self) -> None: self.gripper_command.publish(Bool(data=gripper_val > 0.5)) except Exception as e: logger.debug(f"Failed to publish gripper command: {e}") - + # Log state periodically if loop_count <= 10 or loop_count % 100 == 0: logger.debug( @@ -356,5 +362,6 @@ def _control_loop(self) -> None: logger.info("Control loop stopped") + # Expose blueprint for declarative composition teleop_robot_controller = TeleopRobotController.blueprint From 1f8d2553eee003d074afa6e5306f3a78a8f1b9d4 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam <63036454+ruthwikdasyam@users.noreply.github.com> Date: Sat, 10 Jan 2026 04:04:07 +0000 Subject: [PATCH 38/72] CI code cleanup --- dimos/teleop/base/base_teleop_module.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/dimos/teleop/base/base_teleop_module.py b/dimos/teleop/base/base_teleop_module.py index 4ae2c956ea..513d7009d7 100644 --- a/dimos/teleop/base/base_teleop_module.py +++ b/dimos/teleop/base/base_teleop_module.py @@ -32,7 +32,7 @@ from dataclasses import dataclass, field from enum import Enum import time -from typing import TYPE_CHECKING, Any, Sequence +from typing import TYPE_CHECKING, Any from dimos.core import Module, Out, rpc from dimos.core.global_config import GlobalConfig @@ -43,6 +43,8 @@ from dimos.utils.transform_utils import matrix_to_pose if TYPE_CHECKING: + from collections.abc import Sequence + import numpy as np from numpy.typing import NDArray From 633fb730fdbb64d9b4335902a18a27384af2efe1 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Sat, 10 Jan 2026 13:48:25 -0800 Subject: [PATCH 39/72] Fix: Code Restructured --- dimos/teleop/README.md | 88 +++++++++-------- dimos/teleop/__init__.py | 12 +-- dimos/teleop/devices/__init__.py | 15 +++ dimos/teleop/{ => devices}/base/README.md | 45 ++++++--- dimos/teleop/{ => devices}/base/__init__.py | 7 +- .../{ => devices}/base/base_teleop_module.py | 83 ++-------------- .../{quest3 => devices/vr_headset}/README.md | 73 +++++++------- .../vr_headset}/__init__.py | 11 ++- .../teleop/devices/vr_headset/certs/cert.pem | 19 ++++ dimos/teleop/devices/vr_headset/certs/key.pem | 28 ++++++ .../vr_headset}/control/fastapi_server.py | 10 +- .../vr_headset}/control/tracking_processor.py | 0 .../vr_headset}/static/index.html | 4 +- .../vr_headset/vr_teleop_module.py} | 99 ++++++------------- dimos/teleop/robot_controllers/__init__.py | 25 +++++ .../teleop_arm_controller.py} | 46 ++++----- dimos/teleop/teleop_blueprints.py | 31 ++---- 17 files changed, 288 insertions(+), 308 deletions(-) create mode 100644 dimos/teleop/devices/__init__.py rename dimos/teleop/{ => devices}/base/README.md (64%) rename dimos/teleop/{ => devices}/base/__init__.py (84%) rename dimos/teleop/{ => devices}/base/base_teleop_module.py (82%) rename dimos/teleop/{quest3 => devices/vr_headset}/README.md (57%) rename dimos/teleop/{quest3 => devices/vr_headset}/__init__.py (75%) create mode 100644 dimos/teleop/devices/vr_headset/certs/cert.pem create mode 100644 dimos/teleop/devices/vr_headset/certs/key.pem rename dimos/teleop/{quest3 => devices/vr_headset}/control/fastapi_server.py (98%) rename dimos/teleop/{quest3 => devices/vr_headset}/control/tracking_processor.py (100%) rename dimos/teleop/{quest3 => devices/vr_headset}/static/index.html (99%) rename dimos/teleop/{quest3/quest3_teleop_module.py => devices/vr_headset/vr_teleop_module.py} (71%) create mode 100644 dimos/teleop/robot_controllers/__init__.py rename dimos/teleop/{teleop_robot_controller.py => robot_controllers/teleop_arm_controller.py} (90%) diff --git a/dimos/teleop/README.md b/dimos/teleop/README.md index ef8b77c61a..8e2c8203f7 100644 --- a/dimos/teleop/README.md +++ b/dimos/teleop/README.md @@ -5,31 +5,33 @@ Teleoperation modules for controlling robots with various input devices. ## Overview The teleoperation system consists of: -- **Base module** ([`base/`](base/)) - Common functionality for all devices -- **Device modules** (e.g., [`quest3/`](quest3/)) - Device-specific implementations -- **Robot controller** ([`teleop_robot_controller.py`](teleop_robot_controller.py)) - Applies deltas to robot +- **Base module** ([`devices/base/`](devices/base/)) - Common functionality for all devices +- **Device modules** (e.g., [`devices/vr_headset/`](devices/vr_headset/)) - Device-specific implementations +- **Robot controllers** ([`robot_controllers/`](robot_controllers/)) - Applies deltas to robot ## Folder Structure ``` teleop/ -โ”œโ”€โ”€ __init__.py # Package exports โ”œโ”€โ”€ README.md # This file -โ”œโ”€โ”€ base/ # Base teleoperation module -โ”‚ โ”œโ”€โ”€ __init__.py -โ”‚ โ”œโ”€โ”€ base_teleop_module.py # BaseTeleopModule (calibration, deltas, publishing) -โ”‚ โ””โ”€โ”€ README.md # Guide for creating new devices -โ”œโ”€โ”€ quest3/ # Quest 3 VR implementation -โ”‚ โ”œโ”€โ”€ __init__.py -โ”‚ โ”œโ”€โ”€ quest3_teleop_module.py # Quest3TeleopModule (WebSocket server) -โ”‚ โ”œโ”€โ”€ README.md # Quest3 setup and usage -โ”‚ โ”œโ”€โ”€ control/ -โ”‚ โ”‚ โ”œโ”€โ”€ fastapi_server.py # FastAPI/WebSocket server -โ”‚ โ”‚ โ””โ”€โ”€ tracking_processor.py # VR tracking data processing -โ”‚ โ”œโ”€โ”€ static/ -โ”‚ โ”‚ โ””โ”€โ”€ index.html # VR client (HTML/JS) -โ”‚ โ””โ”€โ”€ certs/ # SSL certificates (auto-generated) -โ”œโ”€โ”€ teleop_robot_controller.py # Applies deltas to robot +โ”œโ”€โ”€ devices/ # Teleop devices +โ”‚ โ”œโ”€โ”€ base/ # Base teleoperation module +โ”‚ โ”‚ โ”œโ”€โ”€ __init__.py +โ”‚ โ”‚ โ”œโ”€โ”€ base_teleop_module.py # BaseTeleopModule (calibration, deltas, publishing) +โ”‚ โ”‚ โ””โ”€โ”€ README.md # Guide for creating new devices +โ”‚ โ””โ”€โ”€ vr_headset/ # VR headset implementation (Quest 3 tested) +โ”‚ โ”œโ”€โ”€ __init__.py +โ”‚ โ”œโ”€โ”€ vr_teleop_module.py # VRTeleopModule (WebSocket server) +โ”‚ โ”œโ”€โ”€ README.md # VR headset setup and usage +โ”‚ โ”œโ”€โ”€ control/ +โ”‚ โ”‚ โ”œโ”€โ”€ fastapi_server.py # FastAPI/WebSocket server +โ”‚ โ”‚ โ””โ”€โ”€ tracking_processor.py # VR tracking data processing +โ”‚ โ”œโ”€โ”€ static/ +โ”‚ โ”‚ โ””โ”€โ”€ index.html # VR client (HTML/JS) +โ”‚ โ””โ”€โ”€ certs/ # SSL certificates (auto-generated) +โ”œโ”€โ”€ robot_controllers/ # Robot-side controllers +โ”‚ โ”œโ”€โ”€ teleop_arm_controller.py # Applies deltas to robot arm +โ”‚ โ””โ”€โ”€ __init__.py โ””โ”€โ”€ teleop_blueprints.py # Pre-built system blueprints ``` @@ -43,12 +45,12 @@ BaseTeleopModule (base class) โ”œโ”€โ”€ Rerun visualization โ””โ”€โ”€ update_controller_poses() โ† Device modules call this โ†‘ -Device Module (Quest3, SpaceMouse, etc.) +Device Module (VR headset, SpaceMouse, etc.) โ”œโ”€โ”€ Inherits from BaseTeleopModule โ”œโ”€โ”€ Device-specific connection (WebSocket, USB, etc.) โ””โ”€โ”€ Receives tracking data โ†’ calls update_controller_poses() โ†“ -TeleopRobotController +TeleopArmController โ”œโ”€โ”€ Receives delta poses โ”œโ”€โ”€ Auto-calibrates robot โ””โ”€โ”€ Applies: target = initial_robot + delta @@ -58,11 +60,11 @@ Robot Driver โ†’ Robot ## Supported Devices -### Quest 3 VR Headset [Any VR headset] -**Module**: [`quest3/`](quest3/) +### VR Headset (Quest 3 tested) +**Module**: [`devices/vr_headset/`](devices/vr_headset/) **Type**: VR controllers (6DOF dual-arm) **Connection**: WebSocket/HTTPS (WebXR) -**Features**: AR mode, dual-arm control, trigger buttons +**Features**: AR mode, multi-controller support, analog trigger values ```python from dimos.teleop.teleop_blueprints import quest3_teleop @@ -71,6 +73,7 @@ coordinator = quest3_teleop.build() coordinator.loop() # Open https://your-ip:8443 on Quest 3 ``` +`quest3_teleop` uses `VRTeleopModule` under the hood with the updated `devices/vr_headset` layout. ## Quick Start @@ -79,14 +82,14 @@ coordinator.loop() ```python from dimos.teleop.teleop_blueprints import quest3_teleop -# Pre-built blueprint with Quest3 + TeleopRobotController +# Pre-built blueprint with VR headset + TeleopArmController coordinator = quest3_teleop.build() coordinator.loop() ``` That's it! The blueprint includes: -- Quest3TeleopModule (VR calibration, delta computation) -- TeleopRobotController (robot calibration, delta application) +- VRTeleopModule (VR calibration, delta computation) +- TeleopArmController (robot calibration, delta application) - All LCM topic connections configured Then: @@ -103,7 +106,7 @@ Then: - Starts publishing **delta poses**: `delta = current - initial` 2. **Robot Calibration** (automatic on first delta): - - TeleopRobotController captures robot's initial pose + - TeleopArmController captures robot's initial pose - Applies deltas: `target_pose = robot_initial + delta` ### Data Flow @@ -121,12 +124,10 @@ Controllers โ†’ Device Module โ†’ Delta Poses โ†’ Arm Controller โ†’ Robot ## LCM Topics -All devices publish: +All devices publish per-controller topics: -- `left_controller_delta: Out[PoseStamped]` - Left delta pose -- `right_controller_delta: Out[PoseStamped]` - Right delta pose -- `left_trigger: Out[Float32]` - Left trigger/gripper value (0.0-1.0) -- `right_trigger: Out[Float32]` - Right trigger/gripper value (0.0-1.0) +- `controller_delta_{i}: Out[PoseStamped]` - Controller i delta pose +- `trigger_value_{i}: Out[Float32]` - Controller i trigger/gripper value (0.0-1.0) ## Example: Full System @@ -134,7 +135,7 @@ All devices publish: ```python from dimos.teleop.teleop_blueprints import quest3_teleop -# Pre-built blueprint with Quest3 + TeleopRobotController +# Pre-built blueprint with VR headset + TeleopArmController coordinator = quest3_teleop.build() coordinator.loop() ``` @@ -145,18 +146,21 @@ Or build custom: from dimos.core.blueprints import autoconnect from dimos.core.transport import LCMTransport from dimos.msgs.geometry_msgs import PoseStamped -from dimos.teleop.quest3 import quest3_teleop_module -from dimos.teleop import teleop_robot_controller +from dimos.msgs.std_msgs import Float32 +from dimos.teleop.devices.vr_headset import vr_teleop_module +from dimos.teleop.robot_controllers import teleop_arm_controller my_system = ( autoconnect( - quest3_teleop_module(signaling_port=8443), - teleop_robot_controller(driver_module_name="RobotDriver"), + vr_teleop_module(signaling_port=8443), + teleop_arm_controller(driver_module_name="RobotDriver"), your_robot, ) .transports({ - ("left_controller_delta", PoseStamped): LCMTransport("/teleop/left", PoseStamped), - ("right_controller_delta", PoseStamped): LCMTransport("/teleop/right", PoseStamped), + ("controller_delta_0", PoseStamped): LCMTransport("/teleop/left", PoseStamped), + ("controller_delta_1", PoseStamped): LCMTransport("/teleop/right", PoseStamped), + ("trigger_value_0", Float32): LCMTransport("/teleop/left_trigger", Float32), + ("trigger_value_1", Float32): LCMTransport("/teleop/right_trigger", Float32), }) ) @@ -166,6 +170,6 @@ coordinator.loop() ## Related Documentation -- [Base Module](base/README.md) - Creating new devices -- [Quest3 Module](quest3/README.md) - Quest 3 VR setup +- [Base Module](devices/base/README.md) - Creating new devices +- [VR Headset Module](devices/vr_headset/README.md) - VR headset setup - [Blueprints](../../docs/api/blueprints.md) - System composition diff --git a/dimos/teleop/__init__.py b/dimos/teleop/__init__.py index a098d2cce7..e9baa31921 100644 --- a/dimos/teleop/__init__.py +++ b/dimos/teleop/__init__.py @@ -12,14 +12,4 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Teleoperation modules for dimos.""" - -from dimos.teleop.base import BaseTeleopConfig, BaseTeleopModule -from dimos.teleop.quest3 import Quest3TeleopConfig, Quest3TeleopModule - -__all__ = [ - "BaseTeleopConfig", - "BaseTeleopModule", - "Quest3TeleopConfig", - "Quest3TeleopModule", -] +"""Teleoperation module for dimos.""" diff --git a/dimos/teleop/devices/__init__.py b/dimos/teleop/devices/__init__.py new file mode 100644 index 0000000000..d4783c1a82 --- /dev/null +++ b/dimos/teleop/devices/__init__.py @@ -0,0 +1,15 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Teleoperation devices.""" diff --git a/dimos/teleop/base/README.md b/dimos/teleop/devices/base/README.md similarity index 64% rename from dimos/teleop/base/README.md rename to dimos/teleop/devices/base/README.md index d7a6037940..8d0a16e3a1 100644 --- a/dimos/teleop/base/README.md +++ b/dimos/teleop/devices/base/README.md @@ -17,14 +17,14 @@ Abstract base class for all teleoperation devices in dimos. BaseTeleopModule (base class) โ”œโ”€โ”€ Calibration logic โ”œโ”€โ”€ Delta pose computation -โ”œโ”€โ”€ LCM publishing +โ”œโ”€โ”€ LCM publishing (per-controller topics) โ”œโ”€โ”€ Rerun visualization โ””โ”€โ”€ update_controller_poses() โ† Device-specific code calls this -Device-Specific Module (e.g., Quest3TeleopModule) +Device-Specific Module (e.g., VRTeleopModule) โ”œโ”€โ”€ Connection logic (WebSocket, USB, etc.) โ”œโ”€โ”€ Data parsing -โ””โ”€โ”€ Calls: update_controller_poses(left_pose, right_pose, left_gripper, right_gripper) +โ””โ”€โ”€ Calls: update_controller_poses(controller_poses, controller_gripper_values) ``` ## Creating a New Teleoperation Module @@ -34,10 +34,12 @@ Device-Specific Module (e.g., Quest3TeleopModule) 3. **Implement connection logic** in `start()` method 4. **Call `update_controller_poses()`** when new tracking data arrives +The module also exposes a blueprint helper as `base_teleop_module`. + ### Example: SpaceMouse Device ```python -from dimos.teleop.base import BaseTeleopConfig, BaseTeleopModule +from dimos.teleop.devices.base import BaseTeleopConfig, BaseTeleopModule @dataclass class SpaceMouseTeleopConfig(BaseTeleopConfig): @@ -57,12 +59,12 @@ class SpaceMouseTeleopModule(BaseTeleopModule): def _read_usb_loop(self): while self._running: # Parse USB data to 4x4 transformation matrices - left_pose, right_pose = parse_spacemouse_data(...) + controller_poses = parse_spacemouse_data(...) # Base class handles everything else! self.update_controller_poses( - left_pose, right_pose, - left_gripper=0.0, right_gripper=0.0 + controller_poses, + controller_gripper_values=[0.0] * len(controller_poses), ) ``` @@ -73,10 +75,13 @@ class SpaceMouseTeleopModule(BaseTeleopModule): ```python @dataclass class BaseTeleopConfig(ModuleConfig): + num_inputs: int = 1 # Number of controllers + enable_inputs: list[bool] = [] # Defaults to all enabled + input_labels: list[str] = [] # Defaults to controller_{i} position_scale: float = 1.0 # Scale factor for positions - enable_left_arm: bool = True # Enable left arm - enable_right_arm: bool = True # Enable right arm visualize_in_rerun: bool = True # Visualize in Rerun + log_input_data: bool = False # Log input pose/gripper data + log_input_data_interval: int = 100 # Log every N publishes when enabled safety_limits: bool = True # Enable safety limits max_velocity: float = 0.5 # m/s workspace_limits: dict[str, tuple[float, float]] # x, y, z limits @@ -86,19 +91,27 @@ class BaseTeleopConfig(ModuleConfig): All devices get these methods automatically: -- `calibrate_vr()` โ†’ Capture initial controller poses +- `calibrate()` โ†’ Capture initial controller poses - `reset_calibration()` โ†’ Reset calibration state -- `is_vr_calibrated()` โ†’ Check if calibrated +- `is_calibrated()` โ†’ Check if calibrated - `get_status()` โ†’ Get teleoperation status ## LCM Topics (Inherited) All devices publish these topics: -- `left_controller_delta: Out[PoseStamped]` - Left controller delta pose -- `right_controller_delta: Out[PoseStamped]` - Right controller delta pose -- `left_trigger: Out[Bool]` - Left trigger state -- `right_trigger: Out[Bool]` - Right trigger state +- `controller_delta_{i}: Out[PoseStamped]` - Controller i delta pose +- `trigger_value_{i}: Out[Float32]` - Controller i trigger/gripper value (0.0-1.0) + +## Status Keys + +`get_status()` returns a flat dict keyed by `TeleopStatusKey` templates: + +- `is_calibrated` +- `controller_{index}_enabled` +- `controller_{index}_has_data` +- `controller_{index}_gripper_value` +- `controller_{index}_label` ## Benefits @@ -108,4 +121,4 @@ All devices publish these topics: ## Existing Implementations -- **Quest3TeleopModule** (`dimos/teleop/quest3/`) - Meta Quest 3 VR headset via WebXR +- **VRTeleopModule** (`dimos/teleop/devices/vr_headset/`) - VR headset via WebXR (Quest 3 tested) diff --git a/dimos/teleop/base/__init__.py b/dimos/teleop/devices/base/__init__.py similarity index 84% rename from dimos/teleop/base/__init__.py rename to dimos/teleop/devices/base/__init__.py index c5c9f896ae..1bc0bcee96 100644 --- a/dimos/teleop/base/__init__.py +++ b/dimos/teleop/devices/base/__init__.py @@ -14,9 +14,12 @@ """Base teleoperation module exports.""" -from dimos.teleop.base.base_teleop_module import ( +from dimos.teleop.devices.base.base_teleop_module import ( BaseTeleopConfig, BaseTeleopModule, ) -__all__ = ["BaseTeleopConfig", "BaseTeleopModule"] +__all__ = [ + "BaseTeleopConfig", + "BaseTeleopModule", +] diff --git a/dimos/teleop/base/base_teleop_module.py b/dimos/teleop/devices/base/base_teleop_module.py similarity index 82% rename from dimos/teleop/base/base_teleop_module.py rename to dimos/teleop/devices/base/base_teleop_module.py index 513d7009d7..e02e1fd9b7 100644 --- a/dimos/teleop/base/base_teleop_module.py +++ b/dimos/teleop/devices/base/base_teleop_module.py @@ -100,16 +100,6 @@ class TeleopStatusKey(str, Enum): class BaseTeleopModule(Module, ABC): """Base class for teleoperation modules. - Provides common functionality for multi-controller teleoperation: - - Calibration: capture initial controller poses - - Delta computation: current - initial - - Publishing: delta poses and trigger states to LCM - - Visualization: Rerun integration - - RPC interface: standard methods for all devices - - Subclasses implement device-specific connection logic and call - `update_controller_poses()` when new tracking data arrives. - ## LCM Topics (Output): - controller_delta_{i}: Out[PoseStamped] - Controller i delta pose - trigger_value_{i}: Out[Float32] - Controller i trigger/gripper value (0.0-1.0) @@ -243,11 +233,7 @@ def calibrate(self) -> dict[str, Any]: @rpc def reset_calibration(self) -> dict[str, Any]: - """Reset calibration. Stops streaming until recalibrated. - - Returns: - Dict with 'success' and 'message' - """ + """Reset calibration. Stops streaming until recalibrated""" self._is_calibrated = False self._initial_poses = [None] * self.config.num_inputs @@ -256,20 +242,12 @@ def reset_calibration(self) -> dict[str, Any]: @rpc def is_calibrated(self) -> bool: - """Check if calibrated. - - Returns: - True if calibrated and streaming deltas - """ + """Check if calibrated""" return self._is_calibrated @rpc def get_status(self) -> dict[str, Any]: - """Get current teleoperation status. - - Returns: - Dictionary with status information keyed by TeleopStatusKey templates. - """ + """Get current teleoperation status""" status: dict[str, Any] = { TeleopStatusKey.IS_CALIBRATED.value: self._is_calibrated, } @@ -298,29 +276,14 @@ def update_controller_poses( controller_poses: Sequence[NDArray[np.float32] | None], controller_gripper_values: Sequence[float], ) -> None: - """Update controller poses and publish deltas if calibrated. - - This method should be called by subclasses when new tracking data arrives - from the device-specific connection. - - Args: - controller_poses: List of 4x4 transformation matrices for all controllers - controller_gripper_values: List of gripper values (0.0-1.0) for all controllers - """ + """Update controller poses and publish deltas if calibrated""" # Track how many tracking messages we've received self._tracking_msg_count += 1 - - # Log first few tracking messages to confirm data is arriving - if self._tracking_msg_count <= 3: - logger.info(f"Received tracking data #{self._tracking_msg_count}") - - # Store absolute poses self._all_poses = list(controller_poses) self._all_gripper_values = list(controller_gripper_values) # Only stream deltas if calibrated if not self._is_calibrated: - # Only log this warning once every 100 messages to avoid spam if self._tracking_msg_count <= 1 or self._tracking_msg_count % 100 == 0: logger.warning("Not calibrated. Calibrate to start teleoperation.") return @@ -345,6 +308,7 @@ def _compute_and_publish_deltas(self) -> None: initial_pose = self._initial_poses[i] pose_obj = matrix_to_pose(pose_matrix) input_label = self.config.input_labels[i] + delta_pose = pose_obj - initial_pose delta_pose_stamped = PoseStamped( ts=current_time, @@ -369,17 +333,6 @@ def _compute_and_publish_deltas(self) -> None: except Exception as e: logger.debug(f"Failed to publish {input_label} gripper: {e}") - if self.config.log_input_data and ( - self._publish_count <= 5 - or self._publish_count % self.config.log_input_data_interval == 0 - ): - logger.info( - f"Published {input_label} delta #{self._publish_count}: " - f"pos=[{delta_pose_stamped.position.x:.3f}, {delta_pose_stamped.position.y:.3f}, {delta_pose_stamped.position.z:.3f}], " - f"rpy=[{delta_pose_stamped.roll:.3f}, {delta_pose_stamped.pitch:.3f}, {delta_pose_stamped.yaw:.3f}], " - f"frame_id={delta_pose_stamped.frame_id}, " - f"trigger_value={self._all_gripper_values[i]:.3f}" - ) except Exception as e: logger.error(f"Error computing/publishing delta poses: {e}") @@ -388,12 +341,7 @@ def _compute_and_publish_deltas(self) -> None: # ========================================================================= def _visualize_controller_in_rerun(self, controller_pose: Pose, controller_label: str) -> None: - """Visualize controller absolute pose in Rerun. - - Args: - controller_pose: Absolute controller pose in robot space - controller_label: Controller label from input_labels - """ + """Visualize controller absolute pose in Rerun""" if not ( self.config.visualize_in_rerun and RERUN_AVAILABLE @@ -402,37 +350,25 @@ def _visualize_controller_in_rerun(self, controller_pose: Pose, controller_label return try: - # Convert to PoseStamped for Rerun visualization controller_pose_stamped = PoseStamped( ts=time.time(), frame_id=f"world/teleop/{controller_label}_controller", position=controller_pose.position, orientation=controller_pose.orientation, ) - # Log absolute controller pose transform rr.log( f"world/teleop/{controller_label}_controller", controller_pose_stamped.to_rerun(), # type: ignore[no-untyped-call] ) - # Log coordinate frame axes to visualize the transform rr.log( - f"world/teleop/{controller_label}_controller", - rr.Arrows3D( # type: ignore[attr-defined] - vectors=[[0.30, 0, 0], [0, 0.30, 0], [0, 0, 0.30]], - origins=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], - colors=[[255, 0, 0], [0, 255, 0], [0, 0, 255]], - ), + f"world/teleop/{controller_label}_controller/forward", + controller_pose_stamped.to_rerun_arrow(), # type: ignore[no-untyped-call] ) except Exception as e: logger.debug(f"Failed to log {controller_label} controller to Rerun: {e}") def _visualize_trigger_in_rerun(self, trigger_value: float, controller_label: str) -> None: - """Visualize trigger value in Rerun as a scalar time series. - - Args: - trigger_value: Trigger value (0.0-1.0) from controller - controller_label: Controller label from input_labels - """ + """Visualize trigger value in Rerun as a scalar time series.""" if not ( self.config.visualize_in_rerun and RERUN_AVAILABLE @@ -441,7 +377,6 @@ def _visualize_trigger_in_rerun(self, trigger_value: float, controller_label: st return try: - # Log trigger value as scalar time series rr.log( f"world/teleop/{controller_label}_controller/trigger", rr.Scalars(trigger_value), # type: ignore[attr-defined] diff --git a/dimos/teleop/quest3/README.md b/dimos/teleop/devices/vr_headset/README.md similarity index 57% rename from dimos/teleop/quest3/README.md rename to dimos/teleop/devices/vr_headset/README.md index c226574d97..27dcf77f43 100644 --- a/dimos/teleop/quest3/README.md +++ b/dimos/teleop/devices/vr_headset/README.md @@ -1,20 +1,20 @@ -# Quest3 VR Teleoperation +# VR Headset Teleoperation -VR teleoperation for Quest 3 headset with robot manipulation. +VR teleoperation for headsets (Quest 3 tested). ## Quick Start ### 1. Start Teleoperation ```python -from dimos.teleop.quest3 import Quest3TeleopModule +from dimos.teleop.devices.vr_headset import VRTeleopModule # Create and start -teleop = Quest3TeleopModule() +teleop = VRTeleopModule() teleop.start() # Output: -# Quest3 Teleoperation Module started on https://0.0.0.0:8443 +# VR Teleoperation Module started on https://0.0.0.0:8443 # Open this URL on Quest 3: https://:8443/ ``` @@ -37,24 +37,24 @@ teleop.start() 5. **Press X button** on controller to start teleoperation - First press: Calibrates (captures initial poses) and starts control - - Press again: Stops control (calibration preserved) - - Press again: Resumes control + - Press again: Stops control and resets calibration + - Press again: Calibrates again and resumes control ## Architecture ``` Quest 3 Browser โ†“ Opens https://your-ip:8443/ (WebXR) - โ†“ X button โ†’ calibrate_vr() via WebSocket + โ†“ X button โ†’ start_teleop/stop_teleop via WebSocket โ†“ FastAPI Server โ†“ Receives tracking data (controller poses) โ†“ -Quest3TeleopModule (inherits BaseTeleopModule) +VRTeleopModule (inherits BaseTeleopModule) โ†“ Computes delta poses (current - initial) โ†“ Publishes delta poses (PoseStamped) โ†“ -TeleopRobotController +TeleopArmController โ†“ Auto-calibrates robot on first delta โ†“ Applies: target = initial_robot + delta โ†“ Publishes cartesian commands (Pose) @@ -66,26 +66,31 @@ Robot Driver ## Key Features -Press X to start/stop teleoperation -Captures initial poses on first X press -Robot follows controller movement relative to initial pose -AR mode +- Press X to start/stop teleoperation (start = calibrate, stop = reset calibration) +- Captures initial poses on start +- Robot follows controller movement relative to initial pose +- AR mode + +The module exposes a blueprint helper as `vr_teleop_module`. ## Configuration ```python -from dimos.teleop.quest3 import Quest3TeleopModule, Quest3TeleopConfig +from dimos.teleop.devices.vr_headset import VRTeleopModule, VRTeleopConfig -config = Quest3TeleopConfig( - # Quest3-specific settings +config = VRTeleopConfig( + # VR headset settings signaling_host="0.0.0.0", signaling_port=8443, # HTTPS port (required for WebXR) use_https=True, # Required for Quest 3 WebXR # Inherited from BaseTeleopConfig - enable_left_arm=True, - enable_right_arm=True, + num_inputs=2, + enable_inputs=[True, True], + input_labels=["left_vr", "right_vr"], visualize_in_rerun=True, # Visualize controllers in Rerun + log_input_data=False, # Log input pose/gripper data + log_input_data_interval=100, # Log every N publishes when enabled position_scale=1.0, max_velocity=0.5, workspace_limits={ @@ -95,7 +100,7 @@ config = Quest3TeleopConfig( }, ) -module = Quest3TeleopModule(config=config) +module = VRTeleopModule(config=config) module.start() ``` @@ -135,19 +140,19 @@ module.start() Inherited from `BaseTeleopModule`: -- `left_controller_delta` (PoseStamped) - Left controller **delta** pose (current - initial) -- `right_controller_delta` (PoseStamped) - Right controller **delta** pose (current - initial) -- `left_trigger` (Bool) - Left trigger button state -- `right_trigger` (Bool) - Right trigger button state +- `controller_delta_0` (PoseStamped) - Controller 0 **delta** pose (current - initial) +- `controller_delta_1` (PoseStamped) - Controller 1 **delta** pose (current - initial) +- `trigger_value_0` (Float32) - Controller 0 trigger value (0.0-1.0) +- `trigger_value_1` (Float32) - Controller 1 trigger value (0.0-1.0) -**Note**: These are **delta poses**, not absolute poses. TeleopRobotController applies these deltas to the robot's initial pose. +**Note**: These are **delta poses**, not absolute poses. TeleopArmController applies these deltas to the robot's initial pose. `input_labels` are used in frame IDs and logging. ## Development ### Standalone Server Testing ```bash -cd dimos/teleop/quest3/control +cd dimos/teleop/devices/vr_headset/control python fastapi_server.py # Server starts on https://0.0.0.0:8443 @@ -158,20 +163,18 @@ python fastapi_server.py ```python from dimos.core.blueprints import autoconnect -from dimos.teleop.quest3 import quest3_teleop_module -from dimos.teleop import teleop_robot_controller +from dimos.teleop.devices.vr_headset import VRTeleopModule +from dimos.teleop.robot_controllers import teleop_arm_controller from your_robot import your_robot_blueprint custom_stack = autoconnect( - quest3_teleop_module( + VRTeleopModule( signaling_port=8443, - enable_left_arm=True, - enable_right_arm=False, + enable_inputs=[True, False], + input_labels=["left_vr", "right_vr"], ), - teleop_robot_controller( + teleop_arm_controller( driver_module_name="YourDriver", - enable_left_arm=True, - enable_right_arm=False, ), your_robot_blueprint, ) @@ -183,4 +186,4 @@ coordinator.loop() ## Related - [`BaseTeleopModule`](../base/) - Base class for all teleoperation devices -- [`TeleopRobotController`](../teleop_robot_controller.py) - Applies deltas to robot +- [`TeleopArmController`](../../robot_controllers/teleop_arm_controller.py) - Applies deltas to robot diff --git a/dimos/teleop/quest3/__init__.py b/dimos/teleop/devices/vr_headset/__init__.py similarity index 75% rename from dimos/teleop/quest3/__init__.py rename to dimos/teleop/devices/vr_headset/__init__.py index 181ff28410..b5c1d28107 100644 --- a/dimos/teleop/quest3/__init__.py +++ b/dimos/teleop/devices/vr_headset/__init__.py @@ -12,11 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Quest3 VR Teleoperation.""" +"""VR headset teleoperation exports.""" -from dimos.teleop.quest3.quest3_teleop_module import Quest3TeleopConfig, Quest3TeleopModule +from dimos.teleop.devices.vr_headset.vr_teleop_module import ( + VRTeleopModule, + vr_teleop_module, +) __all__ = [ - "Quest3TeleopConfig", - "Quest3TeleopModule", + "VRTeleopModule", + "vr_teleop_module", ] diff --git a/dimos/teleop/devices/vr_headset/certs/cert.pem b/dimos/teleop/devices/vr_headset/certs/cert.pem new file mode 100644 index 0000000000..f2bd46c098 --- /dev/null +++ b/dimos/teleop/devices/vr_headset/certs/cert.pem @@ -0,0 +1,19 @@ +-----BEGIN CERTIFICATE----- +MIIDCDCCAfCgAwIBAgITPnPCPHTIoeUModYTakOkeJct0TANBgkqhkiG9w0BAQsF +ADAUMRIwEAYDVQQDDAlsb2NhbGhvc3QwHhcNMjYwMTEwMjA0MzA5WhcNMjcwMTEw +MjA0MzA5WjAUMRIwEAYDVQQDDAlsb2NhbGhvc3QwggEiMA0GCSqGSIb3DQEBAQUA +A4IBDwAwggEKAoIBAQCqEtoiOzdJCJRAZhYegbk6oZ1Pl2SyEoTzapxyzVoqkqxQ +jorskwfWPpf4n9/LDtGn9z2dYg3kLwgC9Ce1gk9vEst1TCzfalCb1t/Hc0ImFT0h +ieJctMBoLlmXk7C+B3/k44xz8z8C5SB/sevK614kx+DXwDE1p+85SmsgELhayY2l +ZUgYsCzL0tGIqQWG/KYpe9VHffNtv+BFNirBsIGuUZjWAsNUQgzu6DoSpf69xhHM +oLDPMYcdKv//XCvCWDM5qLVVtDFHugIIhYB9BQRXswSBkDjwxLJQEr++yRHEQbxS +ivgeD8+lRseQLo288S7DG+5Y518YhdNvye007aS/AgMBAAGjUzBRMB0GA1UdDgQW +BBR8LLYvVdoeoagvZO4Y3YEHhIESfzAfBgNVHSMEGDAWgBR8LLYvVdoeoagvZO4Y +3YEHhIESfzAPBgNVHRMBAf8EBTADAQH/MA0GCSqGSIb3DQEBCwUAA4IBAQAssAOy +2z2RXgfINrz1ESg9GYs6GJtM5lC0ryO6egvtqgyXNjW0OZNOGo0RWyJnj2vbPg1d +x1AIyZAGvXtoEdO7x7Oeo7byGU1SFJAqBabW7zB3PsplpQcMEySmYOzGTlji2U5S +L8rhhRZw7adBRZfcEe5D44TmF7E1ulN2XHsKVor4ZTvF/q6ipVZUak50uVaw9zLK +VgfARoxayLLCZdKrsmO4QdWd/g3t6qvs31U3o2XQHxE8DKaOEhtv9mFzMj/bL+IC +dwIsxeO1yBkqXWqK4b6MoWn7bypHins13gtJVGP3kmQXWOHXOj+x+435X9TGRTcb +Ks4e7yxvfb17UYCd +-----END CERTIFICATE----- diff --git a/dimos/teleop/devices/vr_headset/certs/key.pem b/dimos/teleop/devices/vr_headset/certs/key.pem new file mode 100644 index 0000000000..53b072300f --- /dev/null +++ b/dimos/teleop/devices/vr_headset/certs/key.pem @@ -0,0 +1,28 @@ +-----BEGIN PRIVATE KEY----- +MIIEvAIBADANBgkqhkiG9w0BAQEFAASCBKYwggSiAgEAAoIBAQCqEtoiOzdJCJRA +ZhYegbk6oZ1Pl2SyEoTzapxyzVoqkqxQjorskwfWPpf4n9/LDtGn9z2dYg3kLwgC +9Ce1gk9vEst1TCzfalCb1t/Hc0ImFT0hieJctMBoLlmXk7C+B3/k44xz8z8C5SB/ +sevK614kx+DXwDE1p+85SmsgELhayY2lZUgYsCzL0tGIqQWG/KYpe9VHffNtv+BF +NirBsIGuUZjWAsNUQgzu6DoSpf69xhHMoLDPMYcdKv//XCvCWDM5qLVVtDFHugII +hYB9BQRXswSBkDjwxLJQEr++yRHEQbxSivgeD8+lRseQLo288S7DG+5Y518YhdNv +ye007aS/AgMBAAECggEAEYm4eSTeb8dqM5/eB7mnMQ7JwD445zdgrWdLiHfOT1Xp +CLTs9OC5Q1RuFc3gD+T7JWp/WJOWzMvgHsnbBG0n0CjsgS/gmriwZPmfVvEhm6K/ +YeOBoRS57fmwScte7kxWBHObSrkFQOusJ8Q1sABd03DR1Kh1iqEYMAdmt5ucdIQU +QtfXx0itrNDLW7nBsr1vHRavxbSN1KxbJ6bE6Bs2cZCr+1v7OpCkjdhjONDAXf6C +y1rSep/J10PUbl1WaiQZq1EmJrnLS6Gx8NIx+pNEYLMuw0aD/10j7Q2xLl8MHDp8 +l4Zhp0t5u9AOmigEvcY1d/0KgqzRaHjpHWw+B42XZQKBgQDLzYNxP+rBi/c7h074 +vRIuZmgzLRFqrAaQ+RWxzccasO1uzEkKPLT6lARTGrBRpvu6V05Vi1BWgfdSC1fN +v3sif6KaDyq5DhvWZpS7qeOEl16DLOO2AU3BbbwhCsOMnOQh1wNu6q/oKzO04VpY +8eJDVERK3MipU4DUVr2KEg/zrQKBgQDVodx5f8Rnb7htOGS8Kto0MHtiwLsAX5t6 +uGW4rZoMROK1C0yEhnmkGWKYkWtUwySkpn+PYKiMPhRvAg92OuGePlbwA9AygNUx +4dbNpZiSEUvoZ3IO9rhBE0TGnSIiB7G3eRZRdbKiHSGIpWYF8YCM/YyFvaPcmspC +lMuiDnTnmwKBgC7Tj4nr17KUyD+DPV/lgVHr8bEgf8n0sKWKtbNexoqZcFRu17Fk +dWjFraCylySLq3cLLFJ3agQWZI8TUB9UCaTJksC3D2fpl/fRJgdgZ4hFh0+4drGQ +5x60ae9lm7ypJ7mmv4Eypyw/EOhUhv+8w/IYYICa7fgJ2aXwMCiTMdsZAoGAC9ey +MiYMDDPcRGm449l4SSZa4KmQdD/YjaAFO4ycGowDCUg8EKinu5oQpiaBjaxXrqzw +K1GPZl7WoSS7GLHA6hXImfuMIhCUQPSlBLdmUsqUq6h4YS36HtljmaMCTmKgzmvu ++csNgQEeZ8XLdw7hMm+nx44wtDz8c15uP2iPwHsCgYBK7gXnGNxiZuS/0j13kYnP +A49Hk7UJs2Dienh2V7ORGLwrvWuPT7iyKbEwTEwkx17CxlIAcCCuQy098cohi99K +eVUE2KK8Zf4ffscmuj8ZAuSo9INuLqSr7rQ/7qh55mOmKXQxxu8h46CDlDZb03Wr +hLgQTQLFOMinyyWHvlkn2Q== +-----END PRIVATE KEY----- diff --git a/dimos/teleop/quest3/control/fastapi_server.py b/dimos/teleop/devices/vr_headset/control/fastapi_server.py similarity index 98% rename from dimos/teleop/quest3/control/fastapi_server.py rename to dimos/teleop/devices/vr_headset/control/fastapi_server.py index 7ef36794b6..0354dd055c 100644 --- a/dimos/teleop/quest3/control/fastapi_server.py +++ b/dimos/teleop/devices/vr_headset/control/fastapi_server.py @@ -13,14 +13,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -""" -FastAPI WebSocket server for Quest3 VR teleoperation. - -Handles WebSocket connections and processes VR controller tracking data -using FastAPI for better performance and features. - -Now includes HTTPS support and serves the standalone HTML VR client. -""" from __future__ import annotations @@ -36,7 +28,7 @@ from fastapi.responses import FileResponse import uvicorn -from dimos.teleop.quest3.control.tracking_processor import TrackingProcessor +from dimos.teleop.devices.vr_headset.control.tracking_processor import TrackingProcessor from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: diff --git a/dimos/teleop/quest3/control/tracking_processor.py b/dimos/teleop/devices/vr_headset/control/tracking_processor.py similarity index 100% rename from dimos/teleop/quest3/control/tracking_processor.py rename to dimos/teleop/devices/vr_headset/control/tracking_processor.py diff --git a/dimos/teleop/quest3/static/index.html b/dimos/teleop/devices/vr_headset/static/index.html similarity index 99% rename from dimos/teleop/quest3/static/index.html rename to dimos/teleop/devices/vr_headset/static/index.html index 9659e2fe23..7196d2b7e0 100644 --- a/dimos/teleop/quest3/static/index.html +++ b/dimos/teleop/devices/vr_headset/static/index.html @@ -116,8 +116,8 @@

Quest 3 VR Teleop

let xrRefSpace = null; let gl = null; let lastSendTime = 0; - const sendInterval = 0; // ~90Hz -- running at full speed - // const sendInterval = 1000 / 50; // ~50Hz target to achieve + // const sendInterval = 0; // ~90Hz -- running at full speed/ + const sendInterval = 1000 / 80; //target to achieve // Teleop state let teleopActive = false; diff --git a/dimos/teleop/quest3/quest3_teleop_module.py b/dimos/teleop/devices/vr_headset/vr_teleop_module.py similarity index 71% rename from dimos/teleop/quest3/quest3_teleop_module.py rename to dimos/teleop/devices/vr_headset/vr_teleop_module.py index 7365180817..1ca1f1e8c0 100644 --- a/dimos/teleop/quest3/quest3_teleop_module.py +++ b/dimos/teleop/devices/vr_headset/vr_teleop_module.py @@ -14,9 +14,9 @@ # limitations under the License. """ -Quest3 Teleoperation Module +VR Teleoperation Module -A dimos Module that runs the WebSocket signaling server for Quest3 VR teleoperation, +A dimos Module that runs the WebSocket signaling server for VR teleoperation, receives controller tracking data, and streams DELTA poses to TeleopRobotController. This module inherits from BaseTeleopModule which handles: @@ -26,8 +26,8 @@ - Rerun visualization - Standard RPC interface -This module implements Quest3-specific: -- FastAPI/WebSocket server for Quest3 VR headset +This module implements VR-specific: +- FastAPI/WebSocket server for VR headset - X button handler for calibration trigger """ @@ -40,8 +40,8 @@ from typing import TYPE_CHECKING, Any from dimos.core import Out, rpc -from dimos.teleop.base import BaseTeleopConfig, BaseTeleopModule -from dimos.teleop.quest3.control.fastapi_server import TeleopFastAPIServer +from dimos.teleop.devices.base import BaseTeleopConfig, BaseTeleopModule +from dimos.teleop.devices.vr_headset.control.fastapi_server import TeleopFastAPIServer from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -56,10 +56,10 @@ @dataclass -class Quest3TeleopConfig(BaseTeleopConfig): - """Configuration for Quest3 Teleoperation Module.""" +class VRTeleopConfig(BaseTeleopConfig): + """Configuration for VR Teleoperation Module.""" - # Quest3-specific WebSocket server settings + # VR-specific WebSocket server settings signaling_host: str = "0.0.0.0" signaling_port: int = 8443 # HTTPS port for WebXR use_https: bool = True # Enable HTTPS for WebXR (required by Quest 3) @@ -74,41 +74,21 @@ class Quest3TeleopConfig(BaseTeleopConfig): log_input_data: bool = False # Log input pose/gripper data periodically log_input_data_interval: int = 100 # Log every N publishes when enabled - # Other inherited from BaseTeleopConfig (with defaults): - # safety_limits: bool = True # Enable safety limits - # - position_scale: float = 1.0 - # - max_velocity: float = 0.5 - # - workspace_limits: dict[str, tuple[float, float]] - -class Quest3TeleopModule(BaseTeleopModule): - """Quest3 VR Teleoperation Module. - - Inherits calibration, delta computation, and visualization from BaseTeleopModule. - Implements Quest3-specific WebSocket server and VR connection logic. - - ## Device-Specific Features: - - FastAPI/WebSocket server for Quest3 VR headset communication - - HTTPS support for WebXR (required by Quest 3) - - X button handler for calibration trigger +class VRTeleopModule(BaseTeleopModule): + """VR Teleoperation Module. ## LCM Topics (inherited from base): - controller_delta_0: Out[PoseStamped] - Controller 0 delta pose - - controller_delta_1: Out[PoseStamped] - Controller 1 delta pose - trigger_value_0: Out[Float32] - Controller 0 trigger/gripper value (0.0-1.0) - - trigger_value_1: Out[Float32] - Controller 1 trigger/gripper value (0.0-1.0) - ## RPC Methods (inherited from base): - - start() -> None: Start the module and signaling server - - stop() -> None: Stop the module and signaling server - - calibrate() -> dict: Calibrate (capture initial controller poses) - - reset_calibration() -> dict: Reset calibration - - is_calibrated() -> bool: Check if calibrated - - get_status() -> dict: Get current teleoperation status + ## Additional LCM Topics: + - controller_delta_1: Out[PoseStamped] - Controller 1 delta pose + - trigger_value_1: Out[Float32] - Controller 1 trigger/gripper value (0.0-1.0) """ - default_config = Quest3TeleopConfig - config: Quest3TeleopConfig + default_config = VRTeleopConfig + config: VRTeleopConfig # LCM output topics for VR (controller_0 is inherited from base) controller_delta_1: Out[PoseStamped] = None # type: ignore[assignment] @@ -122,41 +102,34 @@ def __init__( # Pass global_config as positional argument to match base class signature super().__init__(global_config, *args, **kwargs) - # Quest3-specific: FastAPI WebSocket server + # VR-specific: FastAPI WebSocket server self._fastapi_server: TeleopFastAPIServer | None = None self._server_thread: threading.Thread | None = None self._event_loop: asyncio.AbstractEventLoop | None = None - logger.info("Quest3TeleopModule initialized") + logger.info("VRTeleopModule initialized") # ========================================================================= - # Module Lifecycle (Quest3-specific) + # Module Lifecycle (VR-specific) # ========================================================================= @rpc def start(self) -> None: - """Start the Quest3 teleoperation module and signaling server.""" - logger.info("Starting Quest3 Teleoperation Module...") - - # Call base class start (handles Rerun connection, etc.) + """Start the VR teleoperation module and signaling server.""" super().start() - # Start Quest3-specific signaling server + # Start VR-specific signaling server self._start_signaling_server() protocol = "https" if self.config.use_https else "http" logger.info( - f"Quest3 Teleoperation Module started on {protocol}://{self.config.signaling_host}:{self.config.signaling_port}" - ) - logger.info( - f"Open this URL on Quest 3: {protocol}://:{self.config.signaling_port}/" + f"VR Teleoperation Module started: Open this URL on Quest 3: {protocol}://:{self.config.signaling_port}/" ) - logger.info("Press X button in VR to calibrate and start teleoperation") @rpc def stop(self) -> None: - """Stop the Quest3 teleoperation module and signaling server.""" - logger.info("Stopping Quest3 Teleoperation Module...") + """Stop the VR teleoperation module and signaling server.""" + logger.info("Stopping VR Teleoperation Module...") # Stop signaling server self._stop_signaling_server() @@ -214,28 +187,16 @@ def _stop_signaling_server(self) -> None: logger.info("FastAPI server stopped") # ========================================================================= - # Quest3-Specific: X Button Handler + # VR-Specific: X Button Handler # ========================================================================= async def _handle_x_button(self, command_type: str, websocket: Any) -> dict[str, Any]: """Handle X button press from VR client. - - X button toggles calibration: - - If not calibrated: calibrate VR - - If calibrated: reset calibration (stop streaming) - - Args: - command_type: 'start_teleop' or 'stop_teleop' - websocket: WebSocket connection to send responses - - Returns: - Response dictionary to send back to client + X button toggles calibration and Starts/stops teleoperation """ try: if command_type == "start_teleop": logger.info("X button pressed - calibrating VR...") - pose_states = [pose is not None for pose in self._all_poses] - logger.info(f"Current state: controller_poses={pose_states}") result = self.calibrate() logger.info(f"Calibration result: {result}") @@ -266,7 +227,7 @@ async def _handle_x_button(self, command_type: str, websocket: Any) -> dict[str, return {"type": "error", "error": f"Unknown command: {command_type}"} # ========================================================================= - # Quest3-Specific: Controller Data Processing + # Data Processing # ========================================================================= def _on_tracking_data( @@ -274,9 +235,9 @@ def _on_tracking_data( controller_poses: list[NDArray[np.float32]], controller_gripper_values: list[float], ) -> None: - """Receive tracking data from Quest3 VR. + """Receive tracking data from VR. - Called by the FastAPI server when new tracking data arrives from Quest3. + Called by the FastAPI server when new tracking data arrives from VR. Delegates to base class method which handles calibration, delta computation, and publishing. @@ -289,4 +250,4 @@ def _on_tracking_data( # Expose blueprint for declarative composition -quest3_teleop_module = Quest3TeleopModule.blueprint +vr_teleop_module = VRTeleopModule.blueprint diff --git a/dimos/teleop/robot_controllers/__init__.py b/dimos/teleop/robot_controllers/__init__.py new file mode 100644 index 0000000000..545338db6d --- /dev/null +++ b/dimos/teleop/robot_controllers/__init__.py @@ -0,0 +1,25 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Teleoperation robot controllers.""" + +from dimos.teleop.robot_controllers.teleop_arm_controller import ( + TeleopArmController, + teleop_arm_controller, +) + +__all__ = [ + "TeleopArmController", + "teleop_arm_controller", +] diff --git a/dimos/teleop/teleop_robot_controller.py b/dimos/teleop/robot_controllers/teleop_arm_controller.py similarity index 90% rename from dimos/teleop/teleop_robot_controller.py rename to dimos/teleop/robot_controllers/teleop_arm_controller.py index 1068a8fd47..673fc56c92 100644 --- a/dimos/teleop/teleop_robot_controller.py +++ b/dimos/teleop/robot_controllers/teleop_arm_controller.py @@ -13,7 +13,7 @@ # limitations under the License. """ -Teleop Robot Controller +Teleop Arm Controller Receives controller delta poses from Quest3TeleopModule and applies them to the robot. Auto-calibrates robot on first delta received. @@ -46,11 +46,11 @@ @dataclass -class TeleopRobotControllerConfig(ModuleConfig): - """Configuration for Teleop Robot Controller.""" +class TeleopArmControllerConfig(ModuleConfig): + """Configuration for Teleop Arm Controller.""" # Driver settings - driver_module_name: str = "RobotDriver" # Name of the driver module to get robot pose from + driver_module_name: str = "ArmDriver" # Name of the driver module to get robot pose from driver_method_name: str = "get_state" dummy_driver: bool = False # If True, skip RPC calls and use zeros for initial pose @@ -61,8 +61,8 @@ class TeleopRobotControllerConfig(ModuleConfig): delta_timeout: float = 1 # Seconds - stop publishing if no new delta received -class TeleopRobotController(Module): - """Teleop Robot Controller - applies delta poses to robot. +class TeleopArmController(Module): + """Teleop Arm Controller - applies delta poses to robot. This controller: 1. Receives DELTA poses (PoseStamped) from Quest3TeleopModule for a single controller @@ -76,8 +76,8 @@ class TeleopRobotController(Module): - cartesian_command input topic (Pose) """ - default_config = TeleopRobotControllerConfig - config: TeleopRobotControllerConfig + default_config = TeleopArmControllerConfig + config: TeleopArmControllerConfig # Input topics - receiving DELTA poses as PoseStamped controller_delta: In[PoseStamped] = None # type: ignore[assignment] @@ -106,7 +106,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: # Timestamps for delta timeout detection self._delta_timestamp: float | None = None - # Robot initial state (auto-calibrated on first delta) + # Arm initial state (auto-calibrated on first delta) self._robot_initial_pose: Pose | None = None self._robot_calibrated = False @@ -117,7 +117,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: # State lock self._state_lock = threading.Lock() - logger.info("TeleopRobotController initialized - waiting for delta poses") + logger.info("TeleopArmController initialized - waiting for delta poses") # ========================================================================= # Module Lifecycle @@ -126,7 +126,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: @rpc def start(self) -> None: """Start the teleop robot controller.""" - logger.info("Starting TeleopRobotController...") + logger.info("Starting TeleopArmController...") super().start() # Subscribe to input topics (delta poses) @@ -145,16 +145,16 @@ def start(self) -> None: # Start control loop self._stop_event.clear() self._control_thread = threading.Thread( - target=self._control_loop, daemon=True, name="TeleopRobotController" + target=self._control_loop, daemon=True, name="TeleopArmController" ) self._control_thread.start() - logger.info("TeleopRobotController started - waiting for delta poses to auto-calibrate") + logger.info("TeleopArmController started - waiting for delta poses to auto-calibrate") @rpc def stop(self) -> None: - """Stop the teleop Robot controller.""" - logger.info("Stopping TeleopRobotController") + """Stop the teleop Arm controller.""" + logger.info("Stopping TeleopArmController") # Stop control loop self._stop_event.set() @@ -162,7 +162,7 @@ def stop(self) -> None: self._control_thread.join(timeout=2.0) super().stop() - logger.info("TeleopRobotController stopped") + logger.info("TeleopArmController stopped") # ========================================================================= # Input Callbacks @@ -198,7 +198,7 @@ def _on_trigger_value(self, gripper: Float32) -> None: self._gripper_value = float(gripper.data) # ========================================================================= - # Robot Calibration (Auto-triggered on first delta) + # Arm Calibration (Auto-triggered on first delta) # ========================================================================= def _calibrate_robot(self) -> bool: @@ -222,7 +222,7 @@ def _calibrate_robot(self) -> bool: logger.info("Initial pose: pos=[0, 0, 0], rpy=[0, 0, 0]") self._robot_calibrated = True - logger.info("Robot calibration complete (dummy mode) - control active!") + logger.info("Arm calibration complete (dummy mode) - control active!") return True try: @@ -251,9 +251,9 @@ def _calibrate_robot(self) -> bool: position=position, orientation=Quaternion.from_euler(rpy), ) - logger.info(f"Robot initial pose: {pose_data}") + logger.info(f"Arm initial pose: {pose_data}") self._robot_calibrated = True - logger.info("Robot calibration complete - control active!") + logger.info("Arm calibration complete - control active!") return True error_msg = f"Failed to get robot cartesian state: {result}" @@ -261,7 +261,7 @@ def _calibrate_robot(self) -> bool: return False except Exception as e: - logger.error(f"Robot calibration failed: {e}", exc_info=True) + logger.error(f"Arm calibration failed: {e}", exc_info=True) traceback.print_exc() return False @@ -276,7 +276,7 @@ def recalibrate_robot(self) -> dict[str, Any]: success = self._calibrate_robot() if success: - return {"success": True, "message": "Robot recalibrated"} + return {"success": True, "message": "Arm recalibrated"} else: return {"success": False, "error": "Recalibration failed"} @@ -364,4 +364,4 @@ def _control_loop(self) -> None: # Expose blueprint for declarative composition -teleop_robot_controller = TeleopRobotController.blueprint +teleop_arm_controller = TeleopArmController.blueprint diff --git a/dimos/teleop/teleop_blueprints.py b/dimos/teleop/teleop_blueprints.py index f90d98f0f2..421312406d 100644 --- a/dimos/teleop/teleop_blueprints.py +++ b/dimos/teleop/teleop_blueprints.py @@ -49,7 +49,7 @@ enable_inputs=[True, True], input_labels=["left_vr", "right_vr"], ), - teleop_robot_controller( + teleop_arm_controller( driver_module_name="MyRobotDriver", ), my_robot_driver(...), @@ -60,36 +60,26 @@ from dimos.core.transport import LCMTransport from dimos.msgs.geometry_msgs import PoseStamped from dimos.msgs.std_msgs import Float32 -from dimos.teleop.quest3.quest3_teleop_module import quest3_teleop_module -from dimos.teleop.teleop_robot_controller import TeleopRobotController, teleop_robot_controller +from dimos.teleop.devices.vr_headset.vr_teleop_module import vr_teleop_module +from dimos.teleop.robot_controllers import TeleopArmController, teleop_arm_controller # ============================================================================= # Quest3 Teleoperation Blueprint # ============================================================================= -# Combines: -# - Quest3TeleopModule: VR calibration + delta computation -# - TeleopRobotController: Robot calibration + delta application (single controller) -# -# Data flow: -# Quest3TeleopModule.controller_delta_0 -- TeleopRobotController.controller_delta -# Quest3TeleopModule.trigger_value_0 -- TeleopRobotController.trigger_value -# TeleopRobotController.cartesian_command -- Robot Driver (via LCM) -# TeleopRobotController.gripper_command -- Robot Driver (via LCM) -# ============================================================================= quest3_teleop = ( autoconnect( - quest3_teleop_module( + vr_teleop_module( signaling_host="0.0.0.0", signaling_port=8443, # HTTPS port (required for WebXR) use_https=True, # Enable HTTPS for Quest 3 WebXR num_inputs=2, - enable_inputs=[True, False], + enable_inputs=[True, True], input_labels=["left_vr", "right_vr"], visualize_in_rerun=True, safety_limits=True, ), - teleop_robot_controller( + teleop_arm_controller( driver_module_name="DummyDriver", dummy_driver=True, # Skip RPC calls, use zeros for initial pose control_frequency=50.0, # Hz - control loop frequency @@ -97,17 +87,16 @@ ) .remappings( [ - (TeleopRobotController, "controller_delta", "controller_delta_0"), - (TeleopRobotController, "trigger_value", "trigger_value_0"), + (TeleopArmController, "controller_delta", "controller_delta_0"), + (TeleopArmController, "trigger_value", "trigger_value_0"), ] ) .transports( { - # Delta poses from Quest3TeleopModule to TeleopRobotController ("controller_delta_0", PoseStamped): LCMTransport( - "/quest3/controller_delta_0", PoseStamped + "devices/vr_headset/controller_delta", PoseStamped ), - ("trigger_value_0", Float32): LCMTransport("/quest3/trigger_value_0", Float32), + ("trigger_value_0", Float32): LCMTransport("devices/vr_headset/trigger_value", Float32), } ) ) From 9350c218e6d10502bce18af5ddaa728430483291 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Mon, 12 Jan 2026 19:07:25 -0800 Subject: [PATCH 40/72] Feat: Websocket via LCM --- .../teleop/devices/base/base_teleop_module.py | 5 +- ...{fastapi_server.py => fastapi_server_x.py} | 0 .../devices/vr_headset/static/index.html | 173 +++++------ .../vr_headset/static/teleop_server.ts | 70 +++++ .../devices/vr_headset/vr_teleop_module.py | 274 +++++++++--------- dimos/teleop/teleop_blueprints.py | 23 +- 6 files changed, 304 insertions(+), 241 deletions(-) rename dimos/teleop/devices/vr_headset/control/{fastapi_server.py => fastapi_server_x.py} (100%) create mode 100644 dimos/teleop/devices/vr_headset/static/teleop_server.ts diff --git a/dimos/teleop/devices/base/base_teleop_module.py b/dimos/teleop/devices/base/base_teleop_module.py index e02e1fd9b7..5326dcf17c 100644 --- a/dimos/teleop/devices/base/base_teleop_module.py +++ b/dimos/teleop/devices/base/base_teleop_module.py @@ -360,9 +360,10 @@ def _visualize_controller_in_rerun(self, controller_pose: Pose, controller_label f"world/teleop/{controller_label}_controller", controller_pose_stamped.to_rerun(), # type: ignore[no-untyped-call] ) + # Log 3D axes to visualize controller orientation (X=red, Y=green, Z=blue) rr.log( - f"world/teleop/{controller_label}_controller/forward", - controller_pose_stamped.to_rerun_arrow(), # type: ignore[no-untyped-call] + f"world/teleop/{controller_label}_controller/axes", + rr.Axes3D(length=0.15), # type: ignore[attr-defined] ) except Exception as e: logger.debug(f"Failed to log {controller_label} controller to Rerun: {e}") diff --git a/dimos/teleop/devices/vr_headset/control/fastapi_server.py b/dimos/teleop/devices/vr_headset/control/fastapi_server_x.py similarity index 100% rename from dimos/teleop/devices/vr_headset/control/fastapi_server.py rename to dimos/teleop/devices/vr_headset/control/fastapi_server_x.py diff --git a/dimos/teleop/devices/vr_headset/static/index.html b/dimos/teleop/devices/vr_headset/static/index.html index 7196d2b7e0..b6d325a493 100644 --- a/dimos/teleop/devices/vr_headset/static/index.html +++ b/dimos/teleop/devices/vr_headset/static/index.html @@ -110,14 +110,21 @@

Quest 3 VR Teleop

+ + diff --git a/dimos/teleop/web/teleop_server.ts b/dimos/teleop/web/teleop_server.ts new file mode 100644 index 0000000000..e07c8c930f --- /dev/null +++ b/dimos/teleop/web/teleop_server.ts @@ -0,0 +1,70 @@ +#!/usr/bin/env -S deno run --allow-net --allow-read --unstable-net + +// LCM to WebSocket Bridge for Robot Control +// Forwards robot pose to browser, receives twist commands from browser + +import { LCM } from "jsr:@dimos/lcm"; +import { decodePacket, geometry_msgs } from "jsr:@dimos/msgs"; + +const PORT = 8443; +const clients = new Set(); +const cert = await Deno.readTextFile("./certs/cert.pem"); +const key = await Deno.readTextFile("./certs/key.pem"); + +Deno.serve({ port: PORT, cert, key }, async (req) => { + const url = new URL(req.url); + + if (req.headers.get("upgrade") === "websocket") { + const { socket, response } = Deno.upgradeWebSocket(req); + socket.onopen = () => { console.log("Client connected"); clients.add(socket); }; + socket.onclose = () => { console.log("Client disconnected"); clients.delete(socket); }; + socket.onerror = () => clients.delete(socket); + + // Forward binary LCM packets from browser directly to UDP + socket.binaryType = "arraybuffer"; + let msgCount = 0; + socket.onmessage = async (event) => { + msgCount++; + if (event.data instanceof ArrayBuffer) { + const packet = new Uint8Array(event.data); + try { + await lcm.publishPacket(packet); + } catch (e) { + console.error("Forward error:", e); + } + } + }; + + return response; + } + + if (url.pathname === "/" || url.pathname === "/index.html") { + const html = await Deno.readTextFile(new URL("./static/index.html", import.meta.url)); + return new Response(html, { headers: { "content-type": "text/html" } }); + } + + return new Response("Not found", { status: 404 }); +}); + +console.log(`Server: https://localhost:${PORT}`); + +const lcm = new LCM(); +await lcm.start(); + +// Subscribe to pose and just log to show how server can decode messages for itself +// lcm.subscribe("/odom", geometry_msgs.PoseStamped, (msg) => { +// const pos = msg.data.pose.position; +// const ori = msg.data.pose.orientation; +// console.log(`[pose] x=${pos.x.toFixed(2)} y=${pos.y.toFixed(2)} z=${pos.z.toFixed(2)}`); +// }); + +// Forward all raw packets to browser (we are decoding LCM directly in the browser) +lcm.subscribePacket((packet) => { + for (const client of clients) { + if (client.readyState === WebSocket.OPEN) { + client.send(packet); + } + } +}); + +await lcm.run(); From f50818099ceb4c632662b34021c4977843479694 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 14 Jan 2026 01:41:26 -0800 Subject: [PATCH 47/72] Misc: Ifnore certificates --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index 0e6781f3a8..038bf2e1f7 100644 --- a/.gitignore +++ b/.gitignore @@ -63,3 +63,6 @@ yolo11n.pt /.mypy_cache* *mobileclip* + +# Teleop SSL certificates +dimos/teleop/web/certs/ From 4396da9145338ff72066dbbbdc3906c4484128d5 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 14 Jan 2026 01:41:42 -0800 Subject: [PATCH 48/72] Fix: updated addresses --- dimos/teleop/devices/base/README.md | 124 ----- dimos/teleop/devices/base/__init__.py | 25 - .../teleop/devices/base/base_teleop_module.py | 386 --------------- dimos/teleop/devices/vr_headset/README.md | 189 -------- dimos/teleop/devices/vr_headset/__init__.py | 25 - .../teleop/devices/vr_headset/certs/cert.pem | 19 - dimos/teleop/devices/vr_headset/certs/key.pem | 28 -- .../vr_headset/control/fastapi_server_x.py | 449 ------------------ .../vr_headset/control/tracking_processor.py | 153 ------ .../devices/vr_headset/static/index.html | 412 ---------------- .../vr_headset/static/teleop_server.ts | 70 --- .../devices/vr_headset/vr_teleop_module.py | 266 ----------- dimos/teleop/robot_controllers/__init__.py | 25 - .../teleop_arm_controller.py | 367 -------------- 14 files changed, 2538 deletions(-) delete mode 100644 dimos/teleop/devices/base/README.md delete mode 100644 dimos/teleop/devices/base/__init__.py delete mode 100644 dimos/teleop/devices/base/base_teleop_module.py delete mode 100644 dimos/teleop/devices/vr_headset/README.md delete mode 100644 dimos/teleop/devices/vr_headset/__init__.py delete mode 100644 dimos/teleop/devices/vr_headset/certs/cert.pem delete mode 100644 dimos/teleop/devices/vr_headset/certs/key.pem delete mode 100644 dimos/teleop/devices/vr_headset/control/fastapi_server_x.py delete mode 100644 dimos/teleop/devices/vr_headset/control/tracking_processor.py delete mode 100644 dimos/teleop/devices/vr_headset/static/index.html delete mode 100644 dimos/teleop/devices/vr_headset/static/teleop_server.ts delete mode 100644 dimos/teleop/devices/vr_headset/vr_teleop_module.py delete mode 100644 dimos/teleop/robot_controllers/__init__.py delete mode 100644 dimos/teleop/robot_controllers/teleop_arm_controller.py diff --git a/dimos/teleop/devices/base/README.md b/dimos/teleop/devices/base/README.md deleted file mode 100644 index eb1d935af7..0000000000 --- a/dimos/teleop/devices/base/README.md +++ /dev/null @@ -1,124 +0,0 @@ -# Base Teleoperation Module [TODO: Update] - -Abstract base class for all teleoperation devices in dimos. - -## Overview - -`BaseTeleopModule` provides common functionality for teleoperation devices: -- **Calibration**: Capture initial controller poses as reference -- **Delta computation**: Calculate `current - initial` poses -- **Publishing**: Stream delta poses and trigger states to LCM -- **Visualization**: Rerun integration for controller poses -- **RPC interface**: Standard methods across all devices - -## Architecture - -``` -BaseTeleopModule (base class) -โ”œโ”€โ”€ Calibration logic -โ”œโ”€โ”€ Delta pose computation -โ”œโ”€โ”€ LCM publishing (per-controller topics) -โ”œโ”€โ”€ Rerun visualization -โ””โ”€โ”€ update_controller_poses() โ† Device-specific code calls this - -Device-Specific Module (e.g., VRTeleopModule) -โ”œโ”€โ”€ Connection logic (WebSocket, USB, etc.) -โ”œโ”€โ”€ Data parsing -โ””โ”€โ”€ Calls: update_controller_poses(controller_poses, controller_gripper_values) -``` - -## Creating a New Teleoperation Module - -1. **Inherit from `BaseTeleopModule`** -2. **Define device-specific config** (inherit from `BaseTeleopConfig`) -3. **Implement connection logic** in `start()` method -4. **Call `update_controller_poses()`** when new tracking data arrives - -The module also exposes a blueprint helper as `base_teleop_module`. - -### Example: SpaceMouse Device - -```python -from dimos.teleop.devices.base import BaseTeleopConfig, BaseTeleopModule - -@dataclass -class SpaceMouseTeleopConfig(BaseTeleopConfig): - usb_device_id: str = "/dev/input/spacemouse" - # Any additional params - -class SpaceMouseTeleopModule(BaseTeleopModule): - default_config = SpaceMouseTeleopConfig - - @rpc - def start(self): - super().start() - # Start USB reader thread - self._usb_thread = threading.Thread(target=self._read_usb_loop) - self._usb_thread.start() - - def _read_usb_loop(self): - while self._running: - # Parse USB data to 4x4 transformation matrices - controller_poses = parse_spacemouse_data(...) - - # Base class handles everything else! - self.update_controller_poses( - controller_poses, - controller_gripper_values=[0.0] * len(controller_poses), - ) -``` - -## Configuration - -### BaseTeleopConfig - -```python -@dataclass -class BaseTeleopConfig(ModuleConfig): - num_inputs: int = 1 # Number of controllers - enable_inputs: list[bool] = [] # Defaults to all enabled - input_labels: list[str] = [] # Defaults to controller_{i} - position_scale: float = 1.0 # Scale factor for positions - visualize_in_rerun: bool = True # Visualize in Rerun - log_input_data: bool = False # Log input pose/gripper data - log_input_data_interval: int = 100 # Log every N publishes when enabled - safety_limits: bool = True # Enable safety limits - max_velocity: float = 0.5 # m/s - workspace_limits: dict[str, tuple[float, float]] # x, y, z limits -``` - -## RPC Methods (Inherited) - -All devices get these methods automatically: - -- `calibrate()` โ†’ Capture initial controller poses -- `reset_calibration()` โ†’ Reset calibration state -- `is_calibrated()` โ†’ Check if calibrated -- `get_status()` โ†’ Get teleoperation status - -## LCM Topics (Inherited) - -All devices publish these topics: - -- `controller_delta_{i}: Out[PoseStamped]` - Controller i delta pose -- `trigger_value_{i}: Out[Float32]` - Controller i trigger/gripper value (0.0-1.0) - -## Status Keys - -`get_status()` returns a flat dict keyed by `TeleopStatusKey` templates: - -- `is_calibrated` -- `controller_{index}_enabled` -- `controller_{index}_has_data` -- `controller_{index}_gripper_value` -- `controller_{index}_label` - -## Benefits - -- **Reusability**: Write device connection once, inherit everything else -- **Consistency**: Same RPC interface and behavior across all devices -- **Easy testing**: Base class tested independently - -## Existing Implementations - -- **VRTeleopModule** (`dimos/teleop/devices/vr_headset/`) - VR headset via WebXR (Quest 3 tested) diff --git a/dimos/teleop/devices/base/__init__.py b/dimos/teleop/devices/base/__init__.py deleted file mode 100644 index 1bc0bcee96..0000000000 --- a/dimos/teleop/devices/base/__init__.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Base teleoperation module exports.""" - -from dimos.teleop.devices.base.base_teleop_module import ( - BaseTeleopConfig, - BaseTeleopModule, -) - -__all__ = [ - "BaseTeleopConfig", - "BaseTeleopModule", -] diff --git a/dimos/teleop/devices/base/base_teleop_module.py b/dimos/teleop/devices/base/base_teleop_module.py deleted file mode 100644 index 5326dcf17c..0000000000 --- a/dimos/teleop/devices/base/base_teleop_module.py +++ /dev/null @@ -1,386 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -""" -Base Teleoperation Module - -Abstract base class for all teleoperation devices that provides: -- Multi-controller calibration -- Delta pose computation (current - initial) -- LCM topic publishing (delta poses + trigger states) -- Rerun visualization -- Standard RPC interface - -Device-specific modules inherit from this and implement their connection logic. -""" - -from __future__ import annotations - -from abc import ABC -from dataclasses import dataclass, field -from enum import Enum -import time -from typing import TYPE_CHECKING, Any - -from dimos.core import Module, Out, rpc -from dimos.core.global_config import GlobalConfig -from dimos.core.module import ModuleConfig -from dimos.msgs.geometry_msgs import Pose, PoseStamped -from dimos.msgs.std_msgs import Float32 -from dimos.utils.logging_config import setup_logger -from dimos.utils.transform_utils import matrix_to_pose - -if TYPE_CHECKING: - from collections.abc import Sequence - - import numpy as np - from numpy.typing import NDArray - -logger = setup_logger() - -try: - import rerun as rr - - from dimos.dashboard.rerun_init import connect_rerun - - RERUN_AVAILABLE = True -except ImportError: - RERUN_AVAILABLE = False - - -@dataclass -class BaseTeleopConfig(ModuleConfig): - """Base configuration for teleoperation modules.""" - - # Control settings - num_inputs: int = 1 # Number of inputs (controllers) - enable_inputs: list[bool] = field(default_factory=list) - input_labels: list[str] = field(default_factory=list) - - # Visualization settings - visualize_in_rerun: bool = True # Visualize controller poses in Rerun - log_input_data: bool = False # Log input pose/gripper data periodically - log_input_data_interval: int = 100 # Log every N publishes when enabled - - # Safety limits - safety_limits: bool = True - position_scale: float = 1.0 # Scale factor for positions TODO: Implement proportional scaling - max_velocity: float = 0.5 # m/s - workspace_limits: dict[str, tuple[float, float]] = field( - default_factory=lambda: { - "x": (-1.0, 1.0), - "y": (-0.8, 0.8), - "z": (0.1, 2.0), - } - ) - - -class TeleopStatusKey(str, Enum): - """Status dictionary keys (controller_* entries are indexed by controller number).""" - - IS_CALIBRATED = "is_calibrated" - CONTROLLER_ENABLED = "controller_{index}_enabled" - CONTROLLER_HAS_DATA = "controller_{index}_has_data" - CONTROLLER_GRIPPER_VALUE = "controller_{index}_gripper_value" - CONTROLLER_LABEL = "controller_{index}_label" - - -class BaseTeleopModule(Module, ABC): - """Base class for teleoperation modules. - - ## LCM Topics (Output): - - controller_delta_{i}: Out[PoseStamped] - Controller i delta pose - - trigger_value_{i}: Out[Float32] - Controller i trigger/gripper value (0.0-1.0) - - ## RPC Methods: - - calibrate() -> dict: Calibrate by capturing initial poses - - reset_calibration() -> dict: Reset calibration - - is_calibrated() -> bool: Check if calibrated - - get_status() -> dict: Get teleoperation status - """ - - default_config = BaseTeleopConfig - config: BaseTeleopConfig - - # LCM Output topics - controller_delta_0: Out[PoseStamped] = None # type: ignore - trigger_value_0: Out[Float32] = None # type: ignore - - def __init__( - self, global_config: GlobalConfig | None = None, *args: Any, **kwargs: Any - ) -> None: - super().__init__(*args, **kwargs) - - # Get global config for Rerun connection - self._global_config = global_config or GlobalConfig() - - # Calibration state - self._is_calibrated = False - self._initial_poses: list[Pose | None] = [None] * self.config.num_inputs - - # Latest controller data (absolute poses) - self._all_poses: list[NDArray[np.float32] | None] = [None] * self.config.num_inputs - self._all_gripper_values: list[float] = [0.0] * self.config.num_inputs - self._tracking_msg_count = 0 - self._publish_count = 0 - - # Set default values for enable_inputs and input_labels if not provided - if not self.config.enable_inputs: - self.config.enable_inputs = [True] * self.config.num_inputs - if not self.config.input_labels: - self.config.input_labels = [f"controller_{i}" for i in range(self.config.num_inputs)] - - # check for mismatches between num_inputs and enable_inputs or input_labels - if len(self.config.enable_inputs) != self.config.num_inputs: - raise ValueError( - f"Number of enable_inputs ({len(self.config.enable_inputs)}) does not match num_inputs ({self.config.num_inputs})" - ) - if len(self.config.input_labels) != self.config.num_inputs: - raise ValueError( - f"Number of input_labels ({len(self.config.input_labels)}) does not match num_inputs ({self.config.num_inputs})" - ) - - logger.info(f"{self.__class__.__name__} base initialized") - - # ========================================================================= - # Module Lifecycle - # ========================================================================= - - @rpc - def start(self) -> None: - """Start the teleoperation module.""" - logger.info(f"Starting {self.__class__.__name__}...") - super().start() - - # Connect to Rerun for visualization if enabled - if ( - self.config.visualize_in_rerun - and RERUN_AVAILABLE - and self._global_config.viewer_backend.startswith("rerun") - ): - connect_rerun(global_config=self._global_config) - logger.info("Connected to Rerun for controller visualization") - - @rpc - def stop(self) -> None: - """Stop the teleoperation module.""" - logger.info(f"Stopping {self.__class__.__name__}...") - super().stop() - - # ========================================================================= - # Calibration - # ========================================================================= - - @rpc - def calibrate(self) -> dict[str, Any]: - """Calibrate by capturing initial controller poses. - - This is typically called when a calibration button is pressed. - Captures the current controller poses as the "zero" reference. - After calibration, delta poses are published. - - Returns: - Dict with 'success' and optional 'message' or 'error' - """ - logger.info("Calibrating controllers...") - - try: - # Check if we have controller data for enabled inputs - enabled_indices = [i for i, enabled in enumerate(self.config.enable_inputs) if enabled] - if not enabled_indices: - return { - "success": False, - "error": "No controllers are enabled. Enable at least one controller and try again.", - } - - # Capture controller initial poses - for i in enabled_indices: - pose_matrix = self._all_poses[i] - if pose_matrix is not None: - pose_obj = matrix_to_pose(pose_matrix) - self._initial_poses[i] = pose_obj - logger.info( - f"Captured controller initial: " - f"pos=[{pose_obj.x:.3f}, {pose_obj.y:.3f}, {pose_obj.z:.3f}], " - f"rpy=[{pose_obj.roll:.3f}, {pose_obj.pitch:.3f}, {pose_obj.yaw:.3f}]" - ) - else: - return { - "success": False, - "error": f"Controller {self.config.input_labels[i]} data is None. Move controller and try again.", - } - - self._is_calibrated = True - - logger.info("Calibration complete. Now streaming delta poses...") - return {"success": True, "message": "Calibrated - move controllers to control robot"} - - except Exception as e: - logger.error(f"Calibration failed: {e}", exc_info=True) - return {"success": False, "error": str(e)} - - @rpc - def reset_calibration(self) -> dict[str, Any]: - """Reset calibration. Stops streaming until recalibrated""" - self._is_calibrated = False - self._initial_poses = [None] * self.config.num_inputs - - logger.info("Calibration reset. Recalibrate to resume teleoperation...") - return {"success": True, "message": "Calibration reset - recalibrate to resume"} - - @rpc - def is_calibrated(self) -> bool: - """Check if calibrated""" - return self._is_calibrated - - @rpc - def get_status(self) -> dict[str, Any]: - """Get current teleoperation status""" - status: dict[str, Any] = { - TeleopStatusKey.IS_CALIBRATED.value: self._is_calibrated, - } - for i in range(self.config.num_inputs): - status[TeleopStatusKey.CONTROLLER_ENABLED.value.format(index=i)] = ( - self.config.enable_inputs[i] - ) - status[TeleopStatusKey.CONTROLLER_HAS_DATA.value.format(index=i)] = ( - self._all_poses[i] is not None - ) - status[TeleopStatusKey.CONTROLLER_GRIPPER_VALUE.value.format(index=i)] = ( - self._all_gripper_values[i] - ) - status[TeleopStatusKey.CONTROLLER_LABEL.value.format(index=i)] = ( - self.config.input_labels[i] - ) - - return status - - # ========================================================================= - # Controller Data Processing (called by subclasses) - # ========================================================================= - - def update_controller_poses( - self, - controller_poses: Sequence[NDArray[np.float32] | None], - controller_gripper_values: Sequence[float], - ) -> None: - """Update controller poses and publish deltas if calibrated""" - # Track how many tracking messages we've received - self._tracking_msg_count += 1 - self._all_poses = list(controller_poses) - self._all_gripper_values = list(controller_gripper_values) - - # Only stream deltas if calibrated - if not self._is_calibrated: - if self._tracking_msg_count <= 1 or self._tracking_msg_count % 100 == 0: - logger.warning("Not calibrated. Calibrate to start teleoperation.") - return - - # Compute and publish deltas - self._compute_and_publish_deltas() - - def _compute_and_publish_deltas(self) -> None: - """Compute and publish delta poses (current - initial).""" - # Track publish count for logging - self._publish_count += 1 - - try: - for i in range(self.config.num_inputs): - if not self.config.enable_inputs[i]: - continue - - current_time = time.time() - pose_matrix = self._all_poses[i] - if pose_matrix is None: - continue - initial_pose = self._initial_poses[i] - pose_obj = matrix_to_pose(pose_matrix) - input_label = self.config.input_labels[i] - - delta_pose = pose_obj - initial_pose - delta_pose_stamped = PoseStamped( - ts=current_time, - frame_id=f"{self.__class__.__name__.lower()}_{input_label}_controller_delta", - position=delta_pose.position, - orientation=delta_pose.orientation, - ) - - controller_output = getattr(self, f"controller_delta_{i}", None) - if controller_output and hasattr(controller_output, "publish"): - try: - controller_output.publish(delta_pose_stamped) - self._visualize_controller_in_rerun(pose_obj, input_label) - except Exception as e: - logger.error(f"Failed to publish {input_label} delta: {e}") - - trigger_output = getattr(self, f"trigger_value_{i}", None) - if trigger_output and hasattr(trigger_output, "publish"): - try: - trigger_output.publish(Float32(data=self._all_gripper_values[i])) - self._visualize_trigger_in_rerun(self._all_gripper_values[i], input_label) - except Exception as e: - logger.debug(f"Failed to publish {input_label} gripper: {e}") - - except Exception as e: - logger.error(f"Error computing/publishing delta poses: {e}") - - # ========================================================================= - # Rerun Visualization - # ========================================================================= - - def _visualize_controller_in_rerun(self, controller_pose: Pose, controller_label: str) -> None: - """Visualize controller absolute pose in Rerun""" - if not ( - self.config.visualize_in_rerun - and RERUN_AVAILABLE - and self._global_config.viewer_backend.startswith("rerun") - ): - return - - try: - controller_pose_stamped = PoseStamped( - ts=time.time(), - frame_id=f"world/teleop/{controller_label}_controller", - position=controller_pose.position, - orientation=controller_pose.orientation, - ) - rr.log( - f"world/teleop/{controller_label}_controller", - controller_pose_stamped.to_rerun(), # type: ignore[no-untyped-call] - ) - # Log 3D axes to visualize controller orientation (X=red, Y=green, Z=blue) - rr.log( - f"world/teleop/{controller_label}_controller/axes", - rr.Axes3D(length=0.15), # type: ignore[attr-defined] - ) - except Exception as e: - logger.debug(f"Failed to log {controller_label} controller to Rerun: {e}") - - def _visualize_trigger_in_rerun(self, trigger_value: float, controller_label: str) -> None: - """Visualize trigger value in Rerun as a scalar time series.""" - if not ( - self.config.visualize_in_rerun - and RERUN_AVAILABLE - and self._global_config.viewer_backend.startswith("rerun") - ): - return - - try: - rr.log( - f"world/teleop/{controller_label}_controller/trigger", - rr.Scalars(trigger_value), # type: ignore[attr-defined] - ) - except Exception as e: - logger.debug(f"Failed to log {controller_label} trigger to Rerun: {e}") diff --git a/dimos/teleop/devices/vr_headset/README.md b/dimos/teleop/devices/vr_headset/README.md deleted file mode 100644 index ca89581eb9..0000000000 --- a/dimos/teleop/devices/vr_headset/README.md +++ /dev/null @@ -1,189 +0,0 @@ -# VR Headset Teleoperation [TODO: Update] - -VR teleoperation for headsets (Quest 3 tested). - -## Quick Start - -### 1. Start Teleoperation - -```python -from dimos.teleop.devices.vr_headset import VRTeleopModule - -# Create and start -teleop = VRTeleopModule() -teleop.start() - -# Output: -# VR Teleoperation Module started on https://0.0.0.0:8443 -# Open this URL on Quest 3: https://:8443/ -``` - -### 2. Connect Quest 3 - -1. **Find your server IP**: - ```bash - hostname -I - # Example: 10.0.0.217 - ``` - -2. **Open Quest 3 browser** and go to: - ``` - https://10.0.0.217:8443/ - ``` - -3. **Accept security warning** (self-signed certificate is safe) - -4. **Click "Connect"** button and put on headset - -5. **Press X button** on controller to start teleoperation - - First press: Calibrates (captures initial poses) and starts control - - Press again: Stops control and resets calibration - - Press again: Calibrates again and resumes control - -## Architecture - -``` -Quest 3 Browser - โ†“ Opens https://your-ip:8443/ (WebXR) - โ†“ X button โ†’ start_teleop/stop_teleop via WebSocket - โ†“ -FastAPI Server - โ†“ Receives tracking data (controller poses) - โ†“ -VRTeleopModule (inherits BaseTeleopModule) - โ†“ Computes delta poses (current - initial) - โ†“ Publishes delta poses (PoseStamped) - โ†“ -TeleopArmController - โ†“ Auto-calibrates robot on first delta - โ†“ Applies: target = initial_robot + delta - โ†“ Publishes cartesian commands (Pose) - โ†“ -Robot Driver - โ†“ Executes commands -``` - - -## Key Features - -- Press X to start/stop teleoperation (start = calibrate, stop = reset calibration) -- Captures initial poses on start -- Robot follows controller movement relative to initial pose -- AR mode - -The module exposes a blueprint helper as `vr_teleop_module`. - -## Configuration - -```python -from dimos.teleop.devices.vr_headset import VRTeleopModule, VRTeleopConfig - -config = VRTeleopConfig( - # VR headset settings - signaling_host="0.0.0.0", - signaling_port=8443, # HTTPS port (required for WebXR) - use_https=True, # Required for Quest 3 WebXR - - # Inherited from BaseTeleopConfig - num_inputs=2, - enable_inputs=[True, True], - input_labels=["left_vr", "right_vr"], - visualize_in_rerun=True, # Visualize controllers in Rerun - log_input_data=False, # Log input pose/gripper data - log_input_data_interval=100, # Log every N publishes when enabled - position_scale=1.0, - max_velocity=0.5, - workspace_limits={ - "x": (-1.0, 1.0), - "y": (-0.8, 0.8), - "z": (0.1, 2.0), - }, -) - -module = VRTeleopModule(config=config) -module.start() -``` - - -## WebSocket Protocol - -### Handshake -```json -// Client โ†’ Server -{ - "role": "teleop", - "robot_ip": "" -} - -// Server โ†’ Client -{ - "type": "handshake_ack", - "status": "connected" -} -``` - -### X Button Commands -```json -// Client โ†’ Server (when X button pressed) -{ - "type": "start_teleop" // or "stop_teleop" -} - -// Server โ†’ Client (response) -{ - "type": "teleop_started", // or "teleop_stopped" - "message": "Control active - move controllers to control robot" -} -``` - -## Output Topics - -Inherited from `BaseTeleopModule`: - -- `controller_delta_0` (PoseStamped) - Controller 0 **delta** pose (current - initial) -- `controller_delta_1` (PoseStamped) - Controller 1 **delta** pose (current - initial) -- `trigger_value_0` (Float32) - Controller 0 trigger value (0.0-1.0) -- `trigger_value_1` (Float32) - Controller 1 trigger value (0.0-1.0) - -**Note**: These are **delta poses**, not absolute poses. TeleopArmController applies these deltas to the robot's initial pose. `input_labels` are used in frame IDs and logging. - - -## Development - -### Standalone Server Testing -```bash -cd dimos/teleop/devices/vr_headset/control -python fastapi_server.py - -# Server starts on https://0.0.0.0:8443 -# Test: https://localhost:8443/health -``` - -### Custom Integration - -```python -from dimos.core.blueprints import autoconnect -from dimos.teleop.devices.vr_headset import VRTeleopModule -from dimos.teleop.robot_controllers import teleop_arm_controller -from your_robot import your_robot_blueprint - -custom_stack = autoconnect( - VRTeleopModule( - signaling_port=8443, - enable_inputs=[True, False], - input_labels=["left_vr", "right_vr"], - ), - teleop_arm_controller( - driver_module_name="YourDriver", - ), - your_robot_blueprint, -) - -coordinator = custom_stack.build() -coordinator.loop() -``` - -## Related - -- [`BaseTeleopModule`](../base/) - Base class for all teleoperation devices -- [`TeleopArmController`](../../robot_controllers/teleop_arm_controller.py) - Applies deltas to robot diff --git a/dimos/teleop/devices/vr_headset/__init__.py b/dimos/teleop/devices/vr_headset/__init__.py deleted file mode 100644 index b5c1d28107..0000000000 --- a/dimos/teleop/devices/vr_headset/__init__.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""VR headset teleoperation exports.""" - -from dimos.teleop.devices.vr_headset.vr_teleop_module import ( - VRTeleopModule, - vr_teleop_module, -) - -__all__ = [ - "VRTeleopModule", - "vr_teleop_module", -] diff --git a/dimos/teleop/devices/vr_headset/certs/cert.pem b/dimos/teleop/devices/vr_headset/certs/cert.pem deleted file mode 100644 index f2bd46c098..0000000000 --- a/dimos/teleop/devices/vr_headset/certs/cert.pem +++ /dev/null @@ -1,19 +0,0 @@ ------BEGIN CERTIFICATE----- -MIIDCDCCAfCgAwIBAgITPnPCPHTIoeUModYTakOkeJct0TANBgkqhkiG9w0BAQsF -ADAUMRIwEAYDVQQDDAlsb2NhbGhvc3QwHhcNMjYwMTEwMjA0MzA5WhcNMjcwMTEw -MjA0MzA5WjAUMRIwEAYDVQQDDAlsb2NhbGhvc3QwggEiMA0GCSqGSIb3DQEBAQUA -A4IBDwAwggEKAoIBAQCqEtoiOzdJCJRAZhYegbk6oZ1Pl2SyEoTzapxyzVoqkqxQ -jorskwfWPpf4n9/LDtGn9z2dYg3kLwgC9Ce1gk9vEst1TCzfalCb1t/Hc0ImFT0h -ieJctMBoLlmXk7C+B3/k44xz8z8C5SB/sevK614kx+DXwDE1p+85SmsgELhayY2l -ZUgYsCzL0tGIqQWG/KYpe9VHffNtv+BFNirBsIGuUZjWAsNUQgzu6DoSpf69xhHM -oLDPMYcdKv//XCvCWDM5qLVVtDFHugIIhYB9BQRXswSBkDjwxLJQEr++yRHEQbxS -ivgeD8+lRseQLo288S7DG+5Y518YhdNvye007aS/AgMBAAGjUzBRMB0GA1UdDgQW -BBR8LLYvVdoeoagvZO4Y3YEHhIESfzAfBgNVHSMEGDAWgBR8LLYvVdoeoagvZO4Y -3YEHhIESfzAPBgNVHRMBAf8EBTADAQH/MA0GCSqGSIb3DQEBCwUAA4IBAQAssAOy -2z2RXgfINrz1ESg9GYs6GJtM5lC0ryO6egvtqgyXNjW0OZNOGo0RWyJnj2vbPg1d -x1AIyZAGvXtoEdO7x7Oeo7byGU1SFJAqBabW7zB3PsplpQcMEySmYOzGTlji2U5S -L8rhhRZw7adBRZfcEe5D44TmF7E1ulN2XHsKVor4ZTvF/q6ipVZUak50uVaw9zLK -VgfARoxayLLCZdKrsmO4QdWd/g3t6qvs31U3o2XQHxE8DKaOEhtv9mFzMj/bL+IC -dwIsxeO1yBkqXWqK4b6MoWn7bypHins13gtJVGP3kmQXWOHXOj+x+435X9TGRTcb -Ks4e7yxvfb17UYCd ------END CERTIFICATE----- diff --git a/dimos/teleop/devices/vr_headset/certs/key.pem b/dimos/teleop/devices/vr_headset/certs/key.pem deleted file mode 100644 index 53b072300f..0000000000 --- a/dimos/teleop/devices/vr_headset/certs/key.pem +++ /dev/null @@ -1,28 +0,0 @@ ------BEGIN PRIVATE KEY----- -MIIEvAIBADANBgkqhkiG9w0BAQEFAASCBKYwggSiAgEAAoIBAQCqEtoiOzdJCJRA -ZhYegbk6oZ1Pl2SyEoTzapxyzVoqkqxQjorskwfWPpf4n9/LDtGn9z2dYg3kLwgC -9Ce1gk9vEst1TCzfalCb1t/Hc0ImFT0hieJctMBoLlmXk7C+B3/k44xz8z8C5SB/ -sevK614kx+DXwDE1p+85SmsgELhayY2lZUgYsCzL0tGIqQWG/KYpe9VHffNtv+BF -NirBsIGuUZjWAsNUQgzu6DoSpf69xhHMoLDPMYcdKv//XCvCWDM5qLVVtDFHugII -hYB9BQRXswSBkDjwxLJQEr++yRHEQbxSivgeD8+lRseQLo288S7DG+5Y518YhdNv -ye007aS/AgMBAAECggEAEYm4eSTeb8dqM5/eB7mnMQ7JwD445zdgrWdLiHfOT1Xp -CLTs9OC5Q1RuFc3gD+T7JWp/WJOWzMvgHsnbBG0n0CjsgS/gmriwZPmfVvEhm6K/ -YeOBoRS57fmwScte7kxWBHObSrkFQOusJ8Q1sABd03DR1Kh1iqEYMAdmt5ucdIQU -QtfXx0itrNDLW7nBsr1vHRavxbSN1KxbJ6bE6Bs2cZCr+1v7OpCkjdhjONDAXf6C -y1rSep/J10PUbl1WaiQZq1EmJrnLS6Gx8NIx+pNEYLMuw0aD/10j7Q2xLl8MHDp8 -l4Zhp0t5u9AOmigEvcY1d/0KgqzRaHjpHWw+B42XZQKBgQDLzYNxP+rBi/c7h074 -vRIuZmgzLRFqrAaQ+RWxzccasO1uzEkKPLT6lARTGrBRpvu6V05Vi1BWgfdSC1fN -v3sif6KaDyq5DhvWZpS7qeOEl16DLOO2AU3BbbwhCsOMnOQh1wNu6q/oKzO04VpY -8eJDVERK3MipU4DUVr2KEg/zrQKBgQDVodx5f8Rnb7htOGS8Kto0MHtiwLsAX5t6 -uGW4rZoMROK1C0yEhnmkGWKYkWtUwySkpn+PYKiMPhRvAg92OuGePlbwA9AygNUx -4dbNpZiSEUvoZ3IO9rhBE0TGnSIiB7G3eRZRdbKiHSGIpWYF8YCM/YyFvaPcmspC -lMuiDnTnmwKBgC7Tj4nr17KUyD+DPV/lgVHr8bEgf8n0sKWKtbNexoqZcFRu17Fk -dWjFraCylySLq3cLLFJ3agQWZI8TUB9UCaTJksC3D2fpl/fRJgdgZ4hFh0+4drGQ -5x60ae9lm7ypJ7mmv4Eypyw/EOhUhv+8w/IYYICa7fgJ2aXwMCiTMdsZAoGAC9ey -MiYMDDPcRGm449l4SSZa4KmQdD/YjaAFO4ycGowDCUg8EKinu5oQpiaBjaxXrqzw -K1GPZl7WoSS7GLHA6hXImfuMIhCUQPSlBLdmUsqUq6h4YS36HtljmaMCTmKgzmvu -+csNgQEeZ8XLdw7hMm+nx44wtDz8c15uP2iPwHsCgYBK7gXnGNxiZuS/0j13kYnP -A49Hk7UJs2Dienh2V7ORGLwrvWuPT7iyKbEwTEwkx17CxlIAcCCuQy098cohi99K -eVUE2KK8Zf4ffscmuj8ZAuSo9INuLqSr7rQ/7qh55mOmKXQxxu8h46CDlDZb03Wr -hLgQTQLFOMinyyWHvlkn2Q== ------END PRIVATE KEY----- diff --git a/dimos/teleop/devices/vr_headset/control/fastapi_server_x.py b/dimos/teleop/devices/vr_headset/control/fastapi_server_x.py deleted file mode 100644 index 0354dd055c..0000000000 --- a/dimos/teleop/devices/vr_headset/control/fastapi_server_x.py +++ /dev/null @@ -1,449 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -from __future__ import annotations - -import asyncio -from pathlib import Path -import subprocess -import threading -import time -from typing import TYPE_CHECKING, Any - -from fastapi import FastAPI, WebSocket, WebSocketDisconnect -from fastapi.middleware.cors import CORSMiddleware -from fastapi.responses import FileResponse -import uvicorn - -from dimos.teleop.devices.vr_headset.control.tracking_processor import TrackingProcessor -from dimos.utils.logging_config import setup_logger - -if TYPE_CHECKING: - from collections.abc import Awaitable, Callable - - import numpy as np - from numpy.typing import NDArray - -logger = setup_logger() - -# Rate limiting for command messages (in seconds) -COMMAND_DEBOUNCE_SECONDS = 2 - - -class ConnectionManager: - """Manages active WebSocket connections for teleoperation.""" - - def __init__(self) -> None: - self.active_connections: list[WebSocket] = [] - self.active_connections_lock = threading.Lock() - self.data_callback: Callable[[list[NDArray[np.float32]], list[float]], None] | None = None - self.command_callback: Callable[[str, WebSocket], Awaitable[dict[str, Any]]] | None = None - - async def connect(self, websocket: WebSocket) -> None: - """Accept and register a new WebSocket connection. - - Args: - websocket: The WebSocket connection to register - """ - await websocket.accept() - with self.active_connections_lock: - self.active_connections.append(websocket) - logger.info(f"Client connected (total: {len(self.active_connections)})") - - def disconnect(self, websocket: WebSocket) -> None: - """Remove a WebSocket connection from active connections. - - Args: - websocket: The WebSocket connection to remove - """ - with self.active_connections_lock: - if websocket in self.active_connections: - self.active_connections.remove(websocket) - logger.info(f"Client disconnected (remaining: {len(self.active_connections)})") - - def set_callback( - self, - callback: Callable[[list[NDArray[np.float32]], list[float]], None], - ) -> None: - """Set callback function to send controller data to teleop module. - - Args: - callback: Function that takes (controller_poses, controller_gripper_values) - """ - self.data_callback = callback - logger.info("Controller data callback registered") - - def set_command_callback( - self, callback: Callable[[str, WebSocket], Awaitable[dict[str, Any]]] - ) -> None: - """Set callback function to handle teleop commands (start_teleop/stop_teleop). - - Args: - callback: Async function that takes (command_type: str, websocket: WebSocket) -> dict - """ - self.command_callback = callback - logger.info("Command callback registered") - - async def broadcast(self, message: dict[str, Any]) -> None: - """Broadcast a message to all connected clients. - - Args: - message: Dictionary to send as JSON to all clients - """ - disconnected = [] - with self.active_connections_lock: - for connection in self.active_connections: - try: - await connection.send_json(message) - except Exception as e: - logger.error(f"Failed to broadcast to client: {e}") - disconnected.append(connection) - - # Clean up disconnected clients - for conn in disconnected: - self.disconnect(conn) - - def get_connection_count(self) -> int: - """Get the number of active connections. - - Returns: - Number of active WebSocket connections - """ - with self.active_connections_lock: - return len(self.active_connections) - - -class TeleopFastAPIServer: - """FastAPI-based WebSocket server for Quest3 teleoperation.""" - - def __init__( - self, - host: str = "0.0.0.0", - port: int = 8443, # Standard HTTPS port (alternative to 443). Any port works with HTTPS. - use_https: bool = True, - ): - """Initialize the FastAPI server. - - Args: - host: Host address to bind to (default: 0.0.0.0) - port: Port to bind to (default: 8443 for HTTPS) - use_https: Whether to use HTTPS (required for WebXR on Quest 3) - """ - self.host = host - self.port = port - self.use_https = use_https - self.app = FastAPI(title="Quest3 Teleoperation Server") - self.manager = ConnectionManager() - self.server: uvicorn.Server | None = None - - # SSL certificate paths - self.cert_dir = Path(__file__).parent.parent / "certs" # any better way of writing this? - self.cert_file = self.cert_dir / "cert.pem" - self.key_file = self.cert_dir / "key.pem" - - # Static files directory - self.static_dir = Path(__file__).parent.parent / "static" - - # Enable CORS for Quest 3 browser - self.app.add_middleware( - CORSMiddleware, - allow_origins=["*"], # Allow all origins for VR headset - allow_credentials=True, - allow_methods=["*"], - allow_headers=["*"], - ) - - # Register routes - self._setup_routes() - - # Setup SSL certificates if needed - if self.use_https: - self._ensure_ssl_certificates() - - def _ensure_ssl_certificates(self) -> None: - """Generate self-signed SSL certificates if they don't exist.""" - self.cert_dir.mkdir(parents=True, exist_ok=True) - - if self.cert_file.exists() and self.key_file.exists(): - logger.info("SSL certificates found") - return - - logger.info("Generating self-signed SSL certificates...") - try: - subprocess.run( - [ - "openssl", - "req", - "-x509", - "-newkey", - "rsa:2048", - "-nodes", - "-sha256", - "-subj", - "/CN=localhost", - "-keyout", - str(self.key_file), - "-out", - str(self.cert_file), - "-days", - "365", - ], - check=True, - capture_output=True, - ) - logger.info("โœ“ SSL certificates generated successfully") - logger.warning( - "โš  These are self-signed certificates. Quest 3 will show a security warning." - ) - except subprocess.CalledProcessError as e: - logger.error(f"Failed to generate SSL certificates: {e}") - logger.error("Make sure openssl is installed: sudo apt-get install openssl") - raise - except FileNotFoundError: - logger.error("openssl command not found. Please install: sudo apt-get install openssl") - raise - - def _setup_routes(self) -> None: - """Setup FastAPI routes and WebSocket endpoints.""" - - @self.app.get("/", response_model=None) - async def root() -> FileResponse | dict[str, Any]: - """Serve the VR client HTML page.""" - html_file = self.static_dir / "index.html" - if html_file.exists(): - return FileResponse(html_file) - else: - return { - "service": "Quest3 Teleoperation Server", - "status": "running", - "active_connections": self.manager.get_connection_count(), - "error": "VR client HTML not found. Check static/index.html", - } - - @self.app.get("/health") - async def health() -> dict[str, Any]: - """Health check endpoint.""" - return { - "status": "healthy", - "connections": self.manager.get_connection_count(), - } - - @self.app.websocket("/ws") - async def websocket_endpoint(websocket: WebSocket) -> None: - """WebSocket endpoint for VR teleoperation tracking data. - - Args: - websocket: The WebSocket connection from Quest 3 client - """ - await self.manager.connect(websocket) - processor = TrackingProcessor() - message_count = 0 - - try: - # Wait for initial handshake message - initial_data = await websocket.receive_json() - role = initial_data.get("role") - logger.info(f"Client handshake: {initial_data}") - - if role != "teleop": - await websocket.send_json( - {"type": "error", "error": f"Invalid role: {role}. Expected 'teleop'."} - ) - await websocket.close() - return - - # Send acknowledgment - await websocket.send_json({"type": "handshake_ack", "status": "connected"}) - - # Track command timestamps for rate limiting - last_command_time: dict[int, float] = {} - - # Main tracking loop - while True: - # Receive message - message = await websocket.receive_json() - message_count += 1 - - # Check if this is a command message (start_teleop/stop_teleop) - message_type = message.get("type") - if message_type in ("start_teleop", "stop_teleop"): - # Rate limit commands to prevent double-triggers - now = time.time() - if now - last_command_time.get(id(websocket), 0) < COMMAND_DEBOUNCE_SECONDS: - logger.debug(f"Rate limiting command: {message_type}") - continue - last_command_time[id(websocket)] = now - - logger.info(f"Received command: {message_type}") - # Route to command handler - if self.manager.command_callback is not None: - try: - response = await self.manager.command_callback( - message_type, websocket - ) - # Send response back to client - await websocket.send_json(response) - except Exception as e: - logger.error( - f"Error handling command {message_type}: {e}", exc_info=True - ) - await websocket.send_json( - {"type": "error", "error": f"Command failed: {e!s}"} - ) - else: - logger.warning(f"Command callback not set, ignoring {message_type}") - await websocket.send_json( - {"type": "error", "error": "Command handler not available"} - ) - continue - - # Otherwise, process as tracking data - result = processor.process_tracking_message(message) - - if result is not None: - controller_poses, controller_gripper_values = result - - # Send to callback (teleop module) - if self.manager.data_callback is not None: - self.manager.data_callback(controller_poses, controller_gripper_values) - - # Log periodically - if message_count % 100 == 0: - logger.debug(f"Processed {message_count} tracking messages") - - except WebSocketDisconnect: - logger.info("Client disconnected normally") - except Exception as e: - logger.error(f"Error in WebSocket handler: {e}", exc_info=True) - finally: - self.manager.disconnect(websocket) - logger.info(f"Connection closed after {message_count} messages") - - def set_callback( - self, - callback: Callable[[list[NDArray[np.float32]], list[float]], None], - ) -> None: - """Set callback function for controller data. - - Args: - callback: Function that takes (controller_poses, controller_gripper_values) - """ - self.manager.set_callback(callback) - - def set_command_callback( - self, callback: Callable[[str, WebSocket], Awaitable[dict[str, Any]]] - ) -> None: - """Set callback function for teleop commands. - - Args: - callback: Async function that takes (command_type: str, websocket: WebSocket) -> dict - """ - self.manager.set_command_callback(callback) - - async def start_async(self) -> None: - """Start the FastAPI server asynchronously.""" - config_kwargs: dict[str, Any] = { - "app": self.app, - "host": self.host, - "port": self.port, - "log_level": "info", - "access_log": False, # Disable access logs for performance - } - - # Add SSL if enabled - if self.use_https: - config_kwargs["ssl_keyfile"] = str(self.key_file) - config_kwargs["ssl_certfile"] = str(self.cert_file) - protocol = "https" - ws_protocol = "wss" - else: - protocol = "http" - ws_protocol = "ws" - - config = uvicorn.Config(**config_kwargs) # type: ignore[misc] - self.server = uvicorn.Server(config) - - logger.info(f"FastAPI server starting on {protocol}://{self.host}:{self.port}") - logger.info(f"VR Client: {protocol}://{self.host}:{self.port}/") - logger.info(f"WebSocket: {ws_protocol}://{self.host}:{self.port}/ws") - - await self.server.serve() - - def run(self) -> None: - """Run the FastAPI server (blocking). - - This is for standalone testing only. - """ - run_kwargs: dict[str, Any] = { - "app": self.app, - "host": self.host, - "port": self.port, - "log_level": "info", - } - - if self.use_https: - run_kwargs["ssl_keyfile"] = str(self.key_file) - run_kwargs["ssl_certfile"] = str(self.cert_file) - - uvicorn.run(**run_kwargs) # type: ignore[misc] - - async def stop_async(self) -> None: - """Stop the FastAPI server asynchronously.""" - # Close all connections - with self.manager.active_connections_lock: - connections_to_close = list(self.manager.active_connections) - for websocket in connections_to_close: - self.manager.disconnect(websocket) - try: - await websocket.close() - except Exception as e: - logger.debug(f"Error closing websocket: {e}") - - if self.server: - logger.info("Stopping FastAPI server...") - self.server.should_exit = True - await asyncio.sleep(0.1) # Give time for graceful shutdown - - async def broadcast_robot_state(self, state: dict[str, Any]) -> None: - """Broadcast robot state to all connected clients. - - Args: - state: Dictionary containing robot state information - """ - await self.manager.broadcast(state) - - -# Standalone testing -if __name__ == "__main__": - import logging - - logging.basicConfig( - level=logging.INFO, format="%(asctime)s - %(name)s - %(levelname)s - %(message)s" - ) - - def test_callback(controller_poses: Any, controller_gripper_values: Any) -> None: - """Test callback for tracking data.""" - if len(controller_poses) >= 2 and len(controller_gripper_values) >= 2: - print( - f"Left pose: {controller_poses[0][:3, 3]}, gripper: {controller_gripper_values[0]}" - ) - print( - f"Right pose: {controller_poses[1][:3, 3]}, gripper: {controller_gripper_values[1]}" - ) - - server = TeleopFastAPIServer() - server.set_callback(test_callback) - server.run() diff --git a/dimos/teleop/devices/vr_headset/control/tracking_processor.py b/dimos/teleop/devices/vr_headset/control/tracking_processor.py deleted file mode 100644 index 548b975e1e..0000000000 --- a/dimos/teleop/devices/vr_headset/control/tracking_processor.py +++ /dev/null @@ -1,153 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -""" -VR controller tracking processor for Quest3 teleoperation. - -Processes VR controller tracking data, applies coordinate transformations, -and returns robot-space poses and gripper values per controller. -""" - -from __future__ import annotations - -from typing import TYPE_CHECKING, Any - -import numpy as np -from scipy.spatial.transform import Rotation # type: ignore[import-untyped] - -from dimos.utils.logging_config import setup_logger - -if TYPE_CHECKING: - from numpy.typing import NDArray - -logger = setup_logger() - -# Coordinate frame transformation from VR to robot -VR_TO_ROBOT_FRAME = np.array( - [ - [0, 0, -1, 0], # Robot X-axis = -VR Z-axis (flip forward/back) - [-1, 0, 0, 0], # Robot Y-axis = -VR X-axis (flip left/right) - [0, 1, 0, 0], # Robot Z-axis = +VR Y-axis (keep up direction) - [0, 0, 0, 1], # Homogeneous coordinate - ], - dtype=np.float32, -) - - -class TrackingProcessor: - """Processes VR controller tracking data to robot-space poses. - - Handles: - - VR to robot coordinate frame transformation - - Controller orientation alignment - - Safety constraints (Z position clamping) (optional) - """ - - def __init__(self) -> None: - """Initialize tracking processor.""" - # Current poses (4x4 transformation matrices) - self.left_wrist_pose = np.eye(4, dtype=np.float32) - self.right_wrist_pose = np.eye(4, dtype=np.float32) - - # Current gripper values (0.0 = open, 1.0 = closed) - self.left_gripper_value = 0.0 - self.right_gripper_value = 0.0 - - def process_tracking_message( - self, event: dict[str, Any] - ) -> tuple[list[NDArray[np.float32]], list[float]] | None: - """Process VR tracking message and return robot-space poses. - - Args: - event: Dictionary with 'type', 'left', and 'right' controller data - - Returns: - Tuple of (controller_poses, controller_gripper_values) or None if invalid - """ - tracking_type = event.get("type") - if tracking_type != "controller": - logger.debug("Ignoring non-controller tracking type: %s", tracking_type) - return None - - # Process both controllers - for side in ["left", "right"]: - tracking_data = event.get(side) - if tracking_data is None: - continue - if not isinstance(tracking_data, dict): - logger.debug("Invalid %s tracking payload: %s", side, type(tracking_data)) - continue - - # Process target location (pose) - if "targetLocation" in tracking_data: - self._process_target_location(tracking_data["targetLocation"], side) - - # Process gripper (trigger) - if side == "left": - self.left_gripper_value = self._extract_gripper_value(tracking_data) - else: - self.right_gripper_value = self._extract_gripper_value(tracking_data) - - return ( - [self.left_wrist_pose, self.right_wrist_pose], - [self.left_gripper_value, self.right_gripper_value], - ) - - def _process_target_location(self, target_location: list[float], side: str) -> None: - """Process controller pose from VR space to robot space. - - Args: - target_location: Flat 16-element transformation matrix (column-major) - side: 'left' or 'right' - """ - # Convert to 4x4 matrix - target_matrix_flat = np.array(target_location, dtype=np.float32) - if target_matrix_flat.size != 16: - logger.error("Invalid targetLocation size: %d, expected 16", target_matrix_flat.size) - return - - # Reshape and transpose (column-major from JS to row-major) - target_matrix = target_matrix_flat.reshape(4, 4).T - - # Rotate controller 90ยฐ around Z-axis for gripper alignment - direction = -1 if side == "right" else 1 - rotation = Rotation.from_euler("z", 90 * direction, degrees=True) - target_matrix[:3, :3] = target_matrix[:3, :3] @ rotation.as_matrix() - - # Apply VR to robot frame transformation - wrist_mat = VR_TO_ROBOT_FRAME @ target_matrix - - # Store the pose - if side == "left": - self.left_wrist_pose = wrist_mat - else: - self.right_wrist_pose = wrist_mat - - def _extract_gripper_value(self, tracking_data: dict[str, Any]) -> float: - """Extract gripper value from tracking data. - - Args: - tracking_data: Controller data containing 'trigger' - - Returns: - Gripper value (0.0-1.0) from VR controller trigger - """ - gripper_value = tracking_data.get("trigger", 0.0) - try: - # VR trigger value is already 0.0-1.0, just convert to float - return float(gripper_value) - except (ValueError, TypeError): - logger.warning("Invalid trigger value: %s", gripper_value) - return 0.0 diff --git a/dimos/teleop/devices/vr_headset/static/index.html b/dimos/teleop/devices/vr_headset/static/index.html deleted file mode 100644 index b6d325a493..0000000000 --- a/dimos/teleop/devices/vr_headset/static/index.html +++ /dev/null @@ -1,412 +0,0 @@ - - - - - - Quest 3 VR Teleop - - - -
-

Quest 3 VR Teleop

-
Ready to connect
- - -
- - - - - diff --git a/dimos/teleop/devices/vr_headset/static/teleop_server.ts b/dimos/teleop/devices/vr_headset/static/teleop_server.ts deleted file mode 100644 index 608ad850d9..0000000000 --- a/dimos/teleop/devices/vr_headset/static/teleop_server.ts +++ /dev/null @@ -1,70 +0,0 @@ -#!/usr/bin/env -S deno run --allow-net --allow-read --unstable-net - -// LCM to WebSocket Bridge for Robot Control -// Forwards robot pose to browser, receives twist commands from browser - -import { LCM } from "jsr:@dimos/lcm"; -import { decodePacket, geometry_msgs } from "jsr:@dimos/msgs"; - -const PORT = 8443; -const clients = new Set(); -const cert = await Deno.readTextFile("../certs/cert.pem"); -const key = await Deno.readTextFile("../certs/key.pem"); - -Deno.serve({ port: PORT, cert, key }, async (req) => { - const url = new URL(req.url); - - if (req.headers.get("upgrade") === "websocket") { - const { socket, response } = Deno.upgradeWebSocket(req); - socket.onopen = () => { console.log("Client connected"); clients.add(socket); }; - socket.onclose = () => { console.log("Client disconnected"); clients.delete(socket); }; - socket.onerror = () => clients.delete(socket); - - // Forward binary LCM packets from browser directly to UDP - socket.binaryType = "arraybuffer"; - let msgCount = 0; - socket.onmessage = async (event) => { - msgCount++; - if (event.data instanceof ArrayBuffer) { - const packet = new Uint8Array(event.data); - try { - await lcm.publishPacket(packet); - } catch (e) { - console.error("Forward error:", e); - } - } - }; - - return response; - } - - if (url.pathname === "/" || url.pathname === "/index.html") { - const html = await Deno.readTextFile(new URL("./index.html", import.meta.url)); - return new Response(html, { headers: { "content-type": "text/html" } }); - } - - return new Response("Not found", { status: 404 }); -}); - -console.log(`Server: https://localhost:${PORT}`); - -const lcm = new LCM(); -await lcm.start(); - -// Subscribe to pose and just log to show how server can decode messages for itself -// lcm.subscribe("/odom", geometry_msgs.PoseStamped, (msg) => { -// const pos = msg.data.pose.position; -// const ori = msg.data.pose.orientation; -// console.log(`[pose] x=${pos.x.toFixed(2)} y=${pos.y.toFixed(2)} z=${pos.z.toFixed(2)}`); -// }); - -// Forward all raw packets to browser (we are decoding LCM directly in the browser) -lcm.subscribePacket((packet) => { - for (const client of clients) { - if (client.readyState === WebSocket.OPEN) { - client.send(packet); - } - } -}); - -await lcm.run(); diff --git a/dimos/teleop/devices/vr_headset/vr_teleop_module.py b/dimos/teleop/devices/vr_headset/vr_teleop_module.py deleted file mode 100644 index 7234cdd4b1..0000000000 --- a/dimos/teleop/devices/vr_headset/vr_teleop_module.py +++ /dev/null @@ -1,266 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -""" -VR Teleoperation Module - -A dimos Module that runs the WebSocket signaling server for VR teleoperation, -receives controller tracking data, and streams DELTA poses to TeleopRobotController. - -This module inherits from BaseTeleopModule which handles: -- Calibration (capture initial controller poses) -- Delta computation (current - initial) -- Publishing delta poses to LCM -- Rerun visualization -- Standard RPC interface - -This module implements VR-specific: -- FastAPI/WebSocket server for VR headset -- X button handler for calibration trigger -""" - -from __future__ import annotations - -from dataclasses import dataclass, field -import threading -from typing import TYPE_CHECKING, Any - -import numpy as np -from scipy.spatial.transform import Rotation as R - -from dimos.core import In, Out, rpc -from dimos.teleop.devices.base import BaseTeleopConfig, BaseTeleopModule -from dimos.utils.logging_config import setup_logger - -if TYPE_CHECKING: - from dimos_lcm.geometry_msgs import Transform as LCMTransform - from numpy.typing import NDArray - - from dimos.core.global_config import GlobalConfig - from dimos.msgs.geometry_msgs import PoseStamped - from dimos.msgs.std_msgs import Bool, Float32 - -logger = setup_logger() - -# Coordinate frame transformation from VR (WebXR) to robot frame -# WebXR: X=right, Y=up, Z=back (towards user) -# Robot: X=forward, Y=left, Z=up -VR_TO_ROBOT_FRAME = np.array( - [ - [0, 0, -1, 0], # Robot X = -VR Z (forward) - [-1, 0, 0, 0], # Robot Y = -VR X (left) - [0, 1, 0, 0], # Robot Z = +VR Y (up) - [0, 0, 0, 1], - ], - dtype=np.float64, -) - - -@dataclass -class VRTeleopConfig(BaseTeleopConfig): - """Configuration for VR Teleoperation Module.""" - - # Control settings - num_inputs: int = 2 # Number of inputs (controllers) - enable_inputs: list[bool] = field(default_factory=lambda: [True, True]) - input_labels: list[str] = field(default_factory=lambda: ["left_vr", "right_vr"]) - - # Visualization settings - visualize_in_rerun: bool = True # Visualize controller poses in Rerun - log_input_data: bool = False # Log input pose/gripper data periodically - log_input_data_interval: int = 100 # Log every N publishes when enabled - - -class VRTeleopModule(BaseTeleopModule): - """VR Teleoperation Module. - - ## LCM Topics (inherited from base): - - controller_delta_0: Out[PoseStamped] - Controller 0 delta pose - - trigger_value_0: Out[Float32] - Controller 0 trigger/gripper value (0.0-1.0) - - ## Additional LCM Topics: - - controller_delta_1: Out[PoseStamped] - Controller 1 delta pose - - trigger_value_1: Out[Float32] - Controller 1 trigger/gripper value (0.0-1.0) - """ - - default_config = VRTeleopConfig - config: VRTeleopConfig - - # LCM output topics for VR (controller_0 is inherited from base) - controller_delta_1: Out[PoseStamped] = None # type: ignore[assignment] - trigger_value_1: Out[Float32] = None # type: ignore[assignment] - # LCM input topics for raw Transform from web clients (uses dimos_lcm type for correct msg_name) - vr_left_transform: In[LCMTransform] = None # type: ignore[assignment] - vr_right_transform: In[LCMTransform] = None # type: ignore[assignment] - vr_trigger_0: In[Float32] = None # type: ignore[assignment] - vr_trigger_1: In[Float32] = None # type: ignore[assignment] - teleop_enable: In[Bool] = None # type: ignore[assignment] - - def __init__( - self, global_config: GlobalConfig | None = None, *args: Any, **kwargs: Any - ) -> None: - # Remove global_config from kwargs to avoid passing it twice - kwargs.pop("global_config", None) - # Pass global_config as positional argument to match base class signature - super().__init__(global_config, *args, **kwargs) - - self._lcm_lock = threading.Lock() - self._lcm_controller_poses: list[NDArray[np.float32] | None] = [ - None - ] * self.config.num_inputs - self._lcm_gripper_values: list[float] = [0.0] * self.config.num_inputs - - logger.info("VRTeleopModule initialized") - - # ========================================================================= - # Module Lifecycle (VR-specific) - # ========================================================================= - - @rpc - def start(self) -> None: - """Start the VR teleoperation module and signaling server.""" - super().start() - - if ( - self.vr_left_transform - and self.vr_left_transform.transport - and hasattr(self.vr_left_transform, "subscribe") - ): - self.vr_left_transform.subscribe(lambda msg: self._on_lcm_transform(0, msg)) - else: - logger.debug("vr_left_transform not wired; skipping LCM transform subscription") - - if ( - self.vr_right_transform - and self.vr_right_transform.transport - and hasattr(self.vr_right_transform, "subscribe") - ): - self.vr_right_transform.subscribe(lambda msg: self._on_lcm_transform(1, msg)) - else: - logger.debug("vr_right_transform not wired; skipping LCM transform subscription") - - if ( - self.vr_trigger_0 - and self.vr_trigger_0.transport - and hasattr(self.vr_trigger_0, "subscribe") - ): - self.vr_trigger_0.subscribe(lambda msg: self._on_lcm_trigger(0, msg)) - else: - logger.debug("vr_trigger_0 not wired; skipping LCM trigger subscription") - - if ( - self.vr_trigger_1 - and self.vr_trigger_1.transport - and hasattr(self.vr_trigger_1, "subscribe") - ): - self.vr_trigger_1.subscribe(lambda msg: self._on_lcm_trigger(1, msg)) - else: - logger.debug("vr_trigger_1 not wired; skipping LCM trigger subscription") - - if ( - self.teleop_enable - and self.teleop_enable.transport - and hasattr(self.teleop_enable, "subscribe") - ): - self.teleop_enable.subscribe(self._on_lcm_teleop_enable) - logger.info("Subscribed to teleop_enable LCM topic") - else: - logger.warning("teleop_enable not wired; skipping LCM teleop subscription") - - logger.info("VR Teleoperation Module started (LCM-only web client mode)") - - @rpc - def stop(self) -> None: - """Stop the VR teleoperation module and signaling server.""" - logger.info("Stopping VR Teleoperation Module...") - - super().stop() - - @rpc - def start_teleop(self) -> dict[str, Any]: - """RPC helper to trigger calibration for LCM-only clients.""" - logger.info("RPC start_teleop called - calibrating VR...") - return self.calibrate() - - @rpc - def stop_teleop(self) -> dict[str, Any]: - """RPC helper to stop teleop for LCM-only clients.""" - logger.info("RPC stop_teleop called - resetting calibration...") - return self.reset_calibration() - - def _on_lcm_transform(self, index: int, transform: LCMTransform) -> None: - """Handle raw Transform input from LCM and forward to base processing. - - The transform comes directly from WebXR. We: - 1. Convert to 4x4 matrix - 2. Apply controller alignment rotation (in VR space) - 3. Apply VRโ†’robot coordinate frame transformation - - WebXR coordinate system: X=right, Y=up, Z=back (towards user) - Robot coordinate system: X=forward, Y=left, Z=up - """ - # Convert LCM Transform to 4x4 matrix - t = transform.translation - q = transform.rotation - rot_matrix = R.from_quat([q.x, q.y, q.z, q.w]).as_matrix() - - # Build VR-space matrix - vr_matrix = np.eye(4, dtype=np.float64) - vr_matrix[:3, :3] = rot_matrix - vr_matrix[:3, 3] = [t.x, t.y, t.z] - - # Apply controller alignment rotation in VR space (from tracking_processor.py) - # Right controller (index 1) rotates -90ยฐ around Z, left (index 0) rotates +90ยฐ - # This aligns the controller orientation with gripper orientation - direction = 1 if index == 0 else -1 - z_rotation = R.from_euler("z", 90 * direction, degrees=True).as_matrix() - vr_matrix[:3, :3] = vr_matrix[:3, :3] @ z_rotation - - # Apply VR to robot frame transformation - transform_matrix = VR_TO_ROBOT_FRAME @ vr_matrix - - with self._lcm_lock: - if index < 0 or index >= self.config.num_inputs: - logger.warning( - "Ignoring transform index %s (num_inputs=%s)", index, self.config.num_inputs - ) - return - self._lcm_controller_poses[index] = transform_matrix - controller_poses = list(self._lcm_controller_poses) - controller_gripper_values = list(self._lcm_gripper_values) - - self.update_controller_poses(controller_poses, controller_gripper_values) - - def _on_lcm_trigger(self, index: int, msg: Float32) -> None: - """Handle trigger/gripper value from LCM.""" - with self._lcm_lock: - if index < 0 or index >= self.config.num_inputs: - logger.warning( - "Ignoring trigger index %s (num_inputs=%s)", index, self.config.num_inputs - ) - return - self._lcm_gripper_values[index] = float(msg.data) - - def _on_lcm_teleop_enable(self, msg: Bool) -> None: - """Handle teleop enable/disable from LCM.""" - logger.info(f"Received teleop_enable: {msg.data}") - if bool(msg.data): - self.start_teleop() - else: - self.stop_teleop() - - -# Expose blueprint for declarative composition -vr_teleop_module = VRTeleopModule.blueprint diff --git a/dimos/teleop/robot_controllers/__init__.py b/dimos/teleop/robot_controllers/__init__.py deleted file mode 100644 index 545338db6d..0000000000 --- a/dimos/teleop/robot_controllers/__init__.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Teleoperation robot controllers.""" - -from dimos.teleop.robot_controllers.teleop_arm_controller import ( - TeleopArmController, - teleop_arm_controller, -) - -__all__ = [ - "TeleopArmController", - "teleop_arm_controller", -] diff --git a/dimos/teleop/robot_controllers/teleop_arm_controller.py b/dimos/teleop/robot_controllers/teleop_arm_controller.py deleted file mode 100644 index 673fc56c92..0000000000 --- a/dimos/teleop/robot_controllers/teleop_arm_controller.py +++ /dev/null @@ -1,367 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -""" -Teleop Arm Controller - -Receives controller delta poses from Quest3TeleopModule and applies them to the robot. -Auto-calibrates robot on first delta received. - -Architecture: -- Subscribes to controller_delta from Quest3TeleopModule -- On first delta received: gets robot's initial end-effector pose via RPC -- Calculates: target_pose = initial_robot_pose + delta -- Publishes cartesian_command (Pose) to driver -""" - -from dataclasses import dataclass -import threading -import time -import traceback -from typing import TYPE_CHECKING, Any - -import numpy as np - -if TYPE_CHECKING: - from numpy.typing import NDArray - -from dimos.core import In, Module, Out, rpc -from dimos.core.module import ModuleConfig -from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Vector3 -from dimos.msgs.std_msgs import Bool, Float32 -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -@dataclass -class TeleopArmControllerConfig(ModuleConfig): - """Configuration for Teleop Arm Controller.""" - - # Driver settings - driver_module_name: str = "ArmDriver" # Name of the driver module to get robot pose from - driver_method_name: str = "get_state" - dummy_driver: bool = False # If True, skip RPC calls and use zeros for initial pose - - # Control settings - control_frequency: float = 50.0 # Hz - control loop frequency - - # Safety settings - delta_timeout: float = 1 # Seconds - stop publishing if no new delta received - - -class TeleopArmController(Module): - """Teleop Arm Controller - applies delta poses to robot. - - This controller: - 1. Receives DELTA poses (PoseStamped) from Quest3TeleopModule for a single controller - 2. On first delta: gets initial robot end-effector pose via RPC (auto-calibration) - 3. Applies delta to initial robot pose: target = initial_robot + delta - 4. Publishes cartesian commands (Pose) to manipulator driver - 5. Optionally applies workspace limits - - Works with any manipulator driver that provides: - - state RPC method (configurable via driver_method_name) - - cartesian_command input topic (Pose) - """ - - default_config = TeleopArmControllerConfig - config: TeleopArmControllerConfig - - # Input topics - receiving DELTA poses as PoseStamped - controller_delta: In[PoseStamped] = None # type: ignore[assignment] - trigger_value: In[Float32] = None # type: ignore[assignment] # Gripper value 0.0-1.0 - - # Output topics (Pose for commands) - cartesian_command: Out[Pose] = None # type: ignore[assignment] - gripper_command: Out[Bool] = None # type: ignore[assignment] - - # RPC dependencies (dynamically set based on config) - rpc_calls: list[str] = [] - - def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize the teleop robot controller.""" - super().__init__(*args, **kwargs) - - # Set RPC calls dynamically based on driver name - self.rpc_calls = [ - f"{self.config.driver_module_name}.{self.config.driver_method_name}", - ] - - # Latest delta data - self._delta_pose: Pose | None = None - self._gripper_value: float = 0.0 # 0.0 to 1.0 - - # Timestamps for delta timeout detection - self._delta_timestamp: float | None = None - - # Arm initial state (auto-calibrated on first delta) - self._robot_initial_pose: Pose | None = None - self._robot_calibrated = False - - # Control loop - self._control_thread: threading.Thread | None = None - self._stop_event = threading.Event() - - # State lock - self._state_lock = threading.Lock() - - logger.info("TeleopArmController initialized - waiting for delta poses") - - # ========================================================================= - # Module Lifecycle - # ========================================================================= - - @rpc - def start(self) -> None: - """Start the teleop robot controller.""" - logger.info("Starting TeleopArmController...") - super().start() - - # Subscribe to input topics (delta poses) - if self.controller_delta and hasattr(self.controller_delta, "subscribe"): - self.controller_delta.subscribe(self._on_controller_delta) - logger.debug("Subscribed to controller_delta") - else: - logger.warning("controller_delta not wired; no delta subscription") - - if self.trigger_value and hasattr(self.trigger_value, "subscribe"): - self.trigger_value.subscribe(self._on_trigger_value) - logger.debug("Subscribed to trigger_value") - else: - logger.warning("trigger_value not wired; no trigger subscription") - - # Start control loop - self._stop_event.clear() - self._control_thread = threading.Thread( - target=self._control_loop, daemon=True, name="TeleopArmController" - ) - self._control_thread.start() - - logger.info("TeleopArmController started - waiting for delta poses to auto-calibrate") - - @rpc - def stop(self) -> None: - """Stop the teleop Arm controller.""" - logger.info("Stopping TeleopArmController") - - # Stop control loop - self._stop_event.set() - if self._control_thread: - self._control_thread.join(timeout=2.0) - - super().stop() - logger.info("TeleopArmController stopped") - - # ========================================================================= - # Input Callbacks - # ========================================================================= - - def _on_controller_delta(self, delta: PoseStamped) -> None: - """Callback for controller delta pose. - - On first delta, auto-calibrates robot. - """ - # Auto-calibrate robot on first delta received - if not self._robot_calibrated: - logger.info("First delta received - auto-calibrating robot...") - self._calibrate_robot() - - with self._state_lock: - self._delta_pose = Pose(position=delta.position, orientation=delta.orientation) - - # Log first few deltas - if not hasattr(self, "_delta_count"): - self._delta_count = 0 - self._delta_count += 1 - if self._delta_count <= 5: - logger.info( - f"Received controller delta #{self._delta_count}: " - f"pos=[{delta.position.x:.3f}, {delta.position.y:.3f}, {delta.position.z:.3f}], " - f"frame_id={delta.frame_id}" - ) - - def _on_trigger_value(self, gripper: Float32) -> None: - """Callback for controller gripper value (0.0-1.0).""" - with self._state_lock: - self._gripper_value = float(gripper.data) - - # ========================================================================= - # Arm Calibration (Auto-triggered on first delta) - # ========================================================================= - - def _calibrate_robot(self) -> bool: - """Calibrate robot by getting its initial pose via RPC. - - Called automatically when first delta pose is received. - If dummy_driver=True, uses zeros instead of making RPC calls. - - Returns: - True if calibration successful, False otherwise - """ - logger.info("Calibrating robot (getting initial pose)...") - - # If dummy_driver is enabled, use zeros for initial pose - if self.config.dummy_driver: - logger.info("Dummy driver mode - using zeros for initial pose") - self._robot_initial_pose = Pose( - position=Vector3(0.0, 0.0, 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, 0.0)), - ) - logger.info("Initial pose: pos=[0, 0, 0], rpy=[0, 0, 0]") - - self._robot_calibrated = True - logger.info("Arm calibration complete (dummy mode) - control active!") - return True - - try: - rpc_method_name = f"{self.config.driver_module_name}.{self.config.driver_method_name}" - get_state = self.get_rpc_calls(rpc_method_name) - - if get_state is None: - logger.error("RPC callable is None - check blueprint wiring") - return False - - result = get_state() - - if result and result.get("success"): - pose_data = result.get("pose", {}) - - # Store robot initial state - position = Vector3( - pose_data.get("x", 0.0), pose_data.get("y", 0.0), pose_data.get("z", 0.0) - ) - rpy = Vector3( - pose_data.get("roll", 0.0), - pose_data.get("pitch", 0.0), - pose_data.get("yaw", 0.0), - ) - self._robot_initial_pose = Pose( - position=position, - orientation=Quaternion.from_euler(rpy), - ) - logger.info(f"Arm initial pose: {pose_data}") - self._robot_calibrated = True - logger.info("Arm calibration complete - control active!") - return True - - error_msg = f"Failed to get robot cartesian state: {result}" - logger.error(error_msg) - return False - - except Exception as e: - logger.error(f"Arm calibration failed: {e}", exc_info=True) - traceback.print_exc() - return False - - @rpc - def recalibrate_robot(self) -> dict[str, Any]: - """Manually recalibrate robot (get new initial pose). - - Returns: - Dict with 'success' and optional 'message' or 'error' - """ - self._robot_calibrated = False - success = self._calibrate_robot() - - if success: - return {"success": True, "message": "Arm recalibrated"} - else: - return {"success": False, "error": "Recalibration failed"} - - @rpc - def is_robot_calibrated(self) -> bool: - """Check if robot is calibrated. - - Returns: - True if calibrated, False otherwise - """ - return self._robot_calibrated - - @rpc - def get_status(self) -> dict[str, Any]: - """Get controller status. - - Returns: - Dict with status information - """ - return { - "robot_calibrated": self._robot_calibrated, - "has_delta": self._delta_pose is not None, - } - - # ========================================================================= - # Control Loop - # ========================================================================= - - def _control_loop(self) -> None: - """Main control loop - applies deltas to robot initial pose.""" - logger.info("Control loop started") - period = 1.0 / self.config.control_frequency - next_time = time.perf_counter() + period - - loop_count = 0 - while not self._stop_event.is_set(): - try: - loop_count += 1 - - with self._state_lock: - robot_calibrated = self._robot_calibrated - delta_pose = self._delta_pose - gripper_val = self._gripper_value - robot_initial_pose = self._robot_initial_pose - - if robot_calibrated: - # Publishing Pose - if robot_initial_pose is not None and delta_pose is not None: - target_pose = robot_initial_pose + delta_pose - if self.cartesian_command and hasattr(self.cartesian_command, "publish"): - try: - self.cartesian_command.publish(target_pose) - except Exception as e: - logger.error(f"Failed to publish cartesian command: {e}") - - # Publishing Gripper - if self.gripper_command and hasattr(self.gripper_command, "publish"): - try: - self.gripper_command.publish(Bool(data=gripper_val > 0.5)) - except Exception as e: - logger.debug(f"Failed to publish gripper command: {e}") - - # Log state periodically - if loop_count <= 10 or loop_count % 100 == 0: - logger.debug( - f"Control loop #{loop_count}, " - f"robot_calibrated={robot_calibrated}, " - f"has_delta={delta_pose is not None}" - ) - - except Exception as e: - logger.error(f"Error in control loop: {e}", exc_info=True) - - # Rate control - next_time += period - sleep_time = next_time - time.perf_counter() - if sleep_time > 0: - time.sleep(sleep_time) - else: - if loop_count % 100 == 0: - logger.warning("Control loop overrun by %.4fs", -sleep_time) - next_time = time.perf_counter() + period - - logger.info("Control loop stopped") - - -# Expose blueprint for declarative composition -teleop_arm_controller = TeleopArmController.blueprint From 1484689eb4ae65fa27f72b290f16cdb8da14eb60 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 14 Jan 2026 01:42:12 -0800 Subject: [PATCH 49/72] Fix: pre-commit --- dimos/teleop/connectors/__init__.py | 4 ++-- dimos/teleop/connectors/base_connector.py | 5 +++-- dimos/teleop/devices/base_teleop_module.py | 3 ++- dimos/teleop/devices/vr_teleop_module.py | 20 ++++++++++++++------ dimos/teleop/teleop_blueprints.py | 7 ++++++- 5 files changed, 27 insertions(+), 12 deletions(-) diff --git a/dimos/teleop/connectors/__init__.py b/dimos/teleop/connectors/__init__.py index 51a4eb1ba2..50ba811a23 100644 --- a/dimos/teleop/connectors/__init__.py +++ b/dimos/teleop/connectors/__init__.py @@ -39,10 +39,10 @@ ) __all__ = [ - "BaseTeleopConnector", - "ConnectorConfig", "ArmConnector", "ArmConnectorConfig", + "BaseTeleopConnector", + "ConnectorConfig", "QuadrupedConnector", "QuadrupedConnectorConfig", ] diff --git a/dimos/teleop/connectors/base_connector.py b/dimos/teleop/connectors/base_connector.py index 6f04fd207b..989c69b727 100644 --- a/dimos/teleop/connectors/base_connector.py +++ b/dimos/teleop/connectors/base_connector.py @@ -24,9 +24,10 @@ from abc import ABC, abstractmethod from dataclasses import dataclass -from typing import Any +from typing import TYPE_CHECKING, Any -from dimos.msgs.geometry_msgs import Pose +if TYPE_CHECKING: + from dimos.msgs.geometry_msgs import Pose @dataclass diff --git a/dimos/teleop/devices/base_teleop_module.py b/dimos/teleop/devices/base_teleop_module.py index e62306cf2d..58e24370dd 100644 --- a/dimos/teleop/devices/base_teleop_module.py +++ b/dimos/teleop/devices/base_teleop_module.py @@ -38,7 +38,6 @@ from dimos.core.global_config import GlobalConfig from dimos.core.module import ModuleConfig from dimos.msgs.geometry_msgs import Pose, PoseStamped, Twist -from dimos.msgs.std_msgs import Bool from dimos.utils.logging_config import setup_logger from dimos.utils.transform_utils import matrix_to_pose @@ -48,6 +47,8 @@ import numpy as np from numpy.typing import NDArray + from dimos.msgs.std_msgs import Bool + logger = setup_logger() try: diff --git a/dimos/teleop/devices/vr_teleop_module.py b/dimos/teleop/devices/vr_teleop_module.py index c604062b61..2b4553bbdb 100644 --- a/dimos/teleop/devices/vr_teleop_module.py +++ b/dimos/teleop/devices/vr_teleop_module.py @@ -42,15 +42,15 @@ from scipy.spatial.transform import Rotation as R from dimos.core import In, rpc -from dimos.msgs.std_msgs import Bool, Float32 from dimos.teleop.devices.base_teleop_module import BaseTeleopConfig, BaseTeleopModule from dimos.utils.logging_config import setup_logger -from dimos_lcm.geometry_msgs import Transform as LCMTransform if TYPE_CHECKING: + from dimos_lcm.geometry_msgs import Transform as LCMTransform from numpy.typing import NDArray from dimos.core.global_config import GlobalConfig + from dimos.msgs.std_msgs import Bool, Float32 logger = setup_logger() @@ -157,14 +157,20 @@ def start(self) -> None: left_index = enabled_indices[0] if len(enabled_indices) > 0 else 0 right_index = enabled_indices[1] if len(enabled_indices) > 1 else 1 - logger.info(f"VR controller mapping: leftโ†’index {left_index} {self.config.input_labels[left_index]}, rightโ†’index {right_index} {self.config.input_labels[right_index]}") + logger.info( + f"VR controller mapping: leftโ†’index {left_index} {self.config.input_labels[left_index]}, rightโ†’index {right_index} {self.config.input_labels[right_index]}" + ) # Subscribe to LCM inputs with dynamically determined indices if self.vr_left_transform and self.vr_left_transform.transport: - self.vr_left_transform.subscribe(lambda msg, idx=left_index: self._on_lcm_transform(idx, msg)) + self.vr_left_transform.subscribe( + lambda msg, idx=left_index: self._on_lcm_transform(idx, msg) + ) if self.vr_right_transform and self.vr_right_transform.transport: - self.vr_right_transform.subscribe(lambda msg, idx=right_index: self._on_lcm_transform(idx, msg)) + self.vr_right_transform.subscribe( + lambda msg, idx=right_index: self._on_lcm_transform(idx, msg) + ) if self.vr_trigger_0 and self.vr_trigger_0.transport: self.vr_trigger_0.subscribe(lambda msg, idx=left_index: self._on_lcm_trigger(idx, msg)) @@ -217,7 +223,9 @@ def _init_connector_poses(self) -> None: # Get state via RPC if connector has a real driver result = None if not connector.config.dummy_driver: - rpc_method_name = f"{connector.config.driver_module_name}.{connector.config.driver_method_name}" + rpc_method_name = ( + f"{connector.config.driver_module_name}.{connector.config.driver_method_name}" + ) if rpc_method_name in self.rpc_calls: try: get_state = self.get_rpc_calls(rpc_method_name) diff --git a/dimos/teleop/teleop_blueprints.py b/dimos/teleop/teleop_blueprints.py index 589d9a8156..0690561003 100644 --- a/dimos/teleop/teleop_blueprints.py +++ b/dimos/teleop/teleop_blueprints.py @@ -50,7 +50,12 @@ from dimos.core.blueprints import autoconnect from dimos.core.transport import LCMTransport from dimos.msgs.std_msgs import Bool, Float32 -from dimos.teleop.connectors import ArmConnector, ArmConnectorConfig, QuadrupedConnector, QuadrupedConnectorConfig +from dimos.teleop.connectors import ( + ArmConnector, + ArmConnectorConfig, + QuadrupedConnector, + QuadrupedConnectorConfig, +) from dimos.teleop.devices.vr_teleop_module import vr_teleop_module # ============================================================================= From 7b4226ec61bd5e4cbf6d09eceabaf8985030e195 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 14 Jan 2026 17:25:41 -0800 Subject: [PATCH 50/72] Feat: new input format, methods from utils --- dimos/teleop/devices/base_teleop_module.py | 355 +++++++-------------- dimos/teleop/devices/vr_teleop_module.py | 251 ++++----------- dimos/teleop/teleop_blueprints.py | 74 +---- 3 files changed, 185 insertions(+), 495 deletions(-) diff --git a/dimos/teleop/devices/base_teleop_module.py b/dimos/teleop/devices/base_teleop_module.py index 58e24370dd..5271d79c24 100644 --- a/dimos/teleop/devices/base_teleop_module.py +++ b/dimos/teleop/devices/base_teleop_module.py @@ -14,16 +14,10 @@ # limitations under the License. """ -Base Teleoperation Module +Base Teleoperation Module. -Abstract base class for all teleoperation devices that provides: -- Multi-controller calibration -- Delta pose computation (current - initial) -- LCM topic publishing (delta poses + trigger states) -- Rerun visualization -- Standard RPC interface - -Device-specific modules inherit from this and implement their connection logic. +Provides calibration, delta computation, and command publishing. +Subclasses implement device-specific input handling. """ from __future__ import annotations @@ -31,19 +25,21 @@ from abc import ABC from dataclasses import dataclass, field from enum import Enum -import time from typing import TYPE_CHECKING, Any from dimos.core import Module, Out, rpc -from dimos.core.global_config import GlobalConfig from dimos.core.module import ModuleConfig from dimos.msgs.geometry_msgs import Pose, PoseStamped, Twist from dimos.utils.logging_config import setup_logger +from dimos.utils.teleop_transforms import compute_active_indices, parse_pose_from_dict +from dimos.utils.teleop_visualization import ( + init_rerun_visualization, + visualize_controller_pose, + visualize_trigger_value, +) from dimos.utils.transform_utils import matrix_to_pose if TYPE_CHECKING: - from collections.abc import Sequence - import numpy as np from numpy.typing import NDArray @@ -51,41 +47,31 @@ logger = setup_logger() -try: - import rerun as rr - - from dimos.dashboard.rerun_init import connect_rerun - - RERUN_AVAILABLE = True -except ImportError: - RERUN_AVAILABLE = False - @dataclass class BaseTeleopConfig(ModuleConfig): - """Base configuration for teleoperation modules.""" + """Base configuration for teleoperation modules. + + output_types determines active indices: PoseStampedโ†’0,1, Twistโ†’2,3 + """ - # Control settings - num_inputs: int = 1 # Number of inputs (controllers) - enable_inputs: list[bool] = field(default_factory=list) - input_labels: list[str] = field(default_factory=list) + output_types: list[type] = field(default_factory=lambda: [PoseStamped, PoseStamped]) + input_labels: list[str] = field(default_factory=lambda: ["left", "right"]) + robot_pose_rpc_methods: list[str | None] = field(default_factory=list) - # Visualization settings - visualize_in_rerun: bool = True # Visualize controller poses in Rerun - log_input_data: bool = False # Log input pose/gripper data periodically - log_input_data_interval: int = 100 # Log every N publishes when enabled + visualize_in_rerun: bool = True - # Safety limits - safety_limits: bool = True - position_scale: float = 1.0 # Scale factor for positions TODO: Implement proportional scaling - max_velocity: float = 0.5 # m/s + linear_scale: float = 1.0 + angular_scale: float = 1.0 + max_linear_velocity: float = 0.5 + max_angular_velocity: float = 1.0 + gripper_threshold: float = 0.5 class TeleopStatusKey(str, Enum): """Status dictionary keys (controller_* entries are indexed by controller number).""" IS_CALIBRATED = "is_calibrated" - CONTROLLER_ENABLED = "controller_{index}_enabled" CONTROLLER_HAS_DATA = "controller_{index}_has_data" CONTROLLER_TRIGGER_VALUE = "controller_{index}_trigger_value" CONTROLLER_LABEL = "controller_{index}_label" @@ -94,248 +80,200 @@ class TeleopStatusKey(str, Enum): class BaseTeleopModule(Module, ABC): """Base class for teleoperation modules. - ## LCM Topics (Output): - - controller_delta_{i}: Out[PoseStamped] - Controller i delta pose - - trigger_value_{i}: Out[Float32] - Controller i trigger/gripper value (0.0-1.0) - - ## RPC Methods: - - calibrate() -> dict: Calibrate by capturing initial poses - - reset_calibration() -> dict: Reset calibration - - is_calibrated() -> bool: Check if calibrated - - get_status() -> dict: Get teleoperation status + Handles calibration, delta computation, and command publishing. + Subclasses implement device-specific input handling. """ default_config = BaseTeleopConfig config: BaseTeleopConfig - # LCM Output topics + # Output topics: PoseStamped for arms (0,1), Twist for locomotion (2,3) controller_delta_0: Out[PoseStamped] = None # type: ignore trigger_value_0: Out[Bool] = None # type: ignore - controller_delta_1: Out[PoseStamped] = None # type: ignore trigger_value_1: Out[Bool] = None # type: ignore - controller_delta_2: Out[Twist] = None # type: ignore trigger_value_2: Out[Bool] = None # type: ignore - controller_delta_3: Out[Twist] = None # type: ignore trigger_value_3: Out[Bool] = None # type: ignore - def __init__( - self, global_config: GlobalConfig | None = None, *args: Any, **kwargs: Any - ) -> None: + def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) - # Get global config for Rerun connection - self._global_config = global_config or GlobalConfig() - - # Calibration state self._is_calibrated = False - self._initial_poses: list[Pose | None] = [None] * self.config.num_inputs - - # Latest controller data (absolute poses) - self._all_poses: list[NDArray[np.float32] | None] = [None] * self.config.num_inputs - self._all_trigger_values: list[float] = [0.0] * self.config.num_inputs self._tracking_msg_count = 0 self._publish_count = 0 - # Set default values for enable_inputs and input_labels if not provided - if not self.config.enable_inputs: - self.config.enable_inputs = [True] * self.config.num_inputs - if not self.config.input_labels: - self.config.input_labels = [f"controller_{i}" for i in range(self.config.num_inputs)] - - # check for mismatches between num_inputs and enable_inputs or input_labels - if len(self.config.enable_inputs) != self.config.num_inputs: + if len(self.config.input_labels) != len(self.config.output_types): raise ValueError( - f"Number of enable_inputs ({len(self.config.enable_inputs)}) does not match num_inputs ({self.config.num_inputs})" - ) - if len(self.config.input_labels) != self.config.num_inputs: - raise ValueError( - f"Number of input_labels ({len(self.config.input_labels)}) does not match num_inputs ({self.config.num_inputs})" + f"input_labels length ({len(self.config.input_labels)}) must match " + f"output_types length ({len(self.config.output_types)})" ) - logger.info(f"{self.__class__.__name__} base initialized") + self._active_indices = compute_active_indices(self.config.output_types) + self._output_types = { + idx: self.config.output_types[i] for i, idx in enumerate(self._active_indices) + } - # ========================================================================= - # Module Lifecycle - # ========================================================================= + self._initial_poses: dict[int, Pose | None] = {i: None for i in self._active_indices} + self._initial_robot_poses: dict[int, Pose | None] = {i: None for i in self._active_indices} + self._all_poses: dict[int, NDArray[np.float32] | None] = { + i: None for i in self._active_indices + } + self._all_trigger_values: dict[int, float] = {i: 0.0 for i in self._active_indices} + + logger.info( + f"{self.__class__.__name__} initialized: indices={self._active_indices}, " + f"types={[t.__name__ for t in self.config.output_types]}" + ) + + def _get_label(self, index: int) -> str: + """Get label for an active index.""" + try: + i = self._active_indices.index(index) + return self.config.input_labels[i] + except (ValueError, IndexError): + return f"controller_{index}" @rpc def start(self) -> None: - """Start the teleoperation module.""" - logger.info(f"Starting {self.__class__.__name__}...") super().start() - - # Connect to Rerun for visualization if enabled - if ( - self.config.visualize_in_rerun - and RERUN_AVAILABLE - and self._global_config.viewer_backend.startswith("rerun") - ): - connect_rerun(global_config=self._global_config) - logger.info("Connected to Rerun for controller visualization") + logger.info(f"Starting {self.__class__.__name__}...") + if self.config.visualize_in_rerun: + init_rerun_visualization() @rpc def stop(self) -> None: - """Stop the teleoperation module.""" logger.info(f"Stopping {self.__class__.__name__}...") super().stop() - # ========================================================================= - # Calibration - # ========================================================================= - @rpc def calibrate(self) -> dict[str, Any]: - """Calibrate by capturing initial controller poses. - - This is typically called when a calibration button is pressed. - Captures the current controller poses as the "zero" reference. - After calibration, delta poses are published. - - Returns: - Dict with 'success' and optional 'message' or 'error' - """ + """Capture current controller poses as the zero reference.""" logger.info("Calibrating controllers...") try: - # Check if we have controller data for enabled inputs - enabled_indices = [i for i, enabled in enumerate(self.config.enable_inputs) if enabled] - if not enabled_indices: - return { - "success": False, - "error": "No controllers are enabled. Enable at least one controller and try again.", - } - - # Capture controller initial poses - for i in enabled_indices: - pose_matrix = self._all_poses[i] + if not self._active_indices: + return {"success": False, "error": "No controllers are enabled"} + + for i, idx in enumerate(self._active_indices): + pose_matrix = self._all_poses.get(idx) if pose_matrix is not None: pose_obj = matrix_to_pose(pose_matrix) - self._initial_poses[i] = pose_obj - logger.info( - f"Captured controller initial: " - f"pos=[{pose_obj.x:.3f}, {pose_obj.y:.3f}, {pose_obj.z:.3f}], " - f"rpy=[{pose_obj.roll:.3f}, {pose_obj.pitch:.3f}, {pose_obj.yaw:.3f}]" - ) + self._initial_poses[idx] = pose_obj + logger.info(f"Captured controller {self._get_label(idx)} initial: {pose_obj}") else: - logger.error( - f"Controller {self.config.input_labels[i]} data is None during calibration" - ) return { "success": False, - "error": f"Controller {self.config.input_labels[i]} data is None. Move controller and try again.", + "error": f"Controller {self._get_label(idx)} data is None", } - self._is_calibrated = True + # Get initial robot pose via RPC if configured + output_type = self._output_types.get(idx) + if i < len(self.config.robot_pose_rpc_methods): + rpc_method = self.config.robot_pose_rpc_methods[i] + if rpc_method is not None: + self._initial_robot_poses[idx] = self._get_robot_initial_pose( + output_type, rpc_method + ) + else: + self._initial_robot_poses[idx] = parse_pose_from_dict(output_type) + else: + self._initial_robot_poses[idx] = parse_pose_from_dict(output_type) + self._is_calibrated = True logger.info("Calibration complete. Now streaming delta poses...") - return {"success": True, "message": "Calibrated - move controllers to control robot"} + return {"success": True, "message": "Calibrated"} except Exception as e: logger.error(f"Calibration failed: {e}", exc_info=True) return {"success": False, "error": str(e)} + def _get_robot_initial_pose(self, output_type: type, rpc_method: str) -> Pose | None: + """Get initial robot pose via RPC call.""" + try: + rpc_call = self.get_rpc_calls(rpc_method) + rpc_result = rpc_call() + pose = parse_pose_from_dict(output_type, rpc_result) + logger.info(f"Got robot initial pose via {rpc_method}: {pose}") + return pose + except Exception as e: + logger.warning(f"RPC call {rpc_method} failed: {e}, using zero pose") + return parse_pose_from_dict(output_type) + @rpc def reset_calibration(self) -> dict[str, Any]: - """Reset calibration. Stops streaming until recalibrated""" + """Reset calibration. Stops streaming until recalibrated.""" self._is_calibrated = False - self._initial_poses = [None] * self.config.num_inputs - - logger.info("Calibration reset. Recalibrate to resume teleoperation...") - return {"success": True, "message": "Calibration reset - recalibrate to resume"} + self._initial_poses = {i: None for i in self._active_indices} + self._initial_robot_poses = {i: None for i in self._active_indices} + logger.info("Calibration reset.") + return {"success": True, "message": "Calibration reset"} @rpc def is_calibrated(self) -> bool: - """Check if calibrated""" return self._is_calibrated @rpc def get_status(self) -> dict[str, Any]: - """Get current teleoperation status""" + """Get current teleoperation status.""" status: dict[str, Any] = { TeleopStatusKey.IS_CALIBRATED.value: self._is_calibrated, + "active_indices": self._active_indices, } - for i in range(self.config.num_inputs): - status[TeleopStatusKey.CONTROLLER_ENABLED.value.format(index=i)] = ( - self.config.enable_inputs[i] - ) + for i in self._active_indices: status[TeleopStatusKey.CONTROLLER_HAS_DATA.value.format(index=i)] = ( - self._all_poses[i] is not None + self._all_poses.get(i) is not None ) status[TeleopStatusKey.CONTROLLER_TRIGGER_VALUE.value.format(index=i)] = ( - self._all_trigger_values[i] - ) - status[TeleopStatusKey.CONTROLLER_LABEL.value.format(index=i)] = ( - self.config.input_labels[i] + self._all_trigger_values.get(i, 0.0) ) - + status[TeleopStatusKey.CONTROLLER_LABEL.value.format(index=i)] = self._get_label(i) return status - # ========================================================================= - # Controller Data Processing (called by subclasses) - # ========================================================================= - def compute_deltas( self, - controller_poses: Sequence[NDArray[np.float32] | None], - controller_trigger_values: Sequence[float], - ) -> list[Pose | None]: - """Compute delta poses from controller input. - - Args: - controller_poses: List of 4x4 transform matrices for each controller. - controller_trigger_values: List of trigger values (0.0-1.0). - - Returns: - List of delta poses (current - initial) for each input, or None if not available. - """ + controller_poses: dict[int, NDArray[np.float32] | None], + controller_trigger_values: dict[int, float], + ) -> dict[int, Pose | None]: + """Compute delta = current_pose - initial_pose for each controller.""" self._tracking_msg_count += 1 - self._all_poses = list(controller_poses) - self._all_trigger_values = list(controller_trigger_values) + self._all_poses = controller_poses + self._all_trigger_values = controller_trigger_values if not self._is_calibrated: if self._tracking_msg_count <= 1 or self._tracking_msg_count % 100 == 0: logger.warning("Not calibrated. Calibrate to start teleoperation.") - return [None] * self.config.num_inputs + return {i: None for i in self._active_indices} self._publish_count += 1 + deltas: dict[int, Pose | None] = {} - # Compute deltas for each input - deltas: list[Pose | None] = [] - for i in range(self.config.num_inputs): - if not self.config.enable_inputs[i]: - deltas.append(None) + for i in self._active_indices: + pose_matrix = self._all_poses.get(i) + if pose_matrix is None: + deltas[i] = None continue - pose_matrix = self._all_poses[i] - if pose_matrix is None: - deltas.append(None) + initial_pose = self._initial_poses.get(i) + if initial_pose is None: + deltas[i] = None continue - initial_pose = self._initial_poses[i] pose_obj = matrix_to_pose(pose_matrix) delta_pose = pose_obj - initial_pose - # Visualize - self._visualize_controller_in_rerun(pose_obj, self.config.input_labels[i]) - self._visualize_trigger_in_rerun( - self._all_trigger_values[i], self.config.input_labels[i] - ) + if self.config.visualize_in_rerun: + label = self._get_label(i) + visualize_controller_pose(pose_obj, label) + visualize_trigger_value(self._all_trigger_values.get(i, 0.0), label) - deltas.append(delta_pose) + deltas[i] = delta_pose return deltas def publish_command(self, index: int, command: Any, aux_command: Any = None) -> None: - """Publish a command to the output topics for a given input index. - - Args: - index: Input index (0, 1, 2, 3...). - command: The main command (PoseStamped, Twist, etc.) to publish. - aux_command: Optional auxiliary command (Bool for gripper, etc.). - """ + """Publish command to controller_delta_{index} and trigger_value_{index}.""" if command is not None: controller_output = getattr(self, f"controller_delta_{index}", None) if controller_output and hasattr(controller_output, "publish"): @@ -351,52 +289,3 @@ def publish_command(self, index: int, command: Any, aux_command: Any = None) -> trigger_output.publish(aux_command) except Exception as e: logger.debug(f"Failed to publish aux command for index {index}: {e}") - - # ========================================================================= - # Rerun Visualization - # ========================================================================= - - def _visualize_controller_in_rerun(self, controller_pose: Pose, controller_label: str) -> None: - """Visualize controller absolute pose in Rerun""" - if not ( - self.config.visualize_in_rerun - and RERUN_AVAILABLE - and self._global_config.viewer_backend.startswith("rerun") - ): - return - - try: - controller_pose_stamped = PoseStamped( - ts=time.time(), - frame_id=f"world/teleop/{controller_label}_controller", - position=controller_pose.position, - orientation=controller_pose.orientation, - ) - rr.log( - f"world/teleop/{controller_label}_controller", - controller_pose_stamped.to_rerun(), # type: ignore[no-untyped-call] - ) - # Log 3D axes to visualize controller orientation (X=red, Y=green, Z=blue) - rr.log( - f"world/teleop/{controller_label}_controller/axes", - rr.Axes3D(length=0.15), # type: ignore[attr-defined] - ) - except Exception as e: - logger.debug(f"Failed to log {controller_label} controller to Rerun: {e}") - - def _visualize_trigger_in_rerun(self, trigger_value: float, controller_label: str) -> None: - """Visualize trigger value in Rerun as a scalar time series.""" - if not ( - self.config.visualize_in_rerun - and RERUN_AVAILABLE - and self._global_config.viewer_backend.startswith("rerun") - ): - return - - try: - rr.log( - f"world/teleop/{controller_label}_controller/trigger", - rr.Scalars(trigger_value), # type: ignore[attr-defined] - ) - except Exception as e: - logger.debug(f"Failed to log {controller_label} trigger to Rerun: {e}") diff --git a/dimos/teleop/devices/vr_teleop_module.py b/dimos/teleop/devices/vr_teleop_module.py index 2b4553bbdb..cd4b2603f9 100644 --- a/dimos/teleop/devices/vr_teleop_module.py +++ b/dimos/teleop/devices/vr_teleop_module.py @@ -14,21 +14,10 @@ # limitations under the License. """ -VR Teleoperation Module +VR Teleoperation Module for Quest 3 controllers. -A dimos Module that runs the WebSocket signaling server for VR teleoperation, -receives controller tracking data, and streams DELTA poses to TeleopRobotController. - -This module inherits from BaseTeleopModule which handles: -- Calibration (capture initial controller poses) -- Delta computation (current - initial) -- Publishing delta poses to LCM -- Rerun visualization -- Standard RPC interface - -This module implements VR-specific: -- FastAPI/WebSocket server for VR headset -- X button handler for calibration trigger +Receives VR controller tracking data via LCM from Deno bridge, +transforms from WebXR to robot frame, computes deltas, and publishes commands. """ from __future__ import annotations @@ -38,130 +27,73 @@ import time from typing import TYPE_CHECKING, Any -import numpy as np -from scipy.spatial.transform import Rotation as R - from dimos.core import In, rpc +from dimos.msgs.geometry_msgs import PoseStamped from dimos.teleop.devices.base_teleop_module import BaseTeleopConfig, BaseTeleopModule from dimos.utils.logging_config import setup_logger +from dimos.utils.teleop_transforms import transform_delta, transform_vr_to_robot if TYPE_CHECKING: from dimos_lcm.geometry_msgs import Transform as LCMTransform + import numpy as np from numpy.typing import NDArray - from dimos.core.global_config import GlobalConfig from dimos.msgs.std_msgs import Bool, Float32 -logger = setup_logger() -# Coordinate frame transformation from VR (WebXR) to robot frame -# WebXR: X=right, Y=up, Z=back (towards user) -# Robot: X=forward, Y=left, Z=up -VR_TO_ROBOT_FRAME = np.array( - [ - [0, 0, -1, 0], # Robot X = -VR Z (forward) - [-1, 0, 0, 0], # Robot Y = -VR X (left) - [0, 1, 0, 0], # Robot Z = +VR Y (up) - [0, 0, 0, 1], - ], - dtype=np.float64, -) +logger = setup_logger() @dataclass class VRTeleopConfig(BaseTeleopConfig): """Configuration for VR Teleoperation Module.""" - # Control settings - num_inputs: int = 2 # Number of inputs (controllers) - enable_inputs: list[bool] = field(default_factory=lambda: [True, True]) + output_types: list[type] = field(default_factory=lambda: [PoseStamped, PoseStamped]) input_labels: list[str] = field(default_factory=lambda: ["left_vr", "right_vr"]) - - # Visualization settings - visualize_in_rerun: bool = True # Visualize controller poses in Rerun - log_input_data: bool = False # Log input pose/gripper data periodically - log_input_data_interval: int = 100 # Log every N publishes when enabled - - # Injectable connectors (one per input, None for raw delta output) - # Use ArmConnector, QuadrupedConnector, etc. from dimos.teleop.connectors - connectors: list[Any] = field(default_factory=list) - - # Control loop frequency - control_loop_hz: float = 50.0 # Hz + control_loop_hz: float = 50.0 class VRTeleopModule(BaseTeleopModule): - """VR Teleoperation Module""" + """VR Teleoperation Module for Quest 3 controllers. + + Subscribes to controller data from Deno bridge, transforms WebXRโ†’robot frame, + computes deltas from calibration point, and publishes commands. + """ default_config = VRTeleopConfig config: VRTeleopConfig - # LCM input topics for raw Transform from web clients (uses dimos_lcm type for correct msg_name) + # LCM inputs from Deno bridge vr_left_transform: In[LCMTransform] = None # type: ignore[assignment] vr_right_transform: In[LCMTransform] = None # type: ignore[assignment] vr_trigger_0: In[Float32] = None # type: ignore[assignment] vr_trigger_1: In[Float32] = None # type: ignore[assignment] - teleop_enable: In[Bool] = None # type: ignore[assignment] - - # RPC dependencies (dynamically set based on connector configs) - rpc_calls: list[str] = [] + teleop_enable: In[Bool] = None # X button calibration toggle - def __init__( - self, global_config: GlobalConfig | None = None, *args: Any, **kwargs: Any - ) -> None: - # Remove global_config from kwargs to avoid passing it twice - kwargs.pop("global_config", None) - # Pass global_config as positional argument to match base class signature - super().__init__(global_config, *args, **kwargs) + def __init__(self, *args: Any, **kwargs: Any) -> None: + super().__init__(*args, **kwargs) self._lcm_lock = threading.Lock() - self._lcm_controller_poses: list[NDArray[np.float32] | None] = [ - None - ] * self.config.num_inputs - self._lcm_gripper_values: list[float] = [0.0] * self.config.num_inputs - - # Initialize connectors list (pad to num_inputs) - self._connectors: list[Any] = list(self.config.connectors) - while len(self._connectors) < self.config.num_inputs: - self._connectors.append(None) - - # Set RPC calls dynamically based on connector configs - self.rpc_calls = [] - for connector in self._connectors: - if connector is not None and hasattr(connector, "config"): - if not getattr(connector.config, "dummy_driver", False): - driver_name = getattr(connector.config, "driver_module_name", "") - method_name = getattr(connector.config, "driver_method_name", "get_state") - if driver_name: - self.rpc_calls.append(f"{driver_name}.{method_name}") - - # Control loop + self._lcm_controller_poses: dict[int, NDArray[np.float32] | None] = { + i: None for i in self._active_indices + } + self._lcm_gripper_values: dict[int, float] = {i: 0.0 for i in self._active_indices} self._control_loop_thread: threading.Thread | None = None self._control_loop_running = False - logger.info("VRTeleopModule initialized") - - # ========================================================================= - # Module Lifecycle (VR-specific) - # ========================================================================= - @rpc def start(self) -> None: - """Start the VR teleoperation module and signaling server.""" + """Start the VR teleoperation module.""" super().start() - # Determine enabled indices from config - enabled_indices = [i for i, enabled in enumerate(self.config.enable_inputs) if enabled] - - # Map left VR controller to first enabled index, right to second enabled index - left_index = enabled_indices[0] if len(enabled_indices) > 0 else 0 - right_index = enabled_indices[1] if len(enabled_indices) > 1 else 1 + left_index = self._active_indices[0] if len(self._active_indices) > 0 else 0 + right_index = self._active_indices[1] if len(self._active_indices) > 1 else 1 logger.info( - f"VR controller mapping: leftโ†’index {left_index} {self.config.input_labels[left_index]}, rightโ†’index {right_index} {self.config.input_labels[right_index]}" + f"VR controller mapping: leftโ†’{self._get_label(left_index)}(idx {left_index}), " + f"rightโ†’{self._get_label(right_index)}(idx {right_index})" ) - # Subscribe to LCM inputs with dynamically determined indices if self.vr_left_transform and self.vr_left_transform.transport: self.vr_left_transform.subscribe( lambda msg, idx=left_index: self._on_lcm_transform(idx, msg) @@ -185,119 +117,51 @@ def start(self) -> None: @rpc def stop(self) -> None: - """Stop the VR teleoperation module and signaling server.""" + """Stop the VR teleoperation module.""" logger.info("Stopping VR Teleoperation Module...") + self._stop_control_loop() super().stop() @rpc def start_teleop(self) -> dict[str, Any]: - """RPC helper to trigger calibration for LCM-only clients.""" - logger.info("RPC start_teleop called - calibrating VR...") - - # Initialize connectors with robot poses via RPC - self._init_connector_poses() + """Calibrate and start teleoperation (called via X button).""" + logger.info("Starting teleop - calibrating VR...") res = self.calibrate() + if not res.get("success"): + logger.error(f"Calibration failed: {res.get('error')}") return res @rpc def stop_teleop(self) -> dict[str, Any]: - """RPC helper to stop teleop for LCM-only clients.""" - logger.info("RPC stop_teleop called - resetting calibration...") + """Stop teleoperation and reset calibration.""" + logger.info("Stopping teleop - resetting calibration...") self._stop_control_loop() return self.reset_calibration() def _on_lcm_teleop_enable(self, msg: Bool) -> None: - """Handle teleop enable/disable from LCM.""" + """Handle teleop enable/disable from X button.""" logger.info(f"Received teleop_enable: {msg.data}") if bool(msg.data): self.start_teleop() else: self.stop_teleop() - def _init_connector_poses(self) -> None: - """Get initial poses from robot drivers and set on connectors.""" - for connector in self._connectors: - if connector is None: - continue - - # Get state via RPC if connector has a real driver - result = None - if not connector.config.dummy_driver: - rpc_method_name = ( - f"{connector.config.driver_module_name}.{connector.config.driver_method_name}" - ) - if rpc_method_name in self.rpc_calls: - try: - get_state = self.get_rpc_calls(rpc_method_name) - if get_state is not None: - result = get_state() - except Exception as e: - logger.error(f"Failed to get robot state via RPC: {e}") - - connector.set_initial_pos(result) - def _on_lcm_transform(self, index: int, transform: LCMTransform) -> None: - """Handle raw Transform input from LCM and forward to base processing. - - The transform comes directly from WebXR. We: - 1. Convert to 4x4 matrix - 2. Apply controller alignment rotation (in VR space) - 3. Apply VRโ†’robot coordinate frame transformation - - WebXR coordinate system: X=right, Y=up, Z=back (towards user) - Robot coordinate system: X=forward, Y=left, Z=up - """ - # start control loop + """Handle controller transform, converting WebXR to robot frame.""" self._start_control_loop() - - # Convert LCM Transform to 4x4 matrix - t = transform.translation - q = transform.rotation - rot_matrix = R.from_quat([q.x, q.y, q.z, q.w]).as_matrix() - - # Build VR-space matrix - vr_matrix = np.eye(4, dtype=np.float64) - vr_matrix[:3, :3] = rot_matrix - vr_matrix[:3, 3] = [t.x, t.y, t.z] - - # Apply controller alignment rotation in VR space (from tracking_processor.py) - # Right controller (index 1) rotates -90ยฐ around Z, left (index 0) rotates +90ยฐ - # This aligns the controller orientation with gripper orientation - direction = 1 if index == 0 else -1 - z_rotation = R.from_euler("z", 90 * direction, degrees=True).as_matrix() - vr_matrix[:3, :3] = vr_matrix[:3, :3] @ z_rotation - - # Apply VR to robot frame transformation - transform_matrix = VR_TO_ROBOT_FRAME @ vr_matrix - + is_left = index == self._active_indices[0] + transform_matrix = transform_vr_to_robot(transform, is_left_controller=is_left) with self._lcm_lock: - if index < 0 or index >= self.config.num_inputs: - logger.warning( - "Ignoring transform index %s (num_inputs=%s)", index, self.config.num_inputs - ) - return self._lcm_controller_poses[index] = transform_matrix def _on_lcm_trigger(self, index: int, msg: Float32) -> None: - """Handle trigger/gripper value from LCM.""" - - # start control loop + """Handle trigger value for gripper control.""" self._start_control_loop() - with self._lcm_lock: - if index < 0 or index >= self.config.num_inputs: - logger.warning( - "Ignoring trigger index %s (num_inputs=%s)", index, self.config.num_inputs - ) - return self._lcm_gripper_values[index] = float(msg.data) - # ========================================================================= - # Control Loop - # ========================================================================= - def _start_control_loop(self) -> None: - """Start the control loop thread.""" + """Start the control loop thread if not running.""" if self._control_loop_running: return @@ -311,7 +175,6 @@ def _start_control_loop(self) -> None: logger.info(f"Control loop started at {self.config.control_loop_hz} Hz") def _stop_control_loop(self) -> None: - """Stop the control loop thread.""" self._control_loop_running = False if self._control_loop_thread is not None: self._control_loop_thread.join(timeout=1.0) @@ -319,40 +182,44 @@ def _stop_control_loop(self) -> None: logger.info("Control loop stopped") def _control_loop(self) -> None: - """Main control loop running at fixed frequency.""" + """Main control loop: compute deltas and publish commands at fixed rate.""" period = 1.0 / self.config.control_loop_hz while self._control_loop_running: loop_start = time.perf_counter() - # Get current poses and trigger values with self._lcm_lock: - controller_poses = list(self._lcm_controller_poses) - controller_trigger_values = list(self._lcm_gripper_values) + controller_poses = dict(self._lcm_controller_poses) + controller_trigger_values = dict(self._lcm_gripper_values) - # Compute deltas deltas = self.compute_deltas(controller_poses, controller_trigger_values) - # Route each delta through its connector - for i in range(self.config.num_inputs): - delta = deltas[i] if i < len(deltas) else None + for i in self._active_indices: + delta = deltas.get(i) if delta is None: continue - connector = self._connectors[i] if i < len(self._connectors) else None - if connector is None: + output_type = self._output_types.get(i) + if output_type is None: continue - # Transform through connector and publish - command, aux_command = connector.transform_delta(delta, self._all_trigger_values[i]) + command, aux_command = transform_delta( + delta_pose=delta, + trigger_value=self._all_trigger_values.get(i, 0.0), + output_type=output_type, + initial_robot_pose=self._initial_robot_poses.get(i), + linear_scale=self.config.linear_scale, + angular_scale=self.config.angular_scale, + max_linear_velocity=self.config.max_linear_velocity, + max_angular_velocity=self.config.max_angular_velocity, + gripper_threshold=self.config.gripper_threshold, + ) self.publish_command(i, command, aux_command) - # Sleep for remaining time elapsed = time.perf_counter() - loop_start sleep_time = period - elapsed if sleep_time > 0: time.sleep(sleep_time) -# Expose blueprint for declarative composition vr_teleop_module = VRTeleopModule.blueprint diff --git a/dimos/teleop/teleop_blueprints.py b/dimos/teleop/teleop_blueprints.py index 0690561003..7b2494f420 100644 --- a/dimos/teleop/teleop_blueprints.py +++ b/dimos/teleop/teleop_blueprints.py @@ -12,95 +12,29 @@ # See the License for the specific language governing permissions and # limitations under the License. -""" -Blueprints for VR teleoperation. - -This module provides declarative blueprints for VR teleoperation with any robot. - -Architecture (with connectors): - VRTeleopModule (VR calibration, delta computation) - โ†“ Calibrate button โ†’ calibrate VR - โ†“ Computes: delta = current_controller - initial_controller - โ†“ Routes through connector.transform_delta() - โ†“ Publishes: robot commands (PoseStamped for arms, Twist for quadrupeds) - โ†“ - Robot Driver (any manipulator/quadruped driver) - -Usage with connectors: - from dimos.teleop.connectors import ArmConnector, ArmConnectorConfig - from dimos.teleop.devices.vr_teleop_module import vr_teleop_module - from dimos.core.blueprints import autoconnect - - # Create connectors for each input - left_arm = ArmConnector(ArmConnectorConfig(dummy_driver=True)) - right_arm = ArmConnector(ArmConnectorConfig(dummy_driver=True)) - - my_system = autoconnect( - vr_teleop_module( - num_inputs=2, - enable_inputs=[True, True], - input_labels=["left_vr", "right_vr"], - connectors=[left_arm, right_arm], - ), - ) -""" +"""Pre-built blueprints for VR teleoperation.""" from dimos_lcm.geometry_msgs import Transform as LCMTransform from dimos.core.blueprints import autoconnect from dimos.core.transport import LCMTransport +from dimos.msgs.geometry_msgs import PoseStamped, Twist from dimos.msgs.std_msgs import Bool, Float32 -from dimos.teleop.connectors import ( - ArmConnector, - ArmConnectorConfig, - QuadrupedConnector, - QuadrupedConnectorConfig, -) from dimos.teleop.devices.vr_teleop_module import vr_teleop_module -# ============================================================================= -# Quest3 Teleoperation Blueprint (with connectors) -# ============================================================================= - -# Create connectors: left controller โ†’ arm (index 0), right controller โ†’ quadruped (index 2) -# Index 0: PoseStamped output (controller_delta_0) -# Index 1: disabled -# Index 2: Twist output (controller_delta_2) -_left_arm_connector = ArmConnector( - ArmConnectorConfig( - driver_module_name="LeftArmDriver", - dummy_driver=True, - ) -) -_right_quadruped_connector = QuadrupedConnector( - QuadrupedConnectorConfig( - driver_module_name="Go2Driver", - dummy_driver=True, - ) -) - quest3_teleop = autoconnect( vr_teleop_module( - num_inputs=3, - enable_inputs=[True, False, True], - input_labels=["left_vr", "unused", "right_vr"], - connectors=[_left_arm_connector, None, _right_quadruped_connector], + output_types=[PoseStamped, Twist], + input_labels=["left_vr", "right_vr"], visualize_in_rerun=True, - safety_limits=True, ), ).transports( { - # VRTeleopModule inputs (from external LCM - Deno bridge) ("vr_left_transform", LCMTransform): LCMTransport("/vr_left_transform", LCMTransform), ("vr_right_transform", LCMTransform): LCMTransport("/vr_right_transform", LCMTransform), ("vr_trigger_0", Float32): LCMTransport("/vr_trigger_0", Float32), ("vr_trigger_1", Float32): LCMTransport("/vr_trigger_1", Float32), ("teleop_enable", Bool): LCMTransport("/vr_teleop_enable", Bool), - # VRTeleopModule outputs - add transports here when connecting to robot drivers - # ("controller_delta_0", PoseStamped): LCMTransport("/left_arm_command", PoseStamped), - # ("controller_delta_1", PoseStamped): LCMTransport("/right_arm_command", PoseStamped), - # ("trigger_value_0", Bool): LCMTransport("/left_gripper_command", Bool), - # ("trigger_value_1", Bool): LCMTransport("/right_gripper_command", Bool), } ) From 872116ad7e9fed205d16ed722c50d56f1dab36f4 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 14 Jan 2026 17:26:12 -0800 Subject: [PATCH 51/72] Feat: utils for transformations + optional rerun viz --- dimos/utils/teleop_transforms.py | 233 ++++++++++++++++++++++++++++ dimos/utils/teleop_visualization.py | 97 ++++++++++++ 2 files changed, 330 insertions(+) create mode 100644 dimos/utils/teleop_transforms.py create mode 100644 dimos/utils/teleop_visualization.py diff --git a/dimos/utils/teleop_transforms.py b/dimos/utils/teleop_transforms.py new file mode 100644 index 0000000000..71f4fb98de --- /dev/null +++ b/dimos/utils/teleop_transforms.py @@ -0,0 +1,233 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Teleop transform utilities for converting delta poses to robot commands.""" + +from __future__ import annotations + +import time +from typing import TYPE_CHECKING + +import numpy as np +from scipy.spatial.transform import Rotation as R + +from dimos.msgs.geometry_msgs import Pose, PoseStamped, Twist, Vector3 +from dimos.msgs.std_msgs import Bool +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from dimos_lcm.geometry_msgs import Transform as LCMTransform + from numpy.typing import NDArray + +logger = setup_logger() + +# Index mapping: output type โ†’ available indices +OUTPUT_TYPE_INDICES: dict[type, list[int]] = { + PoseStamped: [0, 1], + Twist: [2, 3], +} + +# Coordinate frame transformation from VR (WebXR) to robot frame +# WebXR: X=right, Y=up, Z=back (towards user) +# Robot: X=forward, Y=left, Z=up +VR_TO_ROBOT_FRAME: NDArray[np.float64] = np.array( + [ + [0, 0, -1, 0], # Robot X = -VR Z (forward) + [-1, 0, 0, 0], # Robot Y = -VR X (left) + [0, 1, 0, 0], # Robot Z = +VR Y (up) + [0, 0, 0, 1], + ], + dtype=np.float64, +) + + +def compute_active_indices(output_types: list[type]) -> list[int]: + """Compute active indices from output types. + Example: + [PoseStamped, Twist] โ†’ [0, 2] + [Twist, PoseStamped] โ†’ [2, 0] + [PoseStamped, PoseStamped] โ†’ [0, 1] + [Twist, Twist] โ†’ [2, 3] + """ + indices: list[int] = [] + used_indices: set[int] = set() + + for output_type in output_types: + available = OUTPUT_TYPE_INDICES.get(output_type, []) + for idx in available: + if idx not in used_indices: + indices.append(idx) + used_indices.add(idx) + break + else: + raise ValueError(f"No available index for output type {output_type.__name__}") + + return indices + + +def transform_vr_to_robot( + transform: LCMTransform, + is_left_controller: bool = True, +) -> NDArray[np.float64]: + """Transform VR controller pose to robot coordinate frame. + + Args: + transform: LCM Transform from VR controller. + is_left_controller: True for left controller (+90ยฐ Z rotation), + False for right controller (-90ยฐ Z rotation). + + Returns: + 4x4 transformation matrix in robot frame. + """ + # Convert LCM Transform to 4x4 matrix + t = transform.translation + q = transform.rotation + rot_matrix = R.from_quat([q.x, q.y, q.z, q.w]).as_matrix() + + vr_matrix = np.eye(4, dtype=np.float64) + vr_matrix[:3, :3] = rot_matrix + vr_matrix[:3, 3] = [t.x, t.y, t.z] + + # Apply controller alignment rotation + # Left controller rotates +90ยฐ around Z, right rotates -90ยฐ + direction = 1 if is_left_controller else -1 + z_rotation = R.from_euler("z", 90 * direction, degrees=True).as_matrix() + vr_matrix[:3, :3] = vr_matrix[:3, :3] @ z_rotation + + # Apply VR to robot frame transformation + return VR_TO_ROBOT_FRAME @ vr_matrix + + +def transform_delta( + delta_pose: Pose, + trigger_value: float, + output_type: type, + initial_robot_pose: Pose | None = None, + linear_scale: float = 1.0, + angular_scale: float = 1.0, + max_linear_velocity: float = 0.5, + max_angular_velocity: float = 1.0, + gripper_threshold: float = 0.5, +) -> tuple[PoseStamped | Twist | None, Bool | None]: + """Transform delta pose to robot command based on output type. + + Args: + delta_pose: Delta from teleop device (current - initial). + trigger_value: Trigger value (0.0-1.0). + output_type: PoseStamped or Twist. + initial_robot_pose: Initial robot pose (required for PoseStamped). + linear_scale: Scale factor for linear velocity (Twist only). + angular_scale: Scale factor for angular velocity (Twist only). + max_linear_velocity: Max linear velocity clamp (Twist only). + max_angular_velocity: Max angular velocity clamp (Twist only). + gripper_threshold: Trigger threshold for gripper open/close. + + Returns: + Tuple of (command, trigger_bool). + """ + trigger_bool = Bool(data=trigger_value > gripper_threshold) + + if output_type == PoseStamped: + return _to_pose_stamped(delta_pose, initial_robot_pose), trigger_bool + elif output_type == Twist: + return _to_twist( + delta_pose, linear_scale, angular_scale, max_linear_velocity, max_angular_velocity + ), trigger_bool + else: + logger.warning(f"Unknown output type: {output_type}") + return None, trigger_bool + + +def _to_pose_stamped(delta_pose: Pose, initial_robot_pose: Pose | None) -> PoseStamped: + """Convert delta pose to PoseStamped (target = initial + delta).""" + if initial_robot_pose is not None: + target_pose = initial_robot_pose + delta_pose + else: + target_pose = delta_pose + return PoseStamped( + ts=time.time(), + frame_id="teleop_target", + position=target_pose.position, + orientation=target_pose.orientation, + ) + + +def _to_twist( + delta_pose: Pose, + linear_scale: float, + angular_scale: float, + max_linear: float, + max_angular: float, +) -> Twist: + """Convert delta pose to Twist velocity command.""" + + def clamp(value: float, limit: float) -> float: + return max(-limit, min(limit, value)) + + linear = Vector3( + clamp(delta_pose.position.x * linear_scale, max_linear), + clamp(delta_pose.position.y * linear_scale, max_linear), + clamp(delta_pose.position.z * linear_scale, max_linear), + ) + euler = delta_pose.orientation.to_euler() + angular = Vector3( + clamp(euler.x * angular_scale, max_angular), + clamp(euler.y * angular_scale, max_angular), + clamp(euler.z * angular_scale, max_angular), + ) + return Twist(linear=linear, angular=angular) + + +def parse_pose_from_dict(output_type: type, data: dict | None = None) -> Pose | None: + """Parse a Pose from a dictionary based on output type. + + Args: + output_type: PoseStamped or Twist. + data: Dict with position and orientation keys. + {"position": {"x": float, "y": float, "z": float}, + "orientation": {"x": float, "y": float, "z": float, "w": float}} + + Returns: + Parsed Pose for PoseStamped, None for Twist. + """ + if output_type == Twist: + return None + + if output_type == PoseStamped: + if data is None: + return Pose() + + from dimos.msgs.geometry_msgs import Quaternion + + pos = data.get("position", {}) + ori = data.get("orientation", {}) + + pose = Pose( + position=Vector3( + pos.get("x", 0.0), + pos.get("y", 0.0), + pos.get("z", 0.0), + ), + ) + if ori: + pose.orientation = Quaternion( + ori.get("x", 0.0), + ori.get("y", 0.0), + ori.get("z", 0.0), + ori.get("w", 1.0), + ) + return pose + + return None diff --git a/dimos/utils/teleop_visualization.py b/dimos/utils/teleop_visualization.py new file mode 100644 index 0000000000..08521bfbf8 --- /dev/null +++ b/dimos/utils/teleop_visualization.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Teleop visualization utilities for Rerun.""" + +from __future__ import annotations + +import time + +from dimos.msgs.geometry_msgs import Pose, PoseStamped +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +try: + import rerun as rr + + from dimos.dashboard.rerun_init import connect_rerun + + RERUN_AVAILABLE = True +except ImportError: + RERUN_AVAILABLE = False + + +def init_rerun_visualization() -> bool: + """Initialize Rerun visualization connection.""" + if not RERUN_AVAILABLE: + return False + + try: + connect_rerun() + logger.info("Connected to Rerun for teleop visualization") + return True + except Exception as e: + logger.warning(f"Failed to connect to Rerun: {e}") + return False + + +def visualize_controller_pose(controller_pose: Pose, controller_label: str) -> None: + """Visualize controller absolute pose in Rerun. + + Args: + controller_pose: The controller's current pose. + controller_label: Label for the controller (e.g., "left_vr"). + """ + if not RERUN_AVAILABLE: + return + + try: + controller_pose_stamped = PoseStamped( + ts=time.time(), + frame_id=f"world/teleop/{controller_label}_controller", + position=controller_pose.position, + orientation=controller_pose.orientation, + ) + rr.log( + f"world/teleop/{controller_label}_controller", + controller_pose_stamped.to_rerun(), # type: ignore[no-untyped-call] + ) + # Log 3D axes to visualize controller orientation (X=red, Y=green, Z=blue) + rr.log( + f"world/teleop/{controller_label}_controller/axes", + rr.TransformAxes3D(0.15), # type: ignore[attr-defined] + ) + except Exception as e: + logger.debug(f"Failed to log {controller_label} controller to Rerun: {e}") + + +def visualize_trigger_value(trigger_value: float, controller_label: str) -> None: + """Visualize trigger value in Rerun as a scalar time series. + + Args: + trigger_value: Trigger value (0.0-1.0). + controller_label: Label for the controller (e.g., "left_vr"). + """ + if not RERUN_AVAILABLE: + return + + try: + rr.log( + f"world/teleop/{controller_label}_controller/trigger", + rr.Scalars(trigger_value), # type: ignore[attr-defined] + ) + except Exception as e: + logger.debug(f"Failed to log {controller_label} trigger to Rerun: {e}") From 0364ace7c01d799db0d749d3991a7300dc9b5d65 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 14 Jan 2026 17:26:40 -0800 Subject: [PATCH 52/72] docs: updated - new arch --- dimos/teleop/README.md | 71 +++++++++++++++------------------- dimos/teleop/devices/README.md | 62 ++++++++++++++++++++++------- dimos/teleop/web/README.md | 22 ++++++++--- 3 files changed, 95 insertions(+), 60 deletions(-) diff --git a/dimos/teleop/README.md b/dimos/teleop/README.md index f0de1fa854..543c3207d1 100644 --- a/dimos/teleop/README.md +++ b/dimos/teleop/README.md @@ -1,4 +1,4 @@ -# Teleoperation [TO BE UPDATED] +# Teleoperation VR teleoperation system for controlling robots with Meta Quest controllers. @@ -8,12 +8,8 @@ VR teleoperation system for controlling robots with Meta Quest controllers. teleop/ โ”œโ”€โ”€ __init__.py # Exports quest3_teleop blueprint โ”œโ”€โ”€ teleop_blueprints.py # Pre-built system configurations -โ”œโ”€โ”€ connectors/ # Robot command transformers -โ”‚ โ”œโ”€โ”€ base_connector.py # BaseTeleopConnector (abstract) -โ”‚ โ”œโ”€โ”€ arm_connector.py # ArmConnector โ†’ PoseStamped -โ”‚ โ””โ”€โ”€ quadruped_connector.py # QuadrupedConnector โ†’ Twist โ”œโ”€โ”€ devices/ # Teleop input modules -โ”‚ โ”œโ”€โ”€ base_teleop_module.py # BaseTeleopModule (calibration, deltas) +โ”‚ โ”œโ”€โ”€ base_teleop_module.py # BaseTeleopModule (calibration, deltas, transforms) โ”‚ โ””โ”€โ”€ vr_teleop_module.py # VRTeleopModule (LCM inputs) โ””โ”€โ”€ web/ # Quest WebXR interface โ”œโ”€โ”€ teleop_server.ts # Deno WebSocket-LCM bridge @@ -41,22 +37,16 @@ teleop/ โ”Œโ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ” โ”‚ VRTeleopModule โ”‚ โ”‚ โ”Œโ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ” โ”Œโ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ” โ”Œโ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ” โ”‚ -โ”‚ โ”‚ Calibrate โ”‚ โ†’ โ”‚ Compute Deltaโ”‚ โ†’ โ”‚ Route to โ”‚ โ”‚ -โ”‚ โ”‚ (X button) โ”‚ โ”‚ curr - init โ”‚ โ”‚ Connectors โ”‚ โ”‚ +โ”‚ โ”‚ Calibrate โ”‚ โ†’ โ”‚ Compute Deltaโ”‚ โ†’ โ”‚ Transform โ”‚ โ”‚ +โ”‚ โ”‚ (X button) โ”‚ โ”‚ curr - init โ”‚ โ”‚ & Publish โ”‚ โ”‚ โ”‚ โ””โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”˜ โ””โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”˜ โ””โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”˜ โ”‚ โ””โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”‚โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”˜ โ”‚ โ”Œโ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”ดโ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ” โ–ผ โ–ผ - โ”Œโ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ” โ”Œโ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ” - โ”‚ ArmConnector โ”‚ โ”‚QuadrupedConnector โ”‚ - โ”‚ delta โ†’ PoseStamped โ”‚ delta โ†’ Twist โ”‚ - โ”‚ target = init + ฮ” โ”‚ โ”‚ vel = scale(ฮ”) โ”‚ - โ””โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”ฌโ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”˜ โ””โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”ฌโ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”˜ - โ”‚ โ”‚ - โ–ผ โ–ผ controller_delta_0/1 controller_delta_2/3 (PoseStamped) (Twist) + target = init + ฮ” vel = scale(ฮ”) ``` ## Quick Start @@ -70,53 +60,54 @@ coordinator.loop() Then on Quest 3: Open `https://:8443`, press X to calibrate and start. -## Modular Connector System +## Output Types Configuration -Each VR controller can be assigned any connector type. The connector determines how deltas are transformed into robot commands. +The `output_types` parameter determines how each controller input is mapped: -| Connector | Output | Use Case | -|-----------|--------|----------| -| `ArmConnector` | PoseStamped | Manipulators, end-effector control | -| `QuadrupedConnector` | Twist | Locomotion, velocity control | +| Output Type | Indices | Use Case | +|-------------|---------|----------| +| `PoseStamped` | 0, 1 | Manipulators, end-effector control | +| `Twist` | 2, 3 | Locomotion, velocity control | -Output indices determine the message type: -- Index 0, 1 โ†’ PoseStamped -- Index 2, 3 โ†’ Twist +The system auto-computes active indices from output types: +- `[PoseStamped, PoseStamped]` โ†’ indices `[0, 1]` (dual arm) +- `[Twist, Twist]` โ†’ indices `[2, 3]` (dual locomotion) +- `[PoseStamped, Twist]` โ†’ indices `[0, 2]` (arm + quadruped) ## Custom Setup ```python -from dimos.teleop.connectors import ArmConnector, ArmConnectorConfig, QuadrupedConnector, QuadrupedConnectorConfig from dimos.teleop.devices import vr_teleop_module +from dimos.msgs.geometry_msgs import PoseStamped, Twist from dimos.core.blueprints import autoconnect -# Two arms -left = ArmConnector(ArmConnectorConfig(driver_module_name="LeftArm")) -right = ArmConnector(ArmConnectorConfig(driver_module_name="RightArm")) - +# Dual arm setup (both controllers โ†’ PoseStamped) dual_arm = autoconnect( vr_teleop_module( - num_inputs=2, - enable_inputs=[True, True], - connectors=[left, right], + output_types=[PoseStamped, PoseStamped], + input_labels=["left_arm", "right_arm"], ), ) -# Or: arm + quadruped (use index 0 and 2) -arm = ArmConnector(ArmConnectorConfig(driver_module_name="Arm")) -quad = QuadrupedConnector(QuadrupedConnectorConfig(driver_module_name="Go2")) - +# Arm + Quadruped (left โ†’ PoseStamped index 0, right โ†’ Twist index 2) arm_and_quad = autoconnect( vr_teleop_module( - num_inputs=3, - enable_inputs=[True, False, True], # index 0 and 2 enabled - connectors=[arm, None, quad], + output_types=[PoseStamped, Twist], + input_labels=["left_arm", "right_quad"], + ), +) + +# With RPC for initial robot pose (arm calibration) +arm_with_rpc = autoconnect( + vr_teleop_module( + output_types=[PoseStamped, PoseStamped], + input_labels=["left_arm", "right_arm"], + robot_pose_rpc_methods=["LeftArm.get_ee_pose", "RightArm.get_ee_pose"], ), ) ``` ## Submodules -- [connectors/](connectors/) - Robot command transformers - [devices/](devices/) - Teleop input modules - [web/](web/) - Quest WebXR client and Deno server diff --git a/dimos/teleop/devices/README.md b/dimos/teleop/devices/README.md index 564220e50a..2049dca040 100644 --- a/dimos/teleop/devices/README.md +++ b/dimos/teleop/devices/README.md @@ -1,15 +1,17 @@ -# Teleop Devices [TO BE UPDATED] +# Teleop Devices -Input device modules that capture tracking data and compute deltas for teleoperation. +Input device modules that capture tracking data, compute deltas, and transform to robot commands. ## Modules ### BaseTeleopModule Abstract base class providing: -- Calibration (capture initial poses on X button) +- Multi-controller calibration (capture initial poses) - Delta computation (current - initial) +- Transform to robot commands (PoseStamped or Twist based on `output_types`) - Publishing to `controller_delta_*` outputs +- Optional RPC calls for initial robot pose - Rerun visualization ### VRTeleopModule @@ -17,7 +19,7 @@ Abstract base class providing: VR-specific implementation that: - Subscribes to LCM Transform messages from Deno bridge - Applies coordinate frame transformation (WebXR โ†’ Robot) -- Routes deltas through connectors +- Transforms deltas to robot commands based on output type - Runs control loop at 50Hz ## Data Flow @@ -33,40 +35,70 @@ LCM Input (Transform) โ”‚ โ–ผ โ”Œโ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ” -โ”‚ Calibration โ”‚ On X button: capture initial pose +โ”‚ Calibration โ”‚ On X button: capture initial controller pose +โ”‚ โ”‚ (optional) RPC call for initial robot pose โ””โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”˜ โ”‚ โ–ผ โ”Œโ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ” -โ”‚ Delta Compute โ”‚ delta = current - initial +โ”‚ Delta Compute โ”‚ delta = current_controller - initial_controller โ””โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”˜ โ”‚ โ–ผ โ”Œโ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ” -โ”‚ Connector โ”‚ Transform to robot command +โ”‚ Transform โ”‚ PoseStamped: target = initial_robot + delta +โ”‚ โ”‚ Twist: velocity = scale(delta) โ””โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”˜ โ”‚ โ–ผ - LCM Output + LCM Output (controller_delta_0/1/2/3) ``` ## Configuration ```python from dimos.teleop.devices import VRTeleopConfig +from dimos.msgs.geometry_msgs import PoseStamped, Twist config = VRTeleopConfig( - num_inputs=2, # Number of controllers - enable_inputs=[True, True], # Which inputs to process - input_labels=["left", "right"], # Labels for logging/viz - visualize_in_rerun=True, # Enable Rerun visualization - control_loop_hz=50.0, # Control loop frequency + # Output types determine active indices: + # PoseStamped โ†’ indices 0,1 | Twist โ†’ indices 2,3 + output_types=[PoseStamped, Twist], + input_labels=["left_vr", "right_vr"], + + # Optional RPC methods for initial robot pose (per output) + robot_pose_rpc_methods=["ArmDriver.get_ee_pose", None], + + # Visualization + visualize_in_rerun=True, + + # Control + control_loop_hz=50.0, + + # Transform settings + linear_scale=1.0, + angular_scale=1.0, + max_linear_velocity=0.5, + max_angular_velocity=1.0, + gripper_threshold=0.5, ) ``` +## Output Indices + +The `output_types` parameter auto-computes active indices: + +| Configuration | Active Indices | Outputs | +|--------------|----------------|---------| +| `[PoseStamped, PoseStamped]` | `[0, 1]` | Dual arm | +| `[Twist, Twist]` | `[2, 3]` | Dual locomotion | +| `[PoseStamped, Twist]` | `[0, 2]` | Arm + quadruped | + ## Adding New Devices 1. Inherit from `BaseTeleopModule` 2. Override `start()` to set up input subscriptions -3. Call `_on_lcm_transform(index, pose_matrix)` when new data arrives -4. Use `compute_deltas()` and `publish_command()` from base class +3. Transform input to 4x4 pose matrix in robot frame +4. Call `compute_deltas(poses, trigger_values)` to get delta poses +5. Use `transform_delta()` from utils to convert to command +6. Call `publish_command(index, command, aux_command)` to publish diff --git a/dimos/teleop/web/README.md b/dimos/teleop/web/README.md index 15baa15b75..34defc8dee 100644 --- a/dimos/teleop/web/README.md +++ b/dimos/teleop/web/README.md @@ -1,4 +1,4 @@ -# Teleop Web [TO BE UPDATED] +# Teleop Web WebXR client and server for Quest 3 VR teleoperation. @@ -16,7 +16,7 @@ Deno server that bridges WebSocket and LCM: WebXR client running on Quest 3: - Captures controller poses at 72Hz - Sends Transform messages via WebSocket -- X button triggers calibration +- X button triggers calibration (teleop_enable) ## Running @@ -41,9 +41,21 @@ openssl req -x509 -newkey rsa:2048 -keyout certs/key.pem -out certs/cert.pem -da ``` Quest Browser Deno Server Python โ”‚ โ”‚ โ”‚ - โ”‚โ”€โ”€ Transform (pose) โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ†’ โ”‚โ”€โ”€ LCM publish โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ†’ โ”‚ - โ”‚โ”€โ”€ Float32 (trigger) โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ†’ โ”‚โ”€โ”€ LCM publish โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ†’ โ”‚ - โ”‚โ”€โ”€ Bool (X button) โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ†’ โ”‚โ”€โ”€ LCM publish โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ†’ โ”‚ + โ”‚โ”€โ”€ Transform (left pose) โ”€โ”€โ”€โ”€โ”€โ†’ โ”‚โ”€โ”€ /vr_left_transform โ”€โ”€โ”€โ”€โ”€โ†’ โ”‚ + โ”‚โ”€โ”€ Transform (right pose) โ”€โ”€โ”€โ”€โ†’ โ”‚โ”€โ”€ /vr_right_transform โ”€โ”€โ”€โ”€โ†’ โ”‚ + โ”‚โ”€โ”€ Float32 (left trigger) โ”€โ”€โ”€โ”€โ†’ โ”‚โ”€โ”€ /vr_trigger_0 โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ†’ โ”‚ + โ”‚โ”€โ”€ Float32 (right trigger) โ”€โ”€โ”€โ†’ โ”‚โ”€โ”€ /vr_trigger_1 โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ†’ โ”‚ + โ”‚โ”€โ”€ Bool (X button) โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ†’ โ”‚โ”€โ”€ /vr_teleop_enable โ”€โ”€โ”€โ”€โ”€โ”€โ†’ โ”‚ โ”‚ โ”‚ โ”‚ โ”‚โ†โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€ LCM packets (subscribePacket) โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”‚ ``` + +## LCM Topics + +| Topic | Type | Description | +|-------|------|-------------| +| `/vr_left_transform` | Transform | Left controller pose (WebXR frame) | +| `/vr_right_transform` | Transform | Right controller pose (WebXR frame) | +| `/vr_trigger_0` | Float32 | Left trigger value (0.0-1.0) | +| `/vr_trigger_1` | Float32 | Right trigger value (0.0-1.0) | +| `/vr_teleop_enable` | Bool | Calibration toggle (X button) | From 2c6f5e66bb3c55b3fbd47a884d3fb26db27bfaa5 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 14 Jan 2026 17:29:00 -0800 Subject: [PATCH 53/72] Misc: change of file location, required cert for ws --- dimos/teleop/connectors/README.md | 70 --------- dimos/teleop/connectors/__init__.py | 48 ------ dimos/teleop/connectors/arm_connector.py | 128 ---------------- dimos/teleop/connectors/base_connector.py | 96 ------------ .../teleop/connectors/quadruped_connector.py | 138 ------------------ dimos/teleop/devices/__init__.py | 1 + dimos/teleop/web/teleop_server.ts | 35 ++++- 7 files changed, 34 insertions(+), 482 deletions(-) delete mode 100644 dimos/teleop/connectors/README.md delete mode 100644 dimos/teleop/connectors/__init__.py delete mode 100644 dimos/teleop/connectors/arm_connector.py delete mode 100644 dimos/teleop/connectors/base_connector.py delete mode 100644 dimos/teleop/connectors/quadruped_connector.py diff --git a/dimos/teleop/connectors/README.md b/dimos/teleop/connectors/README.md deleted file mode 100644 index 64790cc810..0000000000 --- a/dimos/teleop/connectors/README.md +++ /dev/null @@ -1,70 +0,0 @@ -# Teleop Connectors [TO BE UPDATED] - -Connectors transform delta poses from teleop devices into robot-specific commands. - -## Available Connectors - -### ArmConnector - -For manipulators and end-effector control. - -- **Input**: Delta pose (Pose) -- **Output**: PoseStamped (target = initial_robot_pose + delta) -- **Aux Output**: Bool (gripper state from trigger) - -```python -from dimos.teleop.connectors import ArmConnector, ArmConnectorConfig - -connector = ArmConnector(ArmConnectorConfig( - driver_module_name="MyArmDriver", # For RPC calibration - dummy_driver=True, # Skip RPC, use zeros - gripper_threshold=0.5, # Trigger threshold -)) -``` - -### QuadrupedConnector - -For quadrupeds and mobile bases. - -- **Input**: Delta pose (Pose) -- **Output**: Twist (velocity = scale(delta)) -- **Aux Output**: Bool (trigger state) - -```python -from dimos.teleop.connectors import QuadrupedConnector, QuadrupedConnectorConfig - -connector = QuadrupedConnector(QuadrupedConnectorConfig( - linear_scale=1.0, # Position delta โ†’ linear velocity - angular_scale=1.0, # Orientation delta โ†’ angular velocity - max_linear_velocity=0.5, # m/s clamp - max_angular_velocity=1.0, # rad/s clamp -)) -``` - -## Creating Custom Connectors - -```python -from dimos.teleop.connectors import BaseTeleopConnector, ConnectorConfig -from dimos.msgs.geometry_msgs import Pose - -class MyConnector(BaseTeleopConnector): - def set_initial_pos(self, rpc_result=None) -> bool: - # Initialize robot state (optional RPC) - self._has_initial_pos = True - return True - - def transform_delta(self, delta_pose: Pose, trigger_value: float): - # Transform delta to your command type - command = ... - aux_command = ... - return command, aux_command -``` - -## Output Index Mapping - -Connectors must be assigned to indices matching their output type: - -| Index | Output Type | Connector | -|-------|-------------|-----------| -| 0, 1 | PoseStamped | ArmConnector | -| 2, 3 | Twist | QuadrupedConnector | diff --git a/dimos/teleop/connectors/__init__.py b/dimos/teleop/connectors/__init__.py deleted file mode 100644 index 50ba811a23..0000000000 --- a/dimos/teleop/connectors/__init__.py +++ /dev/null @@ -1,48 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Teleop Connectors Package. - -Connectors are injectable classes that transform delta poses into robot commands. -They are injected into VRTeleopModule (or other teleop modules) to handle per-input control logic. - -Example usage: - from dimos.teleop.connectors import ArmConnector, ArmConnectorConfig - - connector = ArmConnector(ArmConnectorConfig( - driver_module_name="MyArmDriver", - dummy_driver=True, - )) - - vr_module = VRTeleopModule(config=VRTeleopConfig( - connectors=[connector, connector], # One per input - )) -""" - -from dimos.teleop.connectors.arm_connector import ArmConnector, ArmConnectorConfig -from dimos.teleop.connectors.base_connector import BaseTeleopConnector, ConnectorConfig -from dimos.teleop.connectors.quadruped_connector import ( - QuadrupedConnector, - QuadrupedConnectorConfig, -) - -__all__ = [ - "ArmConnector", - "ArmConnectorConfig", - "BaseTeleopConnector", - "ConnectorConfig", - "QuadrupedConnector", - "QuadrupedConnectorConfig", -] diff --git a/dimos/teleop/connectors/arm_connector.py b/dimos/teleop/connectors/arm_connector.py deleted file mode 100644 index 8cea6576cc..0000000000 --- a/dimos/teleop/connectors/arm_connector.py +++ /dev/null @@ -1,128 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -""" -Arm Teleop Connector - -Connector for robotic arms that transforms delta poses into PoseStamped commands. -""" - -from __future__ import annotations - -from dataclasses import dataclass - -from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Vector3 -from dimos.msgs.std_msgs import Bool -from dimos.teleop.connectors.base_connector import BaseTeleopConnector, ConnectorConfig -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -@dataclass -class ArmConnectorConfig(ConnectorConfig): - """Configuration for arm connector.""" - - driver_module_name: str = "ArmDriver" - driver_method_name: str = "get_state" - dummy_driver: bool = False - gripper_threshold: float = 0.5 # Trigger value above this activates gripper - - -class ArmConnector(BaseTeleopConnector): - """Connector for robotic arms. - - Transforms delta poses into PoseStamped commands for arm drivers. - Auto-calibrates on first delta by getting robot's initial end-effector pose via RPC. - - Output: PoseStamped (use with controller_delta_0 or controller_delta_1) - """ - - config: ArmConnectorConfig - - def __init__(self, config: ArmConnectorConfig | None = None) -> None: - super().__init__(config or ArmConnectorConfig()) - self.config: ArmConnectorConfig = config or ArmConnectorConfig() - - def set_initial_pos(self, rpc_result: dict | None = None) -> bool: - """Set initial robot pose from RPC result. - - Args: - rpc_result: RPC result dict with 'success' and 'pose' keys. If None, uses zeros. - - Returns: - True if successful, False otherwise. - """ - if rpc_result is None or not rpc_result.get("success"): - logger.info("Using zeros for initial pose") - self._initial_robot_pose = Pose( - position=Vector3(0.0, 0.0, 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, 0.0)), - ) - else: - pose_data = rpc_result.get("pose", {}) - position = Vector3( - pose_data.get("x", 0.0), - pose_data.get("y", 0.0), - pose_data.get("z", 0.0), - ) - rpy = Vector3( - pose_data.get("roll", 0.0), - pose_data.get("pitch", 0.0), - pose_data.get("yaw", 0.0), - ) - self._initial_robot_pose = Pose( - position=position, - orientation=Quaternion.from_euler(rpy), - ) - logger.info(f"Arm connector initial position set: {pose_data}") - - self._has_initial_pos = True - return True - - def transform_delta( - self, - delta_pose: Pose, - trigger_value: float, - ) -> tuple[PoseStamped | None, Bool | None]: - """Transform delta pose to target arm pose. - - Computes: target_pose = initial_robot_pose + delta - - Args: - delta_pose: Delta from teleop device (current - initial). - trigger_value: Trigger/gripper value (0.0-1.0). - - Returns: - Tuple of (PoseStamped command, Bool gripper command). - """ - if not self._has_initial_pos: - if not self.set_initial_pos(): - return None, None - - if self._initial_robot_pose is None: - return None, None - - target_pose = self._initial_robot_pose + delta_pose - - command = PoseStamped( - position=target_pose.position, - orientation=target_pose.orientation, - frame_id="arm_command", - ) - - gripper_command = Bool(data=trigger_value > self.config.gripper_threshold) - - return command, gripper_command diff --git a/dimos/teleop/connectors/base_connector.py b/dimos/teleop/connectors/base_connector.py deleted file mode 100644 index 989c69b727..0000000000 --- a/dimos/teleop/connectors/base_connector.py +++ /dev/null @@ -1,96 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -""" -Base Teleop Connector - -Abstract base class for injectable teleop connectors. -Connectors transform delta poses into robot commands. -""" - -from __future__ import annotations - -from abc import ABC, abstractmethod -from dataclasses import dataclass -from typing import TYPE_CHECKING, Any - -if TYPE_CHECKING: - from dimos.msgs.geometry_msgs import Pose - - -@dataclass -class ConnectorConfig: - """Base configuration for teleop connectors.""" - - driver_module_name: str = "driver_name" - driver_method_name: str = "get_state" - dummy_driver: bool = False - - -class BaseTeleopConnector(ABC): - """Abstract base class for teleop connectors. - - Connectors are injectable classes (not Modules) that: - 1. Receive delta poses from the teleop module - 2. Transform deltas into robot commands - 3. Optionally make RPC calls to robot drivers - 4. Return commands to be published by the teleop module - """ - - config: ConnectorConfig - - def __init__(self, config: ConnectorConfig | None = None) -> None: - self.config = config or ConnectorConfig() - self._has_initial_pos = False - self._initial_robot_pose: Pose | None = None - - @abstractmethod - def set_initial_pos(self, rpc_result: dict | None = None) -> bool: - """Set initial robot pose for the connector. - - Args: - rpc_result: RPC result from robot driver (optional, some connectors don't need it). - - Returns: - True if successful, False otherwise. - """ - pass - - @abstractmethod - def transform_delta( - self, - delta_pose: Pose, - trigger_value: float, - ) -> tuple[Any, Any]: - """Transform a delta pose into a robot command. - - Args: - delta_pose: The delta pose (current - initial) from teleop device. - trigger_value: Trigger/gripper value (0.0-1.0). - - Returns: - Tuple of (command to publish, optional gripper/aux command). - """ - pass - - @property - def has_initial_pos(self) -> bool: - """Check if connector has initial position.""" - return self._has_initial_pos - - def reset_initial_pos(self) -> None: - """Reset connector initial position.""" - self._has_initial_pos = False - self._initial_robot_pose = None diff --git a/dimos/teleop/connectors/quadruped_connector.py b/dimos/teleop/connectors/quadruped_connector.py deleted file mode 100644 index 1bf5934fae..0000000000 --- a/dimos/teleop/connectors/quadruped_connector.py +++ /dev/null @@ -1,138 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -""" -Quadruped Teleop Connector - -Connector for quadruped robots that transforms delta poses into Twist velocity commands. -""" - -from __future__ import annotations - -from dataclasses import dataclass - -from dimos.msgs.geometry_msgs import Pose, Twist, Vector3 -from dimos.msgs.std_msgs import Bool -from dimos.teleop.connectors.base_connector import BaseTeleopConnector, ConnectorConfig -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -@dataclass -class QuadrupedConnectorConfig(ConnectorConfig): - """Configuration for quadruped connector.""" - - driver_module_name: str = "QuadrupedDriver" - driver_method_name: str = "get_state" - dummy_driver: bool = False - - linear_scale: float = 1.0 # Scale factor for linear velocity - angular_scale: float = 1.0 # Scale factor for angular velocity - max_linear_velocity: float = 0.5 # m/s - max_angular_velocity: float = 1.0 # rad/s - - -class QuadrupedConnector(BaseTeleopConnector): - """Connector for quadruped robots. - - Transforms delta poses into Twist commands for quadruped locomotion. - Maps position deltas to linear velocity and orientation deltas to angular velocity. - - Output: Twist (use with controller_delta_2 or controller_delta_3) - """ - - config: QuadrupedConnectorConfig - - def __init__(self, config: QuadrupedConnectorConfig | None = None) -> None: - super().__init__(config or QuadrupedConnectorConfig()) - self.config: QuadrupedConnectorConfig = config or QuadrupedConnectorConfig() - - def set_initial_pos(self, rpc_result: dict | None = None) -> bool: - """Quadruped connector doesn't need initial position from robot. - - Args: - rpc_result: Ignored for quadruped. - - Returns: - Always True. - """ - logger.info("Quadruped connector ready (no robot state needed)") - self._has_initial_pos = True - return True - - def transform_delta( - self, - delta_pose: Pose, - trigger_value: float, - ) -> tuple[Twist | None, Bool | None]: - """Transform delta pose to velocity command. - - Maps: - - delta position -> linear velocity (x, y, z) - - delta orientation (euler) -> angular velocity (roll, pitch, yaw rates) - - Args: - delta_pose: Delta from teleop device (current - initial). - trigger_value: Trigger value (0.0-1.0). - - Returns: - Tuple of (Twist command, Bool trigger command). - """ - if not self._has_initial_pos: - self.set_initial_pos() - - # Scale and clamp linear velocity from position delta - linear = Vector3( - self._clamp( - delta_pose.position.x * self.config.linear_scale, - self.config.max_linear_velocity, - ), - self._clamp( - delta_pose.position.y * self.config.linear_scale, - self.config.max_linear_velocity, - ), - self._clamp( - delta_pose.position.z * self.config.linear_scale, - self.config.max_linear_velocity, - ), - ) - - # Scale and clamp angular velocity from orientation delta - # Convert quaternion to euler for angular velocity mapping - euler = delta_pose.orientation.to_euler() - angular = Vector3( - self._clamp( - euler.x * self.config.angular_scale, - self.config.max_angular_velocity, - ), - self._clamp( - euler.y * self.config.angular_scale, - self.config.max_angular_velocity, - ), - self._clamp( - euler.z * self.config.angular_scale, - self.config.max_angular_velocity, - ), - ) - - command = Twist(linear=linear, angular=angular) - trigger_command = Bool(data=trigger_value > 0.5) - - return command, trigger_command - - def _clamp(self, value: float, limit: float) -> float: - """Clamp value to [-limit, limit].""" - return max(-limit, min(limit, value)) diff --git a/dimos/teleop/devices/__init__.py b/dimos/teleop/devices/__init__.py index f868847664..c89dea46d5 100644 --- a/dimos/teleop/devices/__init__.py +++ b/dimos/teleop/devices/__init__.py @@ -17,6 +17,7 @@ from dimos.teleop.devices.base_teleop_module import ( BaseTeleopConfig, BaseTeleopModule, + TeleopStatusKey, ) from dimos.teleop.devices.vr_teleop_module import ( VRTeleopConfig, diff --git a/dimos/teleop/web/teleop_server.ts b/dimos/teleop/web/teleop_server.ts index e07c8c930f..d4ff6c986e 100644 --- a/dimos/teleop/web/teleop_server.ts +++ b/dimos/teleop/web/teleop_server.ts @@ -8,8 +8,39 @@ import { decodePacket, geometry_msgs } from "jsr:@dimos/msgs"; const PORT = 8443; const clients = new Set(); -const cert = await Deno.readTextFile("./certs/cert.pem"); -const key = await Deno.readTextFile("./certs/key.pem"); + +// Auto-generate self-signed certificates if they don't exist +async function ensureCerts(): Promise<{ cert: string; key: string }> { + const certPath = "./certs/cert.pem"; + const keyPath = "./certs/key.pem"; + + try { + const cert = await Deno.readTextFile(certPath); + const key = await Deno.readTextFile(keyPath); + return { cert, key }; + } catch { + console.log("Generating self-signed certificates..."); + await Deno.mkdir("./certs", { recursive: true }); + const cmd = new Deno.Command("openssl", { + args: [ + "req", "-x509", "-newkey", "rsa:2048", + "-keyout", keyPath, "-out", certPath, + "-days", "365", "-nodes", "-subj", "/CN=localhost" + ], + }); + const { code } = await cmd.output(); + if (code !== 0) { + throw new Error("Failed to generate certificates. Is openssl installed?"); + } + console.log("Certificates generated in ./certs/"); + return { + cert: await Deno.readTextFile(certPath), + key: await Deno.readTextFile(keyPath), + }; + } +} + +const { cert, key } = await ensureCerts(); Deno.serve({ port: PORT, cert, key }, async (req) => { const url = new URL(req.url); From 2b1a90e7105f8c0b99b8791c34a37c93a6579992 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 14 Jan 2026 19:36:48 -0800 Subject: [PATCH 54/72] Fix: mypy fixes --- dimos/teleop/devices/base_teleop_module.py | 11 +++----- dimos/teleop/devices/vr_teleop_module.py | 29 +++++++++++----------- dimos/utils/teleop_transforms.py | 8 +++--- 3 files changed, 24 insertions(+), 24 deletions(-) diff --git a/dimos/teleop/devices/base_teleop_module.py b/dimos/teleop/devices/base_teleop_module.py index 5271d79c24..aeffdc22c4 100644 --- a/dimos/teleop/devices/base_teleop_module.py +++ b/dimos/teleop/devices/base_teleop_module.py @@ -30,6 +30,7 @@ from dimos.core import Module, Out, rpc from dimos.core.module import ModuleConfig from dimos.msgs.geometry_msgs import Pose, PoseStamped, Twist +from dimos.msgs.std_msgs import Bool from dimos.utils.logging_config import setup_logger from dimos.utils.teleop_transforms import compute_active_indices, parse_pose_from_dict from dimos.utils.teleop_visualization import ( @@ -43,7 +44,6 @@ import numpy as np from numpy.typing import NDArray - from dimos.msgs.std_msgs import Bool logger = setup_logger() @@ -111,13 +111,10 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: ) self._active_indices = compute_active_indices(self.config.output_types) - self._output_types = { - idx: self.config.output_types[i] for i, idx in enumerate(self._active_indices) - } self._initial_poses: dict[int, Pose | None] = {i: None for i in self._active_indices} self._initial_robot_poses: dict[int, Pose | None] = {i: None for i in self._active_indices} - self._all_poses: dict[int, NDArray[np.float32] | None] = { + self._all_poses: dict[int, NDArray[np.float64] | None] = { i: None for i in self._active_indices } self._all_trigger_values: dict[int, float] = {i: 0.0 for i in self._active_indices} @@ -169,7 +166,7 @@ def calibrate(self) -> dict[str, Any]: } # Get initial robot pose via RPC if configured - output_type = self._output_types.get(idx) + output_type = self.config.output_types[i] if i < len(self.config.robot_pose_rpc_methods): rpc_method = self.config.robot_pose_rpc_methods[i] if rpc_method is not None: @@ -233,7 +230,7 @@ def get_status(self) -> dict[str, Any]: def compute_deltas( self, - controller_poses: dict[int, NDArray[np.float32] | None], + controller_poses: dict[int, NDArray[np.float64] | None], controller_trigger_values: dict[int, float], ) -> dict[int, Pose | None]: """Compute delta = current_pose - initial_pose for each controller.""" diff --git a/dimos/teleop/devices/vr_teleop_module.py b/dimos/teleop/devices/vr_teleop_module.py index cd4b2603f9..ac2d2e06eb 100644 --- a/dimos/teleop/devices/vr_teleop_module.py +++ b/dimos/teleop/devices/vr_teleop_module.py @@ -29,16 +29,16 @@ from dimos.core import In, rpc from dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.std_msgs import Bool, Float32 +from dimos_lcm.geometry_msgs import Transform as LCMTransform from dimos.teleop.devices.base_teleop_module import BaseTeleopConfig, BaseTeleopModule from dimos.utils.logging_config import setup_logger from dimos.utils.teleop_transforms import transform_delta, transform_vr_to_robot if TYPE_CHECKING: - from dimos_lcm.geometry_msgs import Transform as LCMTransform import numpy as np from numpy.typing import NDArray - from dimos.msgs.std_msgs import Bool, Float32 logger = setup_logger() @@ -68,13 +68,13 @@ class VRTeleopModule(BaseTeleopModule): vr_right_transform: In[LCMTransform] = None # type: ignore[assignment] vr_trigger_0: In[Float32] = None # type: ignore[assignment] vr_trigger_1: In[Float32] = None # type: ignore[assignment] - teleop_enable: In[Bool] = None # X button calibration toggle + teleop_enable: In[Bool] = None # type: ignore[assignment] # X button calibration toggle def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) self._lcm_lock = threading.Lock() - self._lcm_controller_poses: dict[int, NDArray[np.float32] | None] = { + self._lcm_controller_poses: dict[int, NDArray[np.float64] | None] = { i: None for i in self._active_indices } self._lcm_gripper_values: dict[int, float] = {i: 0.0 for i in self._active_indices} @@ -96,19 +96,23 @@ def start(self) -> None: if self.vr_left_transform and self.vr_left_transform.transport: self.vr_left_transform.subscribe( - lambda msg, idx=left_index: self._on_lcm_transform(idx, msg) + lambda msg, idx=left_index: self._on_lcm_transform(idx, msg) # type: ignore[misc] ) if self.vr_right_transform and self.vr_right_transform.transport: self.vr_right_transform.subscribe( - lambda msg, idx=right_index: self._on_lcm_transform(idx, msg) + lambda msg, idx=right_index: self._on_lcm_transform(idx, msg) # type: ignore[misc] ) if self.vr_trigger_0 and self.vr_trigger_0.transport: - self.vr_trigger_0.subscribe(lambda msg, idx=left_index: self._on_lcm_trigger(idx, msg)) + self.vr_trigger_0.subscribe( + lambda msg, idx=left_index: self._on_lcm_trigger(idx, msg) # type: ignore[misc] + ) if self.vr_trigger_1 and self.vr_trigger_1.transport: - self.vr_trigger_1.subscribe(lambda msg, idx=right_index: self._on_lcm_trigger(idx, msg)) + self.vr_trigger_1.subscribe( + lambda msg, idx=right_index: self._on_lcm_trigger(idx, msg) # type: ignore[misc] + ) if self.teleop_enable and self.teleop_enable.transport: self.teleop_enable.subscribe(self._on_lcm_teleop_enable) @@ -152,7 +156,7 @@ def _on_lcm_transform(self, index: int, transform: LCMTransform) -> None: is_left = index == self._active_indices[0] transform_matrix = transform_vr_to_robot(transform, is_left_controller=is_left) with self._lcm_lock: - self._lcm_controller_poses[index] = transform_matrix + self._lcm_controller_poses[index] = transform_matrix # type: ignore[assignment] def _on_lcm_trigger(self, index: int, msg: Float32) -> None: """Handle trigger value for gripper control.""" @@ -194,15 +198,12 @@ def _control_loop(self) -> None: deltas = self.compute_deltas(controller_poses, controller_trigger_values) - for i in self._active_indices: + for idx, i in enumerate(self._active_indices): delta = deltas.get(i) if delta is None: continue - output_type = self._output_types.get(i) - if output_type is None: - continue - + output_type = self.config.output_types[idx] command, aux_command = transform_delta( delta_pose=delta, trigger_value=self._all_trigger_values.get(i, 0.0), diff --git a/dimos/utils/teleop_transforms.py b/dimos/utils/teleop_transforms.py index 71f4fb98de..dc4c3da14f 100644 --- a/dimos/utils/teleop_transforms.py +++ b/dimos/utils/teleop_transforms.py @@ -18,10 +18,10 @@ from __future__ import annotations import time -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Any import numpy as np -from scipy.spatial.transform import Rotation as R +from scipy.spatial.transform import Rotation as R # type: ignore[import-untyped] from dimos.msgs.geometry_msgs import Pose, PoseStamped, Twist, Vector3 from dimos.msgs.std_msgs import Bool @@ -190,7 +190,9 @@ def clamp(value: float, limit: float) -> float: return Twist(linear=linear, angular=angular) -def parse_pose_from_dict(output_type: type, data: dict | None = None) -> Pose | None: +def parse_pose_from_dict( # type: ignore[type-arg] + output_type: type, data: dict[str, Any] | None = None +) -> Pose | None: """Parse a Pose from a dictionary based on output type. Args: From 4d720434abbb6521885a475d365d4497dd14817e Mon Sep 17 00:00:00 2001 From: ruthwikdasyam <63036454+ruthwikdasyam@users.noreply.github.com> Date: Thu, 15 Jan 2026 03:37:32 +0000 Subject: [PATCH 55/72] CI code cleanup --- dimos/teleop/devices/base_teleop_module.py | 3 ++- dimos/teleop/devices/vr_teleop_module.py | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/dimos/teleop/devices/base_teleop_module.py b/dimos/teleop/devices/base_teleop_module.py index aeffdc22c4..7575d56c37 100644 --- a/dimos/teleop/devices/base_teleop_module.py +++ b/dimos/teleop/devices/base_teleop_module.py @@ -30,7 +30,6 @@ from dimos.core import Module, Out, rpc from dimos.core.module import ModuleConfig from dimos.msgs.geometry_msgs import Pose, PoseStamped, Twist -from dimos.msgs.std_msgs import Bool from dimos.utils.logging_config import setup_logger from dimos.utils.teleop_transforms import compute_active_indices, parse_pose_from_dict from dimos.utils.teleop_visualization import ( @@ -44,6 +43,8 @@ import numpy as np from numpy.typing import NDArray + from dimos.msgs.std_msgs import Bool + logger = setup_logger() diff --git a/dimos/teleop/devices/vr_teleop_module.py b/dimos/teleop/devices/vr_teleop_module.py index ac2d2e06eb..adacdc9a53 100644 --- a/dimos/teleop/devices/vr_teleop_module.py +++ b/dimos/teleop/devices/vr_teleop_module.py @@ -29,16 +29,16 @@ from dimos.core import In, rpc from dimos.msgs.geometry_msgs import PoseStamped -from dimos.msgs.std_msgs import Bool, Float32 -from dimos_lcm.geometry_msgs import Transform as LCMTransform from dimos.teleop.devices.base_teleop_module import BaseTeleopConfig, BaseTeleopModule from dimos.utils.logging_config import setup_logger from dimos.utils.teleop_transforms import transform_delta, transform_vr_to_robot if TYPE_CHECKING: + from dimos_lcm.geometry_msgs import Transform as LCMTransform import numpy as np from numpy.typing import NDArray + from dimos.msgs.std_msgs import Bool, Float32 logger = setup_logger() From 55e49a64a504c1085a51da149394d4ef67e35528 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 14 Jan 2026 19:50:06 -0800 Subject: [PATCH 56/72] Fix: mypy errors --- dimos/teleop/devices/vr_teleop_module.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/dimos/teleop/devices/vr_teleop_module.py b/dimos/teleop/devices/vr_teleop_module.py index adacdc9a53..d0284f8520 100644 --- a/dimos/teleop/devices/vr_teleop_module.py +++ b/dimos/teleop/devices/vr_teleop_module.py @@ -130,17 +130,18 @@ def stop(self) -> None: def start_teleop(self) -> dict[str, Any]: """Calibrate and start teleoperation (called via X button).""" logger.info("Starting teleop - calibrating VR...") - res = self.calibrate() - if not res.get("success"): - logger.error(f"Calibration failed: {res.get('error')}") - return res + result: dict[str, Any] = self.calibrate() + if not result.get("success"): + logger.error(f"Calibration failed: {result.get('error')}") + return result @rpc def stop_teleop(self) -> dict[str, Any]: """Stop teleoperation and reset calibration.""" logger.info("Stopping teleop - resetting calibration...") self._stop_control_loop() - return self.reset_calibration() + result: dict[str, Any] = self.reset_calibration() + return result def _on_lcm_teleop_enable(self, msg: Bool) -> None: """Handle teleop enable/disable from X button.""" From d4e66b9e2e117b01e2b7e4a0e19613130beda501 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 14 Jan 2026 19:50:17 -0800 Subject: [PATCH 57/72] Fix: updating modules --- dimos/robot/all_blueprints.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index ccae6c5348..2c1f1ef163 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -90,8 +90,7 @@ "cartesian_motion_controller": "dimos.manipulation.control.servo_control.cartesian_motion_controller", "joint_trajectory_controller": "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller", # Teleop modules - "quest3_teleop_module": "dimos.teleop.quest3.quest3_teleop_module", - "teleop_robot_controller": "dimos.teleop.teleop_robot_controller", + "vr_teleop_module": "dimos.teleop.devices.vr_teleop_module", } From 3511878de0600003ee96794cb4fec2e4782a64fd Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 14 Jan 2026 21:24:15 -0800 Subject: [PATCH 58/72] Fix: typechecking imports issue --- dimos/teleop/devices/base_teleop_module.py | 7 ++----- dimos/teleop/devices/vr_teleop_module.py | 8 +++----- 2 files changed, 5 insertions(+), 10 deletions(-) diff --git a/dimos/teleop/devices/base_teleop_module.py b/dimos/teleop/devices/base_teleop_module.py index 7575d56c37..825bea3c50 100644 --- a/dimos/teleop/devices/base_teleop_module.py +++ b/dimos/teleop/devices/base_teleop_module.py @@ -20,8 +20,6 @@ Subclasses implement device-specific input handling. """ -from __future__ import annotations - from abc import ABC from dataclasses import dataclass, field from enum import Enum @@ -30,6 +28,7 @@ from dimos.core import Module, Out, rpc from dimos.core.module import ModuleConfig from dimos.msgs.geometry_msgs import Pose, PoseStamped, Twist +from dimos.msgs.std_msgs import Bool from dimos.utils.logging_config import setup_logger from dimos.utils.teleop_transforms import compute_active_indices, parse_pose_from_dict from dimos.utils.teleop_visualization import ( @@ -43,8 +42,6 @@ import numpy as np from numpy.typing import NDArray - from dimos.msgs.std_msgs import Bool - logger = setup_logger() @@ -231,7 +228,7 @@ def get_status(self) -> dict[str, Any]: def compute_deltas( self, - controller_poses: dict[int, NDArray[np.float64] | None], + controller_poses: "dict[int, NDArray[np.float64] | None]", controller_trigger_values: dict[int, float], ) -> dict[int, Pose | None]: """Compute delta = current_pose - initial_pose for each controller.""" diff --git a/dimos/teleop/devices/vr_teleop_module.py b/dimos/teleop/devices/vr_teleop_module.py index d0284f8520..a74b0c5853 100644 --- a/dimos/teleop/devices/vr_teleop_module.py +++ b/dimos/teleop/devices/vr_teleop_module.py @@ -20,26 +20,24 @@ transforms from WebXR to robot frame, computes deltas, and publishes commands. """ -from __future__ import annotations - from dataclasses import dataclass, field import threading import time from typing import TYPE_CHECKING, Any +from dimos_lcm.geometry_msgs import Transform as LCMTransform + from dimos.core import In, rpc from dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.std_msgs import Bool, Float32 from dimos.teleop.devices.base_teleop_module import BaseTeleopConfig, BaseTeleopModule from dimos.utils.logging_config import setup_logger from dimos.utils.teleop_transforms import transform_delta, transform_vr_to_robot if TYPE_CHECKING: - from dimos_lcm.geometry_msgs import Transform as LCMTransform import numpy as np from numpy.typing import NDArray - from dimos.msgs.std_msgs import Bool, Float32 - logger = setup_logger() From 6093af51995242432c602bddf5a883fff42287a1 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 14 Jan 2026 23:25:04 -0800 Subject: [PATCH 59/72] Fex: Moved certs/ to assets/ + Readme update --- .gitignore | 2 +- dimos/teleop/web/README.md | 14 +++----------- dimos/teleop/web/teleop_server.ts | 24 ++++++++++-------------- 3 files changed, 14 insertions(+), 26 deletions(-) diff --git a/.gitignore b/.gitignore index 038bf2e1f7..e3c46ff03a 100644 --- a/.gitignore +++ b/.gitignore @@ -65,4 +65,4 @@ yolo11n.pt *mobileclip* # Teleop SSL certificates -dimos/teleop/web/certs/ +assets/teleop_certs/ diff --git a/dimos/teleop/web/README.md b/dimos/teleop/web/README.md index 34defc8dee..defe125f8f 100644 --- a/dimos/teleop/web/README.md +++ b/dimos/teleop/web/README.md @@ -14,27 +14,19 @@ Deno server that bridges WebSocket and LCM: ### static/index.html WebXR client running on Quest 3: -- Captures controller poses at 72Hz +- Captures controller poses at set frequency (Also Depends on Refresh rate) - Sends Transform messages via WebSocket - X button triggers calibration (teleop_enable) ## Running ```bash -cd dimos/teleop/web -deno run --allow-net --allow-read --unstable-net teleop_server.ts +deno run --allow-net --allow-read --allow-run --allow-write --unstable-net dimos/teleop/web/teleop_server.ts ``` Server starts at `https://localhost:8443` -## SSL Certificates - -Generate self-signed certs (required for WebXR): - -```bash -mkdir -p certs -openssl req -x509 -newkey rsa:2048 -keyout certs/key.pem -out certs/cert.pem -days 365 -nodes -subj "/CN=localhost" -``` +SSL certificates are generated automatically on first run in `assets/teleop_certs/`. ## Message Flow diff --git a/dimos/teleop/web/teleop_server.ts b/dimos/teleop/web/teleop_server.ts index d4ff6c986e..69332eab6f 100644 --- a/dimos/teleop/web/teleop_server.ts +++ b/dimos/teleop/web/teleop_server.ts @@ -1,26 +1,29 @@ -#!/usr/bin/env -S deno run --allow-net --allow-read --unstable-net +#!/usr/bin/env -S deno run --allow-net --allow-read --allow-run --allow-write --unstable-net // LCM to WebSocket Bridge for Robot Control // Forwards robot pose to browser, receives twist commands from browser import { LCM } from "jsr:@dimos/lcm"; -import { decodePacket, geometry_msgs } from "jsr:@dimos/msgs"; +import { dirname, fromFileUrl, join } from "jsr:@std/path"; const PORT = 8443; const clients = new Set(); +// Resolve paths relative to script location +const scriptDir = dirname(fromFileUrl(import.meta.url)); +const certsDir = join(scriptDir, "../../../assets/teleop_certs"); +const certPath = join(certsDir, "cert.pem"); +const keyPath = join(certsDir, "key.pem"); + // Auto-generate self-signed certificates if they don't exist async function ensureCerts(): Promise<{ cert: string; key: string }> { - const certPath = "./certs/cert.pem"; - const keyPath = "./certs/key.pem"; - try { const cert = await Deno.readTextFile(certPath); const key = await Deno.readTextFile(keyPath); return { cert, key }; } catch { console.log("Generating self-signed certificates..."); - await Deno.mkdir("./certs", { recursive: true }); + await Deno.mkdir(certsDir, { recursive: true }); const cmd = new Deno.Command("openssl", { args: [ "req", "-x509", "-newkey", "rsa:2048", @@ -32,7 +35,7 @@ async function ensureCerts(): Promise<{ cert: string; key: string }> { if (code !== 0) { throw new Error("Failed to generate certificates. Is openssl installed?"); } - console.log("Certificates generated in ./certs/"); + console.log("Certificates generated in assets/teleop_certs/"); return { cert: await Deno.readTextFile(certPath), key: await Deno.readTextFile(keyPath), @@ -82,13 +85,6 @@ console.log(`Server: https://localhost:${PORT}`); const lcm = new LCM(); await lcm.start(); -// Subscribe to pose and just log to show how server can decode messages for itself -// lcm.subscribe("/odom", geometry_msgs.PoseStamped, (msg) => { -// const pos = msg.data.pose.position; -// const ori = msg.data.pose.orientation; -// console.log(`[pose] x=${pos.x.toFixed(2)} y=${pos.y.toFixed(2)} z=${pos.z.toFixed(2)}`); -// }); - // Forward all raw packets to browser (we are decoding LCM directly in the browser) lcm.subscribePacket((packet) => { for (const client of clients) { From be07c99d6ac0412684db36fb367d1c89d67ddf11 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 15 Jan 2026 15:47:01 -0800 Subject: [PATCH 60/72] Feat: Added ControllerPose msg type --- dimos/msgs/geometry_msgs/ControllerPose.py | 112 +++++++++++++++++++++ dimos/msgs/geometry_msgs/__init__.py | 2 + 2 files changed, 114 insertions(+) create mode 100644 dimos/msgs/geometry_msgs/ControllerPose.py diff --git a/dimos/msgs/geometry_msgs/ControllerPose.py b/dimos/msgs/geometry_msgs/ControllerPose.py new file mode 100644 index 0000000000..003caa042d --- /dev/null +++ b/dimos/msgs/geometry_msgs/ControllerPose.py @@ -0,0 +1,112 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Controller pose with conversion utilities for teleop output types.""" + +from __future__ import annotations + +import time + +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 + + +class ControllerPose(Pose): + """Controller pose with conversion utilities for teleop output types. + + Extends Pose with methods to convert to PoseStamped or Twist commands. + """ + + @classmethod + def from_pose(cls, pose: Pose) -> ControllerPose: + """Create ControllerPose from a Pose. + + Args: + pose: Pose to convert. + + Returns: + ControllerPose with same position and orientation. + """ + return cls(position=pose.position, orientation=pose.orientation) + + def to_pose_stamped( + self, + initial_robot_pose: Pose | None = None, + frame_id: str = "teleop_target", + ) -> PoseStamped: + """Convert to PoseStamped (target = initial + delta). + + Args: + initial_robot_pose: Initial robot pose to add delta to. + If None, delta is used directly as target. + frame_id: Frame ID for the PoseStamped message. + + Returns: + PoseStamped target pose. + """ + if initial_robot_pose is not None: + target_pose = initial_robot_pose + self + else: + target_pose = self + + return PoseStamped( + ts=time.time(), + frame_id=frame_id, + position=target_pose.position, + orientation=target_pose.orientation, + ) + + def to_twist( + self, + linear_scale: float = 1.0, + angular_scale: float = 1.0, + max_linear: float = 0.5, + max_angular: float = 1.0, + ) -> Twist: + """Convert to Twist velocity command. + + Args: + linear_scale: Scale factor for linear velocity. + angular_scale: Scale factor for angular velocity. + max_linear: Maximum linear velocity clamp. + max_angular: Maximum angular velocity clamp. + + Returns: + Twist velocity command. + """ + + def clamp(value: float, limit: float) -> float: + return max(-limit, min(limit, value)) + + linear = Vector3( + clamp(self.position.x * linear_scale, max_linear), + clamp(self.position.y * linear_scale, max_linear), + clamp(self.position.z * linear_scale, max_linear), + ) + + euler = self.orientation.to_euler() + angular = Vector3( + clamp(euler.x * angular_scale, max_angular), + clamp(euler.y * angular_scale, max_angular), + clamp(euler.z * angular_scale, max_angular), + ) + + return Twist(linear=linear, angular=angular) + + def __sub__(self, other: Pose) -> ControllerPose: + """Compute delta pose, returning ControllerPose instead of Pose.""" + result = super().__sub__(other) + return ControllerPose.from_pose(result) diff --git a/dimos/msgs/geometry_msgs/__init__.py b/dimos/msgs/geometry_msgs/__init__.py index fd47d5f0ed..f7a53d9fa4 100644 --- a/dimos/msgs/geometry_msgs/__init__.py +++ b/dimos/msgs/geometry_msgs/__init__.py @@ -1,3 +1,4 @@ +from dimos.msgs.geometry_msgs.ControllerPose import ControllerPose from dimos.msgs.geometry_msgs.Pose import Pose, PoseLike, to_pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance @@ -13,6 +14,7 @@ from dimos.msgs.geometry_msgs.WrenchStamped import WrenchStamped __all__ = [ + "ControllerPose", "Pose", "PoseLike", "PoseStamped", From 1df6a984784d25a2d5bb1537010b196b983fcb09 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 15 Jan 2026 15:48:25 -0800 Subject: [PATCH 61/72] Misc: msg type updates --- dimos/teleop/devices/base_teleop_module.py | 129 +++++++++---------- dimos/teleop/devices/vr_teleop_module.py | 99 +++++++-------- dimos/utils/teleop_transforms.py | 136 +-------------------- dimos/utils/teleop_visualization.py | 22 ++-- 4 files changed, 124 insertions(+), 262 deletions(-) diff --git a/dimos/teleop/devices/base_teleop_module.py b/dimos/teleop/devices/base_teleop_module.py index 825bea3c50..554bed954d 100644 --- a/dimos/teleop/devices/base_teleop_module.py +++ b/dimos/teleop/devices/base_teleop_module.py @@ -20,28 +20,21 @@ Subclasses implement device-specific input handling. """ -from abc import ABC from dataclasses import dataclass, field from enum import Enum -from typing import TYPE_CHECKING, Any +from typing import TYPE_CHECKING, Any, TypeVar from dimos.core import Module, Out, rpc from dimos.core.module import ModuleConfig -from dimos.msgs.geometry_msgs import Pose, PoseStamped, Twist +from dimos.msgs.geometry_msgs import ControllerPose, Pose, PoseStamped, Twist from dimos.msgs.std_msgs import Bool from dimos.utils.logging_config import setup_logger -from dimos.utils.teleop_transforms import compute_active_indices, parse_pose_from_dict +from dimos.utils.teleop_transforms import compute_active_indices from dimos.utils.teleop_visualization import ( init_rerun_visualization, visualize_controller_pose, visualize_trigger_value, ) -from dimos.utils.transform_utils import matrix_to_pose - -if TYPE_CHECKING: - import numpy as np - from numpy.typing import NDArray - logger = setup_logger() @@ -75,15 +68,17 @@ class TeleopStatusKey(str, Enum): CONTROLLER_LABEL = "controller_{index}_label" -class BaseTeleopModule(Module, ABC): +TeleopConfigT = TypeVar("TeleopConfigT", bound=BaseTeleopConfig) + + +class BaseTeleopModule(Module[TeleopConfigT]): """Base class for teleoperation modules. Handles calibration, delta computation, and command publishing. Subclasses implement device-specific input handling. """ - default_config = BaseTeleopConfig - config: BaseTeleopConfig + default_config: type[BaseTeleopConfig] = BaseTeleopConfig # Output topics: PoseStamped for arms (0,1), Twist for locomotion (2,3) controller_delta_0: Out[PoseStamped] = None # type: ignore @@ -102,17 +97,23 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: self._tracking_msg_count = 0 self._publish_count = 0 + if not self.config.output_types: + raise ValueError("output_types cannot be empty") + if len(self.config.input_labels) != len(self.config.output_types): raise ValueError( f"input_labels length ({len(self.config.input_labels)}) must match " f"output_types length ({len(self.config.output_types)})" ) + self._active_indices = compute_active_indices(self.config.output_types) - self._initial_poses: dict[int, Pose | None] = {i: None for i in self._active_indices} + self._initial_controller_poses: dict[int, ControllerPose | None] = { + i: None for i in self._active_indices + } self._initial_robot_poses: dict[int, Pose | None] = {i: None for i in self._active_indices} - self._all_poses: dict[int, NDArray[np.float64] | None] = { + self._current_controller_poses: dict[int, ControllerPose | None] = { i: None for i in self._active_indices } self._all_trigger_values: dict[int, float] = {i: 0.0 for i in self._active_indices} @@ -143,67 +144,73 @@ def stop(self) -> None: super().stop() @rpc - def calibrate(self) -> dict[str, Any]: + def calibrate(self) -> bool: """Capture current controller poses as the zero reference.""" logger.info("Calibrating controllers...") try: if not self._active_indices: - return {"success": False, "error": "No controllers are enabled"} + logger.error("Calibration failed: No controllers are enabled") + return False for i, idx in enumerate(self._active_indices): - pose_matrix = self._all_poses.get(idx) - if pose_matrix is not None: - pose_obj = matrix_to_pose(pose_matrix) - self._initial_poses[idx] = pose_obj - logger.info(f"Captured controller {self._get_label(idx)} initial: {pose_obj}") + pose = self._current_controller_poses.get(idx) + if pose is not None: + self._initial_controller_poses[idx] = pose + logger.info(f"Captured controller {self._get_label(idx)} initial: {pose}") else: - return { - "success": False, - "error": f"Controller {self._get_label(idx)} data is None", - } + logger.error( + f"Calibration failed: Controller {self._get_label(idx)} data is None" + ) + return False - # Get initial robot pose via RPC if configured + # Get initial robot state via RPC if configured (only for PoseStamped) output_type = self.config.output_types[i] - if i < len(self.config.robot_pose_rpc_methods): - rpc_method = self.config.robot_pose_rpc_methods[i] - if rpc_method is not None: - self._initial_robot_poses[idx] = self._get_robot_initial_pose( - output_type, rpc_method - ) + if output_type == PoseStamped: + if i < len(self.config.robot_pose_rpc_methods): + rpc_method = self.config.robot_pose_rpc_methods[i] + if rpc_method is not None: + self._initial_robot_poses[idx] = self._get_initial_robot_state( + rpc_method + ) + else: + self._initial_robot_poses[idx] = None else: - self._initial_robot_poses[idx] = parse_pose_from_dict(output_type) + self._initial_robot_poses[idx] = None else: - self._initial_robot_poses[idx] = parse_pose_from_dict(output_type) + # Twist doesn't need initial robot pose + self._initial_robot_poses[idx] = None self._is_calibrated = True logger.info("Calibration complete. Now streaming delta poses...") - return {"success": True, "message": "Calibrated"} + return True except Exception as e: logger.error(f"Calibration failed: {e}", exc_info=True) - return {"success": False, "error": str(e)} - - def _get_robot_initial_pose(self, output_type: type, rpc_method: str) -> Pose | None: - """Get initial robot pose via RPC call.""" + return False + + def _get_initial_robot_state(self, rpc_method: str) -> Pose | None: + """Get initial robot state via RPC call. + Args: + rpc_method: RPC method name that returns the initial state. + Returns: + Robot state from RPC, or origin Pose() if RPC fails. + """ try: - rpc_call = self.get_rpc_calls(rpc_method) - rpc_result = rpc_call() - pose = parse_pose_from_dict(output_type, rpc_result) - logger.info(f"Got robot initial pose via {rpc_method}: {pose}") - return pose + state = self.get_rpc_calls(rpc_method)() + logger.info(f"Got initial robot state via {rpc_method}: {state}") + return state except Exception as e: - logger.warning(f"RPC call {rpc_method} failed: {e}, using zero pose") - return parse_pose_from_dict(output_type) + logger.warning(f"RPC call {rpc_method} failed: {e}, using origin pose") + return None @rpc - def reset_calibration(self) -> dict[str, Any]: + def reset_calibration(self) -> None: """Reset calibration. Stops streaming until recalibrated.""" self._is_calibrated = False - self._initial_poses = {i: None for i in self._active_indices} + self._initial_controller_poses = {i: None for i in self._active_indices} self._initial_robot_poses = {i: None for i in self._active_indices} logger.info("Calibration reset.") - return {"success": True, "message": "Calibration reset"} @rpc def is_calibrated(self) -> bool: @@ -218,7 +225,7 @@ def get_status(self) -> dict[str, Any]: } for i in self._active_indices: status[TeleopStatusKey.CONTROLLER_HAS_DATA.value.format(index=i)] = ( - self._all_poses.get(i) is not None + self._current_controller_poses.get(i) is not None ) status[TeleopStatusKey.CONTROLLER_TRIGGER_VALUE.value.format(index=i)] = ( self._all_trigger_values.get(i, 0.0) @@ -228,12 +235,12 @@ def get_status(self) -> dict[str, Any]: def compute_deltas( self, - controller_poses: "dict[int, NDArray[np.float64] | None]", + controller_poses: dict[int, ControllerPose | None], controller_trigger_values: dict[int, float], - ) -> dict[int, Pose | None]: + ) -> dict[int, ControllerPose | None]: """Compute delta = current_pose - initial_pose for each controller.""" self._tracking_msg_count += 1 - self._all_poses = controller_poses + self._current_controller_poses = controller_poses self._all_trigger_values = controller_trigger_values if not self._is_calibrated: @@ -242,25 +249,23 @@ def compute_deltas( return {i: None for i in self._active_indices} self._publish_count += 1 - deltas: dict[int, Pose | None] = {} + deltas: dict[int, ControllerPose | None] = {} for i in self._active_indices: - pose_matrix = self._all_poses.get(i) - if pose_matrix is None: + current_pose = self._current_controller_poses.get(i) + if current_pose is None: deltas[i] = None continue - initial_pose = self._initial_poses.get(i) + initial_pose = self._initial_controller_poses.get(i) if initial_pose is None: deltas[i] = None continue - pose_obj = matrix_to_pose(pose_matrix) - delta_pose = pose_obj - initial_pose - + delta_pose = current_pose - initial_pose if self.config.visualize_in_rerun: label = self._get_label(i) - visualize_controller_pose(pose_obj, label) + visualize_controller_pose(current_pose, label) visualize_trigger_value(self._all_trigger_values.get(i, 0.0), label) deltas[i] = delta_pose diff --git a/dimos/teleop/devices/vr_teleop_module.py b/dimos/teleop/devices/vr_teleop_module.py index a74b0c5853..7c7f1e8a7b 100644 --- a/dimos/teleop/devices/vr_teleop_module.py +++ b/dimos/teleop/devices/vr_teleop_module.py @@ -23,21 +23,17 @@ from dataclasses import dataclass, field import threading import time -from typing import TYPE_CHECKING, Any +from typing import Any from dimos_lcm.geometry_msgs import Transform as LCMTransform from dimos.core import In, rpc -from dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.geometry_msgs import ControllerPose, PoseStamped, Twist from dimos.msgs.std_msgs import Bool, Float32 from dimos.teleop.devices.base_teleop_module import BaseTeleopConfig, BaseTeleopModule from dimos.utils.logging_config import setup_logger -from dimos.utils.teleop_transforms import transform_delta, transform_vr_to_robot - -if TYPE_CHECKING: - import numpy as np - from numpy.typing import NDArray - +from dimos.utils.teleop_transforms import transform_vr_to_robot +from dimos.utils.transform_utils import matrix_to_pose logger = setup_logger() @@ -48,10 +44,11 @@ class VRTeleopConfig(BaseTeleopConfig): output_types: list[type] = field(default_factory=lambda: [PoseStamped, PoseStamped]) input_labels: list[str] = field(default_factory=lambda: ["left_vr", "right_vr"]) + robot_pose_rpc_methods: list[str | None] = field(default_factory=lambda: [None, None]) control_loop_hz: float = 50.0 -class VRTeleopModule(BaseTeleopModule): +class VRTeleopModule(BaseTeleopModule[VRTeleopConfig]): """VR Teleoperation Module for Quest 3 controllers. Subscribes to controller data from Deno bridge, transforms WebXRโ†’robot frame, @@ -59,7 +56,6 @@ class VRTeleopModule(BaseTeleopModule): """ default_config = VRTeleopConfig - config: VRTeleopConfig # LCM inputs from Deno bridge vr_left_transform: In[LCMTransform] = None # type: ignore[assignment] @@ -72,7 +68,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) self._lcm_lock = threading.Lock() - self._lcm_controller_poses: dict[int, NDArray[np.float64] | None] = { + self._lcm_controller_poses: dict[int, ControllerPose | None] = { i: None for i in self._active_indices } self._lcm_gripper_values: dict[int, float] = {i: 0.0 for i in self._active_indices} @@ -92,28 +88,20 @@ def start(self) -> None: f"rightโ†’{self._get_label(right_index)}(idx {right_index})" ) - if self.vr_left_transform and self.vr_left_transform.transport: - self.vr_left_transform.subscribe( - lambda msg, idx=left_index: self._on_lcm_transform(idx, msg) # type: ignore[misc] - ) - - if self.vr_right_transform and self.vr_right_transform.transport: - self.vr_right_transform.subscribe( - lambda msg, idx=right_index: self._on_lcm_transform(idx, msg) # type: ignore[misc] - ) - - if self.vr_trigger_0 and self.vr_trigger_0.transport: - self.vr_trigger_0.subscribe( - lambda msg, idx=left_index: self._on_lcm_trigger(idx, msg) # type: ignore[misc] - ) - - if self.vr_trigger_1 and self.vr_trigger_1.transport: - self.vr_trigger_1.subscribe( - lambda msg, idx=right_index: self._on_lcm_trigger(idx, msg) # type: ignore[misc] - ) - - if self.teleop_enable and self.teleop_enable.transport: - self.teleop_enable.subscribe(self._on_lcm_teleop_enable) + # Subscribe to LCM inputs + subscriptions = [ + (self.vr_left_transform, lambda msg, idx=left_index: self._on_lcm_transform(idx, msg)), + ( + self.vr_right_transform, + lambda msg, idx=right_index: self._on_lcm_transform(idx, msg), + ), + (self.vr_trigger_0, lambda msg, idx=left_index: self._on_lcm_trigger(idx, msg)), + (self.vr_trigger_1, lambda msg, idx=right_index: self._on_lcm_trigger(idx, msg)), + (self.teleop_enable, self._on_lcm_teleop_enable), + ] + for stream, handler in subscriptions: + if stream and stream.transport: + stream.subscribe(handler) # type: ignore[misc] logger.info("VR Teleoperation Module started") @@ -125,21 +113,17 @@ def stop(self) -> None: super().stop() @rpc - def start_teleop(self) -> dict[str, Any]: + def start_teleop(self) -> bool: """Calibrate and start teleoperation (called via X button).""" logger.info("Starting teleop - calibrating VR...") - result: dict[str, Any] = self.calibrate() - if not result.get("success"): - logger.error(f"Calibration failed: {result.get('error')}") - return result + return self.calibrate() @rpc - def stop_teleop(self) -> dict[str, Any]: + def stop_teleop(self) -> None: """Stop teleoperation and reset calibration.""" logger.info("Stopping teleop - resetting calibration...") self._stop_control_loop() - result: dict[str, Any] = self.reset_calibration() - return result + self.reset_calibration() def _on_lcm_teleop_enable(self, msg: Bool) -> None: """Handle teleop enable/disable from X button.""" @@ -154,8 +138,9 @@ def _on_lcm_transform(self, index: int, transform: LCMTransform) -> None: self._start_control_loop() is_left = index == self._active_indices[0] transform_matrix = transform_vr_to_robot(transform, is_left_controller=is_left) + pose = ControllerPose.from_pose(matrix_to_pose(transform_matrix)) with self._lcm_lock: - self._lcm_controller_poses[index] = transform_matrix # type: ignore[assignment] + self._lcm_controller_poses[index] = pose def _on_lcm_trigger(self, index: int, msg: Float32) -> None: """Handle trigger value for gripper control.""" @@ -202,19 +187,25 @@ def _control_loop(self) -> None: if delta is None: continue + trigger_value = self._all_trigger_values.get(i, 0.0) + trigger_bool = Bool(data=trigger_value > self.config.gripper_threshold) + output_type = self.config.output_types[idx] - command, aux_command = transform_delta( - delta_pose=delta, - trigger_value=self._all_trigger_values.get(i, 0.0), - output_type=output_type, - initial_robot_pose=self._initial_robot_poses.get(i), - linear_scale=self.config.linear_scale, - angular_scale=self.config.angular_scale, - max_linear_velocity=self.config.max_linear_velocity, - max_angular_velocity=self.config.max_angular_velocity, - gripper_threshold=self.config.gripper_threshold, - ) - self.publish_command(i, command, aux_command) + if output_type == PoseStamped: + command = delta.to_pose_stamped( + initial_robot_pose=self._initial_robot_poses.get(i), + ) + elif output_type == Twist: + command = delta.to_twist( + linear_scale=self.config.linear_scale, + angular_scale=self.config.angular_scale, + max_linear=self.config.max_linear_velocity, + max_angular=self.config.max_angular_velocity, + ) + else: + continue + + self.publish_command(i, command, trigger_bool) elapsed = time.perf_counter() - loop_start sleep_time = period - elapsed diff --git a/dimos/utils/teleop_transforms.py b/dimos/utils/teleop_transforms.py index dc4c3da14f..7c66566b8e 100644 --- a/dimos/utils/teleop_transforms.py +++ b/dimos/utils/teleop_transforms.py @@ -13,26 +13,21 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Teleop transform utilities for converting delta poses to robot commands.""" +"""Teleop transform utilities for VR coordinate transforms and index mapping.""" from __future__ import annotations -import time -from typing import TYPE_CHECKING, Any +from typing import TYPE_CHECKING import numpy as np from scipy.spatial.transform import Rotation as R # type: ignore[import-untyped] -from dimos.msgs.geometry_msgs import Pose, PoseStamped, Twist, Vector3 -from dimos.msgs.std_msgs import Bool -from dimos.utils.logging_config import setup_logger +from dimos.msgs.geometry_msgs import PoseStamped, Twist if TYPE_CHECKING: from dimos_lcm.geometry_msgs import Transform as LCMTransform from numpy.typing import NDArray -logger = setup_logger() - # Index mapping: output type โ†’ available indices OUTPUT_TYPE_INDICES: dict[type, list[int]] = { PoseStamped: [0, 1], @@ -108,128 +103,3 @@ def transform_vr_to_robot( # Apply VR to robot frame transformation return VR_TO_ROBOT_FRAME @ vr_matrix - - -def transform_delta( - delta_pose: Pose, - trigger_value: float, - output_type: type, - initial_robot_pose: Pose | None = None, - linear_scale: float = 1.0, - angular_scale: float = 1.0, - max_linear_velocity: float = 0.5, - max_angular_velocity: float = 1.0, - gripper_threshold: float = 0.5, -) -> tuple[PoseStamped | Twist | None, Bool | None]: - """Transform delta pose to robot command based on output type. - - Args: - delta_pose: Delta from teleop device (current - initial). - trigger_value: Trigger value (0.0-1.0). - output_type: PoseStamped or Twist. - initial_robot_pose: Initial robot pose (required for PoseStamped). - linear_scale: Scale factor for linear velocity (Twist only). - angular_scale: Scale factor for angular velocity (Twist only). - max_linear_velocity: Max linear velocity clamp (Twist only). - max_angular_velocity: Max angular velocity clamp (Twist only). - gripper_threshold: Trigger threshold for gripper open/close. - - Returns: - Tuple of (command, trigger_bool). - """ - trigger_bool = Bool(data=trigger_value > gripper_threshold) - - if output_type == PoseStamped: - return _to_pose_stamped(delta_pose, initial_robot_pose), trigger_bool - elif output_type == Twist: - return _to_twist( - delta_pose, linear_scale, angular_scale, max_linear_velocity, max_angular_velocity - ), trigger_bool - else: - logger.warning(f"Unknown output type: {output_type}") - return None, trigger_bool - - -def _to_pose_stamped(delta_pose: Pose, initial_robot_pose: Pose | None) -> PoseStamped: - """Convert delta pose to PoseStamped (target = initial + delta).""" - if initial_robot_pose is not None: - target_pose = initial_robot_pose + delta_pose - else: - target_pose = delta_pose - return PoseStamped( - ts=time.time(), - frame_id="teleop_target", - position=target_pose.position, - orientation=target_pose.orientation, - ) - - -def _to_twist( - delta_pose: Pose, - linear_scale: float, - angular_scale: float, - max_linear: float, - max_angular: float, -) -> Twist: - """Convert delta pose to Twist velocity command.""" - - def clamp(value: float, limit: float) -> float: - return max(-limit, min(limit, value)) - - linear = Vector3( - clamp(delta_pose.position.x * linear_scale, max_linear), - clamp(delta_pose.position.y * linear_scale, max_linear), - clamp(delta_pose.position.z * linear_scale, max_linear), - ) - euler = delta_pose.orientation.to_euler() - angular = Vector3( - clamp(euler.x * angular_scale, max_angular), - clamp(euler.y * angular_scale, max_angular), - clamp(euler.z * angular_scale, max_angular), - ) - return Twist(linear=linear, angular=angular) - - -def parse_pose_from_dict( # type: ignore[type-arg] - output_type: type, data: dict[str, Any] | None = None -) -> Pose | None: - """Parse a Pose from a dictionary based on output type. - - Args: - output_type: PoseStamped or Twist. - data: Dict with position and orientation keys. - {"position": {"x": float, "y": float, "z": float}, - "orientation": {"x": float, "y": float, "z": float, "w": float}} - - Returns: - Parsed Pose for PoseStamped, None for Twist. - """ - if output_type == Twist: - return None - - if output_type == PoseStamped: - if data is None: - return Pose() - - from dimos.msgs.geometry_msgs import Quaternion - - pos = data.get("position", {}) - ori = data.get("orientation", {}) - - pose = Pose( - position=Vector3( - pos.get("x", 0.0), - pos.get("y", 0.0), - pos.get("z", 0.0), - ), - ) - if ori: - pose.orientation = Quaternion( - ori.get("x", 0.0), - ori.get("y", 0.0), - ori.get("z", 0.0), - ori.get("w", 1.0), - ) - return pose - - return None diff --git a/dimos/utils/teleop_visualization.py b/dimos/utils/teleop_visualization.py index 08521bfbf8..d0bb7ee767 100644 --- a/dimos/utils/teleop_visualization.py +++ b/dimos/utils/teleop_visualization.py @@ -17,11 +17,13 @@ from __future__ import annotations -import time +from typing import TYPE_CHECKING -from dimos.msgs.geometry_msgs import Pose, PoseStamped from dimos.utils.logging_config import setup_logger +if TYPE_CHECKING: + from dimos.msgs.geometry_msgs import ControllerPose + logger = setup_logger() try: @@ -38,7 +40,6 @@ def init_rerun_visualization() -> bool: """Initialize Rerun visualization connection.""" if not RERUN_AVAILABLE: return False - try: connect_rerun() logger.info("Connected to Rerun for teleop visualization") @@ -48,7 +49,7 @@ def init_rerun_visualization() -> bool: return False -def visualize_controller_pose(controller_pose: Pose, controller_label: str) -> None: +def visualize_controller_pose(controller_pose: ControllerPose, controller_label: str) -> None: """Visualize controller absolute pose in Rerun. Args: @@ -57,22 +58,18 @@ def visualize_controller_pose(controller_pose: Pose, controller_label: str) -> N """ if not RERUN_AVAILABLE: return - try: - controller_pose_stamped = PoseStamped( - ts=time.time(), - frame_id=f"world/teleop/{controller_label}_controller", - position=controller_pose.position, - orientation=controller_pose.orientation, + pose_stamped = controller_pose.to_pose_stamped( + frame_id=f"world/teleop/{controller_label}_controller" ) rr.log( f"world/teleop/{controller_label}_controller", - controller_pose_stamped.to_rerun(), # type: ignore[no-untyped-call] + pose_stamped.to_rerun(), # type: ignore[no-untyped-call] ) # Log 3D axes to visualize controller orientation (X=red, Y=green, Z=blue) rr.log( f"world/teleop/{controller_label}_controller/axes", - rr.TransformAxes3D(0.15), # type: ignore[attr-defined] + rr.TransformAxes3D(0.10), # type: ignore[attr-defined] ) except Exception as e: logger.debug(f"Failed to log {controller_label} controller to Rerun: {e}") @@ -87,7 +84,6 @@ def visualize_trigger_value(trigger_value: float, controller_label: str) -> None """ if not RERUN_AVAILABLE: return - try: rr.log( f"world/teleop/{controller_label}_controller/trigger", From 59383635c594a460aca675b6adbf90d9a5f6cc8f Mon Sep 17 00:00:00 2001 From: ruthwikdasyam <63036454+ruthwikdasyam@users.noreply.github.com> Date: Thu, 15 Jan 2026 23:49:12 +0000 Subject: [PATCH 62/72] CI code cleanup --- dimos/teleop/devices/base_teleop_module.py | 1 - 1 file changed, 1 deletion(-) diff --git a/dimos/teleop/devices/base_teleop_module.py b/dimos/teleop/devices/base_teleop_module.py index 554bed954d..bbe8f35e53 100644 --- a/dimos/teleop/devices/base_teleop_module.py +++ b/dimos/teleop/devices/base_teleop_module.py @@ -105,7 +105,6 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: f"input_labels length ({len(self.config.input_labels)}) must match " f"output_types length ({len(self.config.output_types)})" ) - self._active_indices = compute_active_indices(self.config.output_types) From 8880f7bce047eb29521d8c0b26d401238e51d8b6 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam <63036454+ruthwikdasyam@users.noreply.github.com> Date: Thu, 15 Jan 2026 23:58:41 +0000 Subject: [PATCH 63/72] CI code cleanup --- dimos/robot/all_blueprints.py | 1 - 1 file changed, 1 deletion(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 2deaf98e8d..3728da8386 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -86,7 +86,6 @@ "web_input": "dimos.agents.cli.web", # Control orchestrator module "control_orchestrator": "dimos.control.orchestrator", - } From 9163e058a1d86e3e517a7b5dc114d050bdac5a60 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 15 Jan 2026 16:12:14 -0800 Subject: [PATCH 64/72] Fix: mypy errors --- dimos/msgs/geometry_msgs/ControllerPose.py | 4 ++-- dimos/teleop/devices/base_teleop_module.py | 6 +++--- dimos/teleop/devices/vr_teleop_module.py | 6 +++--- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/dimos/msgs/geometry_msgs/ControllerPose.py b/dimos/msgs/geometry_msgs/ControllerPose.py index 003caa042d..3fb6570453 100644 --- a/dimos/msgs/geometry_msgs/ControllerPose.py +++ b/dimos/msgs/geometry_msgs/ControllerPose.py @@ -18,7 +18,7 @@ import time -from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.Pose import Pose, PoseConvertable from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -106,7 +106,7 @@ def clamp(value: float, limit: float) -> float: return Twist(linear=linear, angular=angular) - def __sub__(self, other: Pose) -> ControllerPose: + def __sub__(self, other: Pose | PoseConvertable) -> ControllerPose: # type: ignore[override] """Compute delta pose, returning ControllerPose instead of Pose.""" result = super().__sub__(other) return ControllerPose.from_pose(result) diff --git a/dimos/teleop/devices/base_teleop_module.py b/dimos/teleop/devices/base_teleop_module.py index bbe8f35e53..51bb957145 100644 --- a/dimos/teleop/devices/base_teleop_module.py +++ b/dimos/teleop/devices/base_teleop_module.py @@ -78,7 +78,7 @@ class BaseTeleopModule(Module[TeleopConfigT]): Subclasses implement device-specific input handling. """ - default_config: type[BaseTeleopConfig] = BaseTeleopConfig + default_config: type[TeleopConfigT] = BaseTeleopConfig # type: ignore[assignment] # Output topics: PoseStamped for arms (0,1), Twist for locomotion (2,3) controller_delta_0: Out[PoseStamped] = None # type: ignore @@ -196,9 +196,9 @@ def _get_initial_robot_state(self, rpc_method: str) -> Pose | None: Robot state from RPC, or origin Pose() if RPC fails. """ try: - state = self.get_rpc_calls(rpc_method)() + state: Pose = self.get_rpc_calls(rpc_method)() logger.info(f"Got initial robot state via {rpc_method}: {state}") - return state + return state # type: ignore[no-any-return] except Exception as e: logger.warning(f"RPC call {rpc_method} failed: {e}, using origin pose") return None diff --git a/dimos/teleop/devices/vr_teleop_module.py b/dimos/teleop/devices/vr_teleop_module.py index 7c7f1e8a7b..0728eeaa76 100644 --- a/dimos/teleop/devices/vr_teleop_module.py +++ b/dimos/teleop/devices/vr_teleop_module.py @@ -101,7 +101,7 @@ def start(self) -> None: ] for stream, handler in subscriptions: if stream and stream.transport: - stream.subscribe(handler) # type: ignore[misc] + stream.subscribe(handler) # type: ignore[misc, arg-type] logger.info("VR Teleoperation Module started") @@ -113,10 +113,10 @@ def stop(self) -> None: super().stop() @rpc - def start_teleop(self) -> bool: + def start_teleop(self) -> None: """Calibrate and start teleoperation (called via X button).""" logger.info("Starting teleop - calibrating VR...") - return self.calibrate() + self.calibrate() @rpc def stop_teleop(self) -> None: From 8f747ff597d7aa04d84a32c1e3e46e7b3e4c5dd5 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Fri, 16 Jan 2026 00:07:02 -0800 Subject: [PATCH 65/72] Fix: removed to_ros methods --- dimos/msgs/std_msgs/Float32.py | 29 ----------------------------- 1 file changed, 29 deletions(-) diff --git a/dimos/msgs/std_msgs/Float32.py b/dimos/msgs/std_msgs/Float32.py index cb6d71e08f..afd9bd6163 100644 --- a/dimos/msgs/std_msgs/Float32.py +++ b/dimos/msgs/std_msgs/Float32.py @@ -18,11 +18,6 @@ from dimos_lcm.std_msgs import Float32 as LCMFloat32 -try: - from std_msgs.msg import Float32 as ROSFloat32 # type: ignore[attr-defined] -except ImportError: - ROSFloat32 = None # type: ignore[assignment, misc] - class Float32(LCMFloat32): # type: ignore[misc] """ROS-compatible Float32 message.""" @@ -32,27 +27,3 @@ class Float32(LCMFloat32): # type: ignore[misc] def __init__(self, data: float = 0.0) -> None: """Initialize Float32 with data value.""" self.data = data - - @classmethod - def from_ros_msg(cls, ros_msg: ROSFloat32) -> "Float32": - """Create a Float32 from a ROS std_msgs/Float32 message. - - Args: - ros_msg: ROS Float32 message - - Returns: - Float32 instance - """ - return cls(data=ros_msg.data) - - def to_ros_msg(self) -> ROSFloat32: - """Convert to a ROS std_msgs/Float32 message. - - Returns: - ROS Float32 message - """ - if ROSFloat32 is None: - raise ImportError("ROS std_msgs not available") - ros_msg = ROSFloat32() # type: ignore[no-untyped-call] - ros_msg.data = float(self.data) - return ros_msg From 10c7bc469595065cb9f02e131a844c4dc083e087 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Fri, 16 Jan 2026 11:01:20 -0800 Subject: [PATCH 66/72] Fix: pr comments --- dimos/teleop/devices/base_teleop_module.py | 47 +++++++++++++++++----- dimos/teleop/devices/vr_teleop_module.py | 15 +++---- dimos/teleop/teleop_blueprints.py | 13 +----- dimos/utils/teleop_transforms.py | 34 +--------------- 4 files changed, 48 insertions(+), 61 deletions(-) diff --git a/dimos/teleop/devices/base_teleop_module.py b/dimos/teleop/devices/base_teleop_module.py index 51bb957145..9ecec9cfe4 100644 --- a/dimos/teleop/devices/base_teleop_module.py +++ b/dimos/teleop/devices/base_teleop_module.py @@ -29,7 +29,6 @@ from dimos.msgs.geometry_msgs import ControllerPose, Pose, PoseStamped, Twist from dimos.msgs.std_msgs import Bool from dimos.utils.logging_config import setup_logger -from dimos.utils.teleop_transforms import compute_active_indices from dimos.utils.teleop_visualization import ( init_rerun_visualization, visualize_controller_pose, @@ -81,14 +80,14 @@ class BaseTeleopModule(Module[TeleopConfigT]): default_config: type[TeleopConfigT] = BaseTeleopConfig # type: ignore[assignment] # Output topics: PoseStamped for arms (0,1), Twist for locomotion (2,3) - controller_delta_0: Out[PoseStamped] = None # type: ignore - trigger_value_0: Out[Bool] = None # type: ignore - controller_delta_1: Out[PoseStamped] = None # type: ignore - trigger_value_1: Out[Bool] = None # type: ignore - controller_delta_2: Out[Twist] = None # type: ignore - trigger_value_2: Out[Bool] = None # type: ignore - controller_delta_3: Out[Twist] = None # type: ignore - trigger_value_3: Out[Bool] = None # type: ignore + controller_delta_0: Out[PoseStamped] + trigger_value_0: Out[Bool] + controller_delta_1: Out[PoseStamped] + trigger_value_1: Out[Bool] + controller_delta_2: Out[Twist] + trigger_value_2: Out[Bool] + controller_delta_3: Out[Twist] + trigger_value_3: Out[Bool] def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) @@ -106,7 +105,13 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: f"output_types length ({len(self.config.output_types)})" ) - self._active_indices = compute_active_indices(self.config.output_types) + # Index mapping: output type โ†’ available indices + self._output_type_indices: dict[type, list[int]] = { + PoseStamped: [0, 1], + Twist: [2, 3], + } + + self._active_indices = self._compute_active_indices(self.config.output_types) self._initial_controller_poses: dict[int, ControllerPose | None] = { i: None for i in self._active_indices @@ -130,6 +135,28 @@ def _get_label(self, index: int) -> str: except (ValueError, IndexError): return f"controller_{index}" + def _compute_active_indices(self, output_types: list[type]) -> list[int]: + """Compute active indices from output types. + Example: + [PoseStamped, Twist] โ†’ [0, 2] + [Twist, PoseStamped] โ†’ [2, 0] + [PoseStamped, PoseStamped] โ†’ [0, 1] + [Twist, Twist] โ†’ [2, 3] + """ + indices: list[int] = [] + used_indices: set[int] = set() + + for output_type in output_types: + available = self._output_type_indices.get(output_type, []) + for idx in available: + if idx not in used_indices: + indices.append(idx) + used_indices.add(idx) + break + else: + raise ValueError(f"No available index for output type {output_type.__name__}") + return indices + @rpc def start(self) -> None: super().start() diff --git a/dimos/teleop/devices/vr_teleop_module.py b/dimos/teleop/devices/vr_teleop_module.py index 0728eeaa76..fd1ee0013f 100644 --- a/dimos/teleop/devices/vr_teleop_module.py +++ b/dimos/teleop/devices/vr_teleop_module.py @@ -26,6 +26,7 @@ from typing import Any from dimos_lcm.geometry_msgs import Transform as LCMTransform +from reactivex.disposable import Disposable from dimos.core import In, rpc from dimos.msgs.geometry_msgs import ControllerPose, PoseStamped, Twist @@ -58,11 +59,11 @@ class VRTeleopModule(BaseTeleopModule[VRTeleopConfig]): default_config = VRTeleopConfig # LCM inputs from Deno bridge - vr_left_transform: In[LCMTransform] = None # type: ignore[assignment] - vr_right_transform: In[LCMTransform] = None # type: ignore[assignment] - vr_trigger_0: In[Float32] = None # type: ignore[assignment] - vr_trigger_1: In[Float32] = None # type: ignore[assignment] - teleop_enable: In[Bool] = None # type: ignore[assignment] # X button calibration toggle + vr_left_transform: In[LCMTransform] + vr_right_transform: In[LCMTransform] + vr_trigger_0: In[Float32] + vr_trigger_1: In[Float32] + vr_teleop_enable: In[Bool] # X button calibration toggle def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) @@ -97,11 +98,11 @@ def start(self) -> None: ), (self.vr_trigger_0, lambda msg, idx=left_index: self._on_lcm_trigger(idx, msg)), (self.vr_trigger_1, lambda msg, idx=right_index: self._on_lcm_trigger(idx, msg)), - (self.teleop_enable, self._on_lcm_teleop_enable), + (self.vr_teleop_enable, self._on_lcm_teleop_enable), ] for stream, handler in subscriptions: if stream and stream.transport: - stream.subscribe(handler) # type: ignore[misc, arg-type] + self._disposables.add(Disposable(stream.subscribe(handler))) # type: ignore[misc, arg-type] logger.info("VR Teleoperation Module started") diff --git a/dimos/teleop/teleop_blueprints.py b/dimos/teleop/teleop_blueprints.py index 7b2494f420..e0a021defc 100644 --- a/dimos/teleop/teleop_blueprints.py +++ b/dimos/teleop/teleop_blueprints.py @@ -28,16 +28,7 @@ input_labels=["left_vr", "right_vr"], visualize_in_rerun=True, ), -).transports( - { - ("vr_left_transform", LCMTransform): LCMTransport("/vr_left_transform", LCMTransform), - ("vr_right_transform", LCMTransform): LCMTransport("/vr_right_transform", LCMTransform), - ("vr_trigger_0", Float32): LCMTransport("/vr_trigger_0", Float32), - ("vr_trigger_1", Float32): LCMTransport("/vr_trigger_1", Float32), - ("teleop_enable", Bool): LCMTransport("/vr_teleop_enable", Bool), - } ) -__all__ = [ - "quest3_teleop", -] + +__all__ = ["quest3_teleop"] diff --git a/dimos/utils/teleop_transforms.py b/dimos/utils/teleop_transforms.py index 7c66566b8e..24ab3e93c6 100644 --- a/dimos/utils/teleop_transforms.py +++ b/dimos/utils/teleop_transforms.py @@ -13,7 +13,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Teleop transform utilities for VR coordinate transforms and index mapping.""" +"""Teleop transform utilities for VR coordinate transforms.""" from __future__ import annotations @@ -22,18 +22,10 @@ import numpy as np from scipy.spatial.transform import Rotation as R # type: ignore[import-untyped] -from dimos.msgs.geometry_msgs import PoseStamped, Twist - if TYPE_CHECKING: from dimos_lcm.geometry_msgs import Transform as LCMTransform from numpy.typing import NDArray -# Index mapping: output type โ†’ available indices -OUTPUT_TYPE_INDICES: dict[type, list[int]] = { - PoseStamped: [0, 1], - Twist: [2, 3], -} - # Coordinate frame transformation from VR (WebXR) to robot frame # WebXR: X=right, Y=up, Z=back (towards user) # Robot: X=forward, Y=left, Z=up @@ -48,30 +40,6 @@ ) -def compute_active_indices(output_types: list[type]) -> list[int]: - """Compute active indices from output types. - Example: - [PoseStamped, Twist] โ†’ [0, 2] - [Twist, PoseStamped] โ†’ [2, 0] - [PoseStamped, PoseStamped] โ†’ [0, 1] - [Twist, Twist] โ†’ [2, 3] - """ - indices: list[int] = [] - used_indices: set[int] = set() - - for output_type in output_types: - available = OUTPUT_TYPE_INDICES.get(output_type, []) - for idx in available: - if idx not in used_indices: - indices.append(idx) - used_indices.add(idx) - break - else: - raise ValueError(f"No available index for output type {output_type.__name__}") - - return indices - - def transform_vr_to_robot( transform: LCMTransform, is_left_controller: bool = True, From ba6924efa50d6de448320c2b424011b96ca5998c Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Sat, 17 Jan 2026 00:05:28 -0800 Subject: [PATCH 67/72] Fix: removed PoseConvertible from__sub__ --- dimos/msgs/geometry_msgs/Pose.py | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/dimos/msgs/geometry_msgs/Pose.py b/dimos/msgs/geometry_msgs/Pose.py index 458b2d8633..cb2d5143d9 100644 --- a/dimos/msgs/geometry_msgs/Pose.py +++ b/dimos/msgs/geometry_msgs/Pose.py @@ -222,19 +222,17 @@ def __add__(self, other: Pose | PoseConvertable | LCMTransform | Transform) -> P return Pose(new_position, new_orientation) - def __sub__(self, other: Pose | PoseConvertable) -> Pose: + def __sub__(self, other: Pose) -> Pose: """Compute the delta pose: self - other. For position: simple subtraction. For orientation: delta_quat = self.orientation * inverse(other.orientation) - """ - if isinstance(other, Pose): - other_pose = other - else: - other_pose = Pose(other) - delta_position = self.position - other_pose.position - delta_orientation = self.orientation * other_pose.orientation.inverse() + Returns: + A new Pose representing the delta transformation + """ + delta_position = self.position - other.position + delta_orientation = self.orientation * other.orientation.inverse() return Pose(delta_position, delta_orientation) @classmethod From 31cef1d3443bede40e8302b848d090459afbcfd2 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Sat, 17 Jan 2026 00:07:00 -0800 Subject: [PATCH 68/72] Feat: Added frame_id and ts publishing stamped msgs --- dimos/teleop/devices/base_teleop_module.py | 32 ++++++------ dimos/teleop/devices/vr_teleop_module.py | 41 +++++++++++---- dimos/teleop/teleop_blueprints.py | 4 +- dimos/utils/teleop_transforms.py | 61 ++++++++++++++++++++++ 4 files changed, 110 insertions(+), 28 deletions(-) diff --git a/dimos/teleop/devices/base_teleop_module.py b/dimos/teleop/devices/base_teleop_module.py index 9ecec9cfe4..e1fdb162fe 100644 --- a/dimos/teleop/devices/base_teleop_module.py +++ b/dimos/teleop/devices/base_teleop_module.py @@ -26,12 +26,12 @@ from dimos.core import Module, Out, rpc from dimos.core.module import ModuleConfig -from dimos.msgs.geometry_msgs import ControllerPose, Pose, PoseStamped, Twist +from dimos.msgs.geometry_msgs import Pose, PoseStamped, TwistStamped from dimos.msgs.std_msgs import Bool from dimos.utils.logging_config import setup_logger from dimos.utils.teleop_visualization import ( init_rerun_visualization, - visualize_controller_pose, + visualize_pose, visualize_trigger_value, ) @@ -42,7 +42,7 @@ class BaseTeleopConfig(ModuleConfig): """Base configuration for teleoperation modules. - output_types determines active indices: PoseStampedโ†’0,1, Twistโ†’2,3 + output_types determines active indices: PoseStampedโ†’0,1, TwistStampedโ†’2,3 """ output_types: list[type] = field(default_factory=lambda: [PoseStamped, PoseStamped]) @@ -79,14 +79,14 @@ class BaseTeleopModule(Module[TeleopConfigT]): default_config: type[TeleopConfigT] = BaseTeleopConfig # type: ignore[assignment] - # Output topics: PoseStamped for arms (0,1), Twist for locomotion (2,3) + # Output topics: PoseStamped for arms (0,1), TwistStamped for locomotion (2,3) controller_delta_0: Out[PoseStamped] trigger_value_0: Out[Bool] controller_delta_1: Out[PoseStamped] trigger_value_1: Out[Bool] - controller_delta_2: Out[Twist] + controller_delta_2: Out[TwistStamped] trigger_value_2: Out[Bool] - controller_delta_3: Out[Twist] + controller_delta_3: Out[TwistStamped] trigger_value_3: Out[Bool] def __init__(self, *args: Any, **kwargs: Any) -> None: @@ -108,16 +108,16 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: # Index mapping: output type โ†’ available indices self._output_type_indices: dict[type, list[int]] = { PoseStamped: [0, 1], - Twist: [2, 3], + TwistStamped: [2, 3], } self._active_indices = self._compute_active_indices(self.config.output_types) - self._initial_controller_poses: dict[int, ControllerPose | None] = { + self._initial_controller_poses: dict[int, Pose | None] = { i: None for i in self._active_indices } self._initial_robot_poses: dict[int, Pose | None] = {i: None for i in self._active_indices} - self._current_controller_poses: dict[int, ControllerPose | None] = { + self._current_controller_poses: dict[int, Pose | None] = { i: None for i in self._active_indices } self._all_trigger_values: dict[int, float] = {i: 0.0 for i in self._active_indices} @@ -138,10 +138,10 @@ def _get_label(self, index: int) -> str: def _compute_active_indices(self, output_types: list[type]) -> list[int]: """Compute active indices from output types. Example: - [PoseStamped, Twist] โ†’ [0, 2] - [Twist, PoseStamped] โ†’ [2, 0] + [PoseStamped, TwistStamped] โ†’ [0, 2] + [TwistStamped, PoseStamped] โ†’ [2, 0] [PoseStamped, PoseStamped] โ†’ [0, 1] - [Twist, Twist] โ†’ [2, 3] + [TwistStamped, TwistStamped] โ†’ [2, 3] """ indices: list[int] = [] used_indices: set[int] = set() @@ -261,9 +261,9 @@ def get_status(self) -> dict[str, Any]: def compute_deltas( self, - controller_poses: dict[int, ControllerPose | None], + controller_poses: dict[int, Pose | None], controller_trigger_values: dict[int, float], - ) -> dict[int, ControllerPose | None]: + ) -> dict[int, Pose | None]: """Compute delta = current_pose - initial_pose for each controller.""" self._tracking_msg_count += 1 self._current_controller_poses = controller_poses @@ -275,7 +275,7 @@ def compute_deltas( return {i: None for i in self._active_indices} self._publish_count += 1 - deltas: dict[int, ControllerPose | None] = {} + deltas: dict[int, Pose | None] = {} for i in self._active_indices: current_pose = self._current_controller_poses.get(i) @@ -291,7 +291,7 @@ def compute_deltas( delta_pose = current_pose - initial_pose if self.config.visualize_in_rerun: label = self._get_label(i) - visualize_controller_pose(current_pose, label) + visualize_pose(current_pose, label) visualize_trigger_value(self._all_trigger_values.get(i, 0.0), label) deltas[i] = delta_pose diff --git a/dimos/teleop/devices/vr_teleop_module.py b/dimos/teleop/devices/vr_teleop_module.py index fd1ee0013f..6e90cbbf67 100644 --- a/dimos/teleop/devices/vr_teleop_module.py +++ b/dimos/teleop/devices/vr_teleop_module.py @@ -23,19 +23,26 @@ from dataclasses import dataclass, field import threading import time -from typing import Any +from typing import TYPE_CHECKING, Any from dimos_lcm.geometry_msgs import Transform as LCMTransform from reactivex.disposable import Disposable from dimos.core import In, rpc -from dimos.msgs.geometry_msgs import ControllerPose, PoseStamped, Twist +from dimos.msgs.geometry_msgs import PoseStamped, TwistStamped from dimos.msgs.std_msgs import Bool, Float32 from dimos.teleop.devices.base_teleop_module import BaseTeleopConfig, BaseTeleopModule from dimos.utils.logging_config import setup_logger -from dimos.utils.teleop_transforms import transform_vr_to_robot +from dimos.utils.teleop_transforms import ( + pose_to_pose_stamped, + pose_to_twist_stamped, + transform_vr_to_robot, +) from dimos.utils.transform_utils import matrix_to_pose +if TYPE_CHECKING: + from dimos.msgs.geometry_msgs.Pose import Pose + logger = setup_logger() @@ -69,9 +76,9 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) self._lcm_lock = threading.Lock() - self._lcm_controller_poses: dict[int, ControllerPose | None] = { - i: None for i in self._active_indices - } + self._lcm_controller_poses: dict[int, Pose | None] = {i: None for i in self._active_indices} + self._lcm_receive_times: dict[int, float] = {i: 0.0 for i in self._active_indices} + self._lcm_frame_ids: dict[int, str] = {i: "" for i in self._active_indices} self._lcm_gripper_values: dict[int, float] = {i: 0.0 for i in self._active_indices} self._control_loop_thread: threading.Thread | None = None self._control_loop_running = False @@ -136,12 +143,16 @@ def _on_lcm_teleop_enable(self, msg: Bool) -> None: def _on_lcm_transform(self, index: int, transform: LCMTransform) -> None: """Handle controller transform, converting WebXR to robot frame.""" + receive_time = time.time() self._start_control_loop() is_left = index == self._active_indices[0] + frame_id = "left_controller" if is_left else "right_controller" transform_matrix = transform_vr_to_robot(transform, is_left_controller=is_left) - pose = ControllerPose.from_pose(matrix_to_pose(transform_matrix)) + pose = matrix_to_pose(transform_matrix) with self._lcm_lock: self._lcm_controller_poses[index] = pose + self._lcm_receive_times[index] = receive_time + self._lcm_frame_ids[index] = frame_id def _on_lcm_trigger(self, index: int, msg: Float32) -> None: """Handle trigger value for gripper control.""" @@ -180,6 +191,8 @@ def _control_loop(self) -> None: with self._lcm_lock: controller_poses = dict(self._lcm_controller_poses) controller_trigger_values = dict(self._lcm_gripper_values) + receive_times = dict(self._lcm_receive_times) + frame_ids = dict(self._lcm_frame_ids) deltas = self.compute_deltas(controller_poses, controller_trigger_values) @@ -190,18 +203,26 @@ def _control_loop(self) -> None: trigger_value = self._all_trigger_values.get(i, 0.0) trigger_bool = Bool(data=trigger_value > self.config.gripper_threshold) + ts = receive_times.get(i, time.time()) + frame_id = frame_ids.get(i, "") output_type = self.config.output_types[idx] if output_type == PoseStamped: - command = delta.to_pose_stamped( + command = pose_to_pose_stamped( + delta, initial_robot_pose=self._initial_robot_poses.get(i), + ts=ts, + frame_id=frame_id, ) - elif output_type == Twist: - command = delta.to_twist( + elif output_type == TwistStamped: + command = pose_to_twist_stamped( + delta, linear_scale=self.config.linear_scale, angular_scale=self.config.angular_scale, max_linear=self.config.max_linear_velocity, max_angular=self.config.max_angular_velocity, + ts=ts, + frame_id=frame_id, ) else: continue diff --git a/dimos/teleop/teleop_blueprints.py b/dimos/teleop/teleop_blueprints.py index e0a021defc..22f908c4d5 100644 --- a/dimos/teleop/teleop_blueprints.py +++ b/dimos/teleop/teleop_blueprints.py @@ -18,13 +18,13 @@ from dimos.core.blueprints import autoconnect from dimos.core.transport import LCMTransport -from dimos.msgs.geometry_msgs import PoseStamped, Twist +from dimos.msgs.geometry_msgs import PoseStamped, TwistStamped from dimos.msgs.std_msgs import Bool, Float32 from dimos.teleop.devices.vr_teleop_module import vr_teleop_module quest3_teleop = autoconnect( vr_teleop_module( - output_types=[PoseStamped, Twist], + output_types=[PoseStamped, TwistStamped], input_labels=["left_vr", "right_vr"], visualize_in_rerun=True, ), diff --git a/dimos/utils/teleop_transforms.py b/dimos/utils/teleop_transforms.py index 24ab3e93c6..4ffdfb79a0 100644 --- a/dimos/utils/teleop_transforms.py +++ b/dimos/utils/teleop_transforms.py @@ -22,10 +22,16 @@ import numpy as np from scipy.spatial.transform import Rotation as R # type: ignore[import-untyped] +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped +from dimos.msgs.geometry_msgs.Vector3 import Vector3 + if TYPE_CHECKING: from dimos_lcm.geometry_msgs import Transform as LCMTransform from numpy.typing import NDArray + from dimos.msgs.geometry_msgs.Pose import Pose + # Coordinate frame transformation from VR (WebXR) to robot frame # WebXR: X=right, Y=up, Z=back (towards user) # Robot: X=forward, Y=left, Z=up @@ -71,3 +77,58 @@ def transform_vr_to_robot( # Apply VR to robot frame transformation return VR_TO_ROBOT_FRAME @ vr_matrix + + +def pose_to_pose_stamped( + delta_pose: Pose, + initial_robot_pose: Pose | None = None, + frame_id: str = "None", + ts: float | None = None, +) -> PoseStamped: + """Convert delta pose to PoseStamped (target = initial + delta).""" + if initial_robot_pose is not None: + target_pose = initial_robot_pose + delta_pose + else: + target_pose = delta_pose + + return PoseStamped( + ts=ts if ts is not None else 0.0, + frame_id=frame_id, + position=target_pose.position, + orientation=target_pose.orientation, + ) + + +def pose_to_twist_stamped( + delta_pose: Pose, + linear_scale: float = 1.0, + angular_scale: float = 1.0, + max_linear: float = 0.5, + max_angular: float = 1.0, + frame_id: str = "", + ts: float | None = None, +) -> TwistStamped: + """Convert delta pose to TwistStamped velocity command.""" + + def clamp(value: float, limit: float) -> float: + return max(-limit, min(limit, value)) + + linear = Vector3( + clamp(delta_pose.position.x * linear_scale, max_linear), + clamp(delta_pose.position.y * linear_scale, max_linear), + clamp(delta_pose.position.z * linear_scale, max_linear), + ) + + euler = delta_pose.orientation.to_euler() + angular = Vector3( + clamp(euler.x * angular_scale, max_angular), + clamp(euler.y * angular_scale, max_angular), + clamp(euler.z * angular_scale, max_angular), + ) + + return TwistStamped( + ts=ts if ts is not None else 0.0, + frame_id=frame_id, + linear=linear, + angular=angular, + ) From fc85862add237199e1a6bc22667e891ce5393a36 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Sat, 17 Jan 2026 00:09:23 -0800 Subject: [PATCH 69/72] Misc: updates after msg type changes --- dimos/msgs/geometry_msgs/ControllerPose.py | 112 --------------------- dimos/msgs/geometry_msgs/__init__.py | 2 - dimos/teleop/README.md | 14 +-- dimos/teleop/devices/README.md | 14 +-- dimos/utils/teleop_visualization.py | 12 ++- 5 files changed, 21 insertions(+), 133 deletions(-) delete mode 100644 dimos/msgs/geometry_msgs/ControllerPose.py diff --git a/dimos/msgs/geometry_msgs/ControllerPose.py b/dimos/msgs/geometry_msgs/ControllerPose.py deleted file mode 100644 index 3fb6570453..0000000000 --- a/dimos/msgs/geometry_msgs/ControllerPose.py +++ /dev/null @@ -1,112 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Controller pose with conversion utilities for teleop output types.""" - -from __future__ import annotations - -import time - -from dimos.msgs.geometry_msgs.Pose import Pose, PoseConvertable -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 - - -class ControllerPose(Pose): - """Controller pose with conversion utilities for teleop output types. - - Extends Pose with methods to convert to PoseStamped or Twist commands. - """ - - @classmethod - def from_pose(cls, pose: Pose) -> ControllerPose: - """Create ControllerPose from a Pose. - - Args: - pose: Pose to convert. - - Returns: - ControllerPose with same position and orientation. - """ - return cls(position=pose.position, orientation=pose.orientation) - - def to_pose_stamped( - self, - initial_robot_pose: Pose | None = None, - frame_id: str = "teleop_target", - ) -> PoseStamped: - """Convert to PoseStamped (target = initial + delta). - - Args: - initial_robot_pose: Initial robot pose to add delta to. - If None, delta is used directly as target. - frame_id: Frame ID for the PoseStamped message. - - Returns: - PoseStamped target pose. - """ - if initial_robot_pose is not None: - target_pose = initial_robot_pose + self - else: - target_pose = self - - return PoseStamped( - ts=time.time(), - frame_id=frame_id, - position=target_pose.position, - orientation=target_pose.orientation, - ) - - def to_twist( - self, - linear_scale: float = 1.0, - angular_scale: float = 1.0, - max_linear: float = 0.5, - max_angular: float = 1.0, - ) -> Twist: - """Convert to Twist velocity command. - - Args: - linear_scale: Scale factor for linear velocity. - angular_scale: Scale factor for angular velocity. - max_linear: Maximum linear velocity clamp. - max_angular: Maximum angular velocity clamp. - - Returns: - Twist velocity command. - """ - - def clamp(value: float, limit: float) -> float: - return max(-limit, min(limit, value)) - - linear = Vector3( - clamp(self.position.x * linear_scale, max_linear), - clamp(self.position.y * linear_scale, max_linear), - clamp(self.position.z * linear_scale, max_linear), - ) - - euler = self.orientation.to_euler() - angular = Vector3( - clamp(euler.x * angular_scale, max_angular), - clamp(euler.y * angular_scale, max_angular), - clamp(euler.z * angular_scale, max_angular), - ) - - return Twist(linear=linear, angular=angular) - - def __sub__(self, other: Pose | PoseConvertable) -> ControllerPose: # type: ignore[override] - """Compute delta pose, returning ControllerPose instead of Pose.""" - result = super().__sub__(other) - return ControllerPose.from_pose(result) diff --git a/dimos/msgs/geometry_msgs/__init__.py b/dimos/msgs/geometry_msgs/__init__.py index f7a53d9fa4..fd47d5f0ed 100644 --- a/dimos/msgs/geometry_msgs/__init__.py +++ b/dimos/msgs/geometry_msgs/__init__.py @@ -1,4 +1,3 @@ -from dimos.msgs.geometry_msgs.ControllerPose import ControllerPose from dimos.msgs.geometry_msgs.Pose import Pose, PoseLike, to_pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance @@ -14,7 +13,6 @@ from dimos.msgs.geometry_msgs.WrenchStamped import WrenchStamped __all__ = [ - "ControllerPose", "Pose", "PoseLike", "PoseStamped", diff --git a/dimos/teleop/README.md b/dimos/teleop/README.md index 543c3207d1..cf126afc78 100644 --- a/dimos/teleop/README.md +++ b/dimos/teleop/README.md @@ -45,7 +45,7 @@ teleop/ โ”Œโ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”ดโ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ” โ–ผ โ–ผ controller_delta_0/1 controller_delta_2/3 - (PoseStamped) (Twist) + (PoseStamped) (TwistStamped) target = init + ฮ” vel = scale(ฮ”) ``` @@ -67,18 +67,18 @@ The `output_types` parameter determines how each controller input is mapped: | Output Type | Indices | Use Case | |-------------|---------|----------| | `PoseStamped` | 0, 1 | Manipulators, end-effector control | -| `Twist` | 2, 3 | Locomotion, velocity control | +| `TwistStamped` | 2, 3 | Locomotion, velocity control | The system auto-computes active indices from output types: - `[PoseStamped, PoseStamped]` โ†’ indices `[0, 1]` (dual arm) -- `[Twist, Twist]` โ†’ indices `[2, 3]` (dual locomotion) -- `[PoseStamped, Twist]` โ†’ indices `[0, 2]` (arm + quadruped) +- `[TwistStamped, TwistStamped]` โ†’ indices `[2, 3]` (dual locomotion) +- `[PoseStamped, TwistStamped]` โ†’ indices `[0, 2]` (arm + quadruped) ## Custom Setup ```python from dimos.teleop.devices import vr_teleop_module -from dimos.msgs.geometry_msgs import PoseStamped, Twist +from dimos.msgs.geometry_msgs import PoseStamped, TwistStamped from dimos.core.blueprints import autoconnect # Dual arm setup (both controllers โ†’ PoseStamped) @@ -89,10 +89,10 @@ dual_arm = autoconnect( ), ) -# Arm + Quadruped (left โ†’ PoseStamped index 0, right โ†’ Twist index 2) +# Arm + Quadruped (left โ†’ PoseStamped index 0, right โ†’ TwistStamped index 2) arm_and_quad = autoconnect( vr_teleop_module( - output_types=[PoseStamped, Twist], + output_types=[PoseStamped, TwistStamped], input_labels=["left_arm", "right_quad"], ), ) diff --git a/dimos/teleop/devices/README.md b/dimos/teleop/devices/README.md index 2049dca040..d82d18c27d 100644 --- a/dimos/teleop/devices/README.md +++ b/dimos/teleop/devices/README.md @@ -9,7 +9,7 @@ Input device modules that capture tracking data, compute deltas, and transform t Abstract base class providing: - Multi-controller calibration (capture initial poses) - Delta computation (current - initial) -- Transform to robot commands (PoseStamped or Twist based on `output_types`) +- Transform to robot commands (PoseStamped or TwistStamped based on `output_types`) - Publishing to `controller_delta_*` outputs - Optional RPC calls for initial robot pose - Rerun visualization @@ -47,7 +47,7 @@ LCM Input (Transform) โ–ผ โ”Œโ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ” โ”‚ Transform โ”‚ PoseStamped: target = initial_robot + delta -โ”‚ โ”‚ Twist: velocity = scale(delta) +โ”‚ โ”‚ TwistStamped: velocity = scale(delta) โ””โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”˜ โ”‚ โ–ผ @@ -58,12 +58,12 @@ LCM Input (Transform) ```python from dimos.teleop.devices import VRTeleopConfig -from dimos.msgs.geometry_msgs import PoseStamped, Twist +from dimos.msgs.geometry_msgs import PoseStamped, TwistStamped config = VRTeleopConfig( # Output types determine active indices: - # PoseStamped โ†’ indices 0,1 | Twist โ†’ indices 2,3 - output_types=[PoseStamped, Twist], + # PoseStamped โ†’ indices 0,1 | TwistStamped โ†’ indices 2,3 + output_types=[PoseStamped, TwistStamped], input_labels=["left_vr", "right_vr"], # Optional RPC methods for initial robot pose (per output) @@ -91,8 +91,8 @@ The `output_types` parameter auto-computes active indices: | Configuration | Active Indices | Outputs | |--------------|----------------|---------| | `[PoseStamped, PoseStamped]` | `[0, 1]` | Dual arm | -| `[Twist, Twist]` | `[2, 3]` | Dual locomotion | -| `[PoseStamped, Twist]` | `[0, 2]` | Arm + quadruped | +| `[TwistStamped, TwistStamped]` | `[2, 3]` | Dual locomotion | +| `[PoseStamped, TwistStamped]` | `[0, 2]` | Arm + quadruped | ## Adding New Devices diff --git a/dimos/utils/teleop_visualization.py b/dimos/utils/teleop_visualization.py index d0bb7ee767..9690be9db1 100644 --- a/dimos/utils/teleop_visualization.py +++ b/dimos/utils/teleop_visualization.py @@ -22,7 +22,7 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from dimos.msgs.geometry_msgs import ControllerPose + from dimos.msgs.geometry_msgs import Pose logger = setup_logger() @@ -49,18 +49,20 @@ def init_rerun_visualization() -> bool: return False -def visualize_controller_pose(controller_pose: ControllerPose, controller_label: str) -> None: +def visualize_pose(pose: Pose, controller_label: str) -> None: """Visualize controller absolute pose in Rerun. Args: - controller_pose: The controller's current pose. + pose: The controller's current pose. controller_label: Label for the controller (e.g., "left_vr"). """ if not RERUN_AVAILABLE: return try: - pose_stamped = controller_pose.to_pose_stamped( - frame_id=f"world/teleop/{controller_label}_controller" + from dimos.utils.teleop_transforms import pose_to_pose_stamped + + pose_stamped = pose_to_pose_stamped( + pose, frame_id=f"world/teleop/{controller_label}_controller" ) rr.log( f"world/teleop/{controller_label}_controller", From af8c25c1976e9187bd4278303968bb42480472ec Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 21 Jan 2026 16:13:13 -0800 Subject: [PATCH 70/72] Misc: file location changes --- dimos/teleop/devices/base_teleop_module.py | 2 +- dimos/teleop/devices/vr_teleop_module.py | 2 +- dimos/{ => teleop}/utils/teleop_transforms.py | 0 dimos/{ => teleop}/utils/teleop_visualization.py | 2 +- 4 files changed, 3 insertions(+), 3 deletions(-) rename dimos/{ => teleop}/utils/teleop_transforms.py (100%) rename dimos/{ => teleop}/utils/teleop_visualization.py (97%) diff --git a/dimos/teleop/devices/base_teleop_module.py b/dimos/teleop/devices/base_teleop_module.py index e1fdb162fe..6a8082a7f0 100644 --- a/dimos/teleop/devices/base_teleop_module.py +++ b/dimos/teleop/devices/base_teleop_module.py @@ -29,7 +29,7 @@ from dimos.msgs.geometry_msgs import Pose, PoseStamped, TwistStamped from dimos.msgs.std_msgs import Bool from dimos.utils.logging_config import setup_logger -from dimos.utils.teleop_visualization import ( +from dimos.teleop.utils.teleop_visualization import ( init_rerun_visualization, visualize_pose, visualize_trigger_value, diff --git a/dimos/teleop/devices/vr_teleop_module.py b/dimos/teleop/devices/vr_teleop_module.py index 6e90cbbf67..1ce64a81a2 100644 --- a/dimos/teleop/devices/vr_teleop_module.py +++ b/dimos/teleop/devices/vr_teleop_module.py @@ -33,7 +33,7 @@ from dimos.msgs.std_msgs import Bool, Float32 from dimos.teleop.devices.base_teleop_module import BaseTeleopConfig, BaseTeleopModule from dimos.utils.logging_config import setup_logger -from dimos.utils.teleop_transforms import ( +from dimos.teleop.utils.teleop_transforms import ( pose_to_pose_stamped, pose_to_twist_stamped, transform_vr_to_robot, diff --git a/dimos/utils/teleop_transforms.py b/dimos/teleop/utils/teleop_transforms.py similarity index 100% rename from dimos/utils/teleop_transforms.py rename to dimos/teleop/utils/teleop_transforms.py diff --git a/dimos/utils/teleop_visualization.py b/dimos/teleop/utils/teleop_visualization.py similarity index 97% rename from dimos/utils/teleop_visualization.py rename to dimos/teleop/utils/teleop_visualization.py index 9690be9db1..d95e2416f9 100644 --- a/dimos/utils/teleop_visualization.py +++ b/dimos/teleop/utils/teleop_visualization.py @@ -59,7 +59,7 @@ def visualize_pose(pose: Pose, controller_label: str) -> None: if not RERUN_AVAILABLE: return try: - from dimos.utils.teleop_transforms import pose_to_pose_stamped + from dimos.teleop.utils.teleop_transforms import pose_to_pose_stamped pose_stamped = pose_to_pose_stamped( pose, frame_id=f"world/teleop/{controller_label}_controller" From a7f7f3e55b9173837f77d6ed834c8061003cbb30 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam <63036454+ruthwikdasyam@users.noreply.github.com> Date: Thu, 22 Jan 2026 00:13:57 +0000 Subject: [PATCH 71/72] CI code cleanup --- dimos/teleop/devices/base_teleop_module.py | 2 +- dimos/teleop/devices/vr_teleop_module.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dimos/teleop/devices/base_teleop_module.py b/dimos/teleop/devices/base_teleop_module.py index 6a8082a7f0..4c6a10631e 100644 --- a/dimos/teleop/devices/base_teleop_module.py +++ b/dimos/teleop/devices/base_teleop_module.py @@ -28,12 +28,12 @@ from dimos.core.module import ModuleConfig from dimos.msgs.geometry_msgs import Pose, PoseStamped, TwistStamped from dimos.msgs.std_msgs import Bool -from dimos.utils.logging_config import setup_logger from dimos.teleop.utils.teleop_visualization import ( init_rerun_visualization, visualize_pose, visualize_trigger_value, ) +from dimos.utils.logging_config import setup_logger logger = setup_logger() diff --git a/dimos/teleop/devices/vr_teleop_module.py b/dimos/teleop/devices/vr_teleop_module.py index 1ce64a81a2..e20acb47a0 100644 --- a/dimos/teleop/devices/vr_teleop_module.py +++ b/dimos/teleop/devices/vr_teleop_module.py @@ -32,12 +32,12 @@ from dimos.msgs.geometry_msgs import PoseStamped, TwistStamped from dimos.msgs.std_msgs import Bool, Float32 from dimos.teleop.devices.base_teleop_module import BaseTeleopConfig, BaseTeleopModule -from dimos.utils.logging_config import setup_logger from dimos.teleop.utils.teleop_transforms import ( pose_to_pose_stamped, pose_to_twist_stamped, transform_vr_to_robot, ) +from dimos.utils.logging_config import setup_logger from dimos.utils.transform_utils import matrix_to_pose if TYPE_CHECKING: From 64aeb4a08f4441429c5db9bd7f475a7962c0e5bb Mon Sep 17 00:00:00 2001 From: ruthwikdasyam <63036454+ruthwikdasyam@users.noreply.github.com> Date: Thu, 22 Jan 2026 00:32:46 +0000 Subject: [PATCH 72/72] CI code cleanup --- .gitignore | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index e52822ef6a..545a889dec 100644 --- a/.gitignore +++ b/.gitignore @@ -67,4 +67,4 @@ yolo11n.pt /results # Teleop SSL certificates -assets/teleop_certs/ \ No newline at end of file +assets/teleop_certs/