Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
57 commits
Select commit Hold shift + click to select a range
41412df
mem2 recording fixes
ruthwikdasyam May 7, 2026
3d6adae
feat: hosted teleop module
ruthwikdasyam May 7, 2026
33c2571
misc: hosted tests
ruthwikdasyam May 7, 2026
fb43817
misc: no-cloudflare broker
ruthwikdasyam May 7, 2026
7b59671
fix: auth updates
ruthwikdasyam May 8, 2026
4e31bae
updated html + web address
ruthwikdasyam May 8, 2026
ed2d275
working commit
ruthwikdasyam May 9, 2026
b6914e9
feat/workingcommit/keyboard-teleop
ruthwikdasyam May 9, 2026
027dae0
feat/TwistStamped + recording msgs
ruthwikdasyam May 10, 2026
838a2c0
Merge branch 'main' into ruthwik/hosted-teleop
ruthwikdasyam May 18, 2026
a560a77
[autofix.ci] apply automated fixes
autofix-ci[bot] May 18, 2026
0ed9fcd
feat: added seq to Timestamped msg
ruthwikdasyam May 19, 2026
d5ea14e
no seperate sim blueprints needed
ruthwikdasyam May 19, 2026
06516f5
feat: increase workers
ruthwikdasyam May 19, 2026
f88746f
feat: benchmarking
ruthwikdasyam May 19, 2026
d18e495
fix: pre-commit errors
ruthwikdasyam May 19, 2026
b2b11d6
fix: logging spam
ruthwikdasyam May 19, 2026
e20bceb
added seq to cal loss
ruthwikdasyam May 19, 2026
996be8d
benchmarks for twist
ruthwikdasyam May 19, 2026
1b0829d
fix: pre-commit fixes
ruthwikdasyam May 19, 2026
38e51d4
[autofix.ci] apply automated fixes
autofix-ci[bot] May 19, 2026
2043f13
fix: notebook file import
ruthwikdasyam May 19, 2026
3a4cf68
fix: pre-commit
ruthwikdasyam May 19, 2026
b5dfe71
[autofix.ci] apply automated fixes
autofix-ci[bot] May 19, 2026
23423e9
feat: added pingpong for time offset cal
ruthwikdasyam May 20, 2026
e3cf7e9
feat: video stream
ruthwikdasyam May 20, 2026
0544ba2
feat: videostream
ruthwikdasyam May 21, 2026
07b7f6c
feat: state_back channel and mediatrack added
ruthwikdasyam May 22, 2026
83ff3f2
fix: pre-commit
ruthwikdasyam May 22, 2026
89c4509
misc: debug lines removed
ruthwikdasyam May 23, 2026
ee1f1f0
[autofix.ci] apply automated fixes
autofix-ci[bot] May 23, 2026
79b5867
misc: remove debug and comments
ruthwikdasyam May 25, 2026
51ff352
feat: video streams + joystick controls
ruthwikdasyam May 26, 2026
1a22b87
fix: print video stats
ruthwikdasyam May 26, 2026
86ec4e9
feat: live stats compute in mocule
ruthwikdasyam May 29, 2026
69e1141
feat: all stats computation methods from utils
ruthwikdasyam May 29, 2026
494f3fd
docs: readme for hosted module setup
ruthwikdasyam May 29, 2026
4a4bd58
fix: pre-commit fixes
ruthwikdasyam May 29, 2026
3410213
Merge branch 'main' into ruthwik/hosted-teleop
ruthwikdasyam May 29, 2026
2781368
misc: remove dev_local_broker and data_analyzer files
ruthwikdasyam Jun 1, 2026
25a151b
docs: updated folder structure + files
ruthwikdasyam Jun 1, 2026
3cf4f75
fix: remove live logs
ruthwikdasyam Jun 2, 2026
8c3d772
feat: benchmarks added with recorder module
ruthwikdasyam Jun 2, 2026
a141dae
fix: pre-commit issues
ruthwikdasyam Jun 2, 2026
ffecde5
Merge remote-tracking branch 'origin/main' into ruthwik/hosted-teleop
ruthwikdasyam Jun 2, 2026
a3644ee
fix: local teleop blueprint changes
ruthwikdasyam Jun 2, 2026
de2c3ec
fix: max threshold for teleop
ruthwikdasyam Jun 2, 2026
af32aa9
fix: test_clock_sync move
ruthwikdasyam Jun 3, 2026
3397d3f
fix: split video_track code and sdpbundling
ruthwikdasyam Jun 3, 2026
f2684f4
Merge branch 'main' into ruthwik/hosted-teleop
ruthwikdasyam Jun 6, 2026
107f81b
fix: blueprints order
ruthwikdasyam Jun 6, 2026
31c4c67
fix: mpy issues
ruthwikdasyam Jun 6, 2026
75458a4
fix: blurprints
ruthwikdasyam Jun 6, 2026
41eb367
fix: video_stats msg to utils
ruthwikdasyam Jun 6, 2026
fe65780
fix: treat 0 as valid and float32-tolerant equality in teleop stats
ruthwikdasyam Jun 6, 2026
645c595
fix: route pong over state_reliable_back and fix restart event clear
ruthwikdasyam Jun 6, 2026
8a7aaf9
fix: pytest fixes
ruthwikdasyam Jun 6, 2026
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 18 additions & 9 deletions dimos/memory2/module.py
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,6 @@
from dimos.memory2.transform import QualityWindow
from dimos.memory2.type.observation import EmbeddedObservation, Observation
from dimos.models.embedding.base import EmbeddingModel
from dimos.models.embedding.clip import CLIPModel
from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
from dimos.msgs.sensor_msgs.Image import Image
from dimos.utils.data import backup_file
Expand Down Expand Up @@ -192,7 +191,7 @@ def store(self) -> SqliteStore:


