diff --git a/.gitignore b/.gitignore index e7bcd25310..686327e990 100644 --- a/.gitignore +++ b/.gitignore @@ -65,3 +65,5 @@ yolo11n.pt *mobileclip* /results + +/assets/teleop_certs/ diff --git a/dimos/msgs/geometry_msgs/Pose.py b/dimos/msgs/geometry_msgs/Pose.py index bf6a821cc8..cb2d5143d9 100644 --- a/dimos/msgs/geometry_msgs/Pose.py +++ b/dimos/msgs/geometry_msgs/Pose.py @@ -222,6 +222,19 @@ def __add__(self, other: Pose | PoseConvertable | LCMTransform | Transform) -> P return Pose(new_position, new_orientation) + 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) + + 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 def from_ros_msg(cls, ros_msg: ROSPose) -> Pose: """Create a Pose from a ROS geometry_msgs/Pose message. diff --git a/dimos/msgs/std_msgs/UInt32.py b/dimos/msgs/std_msgs/UInt32.py new file mode 100644 index 0000000000..e617c782fe --- /dev/null +++ b/dimos/msgs/std_msgs/UInt32.py @@ -0,0 +1,30 @@ +#!/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. + +"""UInt32 message type.""" + +from typing import ClassVar + +from dimos_lcm.std_msgs import UInt32 as LCMUInt32 + + +class UInt32(LCMUInt32): # type: ignore[misc] + """ROS-compatible UInt32 message.""" + + msg_name: ClassVar[str] = "std_msgs.UInt32" + + def __init__(self, data: int = 0) -> None: + """Initialize UInt32 with data value.""" + self.data = data diff --git a/dimos/msgs/std_msgs/__init__.py b/dimos/msgs/std_msgs/__init__.py index 9002b8c4ef..ae8e3dd8f6 100644 --- a/dimos/msgs/std_msgs/__init__.py +++ b/dimos/msgs/std_msgs/__init__.py @@ -16,5 +16,6 @@ from .Header import Header from .Int8 import Int8 from .Int32 import Int32 +from .UInt32 import UInt32 -__all__ = ["Bool", "Header", "Int8", "Int32"] +__all__ = ["Bool", "Header", "Int8", "Int32", "UInt32"] diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 587b07348a..cf737ffb06 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -16,6 +16,8 @@ # Run `pytest dimos/robot/test_all_blueprints_generation.py` to regenerate. all_blueprints = { + "arm-teleop": "dimos.teleop.blueprints:arm_teleop", + "arm-teleop-visualizing": "dimos.teleop.blueprints:arm_teleop_visualizing", "coordinator-basic": "dimos.control.blueprints:coordinator_basic", "coordinator-cartesian-ik-mock": "dimos.control.blueprints:coordinator_cartesian_ik_mock", "coordinator-cartesian-ik-piper": "dimos.control.blueprints:coordinator_cartesian_ik_piper", @@ -65,6 +67,7 @@ all_modules = { + "arm_teleop_module": "dimos.teleop.quest.quest_extensions", "camera_module": "dimos.hardware.sensors.camera.module", "cartesian_motion_controller": "dimos.manipulation.control.servo_control.cartesian_motion_controller", "control_coordinator": "dimos.control.coordinator", @@ -94,6 +97,7 @@ "osm_skill": "dimos.agents.skills.osm", "person_follow_skill": "dimos.agents.skills.person_follow", "person_tracker_module": "dimos.perception.detection.person_tracker", + "quest_teleop_module": "dimos.teleop.quest.quest_teleop_module", "realsense_camera": "dimos.hardware.sensors.camera.realsense.camera", "replanning_a_star_planner": "dimos.navigation.replanning_a_star.module", "rerun_scene_wiring": "dimos.dashboard.rerun_scene_wiring", @@ -101,8 +105,10 @@ "spatial_memory": "dimos.perception.spatial_perception", "speak_skill": "dimos.agents.skills.speak_skill", "temporal_memory": "dimos.perception.experimental.temporal_memory.temporal_memory", + "twist_teleop_module": "dimos.teleop.quest.quest_extensions", "unitree_skills": "dimos.robot.unitree_webrtc.unitree_skill_container", "utilization": "dimos.utils.monitoring", + "visualizing_teleop_module": "dimos.teleop.quest.quest_extensions", "vlm_agent": "dimos.agents.vlm_agent", "vlm_stream_tester": "dimos.agents.vlm_stream_tester", "voxel_mapper": "dimos.mapping.voxels", diff --git a/dimos/teleop/README.md b/dimos/teleop/README.md new file mode 100644 index 0000000000..6dd544f34c --- /dev/null +++ b/dimos/teleop/README.md @@ -0,0 +1,76 @@ +# Teleop Stack + +Teleoperation modules for DimOS. Currently supports Meta Quest 3 VR controllers. + +## Architecture + +``` +Quest Browser (WebXR) + │ + │ PoseStamped + Joy via WebSocket + ▼ +Deno Bridge (teleop_server.ts) + │ + │ LCM topics + ▼ +QuestTeleopModule + │ WebXR → robot frame transform + │ Pose computation + button state packing + ▼ +PoseStamped / TwistStamped / QuestButtons outputs +``` + +## Modules + +### QuestTeleopModule +Base teleop module. Gets controller data, computes output poses, and publishes them. Default engage: hold primary button (X/A). Subclass to customize. + +### ArmTeleopModule +Toggle-based engage — press primary button once to engage, press again to disengage. + +### TwistTeleopModule +Outputs TwistStamped (linear + angular velocity) instead of PoseStamped. + +### VisualizingTeleopModule +Adds Rerun visualization for debugging. Extends ArmTeleopModule (toggle engage). + +## Subclassing + +`QuestTeleopModule` is designed for extension. Override these methods: + +| Method | Purpose | +|--------|---------| +| `_handle_engage()` | Customize engage/disengage logic | +| `_should_publish()` | Add conditions for when to publish | +| `_get_output_pose()` | Customize pose computation | +| `_publish_msg()` | Change output format | +| `_publish_button_state()` | Change button output | + +### Rules for subclasses + +- **Do not acquire `self._lock` in overrides.** The control loop already holds it. + Access `self._controllers`, `self._current_poses`, `self._is_engaged`, etc. directly. +- **Keep overrides fast** — they run inside the control loop at `control_loop_hz`. + +## File Structure + +``` +teleop/ +├── base/ +│ └── teleop_protocol.py # TeleopProtocol interface +├── quest/ +│ ├── quest_teleop_module.py # Base Quest teleop module +│ ├── quest_extensions.py # ArmTeleop, TwistTeleop, VisualizingTeleop +│ ├── quest_types.py # QuestControllerState, QuestButtons +│ └── web/ # Deno bridge + WebXR client +│ ├── teleop_server.ts +│ └── static/index.html +├── utils/ +│ ├── teleop_transforms.py # WebXR → robot frame math +│ └── teleop_visualization.py # Rerun visualization helpers +└── blueprints.py # Module blueprints for easy instantiation +``` + +## Quick Start + +See [Quest Web README](quest/web/README.md) for running the Deno bridge and connecting the Quest headset. diff --git a/dimos/teleop/__init__.py b/dimos/teleop/__init__.py new file mode 100644 index 0000000000..a8c3c0b21a --- /dev/null +++ b/dimos/teleop/__init__.py @@ -0,0 +1,19 @@ +# 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 TeleopProtocol + +__all__ = ["TeleopProtocol"] diff --git a/dimos/teleop/base/__init__.py b/dimos/teleop/base/__init__.py new file mode 100644 index 0000000000..cf3b18d597 --- /dev/null +++ b/dimos/teleop/base/__init__.py @@ -0,0 +1,19 @@ +# 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 protocol.""" + +from dimos.teleop.base.teleop_protocol import TeleopProtocol + +__all__ = ["TeleopProtocol"] diff --git a/dimos/teleop/base/teleop_protocol.py b/dimos/teleop/base/teleop_protocol.py new file mode 100644 index 0000000000..9e1647d64d --- /dev/null +++ b/dimos/teleop/base/teleop_protocol.py @@ -0,0 +1,58 @@ +#!/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. + +"""Teleoperation specifications: Protocol. + +Defines the interface that all teleoperation modules must implement. +No implementation - just method signatures. +""" + +from typing import Any, Protocol, runtime_checkable + +# ============================================================================ +# TELEOP PROTOCOL +# ============================================================================ + + +@runtime_checkable +class TeleopProtocol(Protocol): + """Protocol defining the teleoperation interface. + + All teleop modules (Quest, keyboard, joystick, etc.) should implement these methods. + No state or implementation here - just the contract. + """ + + # --- Lifecycle --- + + def start(self) -> None: + """Start the teleoperation module.""" + ... + + def stop(self) -> None: + """Stop the teleoperation module.""" + ... + + # --- Engage / Disengage --- + + def engage(self, hand: Any = None) -> bool: + """Engage teleoperation. Hand type is device-specific (e.g., Hand enum for Quest).""" + ... + + def disengage(self, hand: Any = None) -> None: + """Disengage teleoperation. Hand type is device-specific.""" + ... + + +__all__ = ["TeleopProtocol"] diff --git a/dimos/teleop/blueprints.py b/dimos/teleop/blueprints.py new file mode 100644 index 0000000000..bca504b1d8 --- /dev/null +++ b/dimos/teleop/blueprints.py @@ -0,0 +1,51 @@ +#!/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 blueprints for testing and deployment.""" + +from dimos.core.blueprints import autoconnect +from dimos.core.transport import LCMTransport +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.teleop.quest.quest_extensions import arm_teleop_module, visualizing_teleop_module +from dimos.teleop.quest.quest_types import QuestButtons + +# ----------------------------------------------------------------------------- +# Quest Teleop Blueprints +# ----------------------------------------------------------------------------- + +# Arm teleop with toggle-based engage +arm_teleop = autoconnect( + arm_teleop_module(), +).transports( + { + ("left_controller_output", PoseStamped): LCMTransport("/teleop/left_delta", PoseStamped), + ("right_controller_output", PoseStamped): LCMTransport("/teleop/right_delta", PoseStamped), + ("buttons", QuestButtons): LCMTransport("/teleop/buttons", QuestButtons), + } +) + +# Arm teleop with Rerun visualization +arm_teleop_visualizing = autoconnect( + visualizing_teleop_module(), +).transports( + { + ("left_controller_output", PoseStamped): LCMTransport("/teleop/left_delta", PoseStamped), + ("right_controller_output", PoseStamped): LCMTransport("/teleop/right_delta", PoseStamped), + ("buttons", QuestButtons): LCMTransport("/teleop/buttons", QuestButtons), + } +) + + +__all__ = ["arm_teleop", "arm_teleop_visualizing"] diff --git a/dimos/teleop/quest/__init__.py b/dimos/teleop/quest/__init__.py new file mode 100644 index 0000000000..3f3cf50e37 --- /dev/null +++ b/dimos/teleop/quest/__init__.py @@ -0,0 +1,54 @@ +# 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. + +"""Quest teleoperation module.""" + +from dimos.teleop.quest.quest_extensions import ( + ArmTeleopModule, + TwistTeleopModule, + VisualizingTeleopModule, + arm_teleop_module, + twist_teleop_module, + visualizing_teleop_module, +) +from dimos.teleop.quest.quest_teleop_module import ( + Hand, + QuestTeleopConfig, + QuestTeleopModule, + QuestTeleopStatus, + quest_teleop_module, +) +from dimos.teleop.quest.quest_types import ( + QuestButtons, + QuestControllerState, + ThumbstickState, +) + +__all__ = [ + "ArmTeleopModule", + "Hand", + "QuestButtons", + "QuestControllerState", + "QuestTeleopConfig", + "QuestTeleopModule", + "QuestTeleopStatus", + "ThumbstickState", + "TwistTeleopModule", + "VisualizingTeleopModule", + # Blueprints + "arm_teleop_module", + "quest_teleop_module", + "twist_teleop_module", + "visualizing_teleop_module", +] diff --git a/dimos/teleop/quest/quest_extensions.py b/dimos/teleop/quest/quest_extensions.py new file mode 100644 index 0000000000..f82d8db28f --- /dev/null +++ b/dimos/teleop/quest/quest_extensions.py @@ -0,0 +1,163 @@ +#!/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. + +"""Quest teleop module extensions and subclasses. + +Available subclasses: + - ArmTeleopModule: Per-hand toggle engage (X/A press to toggle) + - TwistTeleopModule: Outputs Twist instead of PoseStamped + - VisualizingTeleopModule: Adds Rerun visualization (uses toggle engage) +""" + +from dataclasses import dataclass +from typing import Any + +from dimos.core import Out, rpc +from dimos.msgs.geometry_msgs import PoseStamped, TwistStamped +from dimos.teleop.quest.quest_teleop_module import Hand, QuestTeleopConfig, QuestTeleopModule +from dimos.teleop.utils.teleop_visualization import ( + init_rerun_visualization, + visualize_buttons, + visualize_pose, +) + + +@dataclass +class TwistTeleopConfig(QuestTeleopConfig): + """Configuration for TwistTeleopModule.""" + + linear_scale: float = 1.0 + angular_scale: float = 1.0 + + +# Example implementation to show how to extend QuestTeleopModule for different teleop behaviors and outputs. +class TwistTeleopModule(QuestTeleopModule): + """Quest teleop that outputs TwistStamped instead of PoseStamped. + + Config: + - linear_scale: Scale factor for linear (position) values. Default 1.0. + - angular_scale: Scale factor for angular (orientation) values. Default 1.0. + + Outputs: + - left_twist: TwistStamped (linear + angular velocity) + - right_twist: TwistStamped (linear + angular velocity) + - buttons: QuestButtons (inherited) + """ + + default_config = TwistTeleopConfig + + left_twist: Out[TwistStamped] + right_twist: Out[TwistStamped] + + def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: + """Convert PoseStamped to TwistStamped, apply scaling, and publish.""" + cfg: TwistTeleopConfig = self.config # type: ignore[assignment] + twist = TwistStamped( + ts=output_msg.ts, + frame_id=output_msg.frame_id, + linear=output_msg.position * cfg.linear_scale, + angular=output_msg.orientation.to_euler() * cfg.angular_scale, + ) + if hand == Hand.LEFT: + self.left_twist.publish(twist) + else: + self.right_twist.publish(twist) + + +class ArmTeleopModule(QuestTeleopModule): + """Quest teleop with per-hand toggle engage. + + Each controller's primary button (X for left, A for right) + toggles that hand's engage state independently. + + Outputs: + - left_controller_output: PoseStamped (inherited) + - right_controller_output: PoseStamped (inherited) + - buttons: QuestButtons (inherited) + """ + + def __init__(self, *args: Any, **kwargs: Any) -> None: + super().__init__(*args, **kwargs) + self._prev_primary: dict[Hand, bool] = {Hand.LEFT: False, Hand.RIGHT: False} + + def _handle_engage(self) -> None: + """Toggle per-hand engage on primary button rising edge.""" + for hand in Hand: + controller = self._controllers.get(hand) + if controller is None: + continue + + pressed = controller.primary + if pressed and not self._prev_primary[hand]: + if self._is_engaged[hand]: + self._disengage(hand) + else: + self._engage(hand) + self._prev_primary[hand] = pressed + + +class VisualizingTeleopModule(ArmTeleopModule): + """Quest teleop with Rerun visualization. + + Adds visualization of controller poses and trigger values to Rerun. + Useful for debugging and development. + + Outputs: + - left_controller_output: PoseStamped (inherited) + - right_controller_output: PoseStamped (inherited) + - buttons: QuestButtons (inherited) + """ + + @rpc + def start(self) -> None: + """Start module and initialize Rerun visualization.""" + super().start() + init_rerun_visualization() + + def _get_output_pose(self, hand: Hand) -> PoseStamped | None: + """Get output pose and visualize in Rerun.""" + output_pose = super()._get_output_pose(hand) + + if output_pose is not None: + current_pose = self._current_poses.get(hand) + controller = self._controllers.get(hand) + if current_pose is not None: + label = "left" if hand == Hand.LEFT else "right" + visualize_pose(current_pose, label) + + if controller: + visualize_buttons( + label, + primary=controller.primary, + secondary=controller.secondary, + grip=controller.grip, + trigger=controller.trigger, + ) + return output_pose + + +# Module blueprints for easy instantiation +twist_teleop_module = TwistTeleopModule.blueprint +arm_teleop_module = ArmTeleopModule.blueprint +visualizing_teleop_module = VisualizingTeleopModule.blueprint + +__all__ = [ + "ArmTeleopModule", + "TwistTeleopModule", + "VisualizingTeleopModule", + "arm_teleop_module", + "twist_teleop_module", + "visualizing_teleop_module", +] diff --git a/dimos/teleop/quest/quest_teleop_module.py b/dimos/teleop/quest/quest_teleop_module.py new file mode 100644 index 0000000000..616e6c29a1 --- /dev/null +++ b/dimos/teleop/quest/quest_teleop_module.py @@ -0,0 +1,366 @@ +#!/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. + +""" +Quest Teleoperation Module. + +Receives VR controller tracking data via LCM from Deno bridge, +transforms from WebXR to robot frame, computes deltas, and publishes PoseStamped commands. +""" + +from dataclasses import dataclass +from enum import IntEnum +import threading +import time +from typing import Any + +from reactivex.disposable import Disposable + +from dimos.core import In, Module, Out, rpc +from dimos.core.module import ModuleConfig +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.sensor_msgs import Joy +from dimos.teleop.base import TeleopProtocol +from dimos.teleop.quest.quest_types import QuestButtons, QuestControllerState +from dimos.teleop.utils.teleop_transforms import webxr_to_robot +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class Hand(IntEnum): + """Controller hand index.""" + + LEFT = 0 + RIGHT = 1 + + +@dataclass +class QuestTeleopStatus: + """Current teleoperation status.""" + + left_engaged: bool + right_engaged: bool + left_pose: PoseStamped | None + right_pose: PoseStamped | None + buttons: QuestButtons + + +@dataclass +class QuestTeleopConfig(ModuleConfig): + """Configuration for Quest Teleoperation Module.""" + + control_loop_hz: float = 50.0 + + +class QuestTeleopModule(Module[QuestTeleopConfig], TeleopProtocol): + """Quest Teleoperation Module for Meta Quest controllers. + + Gets controller data from Deno bridge, computes output poses, and publishes them. Subclass to customize pose + computation, output format, and engage behavior. + + Implements TeleopProtocol. + + Outputs: + - left_controller_output: PoseStamped (output pose for left hand) + - right_controller_output: PoseStamped (output pose for right hand) + - buttons: QuestButtons (button states for both controllers) + """ + + default_config = QuestTeleopConfig + + # Inputs from Deno bridge + vr_left_pose: In[PoseStamped] + vr_right_pose: In[PoseStamped] + vr_left_joy: In[Joy] + vr_right_joy: In[Joy] + + # Outputs: delta poses for each controller + left_controller_output: Out[PoseStamped] + right_controller_output: Out[PoseStamped] + buttons: Out[QuestButtons] + + # ------------------------------------------------------------------------- + # Initialization + # ------------------------------------------------------------------------- + + def __init__(self, *args: Any, **kwargs: Any) -> None: + super().__init__(*args, **kwargs) + + # Engage state (per-hand) + self._is_engaged: dict[Hand, bool] = {Hand.LEFT: False, Hand.RIGHT: False} + self._initial_poses: dict[Hand, PoseStamped | None] = {Hand.LEFT: None, Hand.RIGHT: None} + self._current_poses: dict[Hand, PoseStamped | None] = {Hand.LEFT: None, Hand.RIGHT: None} + self._controllers: dict[Hand, QuestControllerState | None] = { + Hand.LEFT: None, + Hand.RIGHT: None, + } + self._lock = threading.RLock() + + # Control loop + self._control_loop_thread: threading.Thread | None = None + self._control_loop_running = False + + logger.info("QuestTeleopModule initialized") + + # ------------------------------------------------------------------------- + # Public RPC Methods + # ------------------------------------------------------------------------- + + @rpc + def start(self) -> None: + """Start the Quest teleoperation module.""" + super().start() + + input_streams = { + "vr_left_pose": (self.vr_left_pose, lambda msg: self._on_pose(Hand.LEFT, msg)), + "vr_right_pose": (self.vr_right_pose, lambda msg: self._on_pose(Hand.RIGHT, msg)), + "vr_left_joy": (self.vr_left_joy, lambda msg: self._on_joy(Hand.LEFT, msg)), + "vr_right_joy": (self.vr_right_joy, lambda msg: self._on_joy(Hand.RIGHT, msg)), + } + connected = [] + for name, (stream, handler) in input_streams.items(): + if not (stream and stream.transport): # type: ignore[attr-defined] + logger.warning(f"Stream '{name}' has no transport — skipping") + continue + self._disposables.add(Disposable(stream.subscribe(handler))) # type: ignore[attr-defined] + connected.append(name) + + if connected: + logger.info(f"Subscribed to: {', '.join(connected)}") + + self._start_control_loop() + logger.info("Quest Teleoperation Module started") + + @rpc + def stop(self) -> None: + """Stop the Quest teleoperation module.""" + logger.info("Stopping Quest Teleoperation Module...") + self._stop_control_loop() + super().stop() + + @rpc + def engage(self, hand: Hand | None = None) -> bool: + """Engage teleoperation for a hand. If hand is None, engage both.""" + with self._lock: + return self._engage(hand) + + @rpc + def disengage(self, hand: Hand | None = None) -> None: + """Disengage teleoperation for a hand. If hand is None, disengage both.""" + with self._lock: + self._disengage(hand) + + # ------------------------------------------------------------------------- + # Internal engage/disengage (assumes lock is held) + # ------------------------------------------------------------------------- + + def _engage(self, hand: Hand | None = None) -> bool: + """Engage a hand. Assumes self._lock is held.""" + hands = [hand] if hand is not None else list(Hand) + for h in hands: + pose = self._current_poses.get(h) + if pose is None: + logger.error(f"Engage failed: {h.name.lower()} controller has no data") + return False + self._initial_poses[h] = pose + self._is_engaged[h] = True + logger.info(f"{h.name} engaged.") + return True + + def _disengage(self, hand: Hand | None = None) -> None: + """Disengage a hand. Assumes self._lock is held.""" + hands = [hand] if hand is not None else list(Hand) + for h in hands: + self._is_engaged[h] = False + logger.info(f"{h.name} disengaged.") + + @rpc + def get_status(self) -> QuestTeleopStatus: + """Get current teleoperation status.""" + with self._lock: + left = self._controllers.get(Hand.LEFT) + right = self._controllers.get(Hand.RIGHT) + return QuestTeleopStatus( + left_engaged=self._is_engaged[Hand.LEFT], + right_engaged=self._is_engaged[Hand.RIGHT], + left_pose=self._current_poses.get(Hand.LEFT), + right_pose=self._current_poses.get(Hand.RIGHT), + buttons=QuestButtons.from_controllers(left, right), + ) + + # ------------------------------------------------------------------------- + # Callbacks and Control Loop + # ------------------------------------------------------------------------- + + def _on_pose(self, hand: Hand, pose_stamped: PoseStamped) -> None: + """Callback for controller pose, converting WebXR to robot frame.""" + is_left = hand == Hand.LEFT + robot_pose_stamped = webxr_to_robot(pose_stamped, is_left_controller=is_left) + with self._lock: + self._current_poses[hand] = robot_pose_stamped + + def _on_joy(self, hand: Hand, joy: Joy) -> None: + """Callback for Joy message, parsing into QuestControllerState.""" + is_left = hand == Hand.LEFT + try: + controller = QuestControllerState.from_joy(joy, is_left=is_left) + except ValueError: + logger.warning( + f"Malformed Joy for {hand.name}: axes={len(joy.axes or [])}, buttons={len(joy.buttons or [])}" + ) + return + with self._lock: + self._controllers[hand] = controller + + def _start_control_loop(self) -> None: + """Start the control loop thread.""" + if self._control_loop_running: + return + + self._control_loop_running = True + self._control_loop_thread = threading.Thread( + target=self._control_loop, + daemon=True, + name="QuestTeleopControlLoop", + ) + self._control_loop_thread.start() + 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) + self._control_loop_thread = None + logger.info("Control loop stopped") + + def _control_loop(self) -> None: + """Main control loop: compute deltas and publish at fixed rate. + + Holds self._lock for the entire iteration so overridable methods + don't need to acquire it themselves. + """ + period = 1.0 / self.config.control_loop_hz + + while self._control_loop_running: + loop_start = time.perf_counter() + try: + with self._lock: + self._handle_engage() + + for hand in Hand: + if not self._should_publish(hand): + continue + output_pose = self._get_output_pose(hand) + if output_pose is not None: + self._publish_msg(hand, output_pose) + + # Always publish buttons regardless of engage state, + # so UI/listeners can react to button presses (e.g., trigger engage). + left = self._controllers.get(Hand.LEFT) + right = self._controllers.get(Hand.RIGHT) + self._publish_button_state(left, right) + except Exception: + logger.exception("Error in teleop control loop") + + elapsed = time.perf_counter() - loop_start + sleep_time = period - elapsed + if sleep_time > 0: + time.sleep(sleep_time) + + # ------------------------------------------------------------------------- + # Control Loop Internals + # ------------------------------------------------------------------------- + + def _handle_engage(self) -> None: + """Check for engage button press and update per-hand engage state. + + Override to customize which button/action triggers engage. + Default: Each controller's primary button (X/A) hold engages that hand. + """ + for hand in Hand: + controller = self._controllers.get(hand) + if controller is None: + continue + if controller.primary: + if not self._is_engaged[hand]: + self._engage(hand) + else: + if self._is_engaged[hand]: + self._disengage(hand) + + def _should_publish(self, hand: Hand) -> bool: + """Check if we should publish commands for a hand. + + Override to add custom conditions. + Default: Returns True if the hand is engaged. + """ + return self._is_engaged[hand] + + def _get_output_pose(self, hand: Hand) -> PoseStamped | None: + """Get the pose to publish for a controller. + + Override to customize pose computation (e.g., send absolute pose, + apply scaling, add filtering). + Default: Computes delta from initial pose. + """ + current_pose = self._current_poses.get(hand) + initial_pose = self._initial_poses.get(hand) + + if current_pose is None or initial_pose is None: + return None + + delta = current_pose - initial_pose + return PoseStamped( + position=delta.position, + orientation=delta.orientation, + ts=current_pose.ts, + frame_id=current_pose.frame_id, + ) + + def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: + """Publish message for a controller. + + Override to customize output (e.g., convert to Twist, scale values). + """ + if hand == Hand.LEFT: + self.left_controller_output.publish(output_msg) + else: + self.right_controller_output.publish(output_msg) + + def _publish_button_state( + self, + left: QuestControllerState | None, + right: QuestControllerState | None, + ) -> None: + """Publish button states for both controllers. + + Override to customize button output format (e.g., different bit layout, + keep analog values, add extra streams). + """ + buttons = QuestButtons.from_controllers(left, right) + self.buttons.publish(buttons) + + +quest_teleop_module = QuestTeleopModule.blueprint + +__all__ = [ + "Hand", + "QuestTeleopConfig", + "QuestTeleopModule", + "QuestTeleopStatus", + "quest_teleop_module", +] diff --git a/dimos/teleop/quest/quest_types.py b/dimos/teleop/quest/quest_types.py new file mode 100644 index 0000000000..0f25eb27fc --- /dev/null +++ b/dimos/teleop/quest/quest_types.py @@ -0,0 +1,169 @@ +#!/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. + +"""Quest controller types with nice API for parsing Joy messages.""" + +from dataclasses import dataclass, field +from typing import ClassVar + +from dimos.msgs.sensor_msgs import Joy +from dimos.msgs.std_msgs import UInt32 + + +@dataclass +class ThumbstickState: + """State of a thumbstick with X/Y axes.""" + + x: float = 0.0 + y: float = 0.0 + + +@dataclass +class QuestControllerState: + """Parsed Quest controller state from Joy message with no data loss. + + Preserves full-fidelity analog values (trigger, grip as floats, thumbstick axes) + from the raw Joy message in a readable format. Use this when you need analog + precision (e.g., proportional grip control). Subclasses can publish this + alongside QuestButtons for float access. + + Axes layout: + 0: thumbstick X, 1: thumbstick Y, 2: trigger (analog), 3: grip (analog) + Button indices (digital, 0 or 1): + 0: trigger, 1: grip, 2: touchpad, 3: thumbstick, + 4: X/A, 5: Y/B, 6: menu + """ + + EXPECTED_AXES: ClassVar[int] = 4 + EXPECTED_BUTTONS: ClassVar[int] = 7 + + is_left: bool = True + # Analog values (0.0-1.0) + trigger: float = 0.0 + grip: float = 0.0 + # Digital buttons + touchpad: bool = False + thumbstick_press: bool = False + primary: bool = False # X on left, A on right + secondary: bool = False # Y on left, B on right + menu: bool = False + # Thumbstick axes + thumbstick: ThumbstickState = field(default_factory=ThumbstickState) + + @classmethod + def from_joy(cls, joy: Joy, is_left: bool = True) -> "QuestControllerState": + """Create QuestControllerState from Joy message. + Expected axes: [thumbstick_x, thumbstick_y, trigger_analog, grip_analog] + Expected buttons: [trigger, grip, touchpad, thumbstick, X/A, Y/B, menu] + Raises: + ValueError: If Joy message doesn't have expected Quest controller format. + """ + buttons = joy.buttons or [] + axes = joy.axes or [] + + if len(buttons) < cls.EXPECTED_BUTTONS: + raise ValueError(f"Expected {cls.EXPECTED_BUTTONS} buttons, got {len(buttons)}") + if len(axes) < cls.EXPECTED_AXES: + raise ValueError(f"Expected {cls.EXPECTED_AXES} axes, got {len(axes)}") + + return cls( + is_left=is_left, + trigger=float(axes[2]), + grip=float(axes[3]), + touchpad=buttons[2] > 0.5, + thumbstick_press=buttons[3] > 0.5, + primary=buttons[4] > 0.5, + secondary=buttons[5] > 0.5, + menu=buttons[6] > 0.5, + thumbstick=ThumbstickState(x=float(axes[0]), y=float(axes[1])), + ) + + +class QuestButtons(UInt32): + """Packed button states for both Quest controllers in a single UInt32. + + All values are collapsed to bools for lightweight transport. Analog values + (trigger, grip) are thresholded at 0.5. If you need the original float + values, access them from QuestControllerState and publish them in a subclass. + + Bit layout: + Left (bits 0-6): trigger, grip, touchpad, thumbstick, X, Y, menu + Right (bits 8-14): trigger, grip, touchpad, thumbstick, A, B, menu + """ + + # Bit positions + BITS = { + "left_trigger": 0, + "left_grip": 1, + "left_touchpad": 2, + "left_thumbstick": 3, + "left_x": 4, + "left_y": 5, + "left_menu": 6, + "right_trigger": 8, + "right_grip": 9, + "right_touchpad": 10, + "right_thumbstick": 11, + "right_a": 12, + "right_b": 13, + "right_menu": 14, + } + + def __getattr__(self, name: str) -> bool: + if name in QuestButtons.BITS: + return bool(self.data & (1 << QuestButtons.BITS[name])) + raise AttributeError(f"'{type(self).__name__}' has no attribute '{name}'") + + def __setattr__(self, name: str, value: bool) -> None: + if name in QuestButtons.BITS: + if value: + self.data |= 1 << QuestButtons.BITS[name] + else: + self.data &= ~(1 << QuestButtons.BITS[name]) + else: + super().__setattr__(name, value) + + @classmethod + def from_controllers( + cls, + left: "QuestControllerState | None", + right: "QuestControllerState | None", + ) -> "QuestButtons": + """Create QuestButtons from two QuestControllerState instances.""" + # Safe: cls() calls UInt32.__init__ which sets self.data = 0 before bit ops. + buttons = cls() + + if left: + buttons.left_trigger = left.trigger > 0.5 + buttons.left_grip = left.grip > 0.5 + buttons.left_touchpad = left.touchpad + buttons.left_thumbstick = left.thumbstick_press + buttons.left_x = left.primary + buttons.left_y = left.secondary + buttons.left_menu = left.menu + + if right: + buttons.right_trigger = right.trigger > 0.5 + buttons.right_grip = right.grip > 0.5 + buttons.right_touchpad = right.touchpad + buttons.right_thumbstick = right.thumbstick_press + buttons.right_a = right.primary + buttons.right_b = right.secondary + buttons.right_menu = right.menu + + return buttons + + +__all__ = ["QuestButtons", "QuestControllerState", "ThumbstickState"] diff --git a/dimos/teleop/quest/web/README.md b/dimos/teleop/quest/web/README.md new file mode 100644 index 0000000000..9a7afbfe03 --- /dev/null +++ b/dimos/teleop/quest/web/README.md @@ -0,0 +1,69 @@ +# Quest Teleop Web + +WebXR client and server for Quest 3 VR teleoperation. + +## Components + +### teleop_server.ts + +Deno server that bridges WebSocket and LCM: +- Serves WebXR client over HTTPS (required for Quest) +- Forwards controller data from browser to LCM + +### static/index.html + +WebXR client running on Quest 3: +- Captures controller poses at ~80Hz +- Sends PoseStamped and Joy messages via WebSocket +- Requires internet connection (loads `@dimos/msgs` from CDN at runtime) + +## Running + +From the repository root (`dimos/`): + +```bash +./dimos/teleop/quest/web/teleop_server.ts +``` + +Server starts at `https://localhost:8443` + +SSL certificates are generated automatically on first run in `assets/teleop_certs/`. + +## Message Flow + +``` +Quest Browser Deno Server Python + │ │ │ + │── PoseStamped (left) ────────→ │── vr_left_pose ───────────→ │ + │── PoseStamped (right) ───────→ │── vr_right_pose ──────────→ │ + │── Joy (left controller) ─────→ │── vr_left_joy ────────────→ │ + │── Joy (right controller) ────→ │── vr_right_joy ───────────→ │ +``` + +## LCM Topics + +| Topic | Type | Description | +|-------|------|-------------| +| `vr_left_pose` | PoseStamped | Left controller pose (WebXR frame) | +| `vr_right_pose` | PoseStamped | Right controller pose (WebXR frame) | +| `vr_left_joy` | Joy | Left controller buttons/axes | +| `vr_right_joy` | Joy | Right controller buttons/axes | + +## Joy Message Format + +Quest controller data is packed into Joy messages: + +**Axes** (indices 0-3): +- 0: thumbstick X (-1.0 to 1.0) +- 1: thumbstick Y (-1.0 to 1.0) +- 2: trigger (analog 0.0-1.0) +- 3: grip (analog 0.0-1.0) + +**Buttons** (indices 0-6, digital 0 or 1): +- 0: trigger (pressed) +- 1: grip (pressed) +- 2: touchpad +- 3: thumbstick press +- 4: X/A (primary) +- 5: Y/B (secondary) +- 6: menu diff --git a/dimos/teleop/quest/web/static/index.html b/dimos/teleop/quest/web/static/index.html new file mode 100644 index 0000000000..507d493011 --- /dev/null +++ b/dimos/teleop/quest/web/static/index.html @@ -0,0 +1,409 @@ + + + + + + Quest 3 VR Teleop + + + +
+

