[Backport release/0.0.14] feat(teleop): add PICO WebXR body tracking

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",

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
 

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"),
+)

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

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])),
         )
 

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,20 +187,98 @@ 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:
                 logger.exception("WebSocket error")
             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()

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