class SemanticSearchConfig(MemoryModuleConfig):
embedding_model: type[EmbeddingModel] = CLIPModel
embedding_model: type[EmbeddingModel] | None = None


class SemanticSearch(MemoryModule):
Expand All @@ -204,7 +203,13 @@ class SemanticSearch(MemoryModule):
def start(self) -> None:
super().start()

self.model = self.register_disposable(self.config.embedding_model())
embedding_cls = self.config.embedding_model
if embedding_cls is None:
from dimos.models.embedding.clip import CLIPModel

embedding_cls = CLIPModel

self.model = self.register_disposable(embedding_cls())
self.model.start()

self.embeddings = self.store.stream("color_image_embedded", Image)
Expand Down Expand Up @@ -320,21 +325,25 @@ def _port_to_stream(self, name: str, input_topic: In[Any], stream: Stream[Any])

default_frame_id = self.config.default_frame_id
tf_tolerance = self.config.tf_tolerance
# Frames already warned about — a stream with no tf (e.g. a cmd_vel
# tagged 'keyboard') would otherwise log on every message.
warned_frames: set[str] = set()

def on_msg(msg: Any) -> None:
ts = getattr(msg, "ts", None) or time.time()
recv_ts = time.time()
ts = getattr(msg, "ts", None) or recv_ts
frame_id = getattr(msg, "frame_id", None) or default_frame_id
transform = self.tf.get("world", frame_id, time_point=ts, time_tolerance=tf_tolerance)
pose = transform.to_pose() if transform is not None else None

if not pose:
if not pose and frame_id not in warned_frames:
warned_frames.add(frame_id)
logger.warning(
"[%s] No tf available for frame '%s' at time %s (msg ts: %s), storing without pose",
"[%s] No tf for frame '%s' — recording without pose "
"(further such messages on this stream are not logged)",
name,
frame_id,
ts,
getattr(msg, "ts", None),
)
stream.append(msg, ts=ts, pose=pose)
stream.append(msg, ts=ts, pose=pose, tags={"recv_ts": recv_ts})

self.register_disposable(Disposable(input_topic.subscribe(on_msg)))
6 changes: 5 additions & 1 deletion dimos/msgs/geometry_msgs/TwistStamped.py
Original file line number Diff line number Diff line change
Expand Up @@ -39,18 +39,21 @@ class TwistStamped(Twist, Timestamped):
msg_name = "geometry_msgs.TwistStamped"
ts: float
frame_id: str
seq: int

@dispatch
def __init__(self, ts: float = 0.0, frame_id: str = "", **kwargs) -> None: # type: ignore[no-untyped-def]
def __init__(self, ts: float = 0.0, frame_id: str = "", seq: int = 0, **kwargs) -> None: # type: ignore[no-untyped-def]
self.frame_id = frame_id
self.ts = ts if ts != 0 else time.time()
self.seq = seq
super().__init__(**kwargs)

def lcm_encode(self) -> bytes:
lcm_msg = LCMTwistStamped()
lcm_msg.twist = self
[lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) # type: ignore[no-untyped-call]
lcm_msg.header.frame_id = self.frame_id
lcm_msg.header.seq = self.seq
return lcm_msg.lcm_encode() # type: ignore[no-any-return]

