diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index ea08606d7c..e7928fa7c4 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -59,6 +59,7 @@ "demo-mid360-pcap-replay": "dimos.hardware.sensors.lidar.livox.livox_blueprints:demo_mid360_pcap_replay", "demo-object-scene-registration": "dimos.perception.experimental.demo_object_scene_registration:demo_object_scene_registration", "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", + "demo-pico-body-tracking": "dimos.teleop.webxr.blueprints:demo_pico_body_tracking", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", "demo-virtual-mid360-fastlio": "dimos.hardware.sensors.lidar.virtual_mid360.blueprints:demo_virtual_mid360_fastlio", "demo-virtual-mid360-pointlio": "dimos.hardware.sensors.lidar.virtual_mid360.blueprints:demo_virtual_mid360_pointlio", diff --git a/dimos/teleop/webxr/README.md b/dimos/teleop/webxr/README.md index bbda7da705..88fb2d1a09 100644 --- a/dimos/teleop/webxr/README.md +++ b/dimos/teleop/webxr/README.md @@ -97,7 +97,29 @@ entire session; both hands must engage again before commands resume. **Axes**: thumbstick X, thumbstick Y, trigger (analog), grip (analog) -**Buttons**: trigger, grip, touchpad, thumbstick, X/A, Y/B, menu +**Buttons**: trigger, grip, touchpad, thumbstick, X/A, Y/B, optional menu. WebXR +omits a platform-reserved menu button on devices such as PICO controllers. + +## Body Tracking Messages + +The WebSocket carries two frame formats. Controller poses and joystick state use +binary LCM messages. When body tracking is enabled, every sampled frame includes +a JSON body-tracking heartbeat. A `null` joint map means the body source is +unavailable; an empty map means the source resolved no joints for that frame. + +The PICO demo requires body tracking. Enable standard DimOS debug logging to +inspect incoming snapshots: + +```bash +DIMOS_LOG_LEVEL=DEBUG uv run dimos run demo-pico-body-tracking +``` + +The WebXR module logs the first resolved body pose at INFO. At DEBUG, it reports +the received snapshot rate, availability, reference space, joint count, and +joint positions every five seconds of incoming messages. Timing starts with the +first snapshot, excluding headset setup time. Null and empty snapshots are valid +and do not generate warnings; malformed messages do. Reports stop when messages +stop arriving, so these diagnostics do not detect a disconnected or silent client. ## File Structure diff --git a/dimos/teleop/webxr/blueprints.py b/dimos/teleop/webxr/blueprints.py index 46db841322..87a80eb02e 100644 --- a/dimos/teleop/webxr/blueprints.py +++ b/dimos/teleop/webxr/blueprints.py @@ -35,6 +35,7 @@ HandTeleopModule, VideoArmTeleopModule, ) +from dimos.teleop.webxr.module import WebXRTeleopModule from dimos.visualization.vis_module import vis_module # Arm teleop with press-and-hold engage (has rerun viz) @@ -141,3 +142,9 @@ (ArmTeleopModule, "left_gripper_command", "left_gripper_command"), ] ) + + +# PICO 4 Ultra WebXR API test: require body tracking; DEBUG logs show joint poses. +demo_pico_body_tracking = autoconnect( + WebXRTeleopModule.blueprint(body_tracking_mode="required"), +) diff --git a/dimos/teleop/webxr/body_tracking.py b/dimos/teleop/webxr/body_tracking.py new file mode 100644 index 0000000000..c5a2ec38d6 --- /dev/null +++ b/dimos/teleop/webxr/body_tracking.py @@ -0,0 +1,50 @@ +# Copyright 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. + +"""Body-joint snapshots received from a WebXR client.""" + +from typing import Annotated, Literal, TypeAlias + +from pydantic import BaseModel, ConfigDict, Field, StringConstraints + +BodyTrackingMode: TypeAlias = Literal["off", "optional", "required"] +_FiniteFloat: TypeAlias = Annotated[float, Field(strict=True, allow_inf_nan=False)] +_NonEmptyString: TypeAlias = Annotated[ + str, + StringConstraints(min_length=1, pattern=r".*\S.*"), +] + + +class BodyJointPose(BaseModel): + """One body joint's pose in the snapshot's WebXR reference space.""" + + model_config = ConfigDict(extra="forbid", frozen=True) + + position: tuple[_FiniteFloat, _FiniteFloat, _FiniteFloat] + orientation: tuple[_FiniteFloat, _FiniteFloat, _FiniteFloat, _FiniteFloat] + + +class BodyTrackingSnapshot(BaseModel): + """Named body-joint poses captured in one WebXR reference space. + + ``joints=None`` means the body source is unavailable. An empty mapping + means the source is available but did not resolve any joints. + """ + + model_config = ConfigDict(extra="forbid", frozen=True) + + type: Literal["body_tracking_snapshot"] + capture_time_s: _FiniteFloat + frame_id: _NonEmptyString + joints: dict[_NonEmptyString, BodyJointPose] | None diff --git a/dimos/teleop/webxr/controller_types.py b/dimos/teleop/webxr/controller_types.py index 543368b103..6a34d47840 100644 --- a/dimos/teleop/webxr/controller_types.py +++ b/dimos/teleop/webxr/controller_types.py @@ -49,11 +49,11 @@ class WebXRControllerState: 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 + 4: X/A, 5: Y/B, 6: menu (optional) """ EXPECTED_AXES: ClassVar[int] = 4 - EXPECTED_BUTTONS: ClassVar[int] = 7 + REQUIRED_BUTTONS: ClassVar[int] = 6 is_left: bool = True # Analog values (0.0-1.0) @@ -72,17 +72,17 @@ class WebXRControllerState: def from_joy(cls, joy: Joy, is_left: bool = True) -> "WebXRControllerState": """Create WebXRControllerState from Joy message. Expected axes: [thumbstick_x, thumbstick_y, trigger_analog, grip_analog] - Expected buttons: [trigger, grip, touchpad, thumbstick, X/A, Y/B, menu] + Expected buttons: [trigger, grip, touchpad, thumbstick, X/A, Y/B, optional menu] Raises: ValueError: If Joy message doesn't have expected WebXR 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)}") + if len(buttons) < cls.REQUIRED_BUTTONS: + raise ValueError(f"Expected {cls.REQUIRED_BUTTONS} buttons, got {len(buttons)}") return cls( is_left=is_left, @@ -92,7 +92,7 @@ def from_joy(cls, joy: Joy, is_left: bool = True) -> "WebXRControllerState": thumbstick_press=buttons[3] > 0.5, primary=buttons[4] > 0.5, secondary=buttons[5] > 0.5, - menu=buttons[6] > 0.5, + menu=len(buttons) > 6 and buttons[6] > 0.5, thumbstick=ThumbstickState(x=float(axes[0]), y=float(axes[1])), ) diff --git a/dimos/teleop/webxr/module.py b/dimos/teleop/webxr/module.py index 4a4a72d46a..66ade12ede 100644 --- a/dimos/teleop/webxr/module.py +++ b/dimos/teleop/webxr/module.py @@ -24,6 +24,7 @@ import asyncio from dataclasses import dataclass import json +import logging import math from pathlib import Path import threading @@ -35,7 +36,7 @@ from fastapi import WebSocket, WebSocketDisconnect from fastapi.responses import HTMLResponse from fastapi.staticfiles import StaticFiles -from pydantic import Field +from pydantic import Field, ValidationError from reactivex.disposable import Disposable from dimos.constants import DIMOS_PROJECT_ROOT @@ -46,6 +47,7 @@ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.Joy import Joy from dimos.teleop.utils.teleop_transforms import webxr_to_robot +from dimos.teleop.webxr.body_tracking import BodyTrackingMode, BodyTrackingSnapshot # Hand is re-exported for callers; it lives in controller_types. from dimos.teleop.webxr.controller_types import Buttons, Hand, WebXRControllerState @@ -83,6 +85,7 @@ class WebXRTeleopConfig(ModuleConfig): control_loop_hz: float = 50.0 server_port: int = 8443 input_timeout_s: float = Field(default=1.0, gt=0) + body_tracking_mode: BodyTrackingMode = "off" _Config = TypeVar("_Config", bound=WebXRTeleopConfig) @@ -99,6 +102,7 @@ class WebXRTeleopModule(Module): - left_controller_output: PoseStamped (output pose for left hand) - right_controller_output: PoseStamped (output pose for right hand) - teleop_buttons: Buttons (button states for both controllers) + - body_tracking: named body-joint poses in their WebXR reference space """ config: WebXRTeleopConfig @@ -108,6 +112,7 @@ class WebXRTeleopModule(Module): right_controller_output: Out[PoseStamped] teleop_buttons: Out[Buttons] status: In[EpisodeStatus] + body_tracking: Out[BodyTrackingSnapshot] def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) @@ -149,6 +154,9 @@ def __init__(self, **kwargs: Any) -> None: self._clients_lock = threading.Lock() self._ws_loop: asyncio.AbstractEventLoop | None = None self._latest_episode_status: EpisodeStatus | None = None + self._body_report_started_at: float | None = None + self._body_snapshots_since_report = 0 + self._body_tracking_acquired = False def _setup_routes(self) -> None: """Register teleop routes on the embedded web server.""" @@ -159,6 +167,10 @@ async def teleop_index() -> HTMLResponse: index_path = STATIC_DIR / "index.html" return HTMLResponse(content=index_path.read_text()) + @self._web_server.app.get("/teleop/config") + async def teleop_config() -> dict[str, Any]: + return self._webxr_client_config() + if STATIC_DIR.is_dir(): self._web_server.app.mount( "/static", StaticFiles(directory=str(STATIC_DIR)), name="teleop_static" @@ -175,13 +187,16 @@ async def websocket_endpoint(ws: WebSocket) -> None: logger.info("WebXR client connected") try: while True: - data = await ws.receive_bytes() - fingerprint = data[:8] - decoder = self._decoders.get(fingerprint) - if decoder: - decoder(data) - else: - logger.warning(f"Unknown message fingerprint: {fingerprint.hex()}") + message = await ws.receive() + if message["type"] == "websocket.disconnect": + logger.info("WebXR client disconnected") + break + data = message.get("bytes") + text = message.get("text") + if data is not None: + self._dispatch_binary_message(data) + elif text is not None: + self._dispatch_text_message(text) except WebSocketDisconnect: logger.info("WebXR client disconnected") except Exception: @@ -189,6 +204,81 @@ async def websocket_endpoint(ws: WebSocket) -> None: finally: self._client_disconnected(ws) + def _webxr_client_config(self) -> dict[str, Any]: + required_features = ["local-floor"] + optional_features = ["hand-tracking"] + session_modes = ["immersive-ar", "immersive-vr"] + + if self.config.body_tracking_mode != "off": + optional_features.append("bounded-floor") + if self.config.body_tracking_mode == "optional": + optional_features.append("body-tracking") + elif self.config.body_tracking_mode == "required": + required_features.append("body-tracking") + session_modes = ["immersive-ar"] + + return { + "body_tracking_mode": self.config.body_tracking_mode, + "session_modes": session_modes, + "session_options": { + "requiredFeatures": required_features, + "optionalFeatures": optional_features, + }, + } + + def _dispatch_binary_message(self, data: bytes) -> bool: + fingerprint = data[:8] + decoder = self._decoders.get(fingerprint) + if decoder is None: + logger.warning("Unknown WebXR message fingerprint", fingerprint=fingerprint.hex()) + return False + decoder(data) + return True + + def _dispatch_text_message(self, payload: str) -> bool: + try: + snapshot = BodyTrackingSnapshot.model_validate_json(payload) + except ValidationError as exc: + logger.warning("Dropping malformed WebXR body snapshot", error=str(exc)) + return False + self.body_tracking.publish(snapshot) + self._log_body_tracking(snapshot) + return True + + def _log_body_tracking(self, snapshot: BodyTrackingSnapshot) -> None: + joints = snapshot.joints + if joints and not self._body_tracking_acquired: + self._body_tracking_acquired = True + logger.info( + "WebXR body tracking acquired", + reference_space=snapshot.frame_id, + resolved_joint_count=len(joints), + ) + + if not logger.isEnabledFor(logging.DEBUG): + return + now = time.monotonic() + if self._body_report_started_at is None: + self._body_report_started_at = now + return + self._body_snapshots_since_report += 1 + elapsed = now - self._body_report_started_at + if elapsed < 5.0: + return + logger.debug( + "WebXR body tracking health", + snapshot_rate_hz=round(self._body_snapshots_since_report / elapsed, 1), + state="unavailable" if joints is None else "empty" if not joints else "tracking", + reference_space=snapshot.frame_id, + resolved_joint_count=len(joints) if joints else 0, + joint_positions={ + name: tuple(round(value, 3) for value in pose.position) + for name, pose in (joints or {}).items() + }, + ) + self._body_report_started_at = now + self._body_snapshots_since_report = 0 + def _client_connected(self, ws: WebSocket) -> bool: with self._clients_lock: if self._connected_clients: @@ -241,6 +331,9 @@ def build(self) -> None: @rpc def start(self) -> None: super().start() + self._body_report_started_at = None + self._body_snapshots_since_report = 0 + self._body_tracking_acquired = False self._web_server = RobotWebInterface(host="0.0.0.0", port=self.config.server_port) self._setup_routes() self._start_server() diff --git a/dimos/teleop/webxr/test_blueprints.py b/dimos/teleop/webxr/test_blueprints.py index 18c588c597..55b4efe6d3 100644 --- a/dimos/teleop/webxr/test_blueprints.py +++ b/dimos/teleop/webxr/test_blueprints.py @@ -20,11 +20,14 @@ from dimos.core.coordination.blueprints import Blueprint from dimos.robot.manipulators.common.blueprints import TeleopBinding from dimos.teleop.webxr.blueprints import ( + demo_pico_body_tracking, teleop_webxr_dual, teleop_webxr_hand_xarm7, teleop_webxr_xarm7, ) +from dimos.teleop.webxr.body_tracking import BodyTrackingSnapshot from dimos.teleop.webxr.extensions import ArmTeleopModule, HandTeleopModule +from dimos.teleop.webxr.module import WebXRTeleopModule def _coordinator_tasks(blueprint: Blueprint) -> list[TaskConfig]: @@ -107,3 +110,13 @@ def test_mixed_arm_blueprint_keeps_two_independent_one_binding_tasks() -> None: teleop_webxr_dual.remapping_map[(ArmTeleopModule.name, "right_controller_output")] == "right_cartesian_command" ) + + +def test_pico_body_tracking_demo_uses_single_required_webxr_module() -> None: + modules = {atom.module for atom in demo_pico_body_tracking.blueprints} + webxr = next( + atom for atom in demo_pico_body_tracking.blueprints if atom.module is WebXRTeleopModule + ) + assert modules == {WebXRTeleopModule} + assert webxr.kwargs["body_tracking_mode"] == "required" + assert ("body_tracking", BodyTrackingSnapshot) not in demo_pico_body_tracking.transport_map diff --git a/dimos/teleop/webxr/test_body_tracking.py b/dimos/teleop/webxr/test_body_tracking.py new file mode 100644 index 0000000000..b7109069fb --- /dev/null +++ b/dimos/teleop/webxr/test_body_tracking.py @@ -0,0 +1,88 @@ +# Copyright 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. + +import json + +from pydantic import ValidationError +import pytest + +from dimos.teleop.webxr.body_tracking import BodyTrackingSnapshot + + +def _payload(*, joints) -> str: + return json.dumps( + { + "type": "body_tracking_snapshot", + "capture_time_s": 1234.5, + "frame_id": "bounded-floor", + "joints": joints, + } + ) + + +def test_body_tracking_snapshot_validates_named_poses() -> None: + snapshot = BodyTrackingSnapshot.model_validate_json( + _payload( + joints={ + "hips": { + "position": [1.0, 2.0, 3.0], + "orientation": [0.1, 0.2, 0.3, 0.9], + }, + "left-foot-ankle": { + "position": [-0.2, 0.1, 0.4], + "orientation": [0.0, 0.0, 0.0, 1.0], + }, + } + ) + ) + + assert snapshot.capture_time_s == 1234.5 + assert snapshot.frame_id == "bounded-floor" + assert snapshot.joints is not None + assert list(snapshot.joints) == ["hips", "left-foot-ankle"] + assert snapshot.joints["hips"].position == (1.0, 2.0, 3.0) + assert snapshot.joints["hips"].orientation == (0.1, 0.2, 0.3, 0.9) + + +@pytest.mark.parametrize("joints", [None, {}]) +def test_body_tracking_snapshot_preserves_absence_state(joints) -> None: + snapshot = BodyTrackingSnapshot.model_validate_json(_payload(joints=joints)) + + assert snapshot.joints == joints + + +@pytest.mark.parametrize( + "payload", + [ + "not json", + '{"type":"unknown"}', + _payload(joints={"hips": {"position": [1.0, 2.0], "orientation": [0, 0, 0, 1]}}), + _payload(joints={"hips": {"position": [1.0, 2.0, 3.0], "orientation": [0, 0, 1]}}), + _payload(joints={"": {"position": [1.0, 2.0, 3.0], "orientation": [0, 0, 0, 1]}}), + _payload(joints={"hips": {"position": [True, 2.0, 3.0], "orientation": [0, 0, 0, 1]}}), + '{"type":"body_tracking_snapshot","capture_time_s":NaN,"frame_id":"local-floor","joints":{}}', + json.dumps( + { + "type": "body_tracking_snapshot", + "capture_time_s": 1.0, + "frame_id": "local-floor", + "joints": {}, + "unexpected": True, + } + ), + ], +) +def test_body_tracking_snapshot_rejects_malformed_payloads(payload: str) -> None: + with pytest.raises(ValidationError): + BodyTrackingSnapshot.model_validate_json(payload) diff --git a/dimos/teleop/webxr/test_module.py b/dimos/teleop/webxr/test_module.py index 18ff2de978..bba8d0eaa2 100644 --- a/dimos/teleop/webxr/test_module.py +++ b/dimos/teleop/webxr/test_module.py @@ -15,14 +15,19 @@ import asyncio from collections.abc import Awaitable, Callable, Iterator import json +import logging from types import SimpleNamespace -from typing import Any +from typing import Any, cast +from fastapi import FastAPI +from fastapi.testclient import TestClient import pytest import pytest_mock from dimos.imitation.collection.episode_monitor import EpisodeStatus from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.Joy import Joy +from dimos.teleop.webxr.body_tracking import BodyTrackingSnapshot from dimos.teleop.webxr.controller_types import ( Buttons, Hand, @@ -33,6 +38,101 @@ from dimos.teleop.webxr.module import WebXRTeleopModule, _ws_send_text +@pytest.mark.parametrize("state", ["unavailable", "empty", "tracking"]) +def test_body_debug_logging_reports_received_snapshots(module, mocker, state) -> None: + logger = mocker.patch("dimos.teleop.webxr.module.logger") + logger.isEnabledFor.return_value = True + clock = mocker.patch("dimos.teleop.webxr.module.time") + clock.monotonic.side_effect = [1000.0, 1001.0, 1005.0, 1006.0, 1010.0] + publish = mocker.patch.object(module.body_tracking, "publish") + joints = { + "vendor-joint": { + "position": [0.12345, 1.23456, -0.34567], + "orientation": [0.0, 0.0, 0.0, 1.0], + } + } + payload = json.dumps( + { + "type": "body_tracking_snapshot", + "capture_time_s": 1.0, + "frame_id": "bounded-floor", + "joints": None if state == "unavailable" else {} if state == "empty" else joints, + } + ) + + for _ in range(2): + assert module._dispatch_text_message(payload) + logger.debug.assert_not_called() + assert module._dispatch_text_message(payload) + logger.debug.assert_called_once_with( + "WebXR body tracking health", + snapshot_rate_hz=0.4, + state=state, + reference_space="bounded-floor", + resolved_joint_count=1 if state == "tracking" else 0, + joint_positions={"vendor-joint": (0.123, 1.235, -0.346)} if state == "tracking" else {}, + ) + assert module._dispatch_text_message(payload) + assert logger.debug.call_count == 1 + assert module._dispatch_text_message(payload) + assert logger.debug.call_count == 2 + assert logger.debug.call_args.kwargs["snapshot_rate_hz"] == 0.4 + assert logger.info.call_count == (1 if state == "tracking" else 0) + logger.warning.assert_not_called() + assert publish.call_count == 5 + + +def test_body_acquisition_logs_once_without_debug_and_resets_on_start(module, mocker) -> None: + logger = mocker.patch("dimos.teleop.webxr.module.logger") + logger.isEnabledFor.return_value = False + clock = mocker.patch("dimos.teleop.webxr.module.time") + publish = mocker.patch.object(module.body_tracking, "publish") + mocker.patch("dimos.teleop.webxr.module.RobotWebInterface") + mocker.patch.object(module, "_setup_routes") + mocker.patch.object(module, "_start_server") + mocker.patch.object(module, "_start_control_loop") + snapshot = { + "type": "body_tracking_snapshot", + "capture_time_s": 1.0, + "frame_id": "local-floor", + "joints": None, + } + assert module._dispatch_text_message(json.dumps(snapshot)) + snapshot["joints"] = {} + assert module._dispatch_text_message(json.dumps(snapshot)) + logger.info.assert_not_called() + snapshot["joints"] = { + "hips": {"position": [0.0, 1.0, 0.0], "orientation": [0.0, 0.0, 0.0, 1.0]} + } + for _ in range(2): + assert module._dispatch_text_message(json.dumps(snapshot)) + logger.info.assert_called_once_with( + "WebXR body tracking acquired", reference_space="local-floor", resolved_joint_count=1 + ) + logger.isEnabledFor.assert_called_with(logging.DEBUG) + logger.debug.assert_not_called() + logger.warning.assert_not_called() + clock.monotonic.assert_not_called() + assert publish.call_count == 4 + + module.start() + logger.info.reset_mock() + assert module._dispatch_text_message(json.dumps(snapshot)) + logger.info.assert_called_once_with( + "WebXR body tracking acquired", reference_space="local-floor", resolved_joint_count=1 + ) + + +def test_malformed_body_message_warns_without_logging_acquisition(module, mocker) -> None: + logger = mocker.patch("dimos.teleop.webxr.module.logger") + publish = mocker.patch.object(module.body_tracking, "publish") + assert not module._dispatch_text_message('{"type":"body_tracking_snapshot"}') + logger.warning.assert_called_once() + logger.info.assert_not_called() + logger.debug.assert_not_called() + publish.assert_not_called() + + @pytest.fixture def module() -> Iterator[WebXRTeleopModule]: module = WebXRTeleopModule(server_port=9443) @@ -42,6 +142,17 @@ def module() -> Iterator[WebXRTeleopModule]: module.stop() +def _setup_test_app( + module: WebXRTeleopModule, + mocker: pytest_mock.MockerFixture, +) -> FastAPI: + app = FastAPI() + web_server = mocker.Mock(app=app) + module._web_server = cast("Any", web_server) + module._setup_routes() + return app + + def test_webxr_web_server_is_initialized_during_start( module: WebXRTeleopModule, mocker: pytest_mock.MockerFixture ) -> None: @@ -217,6 +328,23 @@ def decorator(fn: Callable[[Any], Awaitable[None]]) -> Callable[[Any], Awaitable ws.receive_bytes.assert_not_awaited() +def test_websocket_dispatches_binary_and_text_messages( + module: WebXRTeleopModule, + mocker: pytest_mock.MockerFixture, +) -> None: + app = _setup_test_app(module, mocker) + dispatch_binary = mocker.patch.object(module, "_dispatch_binary_message") + dispatch_text = mocker.patch.object(module, "_dispatch_text_message") + + with TestClient(app) as client: + with client.websocket_connect("/ws") as websocket: + websocket.send_bytes(b"controller") + websocket.send_text('{"type":"body_tracking_snapshot"}') + + dispatch_binary.assert_called_once_with(b"controller") + dispatch_text.assert_called_once_with('{"type":"body_tracking_snapshot"}') + + def test_first_client_connection_rejects_stale_cached_state( module: WebXRTeleopModule, mocker: pytest_mock.MockerFixture ) -> None: @@ -286,6 +414,119 @@ def test_go2_stale_input_publishes_zero_velocity(mocker: pytest_mock.MockerFixtu module.stop() +def test_default_webxr_config_does_not_request_body_tracking( + module: WebXRTeleopModule, +) -> None: + assert module._webxr_client_config() == { + "body_tracking_mode": "off", + "session_modes": ["immersive-ar", "immersive-vr"], + "session_options": { + "requiredFeatures": ["local-floor"], + "optionalFeatures": ["hand-tracking"], + }, + } + + +@pytest.mark.parametrize( + ("mode", "session_modes", "required_features", "optional_features"), + [ + ( + "optional", + ["immersive-ar", "immersive-vr"], + ["local-floor"], + ["hand-tracking", "bounded-floor", "body-tracking"], + ), + ( + "required", + ["immersive-ar"], + ["local-floor", "body-tracking"], + ["hand-tracking", "bounded-floor"], + ), + ], +) +def test_enabled_webxr_config_requests_body_tracking( + mode, + session_modes, + required_features, + optional_features, +) -> None: + module = WebXRTeleopModule(body_tracking_mode=mode) + try: + assert module._webxr_client_config() == { + "body_tracking_mode": mode, + "session_modes": session_modes, + "session_options": { + "requiredFeatures": required_features, + "optionalFeatures": optional_features, + }, + } + finally: + module.stop() + + +def test_webxr_config_route_exposes_body_tracking_mode( + mocker: pytest_mock.MockerFixture, +) -> None: + module = WebXRTeleopModule(body_tracking_mode="required") + app = _setup_test_app(module, mocker) + + try: + with TestClient(app) as client: + response = client.get("/teleop/config") + + assert response.status_code == 200 + assert response.json() == module._webxr_client_config() + finally: + module.stop() + + +def test_go2_accepts_pico_six_button_joystick( + mocker: pytest_mock.MockerFixture, +) -> None: + module = Go2TeleopModule() + publish = mocker.patch.object(module.cmd_vel, "publish") + joy = Joy( + ts=1.0, + frame_id="left", + axes=[0.25, -0.75, 0.0, 0.0], + buttons=[0, 0, 0, 0, 0, 0], + ) + try: + assert module._on_joy_bytes(joy.lcm_encode()) is True + + twist = publish.call_args.args[0] + assert twist.linear.x == pytest.approx(0.75 * module.config.linear_speed) + assert twist.linear.y == pytest.approx(-0.25 * module.config.linear_speed) + assert twist.angular.z == 0.0 + finally: + module.stop() + + +def test_go2_rejects_short_controller_packet_safely( + mocker: pytest_mock.MockerFixture, +) -> None: + module = Go2TeleopModule() + publish = mocker.patch.object(module.cmd_vel, "publish") + joy = Joy( + ts=1.0, + frame_id="left", + axes=[0.25, -0.75, 0.0, 0.0], + buttons=[0, 0, 0, 0, 0], + ) + module._controllers[Hand.LEFT] = WebXRControllerState(thumbstick=ThumbstickState(y=-1.0)) + try: + assert module._on_joy_bytes(joy.lcm_encode()) is False + + assert module._controllers[Hand.LEFT] is None + publish.assert_called_once() + twist = publish.call_args.args[0] + assert twist.linear.x == 0.0 + assert twist.linear.y == 0.0 + assert twist.angular.z == 0.0 + finally: + module.stop() + + def test_go2_malformed_joy_clears_stale_state_and_publishes_zero_velocity( mocker: pytest_mock.MockerFixture, ) -> None: @@ -309,6 +550,23 @@ def test_go2_malformed_joy_clears_stale_state_and_publishes_zero_velocity( module.stop() +def test_webxr_body_reader_is_served_as_javascript( + mocker: pytest_mock.MockerFixture, +) -> None: + module = WebXRTeleopModule() + app = _setup_test_app(module, mocker) + + try: + with TestClient(app) as client: + response = client.get("/static/webxr_body.mjs") + + assert response.status_code == 200 + assert response.headers["content-type"].startswith("text/javascript") + assert "export function captureBody" in response.text + finally: + module.stop() + + def test_go2_unknown_controller_identity_publishes_zero_velocity( mocker: pytest_mock.MockerFixture, ) -> None: @@ -332,6 +590,76 @@ def test_go2_unknown_controller_identity_publishes_zero_velocity( module.stop() +def test_text_body_tracking_snapshot_is_published( + module: WebXRTeleopModule, + mocker: pytest_mock.MockerFixture, +) -> None: + publish = mocker.patch.object(module.body_tracking, "publish") + payload = json.dumps( + { + "type": "body_tracking_snapshot", + "capture_time_s": 3.0, + "frame_id": "bounded-floor", + "joints": { + "hips": { + "position": [1.0, 2.0, 3.0], + "orientation": [0.0, 0.0, 0.0, 1.0], + } + }, + } + ) + + accepted = module._dispatch_text_message(payload) + + assert accepted + snapshot = publish.call_args.args[0] + assert isinstance(snapshot, BodyTrackingSnapshot) + assert snapshot.frame_id == "bounded-floor" + assert snapshot.joints is not None + assert snapshot.joints["hips"].position == (1.0, 2.0, 3.0) + + +def test_malformed_text_message_is_dropped( + module: WebXRTeleopModule, + mocker: pytest_mock.MockerFixture, +) -> None: + publish = mocker.patch.object(module.body_tracking, "publish") + + accepted = module._dispatch_text_message('{"type": "unknown"}') + + assert not accepted + publish.assert_not_called() + + +def test_binary_pose_dispatch_remains_on_existing_decoder( + module: WebXRTeleopModule, + mocker: pytest_mock.MockerFixture, +) -> None: + body_publish = mocker.patch.object(module.body_tracking, "publish") + pose = PoseStamped(ts=1.0, frame_id="left", position=[1.0, 2.0, 3.0]) + + accepted = module._dispatch_binary_message(pose.lcm_encode()) + + assert accepted + assert module._current_poses[Hand.LEFT] is not None + body_publish.assert_not_called() + + +def test_unknown_binary_message_is_dropped( + module: WebXRTeleopModule, + mocker: pytest_mock.MockerFixture, +) -> None: + warning = mocker.patch("dimos.teleop.webxr.module.logger.warning") + + accepted = module._dispatch_binary_message(b"unknown-message") + + assert not accepted + warning.assert_called_once_with( + "Unknown WebXR message fingerprint", + fingerprint=b"unknown-".hex(), + ) + + def test_translation_scale_changes_pose_delta(module: WebXRTeleopModule) -> None: module._initial_poses[Hand.RIGHT] = PoseStamped(position=[1.0, 2.0, 3.0]) module._current_poses[Hand.RIGHT] = PoseStamped(position=[1.2, 1.5, 4.0]) diff --git a/dimos/teleop/webxr/web/static/teleop.js b/dimos/teleop/webxr/web/static/teleop.js index 1b5047aab5..eb3677e92b 100644 --- a/dimos/teleop/webxr/web/static/teleop.js +++ b/dimos/teleop/webxr/web/static/teleop.js @@ -5,14 +5,19 @@ window.onerror = (msg, url, line, col, error) => { }; import { geometry_msgs, std_msgs, sensor_msgs } from "https://esm.sh/jsr/@dimos/msgs@0.1.4"; +import { captureBody } from "./webxr_body.mjs"; // WebSocket and WebXR state let ws = null; let xrSession = null; let xrRefSpace = null; +let xrBodyRefSpace = null; +let xrBodyRefSpaceType = null; let gl = null; let lastSendTime = 0; const sendInterval = 1000 / 80; // ~80Hz target +let webXRClientConfig = null; +const sessionModeSupport = new Map(); const handSelectActive = new Map(); const GRIPPER_PINCH_DISTANCE_METERS = 0.04; @@ -65,6 +70,20 @@ function setStatus(msg) { statusEl.textContent = msg; } +async function loadWebXRClientConfig() { + const response = await fetch('/teleop/config', { cache: 'no-store' }); + if (!response.ok) { + throw new Error(`Failed to load teleop configuration: HTTP ${response.status}`); + } + return response.json(); +} + +function describeSessionRequestError(mode, error) { + const name = error?.name || 'Error'; + const message = error?.message || String(error); + return `${mode} (${name}: ${message})`; +} + // WebSocket setup (LCM bridge) function setupWebSocket() { return new Promise((resolve, reject) => { @@ -89,7 +108,7 @@ function setupWebSocket() { ws.onclose = () => { hudOffline = true; hudDirty = true; - setStatus('WebSocket closed'); + if (xrSession) setStatus('WebSocket closed'); }; // Defer revoking the previous blob URL by one message — revoking // immediately after setting src can race with the browser's load @@ -466,13 +485,12 @@ function sendJoy(handedness, axes, buttons) { } // Send raw controller and wrist tracking data (no processing - done in Python) -function processTracking(frame) { +function processTracking(time, frame) { // Rate limit tracking data - const now = performance.now(); - if (now - lastSendTime < sendInterval) { + if (time - lastSendTime < sendInterval) { return; } - lastSendTime = now; + lastSendTime = time; // Process controller and hand input sources. for (const inputSource of frame.session.inputSources) { @@ -545,14 +563,26 @@ function processTracking(frame) { sendJoy(handedness, axes, buttons); } } + + if (webXRClientConfig.body_tracking_mode !== 'off') { + const joints = captureBody(frame, xrBodyRefSpace); + if (ws && ws.readyState === WebSocket.OPEN) { + ws.send(JSON.stringify({ + type: 'body_tracking_snapshot', + capture_time_s: (performance.timeOrigin + time) / 1000, + frame_id: xrBodyRefSpaceType, + joints, + })); + } + } } // WebXR render loop -function onXRFrame(_time, frame) { +function onXRFrame(time, frame) { if (!xrSession) return; xrSession.requestAnimationFrame(onXRFrame); // Process and send tracking data - processTracking(frame); + processTracking(time, frame); const glLayer = xrSession.renderState.baseLayer; gl.bindFramebuffer(gl.FRAMEBUFFER, glLayer.framebuffer); @@ -577,31 +607,32 @@ function onXRFrame(_time, frame) { } // Start an immersive WebXR session with passthrough when available. -async function startWebXRSession() { +async function startWebXRSession(clientConfig) { try { setStatus('Initializing WebGL...'); initGL(); setStatus('Requesting WebXR session...'); - // Try immersive-ar first (true passthrough), fall back to immersive-vr let session = null; - try { - session = await navigator.xr.requestSession('immersive-ar', { - requiredFeatures: ['local-floor'], - optionalFeatures: ['hand-tracking'] - }); - console.log('Started immersive-ar session (passthrough)'); - } catch (arError) { - console.log('immersive-ar not available, trying immersive-vr'); - session = await navigator.xr.requestSession('immersive-vr', { - requiredFeatures: ['local-floor'], - optionalFeatures: ['hand-tracking'] - }); - console.log('Started immersive-vr session'); + const failures = []; + for (const mode of clientConfig.session_modes) { + try { + session = await navigator.xr.requestSession(mode, clientConfig.session_options); + console.log(`Started ${mode} session`); + break; + } catch (error) { + const failure = describeSessionRequestError(mode, error); + failures.push(failure); + console.warn(`WebXR session request failed: ${failure}`); + } + } + if (!session) { + throw new Error(`WebXR session request failed: ${failures.join('; ')}`); } xrSession = session; hudPlaced = false; + lastSendTime = 0; // Setup WebGL layer const glLayer = new XRWebGLLayer(session, gl); @@ -612,7 +643,18 @@ async function startWebXRSession() { // Get reference space xrRefSpace = await session.requestReferenceSpace('local-floor'); - setStatus('WebXR active'); + if (clientConfig.body_tracking_mode !== 'off') { + try { + xrBodyRefSpace = await session.requestReferenceSpace('bounded-floor'); + xrBodyRefSpaceType = 'bounded-floor'; + } catch (error) { + console.warn('bounded-floor unavailable; using local-floor for body poses', error); + xrBodyRefSpace = xrRefSpace; + xrBodyRefSpaceType = 'local-floor'; + } + } + + setStatus(`WebXR active (${session.mode})`); // Session event handlers session.addEventListener('end', () => { @@ -620,6 +662,8 @@ async function startWebXRSession() { handSelectActive.clear(); hudPlaced = false; xrSession = null; + xrBodyRefSpace = null; + xrBodyRefSpaceType = null; window.disconnect(); }); @@ -655,19 +699,31 @@ window.connect = async function() { if (!navigator.xr) { throw new Error('WebXR not supported. Use a WebXR-capable browser.'); } + if (!webXRClientConfig) { + throw new Error('WebXR configuration is unavailable. Reload the page and try again.'); + } - // Setup WebSocket - await setupWebSocket(); + // Immersive sessions must be requested while this click still carries + // transient user activation. In particular, required body tracking can + // trigger a consent check, so do not await network setup first. + await startWebXRSession(webXRClientConfig); - // Start WebXR - await startWebXRSession(); + // Connect the data channel after the browser grants the XR session. + await setupWebSocket(); // Update UI connectBtn.classList.add('hidden'); disconnectBtn.classList.remove('hidden'); } catch (error) { - setStatus('Connection failed'); + const message = error?.message || String(error); + const failedSession = xrSession; + xrSession = null; + if (failedSession) await failedSession.end().catch(console.error); + const failedWebSocket = ws; + ws = null; + if (failedWebSocket) failedWebSocket.close(); + setStatus(`Connection failed: ${message}`); console.error('Connection error:', error); connectBtn.disabled = false; } @@ -703,15 +759,22 @@ window.addEventListener('load', async () => { } try { - // Check for immersive AR (passthrough) or VR session support. - const arSupported = await navigator.xr.isSessionSupported('immersive-ar').catch(() => false); - const vrSupported = await navigator.xr.isSessionSupported('immersive-vr').catch(() => false); - - if (!arSupported && !vrSupported) { - setStatus('Immersive WebXR not supported'); + webXRClientConfig = await loadWebXRClientConfig(); + await Promise.all(webXRClientConfig.session_modes.map(async (mode) => { + const supported = await navigator.xr.isSessionSupported(mode).catch(() => false); + sessionModeSupport.set(mode, supported); + })); + + const supported = webXRClientConfig.session_modes.some( + (mode) => sessionModeSupport.get(mode), + ); + if (!supported) { + setStatus(`Session modes unsupported: ${webXRClientConfig.session_modes.join(', ')}`); connectBtn.disabled = true; } } catch (error) { - console.error('WebXR check failed:', error); + setStatus(error?.message || String(error)); + connectBtn.disabled = true; + console.error('WebXR setup failed:', error); } }); diff --git a/dimos/teleop/webxr/web/static/webxr_body.mjs b/dimos/teleop/webxr/web/static/webxr_body.mjs new file mode 100644 index 0000000000..68b57e1ce9 --- /dev/null +++ b/dimos/teleop/webxr/web/static/webxr_body.mjs @@ -0,0 +1,20 @@ +// Capture every body-joint pose that resolves in this animation frame. +// A missing body source is different from a present source with no usable poses. +export function captureBody(frame, referenceSpace) { + const body = frame.body; + if (!body) return null; + + const joints = {}; + for (const [jointName, jointSpace] of body) { + const pose = frame.getPose(jointSpace, referenceSpace); + if (!pose) continue; + + const position = pose.transform.position; + const orientation = pose.transform.orientation; + joints[jointName] = { + position: [position.x, position.y, position.z], + orientation: [orientation.x, orientation.y, orientation.z, orientation.w], + }; + } + return joints; +} diff --git a/dimos/utils/logging_config.py b/dimos/utils/logging_config.py index 344feb8950..af1fff2f7d 100644 --- a/dimos/utils/logging_config.py +++ b/dimos/utils/logging_config.py @@ -294,7 +294,7 @@ def setup_logger(*, level: int | None = None) -> Any: file_handler.setFormatter(file_formatter) stdlib_logger.addHandler(file_handler) - return structlog.get_logger(name) + return structlog.wrap_logger(stdlib_logger, wrapper_class=structlog.stdlib.BoundLogger) def setup_exception_handler() -> None: diff --git a/dimos/utils/test_logging_config.py b/dimos/utils/test_logging_config.py index dfbabbbaef..dd9d7531cb 100644 --- a/dimos/utils/test_logging_config.py +++ b/dimos/utils/test_logging_config.py @@ -16,12 +16,46 @@ from __future__ import annotations +import json +import subprocess +import sys + import pytest from dimos.utils import logging_config from dimos.utils.logging_config import _compact_console_processor +@pytest.mark.parametrize("level,debug_enabled", [("INFO", False), ("DEBUG", True)]) +def test_setup_logger_level_check_matches_output(monkeypatch, tmp_path, level, debug_enabled): + monkeypatch.setenv("DIMOS_LOG_LEVEL", level) + monkeypatch.setenv("DIMOS_RUN_LOG_DIR", str(tmp_path)) + result = subprocess.run( + [ + sys.executable, + "-c", + """ +import logging +from dimos.utils.logging_config import setup_logger +logger = setup_logger() +print(logger.isEnabledFor(logging.DEBUG)) +logger.debug("body details", joints=2) +logger.info("body acquired") +""", + ], + capture_output=True, + text=True, + timeout=20, + check=True, + ) + assert result.stdout.splitlines()[0] == str(debug_enabled) + assert ("body details" in result.stdout) == debug_enabled + records = [json.loads(line) for line in (tmp_path / "main.jsonl").read_text().splitlines()] + assert [record["event"] for record in records] == ( + ["body details", "body acquired"] if debug_enabled else ["body acquired"] + ) + + def test_module_key_leads_the_kv_tail(monkeypatch: pytest.MonkeyPatch) -> None: monkeypatch.setattr(logging_config, "_CONSOLE_USE_COLORS", False) line = _compact_console_processor(