DimOS Quest-3 Teleop

+
Ready to connect
+ + +
+ + + + + diff --git a/dimos/teleop/quest/web/teleop_server.ts b/dimos/teleop/quest/web/teleop_server.ts new file mode 100755 index 0000000000..2bff24b34f --- /dev/null +++ b/dimos/teleop/quest/web/teleop_server.ts @@ -0,0 +1,84 @@ +#!/usr/bin/env -S deno run --allow-net --allow-read --allow-run --allow-write --unstable-net + +// WebSocket to LCM Bridge for Quest VR Teleop +// Forwards controller data from browser to LCM + +import { LCM } from "jsr:@dimos/lcm"; +import { dirname, fromFileUrl, join } from "jsr:@std/path"; + +const PORT = 8443; + +// 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 }> { + 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(certsDir, { 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 assets/teleop_certs/"); + return { + cert: await Deno.readTextFile(certPath), + key: await Deno.readTextFile(keyPath), + }; + } +} + +const { cert, key } = await ensureCerts(); + +const lcm = new LCM(); +await lcm.start(); + +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"); + socket.onclose = () => console.log("Client disconnected"); + + // Forward binary LCM packets from browser directly to UDP + socket.binaryType = "arraybuffer"; + socket.onmessage = async (event) => { + 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}`); + +await lcm.run(); diff --git a/dimos/teleop/utils/__init__.py b/dimos/teleop/utils/__init__.py new file mode 100644 index 0000000000..ae8c375e8f --- /dev/null +++ b/dimos/teleop/utils/__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 utilities.""" diff --git a/dimos/teleop/utils/teleop_transforms.py b/dimos/teleop/utils/teleop_transforms.py new file mode 100644 index 0000000000..15fd3be120 --- /dev/null +++ b/dimos/teleop/utils/teleop_transforms.py @@ -0,0 +1,79 @@ +#!/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 VR coordinate transforms.""" + +from __future__ import annotations + +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 PoseStamped +from dimos.utils.transform_utils import matrix_to_pose, pose_to_matrix + +if TYPE_CHECKING: + from numpy.typing import NDArray + +# 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 webxr_to_robot( + pose_stamped: PoseStamped, + is_left_controller: bool = True, +) -> PoseStamped: + """Transform VR controller pose to robot coordinate frame. + + Args: + pose_stamped: PoseStamped from VR controller in WebXR frame. + is_left_controller: True for left controller (+90 deg Z rotation), + False for right controller (-90 deg Z rotation). + + Returns: + PoseStamped in robot frame (preserves original ts and frame_id). + """ + vr_matrix = pose_to_matrix(pose_stamped) + + # Apply controller alignment rotation + # Left controller rotates +90 deg around Z, right rotates -90 deg + 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 + robot_matrix = VR_TO_ROBOT_FRAME @ vr_matrix + robot_pose = matrix_to_pose(robot_matrix) + + return PoseStamped( + position=robot_pose.position, + orientation=robot_pose.orientation, + ts=pose_stamped.ts, + frame_id=pose_stamped.frame_id, + ) + + +__all__ = ["VR_TO_ROBOT_FRAME", "webxr_to_robot"] diff --git a/dimos/teleop/utils/teleop_visualization.py b/dimos/teleop/utils/teleop_visualization.py new file mode 100644 index 0000000000..5b0ecde092 --- /dev/null +++ b/dimos/teleop/utils/teleop_visualization.py @@ -0,0 +1,80 @@ +#!/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 + +from typing import TYPE_CHECKING + +import rerun as rr + +from dimos.dashboard.rerun_init import connect_rerun +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from dimos.core.global_config import GlobalConfig + from dimos.msgs.geometry_msgs import PoseStamped + +logger = setup_logger() + + +def init_rerun_visualization(global_config: GlobalConfig | None = None) -> bool: + """Initialize Rerun visualization connection.""" + if global_config is None: + from dimos.core.global_config import GlobalConfig as GC + + global_config = GC() + if not global_config.viewer_backend.startswith("rerun"): + logger.debug(f"Skipping Rerun init: viewer_backend={global_config.viewer_backend}") + return False + + try: + connect_rerun(global_config=global_config) + 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_pose(pose_stamped: PoseStamped, controller_label: str) -> None: + """Visualize controller absolute pose in Rerun.""" + try: + rr.log(f"world/teleop/{controller_label}_controller", pose_stamped.to_rerun()) # type: ignore[no-untyped-call] + rr.log(f"world/teleop/{controller_label}_controller/axes", rr.TransformAxes3D(0.10)) # type: ignore[attr-defined] + except Exception as e: + logger.debug(f"Failed to log {controller_label} controller to Rerun: {e}") + + +def visualize_buttons( + controller_label: str, + primary: bool = False, + secondary: bool = False, + grip: float = 0.0, + trigger: float = 0.0, +) -> None: + """Visualize button states in Rerun as scalar time series.""" + try: + base_path = f"world/teleop/{controller_label}_controller" + rr.log(f"{base_path}/primary", rr.Scalars(float(primary))) # type: ignore[attr-defined] + rr.log(f"{base_path}/secondary", rr.Scalars(float(secondary))) # type: ignore[attr-defined] + rr.log(f"{base_path}/grip", rr.Scalars(grip)) # type: ignore[attr-defined] + rr.log(f"{base_path}/trigger", rr.Scalars(trigger)) # type: ignore[attr-defined] + except Exception as e: + logger.debug(f"Failed to log {controller_label} buttons to Rerun: {e}") + + +__all__ = ["init_rerun_visualization", "visualize_buttons", "visualize_pose"]