@classmethod
Expand All @@ -59,6 +62,7 @@ def lcm_decode(cls, data: bytes | BinaryIO) -> TwistStamped:
return cls(
ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000),
frame_id=lcm_msg.header.frame_id,
seq=lcm_msg.header.seq,
linear=[lcm_msg.twist.linear.x, lcm_msg.twist.linear.y, lcm_msg.twist.linear.z],
angular=[lcm_msg.twist.angular.x, lcm_msg.twist.angular.y, lcm_msg.twist.angular.z],
)
Expand Down
7 changes: 7 additions & 0 deletions dimos/robot/all_blueprints.py
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,8 @@
"openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_mock_planner_coordinator",
"openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_planner_coordinator",
"path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval",
"teleop-hosted-go2": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_go2",
"teleop-hosted-xarm7": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_xarm7",
"teleop-phone": "dimos.teleop.phone.blueprints:teleop_phone",
"teleop-phone-go2": "dimos.teleop.phone.blueprints:teleop_phone_go2",
"teleop-phone-go2-fleet": "dimos.teleop.phone.blueprints:teleop_phone_go2_fleet",
Expand Down Expand Up @@ -165,6 +167,10 @@
"gps-nav-skill-container": "dimos.agents.skills.gps_nav_skill.GpsNavSkillContainer",
"grasping-module": "dimos.manipulation.grasping.grasping.GraspingModule",
"gstreamer-camera-module": "dimos.hardware.sensors.camera.gstreamer.gstreamer_camera.GstreamerCameraModule",
"hosted-arm-teleop-module": "dimos.teleop.quest_hosted.hosted_extensions.HostedArmTeleopModule",
"hosted-teleop-module": "dimos.teleop.quest_hosted.hosted_teleop_module.HostedTeleopModule",
"hosted-teleop-recorder": "dimos.teleop.quest_hosted.blueprints.HostedTeleopRecorder",
"hosted-twist-teleop-module": "dimos.teleop.quest_hosted.hosted_extensions.HostedTwistTeleopModule",
"joint-trajectory-controller": "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller.JointTrajectoryController",
"joystick-module": "dimos.robot.unitree.b1.joystick_module.JoystickModule",
"keyboard-teleop": "dimos.robot.unitree.keyboard_teleop.KeyboardTeleop",
Expand Down Expand Up @@ -216,6 +222,7 @@
"spatial-memory": "dimos.perception.spatial_perception.SpatialMemory",
"speak-skill": "dimos.agents.skills.speak_skill.SpeakSkill",
"tare-planner": "dimos.navigation.nav_stack.modules.tare_planner.tare_planner.TarePlanner",
"teleop-recorder": "dimos.teleop.utils.recorder.TeleopRecorder",
"temporal-memory": "dimos.perception.experimental.temporal_memory.temporal_memory.TemporalMemory",
"terrain-analysis": "dimos.navigation.nav_stack.modules.terrain_analysis.terrain_analysis.TerrainAnalysis",
"terrain-map-ext": "dimos.navigation.nav_stack.modules.terrain_map_ext.terrain_map_ext.TerrainMapExt",
Expand Down
2 changes: 2 additions & 0 deletions dimos/robot/test_all_blueprints.py
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,8 @@
"coordinator-xarm6",
"coordinator-xarm7",
"dual-xarm6-planner",
"teleop-hosted-go2",
"teleop-hosted-xarm7",
"teleop-quest-dual",
"teleop-quest-go2",
"teleop-quest-piper",
Expand Down
10 changes: 9 additions & 1 deletion dimos/teleop/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -64,11 +64,16 @@ Filters to mobile-base axes (linear.x, linear.y, angular.z) and publishes as `Tw
```
teleop/
├── quest/
│ ├── quest_teleop_module.py # Base Quest teleop module
│ ├── quest_teleop_module.py # Base Quest teleop module (local WebSocket)
│ ├── quest_extensions.py # ArmTeleop, TwistTeleop
│ ├── quest_types.py # QuestControllerState, Buttons
│ └── web/
│ └── static/index.html # WebXR client
├── quest_hosted/
│ ├── hosted_teleop_module.py # Hosted Quest teleop (Cloudflare SFU broker)
│ ├── hosted_extensions.py # HostedArmTeleop, HostedTwistTeleop
│ ├── blueprints.py # Pre-wired blueprints
│ └── README.md # Channel/CF gotchas, threads, sidecars
├── phone/
│ ├── phone_teleop_module.py # Base Phone teleop module
│ ├── phone_extensions.py # SimplePhoneTeleop
Expand All @@ -77,6 +82,9 @@ teleop/
│ └── static/index.html # Mobile sensor web app
├── utils/
│ ├── teleop_transforms.py # WebXR → robot frame math
│ ├── recorder.py # Generic SQLite recorder (writes .db + report.md on stop)
│ ├── report.py # generate_report(db_path) — read .db, emit report.md + PNGs
│ └── stream_stats.py # LiveStreamStats + pcts/loss_pct (shared math)
└── blueprints.py # Module blueprints for easy instantiation
```

Expand Down
119 changes: 119 additions & 0 deletions dimos/teleop/quest_hosted/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,119 @@
# Hosted Teleop Module

Robot dials out to the [dimensional-teleop](https://github.com/dimensionalOS/dimensional-teleop)
broker (Cloudflare Realtime SFU) — no inbound ports needed. The browser/VR
operator connects through the broker; commands arrive over WebRTC datachannels,
robot video goes out as a WebRTC track.

## Files

- **`hosted_teleop_module.py`** — base. Owns the dial-out, datachannel
lifecycle, video send, clock-sync, and the command-plane telemetry pushed
back to the HUD. Subclassed for actuation.
- **`hosted_extensions.py`** — concrete subclasses: arm IK, mobile-base twist.
- **`blueprints.py`** — wires the module to a robot driver.

The operator HTML lives in the [dimensional-teleop](https://github.com/dimensionalOS/dimensional-teleop)
broker repo (`web/`), not here.

## How a session connects

1. Robot creates an `RTCPeerConnection` (MAX_BUNDLE, **must** — see below),
`addTrack(video)`, opens a throwaway negotiated DataChannel on SCTP id 0,
creates an offer, gathers ICE non-trickle.
2. `POST /api/v1/sessions` to the broker with the offer. Broker creates a CF
session, returns the answer + a `session_id` keyed off the robot.
3. SDP answer's candidates are propagated across bundled m-sections (aiortc
workaround — see below) before `setRemoteDescription`.
4. Heartbeat thread polls `/sessions/{id}/heartbeat`; each ack carries the SCTP
ids the broker has assigned for `cmd_unreliable`, `state_reliable`, and
`state_reliable_back`. We open / re-open / close negotiated channels to
track the broker's view.
5. Once `pc.connectionState == "connected"`, `CameraVideoTrack.arm()` starts
delivering frames (drops everything before the operator was actually able
to receive).
6. Telemetry thread pushes command-plane stats (latency / jitter / loss / rate
from the inbound twist stream) on `state_reliable_back` at `telemetry_hz`,
so the operator HUD can show what *arrived* — the operator only knows what
it *sent*.

## Datachannels (this is the trickiest part)

CF Realtime bridges datachannels **publisher → subscriber, one direction
only**. That's why we need two reliable channels — one for each direction:

| Channel | Direction | Reliable? | What it carries |
|---|---|---|---|
| `cmd_unreliable` | operator → robot | no (unordered, 0 retransmits) | TwistStamped / Joy / PoseStamped LCM |
| `state_reliable` | operator → robot | yes | JSON: `ping`, `clock_report`, `video_stats` |
| `state_reliable_back` | robot → operator | yes | JSON: `pong`, `robot_telemetry` |

All three are **negotiated by SCTP id** (broker assigns; we never pick).

### SCTP id 0 reservation (the throwaway DC)

A plain `createDataChannel` auto-grabs SCTP id 1 at connect time — same id the
broker tends to assign `cmd_unreliable`. Collision → `createDataChannel(id=1)`
throws. So at offer time we pin a *throwaway* negotiated channel to id 0
(reserved, never handed out by the broker). It also forces an SCTP m-line into
the offer so the SFU has a transport to bind the real channels to.

**Do not close that channel.** Under MAX_BUNDLE the SCTP shares the one bundled
ICE/DTLS transport with the video track; closing the only datachannel risks
the transport.

## aiortc / Cloudflare quirks (do not regress)

These are **hard-won, not in any docs** — corresponding fixes are commented at
the call sites but the *why* lives here:

- **MAX_BUNDLE is mandatory.** aiortc 1.14's default (BALANCED) puts video and
SCTP on separate ICE transports. CF Realtime publishes one bundled transport;
the video one fails ICE and you get a black quad forever. Force
`RTCBundlePolicy.MAX_BUNDLE` on `RTCConfiguration`.

- **`addTrack` BEFORE `createDataChannel`.** Otherwise the SCTP m-line is
created without a transceiver and aiortc's bundle-collapse discards the
shared transport. ICE never starts.

- **`_propagate_bundle_candidates`.** aiortc keys remote candidates by transport,
and under one bundled transport the *last* m-section processed wins. CF puts
`a=candidate` only on the video section; the empty SCTP section overwrites
it → remote-candidates=0 → ICE stalls at "checking". The helper replicates
the candidate block into every m-section that lacks one. **Do not remove.**

- **`makeXRCompatible()` on real hardware.** The operator side, not us, but
worth knowing: `xrCompatible: true` at context creation is not enough on
Quest — `await gl.makeXRCompatible()` is required before building the
`XRWebGLLayer`.

## Sidecar files

- **`/tmp/dimos_netem_profile`** — written by `data/notes/benchmarks/netem/apply.sh`
before a run; `TeleopRecorder`'s report writer reads it for the report
header so the netem profile is part of the artifact. We don't touch it.

## Threads (the runtime)

- **Event loop thread** (`HostedTeleopLoop`) — runs asyncio for aiortc + httpx.
Datachannel `send()` calls **must** happen here (aiortc datachannels aren't
thread-safe). The pong path is already on the loop (it fires from the
channel's `message` callback); the telemetry thread uses
`loop.call_soon_threadsafe` for the same reason.
- **Heartbeat thread** — HTTP polls the broker; reacts to channel-id changes.
- **Telemetry thread** — pushes `robot_telemetry` JSON on `state_reliable_back`
at `telemetry_hz` (default 3 Hz).
- **Control loop** — `control_loop_hz` (default 50 Hz). Calls the
subclass-overridable engage / publish hooks.

## Reconnect

Operator-side reconnect is handled in the broker (`fix3/reconnection`) — it
closes the stale `state_reliable_back` push (CF `datachannels/close`, not in
prose docs but in the OpenAPI spec) before re-pushing. CF does **not** auto-reap
datachannel pushes (the 30s GC is media-only), so without that close, the long-
lived robot session accumulates half-dead pushes and the second bridge 502s
with `repeated_local_track_error`.

Robot-side auto-redial (R2b in the roadmap) is not yet implemented and is
gated behind TURN landing first.
85 changes: 85 additions & 0 deletions dimos/teleop/quest_hosted/blueprints.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,85 @@
#!/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.

"""Hosted teleop blueprints (WebRTC transport)."""

from pathlib import Path

from dimos.constants import DIMOS_PROJECT_ROOT
from dimos.control.blueprints.teleop import coordinator_teleop_xarm7
from dimos.core.coordination.blueprints import autoconnect
from dimos.core.global_config import global_config
from dimos.core.transport import LCMTransport
from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic
from dimos.teleop.quest.quest_types import Buttons
from dimos.teleop.quest_hosted.hosted_extensions import (
HostedArmTeleopModule,
HostedTwistTeleopModule,
)
from dimos.teleop.utils.recorder import TeleopRecorder, TeleopRecorderConfig

global_config.rerun_open = "none"

# Single XArm7 teleop via the hosted (WebRTC) client. Pass `--simulation` to
# run the coordinator inside MuJoCo, omit it for real hardware.
teleop_hosted_xarm7 = autoconnect(
HostedArmTeleopModule.blueprint(task_names={"right": "teleop_xarm"}),
coordinator_teleop_xarm7,
).transports(
{
("right_controller_output", PoseStamped): LCMTransport(
"/coordinator/cartesian_command", PoseStamped
),
("buttons", Buttons): LCMTransport("/teleop/buttons", Buttons),
}
)


# viewer="none" drops the rerun window (operator gets video over WebRTC, so the
# robot-side rerun view is unwanted here).
teleop_hosted_go2 = autoconnect(
HostedTwistTeleopModule.blueprint(),
unitree_go2_basic,
).global_config(n_workers=8, viewer="none")


HOSTED_RECORDINGS_DIR = DIMOS_PROJECT_ROOT / "data/hosted_teleop/recordings"


class HostedTeleopRecorderConfig(TeleopRecorderConfig):
# Same generic recorder, just defaulting recordings into the hosted dir.
db_path: str | Path = HOSTED_RECORDINGS_DIR / "recording_hosted.db"


class HostedTeleopRecorder(TeleopRecorder):
"""Generic ``TeleopRecorder`` defaulting to the hosted recordings dir.

Ports + per-run timestamping are inherited; this only changes the default
output path. Compose at the CLI::

dimos run teleop-hosted-xarm7 hosted-teleop-recorder
dimos run teleop-hosted-go2 hosted-teleop-recorder
"""

config: HostedTeleopRecorderConfig


__all__ = [
"HostedTeleopRecorder",
"HostedTeleopRecorderConfig",
"teleop_hosted_go2",
"teleop_hosted_xarm7",
]
Loading
Loading