From 52f85ded367ef1807e5043971b651b8005f927fb Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Tue, 14 Jul 2026 19:57:45 -0500 Subject: [PATCH 01/26] feat(spot): minimal Spot velocity control + keyboard teleop blueprint Add SpotCmdVel (cmd_vel -> Boston Dynamics body velocity via bosdyn-client) with WiFi/Ethernet IP auto-detect, and a `spot` blueprint wiring keyboard teleop + a Rerun viewer. SpotCmdVel runs in a dedicated worker like the other robot connection modules so its Zenoh RPC server isn't wedged by sharing a process with the pygame teleop. Make KeyboardTeleop's forced SDL x11 driver Linux-only so the teleop window opens on macOS (cocoa) instead of failing at startup. --- dimos/robot/all_blueprints.py | 2 + dimos/robot/bosdyn/spot/blueprints/spot.py | 44 +++ dimos/robot/bosdyn/spot/cmd_vel.py | 346 +++++++++++++++++++++ dimos/robot/unitree/keyboard_teleop.py | 7 +- pyproject.toml | 8 + uv.lock | 87 +++++- 6 files changed, 490 insertions(+), 4 deletions(-) create mode 100644 dimos/robot/bosdyn/spot/blueprints/spot.py create mode 100644 dimos/robot/bosdyn/spot/cmd_vel.py diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 643830a719..d2ce574dec 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -80,6 +80,7 @@ "openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_mock_planner_coordinator", "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_planner_coordinator", "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", + "spot": "dimos.robot.bosdyn.spot.blueprints.spot:spot", "teleop-hosted-go2": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_go2", "teleop-hosted-go2-transport": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_go2_transport", "teleop-hosted-xarm7": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_xarm7", @@ -250,6 +251,7 @@ "simple-planner": "dimos.navigation.cmu_nav.modules.simple_planner.simple_planner.SimplePlanner", "spatial-memory": "dimos.perception.spatial_perception.SpatialMemory", "speak-skill": "dimos.agents.skills.speak_skill.SpeakSkill", + "spot-cmd-vel": "dimos.robot.bosdyn.spot.cmd_vel.SpotCmdVel", "static-tf-publisher": "dimos.protocol.tf.static_tf_publisher.StaticTfPublisher", "tare-planner": "dimos.navigation.cmu_nav.modules.tare_planner.tare_planner.TarePlanner", "teleop-recorder": "dimos.teleop.utils.recorder.TeleopRecorder", diff --git a/dimos/robot/bosdyn/spot/blueprints/spot.py b/dimos/robot/bosdyn/spot/blueprints/spot.py new file mode 100644 index 0000000000..8e83c2589a --- /dev/null +++ b/dimos/robot/bosdyn/spot/blueprints/spot.py @@ -0,0 +1,44 @@ +# 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. + +"""Boston Dynamics Spot keyboard teleop with a Rerun viewer. + +Pygame keyboard -> Twist on `cmd_vel` -> SpotCmdVel -> robot, plus a Rerun +bridge that spawns the viewer. WASD to move/turn, QE to strafe, Space for +e-stop, ESC to quit. + +The ip auto-detects: with no `-o spotcmdvel.ip=` given, it probes Spot's WiFi +AP address (192.168.80.3) then the Ethernet address (10.0.0.3) and uses +whichever answers. + +Usage: + dimos run spot \ + -o spotcmdvel.username=admin \ + -o spotcmdvel.password= + # or force an address: + dimos run spot ... -o spotcmdvel.ip=10.0.0.3 +""" + +from __future__ import annotations + +from dimos.core.coordination.blueprints import autoconnect +from dimos.robot.bosdyn.spot.cmd_vel import SpotCmdVel +from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop +from dimos.visualization.rerun.bridge import RerunBridgeModule + +spot = autoconnect( + SpotCmdVel.blueprint(), + KeyboardTeleop.blueprint(), + RerunBridgeModule.blueprint(), +) diff --git a/dimos/robot/bosdyn/spot/cmd_vel.py b/dimos/robot/bosdyn/spot/cmd_vel.py new file mode 100644 index 0000000000..92333b1fbc --- /dev/null +++ b/dimos/robot/bosdyn/spot/cmd_vel.py @@ -0,0 +1,346 @@ +# 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. + +"""Minimal Boston Dynamics Spot velocity control via the `bosdyn-client` SDK. + +A single async `Module`: `main()` acquires the lease/E-stop, powers on, and +stands the robot, then hands control to the `cmd_vel` stream. Each Twist is +forwarded to Spot as a synchronized body velocity command. Teardown sits the +robot and powers the motors back off. + +`bosdyn` is an optional extra (`uv sync --extra spot`); its imports live inside +`main()` so this file stays importable — and blueprint discovery keeps working — +on hosts where the SDK isn't installed. +""" + +from __future__ import annotations + +import asyncio +from collections.abc import AsyncIterator +from dataclasses import field +import os +import time +from typing import Any + +from dimos.agents.annotation import skill +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +_POWER_ON_TIMEOUT_S = 20.0 +_POWER_OFF_TIMEOUT_S = 20.0 +_STAND_TIMEOUT_S = 10.0 +_SIT_TIMEOUT_S = 10.0 + +# Spot's fixed default addresses: 192.168.80.3 when it hosts its own WiFi AP, +# 10.0.0.3 on the rear Ethernet port. Probed in order when no ip is given. +_SPOT_WIFI_AP_IP = "192.168.80.3" +_SPOT_ETHERNET_IP = "10.0.0.3" +_IP_LABELS = {_SPOT_WIFI_AP_IP: "WiFi", _SPOT_ETHERNET_IP: "Ethernet"} +# Spot's gRPC API listens on HTTPS/443; a TCP connect confirms real reachability +# (and matches what the SDK does) better than an ICMP ping. +_SPOT_API_PORT = 443 +_REACHABILITY_PROBE_TIMEOUT_S = 2.0 + + +class SpotCmdVelConfig(ModuleConfig): + """Connection, credentials, and safety gating for a Spot robot.""" + + # Explicit address always wins (`-o spotcmdvel.ip=`). When left blank, + # main() probes `candidate_ips` and uses the first that answers on the API + # port — so plugging in over Ethernet or joining Spot's WiFi both "just work". + ip: str = "" + candidate_ips: list[str] = field(default_factory=lambda: [_SPOT_WIFI_AP_IP, _SPOT_ETHERNET_IP]) + + # Auth — falls back to BOSDYN_CLIENT_USERNAME / BOSDYN_CLIENT_PASSWORD at + # start time when left as None. Startup fails fast if neither is supplied. + username: str | None = None + password: str | None = None + + # Safety / startup gating. + enable_estop: bool = True + acquire_lease: bool = True + power_on_at_start: bool = True + stand_at_start: bool = True + + # Spot rejects velocity commands without an `end_time_secs`. This duration + # is added to now() for each command and doubles as the auto-stop window: + # if the cmd_vel stream stalls for longer than this, the robot halts. + cmd_vel_timeout: float = 0.5 + + # Spot E-stops itself if it doesn't see a keep-alive check-in within this + # window. 9.0 s matches the bosdyn-client default. + estop_timeout: float = 9.0 + + +class SpotCmdVel(Module): + """Drives a Boston Dynamics Spot from a `cmd_vel` Twist stream.""" + + # A hardware-driving module gets its own worker process, matching the other + # robot connection modules (go2/b1/drone). Sharing a process with a GUI + # module like KeyboardTeleop wedges this module's RPC server at startup. + dedicated_worker = True + + cmd_vel: In[Twist] + + config: SpotCmdVelConfig + + def __init__(self, *args: Any, **kwargs: Any) -> None: + super().__init__(*args, **kwargs) + self._robot: Any = None + self._command_client: Any = None + self._state_client: Any = None + self._estop_keepalive: Any = None + self._lease_keepalive: Any = None + self._standing = False + # cmd_vel handlers wait on this so a velocity command issued mid-setup + # never reaches a half-initialised SDK. + self._ready = asyncio.Event() + + async def main(self) -> AsyncIterator[None]: + username, password = self._resolve_credentials() + ip = self.config.ip or await self._resolve_ip() + + from bosdyn.client import create_standard_sdk # type: ignore[import-not-found] + from bosdyn.client.estop import ( # type: ignore[import-not-found] + EstopClient, + EstopEndpoint, + EstopKeepAlive, + ) + from bosdyn.client.lease import ( # type: ignore[import-not-found] + LeaseClient, + LeaseKeepAlive, + ) + from bosdyn.client.robot_command import ( # type: ignore[import-not-found] + RobotCommandClient, + blocking_stand, + ) + from bosdyn.client.robot_state import ( # type: ignore[import-not-found] + RobotStateClient, + ) + + logger.info(f"Connecting to Spot at {ip}") + sdk = await asyncio.to_thread(create_standard_sdk, "dimos-spot") + self._robot = await asyncio.to_thread(sdk.create_robot, ip) + await asyncio.to_thread(self._robot.authenticate, username, password) + await asyncio.to_thread(self._robot.time_sync.wait_for_sync) + + if self.config.enable_estop: + estop_client = self._robot.ensure_client(EstopClient.default_service_name) + endpoint = EstopEndpoint( + client=estop_client, + name="dimos-spot", + estop_timeout=self.config.estop_timeout, + ) + await asyncio.to_thread(endpoint.force_simple_setup) + self._estop_keepalive = EstopKeepAlive(endpoint) + + if self.config.acquire_lease: + lease_client = self._robot.ensure_client(LeaseClient.default_service_name) + await asyncio.to_thread(lease_client.take) + self._lease_keepalive = LeaseKeepAlive(lease_client) + + self._command_client = self._robot.ensure_client(RobotCommandClient.default_service_name) + self._state_client = self._robot.ensure_client(RobotStateClient.default_service_name) + + if self.config.power_on_at_start: + logger.info("Powering on Spot motors") + await asyncio.to_thread(self._robot.power_on, timeout_sec=_POWER_ON_TIMEOUT_S) + + if self.config.stand_at_start: + logger.info("Standing Spot") + await asyncio.to_thread( + blocking_stand, self._command_client, timeout_sec=_STAND_TIMEOUT_S + ) + self._standing = True + + self._ready.set() + logger.info("Spot cmd_vel control ready") + + yield + + self._ready.clear() + if self._standing: + try: + from bosdyn.client.robot_command import ( # type: ignore[import-not-found] + blocking_sit, + ) + + await asyncio.to_thread( + blocking_sit, self._command_client, timeout_sec=_SIT_TIMEOUT_S + ) + except Exception as error: + logger.error(f"Spot sit during teardown failed: {error}") + self._standing = False + + if self.config.power_on_at_start and self._robot is not None: + try: + await asyncio.to_thread( + self._robot.power_off, cut_immediately=False, timeout_sec=_POWER_OFF_TIMEOUT_S + ) + except Exception as error: + logger.error(f"Spot power_off during teardown failed: {error}") + + for keepalive_name, keepalive in ( + ("lease", self._lease_keepalive), + ("estop", self._estop_keepalive), + ): + if keepalive is not None: + try: + await asyncio.to_thread(keepalive.shutdown) + except Exception as error: + logger.error(f"Spot {keepalive_name} shutdown failed: {error}") + self._lease_keepalive = None + self._estop_keepalive = None + logger.info("Spot cmd_vel control torn down") + + async def handle_cmd_vel(self, msg: Twist) -> None: + if not self._ready.is_set(): + return + await self._send_velocity(msg.linear.x, msg.linear.y, msg.angular.z) + + async def _send_velocity( + self, forward: float, strafe: float, yaw: float, duration: float = 0.0 + ) -> bool: + from bosdyn.client.robot_command import ( # type: ignore[import-not-found] + RobotCommandBuilder, + ) + + command = RobotCommandBuilder.synchro_velocity_command(v_x=forward, v_y=strafe, v_rot=yaw) + window = duration if duration > 0 else self.config.cmd_vel_timeout + try: + await asyncio.to_thread( + self._command_client.robot_command, + command, + end_time_secs=time.time() + window, + ) + return True + except Exception as error: + logger.error(f"Spot velocity command failed: {error}") + return False + + @rpc + async def move(self, twist: Twist, duration: float = 0.0) -> bool: + """Send a Twist as a body velocity command, optionally for `duration` seconds.""" + return await self._send_velocity(twist.linear.x, twist.linear.y, twist.angular.z, duration) + + @rpc + async def get_state(self) -> str: + if self._state_client is None: + return "DISCONNECTED" + try: + state = await asyncio.to_thread(self._state_client.get_robot_state) + return str(state.power_state.motor_power_state) + except Exception as error: + logger.error(f"Spot get_state failed: {error}") + return "UNKNOWN" + + @skill + async def move_velocity( + self, x: float, y: float = 0.0, yaw: float = 0.0, duration: float = 0.0 + ) -> str: + """Move Spot with a direct body velocity command. + + Args: + x: Forward velocity (m/s). + y: Left/right velocity (m/s). + yaw: Rotational velocity (rad/s). + duration: Seconds to move. 0 uses one `cmd_vel_timeout` window. + """ + twist = Twist(linear=Vector3(x, y, 0), angular=Vector3(0, 0, yaw)) + if await self.move(twist, duration=duration): + return f"Moving with velocity=({x}, {y}, {yaw}) for {duration} seconds" + return f"Failed to move with velocity=({x}, {y}, {yaw})" + + @skill + async def stand(self) -> str: + """Make Spot stand up. Spot must already be powered on.""" + if self._command_client is None: + return "Spot is not connected." + try: + from bosdyn.client.robot_command import ( # type: ignore[import-not-found] + blocking_stand, + ) + + await asyncio.to_thread( + blocking_stand, self._command_client, timeout_sec=_STAND_TIMEOUT_S + ) + self._standing = True + return "Spot is standing." + except Exception as error: + logger.error(f"Spot stand failed: {error}") + return f"Stand failed: {error}" + + @skill + async def sit(self) -> str: + """Make Spot sit down.""" + if self._command_client is None: + return "Spot is not connected." + try: + from bosdyn.client.robot_command import ( # type: ignore[import-not-found] + blocking_sit, + ) + + await asyncio.to_thread(blocking_sit, self._command_client, timeout_sec=_SIT_TIMEOUT_S) + self._standing = False + return "Spot is sitting." + except Exception as error: + logger.error(f"Spot sit failed: {error}") + return f"Sit failed: {error}" + + async def _resolve_ip(self) -> str: + for candidate in self.config.candidate_ips: + if await self._is_reachable(candidate): + logger.info(f"Spot reachable at {candidate}") + return candidate + described = " or ".join( + f"{candidate} ({_IP_LABELS[candidate]})" if candidate in _IP_LABELS else candidate + for candidate in self.config.candidate_ips + ) + raise ConnectionError( + f"I'm unable to connect to {described}. Did you forget to connect to " + "Spot's WiFi or plug in an Ethernet cable to Spot?" + ) + + async def _is_reachable(self, ip: str) -> bool: + try: + _, writer = await asyncio.wait_for( + asyncio.open_connection(ip, _SPOT_API_PORT), + timeout=_REACHABILITY_PROBE_TIMEOUT_S, + ) + except (OSError, asyncio.TimeoutError): + return False + # Reachable — a successful TCP handshake is all we need. Close without + # awaiting wait_closed(); the API port speaks TLS and never completes a + # clean plaintext close, which would otherwise hang the probe. + writer.close() + return True + + def _resolve_credentials(self) -> tuple[str, str]: + username = self.config.username or os.environ.get("BOSDYN_CLIENT_USERNAME") + password = self.config.password or os.environ.get("BOSDYN_CLIENT_PASSWORD") + if not username or not password: + raise ValueError( + "Spot credentials missing — set username/password in config " + "or BOSDYN_CLIENT_USERNAME / BOSDYN_CLIENT_PASSWORD env vars" + ) + return username, password + + +__all__ = ["SpotCmdVel", "SpotCmdVelConfig"] diff --git a/dimos/robot/unitree/keyboard_teleop.py b/dimos/robot/unitree/keyboard_teleop.py index 07af844c60..b9a8a8e707 100644 --- a/dimos/robot/unitree/keyboard_teleop.py +++ b/dimos/robot/unitree/keyboard_teleop.py @@ -14,6 +14,7 @@ # limitations under the License. import os +import sys import threading from typing import Any @@ -29,8 +30,10 @@ logger = setup_logger() -# Force X11 driver to avoid OpenGL threading issues -os.environ["SDL_VIDEODRIVER"] = "x11" +# Force X11 driver on Linux to avoid OpenGL threading issues. macOS has no X11 +# driver (SDL uses cocoa); forcing x11 there makes pygame.display fail outright. +if sys.platform.startswith("linux"): + os.environ["SDL_VIDEODRIVER"] = "x11" DEFAULT_LINEAR_SPEED: float = 0.5 # m/s DEFAULT_ANGULAR_SPEED: float = 0.8 # rad/s diff --git a/pyproject.toml b/pyproject.toml index f75f4ba326..7c818d04eb 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -197,6 +197,12 @@ misc = [ "portal", ] +spot = [ + "bosdyn-client>=4.0.0", + "bosdyn-api>=4.0.0", + "bosdyn-core>=4.0.0", +] + visualization = [ "rerun-sdk==0.32.0", "dimos-viewer==0.32.0a1", @@ -551,6 +557,8 @@ exclude = "^dimos/models/Detic(/|$)|.*/test_.|.*/tool_.|.*/conftest.py*" module = [ "a750_control", "a750_control.*", + "bosdyn", + "bosdyn.*", "cyclonedds", "cyclonedds.*", "dimos_lcm.*", diff --git a/uv.lock b/uv.lock index b3dfd7a5a8..c5d0b63b82 100644 --- a/uv.lock +++ b/uv.lock @@ -26,7 +26,7 @@ resolution-markers = [ ] [options] -exclude-newer = "0001-01-01T00:00:00Z" # This has no effect and is included for backwards compatibility when using relative exclude-newer values. +exclude-newer = "2026-07-07T21:43:28.747446Z" exclude-newer-span = "P7D" [options.exclude-newer-package] @@ -517,6 +517,48 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/10/cb/f2ad4230dc2eb1a74edf38f1a38b9b52277f75bef262d8908e60d957e13c/blinker-1.9.0-py3-none-any.whl", hash = "sha256:ba0efaa9080b619ff2f3459d1d500c57bddea4a6b424b60a91141db6fd2f08bc", size = 8458, upload-time = "2024-11-08T17:25:46.184Z" }, ] +[[package]] +name = "bosdyn-api" +version = "5.1.4" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "protobuf" }, +] +wheels = [ + { url = "https://files.pythonhosted.org/packages/17/1a/25e0923342e4a11e1ace03afcfadbe5757e408fe9dd92179c9f53a527341/bosdyn_api-5.1.4-py3-none-any.whl", hash = "sha256:2da734a902434ed92a0039e60bb943df0ad49643fa7c0e5d91346b60b8a04d06", size = 420740, upload-time = "2026-03-18T13:49:30.379Z" }, +] + +[[package]] +name = "bosdyn-client" +version = "5.1.4" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "bosdyn-api" }, + { name = "bosdyn-core" }, + { name = "deprecated" }, + { name = "grpcio" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "pyjwt" }, + { name = "pynmea2" }, + { name = "requests" }, +] +wheels = [ + { url = "https://files.pythonhosted.org/packages/ec/aa/ea17bb7a5c509e41bf7ff61d2bc72dea8d5a7a4ad3a2a9eca1618214eebc/bosdyn_client-5.1.4-py3-none-any.whl", hash = "sha256:5b49c94340adaa5ee9c32b8cd8aff22b733b8caf024630752a648398a8eb8718", size = 308263, upload-time = "2026-03-18T13:49:33.571Z" }, +] + +[[package]] +name = "bosdyn-core" +version = "5.1.4" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "bosdyn-api" }, + { name = "deprecated" }, +] +wheels = [ + { url = "https://files.pythonhosted.org/packages/a7/bb/fb78b84e5e5b1dda58625be89f3cd66aa48b5e9600689d8e0b9b3eb5667c/bosdyn_core-5.1.4-py3-none-any.whl", hash = "sha256:b4661fb162fe50e788268530c21fca91ca25e6f595ac9535722954145995b9ac", size = 32228, upload-time = "2026-03-18T13:49:36.964Z" }, +] + [[package]] name = "brax" version = "0.14.1" @@ -1519,6 +1561,18 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/4e/8c/f3147f5c4b73e7550fe5f9352eaa956ae838d5c51eb58e7a25b9f3e2643b/decorator-5.2.1-py3-none-any.whl", hash = "sha256:d316bb415a2d9e2d2b3abcc4084c6502fc09240e292cd76a76afc106a1c8e04a", size = 9190, upload-time = "2025-02-24T04:41:32.565Z" }, ] +[[package]] +name = "deprecated" +version = "1.2.18" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "wrapt" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/98/97/06afe62762c9a8a86af0cfb7bfdab22a43ad17138b07af5b1a58442690a2/deprecated-1.2.18.tar.gz", hash = "sha256:422b6f6d859da6f2ef57857761bfb392480502a64c3028ca9bbe86085d72115d", size = 2928744, upload-time = "2025-01-27T10:46:25.7Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/6e/c6/ac0b6c1e2d138f1002bcf799d330bd6d85084fece321e662a14223794041/Deprecated-1.2.18-py2.py3-none-any.whl", hash = "sha256:bd5011788200372a32418f888e326a09ff80d0214bd961147cfed01b5c018eec", size = 9998, upload-time = "2025-01-27T10:46:09.186Z" }, +] + [[package]] name = "dill" version = "0.4.1" @@ -1759,6 +1813,11 @@ sim = [ { name = "playground" }, { name = "pygame" }, ] +spot = [ + { name = "bosdyn-api" }, + { name = "bosdyn-client" }, + { name = "bosdyn-core" }, +] unitree = [ { name = "chromadb" }, { name = "dimos-viewer" }, @@ -2008,6 +2067,9 @@ requires-dist = [ { name = "aiortc", marker = "extra == 'webrtc'", specifier = ">=1.14.0" }, { name = "annotation-protocol", specifier = ">=1.4.0" }, { name = "bleak", specifier = ">=3.0.2" }, + { name = "bosdyn-api", marker = "extra == 'spot'", specifier = ">=4.0.0" }, + { name = "bosdyn-client", marker = "extra == 'spot'", specifier = ">=4.0.0" }, + { name = "bosdyn-core", marker = "extra == 'spot'", specifier = ">=4.0.0" }, { name = "chromadb", marker = "extra == 'perception'", specifier = ">=1.0.0" }, { name = "coacd", marker = "extra == 'scene'", specifier = ">=1.0.0" }, { name = "cryptography", specifier = ">=46.0.5" }, @@ -2121,7 +2183,7 @@ requires-dist = [ { name = "xarm-python-sdk", marker = "extra == 'misc'", specifier = ">=1.17.0" }, { name = "yourdfpy", marker = "(platform_machine != 'aarch64' and extra == 'visualization') or (sys_platform != 'linux' and extra == 'visualization')", specifier = ">=0.0.60" }, ] -provides-extras = ["misc", "visualization", "learning", "agents", "web", "perception", "unitree", "unitree-dds", "manipulation", "cpu", "cuda", "sim", "mapping", "drone", "dds", "webrtc", "base", "apriltag", "scene", "all"] +provides-extras = ["misc", "spot", "visualization", "learning", "agents", "web", "perception", "unitree", "unitree-dds", "manipulation", "cpu", "cuda", "sim", "mapping", "drone", "dds", "webrtc", "base", "apriltag", "scene", "all"] [package.metadata.requires-dev] autofix = [{ name = "ruff", specifier = "==0.14.3" }] @@ -6824,6 +6886,18 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/c7/21/705964c7812476f378728bdf590ca4b771ec72385c533964653c68e86bdc/pygments-2.19.2-py3-none-any.whl", hash = "sha256:86540386c03d588bb81d44bc3928634ff26449851e99741617ecb9037ee5ec0b", size = 1225217, upload-time = "2025-06-21T13:39:07.939Z" }, ] +[[package]] +name = "pyjwt" +version = "2.13.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "typing-extensions", marker = "python_full_version < '3.11'" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/3b/81/58d0ac84e1ef3a3843791d6954d94c0b33d526c75eeb1efbce9d0a4c4077/pyjwt-2.13.0.tar.gz", hash = "sha256:41571c89ca91598c79e8ef18a2d07367d4810fbbd6f637794879baf1b7703423", size = 107515, upload-time = "2026-05-21T19:54:36.618Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/a3/5e/ecf12fdb62546d64385c158514e9b2b671f7832108ef2ecd2020ce0af2d1/pyjwt-2.13.0-py3-none-any.whl", hash = "sha256:66adcc2aff09b3f1bbd95fc1e1577df8ac8723c978552fd43304c8a290ac5728", size = 31274, upload-time = "2026-05-21T19:54:35.362Z" }, +] + [[package]] name = "pylibsrtp" version = "1.0.0" @@ -6913,6 +6987,15 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/25/ca/995d1201925ad49fb6b174a9d488f1d90b77256b1088ebd3d7f192b0f65a/pymavlink-2.4.49-cp312-cp312-win_arm64.whl", hash = "sha256:c7415592166d9cbd4434775828b00c71bebf292c8367744d861e3ccd2dab9f3e", size = 6231742, upload-time = "2025-08-01T23:32:20.707Z" }, ] +[[package]] +name = "pynmea2" +version = "1.19.0" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/c0/d7/5d0e36d00cd8aa19a6e717bcd1216ca805d86865501dfebdd18667c9edbd/pynmea2-1.19.0.tar.gz", hash = "sha256:1daa79b93279f887d1c235e5cc5c79e32644564138ce46989ab0f4a2fc970d7c", size = 36240, upload-time = "2023-01-19T20:52:56.289Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/75/24/1f575eb17a8135e54b3c243ff87e2f4d6b2389942836021d0628ed837559/pynmea2-1.19.0-py3-none-any.whl", hash = "sha256:5138558b4fb5daa587b2c17de99eb43df0297039de1c98010c996624abfb00eb", size = 30016, upload-time = "2023-01-19T20:52:54.38Z" }, +] + [[package]] name = "pyobjc-core" version = "12.1" From 9cdad08187ec5aec6ee56d1fb8fef408f86a08d9 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 15 Jul 2026 19:33:45 -0500 Subject: [PATCH 02/26] cmd_vel works --- dimos/robot/all_blueprints.py | 3 +- .../bosdyn/spot/blueprints/spot_cmd_vel.py | 51 ++++++++ dimos/robot/bosdyn/spot/config.py | 110 ++++++++++++++++++ .../bosdyn/spot/{ => effectors}/cmd_vel.py | 97 +++++---------- 4 files changed, 192 insertions(+), 69 deletions(-) create mode 100644 dimos/robot/bosdyn/spot/blueprints/spot_cmd_vel.py create mode 100644 dimos/robot/bosdyn/spot/config.py rename dimos/robot/bosdyn/spot/{ => effectors}/cmd_vel.py (79%) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index d2ce574dec..0436ed6528 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -81,6 +81,7 @@ "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_planner_coordinator", "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", "spot": "dimos.robot.bosdyn.spot.blueprints.spot:spot", + "spot-cmd-vel": "dimos.robot.bosdyn.spot.blueprints.spot_cmd_vel:spot_cmd_vel", "teleop-hosted-go2": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_go2", "teleop-hosted-go2-transport": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_go2_transport", "teleop-hosted-xarm7": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_xarm7", @@ -251,7 +252,7 @@ "simple-planner": "dimos.navigation.cmu_nav.modules.simple_planner.simple_planner.SimplePlanner", "spatial-memory": "dimos.perception.spatial_perception.SpatialMemory", "speak-skill": "dimos.agents.skills.speak_skill.SpeakSkill", - "spot-cmd-vel": "dimos.robot.bosdyn.spot.cmd_vel.SpotCmdVel", + "spot-high-level": "dimos.robot.bosdyn.spot.effectors.high_level.SpotHighLevel", "static-tf-publisher": "dimos.protocol.tf.static_tf_publisher.StaticTfPublisher", "tare-planner": "dimos.navigation.cmu_nav.modules.tare_planner.tare_planner.TarePlanner", "teleop-recorder": "dimos.teleop.utils.recorder.TeleopRecorder", diff --git a/dimos/robot/bosdyn/spot/blueprints/spot_cmd_vel.py b/dimos/robot/bosdyn/spot/blueprints/spot_cmd_vel.py new file mode 100644 index 0000000000..8709544815 --- /dev/null +++ b/dimos/robot/bosdyn/spot/blueprints/spot_cmd_vel.py @@ -0,0 +1,51 @@ +# 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. + +"""Boston Dynamics Spot keyboard teleop + full sensor streaming with a Rerun viewer. + +Pygame keyboard -> Twist on `cmd_vel` -> SpotHighLevel -> robot. The same module +streams the five fisheye + five depth cameras and body odometry. A Rerun bridge +spawns the viewer. WASD to move/turn, QE to strafe, Space for e-stop, ESC to quit. + +The ip auto-detects: with no `-o spothighlevel.ip=` given, it probes Spot's WiFi +AP address (192.168.80.3) then the Ethernet address (10.0.0.3) and uses +whichever answers. + +Usage: + dimos run spot \ + -o spothighlevel.username=admin -o spothighlevel.password= + # or force an address: + dimos run spot ... -o spothighlevel.ip=10.0.0.3 +""" + +from __future__ import annotations + +from dimos.core.coordination.blueprints import autoconnect +from dimos.robot.bosdyn.spot.effectors.cmd_vel import SpotCmdVel + +# from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop +from dimos.visualization.vis_module import RerunWebSocketServer, vis_module + +spot_cmd_vel = autoconnect( + SpotCmdVel.blueprint(), + # KeyboardTeleop.blueprint(), + vis_module( + "rerun", + rerun_config={}, + ).remappings( + [ + (RerunWebSocketServer, "tele_cmd_vel", "cmd_vel"), + ] + ), +) diff --git a/dimos/robot/bosdyn/spot/config.py b/dimos/robot/bosdyn/spot/config.py new file mode 100644 index 0000000000..9c6a68aed0 --- /dev/null +++ b/dimos/robot/bosdyn/spot/config.py @@ -0,0 +1,110 @@ +# 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. + +"""Constants and pure address/credential helpers shared by the Spot modules. + +Intentionally free of any `bosdyn` import so it stays importable — and blueprint +discovery keeps working — on hosts without the SDK. +""" + +from __future__ import annotations + +import asyncio + +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +# Spot's fixed default addresses: 192.168.80.3 when it hosts its own WiFi AP, +# 10.0.0.3 on the rear Ethernet port. Probed in order when no ip is given. +SPOT_WIFI_AP_IP = "192.168.80.3" +SPOT_ETHERNET_IP = "10.0.0.3" +IP_LABELS = {SPOT_WIFI_AP_IP: "WiFi", SPOT_ETHERNET_IP: "Ethernet"} + +# Spot's gRPC API listens on HTTPS/443; a TCP connect confirms real reachability +# (and matches what the SDK does) better than an ICMP ping. +SPOT_API_PORT = 443 +REACHABILITY_PROBE_TIMEOUT_S = 2.0 + +# Motor power / posture command timeouts (seconds). +POWER_ON_TIMEOUT_S = 20.0 +POWER_OFF_TIMEOUT_S = 20.0 +STAND_TIMEOUT_S = 10.0 +SIT_TIMEOUT_S = 10.0 + +# Spot's five body fisheye cameras and their matching depth cameras, ordered to +# match the grayscale_image_N / depth_image_N output streams. +GRAYSCALE_SOURCES = [ + "frontleft_fisheye_image", + "frontright_fisheye_image", + "left_fisheye_image", + "right_fisheye_image", + "back_fisheye_image", +] +DEPTH_SOURCES = [ + "frontleft_depth", + "frontright_depth", + "left_depth", + "right_depth", + "back_depth", +] + +# Spot reports poses in its gravity-agnostic "vision" frame; body is the moving +# base frame. These mirror the bosdyn frame-helper constants, inlined so this +# file imports without the SDK. +VISION_FRAME = "vision" +BODY_FRAME = "body" + + +def default_candidate_ips() -> list[str]: + return [SPOT_WIFI_AP_IP, SPOT_ETHERNET_IP] + + +async def is_reachable(ip: str) -> bool: + try: + _, writer = await asyncio.wait_for( + asyncio.open_connection(ip, SPOT_API_PORT), + timeout=REACHABILITY_PROBE_TIMEOUT_S, + ) + except (OSError, asyncio.TimeoutError): + return False + # Reachable — a successful TCP handshake is all we need. Close without + # awaiting wait_closed(); the API port speaks TLS and never completes a + # clean plaintext close, which would otherwise hang the probe. + writer.close() + return True + + +async def resolve_ip(candidate_ips: list[str]) -> str: + for candidate in candidate_ips: + if await is_reachable(candidate): + logger.info(f"Spot reachable at {candidate}") + return candidate + described = " or ".join( + f"{candidate} ({IP_LABELS[candidate]})" if candidate in IP_LABELS else candidate + for candidate in candidate_ips + ) + raise ConnectionError( + f"I'm unable to connect to {described}. Did you forget to connect to " + "Spot's WiFi or plug in an Ethernet cable to Spot?" + ) + + +def resolve_credentials(username: str | None, password: str | None) -> tuple[str, str]: + if not username or not password: + raise ValueError( + "Spot credentials missing — pass username/password in config " + "(-o .username=... -o .password=...)" + ) + return username, password diff --git a/dimos/robot/bosdyn/spot/cmd_vel.py b/dimos/robot/bosdyn/spot/effectors/cmd_vel.py similarity index 79% rename from dimos/robot/bosdyn/spot/cmd_vel.py rename to dimos/robot/bosdyn/spot/effectors/cmd_vel.py index 92333b1fbc..e386e50737 100644 --- a/dimos/robot/bosdyn/spot/cmd_vel.py +++ b/dimos/robot/bosdyn/spot/effectors/cmd_vel.py @@ -29,7 +29,6 @@ import asyncio from collections.abc import AsyncIterator from dataclasses import field -import os import time from typing import Any @@ -39,25 +38,19 @@ from dimos.core.stream import In from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.robot.bosdyn.spot.config import ( + POWER_OFF_TIMEOUT_S, + POWER_ON_TIMEOUT_S, + SIT_TIMEOUT_S, + STAND_TIMEOUT_S, + default_candidate_ips, + resolve_credentials, + resolve_ip, +) from dimos.utils.logging_config import setup_logger logger = setup_logger() -_POWER_ON_TIMEOUT_S = 20.0 -_POWER_OFF_TIMEOUT_S = 20.0 -_STAND_TIMEOUT_S = 10.0 -_SIT_TIMEOUT_S = 10.0 - -# Spot's fixed default addresses: 192.168.80.3 when it hosts its own WiFi AP, -# 10.0.0.3 on the rear Ethernet port. Probed in order when no ip is given. -_SPOT_WIFI_AP_IP = "192.168.80.3" -_SPOT_ETHERNET_IP = "10.0.0.3" -_IP_LABELS = {_SPOT_WIFI_AP_IP: "WiFi", _SPOT_ETHERNET_IP: "Ethernet"} -# Spot's gRPC API listens on HTTPS/443; a TCP connect confirms real reachability -# (and matches what the SDK does) better than an ICMP ping. -_SPOT_API_PORT = 443 -_REACHABILITY_PROBE_TIMEOUT_S = 2.0 - class SpotCmdVelConfig(ModuleConfig): """Connection, credentials, and safety gating for a Spot robot.""" @@ -66,10 +59,9 @@ class SpotCmdVelConfig(ModuleConfig): # main() probes `candidate_ips` and uses the first that answers on the API # port — so plugging in over Ethernet or joining Spot's WiFi both "just work". ip: str = "" - candidate_ips: list[str] = field(default_factory=lambda: [_SPOT_WIFI_AP_IP, _SPOT_ETHERNET_IP]) + candidate_ips: list[str] = field(default_factory=default_candidate_ips) - # Auth — falls back to BOSDYN_CLIENT_USERNAME / BOSDYN_CLIENT_PASSWORD at - # start time when left as None. Startup fails fast if neither is supplied. + # Auth — required. Startup fails fast if either is missing. username: str | None = None password: str | None = None @@ -114,8 +106,8 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: self._ready = asyncio.Event() async def main(self) -> AsyncIterator[None]: - username, password = self._resolve_credentials() - ip = self.config.ip or await self._resolve_ip() + username, password = resolve_credentials(self.config.username, self.config.password) + ip = self.config.ip or await resolve_ip(self.config.candidate_ips) from bosdyn.client import create_standard_sdk # type: ignore[import-not-found] from bosdyn.client.estop import ( # type: ignore[import-not-found] @@ -161,21 +153,25 @@ async def main(self) -> AsyncIterator[None]: if self.config.power_on_at_start: logger.info("Powering on Spot motors") - await asyncio.to_thread(self._robot.power_on, timeout_sec=_POWER_ON_TIMEOUT_S) + await asyncio.to_thread(self._robot.power_on, timeout_sec=POWER_ON_TIMEOUT_S) if self.config.stand_at_start: logger.info("Standing Spot") await asyncio.to_thread( - blocking_stand, self._command_client, timeout_sec=_STAND_TIMEOUT_S + blocking_stand, self._command_client, timeout_sec=STAND_TIMEOUT_S ) self._standing = True + await self._on_connected() + self._ready.set() logger.info("Spot cmd_vel control ready") yield self._ready.clear() + await self._on_teardown() + if self._standing: try: from bosdyn.client.robot_command import ( # type: ignore[import-not-found] @@ -183,7 +179,7 @@ async def main(self) -> AsyncIterator[None]: ) await asyncio.to_thread( - blocking_sit, self._command_client, timeout_sec=_SIT_TIMEOUT_S + blocking_sit, self._command_client, timeout_sec=SIT_TIMEOUT_S ) except Exception as error: logger.error(f"Spot sit during teardown failed: {error}") @@ -192,7 +188,7 @@ async def main(self) -> AsyncIterator[None]: if self.config.power_on_at_start and self._robot is not None: try: await asyncio.to_thread( - self._robot.power_off, cut_immediately=False, timeout_sec=_POWER_OFF_TIMEOUT_S + self._robot.power_off, cut_immediately=False, timeout_sec=POWER_OFF_TIMEOUT_S ) except Exception as error: logger.error(f"Spot power_off during teardown failed: {error}") @@ -210,6 +206,12 @@ async def main(self) -> AsyncIterator[None]: self._estop_keepalive = None logger.info("Spot cmd_vel control torn down") + async def _on_connected(self) -> None: + """Hook for subclasses to start extra services once the robot is up.""" + + async def _on_teardown(self) -> None: + """Hook for subclasses to stop extra services before the robot sits.""" + async def handle_cmd_vel(self, msg: Twist) -> None: if not self._ready.is_set(): return @@ -279,7 +281,7 @@ async def stand(self) -> str: ) await asyncio.to_thread( - blocking_stand, self._command_client, timeout_sec=_STAND_TIMEOUT_S + blocking_stand, self._command_client, timeout_sec=STAND_TIMEOUT_S ) self._standing = True return "Spot is standing." @@ -297,50 +299,9 @@ async def sit(self) -> str: blocking_sit, ) - await asyncio.to_thread(blocking_sit, self._command_client, timeout_sec=_SIT_TIMEOUT_S) + await asyncio.to_thread(blocking_sit, self._command_client, timeout_sec=SIT_TIMEOUT_S) self._standing = False return "Spot is sitting." except Exception as error: logger.error(f"Spot sit failed: {error}") return f"Sit failed: {error}" - - async def _resolve_ip(self) -> str: - for candidate in self.config.candidate_ips: - if await self._is_reachable(candidate): - logger.info(f"Spot reachable at {candidate}") - return candidate - described = " or ".join( - f"{candidate} ({_IP_LABELS[candidate]})" if candidate in _IP_LABELS else candidate - for candidate in self.config.candidate_ips - ) - raise ConnectionError( - f"I'm unable to connect to {described}. Did you forget to connect to " - "Spot's WiFi or plug in an Ethernet cable to Spot?" - ) - - async def _is_reachable(self, ip: str) -> bool: - try: - _, writer = await asyncio.wait_for( - asyncio.open_connection(ip, _SPOT_API_PORT), - timeout=_REACHABILITY_PROBE_TIMEOUT_S, - ) - except (OSError, asyncio.TimeoutError): - return False - # Reachable — a successful TCP handshake is all we need. Close without - # awaiting wait_closed(); the API port speaks TLS and never completes a - # clean plaintext close, which would otherwise hang the probe. - writer.close() - return True - - def _resolve_credentials(self) -> tuple[str, str]: - username = self.config.username or os.environ.get("BOSDYN_CLIENT_USERNAME") - password = self.config.password or os.environ.get("BOSDYN_CLIENT_PASSWORD") - if not username or not password: - raise ValueError( - "Spot credentials missing — set username/password in config " - "or BOSDYN_CLIENT_USERNAME / BOSDYN_CLIENT_PASSWORD env vars" - ) - return username, password - - -__all__ = ["SpotCmdVel", "SpotCmdVelConfig"] From d6067bc9b454b97e35bcb8534434b3c7319791e0 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 16 Jul 2026 23:17:44 -0500 Subject: [PATCH 03/26] feat(spot): flatten cmd_vel into SpotHighLevel; add cameras/record blueprints, recorder, URDF SpotHighLevel is now the single Spot module (cmd_vel merged in; old cmd_vel.py + spot-cmd-vel blueprint removed). The default `spot` blueprint drives via the Rerun web UI (MovementManager + RerunWebSocketServer) instead of pygame teleop, so it runs on macOS. Adds spot-cameras (depth-left/camera-right viewer) and spot-record (drive + record) blueprints, a memory2 SpotRecorder, and a flattened Spot base URDF. --- dimos/robot/all_blueprints.py | 4 +- dimos/robot/bosdyn/spot/README.md | 42 ++ dimos/robot/bosdyn/spot/blueprints/spot.py | 39 +- .../bosdyn/spot/blueprints/spot_cameras.py | 61 ++ .../bosdyn/spot/blueprints/spot_cmd_vel.py | 51 -- .../bosdyn/spot/blueprints/spot_record.py | 56 ++ dimos/robot/bosdyn/spot/config.urdf | 302 ++++++++++ dimos/robot/bosdyn/spot/effectors/cmd_vel.py | 307 ---------- .../robot/bosdyn/spot/effectors/high_level.py | 539 ++++++++++++++++++ dimos/robot/bosdyn/spot/recorder.py | 56 ++ 10 files changed, 1085 insertions(+), 372 deletions(-) create mode 100644 dimos/robot/bosdyn/spot/README.md create mode 100644 dimos/robot/bosdyn/spot/blueprints/spot_cameras.py delete mode 100644 dimos/robot/bosdyn/spot/blueprints/spot_cmd_vel.py create mode 100644 dimos/robot/bosdyn/spot/blueprints/spot_record.py create mode 100644 dimos/robot/bosdyn/spot/config.urdf delete mode 100644 dimos/robot/bosdyn/spot/effectors/cmd_vel.py create mode 100644 dimos/robot/bosdyn/spot/effectors/high_level.py create mode 100644 dimos/robot/bosdyn/spot/recorder.py diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 0436ed6528..bf09322306 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -81,7 +81,8 @@ "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_planner_coordinator", "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", "spot": "dimos.robot.bosdyn.spot.blueprints.spot:spot", - "spot-cmd-vel": "dimos.robot.bosdyn.spot.blueprints.spot_cmd_vel:spot_cmd_vel", + "spot-cameras": "dimos.robot.bosdyn.spot.blueprints.spot_cameras:spot_cameras", + "spot-record": "dimos.robot.bosdyn.spot.blueprints.spot_record:spot_record", "teleop-hosted-go2": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_go2", "teleop-hosted-go2-transport": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_go2_transport", "teleop-hosted-xarm7": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_xarm7", @@ -253,6 +254,7 @@ "spatial-memory": "dimos.perception.spatial_perception.SpatialMemory", "speak-skill": "dimos.agents.skills.speak_skill.SpeakSkill", "spot-high-level": "dimos.robot.bosdyn.spot.effectors.high_level.SpotHighLevel", + "spot-recorder": "dimos.robot.bosdyn.spot.recorder.SpotRecorder", "static-tf-publisher": "dimos.protocol.tf.static_tf_publisher.StaticTfPublisher", "tare-planner": "dimos.navigation.cmu_nav.modules.tare_planner.tare_planner.TarePlanner", "teleop-recorder": "dimos.teleop.utils.recorder.TeleopRecorder", diff --git a/dimos/robot/bosdyn/spot/README.md b/dimos/robot/bosdyn/spot/README.md new file mode 100644 index 0000000000..5363d603fe --- /dev/null +++ b/dimos/robot/bosdyn/spot/README.md @@ -0,0 +1,42 @@ +# Spot + +Boston Dynamics Spot control for DimOS: velocity teleop + camera/odometry streaming. + +## Install + +```bash +uv sync --extra spot +``` + +## Connect to Spot + +Pick one: + +- **WiFi (easiest):** join Spot's WiFi AP, robot is at `192.168.80.3`. +- **Ethernet:** plug into Spot's rear port. Robot defaults to `10.0.0.3/24`; set your + interface to a static IP on that subnet (e.g. `10.0.0.20/24`, **no gateway**). + +The rear-port IP is configurable, so if ethernet won't connect, check the actual +address in the Spot Admin Console (`https://192.168.80.3` → Network Setup → Ethernet). + +## Run + +```bash +dimos run spot \ + -o spothighlevel.username= \ + -o spothighlevel.password= +``` + +The IP auto-detects (WiFi then Ethernet). Force one with `-o spothighlevel.ip=`. + +Keyboard teleop: WASD move/turn, QE strafe, Space soft-stop, ESC quit. A Rerun +viewer opens with the fisheye/depth cameras and odometry. + +## Layout + +- `config.py` — constants + pure address/credential helpers (no `bosdyn` import). +- `effectors/high_level.py` — `SpotHighLevel`: the single Spot module — + lease/E-stop/power/stand + velocity commands plus the five fisheye + five depth + cameras and body odometry. +- `recorder.py` — `SpotRecorder`: records every Spot stream to disk. +- `blueprints/spot.py` — the runnable `spot` blueprint (click/teleop + sensors + Rerun). diff --git a/dimos/robot/bosdyn/spot/blueprints/spot.py b/dimos/robot/bosdyn/spot/blueprints/spot.py index 8e83c2589a..a6032e3146 100644 --- a/dimos/robot/bosdyn/spot/blueprints/spot.py +++ b/dimos/robot/bosdyn/spot/blueprints/spot.py @@ -12,33 +12,46 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Boston Dynamics Spot keyboard teleop with a Rerun viewer. +"""Boston Dynamics Spot: click/teleop driving plus full sensor streaming + Rerun. -Pygame keyboard -> Twist on `cmd_vel` -> SpotCmdVel -> robot, plus a Rerun -bridge that spawns the viewer. WASD to move/turn, QE to strafe, Space for -e-stop, ESC to quit. +The Rerun web UI is the driver: `RerunWebSocketServer` turns clicks into +`clicked_point` and browser keys into `tele_cmd_vel`. `MovementManager` muxes +those (and any `nav_cmd_vel`) into a single `cmd_vel`, which `SpotHighLevel` +executes. The same module streams the five fisheye + five depth cameras and body +odometry. Because the browser is the input surface there is no on-main-thread +pygame window, so this runs on macOS. -The ip auto-detects: with no `-o spotcmdvel.ip=` given, it probes Spot's WiFi +The ip auto-detects: with no `-o spothighlevel.ip=` given, it probes Spot's WiFi AP address (192.168.80.3) then the Ethernet address (10.0.0.3) and uses whichever answers. Usage: dimos run spot \ - -o spotcmdvel.username=admin \ - -o spotcmdvel.password= + -o spothighlevel.username=admin -o spothighlevel.password= # or force an address: - dimos run spot ... -o spotcmdvel.ip=10.0.0.3 + dimos run spot ... -o spothighlevel.ip=10.0.0.3 """ from __future__ import annotations from dimos.core.coordination.blueprints import autoconnect -from dimos.robot.bosdyn.spot.cmd_vel import SpotCmdVel -from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop +from dimos.navigation.movement_manager.movement_manager import MovementManager +from dimos.robot.bosdyn.spot.blueprints.spot_cameras import spot_camera_layout +from dimos.robot.bosdyn.spot.effectors.high_level import SpotHighLevel from dimos.visualization.rerun.bridge import RerunBridgeModule +from dimos.visualization.rerun.websocket_server import RerunWebSocketServer spot = autoconnect( - SpotCmdVel.blueprint(), - KeyboardTeleop.blueprint(), - RerunBridgeModule.blueprint(), + SpotHighLevel.blueprint(), + MovementManager.blueprint(), + RerunBridgeModule.blueprint(blueprint=spot_camera_layout), + RerunWebSocketServer.blueprint(), +).remappings( + [ + # No nav stack here, so MovementManager's goal/way_point/stop_movement + # outputs have no consumer — park them so autoconnect stays quiet. + (MovementManager, "goal", "_spot_goal_unused"), + (MovementManager, "way_point", "_spot_way_point_unused"), + (MovementManager, "stop_movement", "_spot_stop_movement_unused"), + ] ) diff --git a/dimos/robot/bosdyn/spot/blueprints/spot_cameras.py b/dimos/robot/bosdyn/spot/blueprints/spot_cameras.py new file mode 100644 index 0000000000..c8cc107a62 --- /dev/null +++ b/dimos/robot/bosdyn/spot/blueprints/spot_cameras.py @@ -0,0 +1,61 @@ +# 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. + +"""Spot camera/depth/odometry viewer. + +Powers on, stands the robot, and streams the five fisheye + five depth cameras +and body odometry into a Rerun viewer laid out as two vertical columns: the five +depth cameras on the left, the five grayscale cameras on the right. Unlike the +full `spot` blueprint there is no teleop, so the robot stands but does not drive. + +Everything rides the default Zenoh transport, which the Rerun bridge subscribes +to, so the viewer receives and renders the frames. + +Usage: + dimos run spot-cameras \ + -o spothighlevel.username=admin -o spothighlevel.password= +""" + +from __future__ import annotations + +import rerun.blueprint as rrb + +from dimos.core.coordination.blueprints import autoconnect +from dimos.robot.bosdyn.spot.effectors.high_level import SpotHighLevel +from dimos.visualization.rerun.bridge import RerunBridgeModule + +_NUM_CAMERAS = 5 + + +def spot_camera_layout() -> rrb.Blueprint: + """Depth column on the left, grayscale column on the right.""" + depth_column = rrb.Vertical( + *[ + rrb.Spatial2DView(origin=f"world/depth_image_{index}", name=f"depth {index}") + for index in range(1, _NUM_CAMERAS + 1) + ] + ) + grayscale_column = rrb.Vertical( + *[ + rrb.Spatial2DView(origin=f"world/grayscale_image_{index}", name=f"camera {index}") + for index in range(1, _NUM_CAMERAS + 1) + ] + ) + return rrb.Blueprint(rrb.Horizontal(depth_column, grayscale_column), collapse_panels=True) + + +spot_cameras = autoconnect( + SpotHighLevel.blueprint(), + RerunBridgeModule.blueprint(blueprint=spot_camera_layout), +) diff --git a/dimos/robot/bosdyn/spot/blueprints/spot_cmd_vel.py b/dimos/robot/bosdyn/spot/blueprints/spot_cmd_vel.py deleted file mode 100644 index 8709544815..0000000000 --- a/dimos/robot/bosdyn/spot/blueprints/spot_cmd_vel.py +++ /dev/null @@ -1,51 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Boston Dynamics Spot keyboard teleop + full sensor streaming with a Rerun viewer. - -Pygame keyboard -> Twist on `cmd_vel` -> SpotHighLevel -> robot. The same module -streams the five fisheye + five depth cameras and body odometry. A Rerun bridge -spawns the viewer. WASD to move/turn, QE to strafe, Space for e-stop, ESC to quit. - -The ip auto-detects: with no `-o spothighlevel.ip=` given, it probes Spot's WiFi -AP address (192.168.80.3) then the Ethernet address (10.0.0.3) and uses -whichever answers. - -Usage: - dimos run spot \ - -o spothighlevel.username=admin -o spothighlevel.password= - # or force an address: - dimos run spot ... -o spothighlevel.ip=10.0.0.3 -""" - -from __future__ import annotations - -from dimos.core.coordination.blueprints import autoconnect -from dimos.robot.bosdyn.spot.effectors.cmd_vel import SpotCmdVel - -# from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop -from dimos.visualization.vis_module import RerunWebSocketServer, vis_module - -spot_cmd_vel = autoconnect( - SpotCmdVel.blueprint(), - # KeyboardTeleop.blueprint(), - vis_module( - "rerun", - rerun_config={}, - ).remappings( - [ - (RerunWebSocketServer, "tele_cmd_vel", "cmd_vel"), - ] - ), -) diff --git a/dimos/robot/bosdyn/spot/blueprints/spot_record.py b/dimos/robot/bosdyn/spot/blueprints/spot_record.py new file mode 100644 index 0000000000..4f9bd1d22d --- /dev/null +++ b/dimos/robot/bosdyn/spot/blueprints/spot_record.py @@ -0,0 +1,56 @@ +# 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. + +"""Spot: drive it from the Rerun web UI while recording every data stream. + +The same click/teleop + camera stack as the default `spot` blueprint — +`RerunWebSocketServer` turns browser clicks/keys into `clicked_point` / +`tele_cmd_vel`, `MovementManager` muxes them into `cmd_vel`, and `SpotHighLevel` +executes it while streaming the five fisheye + five depth cameras and odometry — +with `SpotRecorder` added so every one of those streams (plus the live tf tree) +is written to a memory2 SQLite db as you drive. `autoconnect` wires the +recorder's In ports to `SpotHighLevel`'s outputs by name. + +Usage: + dimos run spot-record \ + -o spothighlevel.username=admin -o spothighlevel.password= + # choose where the recording lands: + dimos run spot-record ... -o spotrecorder.db_path=/path/to/spot.db +""" + +from __future__ import annotations + +from dimos.core.coordination.blueprints import autoconnect +from dimos.navigation.movement_manager.movement_manager import MovementManager +from dimos.robot.bosdyn.spot.blueprints.spot_cameras import spot_camera_layout +from dimos.robot.bosdyn.spot.effectors.high_level import SpotHighLevel +from dimos.robot.bosdyn.spot.recorder import SpotRecorder +from dimos.visualization.rerun.bridge import RerunBridgeModule +from dimos.visualization.rerun.websocket_server import RerunWebSocketServer + +spot_record = autoconnect( + SpotHighLevel.blueprint(), + MovementManager.blueprint(), + SpotRecorder.blueprint(), + RerunBridgeModule.blueprint(blueprint=spot_camera_layout), + RerunWebSocketServer.blueprint(), +).remappings( + [ + # No nav stack here, so MovementManager's goal/way_point/stop_movement + # outputs have no consumer — park them so autoconnect stays quiet. + (MovementManager, "goal", "_spot_goal_unused"), + (MovementManager, "way_point", "_spot_way_point_unused"), + (MovementManager, "stop_movement", "_spot_stop_movement_unused"), + ] +) diff --git a/dimos/robot/bosdyn/spot/config.urdf b/dimos/robot/bosdyn/spot/config.urdf new file mode 100644 index 0000000000..a4a3a89817 --- /dev/null +++ b/dimos/robot/bosdyn/spot/config.urdf @@ -0,0 +1,302 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/dimos/robot/bosdyn/spot/effectors/cmd_vel.py b/dimos/robot/bosdyn/spot/effectors/cmd_vel.py deleted file mode 100644 index e386e50737..0000000000 --- a/dimos/robot/bosdyn/spot/effectors/cmd_vel.py +++ /dev/null @@ -1,307 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Minimal Boston Dynamics Spot velocity control via the `bosdyn-client` SDK. - -A single async `Module`: `main()` acquires the lease/E-stop, powers on, and -stands the robot, then hands control to the `cmd_vel` stream. Each Twist is -forwarded to Spot as a synchronized body velocity command. Teardown sits the -robot and powers the motors back off. - -`bosdyn` is an optional extra (`uv sync --extra spot`); its imports live inside -`main()` so this file stays importable — and blueprint discovery keeps working — -on hosts where the SDK isn't installed. -""" - -from __future__ import annotations - -import asyncio -from collections.abc import AsyncIterator -from dataclasses import field -import time -from typing import Any - -from dimos.agents.annotation import skill -from dimos.core.core import rpc -from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import In -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.robot.bosdyn.spot.config import ( - POWER_OFF_TIMEOUT_S, - POWER_ON_TIMEOUT_S, - SIT_TIMEOUT_S, - STAND_TIMEOUT_S, - default_candidate_ips, - resolve_credentials, - resolve_ip, -) -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -class SpotCmdVelConfig(ModuleConfig): - """Connection, credentials, and safety gating for a Spot robot.""" - - # Explicit address always wins (`-o spotcmdvel.ip=`). When left blank, - # main() probes `candidate_ips` and uses the first that answers on the API - # port — so plugging in over Ethernet or joining Spot's WiFi both "just work". - ip: str = "" - candidate_ips: list[str] = field(default_factory=default_candidate_ips) - - # Auth — required. Startup fails fast if either is missing. - username: str | None = None - password: str | None = None - - # Safety / startup gating. - enable_estop: bool = True - acquire_lease: bool = True - power_on_at_start: bool = True - stand_at_start: bool = True - - # Spot rejects velocity commands without an `end_time_secs`. This duration - # is added to now() for each command and doubles as the auto-stop window: - # if the cmd_vel stream stalls for longer than this, the robot halts. - cmd_vel_timeout: float = 0.5 - - # Spot E-stops itself if it doesn't see a keep-alive check-in within this - # window. 9.0 s matches the bosdyn-client default. - estop_timeout: float = 9.0 - - -class SpotCmdVel(Module): - """Drives a Boston Dynamics Spot from a `cmd_vel` Twist stream.""" - - # A hardware-driving module gets its own worker process, matching the other - # robot connection modules (go2/b1/drone). Sharing a process with a GUI - # module like KeyboardTeleop wedges this module's RPC server at startup. - dedicated_worker = True - - cmd_vel: In[Twist] - - config: SpotCmdVelConfig - - def __init__(self, *args: Any, **kwargs: Any) -> None: - super().__init__(*args, **kwargs) - self._robot: Any = None - self._command_client: Any = None - self._state_client: Any = None - self._estop_keepalive: Any = None - self._lease_keepalive: Any = None - self._standing = False - # cmd_vel handlers wait on this so a velocity command issued mid-setup - # never reaches a half-initialised SDK. - self._ready = asyncio.Event() - - async def main(self) -> AsyncIterator[None]: - username, password = resolve_credentials(self.config.username, self.config.password) - ip = self.config.ip or await resolve_ip(self.config.candidate_ips) - - from bosdyn.client import create_standard_sdk # type: ignore[import-not-found] - from bosdyn.client.estop import ( # type: ignore[import-not-found] - EstopClient, - EstopEndpoint, - EstopKeepAlive, - ) - from bosdyn.client.lease import ( # type: ignore[import-not-found] - LeaseClient, - LeaseKeepAlive, - ) - from bosdyn.client.robot_command import ( # type: ignore[import-not-found] - RobotCommandClient, - blocking_stand, - ) - from bosdyn.client.robot_state import ( # type: ignore[import-not-found] - RobotStateClient, - ) - - logger.info(f"Connecting to Spot at {ip}") - sdk = await asyncio.to_thread(create_standard_sdk, "dimos-spot") - self._robot = await asyncio.to_thread(sdk.create_robot, ip) - await asyncio.to_thread(self._robot.authenticate, username, password) - await asyncio.to_thread(self._robot.time_sync.wait_for_sync) - - if self.config.enable_estop: - estop_client = self._robot.ensure_client(EstopClient.default_service_name) - endpoint = EstopEndpoint( - client=estop_client, - name="dimos-spot", - estop_timeout=self.config.estop_timeout, - ) - await asyncio.to_thread(endpoint.force_simple_setup) - self._estop_keepalive = EstopKeepAlive(endpoint) - - if self.config.acquire_lease: - lease_client = self._robot.ensure_client(LeaseClient.default_service_name) - await asyncio.to_thread(lease_client.take) - self._lease_keepalive = LeaseKeepAlive(lease_client) - - self._command_client = self._robot.ensure_client(RobotCommandClient.default_service_name) - self._state_client = self._robot.ensure_client(RobotStateClient.default_service_name) - - if self.config.power_on_at_start: - logger.info("Powering on Spot motors") - await asyncio.to_thread(self._robot.power_on, timeout_sec=POWER_ON_TIMEOUT_S) - - if self.config.stand_at_start: - logger.info("Standing Spot") - await asyncio.to_thread( - blocking_stand, self._command_client, timeout_sec=STAND_TIMEOUT_S - ) - self._standing = True - - await self._on_connected() - - self._ready.set() - logger.info("Spot cmd_vel control ready") - - yield - - self._ready.clear() - await self._on_teardown() - - if self._standing: - try: - from bosdyn.client.robot_command import ( # type: ignore[import-not-found] - blocking_sit, - ) - - await asyncio.to_thread( - blocking_sit, self._command_client, timeout_sec=SIT_TIMEOUT_S - ) - except Exception as error: - logger.error(f"Spot sit during teardown failed: {error}") - self._standing = False - - if self.config.power_on_at_start and self._robot is not None: - try: - await asyncio.to_thread( - self._robot.power_off, cut_immediately=False, timeout_sec=POWER_OFF_TIMEOUT_S - ) - except Exception as error: - logger.error(f"Spot power_off during teardown failed: {error}") - - for keepalive_name, keepalive in ( - ("lease", self._lease_keepalive), - ("estop", self._estop_keepalive), - ): - if keepalive is not None: - try: - await asyncio.to_thread(keepalive.shutdown) - except Exception as error: - logger.error(f"Spot {keepalive_name} shutdown failed: {error}") - self._lease_keepalive = None - self._estop_keepalive = None - logger.info("Spot cmd_vel control torn down") - - async def _on_connected(self) -> None: - """Hook for subclasses to start extra services once the robot is up.""" - - async def _on_teardown(self) -> None: - """Hook for subclasses to stop extra services before the robot sits.""" - - async def handle_cmd_vel(self, msg: Twist) -> None: - if not self._ready.is_set(): - return - await self._send_velocity(msg.linear.x, msg.linear.y, msg.angular.z) - - async def _send_velocity( - self, forward: float, strafe: float, yaw: float, duration: float = 0.0 - ) -> bool: - from bosdyn.client.robot_command import ( # type: ignore[import-not-found] - RobotCommandBuilder, - ) - - command = RobotCommandBuilder.synchro_velocity_command(v_x=forward, v_y=strafe, v_rot=yaw) - window = duration if duration > 0 else self.config.cmd_vel_timeout - try: - await asyncio.to_thread( - self._command_client.robot_command, - command, - end_time_secs=time.time() + window, - ) - return True - except Exception as error: - logger.error(f"Spot velocity command failed: {error}") - return False - - @rpc - async def move(self, twist: Twist, duration: float = 0.0) -> bool: - """Send a Twist as a body velocity command, optionally for `duration` seconds.""" - return await self._send_velocity(twist.linear.x, twist.linear.y, twist.angular.z, duration) - - @rpc - async def get_state(self) -> str: - if self._state_client is None: - return "DISCONNECTED" - try: - state = await asyncio.to_thread(self._state_client.get_robot_state) - return str(state.power_state.motor_power_state) - except Exception as error: - logger.error(f"Spot get_state failed: {error}") - return "UNKNOWN" - - @skill - async def move_velocity( - self, x: float, y: float = 0.0, yaw: float = 0.0, duration: float = 0.0 - ) -> str: - """Move Spot with a direct body velocity command. - - Args: - x: Forward velocity (m/s). - y: Left/right velocity (m/s). - yaw: Rotational velocity (rad/s). - duration: Seconds to move. 0 uses one `cmd_vel_timeout` window. - """ - twist = Twist(linear=Vector3(x, y, 0), angular=Vector3(0, 0, yaw)) - if await self.move(twist, duration=duration): - return f"Moving with velocity=({x}, {y}, {yaw}) for {duration} seconds" - return f"Failed to move with velocity=({x}, {y}, {yaw})" - - @skill - async def stand(self) -> str: - """Make Spot stand up. Spot must already be powered on.""" - if self._command_client is None: - return "Spot is not connected." - try: - from bosdyn.client.robot_command import ( # type: ignore[import-not-found] - blocking_stand, - ) - - await asyncio.to_thread( - blocking_stand, self._command_client, timeout_sec=STAND_TIMEOUT_S - ) - self._standing = True - return "Spot is standing." - except Exception as error: - logger.error(f"Spot stand failed: {error}") - return f"Stand failed: {error}" - - @skill - async def sit(self) -> str: - """Make Spot sit down.""" - if self._command_client is None: - return "Spot is not connected." - try: - from bosdyn.client.robot_command import ( # type: ignore[import-not-found] - blocking_sit, - ) - - await asyncio.to_thread(blocking_sit, self._command_client, timeout_sec=SIT_TIMEOUT_S) - self._standing = False - return "Spot is sitting." - except Exception as error: - logger.error(f"Spot sit failed: {error}") - return f"Sit failed: {error}" diff --git a/dimos/robot/bosdyn/spot/effectors/high_level.py b/dimos/robot/bosdyn/spot/effectors/high_level.py new file mode 100644 index 0000000000..aeaa49cc47 --- /dev/null +++ b/dimos/robot/bosdyn/spot/effectors/high_level.py @@ -0,0 +1,539 @@ +# 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. + +"""Full Boston Dynamics Spot control: velocity driving plus sensor streaming. + +`SpotHighLevel` is the single Spot hardware module. Over the one robot +connection it owns it: acquires the lease/E-stop, powers on and stands, drives +from a `cmd_vel` Twist stream, and streams every onboard camera plus body +odometry: + +- `grayscale_image_1..5` — the five fisheye body cameras (front-left, front-right, + left, right, back), in that order. +- `depth_image_1..5` — the matching depth cameras, same ordering. +- `odom` — body pose + velocity in Spot's `vision` frame, also broadcast on TF. + +`bosdyn` is an optional extra (`uv sync --extra spot`); its imports live inside +methods so this file stays importable — and blueprint discovery keeps working — +on hosts where the SDK isn't installed. +""" + +from __future__ import annotations + +import asyncio +from collections.abc import AsyncIterator +from dataclasses import field +import time +from typing import Any + +import numpy as np + +from dimos.agents.annotation import skill +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.robot.bosdyn.spot.config import ( + BODY_FRAME, + DEPTH_SOURCES, + GRAYSCALE_SOURCES, + POWER_OFF_TIMEOUT_S, + POWER_ON_TIMEOUT_S, + SIT_TIMEOUT_S, + STAND_TIMEOUT_S, + VISION_FRAME, + default_candidate_ips, + resolve_credentials, + resolve_ip, +) +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class SpotHighLevelConfig(ModuleConfig): + """Connection, credentials, safety gating, and sensor capture for a Spot.""" + + # Explicit address always wins (`-o spothighlevel.ip=`). When left + # blank, main() probes `candidate_ips` and uses the first that answers on the + # API port — so plugging in over Ethernet or joining Spot's WiFi both work. + ip: str = "" + candidate_ips: list[str] = field(default_factory=default_candidate_ips) + + # Auth — required. Startup fails fast if either is missing. + username: str | None = None + password: str | None = None + + # Safety / startup gating. + enable_estop: bool = True + acquire_lease: bool = True + power_on_at_start: bool = True + stand_at_start: bool = True + + # Spot rejects velocity commands without an `end_time_secs`. This duration + # is added to now() for each command and doubles as the auto-stop window: + # if the cmd_vel stream stalls for longer than this, the robot halts. + cmd_vel_timeout: float = 0.5 + + # Spot E-stops itself if it doesn't see a keep-alive check-in within this + # window. 9.0 s matches the bosdyn-client default. + estop_timeout: float = 9.0 + + # Which sources feed grayscale_image_N / depth_image_N (index N-1). Trim these + # to capture fewer cameras. + grayscale_sources: list[str] = field(default_factory=lambda: list(GRAYSCALE_SOURCES)) + depth_sources: list[str] = field(default_factory=lambda: list(DEPTH_SOURCES)) + + # Clockwise rotation (degrees, multiple of 90) applied to a camera's + # grayscale + depth before publishing, correcting for physically rotated + # fisheye mounts. Keyed by 1-based camera index. + image_rotations_cw: dict[int, int] = field(default_factory=lambda: {1: 90, 2: 90, 4: 180}) + + image_rate_hz: float = 5.0 + odom_rate_hz: float = 20.0 + + +class SpotHighLevel(Module): + """Drives Spot and streams its fisheye cameras, depth cameras, and odometry.""" + + # A hardware-driving module gets its own worker process, matching the other + # robot connection modules (go2/b1/drone). Sharing a process with a GUI + # module like KeyboardTeleop wedges this module's RPC server at startup. + dedicated_worker = True + + cmd_vel: In[Twist] + + grayscale_image_1: Out[Image] + grayscale_image_2: Out[Image] + grayscale_image_3: Out[Image] + grayscale_image_4: Out[Image] + grayscale_image_5: Out[Image] + + depth_image_1: Out[Image] + depth_image_2: Out[Image] + depth_image_3: Out[Image] + depth_image_4: Out[Image] + depth_image_5: Out[Image] + + odom: Out[Odometry] + + config: SpotHighLevelConfig + + def __init__(self, *args: Any, **kwargs: Any) -> None: + super().__init__(*args, **kwargs) + self._robot: Any = None + self._command_client: Any = None + self._state_client: Any = None + self._image_client: Any = None + self._estop_keepalive: Any = None + self._lease_keepalive: Any = None + self._standing = False + self._image_task: asyncio.Task[None] | None = None + self._odom_task: asyncio.Task[None] | None = None + # cmd_vel handlers wait on this so a velocity command issued mid-setup + # never reaches a half-initialised SDK. + self._ready = asyncio.Event() + + async def main(self) -> AsyncIterator[None]: + username, password = resolve_credentials(self.config.username, self.config.password) + ip = self.config.ip or await resolve_ip(self.config.candidate_ips) + + from bosdyn.client import create_standard_sdk # type: ignore[import-not-found] + from bosdyn.client.estop import ( # type: ignore[import-not-found] + EstopClient, + EstopEndpoint, + EstopKeepAlive, + ) + from bosdyn.client.image import ImageClient # type: ignore[import-not-found] + from bosdyn.client.lease import ( # type: ignore[import-not-found] + LeaseClient, + LeaseKeepAlive, + ) + from bosdyn.client.robot_command import ( # type: ignore[import-not-found] + RobotCommandClient, + blocking_stand, + ) + from bosdyn.client.robot_state import ( # type: ignore[import-not-found] + RobotStateClient, + ) + + logger.info(f"Connecting to Spot at {ip}") + sdk = await asyncio.to_thread(create_standard_sdk, "dimos-spot") + self._robot = await asyncio.to_thread(sdk.create_robot, ip) + await asyncio.to_thread(self._robot.authenticate, username, password) + await asyncio.to_thread(self._robot.time_sync.wait_for_sync) + + if self.config.enable_estop: + estop_client = self._robot.ensure_client(EstopClient.default_service_name) + endpoint = EstopEndpoint( + client=estop_client, + name="dimos-spot", + estop_timeout=self.config.estop_timeout, + ) + await asyncio.to_thread(endpoint.force_simple_setup) + self._estop_keepalive = EstopKeepAlive(endpoint) + + if self.config.acquire_lease: + lease_client = self._robot.ensure_client(LeaseClient.default_service_name) + await asyncio.to_thread(lease_client.take) + self._lease_keepalive = LeaseKeepAlive(lease_client) + + self._command_client = self._robot.ensure_client(RobotCommandClient.default_service_name) + self._state_client = self._robot.ensure_client(RobotStateClient.default_service_name) + self._image_client = self._robot.ensure_client(ImageClient.default_service_name) + + if self.config.power_on_at_start: + logger.info("Powering on Spot motors") + await asyncio.to_thread(self._robot.power_on, timeout_sec=POWER_ON_TIMEOUT_S) + + if self.config.stand_at_start: + logger.info("Standing Spot") + await asyncio.to_thread( + blocking_stand, self._command_client, timeout_sec=STAND_TIMEOUT_S + ) + self._standing = True + + self.tf.start() + self._image_task = asyncio.create_task(self._poll_images()) + self._odom_task = asyncio.create_task(self._poll_odom()) + + self._ready.set() + logger.info("Spot control + sensors ready") + + yield + + self._ready.clear() + for task in (self._image_task, self._odom_task): + if task is not None: + task.cancel() + + if self._standing: + try: + from bosdyn.client.robot_command import ( # type: ignore[import-not-found] + blocking_sit, + ) + + await asyncio.to_thread( + blocking_sit, self._command_client, timeout_sec=SIT_TIMEOUT_S + ) + except Exception as error: + logger.error(f"Spot sit during teardown failed: {error}") + self._standing = False + + if self.config.power_on_at_start and self._robot is not None: + try: + await asyncio.to_thread( + self._robot.power_off, cut_immediately=False, timeout_sec=POWER_OFF_TIMEOUT_S + ) + except Exception as error: + logger.error(f"Spot power_off during teardown failed: {error}") + + for keepalive_name, keepalive in ( + ("lease", self._lease_keepalive), + ("estop", self._estop_keepalive), + ): + if keepalive is not None: + try: + await asyncio.to_thread(keepalive.shutdown) + except Exception as error: + logger.error(f"Spot {keepalive_name} shutdown failed: {error}") + self._lease_keepalive = None + self._estop_keepalive = None + logger.info("Spot control torn down") + + async def handle_cmd_vel(self, msg: Twist) -> None: + if not self._ready.is_set(): + return + await self._send_velocity(msg.linear.x, msg.linear.y, msg.angular.z) + + async def _send_velocity( + self, forward: float, strafe: float, yaw: float, duration: float = 0.0 + ) -> bool: + from bosdyn.client.robot_command import ( # type: ignore[import-not-found] + RobotCommandBuilder, + ) + + command = RobotCommandBuilder.synchro_velocity_command(v_x=forward, v_y=strafe, v_rot=yaw) + window = duration if duration > 0 else self.config.cmd_vel_timeout + try: + await asyncio.to_thread( + self._command_client.robot_command, + command, + end_time_secs=time.time() + window, + ) + return True + except Exception as error: + logger.error(f"Spot velocity command failed: {error}") + return False + + def _grayscale_outputs(self) -> list[Out[Image]]: + return [ + self.grayscale_image_1, + self.grayscale_image_2, + self.grayscale_image_3, + self.grayscale_image_4, + self.grayscale_image_5, + ] + + def _depth_outputs(self) -> list[Out[Image]]: + return [ + self.depth_image_1, + self.depth_image_2, + self.depth_image_3, + self.depth_image_4, + self.depth_image_5, + ] + + async def _poll_images(self) -> None: + period = 1.0 / self.config.image_rate_hz + sources = self.config.grayscale_sources + self.config.depth_sources + # source name -> (Out stream that carries it, np.rot90 k applied before publish). + routing: dict[str, tuple[Out[Image], int]] = {} + # Fewer configured sources than output streams is fine — extra streams + # just stay silent, so pair only as many as the shorter list. + for index, (source, out) in enumerate( + zip(self.config.grayscale_sources, self._grayscale_outputs(), strict=False) + ): + routing[source] = (out, self._rotation_k(index + 1)) + for index, (source, out) in enumerate( + zip(self.config.depth_sources, self._depth_outputs(), strict=False) + ): + routing[source] = (out, self._rotation_k(index + 1)) + + while True: + start = time.monotonic() + try: + responses = await asyncio.to_thread( + self._image_client.get_image_from_sources, sources + ) + except Exception as error: + logger.error(f"Spot image capture failed: {error}") + await asyncio.sleep(period) + continue + + for response in responses: + source_name = response.source.name + route = routing.get(source_name) + if route is None: + continue + out, rotation_k = route + image = _decode_image(response, source_name) + if image is None: + continue + if rotation_k: + image = _rotate_image(image, rotation_k) + out.publish(image) + + await asyncio.sleep(max(0.0, period - (time.monotonic() - start))) + + def _rotation_k(self, camera_number: int) -> int: + """Convert a clockwise degree rotation into an np.rot90 k (0-3).""" + degrees_cw = self.config.image_rotations_cw.get(camera_number, 0) + return (-degrees_cw // 90) % 4 + + async def _poll_odom(self) -> None: + from bosdyn.client.frame_helpers import ( # type: ignore[import-not-found] + BODY_FRAME_NAME, + VISION_FRAME_NAME, + get_a_tform_b, + ) + + period = 1.0 / self.config.odom_rate_hz + while True: + start = time.monotonic() + try: + state = await asyncio.to_thread(self._state_client.get_robot_state) + except Exception as error: + logger.error(f"Spot state capture failed: {error}") + await asyncio.sleep(period) + continue + + kinematic_state = state.kinematic_state + vision_tform_body = get_a_tform_b( + kinematic_state.transforms_snapshot, VISION_FRAME_NAME, BODY_FRAME_NAME + ) + velocity = kinematic_state.velocity_of_body_in_vision + self._publish_odom(vision_tform_body, velocity) + + await asyncio.sleep(max(0.0, period - (time.monotonic() - start))) + + def _publish_odom(self, vision_tform_body: Any, velocity: Any) -> None: + from dimos.msgs.geometry_msgs.Quaternion import Quaternion + from dimos.msgs.geometry_msgs.Transform import Transform + + now = time.time() + pose = Pose( + position=[vision_tform_body.x, vision_tform_body.y, vision_tform_body.z], + orientation=[ + vision_tform_body.rot.x, + vision_tform_body.rot.y, + vision_tform_body.rot.z, + vision_tform_body.rot.w, + ], + ) + twist = Twist( + linear=[velocity.linear.x, velocity.linear.y, velocity.linear.z], + angular=[velocity.angular.x, velocity.angular.y, velocity.angular.z], + ) + odometry = Odometry( + ts=now, + frame_id=VISION_FRAME, + child_frame_id=BODY_FRAME, + pose=pose, + twist=twist, + ) + self.odom.publish(odometry) + self.tf.publish( + Transform( + translation=Vector3(vision_tform_body.x, vision_tform_body.y, vision_tform_body.z), + rotation=Quaternion( + vision_tform_body.rot.x, + vision_tform_body.rot.y, + vision_tform_body.rot.z, + vision_tform_body.rot.w, + ), + frame_id=VISION_FRAME, + child_frame_id=BODY_FRAME, + ts=now, + ) + ) + + @rpc + async def move(self, twist: Twist, duration: float = 0.0) -> bool: + """Send a Twist as a body velocity command, optionally for `duration` seconds.""" + return await self._send_velocity(twist.linear.x, twist.linear.y, twist.angular.z, duration) + + @rpc + async def get_state(self) -> str: + if self._state_client is None: + return "DISCONNECTED" + try: + state = await asyncio.to_thread(self._state_client.get_robot_state) + return str(state.power_state.motor_power_state) + except Exception as error: + logger.error(f"Spot get_state failed: {error}") + return "UNKNOWN" + + @skill + async def move_velocity( + self, x: float, y: float = 0.0, yaw: float = 0.0, duration: float = 0.0 + ) -> str: + """Move Spot with a direct body velocity command. + + Args: + x: Forward velocity (m/s). + y: Left/right velocity (m/s). + yaw: Rotational velocity (rad/s). + duration: Seconds to move. 0 uses one `cmd_vel_timeout` window. + """ + twist = Twist(linear=Vector3(x, y, 0), angular=Vector3(0, 0, yaw)) + if await self.move(twist, duration=duration): + return f"Moving with velocity=({x}, {y}, {yaw}) for {duration} seconds" + return f"Failed to move with velocity=({x}, {y}, {yaw})" + + @skill + async def stand(self) -> str: + """Make Spot stand up. Spot must already be powered on.""" + if self._command_client is None: + return "Spot is not connected." + try: + from bosdyn.client.robot_command import ( # type: ignore[import-not-found] + blocking_stand, + ) + + await asyncio.to_thread( + blocking_stand, self._command_client, timeout_sec=STAND_TIMEOUT_S + ) + self._standing = True + return "Spot is standing." + except Exception as error: + logger.error(f"Spot stand failed: {error}") + return f"Stand failed: {error}" + + @skill + async def sit(self) -> str: + """Make Spot sit down.""" + if self._command_client is None: + return "Spot is not connected." + try: + from bosdyn.client.robot_command import ( # type: ignore[import-not-found] + blocking_sit, + ) + + await asyncio.to_thread(blocking_sit, self._command_client, timeout_sec=SIT_TIMEOUT_S) + self._standing = False + return "Spot is sitting." + except Exception as error: + logger.error(f"Spot sit failed: {error}") + return f"Sit failed: {error}" + + +def _rotate_image(image: Image, rotation_k: int) -> Image: + """Rotate an image counterclockwise by rotation_k * 90 degrees.""" + rotated = np.ascontiguousarray(np.rot90(image.data, rotation_k)) + return Image(data=rotated, format=image.format, frame_id=image.frame_id, ts=image.ts) + + +def _decode_image(response: Any, source_name: str) -> Image | None: + """Turn a bosdyn ImageResponse into a dimos Image, or None if unsupported.""" + from bosdyn.api import image_pb2 # type: ignore[import-not-found] + + shot = response.shot.image + pixel_format = shot.pixel_format + now = time.time() + + if shot.format == image_pb2.Image.FORMAT_JPEG: + import cv2 + + buffer = np.frombuffer(shot.data, dtype=np.uint8) + decoded = cv2.imdecode(buffer, cv2.IMREAD_UNCHANGED) + if decoded is None: + logger.error(f"Failed to decode JPEG image from {source_name}") + return None + image_format = ImageFormat.GRAY if decoded.ndim == 2 else ImageFormat.BGR + return Image.from_numpy(decoded, format=image_format, frame_id=source_name, ts=now) + + if shot.format != image_pb2.Image.FORMAT_RAW: + logger.error(f"Unsupported Spot image encoding {shot.format} from {source_name}") + return None + + dtype, channels, image_format = _raw_layout(pixel_format) + if dtype is None: + logger.error(f"Unsupported Spot pixel format {pixel_format} from {source_name}") + return None + + array = np.frombuffer(shot.data, dtype=dtype) + array = ( + array.reshape(shot.rows, shot.cols) + if channels == 1 + else array.reshape(shot.rows, shot.cols, channels) + ) + return Image.from_numpy(array, format=image_format, frame_id=source_name, ts=now) + + +def _raw_layout(pixel_format: int) -> tuple[Any, int, ImageFormat]: + from bosdyn.api import image_pb2 # type: ignore[import-not-found] + + layouts: dict[int, tuple[Any, int, ImageFormat]] = { + image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U8: (np.uint8, 1, ImageFormat.GRAY), + image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U16: (np.uint16, 1, ImageFormat.GRAY16), + image_pb2.Image.PIXEL_FORMAT_DEPTH_U16: (np.uint16, 1, ImageFormat.DEPTH16), + image_pb2.Image.PIXEL_FORMAT_RGB_U8: (np.uint8, 3, ImageFormat.RGB), + image_pb2.Image.PIXEL_FORMAT_RGBA_U8: (np.uint8, 4, ImageFormat.RGBA), + } + return layouts.get(pixel_format, (None, 0, ImageFormat.GRAY)) diff --git a/dimos/robot/bosdyn/spot/recorder.py b/dimos/robot/bosdyn/spot/recorder.py new file mode 100644 index 0000000000..69df2585f8 --- /dev/null +++ b/dimos/robot/bosdyn/spot/recorder.py @@ -0,0 +1,56 @@ +# 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. + +"""Record every Spot data stream into a memory2 SQLite db. + +A ``Recorder`` whose In ports mirror `SpotHighLevel`'s outputs — the five +grayscale cameras, five depth cameras, and body odometry — so `autoconnect` +wires them by name. The base class writes each port (plus the live tf tree) to +``db_path``; poses come from tf, so recorded frames stay spatially anchored. +""" + +from __future__ import annotations + +from pathlib import Path + +from dimos.core.stream import In +from dimos.memory2.module import OnExisting, Recorder, RecorderConfig +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.Image import Image + + +class SpotRecorderConfig(RecorderConfig): + db_path: str | Path = "spot_recording.db" + # Append into a populated db so re-runs add to the same recording. + on_existing: OnExisting = OnExisting.APPEND + + +class SpotRecorder(Recorder): + """Records Spot's fisheye + depth cameras and odometry to a memory2 db.""" + + config: SpotRecorderConfig + + grayscale_image_1: In[Image] + grayscale_image_2: In[Image] + grayscale_image_3: In[Image] + grayscale_image_4: In[Image] + grayscale_image_5: In[Image] + + depth_image_1: In[Image] + depth_image_2: In[Image] + depth_image_3: In[Image] + depth_image_4: In[Image] + depth_image_5: In[Image] + + odom: In[Odometry] From bf92efed49f4c99db9db4f8c92ea832923bc0a05 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 17 Jul 2026 14:01:27 -0500 Subject: [PATCH 04/26] add lossless recording option --- dimos/memory2/module.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/dimos/memory2/module.py b/dimos/memory2/module.py index e16c1527e7..81fbdfd9fd 100644 --- a/dimos/memory2/module.py +++ b/dimos/memory2/module.py @@ -272,6 +272,8 @@ class RecorderConfig(MemoryModuleConfig): # read the active remappings from inside the module (AFAIK), so this config # arg does the per-stream rename directly. stream_remapping: dict[str, str] = Field(default_factory=dict) + # ex: {"depth_image_1": "lz4+lcm"} for lossless depth recording + stream_codecs: dict[str, str] = Field(default_factory=dict) PoseSetter = Callable[[Any], "Awaitable[Pose | None]"] @@ -359,7 +361,9 @@ def start(self) -> None: for name, port in self.inputs.items(): stream_name = self.config.stream_remapping.get(name, name) - stream: Stream[Any] = self.store.stream(stream_name, port.type) + codec = self.config.stream_codecs.get(stream_name) + overrides = {"codec": codec} if codec is not None else {} + stream: Stream[Any] = self.store.stream(stream_name, port.type, **overrides) self._port_to_stream(name, port, stream) logger.info("Recording %s -> %s (%s)", name, stream_name, port.type.__name__) From 624997af2c7ecaf1e3988bf9074cabcc0be38d07 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 17 Jul 2026 16:32:02 -0500 Subject: [PATCH 05/26] organize, untested --- dimos/robot/all_blueprints.py | 1 - dimos/robot/bosdyn/spot/blueprints/spot.py | 9 +- .../bosdyn/spot/blueprints/spot_cameras.py | 61 --- .../bosdyn/spot/blueprints/spot_record.py | 34 +- dimos/robot/bosdyn/spot/config.py | 80 +--- .../robot/bosdyn/spot/effectors/high_level.py | 434 ++++++++++-------- dimos/robot/bosdyn/spot/recorder.py | 43 +- dimos/robot/bosdyn/spot/rerun.py | 88 ++++ .../bosdyn/spot/{config.urdf => spot.urdf} | 74 ++- dimos/robot/bosdyn/spot/utils.py | 149 ++++++ dimos/robot/model_parser.py | 18 + 11 files changed, 620 insertions(+), 371 deletions(-) delete mode 100644 dimos/robot/bosdyn/spot/blueprints/spot_cameras.py create mode 100644 dimos/robot/bosdyn/spot/rerun.py rename dimos/robot/bosdyn/spot/{config.urdf => spot.urdf} (76%) create mode 100644 dimos/robot/bosdyn/spot/utils.py diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index bf09322306..ae0bafdd3e 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -81,7 +81,6 @@ "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_planner_coordinator", "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", "spot": "dimos.robot.bosdyn.spot.blueprints.spot:spot", - "spot-cameras": "dimos.robot.bosdyn.spot.blueprints.spot_cameras:spot_cameras", "spot-record": "dimos.robot.bosdyn.spot.blueprints.spot_record:spot_record", "teleop-hosted-go2": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_go2", "teleop-hosted-go2-transport": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_go2_transport", diff --git a/dimos/robot/bosdyn/spot/blueprints/spot.py b/dimos/robot/bosdyn/spot/blueprints/spot.py index a6032e3146..4a80016ff0 100644 --- a/dimos/robot/bosdyn/spot/blueprints/spot.py +++ b/dimos/robot/bosdyn/spot/blueprints/spot.py @@ -35,17 +35,16 @@ from __future__ import annotations from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config from dimos.navigation.movement_manager.movement_manager import MovementManager -from dimos.robot.bosdyn.spot.blueprints.spot_cameras import spot_camera_layout from dimos.robot.bosdyn.spot.effectors.high_level import SpotHighLevel -from dimos.visualization.rerun.bridge import RerunBridgeModule -from dimos.visualization.rerun.websocket_server import RerunWebSocketServer +from dimos.robot.bosdyn.spot.rerun import spot_camera_layout +from dimos.visualization.vis_module import vis_module spot = autoconnect( SpotHighLevel.blueprint(), MovementManager.blueprint(), - RerunBridgeModule.blueprint(blueprint=spot_camera_layout), - RerunWebSocketServer.blueprint(), + vis_module(global_config.viewer, rerun_config={"blueprint": spot_camera_layout}), ).remappings( [ # No nav stack here, so MovementManager's goal/way_point/stop_movement diff --git a/dimos/robot/bosdyn/spot/blueprints/spot_cameras.py b/dimos/robot/bosdyn/spot/blueprints/spot_cameras.py deleted file mode 100644 index c8cc107a62..0000000000 --- a/dimos/robot/bosdyn/spot/blueprints/spot_cameras.py +++ /dev/null @@ -1,61 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Spot camera/depth/odometry viewer. - -Powers on, stands the robot, and streams the five fisheye + five depth cameras -and body odometry into a Rerun viewer laid out as two vertical columns: the five -depth cameras on the left, the five grayscale cameras on the right. Unlike the -full `spot` blueprint there is no teleop, so the robot stands but does not drive. - -Everything rides the default Zenoh transport, which the Rerun bridge subscribes -to, so the viewer receives and renders the frames. - -Usage: - dimos run spot-cameras \ - -o spothighlevel.username=admin -o spothighlevel.password= -""" - -from __future__ import annotations - -import rerun.blueprint as rrb - -from dimos.core.coordination.blueprints import autoconnect -from dimos.robot.bosdyn.spot.effectors.high_level import SpotHighLevel -from dimos.visualization.rerun.bridge import RerunBridgeModule - -_NUM_CAMERAS = 5 - - -def spot_camera_layout() -> rrb.Blueprint: - """Depth column on the left, grayscale column on the right.""" - depth_column = rrb.Vertical( - *[ - rrb.Spatial2DView(origin=f"world/depth_image_{index}", name=f"depth {index}") - for index in range(1, _NUM_CAMERAS + 1) - ] - ) - grayscale_column = rrb.Vertical( - *[ - rrb.Spatial2DView(origin=f"world/grayscale_image_{index}", name=f"camera {index}") - for index in range(1, _NUM_CAMERAS + 1) - ] - ) - return rrb.Blueprint(rrb.Horizontal(depth_column, grayscale_column), collapse_panels=True) - - -spot_cameras = autoconnect( - SpotHighLevel.blueprint(), - RerunBridgeModule.blueprint(blueprint=spot_camera_layout), -) diff --git a/dimos/robot/bosdyn/spot/blueprints/spot_record.py b/dimos/robot/bosdyn/spot/blueprints/spot_record.py index 4f9bd1d22d..a541d375b7 100644 --- a/dimos/robot/bosdyn/spot/blueprints/spot_record.py +++ b/dimos/robot/bosdyn/spot/blueprints/spot_record.py @@ -14,13 +14,11 @@ """Spot: drive it from the Rerun web UI while recording every data stream. -The same click/teleop + camera stack as the default `spot` blueprint — -`RerunWebSocketServer` turns browser clicks/keys into `clicked_point` / -`tele_cmd_vel`, `MovementManager` muxes them into `cmd_vel`, and `SpotHighLevel` -executes it while streaming the five fisheye + five depth cameras and odometry — -with `SpotRecorder` added so every one of those streams (plus the live tf tree) -is written to a memory2 SQLite db as you drive. `autoconnect` wires the -recorder's In ports to `SpotHighLevel`'s outputs by name. +The default `spot` blueprint (click/teleop driving + full sensor streaming + +Rerun) with `SpotRecorder` added, so every one of `SpotHighLevel`'s streams +(the five fisheye + five depth cameras and odometry, plus the live tf tree) is +written to a memory2 SQLite db as you drive. `autoconnect` wires the recorder's +In ports to `SpotHighLevel`'s outputs by name. Usage: dimos run spot-record \ @@ -32,25 +30,7 @@ from __future__ import annotations from dimos.core.coordination.blueprints import autoconnect -from dimos.navigation.movement_manager.movement_manager import MovementManager -from dimos.robot.bosdyn.spot.blueprints.spot_cameras import spot_camera_layout -from dimos.robot.bosdyn.spot.effectors.high_level import SpotHighLevel +from dimos.robot.bosdyn.spot.blueprints.spot import spot from dimos.robot.bosdyn.spot.recorder import SpotRecorder -from dimos.visualization.rerun.bridge import RerunBridgeModule -from dimos.visualization.rerun.websocket_server import RerunWebSocketServer -spot_record = autoconnect( - SpotHighLevel.blueprint(), - MovementManager.blueprint(), - SpotRecorder.blueprint(), - RerunBridgeModule.blueprint(blueprint=spot_camera_layout), - RerunWebSocketServer.blueprint(), -).remappings( - [ - # No nav stack here, so MovementManager's goal/way_point/stop_movement - # outputs have no consumer — park them so autoconnect stays quiet. - (MovementManager, "goal", "_spot_goal_unused"), - (MovementManager, "way_point", "_spot_way_point_unused"), - (MovementManager, "stop_movement", "_spot_stop_movement_unused"), - ] -) +spot_record = autoconnect(spot, SpotRecorder.blueprint()) diff --git a/dimos/robot/bosdyn/spot/config.py b/dimos/robot/bosdyn/spot/config.py index 9c6a68aed0..e9a708e97c 100644 --- a/dimos/robot/bosdyn/spot/config.py +++ b/dimos/robot/bosdyn/spot/config.py @@ -20,11 +20,7 @@ from __future__ import annotations -import asyncio - -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() +from pathlib import Path # Spot's fixed default addresses: 192.168.80.3 when it hosts its own WiFi AP, # 10.0.0.3 on the rear Ethernet port. Probed in order when no ip is given. @@ -43,68 +39,16 @@ STAND_TIMEOUT_S = 10.0 SIT_TIMEOUT_S = 10.0 -# Spot's five body fisheye cameras and their matching depth cameras, ordered to -# match the grayscale_image_N / depth_image_N output streams. -GRAYSCALE_SOURCES = [ - "frontleft_fisheye_image", - "frontright_fisheye_image", - "left_fisheye_image", - "right_fisheye_image", - "back_fisheye_image", -] -DEPTH_SOURCES = [ - "frontleft_depth", - "frontright_depth", - "left_depth", - "right_depth", - "back_depth", -] - -# Spot reports poses in its gravity-agnostic "vision" frame; body is the moving -# base frame. These mirror the bosdyn frame-helper constants, inlined so this -# file imports without the SDK. -VISION_FRAME = "vision" -BODY_FRAME = "body" - - -def default_candidate_ips() -> list[str]: - return [SPOT_WIFI_AP_IP, SPOT_ETHERNET_IP] - - -async def is_reachable(ip: str) -> bool: - try: - _, writer = await asyncio.wait_for( - asyncio.open_connection(ip, SPOT_API_PORT), - timeout=REACHABILITY_PROBE_TIMEOUT_S, - ) - except (OSError, asyncio.TimeoutError): - return False - # Reachable — a successful TCP handshake is all we need. Close without - # awaiting wait_closed(); the API port speaks TLS and never completes a - # clean plaintext close, which would otherwise hang the probe. - writer.close() - return True - - -async def resolve_ip(candidate_ips: list[str]) -> str: - for candidate in candidate_ips: - if await is_reachable(candidate): - logger.info(f"Spot reachable at {candidate}") - return candidate - described = " or ".join( - f"{candidate} ({IP_LABELS[candidate]})" if candidate in IP_LABELS else candidate - for candidate in candidate_ips - ) - raise ConnectionError( - f"I'm unable to connect to {described}. Did you forget to connect to " - "Spot's WiFi or plug in an Ethernet cable to Spot?" - ) +# Static camera-mount geometry lives in this URDF (base_link -> body -> {pos}_camera +# -> {pos}_camera_optical, all fixed joints). SpotHighLevel publishes those mounts +# as static tf; the moving odom->base_link edge stays live from Spot's state. +SPOT_URDF_PATH = Path(__file__).parent / "spot.urdf" +# Stream-name suffix per camera mount, ordered to match the fisheye/depth source +# lists in SpotHighLevel. Names the grayscale_image_* / depth_image_* streams by position. +CAMERA_STREAM_SUFFIXES = ["front_left", "front_right", "left", "right", "back"] -def resolve_credentials(username: str | None, password: str | None) -> tuple[str, str]: - if not username or not password: - raise ValueError( - "Spot credentials missing — pass username/password in config " - "(-o .username=... -o .password=...)" - ) - return username, password +# Spot's body fisheye + depth cameras top out around 15 Hz. Poll at that rate so +# we never miss a frame; the acquisition_time dedup in SpotHighLevel drops any +# repeat returned by polling faster than a given camera actually refreshes. +CAMERA_MAX_HZ = 15.0 diff --git a/dimos/robot/bosdyn/spot/effectors/high_level.py b/dimos/robot/bosdyn/spot/effectors/high_level.py index aeaa49cc47..c866fd9cbb 100644 --- a/dimos/robot/bosdyn/spot/effectors/high_level.py +++ b/dimos/robot/bosdyn/spot/effectors/high_level.py @@ -19,10 +19,16 @@ from a `cmd_vel` Twist stream, and streams every onboard camera plus body odometry: -- `grayscale_image_1..5` — the five fisheye body cameras (front-left, front-right, - left, right, back), in that order. -- `depth_image_1..5` — the matching depth cameras, same ordering. -- `odom` — body pose + velocity in Spot's `vision` frame, also broadcast on TF. +- `grayscale_image_{front_left,front_right,left,right,back}` — the five fisheye + body cameras. +- `depth_image_{front_left,front_right,left,right,back}` — the matching depth cameras. +- `odom` — base pose + velocity, published live as `odom`->`base_link` on TF + (frame names configurable via `odom_frame_id` / `base_frame_id`). + +The fixed camera mounts (`base_link`->`{pos}_camera_optical`) come from the URDF +at `SPOT_URDF_PATH` instead of Spot's live snapshot: `SpotHighLevel` subclasses +`StaticTfPublisher`, which republishes those static extrinsics on an interval so +the moving odom edge and rigid mounts together anchor every recorded frame. `bosdyn` is an optional extra (`uv sync --extra spot`); its imports live inside methods so this file stays importable — and blueprint discovery keeps working — @@ -37,43 +43,46 @@ import time from typing import Any -import numpy as np - from dimos.agents.annotation import skill from dimos.core.core import rpc -from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.msgs.sensor_msgs.Image import Image +from dimos.protocol.tf.static_tf_publisher import StaticTfPublisher, StaticTfPublisherConfig from dimos.robot.bosdyn.spot.config import ( - BODY_FRAME, - DEPTH_SOURCES, - GRAYSCALE_SOURCES, + CAMERA_MAX_HZ, + IP_LABELS, POWER_OFF_TIMEOUT_S, POWER_ON_TIMEOUT_S, + REACHABILITY_PROBE_TIMEOUT_S, SIT_TIMEOUT_S, + SPOT_API_PORT, + SPOT_URDF_PATH, STAND_TIMEOUT_S, - VISION_FRAME, - default_candidate_ips, - resolve_credentials, - resolve_ip, +) +from dimos.robot.bosdyn.spot.utils import ( + camera_info_from_response, + camera_mount_transforms, + decode_image, ) from dimos.utils.logging_config import setup_logger logger = setup_logger() -class SpotHighLevelConfig(ModuleConfig): - """Connection, credentials, safety gating, and sensor capture for a Spot.""" +class SpotHighLevelConfig(StaticTfPublisherConfig): + """Cmd vel, sensors, credentials, and safety gating a Spot robot.""" - # Explicit address always wins (`-o spothighlevel.ip=`). When left - # blank, main() probes `candidate_ips` and uses the first that answers on the + # When left blank, main() probes `candidate_ips` and uses the first that answers on the # API port — so plugging in over Ethernet or joining Spot's WiFi both work. ip: str = "" - candidate_ips: list[str] = field(default_factory=default_candidate_ips) + candidate_ips: list[str] = field(default_factory=lambda: list(IP_LABELS)) # Auth — required. Startup fails fast if either is missing. username: str | None = None @@ -94,45 +103,46 @@ class SpotHighLevelConfig(ModuleConfig): # window. 9.0 s matches the bosdyn-client default. estop_timeout: float = 9.0 - # Which sources feed grayscale_image_N / depth_image_N (index N-1). Trim these - # to capture fewer cameras. - grayscale_sources: list[str] = field(default_factory=lambda: list(GRAYSCALE_SOURCES)) - depth_sources: list[str] = field(default_factory=lambda: list(DEPTH_SOURCES)) - - # Clockwise rotation (degrees, multiple of 90) applied to a camera's - # grayscale + depth before publishing, correcting for physically rotated - # fisheye mounts. Keyed by 1-based camera index. - image_rotations_cw: dict[int, int] = field(default_factory=lambda: {1: 90, 2: 90, 4: 180}) - - image_rate_hz: float = 5.0 + # frame_id's + odom_frame_id: str = "odom" + base_frame_id: str = "base_link" + frontleft_camera_frame_id: str = "frontleft_camera_optical" + frontright_camera_frame_id: str = "frontright_camera_optical" + left_camera_frame_id: str = "left_camera_optical" + right_camera_frame_id: str = "right_camera_optical" + back_camera_frame_id: str = "back_camera_optical" + + # Poll at the camera's max rate; SpotHighLevel dedups repeats by acquisition_time. + image_rate_hz: float = CAMERA_MAX_HZ odom_rate_hz: float = 20.0 -class SpotHighLevel(Module): +class SpotHighLevel(StaticTfPublisher): """Drives Spot and streams its fisheye cameras, depth cameras, and odometry.""" - # A hardware-driving module gets its own worker process, matching the other - # robot connection modules (go2/b1/drone). Sharing a process with a GUI - # module like KeyboardTeleop wedges this module's RPC server at startup. - dedicated_worker = True + config: SpotHighLevelConfig cmd_vel: In[Twist] - grayscale_image_1: Out[Image] - grayscale_image_2: Out[Image] - grayscale_image_3: Out[Image] - grayscale_image_4: Out[Image] - grayscale_image_5: Out[Image] + grayscale_image_front_left: Out[Image] + grayscale_image_front_right: Out[Image] + grayscale_image_left: Out[Image] + grayscale_image_right: Out[Image] + grayscale_image_back: Out[Image] + + depth_image_front_left: Out[Image] + depth_image_front_right: Out[Image] + depth_image_left: Out[Image] + depth_image_right: Out[Image] + depth_image_back: Out[Image] - depth_image_1: Out[Image] - depth_image_2: Out[Image] - depth_image_3: Out[Image] - depth_image_4: Out[Image] - depth_image_5: Out[Image] + # All five grayscale cameras share one lens model and all five depth cameras share another + grayscale_info: Out[CameraInfo] + depth_info: Out[CameraInfo] odom: Out[Odometry] - config: SpotHighLevelConfig + dedicated_worker = True def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) @@ -149,9 +159,33 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: # never reaches a half-initialised SDK. self._ready = asyncio.Event() + def transforms(self) -> list[Transform]: + """Static base_link -> camera-optical extrinsics parsed from the URDF. + + `StaticTfPublisher` republishes these on a fixed interval; the moving + odom->base_link edge stays live (see `_publish_odom`). + """ + return camera_mount_transforms( + SPOT_URDF_PATH, + self.config.base_frame_id, + [ + self.config.frontleft_camera_frame_id, + self.config.frontright_camera_frame_id, + self.config.left_camera_frame_id, + self.config.right_camera_frame_id, + self.config.back_camera_frame_id, + ], + ) + async def main(self) -> AsyncIterator[None]: - username, password = resolve_credentials(self.config.username, self.config.password) - ip = self.config.ip or await resolve_ip(self.config.candidate_ips) + username, password = self.config.username, self.config.password + if not username or not password: + raise ValueError( + "Spot credentials missing — pass username/password in config " + "(-o .username=... -o .password=...)" + ) + + ip = await self.resolve_ip() from bosdyn.client import create_standard_sdk # type: ignore[import-not-found] from bosdyn.client.estop import ( # type: ignore[import-not-found] @@ -176,7 +210,7 @@ async def main(self) -> AsyncIterator[None]: sdk = await asyncio.to_thread(create_standard_sdk, "dimos-spot") self._robot = await asyncio.to_thread(sdk.create_robot, ip) await asyncio.to_thread(self._robot.authenticate, username, password) - await asyncio.to_thread(self._robot.time_sync.wait_for_sync) + await self.sync_clocks() if self.config.enable_estop: estop_client = self._robot.ensure_client(EstopClient.default_service_name) @@ -223,17 +257,7 @@ async def main(self) -> AsyncIterator[None]: task.cancel() if self._standing: - try: - from bosdyn.client.robot_command import ( # type: ignore[import-not-found] - blocking_sit, - ) - - await asyncio.to_thread( - blocking_sit, self._command_client, timeout_sec=SIT_TIMEOUT_S - ) - except Exception as error: - logger.error(f"Spot sit during teardown failed: {error}") - self._standing = False + await self.lie_down() if self.config.power_on_at_start and self._robot is not None: try: @@ -259,61 +283,72 @@ async def main(self) -> AsyncIterator[None]: async def handle_cmd_vel(self, msg: Twist) -> None: if not self._ready.is_set(): return - await self._send_velocity(msg.linear.x, msg.linear.y, msg.angular.z) - - async def _send_velocity( - self, forward: float, strafe: float, yaw: float, duration: float = 0.0 - ) -> bool: - from bosdyn.client.robot_command import ( # type: ignore[import-not-found] - RobotCommandBuilder, - ) - - command = RobotCommandBuilder.synchro_velocity_command(v_x=forward, v_y=strafe, v_rot=yaw) - window = duration if duration > 0 else self.config.cmd_vel_timeout - try: - await asyncio.to_thread( - self._command_client.robot_command, - command, - end_time_secs=time.time() + window, - ) - return True - except Exception as error: - logger.error(f"Spot velocity command failed: {error}") - return False - - def _grayscale_outputs(self) -> list[Out[Image]]: - return [ - self.grayscale_image_1, - self.grayscale_image_2, - self.grayscale_image_3, - self.grayscale_image_4, - self.grayscale_image_5, - ] - - def _depth_outputs(self) -> list[Out[Image]]: - return [ - self.depth_image_1, - self.depth_image_2, - self.depth_image_3, - self.depth_image_4, - self.depth_image_5, - ] + await self.move(msg) async def _poll_images(self) -> None: period = 1.0 / self.config.image_rate_hz - sources = self.config.grayscale_sources + self.config.depth_sources - # source name -> (Out stream that carries it, np.rot90 k applied before publish). - routing: dict[str, tuple[Out[Image], int]] = {} - # Fewer configured sources than output streams is fine — extra streams - # just stay silent, so pair only as many as the shorter list. - for index, (source, out) in enumerate( - zip(self.config.grayscale_sources, self._grayscale_outputs(), strict=False) - ): - routing[source] = (out, self._rotation_k(index + 1)) - for index, (source, out) in enumerate( - zip(self.config.depth_sources, self._depth_outputs(), strict=False) - ): - routing[source] = (out, self._rotation_k(index + 1)) + config = self.config + # bosdyn source name -> (image Out, CameraInfo Out, frame_id). Images are + # published in Spot's native sensor orientation (no rotation); each image's + # frame_id names a URDF optical frame anchored by the static camera tf. + # Grayscale and depth at the same mount share that mount's optical frame. + routing: dict[str, tuple[Out[Image], Out[CameraInfo], str]] = { + "frontleft_fisheye_image": ( + self.grayscale_image_front_left, + self.grayscale_info, + config.frontleft_camera_frame_id, + ), + "frontright_fisheye_image": ( + self.grayscale_image_front_right, + self.grayscale_info, + config.frontright_camera_frame_id, + ), + "left_fisheye_image": ( + self.grayscale_image_left, + self.grayscale_info, + config.left_camera_frame_id, + ), + "right_fisheye_image": ( + self.grayscale_image_right, + self.grayscale_info, + config.right_camera_frame_id, + ), + "back_fisheye_image": ( + self.grayscale_image_back, + self.grayscale_info, + config.back_camera_frame_id, + ), + "frontleft_depth": ( + self.depth_image_front_left, + self.depth_info, + config.frontleft_camera_frame_id, + ), + "frontright_depth": ( + self.depth_image_front_right, + self.depth_info, + config.frontright_camera_frame_id, + ), + "left_depth": ( + self.depth_image_left, + self.depth_info, + config.left_camera_frame_id, + ), + "right_depth": ( + self.depth_image_right, + self.depth_info, + config.right_camera_frame_id, + ), + "back_depth": ( + self.depth_image_back, + self.depth_info, + config.back_camera_frame_id, + ), + } + sources = list(routing) + # Sensor capture time of the last frame published per source. Polling above + # the sensor's frame rate re-returns the same frame; matching acquisition + # time means it's a repeat, so skip it and never publish a frame twice. + last_published_ts: dict[str, float] = {} while True: start = time.monotonic() @@ -321,6 +356,7 @@ async def _poll_images(self) -> None: responses = await asyncio.to_thread( self._image_client.get_image_from_sources, sources ) + time_converter = self._robot.time_sync.get_robot_time_converter() except Exception as error: logger.error(f"Spot image capture failed: {error}") await asyncio.sleep(period) @@ -331,21 +367,20 @@ async def _poll_images(self) -> None: route = routing.get(source_name) if route is None: continue - out, rotation_k = route - image = _decode_image(response, source_name) + out, info_out, frame_id = route + image = decode_image(response, frame_id, time_converter) if image is None: continue - if rotation_k: - image = _rotate_image(image, rotation_k) + if last_published_ts.get(source_name) == image.ts: + continue + last_published_ts[source_name] = image.ts out.publish(image) + camera_info = camera_info_from_response(response, frame_id, image.ts) + if camera_info is not None: + info_out.publish(camera_info) await asyncio.sleep(max(0.0, period - (time.monotonic() - start))) - def _rotation_k(self, camera_number: int) -> int: - """Convert a clockwise degree rotation into an np.rot90 k (0-3).""" - degrees_cw = self.config.image_rotations_cw.get(camera_number, 0) - return (-degrees_cw // 90) % 4 - async def _poll_odom(self) -> None: from bosdyn.client.frame_helpers import ( # type: ignore[import-not-found] BODY_FRAME_NAME, @@ -373,9 +408,6 @@ async def _poll_odom(self) -> None: await asyncio.sleep(max(0.0, period - (time.monotonic() - start))) def _publish_odom(self, vision_tform_body: Any, velocity: Any) -> None: - from dimos.msgs.geometry_msgs.Quaternion import Quaternion - from dimos.msgs.geometry_msgs.Transform import Transform - now = time.time() pose = Pose( position=[vision_tform_body.x, vision_tform_body.y, vision_tform_body.z], @@ -392,8 +424,8 @@ def _publish_odom(self, vision_tform_body: Any, velocity: Any) -> None: ) odometry = Odometry( ts=now, - frame_id=VISION_FRAME, - child_frame_id=BODY_FRAME, + frame_id=self.config.odom_frame_id, + child_frame_id=self.config.base_frame_id, pose=pose, twist=twist, ) @@ -407,8 +439,8 @@ def _publish_odom(self, vision_tform_body: Any, velocity: Any) -> None: vision_tform_body.rot.z, vision_tform_body.rot.w, ), - frame_id=VISION_FRAME, - child_frame_id=BODY_FRAME, + frame_id=self.config.odom_frame_id, + child_frame_id=self.config.base_frame_id, ts=now, ) ) @@ -416,7 +448,26 @@ def _publish_odom(self, vision_tform_body: Any, velocity: Any) -> None: @rpc async def move(self, twist: Twist, duration: float = 0.0) -> bool: """Send a Twist as a body velocity command, optionally for `duration` seconds.""" - return await self._send_velocity(twist.linear.x, twist.linear.y, twist.angular.z, duration) + if self._command_client is None: + return False + from bosdyn.client.robot_command import ( # type: ignore[import-not-found] + RobotCommandBuilder, + ) + + command = RobotCommandBuilder.synchro_velocity_command( + v_x=twist.linear.x, v_y=twist.linear.y, v_rot=twist.angular.z + ) + window = duration if duration > 0 else self.config.cmd_vel_timeout + try: + await asyncio.to_thread( + self._command_client.robot_command, + command, + end_time_secs=time.time() + window, + ) + return True + except Exception as error: + logger.error(f"Spot velocity command failed: {error}") + return False @rpc async def get_state(self) -> str: @@ -429,6 +480,21 @@ async def get_state(self) -> str: logger.error(f"Spot get_state failed: {error}") return "UNKNOWN" + @skill + async def get_battery_soc(self) -> str: + """Report Spot's battery state of charge as a percentage.""" + if self._state_client is None: + return "Spot is not connected." + try: + state = await asyncio.to_thread(self._state_client.get_robot_state) + except Exception as error: + logger.error(f"Spot get_battery_soc failed: {error}") + return "Failed to read Spot battery state." + for battery in state.battery_states: + if battery.HasField("charge_percentage"): + return f"Battery is at {battery.charge_percentage.value:.0f}%." + return "Battery charge is unavailable." + @skill async def move_velocity( self, x: float, y: float = 0.0, yaw: float = 0.0, duration: float = 0.0 @@ -466,8 +532,8 @@ async def stand(self) -> str: return f"Stand failed: {error}" @skill - async def sit(self) -> str: - """Make Spot sit down.""" + async def lie_down(self) -> str: + """Make Spot lie down.""" if self._command_client is None: return "Spot is not connected." try: @@ -477,63 +543,49 @@ async def sit(self) -> str: await asyncio.to_thread(blocking_sit, self._command_client, timeout_sec=SIT_TIMEOUT_S) self._standing = False - return "Spot is sitting." + return "Spot is lying down." except Exception as error: - logger.error(f"Spot sit failed: {error}") - return f"Sit failed: {error}" - - -def _rotate_image(image: Image, rotation_k: int) -> Image: - """Rotate an image counterclockwise by rotation_k * 90 degrees.""" - rotated = np.ascontiguousarray(np.rot90(image.data, rotation_k)) - return Image(data=rotated, format=image.format, frame_id=image.frame_id, ts=image.ts) - - -def _decode_image(response: Any, source_name: str) -> Image | None: - """Turn a bosdyn ImageResponse into a dimos Image, or None if unsupported.""" - from bosdyn.api import image_pb2 # type: ignore[import-not-found] - - shot = response.shot.image - pixel_format = shot.pixel_format - now = time.time() - - if shot.format == image_pb2.Image.FORMAT_JPEG: - import cv2 - - buffer = np.frombuffer(shot.data, dtype=np.uint8) - decoded = cv2.imdecode(buffer, cv2.IMREAD_UNCHANGED) - if decoded is None: - logger.error(f"Failed to decode JPEG image from {source_name}") - return None - image_format = ImageFormat.GRAY if decoded.ndim == 2 else ImageFormat.BGR - return Image.from_numpy(decoded, format=image_format, frame_id=source_name, ts=now) - - if shot.format != image_pb2.Image.FORMAT_RAW: - logger.error(f"Unsupported Spot image encoding {shot.format} from {source_name}") - return None - - dtype, channels, image_format = _raw_layout(pixel_format) - if dtype is None: - logger.error(f"Unsupported Spot pixel format {pixel_format} from {source_name}") - return None - - array = np.frombuffer(shot.data, dtype=dtype) - array = ( - array.reshape(shot.rows, shot.cols) - if channels == 1 - else array.reshape(shot.rows, shot.cols, channels) - ) - return Image.from_numpy(array, format=image_format, frame_id=source_name, ts=now) - - -def _raw_layout(pixel_format: int) -> tuple[Any, int, ImageFormat]: - from bosdyn.api import image_pb2 # type: ignore[import-not-found] - - layouts: dict[int, tuple[Any, int, ImageFormat]] = { - image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U8: (np.uint8, 1, ImageFormat.GRAY), - image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U16: (np.uint16, 1, ImageFormat.GRAY16), - image_pb2.Image.PIXEL_FORMAT_DEPTH_U16: (np.uint16, 1, ImageFormat.DEPTH16), - image_pb2.Image.PIXEL_FORMAT_RGB_U8: (np.uint8, 3, ImageFormat.RGB), - image_pb2.Image.PIXEL_FORMAT_RGBA_U8: (np.uint8, 4, ImageFormat.RGBA), - } - return layouts.get(pixel_format, (None, 0, ImageFormat.GRAY)) + logger.error(f"Spot lie_down failed: {error}") + return f"Lie down failed: {error}" + + async def resolve_ip(self) -> str: + """The Spot IP to connect to: explicit `config.ip`, else the first reachable candidate. + + With no `config.ip`, each candidate gets a short TCP connect to the API + port and the first successful handshake wins — so Ethernet or Spot's WiFi + both work without configuration. Raises `ConnectionError` if none answer. + """ + if self.config.ip: + return self.config.ip + for candidate in self.config.candidate_ips: + try: + _, writer = await asyncio.wait_for( + asyncio.open_connection(candidate, SPOT_API_PORT), + timeout=REACHABILITY_PROBE_TIMEOUT_S, + ) + except (OSError, asyncio.TimeoutError): + continue + # A successful TCP handshake is all we need. Close without awaiting + # wait_closed(); the API port speaks TLS and never completes a clean + # plaintext close, which would otherwise hang the probe. + writer.close() + logger.info(f"Spot reachable at {candidate}") + return candidate + described = " or ".join( + f"{candidate} ({IP_LABELS[candidate]})" if candidate in IP_LABELS else candidate + for candidate in self.config.candidate_ips + ) + raise ConnectionError( + f"I'm unable to connect to {described}. Did you forget to connect to " + "Spot's WiFi or plug in an Ethernet cable to Spot?" + ) + + async def sync_clocks(self) -> None: + """Establish time sync so robot-clock image timestamps convert to local time. + + Touching `robot.time_sync` starts bosdyn's background sync thread, which keeps + re-estimating clock skew on an interval; this blocks until the first estimate + lands. `_poll_images` then pulls a live `RobotTimeConverter` each cycle rather + than freezing one offset here, since the skew drifts and would go stale. + """ + await asyncio.to_thread(self._robot.time_sync.wait_for_sync) diff --git a/dimos/robot/bosdyn/spot/recorder.py b/dimos/robot/bosdyn/spot/recorder.py index 69df2585f8..d89a518f5a 100644 --- a/dimos/robot/bosdyn/spot/recorder.py +++ b/dimos/robot/bosdyn/spot/recorder.py @@ -24,16 +24,34 @@ from pathlib import Path +from pydantic import Field + from dimos.core.stream import In from dimos.memory2.module import OnExisting, Recorder, RecorderConfig from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image +from dimos.robot.bosdyn.spot.config import CAMERA_STREAM_SUFFIXES + +# jpeg codec quantises depth it to ~25cm and adds block artifacts (horrible) +LOSSLESS_CODEC = "lz4+lcm" class SpotRecorderConfig(RecorderConfig): db_path: str | Path = "spot_recording.db" # Append into a populated db so re-runs add to the same recording. on_existing: OnExisting = OnExisting.APPEND + # The odom frame is the fixed root SpotHighLevel anchors against. It publishes + # odom->base_link and base_link->camera edges, so image/odom poses resolve + # against this root. Must match SpotHighLevelConfig.odom_frame_id. + root_frame: str = "odom" + stream_codecs: dict[str, str] = Field( + default_factory=lambda: { + f"{kind}_image_{suffix}": LOSSLESS_CODEC + for kind in ("grayscale", "depth") + for suffix in CAMERA_STREAM_SUFFIXES + } + ) class SpotRecorder(Recorder): @@ -41,16 +59,19 @@ class SpotRecorder(Recorder): config: SpotRecorderConfig - grayscale_image_1: In[Image] - grayscale_image_2: In[Image] - grayscale_image_3: In[Image] - grayscale_image_4: In[Image] - grayscale_image_5: In[Image] - - depth_image_1: In[Image] - depth_image_2: In[Image] - depth_image_3: In[Image] - depth_image_4: In[Image] - depth_image_5: In[Image] + grayscale_image_front_left: In[Image] + grayscale_image_front_right: In[Image] + grayscale_image_left: In[Image] + grayscale_image_right: In[Image] + grayscale_image_back: In[Image] + + depth_image_front_left: In[Image] + depth_image_front_right: In[Image] + depth_image_left: In[Image] + depth_image_right: In[Image] + depth_image_back: In[Image] + + grayscale_info: In[CameraInfo] + depth_info: In[CameraInfo] odom: In[Odometry] diff --git a/dimos/robot/bosdyn/spot/rerun.py b/dimos/robot/bosdyn/spot/rerun.py new file mode 100644 index 0000000000..f868ca2a20 --- /dev/null +++ b/dimos/robot/bosdyn/spot/rerun.py @@ -0,0 +1,88 @@ +# 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. + +"""Rerun visualization helpers for Spot blueprints.""" + +from __future__ import annotations + +import rerun as rr +import rerun.blueprint as rrb + + +def _grayscale_origin(suffix: str) -> str: + return f"world/grayscale_image_{suffix}" + + +def _depth_origin(suffix: str) -> str: + return f"world/depth_image_{suffix}" + + +def _camera_view(origin: str, name: str) -> rrb.Spatial2DView: + return rrb.Spatial2DView(origin=origin, name=name) + + +def spot_camera_layout() -> rrb.Blueprint: + """A 3D view of Spot beside tabbed 2D camera panels. + + Left: the robot's tf tree and every camera frustum in place. Right: tabs of + grayscale/depth feeds labelled by URDF mount position. + """ + world_view = rrb.Spatial3DView( + origin="world", + name="3D", + background=rrb.Background(kind="SolidColor", color=[0, 0, 0]), + line_grid=rrb.LineGrid3D(plane=rr.components.Plane3D.XY.with_distance(0.0)), + ) + + front_right_gray = _camera_view(_grayscale_origin("front_right"), "front right") + front_left_gray = _camera_view(_grayscale_origin("front_left"), "front left") + front_right_depth = _camera_view(_depth_origin("front_right"), "front right (depth)") + front_left_depth = _camera_view(_depth_origin("front_left"), "front left (depth)") + + front_tab = rrb.Horizontal( + front_right_gray, + front_left_gray, + front_right_depth, + front_left_depth, + name="front", + ) + + grayscale_tab = rrb.Vertical( + rrb.Horizontal(front_right_gray, front_left_gray), + rrb.Horizontal( + _camera_view(_grayscale_origin("left"), "left"), + _camera_view(_grayscale_origin("right"), "right"), + ), + _camera_view(_grayscale_origin("back"), "back"), + name="grayscale", + ) + + depth_tab = rrb.Vertical( + rrb.Horizontal(front_right_depth, front_left_depth), + rrb.Horizontal( + _camera_view(_depth_origin("left"), "left"), + _camera_view(_depth_origin("right"), "right"), + ), + _camera_view(_depth_origin("back"), "back"), + name="depth", + ) + + return rrb.Blueprint( + rrb.Horizontal( + world_view, + rrb.Tabs(front_tab, grayscale_tab, depth_tab), + column_shares=[2, 1], + ), + collapse_panels=True, + ) diff --git a/dimos/robot/bosdyn/spot/config.urdf b/dimos/robot/bosdyn/spot/spot.urdf similarity index 76% rename from dimos/robot/bosdyn/spot/config.urdf rename to dimos/robot/bosdyn/spot/spot.urdf index a4a3a89817..0f5dc71ab2 100644 --- a/dimos/robot/bosdyn/spot/config.urdf +++ b/dimos/robot/bosdyn/spot/spot.urdf @@ -1,8 +1,6 @@ - - - + @@ -295,8 +293,70 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/dimos/robot/bosdyn/spot/utils.py b/dimos/robot/bosdyn/spot/utils.py new file mode 100644 index 0000000000..972da4a895 --- /dev/null +++ b/dimos/robot/bosdyn/spot/utils.py @@ -0,0 +1,149 @@ +# 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. + +"""Pure helpers for Spot: URDF mount extrinsics and bosdyn image decoding.""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any + +import numpy as np + +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.robot.model_parser import JointDescription, parse_model +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +def joint_to_transform(joint: JointDescription) -> Transform: + return Transform( + translation=Vector3(*joint.origin_xyz), + rotation=Quaternion.from_euler(Vector3(*joint.origin_rpy)), + frame_id=joint.parent_link, + child_frame_id=joint.child_link, + ) + + +def camera_mount_transforms( + urdf_path: str | Path, base_frame_id: str, optical_frames: list[str] +) -> list[Transform]: + """Compose each base_frame_id -> optical_frame extrinsic from the URDF's fixed joints. + + Walks the fixed-joint chain (base_link -> body -> {pos}_camera -> optical) up + from each optical frame and folds the per-joint origins into one transform, so + the recorded images resolve a pose against the live odom->base_link edge. The + URDF's root link is renamed to `base_frame_id` so callers can rebase the tree. + """ + model = parse_model(urdf_path) + urdf_root = model.root_link + joint_by_child = {joint.child_link: joint for joint in model.joints} + transforms: list[Transform] = [] + for optical_frame in optical_frames: + chain: list[JointDescription] = [] + current = optical_frame + while current != urdf_root and current in joint_by_child: + joint = joint_by_child[current] + chain.append(joint) + current = joint.parent_link + if current != urdf_root: + logger.warning(f"URDF has no fixed chain from {urdf_root} to {optical_frame}") + continue + edges = [joint_to_transform(joint) for joint in reversed(chain)] + composed = edges[0] + for edge in edges[1:]: + composed = composed + edge + composed.frame_id = base_frame_id + transforms.append(composed) + return transforms + + +def camera_info_from_response(response: Any, source_name: str, ts: float) -> CameraInfo | None: + """Build a CameraInfo from a bosdyn image response's pinhole intrinsics.""" + source = response.source + if not source.HasField("pinhole"): + return None + intrinsics = source.pinhole.intrinsics + info = CameraInfo.from_intrinsics( + fx=intrinsics.focal_length.x, + fy=intrinsics.focal_length.y, + cx=intrinsics.principal_point.x, + cy=intrinsics.principal_point.y, + width=source.cols, + height=source.rows, + frame_id=source_name, + ) + return info.with_ts(ts) + + +def decode_image(response: Any, frame_id: str, time_converter: Any) -> Image | None: + """Turn a bosdyn ImageResponse into a dimos Image, or None if unsupported. + + Stamps each image with its true capture time: the robot-clock `acquisition_time` + converted to local time by `time_converter`, bosdyn's live clock-skew estimate + (`RobotTimeConverter`). Polling faster than the sensor returns the same frame, so + keeping the sensor timestamp lets downstream drop the repeat instead of seeing a + fresh wall-clock stamp. + """ + from bosdyn.api import image_pb2 # type: ignore[import-not-found] + + shot = response.shot.image + pixel_format = shot.pixel_format + ts = time_converter.local_seconds_from_robot_timestamp(response.shot.acquisition_time) + + if shot.format == image_pb2.Image.FORMAT_JPEG: + import cv2 + + buffer = np.frombuffer(shot.data, dtype=np.uint8) + decoded = cv2.imdecode(buffer, cv2.IMREAD_UNCHANGED) + if decoded is None: + logger.error(f"Failed to decode JPEG image from {frame_id}") + return None + image_format = ImageFormat.GRAY if decoded.ndim == 2 else ImageFormat.BGR + return Image.from_numpy(decoded, format=image_format, frame_id=frame_id, ts=ts) + + if shot.format != image_pb2.Image.FORMAT_RAW: + logger.error(f"Unsupported Spot image encoding {shot.format} from {frame_id}") + return None + + dtype, channels, image_format = raw_layout(pixel_format) + if dtype is None: + logger.error(f"Unsupported Spot pixel format {pixel_format} from {frame_id}") + return None + + array = np.frombuffer(shot.data, dtype=dtype) + array = ( + array.reshape(shot.rows, shot.cols) + if channels == 1 + else array.reshape(shot.rows, shot.cols, channels) + ) + return Image.from_numpy(array, format=image_format, frame_id=frame_id, ts=ts) + + +def raw_layout(pixel_format: int) -> tuple[Any, int, ImageFormat]: + from bosdyn.api import image_pb2 # type: ignore[import-not-found] + + layouts: dict[int, tuple[Any, int, ImageFormat]] = { + image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U8: (np.uint8, 1, ImageFormat.GRAY), + image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U16: (np.uint16, 1, ImageFormat.GRAY16), + image_pb2.Image.PIXEL_FORMAT_DEPTH_U16: (np.uint16, 1, ImageFormat.DEPTH16), + image_pb2.Image.PIXEL_FORMAT_RGB_U8: (np.uint8, 3, ImageFormat.RGB), + image_pb2.Image.PIXEL_FORMAT_RGBA_U8: (np.uint8, 4, ImageFormat.RGBA), + } + return layouts.get(pixel_format, (None, 0, ImageFormat.GRAY)) diff --git a/dimos/robot/model_parser.py b/dimos/robot/model_parser.py index 7040d98fc3..25e2b6cc22 100644 --- a/dimos/robot/model_parser.py +++ b/dimos/robot/model_parser.py @@ -37,6 +37,8 @@ class JointDescription: effort_limit: float | None = None parent_link: str = "" child_link: str = "" + origin_xyz: tuple[float, float, float] = (0.0, 0.0, 0.0) + origin_rpy: tuple[float, float, float] = (0.0, 0.0, 0.0) @dataclass @@ -131,6 +133,10 @@ def _parse_urdf_string(xml_string: str) -> ModelDescription: if child_link: child_links.add(child_link) + origin_elem = joint_elem.find("origin") + origin_xyz = _triple(origin_elem.get("xyz") if origin_elem is not None else None) + origin_rpy = _triple(origin_elem.get("rpy") if origin_elem is not None else None) + lower = upper = velocity = effort = None limit_elem = joint_elem.find("limit") if limit_elem is not None: @@ -149,6 +155,8 @@ def _parse_urdf_string(xml_string: str) -> ModelDescription: effort_limit=effort, parent_link=parent_link, child_link=child_link, + origin_xyz=origin_xyz, + origin_rpy=origin_rpy, ) ) @@ -230,6 +238,16 @@ def _walk_mjcf_bodies( _walk_mjcf_bodies(body, joints, links, parent_body=body_name) +def _triple(value: str | None) -> tuple[float, float, float]: + """Parse a URDF space-separated 3-vector (xyz/rpy), defaulting to zeros.""" + if not value: + return (0.0, 0.0, 0.0) + parts = [float(part) for part in value.split()] + if len(parts) != 3: + raise ValueError(f"Expected 3 values, got {value!r}") + return (parts[0], parts[1], parts[2]) + + def _float_or_none(value: str | None) -> float | None: if value is None: return None From 21c6c883e98080974638654917d66880ad1008b8 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sat, 18 Jul 2026 13:41:31 -0500 Subject: [PATCH 06/26] fixup, hardware tested, odom timestamps fix --- .../robot/bosdyn/spot/effectors/high_level.py | 23 +++++++++++-------- dimos/robot/bosdyn/spot/recorder.py | 10 +++++--- 2 files changed, 20 insertions(+), 13 deletions(-) diff --git a/dimos/robot/bosdyn/spot/effectors/high_level.py b/dimos/robot/bosdyn/spot/effectors/high_level.py index c866fd9cbb..b6a681ba7f 100644 --- a/dimos/robot/bosdyn/spot/effectors/high_level.py +++ b/dimos/robot/bosdyn/spot/effectors/high_level.py @@ -22,8 +22,8 @@ - `grayscale_image_{front_left,front_right,left,right,back}` — the five fisheye body cameras. - `depth_image_{front_left,front_right,left,right,back}` — the matching depth cameras. -- `odom` — base pose + velocity, published live as `odom`->`base_link` on TF - (frame names configurable via `odom_frame_id` / `base_frame_id`). +- `odometry` — base pose + velocity, also published live as `odom`->`base_link` + on TF (frame names configurable via `odom_frame_id` / `base_frame_id`). The fixed camera mounts (`base_link`->`{pos}_camera_optical`) come from the URDF at `SPOT_URDF_PATH` instead of Spot's live snapshot: `SpotHighLevel` subclasses @@ -114,7 +114,7 @@ class SpotHighLevelConfig(StaticTfPublisherConfig): # Poll at the camera's max rate; SpotHighLevel dedups repeats by acquisition_time. image_rate_hz: float = CAMERA_MAX_HZ - odom_rate_hz: float = 20.0 + odom_rate_hz: float = 60.0 class SpotHighLevel(StaticTfPublisher): @@ -140,7 +140,7 @@ class SpotHighLevel(StaticTfPublisher): grayscale_info: Out[CameraInfo] depth_info: Out[CameraInfo] - odom: Out[Odometry] + odometry: Out[Odometry] dedicated_worker = True @@ -403,12 +403,15 @@ async def _poll_odom(self) -> None: kinematic_state.transforms_snapshot, VISION_FRAME_NAME, BODY_FRAME_NAME ) velocity = kinematic_state.velocity_of_body_in_vision - self._publish_odom(vision_tform_body, velocity) + time_converter = self._robot.time_sync.get_robot_time_converter() + ts = time_converter.local_seconds_from_robot_timestamp( + kinematic_state.acquisition_timestamp + ) + self._publish_odom(vision_tform_body, velocity, ts) await asyncio.sleep(max(0.0, period - (time.monotonic() - start))) - def _publish_odom(self, vision_tform_body: Any, velocity: Any) -> None: - now = time.time() + def _publish_odom(self, vision_tform_body: Any, velocity: Any, ts: float) -> None: pose = Pose( position=[vision_tform_body.x, vision_tform_body.y, vision_tform_body.z], orientation=[ @@ -423,13 +426,13 @@ def _publish_odom(self, vision_tform_body: Any, velocity: Any) -> None: angular=[velocity.angular.x, velocity.angular.y, velocity.angular.z], ) odometry = Odometry( - ts=now, + ts=ts, frame_id=self.config.odom_frame_id, child_frame_id=self.config.base_frame_id, pose=pose, twist=twist, ) - self.odom.publish(odometry) + self.odometry.publish(odometry) self.tf.publish( Transform( translation=Vector3(vision_tform_body.x, vision_tform_body.y, vision_tform_body.z), @@ -441,7 +444,7 @@ def _publish_odom(self, vision_tform_body: Any, velocity: Any) -> None: ), frame_id=self.config.odom_frame_id, child_frame_id=self.config.base_frame_id, - ts=now, + ts=ts, ) ) diff --git a/dimos/robot/bosdyn/spot/recorder.py b/dimos/robot/bosdyn/spot/recorder.py index d89a518f5a..39af57186a 100644 --- a/dimos/robot/bosdyn/spot/recorder.py +++ b/dimos/robot/bosdyn/spot/recorder.py @@ -22,6 +22,7 @@ from __future__ import annotations +from datetime import datetime from pathlib import Path from pydantic import Field @@ -38,8 +39,11 @@ class SpotRecorderConfig(RecorderConfig): - db_path: str | Path = "spot_recording.db" - # Append into a populated db so re-runs add to the same recording. + # Timestamped by default so each run lands in its own file instead of appending + # onto the last one. Override with `-o spotrecorder.db_path=...` to pick a path. + db_path: str | Path = Field( + default_factory=lambda: f"spot_recording_{datetime.now():%Y-%m-%d_%H-%M-%S}.db" + ) on_existing: OnExisting = OnExisting.APPEND # The odom frame is the fixed root SpotHighLevel anchors against. It publishes # odom->base_link and base_link->camera edges, so image/odom poses resolve @@ -74,4 +78,4 @@ class SpotRecorder(Recorder): grayscale_info: In[CameraInfo] depth_info: In[CameraInfo] - odom: In[Odometry] + odometry: In[Odometry] From 1b49dfac81748111d9f1138136b717e51dbf896d Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sat, 18 Jul 2026 14:18:04 -0500 Subject: [PATCH 07/26] redo how rotations work, tested on hardware --- dimos/robot/bosdyn/spot/config.py | 10 +++++++ .../robot/bosdyn/spot/effectors/high_level.py | 29 ++++++++++++++----- dimos/robot/bosdyn/spot/rerun.py | 25 +++++++++------- dimos/robot/bosdyn/spot/utils.py | 23 +++++++++++++++ 4 files changed, 69 insertions(+), 18 deletions(-) diff --git a/dimos/robot/bosdyn/spot/config.py b/dimos/robot/bosdyn/spot/config.py index e9a708e97c..44cc3855a0 100644 --- a/dimos/robot/bosdyn/spot/config.py +++ b/dimos/robot/bosdyn/spot/config.py @@ -48,6 +48,16 @@ # lists in SpotHighLevel. Names the grayscale_image_* / depth_image_* streams by position. CAMERA_STREAM_SUFFIXES = ["front_left", "front_right", "left", "right", "back"] +# Spot mounts the two front body cameras rotated ~90° clockwise, so their raw +# fisheye/depth frames arrive sideways. Rotate them back one quarter turn (CW, +# hence -1 for np.rot90) — together with their intrinsics — before publishing so +# each image lines up with its optical frame. Side/back cameras arrive upright. +FRONT_CAMERA_ROTATE_UPRIGHT = -1 + +# Spot's right body camera arrives upside down; a half turn (180°) rights it. Its +# intrinsics are unchanged in width/height, only the principal point flips. +RIGHT_CAMERA_ROTATE_UPRIGHT = 2 + # Spot's body fisheye + depth cameras top out around 15 Hz. Poll at that rate so # we never miss a frame; the acquisition_time dedup in SpotHighLevel drops any # repeat returned by polling faster than a given camera actually refreshes. diff --git a/dimos/robot/bosdyn/spot/effectors/high_level.py b/dimos/robot/bosdyn/spot/effectors/high_level.py index b6a681ba7f..c28ac3b056 100644 --- a/dimos/robot/bosdyn/spot/effectors/high_level.py +++ b/dimos/robot/bosdyn/spot/effectors/high_level.py @@ -57,10 +57,12 @@ from dimos.protocol.tf.static_tf_publisher import StaticTfPublisher, StaticTfPublisherConfig from dimos.robot.bosdyn.spot.config import ( CAMERA_MAX_HZ, + FRONT_CAMERA_ROTATE_UPRIGHT, IP_LABELS, POWER_OFF_TIMEOUT_S, POWER_ON_TIMEOUT_S, REACHABILITY_PROBE_TIMEOUT_S, + RIGHT_CAMERA_ROTATE_UPRIGHT, SIT_TIMEOUT_S, SPOT_API_PORT, SPOT_URDF_PATH, @@ -70,6 +72,8 @@ camera_info_from_response, camera_mount_transforms, decode_image, + rotate_camera_info_quarter_turns, + rotate_image_quarter_turns, ) from dimos.utils.logging_config import setup_logger @@ -288,60 +292,67 @@ async def handle_cmd_vel(self, msg: Twist) -> None: async def _poll_images(self) -> None: period = 1.0 / self.config.image_rate_hz config = self.config - # bosdyn source name -> (image Out, CameraInfo Out, frame_id). Images are - # published in Spot's native sensor orientation (no rotation); each image's - # frame_id names a URDF optical frame anchored by the static camera tf. - # Grayscale and depth at the same mount share that mount's optical frame. - routing: dict[str, tuple[Out[Image], Out[CameraInfo], str]] = { + # bosdyn source name -> (image Out, CameraInfo Out, frame_id, quarter_turns). + routing: dict[str, tuple[Out[Image], Out[CameraInfo], str, int]] = { "frontleft_fisheye_image": ( self.grayscale_image_front_left, self.grayscale_info, config.frontleft_camera_frame_id, + FRONT_CAMERA_ROTATE_UPRIGHT, ), "frontright_fisheye_image": ( self.grayscale_image_front_right, self.grayscale_info, config.frontright_camera_frame_id, + FRONT_CAMERA_ROTATE_UPRIGHT, ), "left_fisheye_image": ( self.grayscale_image_left, self.grayscale_info, config.left_camera_frame_id, + 0, ), "right_fisheye_image": ( self.grayscale_image_right, self.grayscale_info, config.right_camera_frame_id, + RIGHT_CAMERA_ROTATE_UPRIGHT, ), "back_fisheye_image": ( self.grayscale_image_back, self.grayscale_info, config.back_camera_frame_id, + 0, ), "frontleft_depth": ( self.depth_image_front_left, self.depth_info, config.frontleft_camera_frame_id, + FRONT_CAMERA_ROTATE_UPRIGHT, ), "frontright_depth": ( self.depth_image_front_right, self.depth_info, config.frontright_camera_frame_id, + FRONT_CAMERA_ROTATE_UPRIGHT, ), "left_depth": ( self.depth_image_left, self.depth_info, config.left_camera_frame_id, + 0, ), "right_depth": ( self.depth_image_right, self.depth_info, config.right_camera_frame_id, + RIGHT_CAMERA_ROTATE_UPRIGHT, ), "back_depth": ( self.depth_image_back, self.depth_info, config.back_camera_frame_id, + 0, ), } sources = list(routing) @@ -367,15 +378,19 @@ async def _poll_images(self) -> None: route = routing.get(source_name) if route is None: continue - out, info_out, frame_id = route + out, info_out, frame_id, quarter_turns = route image = decode_image(response, frame_id, time_converter) if image is None: continue if last_published_ts.get(source_name) == image.ts: continue last_published_ts[source_name] = image.ts - out.publish(image) camera_info = camera_info_from_response(response, frame_id, image.ts) + if quarter_turns: + image = rotate_image_quarter_turns(image, quarter_turns) + if camera_info is not None: + camera_info = rotate_camera_info_quarter_turns(camera_info, quarter_turns) + out.publish(image) if camera_info is not None: info_out.publish(camera_info) diff --git a/dimos/robot/bosdyn/spot/rerun.py b/dimos/robot/bosdyn/spot/rerun.py index f868ca2a20..138b72a9df 100644 --- a/dimos/robot/bosdyn/spot/rerun.py +++ b/dimos/robot/bosdyn/spot/rerun.py @@ -45,21 +45,21 @@ def spot_camera_layout() -> rrb.Blueprint: line_grid=rrb.LineGrid3D(plane=rr.components.Plane3D.XY.with_distance(0.0)), ) - front_right_gray = _camera_view(_grayscale_origin("front_right"), "front right") - front_left_gray = _camera_view(_grayscale_origin("front_left"), "front left") - front_right_depth = _camera_view(_depth_origin("front_right"), "front right (depth)") - front_left_depth = _camera_view(_depth_origin("front_left"), "front left (depth)") - + # Each rrb view may live in exactly one place in the blueprint tree, so every + # container below builds its own fresh view instances rather than sharing one. front_tab = rrb.Horizontal( - front_right_gray, - front_left_gray, - front_right_depth, - front_left_depth, + _camera_view(_grayscale_origin("front_right"), "front right"), + _camera_view(_grayscale_origin("front_left"), "front left"), + _camera_view(_depth_origin("front_right"), "front right (depth)"), + _camera_view(_depth_origin("front_left"), "front left (depth)"), name="front", ) grayscale_tab = rrb.Vertical( - rrb.Horizontal(front_right_gray, front_left_gray), + rrb.Horizontal( + _camera_view(_grayscale_origin("front_right"), "front right"), + _camera_view(_grayscale_origin("front_left"), "front left"), + ), rrb.Horizontal( _camera_view(_grayscale_origin("left"), "left"), _camera_view(_grayscale_origin("right"), "right"), @@ -69,7 +69,10 @@ def spot_camera_layout() -> rrb.Blueprint: ) depth_tab = rrb.Vertical( - rrb.Horizontal(front_right_depth, front_left_depth), + rrb.Horizontal( + _camera_view(_depth_origin("front_right"), "front right (depth)"), + _camera_view(_depth_origin("front_left"), "front left (depth)"), + ), rrb.Horizontal( _camera_view(_depth_origin("left"), "left"), _camera_view(_depth_origin("right"), "right"), diff --git a/dimos/robot/bosdyn/spot/utils.py b/dimos/robot/bosdyn/spot/utils.py index 972da4a895..5b437f0c01 100644 --- a/dimos/robot/bosdyn/spot/utils.py +++ b/dimos/robot/bosdyn/spot/utils.py @@ -74,6 +74,29 @@ def camera_mount_transforms( return transforms +def rotate_image_quarter_turns(image: Image, quarter_turns: int) -> Image: + """Rotate an Image by `quarter_turns` * 90° CCW (negative for CW).""" + rotated = np.rot90(image.data, k=quarter_turns) + return Image.from_numpy(rotated, format=image.format, frame_id=image.frame_id, ts=image.ts) + + +def rotate_camera_info_quarter_turns(info: CameraInfo, quarter_turns: int) -> CameraInfo: + """Rotate a pinhole CameraInfo to match `rotate_image_quarter_turns`. + + Each CCW quarter turn swaps the focal lengths and remaps the principal point so + the intrinsics stay consistent with the rotated pixel grid (width/height swap). + """ + fx, fy, cx, cy = info.K[0], info.K[4], info.K[2], info.K[5] + width, height = info.width, info.height + for _ in range(quarter_turns % 4): + fx, fy = fy, fx + cx, cy = cy, (width - 1) - cx + width, height = height, width + return CameraInfo.from_intrinsics( + fx=fx, fy=fy, cx=cx, cy=cy, width=width, height=height, frame_id=info.frame_id + ).with_ts(info.ts) + + def camera_info_from_response(response: Any, source_name: str, ts: float) -> CameraInfo | None: """Build a CameraInfo from a bosdyn image response's pinhole intrinsics.""" source = response.source From 77d29a4ff16acf41b8cd29b340062ab1e3d91966 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sat, 18 Jul 2026 14:21:48 -0500 Subject: [PATCH 08/26] fix too large angular vel --- dimos/robot/bosdyn/spot/config.py | 4 ++++ dimos/robot/bosdyn/spot/effectors/high_level.py | 7 ++++++- dimos/robot/bosdyn/spot/utils.py | 4 ++++ 3 files changed, 14 insertions(+), 1 deletion(-) diff --git a/dimos/robot/bosdyn/spot/config.py b/dimos/robot/bosdyn/spot/config.py index 44cc3855a0..a08998d6aa 100644 --- a/dimos/robot/bosdyn/spot/config.py +++ b/dimos/robot/bosdyn/spot/config.py @@ -33,6 +33,10 @@ SPOT_API_PORT = 443 REACHABILITY_PROBE_TIMEOUT_S = 2.0 +# not configutable, these are limits set by the spot API +MAX_LINEAR_VELOCITY = 1.6 +MAX_ANGULAR_VELOCITY = 1.6 + # Motor power / posture command timeouts (seconds). POWER_ON_TIMEOUT_S = 20.0 POWER_OFF_TIMEOUT_S = 20.0 diff --git a/dimos/robot/bosdyn/spot/effectors/high_level.py b/dimos/robot/bosdyn/spot/effectors/high_level.py index c28ac3b056..b6e5aa1508 100644 --- a/dimos/robot/bosdyn/spot/effectors/high_level.py +++ b/dimos/robot/bosdyn/spot/effectors/high_level.py @@ -59,6 +59,8 @@ CAMERA_MAX_HZ, FRONT_CAMERA_ROTATE_UPRIGHT, IP_LABELS, + MAX_ANGULAR_VELOCITY, + MAX_LINEAR_VELOCITY, POWER_OFF_TIMEOUT_S, POWER_ON_TIMEOUT_S, REACHABILITY_PROBE_TIMEOUT_S, @@ -71,6 +73,7 @@ from dimos.robot.bosdyn.spot.utils import ( camera_info_from_response, camera_mount_transforms, + clamp, decode_image, rotate_camera_info_quarter_turns, rotate_image_quarter_turns, @@ -473,7 +476,9 @@ async def move(self, twist: Twist, duration: float = 0.0) -> bool: ) command = RobotCommandBuilder.synchro_velocity_command( - v_x=twist.linear.x, v_y=twist.linear.y, v_rot=twist.angular.z + v_x=clamp(twist.linear.x, -MAX_LINEAR_VELOCITY, MAX_LINEAR_VELOCITY), + v_y=clamp(twist.linear.y, -MAX_LINEAR_VELOCITY, MAX_LINEAR_VELOCITY), + v_rot=clamp(twist.angular.z, -MAX_ANGULAR_VELOCITY, MAX_ANGULAR_VELOCITY), ) window = duration if duration > 0 else self.config.cmd_vel_timeout try: diff --git a/dimos/robot/bosdyn/spot/utils.py b/dimos/robot/bosdyn/spot/utils.py index 5b437f0c01..45eb0010f2 100644 --- a/dimos/robot/bosdyn/spot/utils.py +++ b/dimos/robot/bosdyn/spot/utils.py @@ -32,6 +32,10 @@ logger = setup_logger() +def clamp(value: float, low: float, high: float) -> float: + return max(low, min(high, value)) + + def joint_to_transform(joint: JointDescription) -> Transform: return Transform( translation=Vector3(*joint.origin_xyz), From 09efc07b4b65e8f9d277be3ca561a25a2c74f141 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 20 Jul 2026 00:56:17 -0700 Subject: [PATCH 09/26] docs(spot): move Spot README into Mintlify docs tree Relocate the Spot guide to docs/platforms/quadruped/spot/index.md matching the go2/g1 structure, add it to the docs nav, and add redirects. --- docs/docs.json | 14 +++++++++++++ .../platforms/quadruped/spot/index.md | 21 +++++++++++++++++-- 2 files changed, 33 insertions(+), 2 deletions(-) rename dimos/robot/bosdyn/spot/README.md => docs/platforms/quadruped/spot/index.md (64%) diff --git a/docs/docs.json b/docs/docs.json index 79cff4b742..5d129b34fc 100644 --- a/docs/docs.json +++ b/docs/docs.json @@ -146,6 +146,12 @@ "platforms/quadruped/go2/setup", "platforms/quadruped/go2/simulation" ] + }, + { + "group": "Boston Dynamics Spot", + "pages": [ + "platforms/quadruped/spot/index" + ] } ] }, @@ -479,6 +485,14 @@ "source": "/docs/platforms/quadruped/go2/simulation.md", "destination": "/platforms/quadruped/go2/simulation" }, + { + "source": "/docs/platforms/quadruped/spot/index.md", + "destination": "/platforms/quadruped/spot/index" + }, + { + "source": "/docs/platforms/quadruped/spot", + "destination": "/platforms/quadruped/spot/index" + }, { "source": "/docs/platforms/humanoid/g1/index.md", "destination": "/platforms/humanoid/g1/index" diff --git a/dimos/robot/bosdyn/spot/README.md b/docs/platforms/quadruped/spot/index.md similarity index 64% rename from dimos/robot/bosdyn/spot/README.md rename to docs/platforms/quadruped/spot/index.md index 5363d603fe..1cae02346a 100644 --- a/dimos/robot/bosdyn/spot/README.md +++ b/docs/platforms/quadruped/spot/index.md @@ -1,6 +1,9 @@ -# Spot +--- +title: "Boston Dynamics Spot" +--- -Boston Dynamics Spot control for DimOS: velocity teleop + camera/odometry streaming. +Boston Dynamics Spot control for DimOS: velocity teleop plus fisheye/depth camera and +odometry streaming. ## Install @@ -32,6 +35,13 @@ The IP auto-detects (WiFi then Ethernet). Force one with `-o spothighlevel.ip= Date: Mon, 20 Jul 2026 01:38:36 -0700 Subject: [PATCH 10/26] comment cleanup --- dimos/robot/bosdyn/spot/config.py | 8 -------- dimos/robot/bosdyn/spot/effectors/high_level.py | 2 +- dimos/robot/bosdyn/spot/recorder.py | 3 --- docs/platforms/quadruped/spot/index.md | 5 ++++- 4 files changed, 5 insertions(+), 13 deletions(-) diff --git a/dimos/robot/bosdyn/spot/config.py b/dimos/robot/bosdyn/spot/config.py index a08998d6aa..a8617b3b4f 100644 --- a/dimos/robot/bosdyn/spot/config.py +++ b/dimos/robot/bosdyn/spot/config.py @@ -43,13 +43,8 @@ STAND_TIMEOUT_S = 10.0 SIT_TIMEOUT_S = 10.0 -# Static camera-mount geometry lives in this URDF (base_link -> body -> {pos}_camera -# -> {pos}_camera_optical, all fixed joints). SpotHighLevel publishes those mounts -# as static tf; the moving odom->base_link edge stays live from Spot's state. SPOT_URDF_PATH = Path(__file__).parent / "spot.urdf" -# Stream-name suffix per camera mount, ordered to match the fisheye/depth source -# lists in SpotHighLevel. Names the grayscale_image_* / depth_image_* streams by position. CAMERA_STREAM_SUFFIXES = ["front_left", "front_right", "left", "right", "back"] # Spot mounts the two front body cameras rotated ~90° clockwise, so their raw @@ -62,7 +57,4 @@ # intrinsics are unchanged in width/height, only the principal point flips. RIGHT_CAMERA_ROTATE_UPRIGHT = 2 -# Spot's body fisheye + depth cameras top out around 15 Hz. Poll at that rate so -# we never miss a frame; the acquisition_time dedup in SpotHighLevel drops any -# repeat returned by polling faster than a given camera actually refreshes. CAMERA_MAX_HZ = 15.0 diff --git a/dimos/robot/bosdyn/spot/effectors/high_level.py b/dimos/robot/bosdyn/spot/effectors/high_level.py index b6e5aa1508..9cd5e80ca5 100644 --- a/dimos/robot/bosdyn/spot/effectors/high_level.py +++ b/dimos/robot/bosdyn/spot/effectors/high_level.py @@ -119,7 +119,6 @@ class SpotHighLevelConfig(StaticTfPublisherConfig): right_camera_frame_id: str = "right_camera_optical" back_camera_frame_id: str = "back_camera_optical" - # Poll at the camera's max rate; SpotHighLevel dedups repeats by acquisition_time. image_rate_hz: float = CAMERA_MAX_HZ odom_rate_hz: float = 60.0 @@ -292,6 +291,7 @@ async def handle_cmd_vel(self, msg: Twist) -> None: return await self.move(msg) + # the spot API can only poll - no callbacks async def _poll_images(self) -> None: period = 1.0 / self.config.image_rate_hz config = self.config diff --git a/dimos/robot/bosdyn/spot/recorder.py b/dimos/robot/bosdyn/spot/recorder.py index 39af57186a..40d7dd65ff 100644 --- a/dimos/robot/bosdyn/spot/recorder.py +++ b/dimos/robot/bosdyn/spot/recorder.py @@ -45,9 +45,6 @@ class SpotRecorderConfig(RecorderConfig): default_factory=lambda: f"spot_recording_{datetime.now():%Y-%m-%d_%H-%M-%S}.db" ) on_existing: OnExisting = OnExisting.APPEND - # The odom frame is the fixed root SpotHighLevel anchors against. It publishes - # odom->base_link and base_link->camera edges, so image/odom poses resolve - # against this root. Must match SpotHighLevelConfig.odom_frame_id. root_frame: str = "odom" stream_codecs: dict[str, str] = Field( default_factory=lambda: { diff --git a/docs/platforms/quadruped/spot/index.md b/docs/platforms/quadruped/spot/index.md index 1cae02346a..bb1a12f327 100644 --- a/docs/platforms/quadruped/spot/index.md +++ b/docs/platforms/quadruped/spot/index.md @@ -30,6 +30,9 @@ dimos run spot \ -o spothighlevel.password= ``` +The username and password are printed on the sticker inside Spot's battery bay (visible +when the battery is removed). + The IP auto-detects (WiFi then Ethernet). Force one with `-o spothighlevel.ip=`. Keyboard teleop: WASD move/turn, QE strafe, Space soft-stop, ESC quit. A Rerun @@ -51,7 +54,7 @@ viewer opens with the fisheye/depth cameras and odometry. - `recorder.py` — `SpotRecorder`: records every Spot stream to disk. - `blueprints/spot.py` — the runnable `spot` blueprint (click/teleop + sensors + Rerun). -## Deep Dive +## Dimos Tools - [Visualization](/docs/usage/visualization.md) — Rerun, performance tuning - [Data Streams](/docs/usage/data_streams/index.md) — RxPY streams, backpressure, quality filtering From 9f03c1c7c65596c561205d04db1d89baaf0fe5b9 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 20 Jul 2026 01:42:22 -0700 Subject: [PATCH 11/26] refactor(spot): reorder utils helpers, drop stale docstring line Move decode_image to the top and clamp next to raw_layout; remove the outdated sentence about base_frame_id renaming in camera_mount_transforms. --- dimos/robot/bosdyn/spot/utils.py | 91 ++++++++++++++++---------------- 1 file changed, 45 insertions(+), 46 deletions(-) diff --git a/dimos/robot/bosdyn/spot/utils.py b/dimos/robot/bosdyn/spot/utils.py index 45eb0010f2..bc7718dfda 100644 --- a/dimos/robot/bosdyn/spot/utils.py +++ b/dimos/robot/bosdyn/spot/utils.py @@ -32,8 +32,48 @@ logger = setup_logger() -def clamp(value: float, low: float, high: float) -> float: - return max(low, min(high, value)) +def decode_image(response: Any, frame_id: str, time_converter: Any) -> Image | None: + """Turn a bosdyn ImageResponse into a dimos Image, or None if unsupported. + + Stamps each image with its true capture time: the robot-clock `acquisition_time` + converted to local time by `time_converter`, bosdyn's live clock-skew estimate + (`RobotTimeConverter`). Polling faster than the sensor returns the same frame, so + keeping the sensor timestamp lets downstream drop the repeat instead of seeing a + fresh wall-clock stamp. + """ + from bosdyn.api import image_pb2 # type: ignore[import-not-found] + + shot = response.shot.image + pixel_format = shot.pixel_format + ts = time_converter.local_seconds_from_robot_timestamp(response.shot.acquisition_time) + + if shot.format == image_pb2.Image.FORMAT_JPEG: + import cv2 + + buffer = np.frombuffer(shot.data, dtype=np.uint8) + decoded = cv2.imdecode(buffer, cv2.IMREAD_UNCHANGED) + if decoded is None: + logger.error(f"Failed to decode JPEG image from {frame_id}") + return None + image_format = ImageFormat.GRAY if decoded.ndim == 2 else ImageFormat.BGR + return Image.from_numpy(decoded, format=image_format, frame_id=frame_id, ts=ts) + + if shot.format != image_pb2.Image.FORMAT_RAW: + logger.error(f"Unsupported Spot image encoding {shot.format} from {frame_id}") + return None + + dtype, channels, image_format = raw_layout(pixel_format) + if dtype is None: + logger.error(f"Unsupported Spot pixel format {pixel_format} from {frame_id}") + return None + + array = np.frombuffer(shot.data, dtype=dtype) + array = ( + array.reshape(shot.rows, shot.cols) + if channels == 1 + else array.reshape(shot.rows, shot.cols, channels) + ) + return Image.from_numpy(array, format=image_format, frame_id=frame_id, ts=ts) def joint_to_transform(joint: JointDescription) -> Transform: @@ -52,8 +92,7 @@ def camera_mount_transforms( Walks the fixed-joint chain (base_link -> body -> {pos}_camera -> optical) up from each optical frame and folds the per-joint origins into one transform, so - the recorded images resolve a pose against the live odom->base_link edge. The - URDF's root link is renamed to `base_frame_id` so callers can rebase the tree. + the recorded images resolve a pose against the live odom->base_link edge. """ model = parse_model(urdf_path) urdf_root = model.root_link @@ -119,48 +158,8 @@ def camera_info_from_response(response: Any, source_name: str, ts: float) -> Cam return info.with_ts(ts) -def decode_image(response: Any, frame_id: str, time_converter: Any) -> Image | None: - """Turn a bosdyn ImageResponse into a dimos Image, or None if unsupported. - - Stamps each image with its true capture time: the robot-clock `acquisition_time` - converted to local time by `time_converter`, bosdyn's live clock-skew estimate - (`RobotTimeConverter`). Polling faster than the sensor returns the same frame, so - keeping the sensor timestamp lets downstream drop the repeat instead of seeing a - fresh wall-clock stamp. - """ - from bosdyn.api import image_pb2 # type: ignore[import-not-found] - - shot = response.shot.image - pixel_format = shot.pixel_format - ts = time_converter.local_seconds_from_robot_timestamp(response.shot.acquisition_time) - - if shot.format == image_pb2.Image.FORMAT_JPEG: - import cv2 - - buffer = np.frombuffer(shot.data, dtype=np.uint8) - decoded = cv2.imdecode(buffer, cv2.IMREAD_UNCHANGED) - if decoded is None: - logger.error(f"Failed to decode JPEG image from {frame_id}") - return None - image_format = ImageFormat.GRAY if decoded.ndim == 2 else ImageFormat.BGR - return Image.from_numpy(decoded, format=image_format, frame_id=frame_id, ts=ts) - - if shot.format != image_pb2.Image.FORMAT_RAW: - logger.error(f"Unsupported Spot image encoding {shot.format} from {frame_id}") - return None - - dtype, channels, image_format = raw_layout(pixel_format) - if dtype is None: - logger.error(f"Unsupported Spot pixel format {pixel_format} from {frame_id}") - return None - - array = np.frombuffer(shot.data, dtype=dtype) - array = ( - array.reshape(shot.rows, shot.cols) - if channels == 1 - else array.reshape(shot.rows, shot.cols, channels) - ) - return Image.from_numpy(array, format=image_format, frame_id=frame_id, ts=ts) +def clamp(value: float, low: float, high: float) -> float: + return max(low, min(high, value)) def raw_layout(pixel_format: int) -> tuple[Any, int, ImageFormat]: From a6f6c60e9a032e4ef6cc58c5e7e1c5ced41bc095 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 20 Jul 2026 02:15:21 -0700 Subject: [PATCH 12/26] misc --- dimos/robot/all_blueprints.py | 1 + dimos/robot/bosdyn/spot/config.py | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 5fc37a0527..ee835f69ec 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -85,6 +85,7 @@ "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", "spot": "dimos.robot.bosdyn.spot.blueprints.spot:spot", "spot-record": "dimos.robot.bosdyn.spot.blueprints.spot_record:spot_record", + "spot-replay": "dimos.robot.bosdyn.spot.blueprints.spot_replay:spot_replay", "teleop-hosted-go2": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_go2", "teleop-hosted-go2-multicam": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_go2_multicam", "teleop-hosted-go2-transport": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_go2_transport", diff --git a/dimos/robot/bosdyn/spot/config.py b/dimos/robot/bosdyn/spot/config.py index a8617b3b4f..66776ab9f6 100644 --- a/dimos/robot/bosdyn/spot/config.py +++ b/dimos/robot/bosdyn/spot/config.py @@ -34,8 +34,8 @@ REACHABILITY_PROBE_TIMEOUT_S = 2.0 # not configutable, these are limits set by the spot API -MAX_LINEAR_VELOCITY = 1.6 -MAX_ANGULAR_VELOCITY = 1.6 +MAX_LINEAR_VELOCITY = 1.599 +MAX_ANGULAR_VELOCITY = 1.599 # Motor power / posture command timeouts (seconds). POWER_ON_TIMEOUT_S = 20.0 From e54e6e41e75c392b734deff4e21e9cfbac41abec Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 20 Jul 2026 02:49:39 -0700 Subject: [PATCH 13/26] chore(spot): drop stray spot-replay registration from no-replay PR The spot-replay blueprint lives on its own branch; its module file is not part of this branch, so registering it here would break blueprint generation. --- dimos/robot/all_blueprints.py | 1 - 1 file changed, 1 deletion(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index ee835f69ec..5fc37a0527 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -85,7 +85,6 @@ "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", "spot": "dimos.robot.bosdyn.spot.blueprints.spot:spot", "spot-record": "dimos.robot.bosdyn.spot.blueprints.spot_record:spot_record", - "spot-replay": "dimos.robot.bosdyn.spot.blueprints.spot_replay:spot_replay", "teleop-hosted-go2": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_go2", "teleop-hosted-go2-multicam": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_go2_multicam", "teleop-hosted-go2-transport": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_go2_transport", From 6209f4068dc9001568ad3257f8a07d28180bd941 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 20 Jul 2026 12:19:42 -0700 Subject: [PATCH 14/26] fix(spot): roll front camera optical tf to match upright images Front images and their intrinsics are rotated upright before publishing, but the optical tf frame stayed at its raw sideways mount, so recorded depth back-projection and frustums came out rotated. Bake the same roll into the published static tf (frontleft -1, frontright +1 for the mirror) so tf, intrinsics, and pixels stay mutually consistent. --- dimos/robot/bosdyn/spot/config.py | 4 ++++ .../robot/bosdyn/spot/effectors/high_level.py | 18 +++++++++++++++-- dimos/robot/bosdyn/spot/utils.py | 20 +++++++++++++++++++ 3 files changed, 40 insertions(+), 2 deletions(-) diff --git a/dimos/robot/bosdyn/spot/config.py b/dimos/robot/bosdyn/spot/config.py index 66776ab9f6..07e10c23a9 100644 --- a/dimos/robot/bosdyn/spot/config.py +++ b/dimos/robot/bosdyn/spot/config.py @@ -53,6 +53,10 @@ # each image lines up with its optical frame. Side/back cameras arrive upright. FRONT_CAMERA_ROTATE_UPRIGHT = -1 +# The two front cameras mount mirror-imaged, so frontright's optical frame sits a +# half turn (2 quarter turns) past frontleft's when rolling frames upright in 3D. +FRONT_CAMERA_MIRROR_HALF_TURN = 2 + # Spot's right body camera arrives upside down; a half turn (180°) rights it. Its # intrinsics are unchanged in width/height, only the principal point flips. RIGHT_CAMERA_ROTATE_UPRIGHT = 2 diff --git a/dimos/robot/bosdyn/spot/effectors/high_level.py b/dimos/robot/bosdyn/spot/effectors/high_level.py index 9cd5e80ca5..4c5b0d7d89 100644 --- a/dimos/robot/bosdyn/spot/effectors/high_level.py +++ b/dimos/robot/bosdyn/spot/effectors/high_level.py @@ -57,6 +57,7 @@ from dimos.protocol.tf.static_tf_publisher import StaticTfPublisher, StaticTfPublisherConfig from dimos.robot.bosdyn.spot.config import ( CAMERA_MAX_HZ, + FRONT_CAMERA_MIRROR_HALF_TURN, FRONT_CAMERA_ROTATE_UPRIGHT, IP_LABELS, MAX_ANGULAR_VELOCITY, @@ -75,6 +76,7 @@ camera_mount_transforms, clamp, decode_image, + roll_optical_frame, rotate_camera_info_quarter_turns, rotate_image_quarter_turns, ) @@ -169,9 +171,11 @@ def transforms(self) -> list[Transform]: """Static base_link -> camera-optical extrinsics parsed from the URDF. `StaticTfPublisher` republishes these on a fixed interval; the moving - odom->base_link edge stays live (see `_publish_odom`). + odom->base_link edge stays live (see `_publish_odom`). The front optical + frames are rolled to match the upright-rotated front images so recorded + tf, intrinsics, and pixels stay mutually consistent for depth reprojection. """ - return camera_mount_transforms( + mounts = camera_mount_transforms( SPOT_URDF_PATH, self.config.base_frame_id, [ @@ -182,6 +186,16 @@ def transforms(self) -> list[Transform]: self.config.back_camera_frame_id, ], ) + front_frame_rolls = { + self.config.frontleft_camera_frame_id: FRONT_CAMERA_ROTATE_UPRIGHT, + self.config.frontright_camera_frame_id: ( + FRONT_CAMERA_ROTATE_UPRIGHT + FRONT_CAMERA_MIRROR_HALF_TURN + ), + } + return [ + roll_optical_frame(mount, front_frame_rolls.get(mount.child_frame_id, 0)) + for mount in mounts + ] async def main(self) -> AsyncIterator[None]: username, password = self.config.username, self.config.password diff --git a/dimos/robot/bosdyn/spot/utils.py b/dimos/robot/bosdyn/spot/utils.py index bc7718dfda..9f9a27aa43 100644 --- a/dimos/robot/bosdyn/spot/utils.py +++ b/dimos/robot/bosdyn/spot/utils.py @@ -16,6 +16,7 @@ from __future__ import annotations +import math from pathlib import Path from typing import Any @@ -117,6 +118,25 @@ def camera_mount_transforms( return transforms +def roll_optical_frame(transform: Transform, quarter_turns: int) -> Transform: + """Roll a camera's optical frame `quarter_turns` * 90° about its viewing (z) axis. + + Pairs with `rotate_image_quarter_turns`: rotating the image alone leaves the 3D + frame at its raw mount orientation, so frustums and depth back-projection land + rotated. Rolling the frame by the same amount realigns 3D with the upright image. + """ + if not quarter_turns: + return transform + roll = Quaternion.from_euler(Vector3(0.0, 0.0, quarter_turns * math.pi / 2)) + return Transform( + translation=transform.translation, + rotation=transform.rotation * roll, + frame_id=transform.frame_id, + child_frame_id=transform.child_frame_id, + ts=transform.ts, + ) + + def rotate_image_quarter_turns(image: Image, quarter_turns: int) -> Image: """Rotate an Image by `quarter_turns` * 90° CCW (negative for CW).""" rotated = np.rot90(image.data, k=quarter_turns) From fcb38158cdfde30d1d8c74c61981ac8346b0ba2e Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 20 Jul 2026 12:51:39 -0700 Subject: [PATCH 15/26] fix(spot): tear down on setup failure and fail loud on missing odom TF If setup raised after acquiring the lease/E-stop keepalives or powering on (e.g. blocking_stand fails), main() exited before its yield so teardown never ran, leaving Spot powered with no manager. Wrap setup so any failure runs _teardown() before re-raising. Also, when the robot state snapshot lacks the vision->body transform, _poll_odom now logs a clear error and raises instead of dereferencing None or silently skipping. cmd_vel runs on a separate handler so driving keeps working; only odom and the odom->base_link tf stop, loudly. --- .../robot/bosdyn/spot/effectors/high_level.py | 130 ++++++++++-------- 1 file changed, 75 insertions(+), 55 deletions(-) diff --git a/dimos/robot/bosdyn/spot/effectors/high_level.py b/dimos/robot/bosdyn/spot/effectors/high_level.py index 4c5b0d7d89..552a8bb3fc 100644 --- a/dimos/robot/bosdyn/spot/effectors/high_level.py +++ b/dimos/robot/bosdyn/spot/effectors/high_level.py @@ -205,72 +205,83 @@ async def main(self) -> AsyncIterator[None]: "(-o .username=... -o .password=...)" ) - ip = await self.resolve_ip() - - from bosdyn.client import create_standard_sdk # type: ignore[import-not-found] - from bosdyn.client.estop import ( # type: ignore[import-not-found] - EstopClient, - EstopEndpoint, - EstopKeepAlive, - ) - from bosdyn.client.image import ImageClient # type: ignore[import-not-found] - from bosdyn.client.lease import ( # type: ignore[import-not-found] - LeaseClient, - LeaseKeepAlive, - ) - from bosdyn.client.robot_command import ( # type: ignore[import-not-found] - RobotCommandClient, - blocking_stand, - ) - from bosdyn.client.robot_state import ( # type: ignore[import-not-found] - RobotStateClient, - ) + # Any setup failure past this point must undo the lease/E-stop keepalives and + # motor power it already acquired, else Spot is left powered with no manager. + try: + ip = await self.resolve_ip() - logger.info(f"Connecting to Spot at {ip}") - sdk = await asyncio.to_thread(create_standard_sdk, "dimos-spot") - self._robot = await asyncio.to_thread(sdk.create_robot, ip) - await asyncio.to_thread(self._robot.authenticate, username, password) - await self.sync_clocks() - - if self.config.enable_estop: - estop_client = self._robot.ensure_client(EstopClient.default_service_name) - endpoint = EstopEndpoint( - client=estop_client, - name="dimos-spot", - estop_timeout=self.config.estop_timeout, + from bosdyn.client import create_standard_sdk # type: ignore[import-not-found] + from bosdyn.client.estop import ( # type: ignore[import-not-found] + EstopClient, + EstopEndpoint, + EstopKeepAlive, + ) + from bosdyn.client.image import ImageClient # type: ignore[import-not-found] + from bosdyn.client.lease import ( # type: ignore[import-not-found] + LeaseClient, + LeaseKeepAlive, + ) + from bosdyn.client.robot_command import ( # type: ignore[import-not-found] + RobotCommandClient, + blocking_stand, + ) + from bosdyn.client.robot_state import ( # type: ignore[import-not-found] + RobotStateClient, ) - await asyncio.to_thread(endpoint.force_simple_setup) - self._estop_keepalive = EstopKeepAlive(endpoint) - - if self.config.acquire_lease: - lease_client = self._robot.ensure_client(LeaseClient.default_service_name) - await asyncio.to_thread(lease_client.take) - self._lease_keepalive = LeaseKeepAlive(lease_client) - self._command_client = self._robot.ensure_client(RobotCommandClient.default_service_name) - self._state_client = self._robot.ensure_client(RobotStateClient.default_service_name) - self._image_client = self._robot.ensure_client(ImageClient.default_service_name) + logger.info(f"Connecting to Spot at {ip}") + sdk = await asyncio.to_thread(create_standard_sdk, "dimos-spot") + self._robot = await asyncio.to_thread(sdk.create_robot, ip) + await asyncio.to_thread(self._robot.authenticate, username, password) + await self.sync_clocks() + + if self.config.enable_estop: + estop_client = self._robot.ensure_client(EstopClient.default_service_name) + endpoint = EstopEndpoint( + client=estop_client, + name="dimos-spot", + estop_timeout=self.config.estop_timeout, + ) + await asyncio.to_thread(endpoint.force_simple_setup) + self._estop_keepalive = EstopKeepAlive(endpoint) - if self.config.power_on_at_start: - logger.info("Powering on Spot motors") - await asyncio.to_thread(self._robot.power_on, timeout_sec=POWER_ON_TIMEOUT_S) + if self.config.acquire_lease: + lease_client = self._robot.ensure_client(LeaseClient.default_service_name) + await asyncio.to_thread(lease_client.take) + self._lease_keepalive = LeaseKeepAlive(lease_client) - if self.config.stand_at_start: - logger.info("Standing Spot") - await asyncio.to_thread( - blocking_stand, self._command_client, timeout_sec=STAND_TIMEOUT_S + self._command_client = self._robot.ensure_client( + RobotCommandClient.default_service_name ) - self._standing = True + self._state_client = self._robot.ensure_client(RobotStateClient.default_service_name) + self._image_client = self._robot.ensure_client(ImageClient.default_service_name) - self.tf.start() - self._image_task = asyncio.create_task(self._poll_images()) - self._odom_task = asyncio.create_task(self._poll_odom()) + if self.config.power_on_at_start: + logger.info("Powering on Spot motors") + await asyncio.to_thread(self._robot.power_on, timeout_sec=POWER_ON_TIMEOUT_S) - self._ready.set() - logger.info("Spot control + sensors ready") + if self.config.stand_at_start: + logger.info("Standing Spot") + await asyncio.to_thread( + blocking_stand, self._command_client, timeout_sec=STAND_TIMEOUT_S + ) + self._standing = True + + self.tf.start() + self._image_task = asyncio.create_task(self._poll_images()) + self._odom_task = asyncio.create_task(self._poll_odom()) + + self._ready.set() + logger.info("Spot control + sensors ready") + except BaseException: + await self._teardown() + raise yield + await self._teardown() + + async def _teardown(self) -> None: self._ready.clear() for task in (self._image_task, self._odom_task): if task is not None: @@ -434,6 +445,15 @@ async def _poll_odom(self) -> None: vision_tform_body = get_a_tform_b( kinematic_state.transforms_snapshot, VISION_FRAME_NAME, BODY_FRAME_NAME ) + # Fail this task loudly rather than publish nothing silently. cmd_vel runs on a + # separate handler, so driving keeps working; only odom + the odom->base_link tf stop. + if vision_tform_body is None: + logger.error( + "Spot odom stopped: robot state has no vision->body transform, so the " + "odom->base_link TF cannot be published. cmd_vel still works, but anything " + "needing TF (nav, mapping) will not until the transform is available." + ) + raise RuntimeError("Spot state snapshot missing vision->body transform") velocity = kinematic_state.velocity_of_body_in_vision time_converter = self._robot.time_sync.get_robot_time_converter() ts = time_converter.local_seconds_from_robot_timestamp( From 59827cba7066df52c84e096b22693f7412df9aa9 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 20 Jul 2026 13:14:12 -0700 Subject: [PATCH 16/26] - --- .../robot/bosdyn/spot/effectors/high_level.py | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/dimos/robot/bosdyn/spot/effectors/high_level.py b/dimos/robot/bosdyn/spot/effectors/high_level.py index 552a8bb3fc..12b70cf150 100644 --- a/dimos/robot/bosdyn/spot/effectors/high_level.py +++ b/dimos/robot/bosdyn/spot/effectors/high_level.py @@ -430,6 +430,7 @@ async def _poll_odom(self) -> None: VISION_FRAME_NAME, get_a_tform_b, ) + from bosdyn.client.math_helpers import SE3Pose # type: ignore[import-not-found] period = 1.0 / self.config.odom_rate_hz while True: @@ -442,23 +443,23 @@ async def _poll_odom(self) -> None: continue kinematic_state = state.kinematic_state + # note: creating new time_converter's is intenional, time_sync changes itself over time, time_converter doesn't + time_converter = self._robot.time_sync.get_robot_time_converter() + ts = time_converter.local_seconds_from_robot_timestamp( + kinematic_state.acquisition_timestamp + ) vision_tform_body = get_a_tform_b( kinematic_state.transforms_snapshot, VISION_FRAME_NAME, BODY_FRAME_NAME ) - # Fail this task loudly rather than publish nothing silently. cmd_vel runs on a - # separate handler, so driving keeps working; only odom + the odom->base_link tf stop. + # No vision->body: fall back to identity so odom->base_link keeps publishing and the + # tf tree stays connected (cameras keep resolving). Odom pose reads zero until it returns. if vision_tform_body is None: - logger.error( - "Spot odom stopped: robot state has no vision->body transform, so the " - "odom->base_link TF cannot be published. cmd_vel still works, but anything " - "needing TF (nav, mapping) will not until the transform is available." + logger.warning( + "Spot state has no vision->body transform; using identity " + "odom->base_link until it returns (odometry pose reads zero)." ) - raise RuntimeError("Spot state snapshot missing vision->body transform") + vision_tform_body = SE3Pose.from_identity() velocity = kinematic_state.velocity_of_body_in_vision - time_converter = self._robot.time_sync.get_robot_time_converter() - ts = time_converter.local_seconds_from_robot_timestamp( - kinematic_state.acquisition_timestamp - ) self._publish_odom(vision_tform_body, velocity, ts) await asyncio.sleep(max(0.0, period - (time.monotonic() - start))) From 5094d53fe74331cbdde65d44f17687fe03e3ef0d Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 10 Aug 2026 12:12:21 -0700 Subject: [PATCH 17/26] refactor(spot): move Spot support under dimos/experimental Spot is not production-ready hardware support yet, so mirror the robot tree under dimos/experimental/robot/bosdyn/spot instead of shipping it alongside supported platforms. --- .../robot/bosdyn/spot/blueprints/spot.py | 4 ++-- .../bosdyn/spot/blueprints/spot_record.py | 4 ++-- .../robot/bosdyn/spot/config.py | 0 .../robot/bosdyn/spot/effectors/high_level.py | 22 +++++++++---------- .../robot/bosdyn/spot/recorder.py | 2 +- .../robot/bosdyn/spot/rerun.py | 0 .../robot/bosdyn/spot/spot.urdf | 0 .../robot/bosdyn/spot/utils.py | 0 dimos/robot/all_blueprints.py | 8 +++---- docs/platforms/quadruped/spot/index.md | 2 ++ 10 files changed, 22 insertions(+), 20 deletions(-) rename dimos/{ => experimental}/robot/bosdyn/spot/blueprints/spot.py (93%) rename dimos/{ => experimental}/robot/bosdyn/spot/blueprints/spot_record.py (90%) rename dimos/{ => experimental}/robot/bosdyn/spot/config.py (100%) rename dimos/{ => experimental}/robot/bosdyn/spot/effectors/high_level.py (99%) rename dimos/{ => experimental}/robot/bosdyn/spot/recorder.py (97%) rename dimos/{ => experimental}/robot/bosdyn/spot/rerun.py (100%) rename dimos/{ => experimental}/robot/bosdyn/spot/spot.urdf (100%) rename dimos/{ => experimental}/robot/bosdyn/spot/utils.py (100%) diff --git a/dimos/robot/bosdyn/spot/blueprints/spot.py b/dimos/experimental/robot/bosdyn/spot/blueprints/spot.py similarity index 93% rename from dimos/robot/bosdyn/spot/blueprints/spot.py rename to dimos/experimental/robot/bosdyn/spot/blueprints/spot.py index 4a80016ff0..31d77b8706 100644 --- a/dimos/robot/bosdyn/spot/blueprints/spot.py +++ b/dimos/experimental/robot/bosdyn/spot/blueprints/spot.py @@ -36,9 +36,9 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config +from dimos.experimental.robot.bosdyn.spot.effectors.high_level import SpotHighLevel +from dimos.experimental.robot.bosdyn.spot.rerun import spot_camera_layout from dimos.navigation.movement_manager.movement_manager import MovementManager -from dimos.robot.bosdyn.spot.effectors.high_level import SpotHighLevel -from dimos.robot.bosdyn.spot.rerun import spot_camera_layout from dimos.visualization.vis_module import vis_module spot = autoconnect( diff --git a/dimos/robot/bosdyn/spot/blueprints/spot_record.py b/dimos/experimental/robot/bosdyn/spot/blueprints/spot_record.py similarity index 90% rename from dimos/robot/bosdyn/spot/blueprints/spot_record.py rename to dimos/experimental/robot/bosdyn/spot/blueprints/spot_record.py index a541d375b7..0827bb6222 100644 --- a/dimos/robot/bosdyn/spot/blueprints/spot_record.py +++ b/dimos/experimental/robot/bosdyn/spot/blueprints/spot_record.py @@ -30,7 +30,7 @@ from __future__ import annotations from dimos.core.coordination.blueprints import autoconnect -from dimos.robot.bosdyn.spot.blueprints.spot import spot -from dimos.robot.bosdyn.spot.recorder import SpotRecorder +from dimos.experimental.robot.bosdyn.spot.blueprints.spot import spot +from dimos.experimental.robot.bosdyn.spot.recorder import SpotRecorder spot_record = autoconnect(spot, SpotRecorder.blueprint()) diff --git a/dimos/robot/bosdyn/spot/config.py b/dimos/experimental/robot/bosdyn/spot/config.py similarity index 100% rename from dimos/robot/bosdyn/spot/config.py rename to dimos/experimental/robot/bosdyn/spot/config.py diff --git a/dimos/robot/bosdyn/spot/effectors/high_level.py b/dimos/experimental/robot/bosdyn/spot/effectors/high_level.py similarity index 99% rename from dimos/robot/bosdyn/spot/effectors/high_level.py rename to dimos/experimental/robot/bosdyn/spot/effectors/high_level.py index 12b70cf150..8f7ae19bae 100644 --- a/dimos/robot/bosdyn/spot/effectors/high_level.py +++ b/dimos/experimental/robot/bosdyn/spot/effectors/high_level.py @@ -46,16 +46,7 @@ from dimos.agents.annotation import skill from dimos.core.core import rpc from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.protocol.tf.static_tf_publisher import StaticTfPublisher, StaticTfPublisherConfig -from dimos.robot.bosdyn.spot.config import ( +from dimos.experimental.robot.bosdyn.spot.config import ( CAMERA_MAX_HZ, FRONT_CAMERA_MIRROR_HALF_TURN, FRONT_CAMERA_ROTATE_UPRIGHT, @@ -71,7 +62,7 @@ SPOT_URDF_PATH, STAND_TIMEOUT_S, ) -from dimos.robot.bosdyn.spot.utils import ( +from dimos.experimental.robot.bosdyn.spot.utils import ( camera_info_from_response, camera_mount_transforms, clamp, @@ -80,6 +71,15 @@ rotate_camera_info_quarter_turns, rotate_image_quarter_turns, ) +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.msgs.sensor_msgs.Image import Image +from dimos.protocol.tf.static_tf_publisher import StaticTfPublisher, StaticTfPublisherConfig from dimos.utils.logging_config import setup_logger logger = setup_logger() diff --git a/dimos/robot/bosdyn/spot/recorder.py b/dimos/experimental/robot/bosdyn/spot/recorder.py similarity index 97% rename from dimos/robot/bosdyn/spot/recorder.py rename to dimos/experimental/robot/bosdyn/spot/recorder.py index 40d7dd65ff..02f2aeb397 100644 --- a/dimos/robot/bosdyn/spot/recorder.py +++ b/dimos/experimental/robot/bosdyn/spot/recorder.py @@ -28,11 +28,11 @@ from pydantic import Field from dimos.core.stream import In +from dimos.experimental.robot.bosdyn.spot.config import CAMERA_STREAM_SUFFIXES from dimos.memory2.module import OnExisting, Recorder, RecorderConfig from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image -from dimos.robot.bosdyn.spot.config import CAMERA_STREAM_SUFFIXES # jpeg codec quantises depth it to ~25cm and adds block artifacts (horrible) LOSSLESS_CODEC = "lz4+lcm" diff --git a/dimos/robot/bosdyn/spot/rerun.py b/dimos/experimental/robot/bosdyn/spot/rerun.py similarity index 100% rename from dimos/robot/bosdyn/spot/rerun.py rename to dimos/experimental/robot/bosdyn/spot/rerun.py diff --git a/dimos/robot/bosdyn/spot/spot.urdf b/dimos/experimental/robot/bosdyn/spot/spot.urdf similarity index 100% rename from dimos/robot/bosdyn/spot/spot.urdf rename to dimos/experimental/robot/bosdyn/spot/spot.urdf diff --git a/dimos/robot/bosdyn/spot/utils.py b/dimos/experimental/robot/bosdyn/spot/utils.py similarity index 100% rename from dimos/robot/bosdyn/spot/utils.py rename to dimos/experimental/robot/bosdyn/spot/utils.py diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 1ed2c891a1..8d54d91e44 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -91,8 +91,8 @@ "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_planner_coordinator", "openyam-planner-coordinator": "dimos.robot.manipulators.openyam.blueprints.basic:openyam_planner_coordinator", "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", - "spot": "dimos.robot.bosdyn.spot.blueprints.spot:spot", - "spot-record": "dimos.robot.bosdyn.spot.blueprints.spot_record:spot_record", + "spot": "dimos.experimental.robot.bosdyn.spot.blueprints.spot:spot", + "spot-record": "dimos.experimental.robot.bosdyn.spot.blueprints.spot_record:spot_record", "teleop-hosted-go2-multicam": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_go2_multicam", "teleop-hosted-go2-transport": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_go2_transport", "teleop-hosted-xarm6": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_xarm6", @@ -280,8 +280,8 @@ "simple-planner": "dimos.navigation.cmu_nav.modules.simple_planner.simple_planner.SimplePlanner", "spatial-memory": "dimos.perception.experimental.spatial_perception.SpatialMemory", "speak-skill": "dimos.agents.skills.speak_skill.SpeakSkill", - "spot-high-level": "dimos.robot.bosdyn.spot.effectors.high_level.SpotHighLevel", - "spot-recorder": "dimos.robot.bosdyn.spot.recorder.SpotRecorder", + "spot-high-level": "dimos.experimental.robot.bosdyn.spot.effectors.high_level.SpotHighLevel", + "spot-recorder": "dimos.experimental.robot.bosdyn.spot.recorder.SpotRecorder", "static-tf-publisher": "dimos.protocol.tf.static_tf_publisher.StaticTfPublisher", "tare-planner": "dimos.navigation.cmu_nav.modules.tare_planner.tare_planner.TarePlanner", "teleop-recorder": "dimos.teleop.utils.recorder.TeleopRecorder", diff --git a/docs/platforms/quadruped/spot/index.md b/docs/platforms/quadruped/spot/index.md index bb1a12f327..51515b6fd2 100644 --- a/docs/platforms/quadruped/spot/index.md +++ b/docs/platforms/quadruped/spot/index.md @@ -47,6 +47,8 @@ viewer opens with the fisheye/depth cameras and odometry. ## Layout +Spot support is experimental and lives in `dimos/experimental/robot/bosdyn/spot/`. + - `config.py` — constants + pure address/credential helpers (no `bosdyn` import). - `effectors/high_level.py` — `SpotHighLevel`: the single Spot module — lease/E-stop/power/stand + velocity commands plus the five fisheye + five depth From c57c36bf2e9a076473cfd06827e3ffe9762b9317 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 10 Aug 2026 12:20:19 -0700 Subject: [PATCH 18/26] fix(spot): adapt to the retired TF service MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit tf is now a plain Out[TFMessage] topic, so the odometry edge must be wrapped in a TFMessage and the tf stream no longer needs starting — StaticTfPublisher.start() already drives the static mount transforms. --- .../robot/bosdyn/spot/effectors/high_level.py | 28 +++++++++++-------- 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/dimos/experimental/robot/bosdyn/spot/effectors/high_level.py b/dimos/experimental/robot/bosdyn/spot/effectors/high_level.py index 8f7ae19bae..b713e1cdd1 100644 --- a/dimos/experimental/robot/bosdyn/spot/effectors/high_level.py +++ b/dimos/experimental/robot/bosdyn/spot/effectors/high_level.py @@ -79,6 +79,7 @@ from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.protocol.tf.static_tf_publisher import StaticTfPublisher, StaticTfPublisherConfig from dimos.utils.logging_config import setup_logger @@ -267,7 +268,6 @@ async def main(self) -> AsyncIterator[None]: ) self._standing = True - self.tf.start() self._image_task = asyncio.create_task(self._poll_images()) self._odom_task = asyncio.create_task(self._poll_odom()) @@ -487,17 +487,21 @@ def _publish_odom(self, vision_tform_body: Any, velocity: Any, ts: float) -> Non ) self.odometry.publish(odometry) self.tf.publish( - Transform( - translation=Vector3(vision_tform_body.x, vision_tform_body.y, vision_tform_body.z), - rotation=Quaternion( - vision_tform_body.rot.x, - vision_tform_body.rot.y, - vision_tform_body.rot.z, - vision_tform_body.rot.w, - ), - frame_id=self.config.odom_frame_id, - child_frame_id=self.config.base_frame_id, - ts=ts, + TFMessage( + Transform( + translation=Vector3( + vision_tform_body.x, vision_tform_body.y, vision_tform_body.z + ), + rotation=Quaternion( + vision_tform_body.rot.x, + vision_tform_body.rot.y, + vision_tform_body.rot.z, + vision_tform_body.rot.w, + ), + frame_id=self.config.odom_frame_id, + child_frame_id=self.config.base_frame_id, + ts=ts, + ) ) ) From c6a7b1cb51a1f95663c59913c90b19df372be0a9 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 10 Aug 2026 12:31:33 -0700 Subject: [PATCH 19/26] fix(spot): defer the rerun imports in spot/rerun.py The codebase check forbids module-level cv2/open3d/rerun imports so worker processes don't pay for a native extension they never touch. --- dimos/experimental/robot/bosdyn/spot/rerun.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/dimos/experimental/robot/bosdyn/spot/rerun.py b/dimos/experimental/robot/bosdyn/spot/rerun.py index 138b72a9df..a7ef8ac80f 100644 --- a/dimos/experimental/robot/bosdyn/spot/rerun.py +++ b/dimos/experimental/robot/bosdyn/spot/rerun.py @@ -16,8 +16,10 @@ from __future__ import annotations -import rerun as rr -import rerun.blueprint as rrb +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + import rerun.blueprint as rrb def _grayscale_origin(suffix: str) -> str: @@ -29,6 +31,8 @@ def _depth_origin(suffix: str) -> str: def _camera_view(origin: str, name: str) -> rrb.Spatial2DView: + import rerun.blueprint as rrb + return rrb.Spatial2DView(origin=origin, name=name) @@ -38,6 +42,9 @@ def spot_camera_layout() -> rrb.Blueprint: Left: the robot's tf tree and every camera frustum in place. Right: tabs of grayscale/depth feeds labelled by URDF mount position. """ + import rerun as rr + import rerun.blueprint as rrb + world_view = rrb.Spatial3DView( origin="world", name="3D", From 901e71b36fdc1dbb47cfeea74186a62eb801bc0a Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 10 Aug 2026 12:42:10 -0700 Subject: [PATCH 20/26] docs(spot): use the current CLI flag syntax, not the removed -o form Merging main brought in the removal of the legacy -o/--option syntax, so every documented spot invocation was stale. --- dimos/experimental/robot/bosdyn/spot/blueprints/spot.py | 6 +++--- .../robot/bosdyn/spot/blueprints/spot_record.py | 4 ++-- dimos/experimental/robot/bosdyn/spot/recorder.py | 2 +- docs/platforms/quadruped/spot/index.md | 6 +++--- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/dimos/experimental/robot/bosdyn/spot/blueprints/spot.py b/dimos/experimental/robot/bosdyn/spot/blueprints/spot.py index 31d77b8706..f17478613e 100644 --- a/dimos/experimental/robot/bosdyn/spot/blueprints/spot.py +++ b/dimos/experimental/robot/bosdyn/spot/blueprints/spot.py @@ -21,15 +21,15 @@ odometry. Because the browser is the input surface there is no on-main-thread pygame window, so this runs on macOS. -The ip auto-detects: with no `-o spothighlevel.ip=` given, it probes Spot's WiFi +The ip auto-detects: with no `--spothighlevel.ip=` given, it probes Spot's WiFi AP address (192.168.80.3) then the Ethernet address (10.0.0.3) and uses whichever answers. Usage: dimos run spot \ - -o spothighlevel.username=admin -o spothighlevel.password= + --spothighlevel.username=admin --spothighlevel.password= # or force an address: - dimos run spot ... -o spothighlevel.ip=10.0.0.3 + dimos run spot ... --spothighlevel.ip=10.0.0.3 """ from __future__ import annotations diff --git a/dimos/experimental/robot/bosdyn/spot/blueprints/spot_record.py b/dimos/experimental/robot/bosdyn/spot/blueprints/spot_record.py index 0827bb6222..2d0b854cb7 100644 --- a/dimos/experimental/robot/bosdyn/spot/blueprints/spot_record.py +++ b/dimos/experimental/robot/bosdyn/spot/blueprints/spot_record.py @@ -22,9 +22,9 @@ Usage: dimos run spot-record \ - -o spothighlevel.username=admin -o spothighlevel.password= + --spothighlevel.username=admin --spothighlevel.password= # choose where the recording lands: - dimos run spot-record ... -o spotrecorder.db_path=/path/to/spot.db + dimos run spot-record ... --spotrecorder.db_path=/path/to/spot.db """ from __future__ import annotations diff --git a/dimos/experimental/robot/bosdyn/spot/recorder.py b/dimos/experimental/robot/bosdyn/spot/recorder.py index 02f2aeb397..9ecafb2524 100644 --- a/dimos/experimental/robot/bosdyn/spot/recorder.py +++ b/dimos/experimental/robot/bosdyn/spot/recorder.py @@ -40,7 +40,7 @@ class SpotRecorderConfig(RecorderConfig): # Timestamped by default so each run lands in its own file instead of appending - # onto the last one. Override with `-o spotrecorder.db_path=...` to pick a path. + # onto the last one. Override with `--spotrecorder.db_path=...` to pick a path. db_path: str | Path = Field( default_factory=lambda: f"spot_recording_{datetime.now():%Y-%m-%d_%H-%M-%S}.db" ) diff --git a/docs/platforms/quadruped/spot/index.md b/docs/platforms/quadruped/spot/index.md index 51515b6fd2..6f04a98e10 100644 --- a/docs/platforms/quadruped/spot/index.md +++ b/docs/platforms/quadruped/spot/index.md @@ -26,14 +26,14 @@ address in the Spot Admin Console (`https://192.168.80.3` → Network Setup → ```bash dimos run spot \ - -o spothighlevel.username= \ - -o spothighlevel.password= + --spothighlevel.username= \ + --spothighlevel.password= ``` The username and password are printed on the sticker inside Spot's battery bay (visible when the battery is removed). -The IP auto-detects (WiFi then Ethernet). Force one with `-o spothighlevel.ip=`. +The IP auto-detects (WiFi then Ethernet). Force one with `--spothighlevel.ip=`. Keyboard teleop: WASD move/turn, QE strafe, Space soft-stop, ESC quit. A Rerun viewer opens with the fisheye/depth cameras and odometry. From 04749b5aab67e8bc9a129bb5784739835ed7d1bb Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 10 Aug 2026 12:34:12 -0700 Subject: [PATCH 21/26] feat(spot): add spot-replay blueprint + per-camera frustum anchoring SpotReplay plays a memory2 recording back onto SpotHighLevel's stream names so the Rerun camera layout lights up with no robot. spot_camera_visual_overrides routes the two shared CameraInfo streams onto each camera's image entity so every frustum anchors to its optical frame, and a green base_link box stands in for the body. --- .../robot/bosdyn/spot/blueprints/spot.py | 13 +- .../bosdyn/spot/blueprints/spot_replay.py | 58 ++++++ .../experimental/robot/bosdyn/spot/replay.py | 185 ++++++++++++++++++ dimos/experimental/robot/bosdyn/spot/rerun.py | 84 +++++++- dimos/robot/all_blueprints.py | 1 + 5 files changed, 338 insertions(+), 3 deletions(-) create mode 100644 dimos/experimental/robot/bosdyn/spot/blueprints/spot_replay.py create mode 100644 dimos/experimental/robot/bosdyn/spot/replay.py diff --git a/dimos/experimental/robot/bosdyn/spot/blueprints/spot.py b/dimos/experimental/robot/bosdyn/spot/blueprints/spot.py index f17478613e..bcb44de199 100644 --- a/dimos/experimental/robot/bosdyn/spot/blueprints/spot.py +++ b/dimos/experimental/robot/bosdyn/spot/blueprints/spot.py @@ -37,14 +37,23 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.experimental.robot.bosdyn.spot.effectors.high_level import SpotHighLevel -from dimos.experimental.robot.bosdyn.spot.rerun import spot_camera_layout +from dimos.experimental.robot.bosdyn.spot.rerun import ( + spot_camera_layout, + spot_camera_visual_overrides, +) from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.visualization.vis_module import vis_module spot = autoconnect( SpotHighLevel.blueprint(), MovementManager.blueprint(), - vis_module(global_config.viewer, rerun_config={"blueprint": spot_camera_layout}), + vis_module( + global_config.viewer, + rerun_config={ + "blueprint": spot_camera_layout, + "visual_override": spot_camera_visual_overrides(), + }, + ), ).remappings( [ # No nav stack here, so MovementManager's goal/way_point/stop_movement diff --git a/dimos/experimental/robot/bosdyn/spot/blueprints/spot_replay.py b/dimos/experimental/robot/bosdyn/spot/blueprints/spot_replay.py new file mode 100644 index 0000000000..385b57dded --- /dev/null +++ b/dimos/experimental/robot/bosdyn/spot/blueprints/spot_replay.py @@ -0,0 +1,58 @@ +# 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. + +"""Replay the latest Spot recording to Rerun — no robot, runs anywhere. + +`SpotReplay` plays a memory2 recording (from `spot-record`) back onto the same +stream names `SpotHighLevel` uses, so the Spot camera layout and per-camera +frustums light up in the Rerun 3D view. The `visual_override` routes the two +shared CameraInfo streams onto each camera's image entity so every frustum +anchors to its optical frame. + +Usage: + # newest *.db under ~/datasets/spot: + dimos run spot-replay + # a specific recording: + dimos run spot-replay -o spotreplay.db_path=/path/to/spot.db +""" + +from __future__ import annotations + +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config +from dimos.experimental.robot.bosdyn.spot.replay import SpotReplay +from dimos.experimental.robot.bosdyn.spot.rerun import ( + spot_body_static_overrides, + spot_camera_layout, + spot_camera_visual_overrides, +) +from dimos.protocol.pubsub.impl.lcmpubsub import LCM +from dimos.visualization.rerun.bridge import RerunBridgeModule +from dimos.visualization.rerun.websocket_server import RerunWebSocketServer + +# Compose only the Rerun bridge (+ its websocket server) directly instead of the +# shared `vis_module`: replay just needs the 3D viewer, and `vis_module` also +# bundles the WebsocketVisModule, which auto-opens the 7779 Command Center tab. +spot_replay = autoconnect( + SpotReplay.blueprint(), + RerunBridgeModule.blueprint( + pubsubs=[LCM()], + rerun_open=global_config.rerun_open, + rerun_web=global_config.rerun_web, + blueprint=spot_camera_layout, + visual_override=spot_camera_visual_overrides(), + static=spot_body_static_overrides(), + ), + RerunWebSocketServer.blueprint(), +) diff --git a/dimos/experimental/robot/bosdyn/spot/replay.py b/dimos/experimental/robot/bosdyn/spot/replay.py new file mode 100644 index 0000000000..f9f8210d6c --- /dev/null +++ b/dimos/experimental/robot/bosdyn/spot/replay.py @@ -0,0 +1,185 @@ +# 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. + +"""Replay a recorded Spot session to streams, mirroring `SpotHighLevel`'s outputs. + +Opens a memory2 SQLite recording (written by `SpotRecorder`) and replays every +camera, depth, and odometry stream onto Out ports named exactly like +`SpotHighLevel`'s, so the same Rerun visualization wires up by name — no robot +required. The recorded ``tf`` tree (odom->base_link plus the base_link->camera +mounts) is republished so every frame stays spatially anchored in 3D. +""" + +from __future__ import annotations + +from collections.abc import AsyncIterator +import math +from pathlib import Path + +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import Out +from dimos.experimental.robot.bosdyn.spot.config import ( + CAMERA_STREAM_SUFFIXES, + FRONT_CAMERA_ROTATE_UPRIGHT, +) +from dimos.memory2.store.sqlite import SqliteStore +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.nav_msgs.Path import Path as NavPath +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +# Image Out ports to replay, matching SpotHighLevel / SpotRecorder stream names. +_IMAGE_STREAMS = [ + f"{kind}_image_{suffix}" for kind in ("grayscale", "depth") for suffix in CAMERA_STREAM_SUFFIXES +] +_PLAYBACK_STREAMS = [*_IMAGE_STREAMS, "grayscale_info", "depth_info", "odometry"] + +# Cap the accumulated odom trail so a looping replay doesn't grow it forever. +_MAX_PATH_POSES = 2000 + +# SpotHighLevel rights the sideways front camera images but leaves their optical +# tf frames at the raw mount orientation, so the 3D frustum + depth +# back-projection land rotated. Roll each front optical frame about its viewing +# (z) axis to bring it back upright. The two front cameras mount mirror-imaged, +# so frontright sits a half turn (2 quarter turns) past frontleft. +_HALF_TURN_QUARTERS = 2 +_OPTICAL_FRAME_ROLL_TURNS = { + "frontleft_camera_optical": FRONT_CAMERA_ROTATE_UPRIGHT, + "frontright_camera_optical": FRONT_CAMERA_ROTATE_UPRIGHT + _HALF_TURN_QUARTERS, +} + + +class SpotReplayConfig(ModuleConfig): + """Where to read the recording from and how to play it back.""" + + # Explicit recording path. Empty -> newest ``*.db`` in ``dataset_dir``. + db_path: str = "" + dataset_dir: str = "~/datasets/spot" + + speed: float = 1.0 + loop: bool = True + seek: float | None = None + duration: float | None = None + + # Roll the front optical tf frames upright at replay time. Off by default: + # recordings are expected to already store upright front frames. Turn on to + # view an old recording whose tf still holds the raw sideways mount. + roll_front_frames: bool = False + + +class SpotReplay(Module): + """Replays Spot's fisheye + depth cameras, odometry, and tf from a recording.""" + + config: SpotReplayConfig + dedicated_worker = True + + grayscale_image_front_left: Out[Image] + grayscale_image_front_right: Out[Image] + grayscale_image_left: Out[Image] + grayscale_image_right: Out[Image] + grayscale_image_back: Out[Image] + + depth_image_front_left: Out[Image] + depth_image_front_right: Out[Image] + depth_image_left: Out[Image] + depth_image_right: Out[Image] + depth_image_back: Out[Image] + + grayscale_info: Out[CameraInfo] + depth_info: Out[CameraInfo] + + odometry: Out[Odometry] + odom_path: Out[NavPath] + tf: Out[TFMessage] + + _odom_path: NavPath + + def _resolve_db_path(self) -> Path: + if self.config.db_path: + return Path(self.config.db_path).expanduser() + directory = Path(self.config.dataset_dir).expanduser() + recordings = sorted(directory.glob("*.db"), key=lambda path: path.stat().st_mtime) + if not recordings: + raise FileNotFoundError(f"No .db recordings found in {directory}") + return recordings[-1] + + def _republish_tf(self, message: TFMessage) -> None: + self.tf.publish( + TFMessage(*(self._roll_optical_frame(transform) for transform in message.transforms)) + ) + + def _roll_optical_frame(self, transform: Transform) -> Transform: + if not self.config.roll_front_frames: + return transform + turns = _OPTICAL_FRAME_ROLL_TURNS.get(transform.child_frame_id) + if not turns: + return transform + roll = Quaternion.from_euler(Vector3(0.0, 0.0, turns * math.pi / 2)) + return Transform( + translation=transform.translation, + rotation=transform.rotation * roll, + frame_id=transform.frame_id, + child_frame_id=transform.child_frame_id, + ts=transform.ts, + ) + + def _republish_odometry(self, message: Odometry) -> None: + self.odometry.publish(message) + self._odom_path.frame_id = message.frame_id or self._odom_path.frame_id + self._odom_path.push_mut(message.to_pose_stamped()) + if len(self._odom_path.poses) > _MAX_PATH_POSES: + del self._odom_path.poses[:-_MAX_PATH_POSES] + self.odom_path.publish(self._odom_path) + + async def main(self) -> AsyncIterator[None]: + db_path = self._resolve_db_path() + logger.info(f"Replaying Spot recording from {db_path}") + + store = SqliteStore(path=str(db_path), must_exist=True) + store.start() + self.register_disposable(store) + + replay = store.replay( + speed=self.config.speed, + loop=self.config.loop, + seek=self.config.seek, + duration=self.config.duration, + ) + available = set(replay.list_streams()) + + self._odom_path = NavPath(frame_id="odom") + + for name in _PLAYBACK_STREAMS: + if name not in available: + logger.warning(f"Spot replay: stream {name!r} missing from recording; skipping") + continue + if name == "odometry": + subscriber = self._republish_odometry + else: + subscriber = getattr(self, name).publish + self.register_disposable(replay.stream(name).observable().subscribe(subscriber)) + + if "tf" in available: + self.register_disposable(replay.stream("tf").observable().subscribe(self._republish_tf)) + else: + logger.warning("Spot replay: no tf stream in recording; 3D frames will be missing") + + yield diff --git a/dimos/experimental/robot/bosdyn/spot/rerun.py b/dimos/experimental/robot/bosdyn/spot/rerun.py index a7ef8ac80f..12414e618f 100644 --- a/dimos/experimental/robot/bosdyn/spot/rerun.py +++ b/dimos/experimental/robot/bosdyn/spot/rerun.py @@ -16,11 +16,34 @@ from __future__ import annotations -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Any if TYPE_CHECKING: + from collections.abc import Callable + import rerun.blueprint as rrb + from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo + from dimos.visualization.rerun.bridge import RerunData + +# Optical tf frame_id (SpotHighLevelConfig defaults) -> stream-name suffix. +_OPTICAL_FRAME_TO_SUFFIX = { + "frontleft_camera_optical": "front_left", + "frontright_camera_optical": "front_right", + "left_camera_optical": "left", + "right_camera_optical": "right", + "back_camera_optical": "back", +} + +# The five cameras mount ~0.3 m apart on Spot's body; a full 1.0 m image plane +# makes neighbouring frustums overlap in the 3D view, so draw them shorter. +_FRUSTUM_PLANE_DISTANCE = 0.3 + +# Spot's body is ~1.1 m long, 0.5 m wide, 0.19 m tall and base_link sits at its +# center, so a box that size on the frame stands in for the robot. +_SPOT_BODY_SIZE = (1.1, 0.5, 0.19) +_SPOT_GREEN = (0, 255, 0) + def _grayscale_origin(suffix: str) -> str: return f"world/grayscale_image_{suffix}" @@ -30,6 +53,65 @@ def _depth_origin(suffix: str) -> str: return f"world/depth_image_{suffix}" +def _camera_info_pinhole(camera_info: CameraInfo, origin: Callable[[str], str]) -> RerunData | None: + """Re-emit a shared CameraInfo onto its camera's image entity as a Pinhole. + + The Pinhole is bare (no ``optical_frame``): the matching Image message + carries the same ``frame_id`` and the bridge attaches that tf transform to + the entity, so setting ``parent_frame`` here too would create a second + parent, which Rerun rejects. + """ + suffix = _OPTICAL_FRAME_TO_SUFFIX.get(camera_info.frame_id) + if suffix is None: + return None + return camera_info.to_rerun( + image_plane_distance=_FRUSTUM_PLANE_DISTANCE, + image_topic=origin(suffix), + ) + + +# Module-level (not closures) so the RerunBridgeModule config stays picklable +# when it is shipped to its worker process. +def _grayscale_info_to_pinhole(camera_info: CameraInfo) -> RerunData | None: + return _camera_info_pinhole(camera_info, _grayscale_origin) + + +def _depth_info_to_pinhole(camera_info: CameraInfo) -> RerunData | None: + return _camera_info_pinhole(camera_info, _depth_origin) + + +def spot_camera_visual_overrides() -> dict[str, Callable[[CameraInfo], RerunData | None]]: + """Anchor per-camera frustums from the two shared CameraInfo streams. + + ``grayscale_info``/``depth_info`` are shared across all five cameras, so + their default Pinhole lands on one throwaway entity and no camera renders a + frustum. Route each message onto its camera's image entity (picked by the + CameraInfo's optical ``frame_id``) so every image plane gets a projection. + """ + return { + "world/grayscale_info": _grayscale_info_to_pinhole, + "world/depth_info": _depth_info_to_pinhole, + } + + +def _spot_body_box(rerun_module: Any) -> list[Any]: + """A green box at base_link standing in for Spot's body.""" + return [ + rerun_module.Transform3D(parent_frame="tf#/base_link"), + rerun_module.Boxes3D( + centers=[(0.0, 0.0, 0.0)], + sizes=[_SPOT_BODY_SIZE], + colors=[_SPOT_GREEN], + fill_mode="solid", + ), + ] + + +def spot_body_static_overrides() -> dict[str, Callable[[Any], Any]]: + """Draw a green body box anchored to the moving base_link frame.""" + return {"world/spot_body": _spot_body_box} + + def _camera_view(origin: str, name: str) -> rrb.Spatial2DView: import rerun.blueprint as rrb diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 8d54d91e44..a56aacc225 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -93,6 +93,7 @@ "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", "spot": "dimos.experimental.robot.bosdyn.spot.blueprints.spot:spot", "spot-record": "dimos.experimental.robot.bosdyn.spot.blueprints.spot_record:spot_record", + "spot-replay": "dimos.experimental.robot.bosdyn.spot.blueprints.spot_replay:spot_replay", "teleop-hosted-go2-multicam": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_go2_multicam", "teleop-hosted-go2-transport": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_go2_transport", "teleop-hosted-xarm6": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_xarm6", From 1e61021ad2b868930503a4d1481e8092ef966946 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 10 Aug 2026 13:00:56 -0700 Subject: [PATCH 22/26] feat(spot): draw the whole travelled path in spot-replay SpotReplay accumulated its own trail and capped it at 2000 poses, so a long recording only ever showed its tail. OdometryPath (from the cuvslam branch) does the same job as a reusable module with a 20000-pose horizon and a publish-rate ceiling, and works on a live robot too. --- .../bosdyn/spot/blueprints/spot_replay.py | 6 +- .../experimental/robot/bosdyn/spot/replay.py | 28 +---- dimos/mapping/odometry_path.py | 115 ++++++++++++++++++ 3 files changed, 125 insertions(+), 24 deletions(-) create mode 100644 dimos/mapping/odometry_path.py diff --git a/dimos/experimental/robot/bosdyn/spot/blueprints/spot_replay.py b/dimos/experimental/robot/bosdyn/spot/blueprints/spot_replay.py index 385b57dded..83a79383d0 100644 --- a/dimos/experimental/robot/bosdyn/spot/blueprints/spot_replay.py +++ b/dimos/experimental/robot/bosdyn/spot/blueprints/spot_replay.py @@ -24,7 +24,7 @@ # newest *.db under ~/datasets/spot: dimos run spot-replay # a specific recording: - dimos run spot-replay -o spotreplay.db_path=/path/to/spot.db + dimos run spot-replay --db-path=/path/to/spot.db """ from __future__ import annotations @@ -37,6 +37,7 @@ spot_camera_layout, spot_camera_visual_overrides, ) +from dimos.mapping.odometry_path import OdometryPath from dimos.protocol.pubsub.impl.lcmpubsub import LCM from dimos.visualization.rerun.bridge import RerunBridgeModule from dimos.visualization.rerun.websocket_server import RerunWebSocketServer @@ -46,6 +47,7 @@ # bundles the WebsocketVisModule, which auto-opens the 7779 Command Center tab. spot_replay = autoconnect( SpotReplay.blueprint(), + OdometryPath.blueprint(), RerunBridgeModule.blueprint( pubsubs=[LCM()], rerun_open=global_config.rerun_open, @@ -55,4 +57,4 @@ static=spot_body_static_overrides(), ), RerunWebSocketServer.blueprint(), -) +).remappings([(OdometryPath, "path", "odom_path")]) diff --git a/dimos/experimental/robot/bosdyn/spot/replay.py b/dimos/experimental/robot/bosdyn/spot/replay.py index f9f8210d6c..e2a271b1e2 100644 --- a/dimos/experimental/robot/bosdyn/spot/replay.py +++ b/dimos/experimental/robot/bosdyn/spot/replay.py @@ -19,6 +19,9 @@ `SpotHighLevel`'s, so the same Rerun visualization wires up by name — no robot required. The recorded ``tf`` tree (odom->base_link plus the base_link->camera mounts) is republished so every frame stays spatially anchored in 3D. + +The travelled trail is not built here: ``odometry`` feeds ``OdometryPath``, +which accumulates it and is equally happy on a live robot. """ from __future__ import annotations @@ -38,7 +41,6 @@ from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.nav_msgs.Path import Path as NavPath from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.tf2_msgs.TFMessage import TFMessage @@ -52,9 +54,6 @@ ] _PLAYBACK_STREAMS = [*_IMAGE_STREAMS, "grayscale_info", "depth_info", "odometry"] -# Cap the accumulated odom trail so a looping replay doesn't grow it forever. -_MAX_PATH_POSES = 2000 - # SpotHighLevel rights the sideways front camera images but leaves their optical # tf frames at the raw mount orientation, so the 3D frustum + depth # back-projection land rotated. Roll each front optical frame about its viewing @@ -107,11 +106,8 @@ class SpotReplay(Module): depth_info: Out[CameraInfo] odometry: Out[Odometry] - odom_path: Out[NavPath] tf: Out[TFMessage] - _odom_path: NavPath - def _resolve_db_path(self) -> Path: if self.config.db_path: return Path(self.config.db_path).expanduser() @@ -141,14 +137,6 @@ def _roll_optical_frame(self, transform: Transform) -> Transform: ts=transform.ts, ) - def _republish_odometry(self, message: Odometry) -> None: - self.odometry.publish(message) - self._odom_path.frame_id = message.frame_id or self._odom_path.frame_id - self._odom_path.push_mut(message.to_pose_stamped()) - if len(self._odom_path.poses) > _MAX_PATH_POSES: - del self._odom_path.poses[:-_MAX_PATH_POSES] - self.odom_path.publish(self._odom_path) - async def main(self) -> AsyncIterator[None]: db_path = self._resolve_db_path() logger.info(f"Replaying Spot recording from {db_path}") @@ -165,17 +153,13 @@ async def main(self) -> AsyncIterator[None]: ) available = set(replay.list_streams()) - self._odom_path = NavPath(frame_id="odom") - for name in _PLAYBACK_STREAMS: if name not in available: logger.warning(f"Spot replay: stream {name!r} missing from recording; skipping") continue - if name == "odometry": - subscriber = self._republish_odometry - else: - subscriber = getattr(self, name).publish - self.register_disposable(replay.stream(name).observable().subscribe(subscriber)) + self.register_disposable( + replay.stream(name).observable().subscribe(getattr(self, name).publish) + ) if "tf" in available: self.register_disposable(replay.stream("tf").observable().subscribe(self._republish_tf)) diff --git a/dimos/mapping/odometry_path.py b/dimos/mapping/odometry_path.py new file mode 100644 index 0000000000..9fb474e15c --- /dev/null +++ b/dimos/mapping/odometry_path.py @@ -0,0 +1,115 @@ +# 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. + +"""Accumulate an odometry stream into the path it has travelled.""" + +from __future__ import annotations + +import math +from typing import Any + +from reactivex.disposable import Disposable + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.nav_msgs.Path import Path + + +class OdometryPathConfig(ModuleConfig): + # Frame the path is stamped in. Left empty it follows the odometry's own + # ``frame_id``, which is what puts the line under the right node of the tf + # tree in a viewer. + frame_id: str = "" + # Poses closer together than this are dropped. A stationary robot otherwise + # piles thousands of identical points onto the same spot, and every one of + # them is re-encoded on every publish. + min_step_m: float = 0.02 + # Oldest poses fall off past this. Decimating instead would keep the whole + # history, but a trail whose shape changes under you is worse than one with a + # known, honest horizon. + max_poses: int = 20000 + # Publish rate ceiling, in seconds between messages. The path is republished + # whole, so at 30 Hz odometry an unbounded rate spends more time serializing + # the trail than tracking. + min_publish_interval_s: float = 0.1 + + +class OdometryPath(Module): + """``odometry`` in, the trail it has drawn out. + + A viewer shows odometry as a pose: where the robot is now, and nothing about + where it has been. This keeps the history and republishes it as a + ``nav_msgs/Path``, which renders as a line. + + The trail inherits the odometry's drift -- it is where the *estimator* thinks + the robot went. Feeding it a SLAM-corrected pose instead makes the trail jump + at every loop closure, so prefer the continuous odometry and let the viewer's + ``map`` -> ``odom`` edge carry the correction. + """ + + config: OdometryPathConfig + + odometry: In[Odometry] + + path: Out[Path] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._poses: list[PoseStamped] = [] + self._last_publish_ts = 0.0 + + @rpc + def start(self) -> None: + super().start() + self.register_disposable(Disposable(self.odometry.subscribe(self._on_odometry))) + + def _on_odometry(self, msg: Odometry) -> None: + position = msg.pose.position + previous = self._poses[-1] if self._poses else None + if previous is not None and ( + math.dist( + (previous.x, previous.y, previous.z), + (position.x, position.y, position.z), + ) + < self.config.min_step_m + ): + return + + orientation = msg.pose.orientation + self._poses.append( + PoseStamped( + ts=msg.ts, + frame_id=self.config.frame_id or msg.frame_id, + position=[position.x, position.y, position.z], + orientation=[orientation.x, orientation.y, orientation.z, orientation.w], + ) + ) + if len(self._poses) > self.config.max_poses: + del self._poses[: len(self._poses) - self.config.max_poses] + + if msg.ts - self._last_publish_ts < self.config.min_publish_interval_s: + return + self._last_publish_ts = msg.ts + # A copy, because Path holds the list by reference and the next pose would + # otherwise mutate a message already on its way out. + self.path.publish( + Path( + ts=msg.ts, + frame_id=self.config.frame_id or msg.frame_id, + poses=list(self._poses), + ) + ) From a0c8840bc3f07ba17516ce1f20e65ff639e78c01 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 10 Aug 2026 13:05:04 -0700 Subject: [PATCH 23/26] refactor(mapping): take the newer async-handler OdometryPath I had copied the version from alfred_cuvslam, which still drives itself with an explicit start()/subscribe. cuvslam carries the later rewrite onto an async handle_odometry, which is the convention the rest of the modules follow. --- dimos/mapping/odometry_path.py | 77 ++++++++-------------------------- 1 file changed, 18 insertions(+), 59 deletions(-) diff --git a/dimos/mapping/odometry_path.py b/dimos/mapping/odometry_path.py index 9fb474e15c..da71de1eaf 100644 --- a/dimos/mapping/odometry_path.py +++ b/dimos/mapping/odometry_path.py @@ -16,12 +16,10 @@ from __future__ import annotations +from collections import deque import math from typing import Any -from reactivex.disposable import Disposable - -from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -30,36 +28,15 @@ class OdometryPathConfig(ModuleConfig): - # Frame the path is stamped in. Left empty it follows the odometry's own - # ``frame_id``, which is what puts the line under the right node of the tf - # tree in a viewer. + # Empty follows the odometry's own frame_id. frame_id: str = "" - # Poses closer together than this are dropped. A stationary robot otherwise - # piles thousands of identical points onto the same spot, and every one of - # them is re-encoded on every publish. - min_step_m: float = 0.02 - # Oldest poses fall off past this. Decimating instead would keep the whole - # history, but a trail whose shape changes under you is worse than one with a - # known, honest horizon. + min_step_meters: float = 0.02 max_poses: int = 20000 - # Publish rate ceiling, in seconds between messages. The path is republished - # whole, so at 30 Hz odometry an unbounded rate spends more time serializing - # the trail than tracking. - min_publish_interval_s: float = 0.1 + min_publish_interval_seconds: float = 0.1 class OdometryPath(Module): - """``odometry`` in, the trail it has drawn out. - - A viewer shows odometry as a pose: where the robot is now, and nothing about - where it has been. This keeps the history and republishes it as a - ``nav_msgs/Path``, which renders as a line. - - The trail inherits the odometry's drift -- it is where the *estimator* thinks - the robot went. Feeding it a SLAM-corrected pose instead makes the trail jump - at every loop closure, so prefer the continuous odometry and let the viewer's - ``map`` -> ``odom`` edge carry the correction. - """ + """``odometry`` in, the trail it has drawn out, as a ``nav_msgs/Path``.""" config: OdometryPathConfig @@ -69,47 +46,29 @@ class OdometryPath(Module): def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) - self._poses: list[PoseStamped] = [] + self._poses: deque[PoseStamped] = deque(maxlen=self.config.max_poses) self._last_publish_ts = 0.0 - @rpc - def start(self) -> None: - super().start() - self.register_disposable(Disposable(self.odometry.subscribe(self._on_odometry))) - - def _on_odometry(self, msg: Odometry) -> None: + async def handle_odometry(self, msg: Odometry) -> None: position = msg.pose.position - previous = self._poses[-1] if self._poses else None - if previous is not None and ( - math.dist( - (previous.x, previous.y, previous.z), - (position.x, position.y, position.z), - ) - < self.config.min_step_m - ): - return + point = (position.x, position.y, position.z) + if self._poses: + previous = self._poses[-1] + if math.dist((previous.x, previous.y, previous.z), point) < self.config.min_step_meters: + return orientation = msg.pose.orientation + frame_id = self.config.frame_id or msg.frame_id self._poses.append( PoseStamped( ts=msg.ts, - frame_id=self.config.frame_id or msg.frame_id, - position=[position.x, position.y, position.z], + frame_id=frame_id, + position=list(point), orientation=[orientation.x, orientation.y, orientation.z, orientation.w], ) ) - if len(self._poses) > self.config.max_poses: - del self._poses[: len(self._poses) - self.config.max_poses] - - if msg.ts - self._last_publish_ts < self.config.min_publish_interval_s: + if msg.ts - self._last_publish_ts < self.config.min_publish_interval_seconds: return self._last_publish_ts = msg.ts - # A copy, because Path holds the list by reference and the next pose would - # otherwise mutate a message already on its way out. - self.path.publish( - Path( - ts=msg.ts, - frame_id=self.config.frame_id or msg.frame_id, - poses=list(self._poses), - ) - ) + # A copy: Path holds the list by reference. + self.path.publish(Path(ts=msg.ts, frame_id=frame_id, poses=list(self._poses))) From a3513c66cc0a67fd78e5a39d024c0dc53d8164be Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 10 Aug 2026 14:22:01 -0700 Subject: [PATCH 24/26] fixup jitter --- dimos/experimental/robot/bosdyn/spot/rerun.py | 96 ++++++++++++++++--- 1 file changed, 83 insertions(+), 13 deletions(-) diff --git a/dimos/experimental/robot/bosdyn/spot/rerun.py b/dimos/experimental/robot/bosdyn/spot/rerun.py index 12414e618f..0f028b571a 100644 --- a/dimos/experimental/robot/bosdyn/spot/rerun.py +++ b/dimos/experimental/robot/bosdyn/spot/rerun.py @@ -18,12 +18,15 @@ from typing import TYPE_CHECKING, Any +from dimos.protocol.tf.tf import MultiTBuffer + if TYPE_CHECKING: from collections.abc import Callable import rerun.blueprint as rrb from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo + from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.visualization.rerun.bridge import RerunData # Optical tf frame_id (SpotHighLevelConfig defaults) -> stream-name suffix. @@ -44,6 +47,25 @@ _SPOT_BODY_SIZE = (1.1, 0.5, 0.19) _SPOT_GREEN = (0, 255, 0) +# Mirrors the tf stream so a camera pose can be looked up at the moment its +# image was captured. Module-level, not an attribute of the override objects: +# those get pickled out to the bridge's worker and would each carry a copy. +_tf_buffer = MultiTBuffer() + + +def _anchor_frame() -> str | None: + """The frame that is a parent but never a child - the one that never moves. + + Everything else in the tf tree hangs off it, so a pose expressed against it + stays put no matter what the robot does afterwards. Discovered rather than + named because the odometry frame is configurable. None while the tree is + still filling in, or if it has more than one root. + """ + parents = {parent for parent, _ in _tf_buffer.buffers} + children = {child for _, child in _tf_buffer.buffers} + roots = parents - children + return roots.pop() if len(roots) == 1 else None + def _grayscale_origin(suffix: str) -> str: return f"world/grayscale_image_{suffix}" @@ -53,20 +75,30 @@ def _depth_origin(suffix: str) -> str: return f"world/depth_image_{suffix}" -def _camera_info_pinhole(camera_info: CameraInfo, origin: Callable[[str], str]) -> RerunData | None: - """Re-emit a shared CameraInfo onto its camera's image entity as a Pinhole. +def _camera_entity(origin: str) -> str: + """Child entity holding the projection and the pixels, under the camera's pose. - The Pinhole is bare (no ``optical_frame``): the matching Image message - carries the same ``frame_id`` and the bridge attaches that tf transform to - the entity, so setting ``parent_frame`` here too would create a second - parent, which Rerun rejects. + A Pinhole is a parent link, and rerun gives an entity exactly one — so an + entity carrying a projection cannot also carry the camera's pose. One level + down the pixels inherit the pose logged above them. """ + return f"{origin}/camera" + + +def _tf_to_rerun(tf_message: TFMessage) -> RerunData: + """Log tf as usual, and keep a copy for the camera pose lookups.""" + _tf_buffer.receive_tfmessage(tf_message) + return tf_message.to_rerun() + + +def _camera_info_pinhole(camera_info: CameraInfo, origin: Callable[[str], str]) -> RerunData | None: + """Re-emit a shared CameraInfo onto its camera's image entity as a Pinhole.""" suffix = _OPTICAL_FRAME_TO_SUFFIX.get(camera_info.frame_id) if suffix is None: return None return camera_info.to_rerun( image_plane_distance=_FRUSTUM_PLANE_DISTANCE, - image_topic=origin(suffix), + image_topic=_camera_entity(origin(suffix)), ) @@ -80,18 +112,24 @@ def _depth_info_to_pinhole(camera_info: CameraInfo) -> RerunData | None: return _camera_info_pinhole(camera_info, _depth_origin) -def spot_camera_visual_overrides() -> dict[str, Callable[[CameraInfo], RerunData | None]]: - """Anchor per-camera frustums from the two shared CameraInfo streams. +def spot_camera_visual_overrides() -> dict[str, Callable[[Any], RerunData | None]]: + """Give each camera its own frustum, posed where it was at capture time. ``grayscale_info``/``depth_info`` are shared across all five cameras, so their default Pinhole lands on one throwaway entity and no camera renders a - frustum. Route each message onto its camera's image entity (picked by the - CameraInfo's optical ``frame_id``) so every image plane gets a projection. + frustum. Route each message onto its camera's entity (picked by the + CameraInfo's optical ``frame_id``) so every image plane gets a projection, + and hand the images a pose of their own — see :class:`_ImageBakedIntoAnchor`. """ - return { + overrides: dict[str, Callable[[Any], RerunData | None]] = { + "world/tf": _tf_to_rerun, "world/grayscale_info": _grayscale_info_to_pinhole, "world/depth_info": _depth_info_to_pinhole, } + for suffix in _OPTICAL_FRAME_TO_SUFFIX.values(): + for origin in (_grayscale_origin, _depth_origin): + overrides[origin(suffix)] = _ImageBakedIntoAnchor(origin(suffix)) + return overrides def _spot_body_box(rerun_module: Any) -> list[Any]: @@ -115,7 +153,7 @@ def spot_body_static_overrides() -> dict[str, Callable[[Any], Any]]: def _camera_view(origin: str, name: str) -> rrb.Spatial2DView: import rerun.blueprint as rrb - return rrb.Spatial2DView(origin=origin, name=name) + return rrb.Spatial2DView(origin=_camera_entity(origin), name=name) def spot_camera_layout() -> rrb.Blueprint: @@ -178,3 +216,35 @@ def spot_camera_layout() -> rrb.Blueprint: ), collapse_panels=True, ) + + +# This is a correction to depth jitter +# Depth starts in the right location then immediately moves slightly to the wrong location because of the odom updates +# Odom is faster than depth hz, but depth is anchored dynamically to odom +# The solution is to anchor the image to the frame *at a specific time* +# Probably hasn't been noticed earlier because its less obvious for color image and for 60/30zh +# we should fix this in our rerun bridge but I'm not going to do that in this spot PR +class _ImageBakedIntoAnchor: + def __init__(self, origin: str) -> None: + self.origin = origin + + def __call__(self, image: Any) -> RerunData | None: + anchor = _anchor_frame() + if anchor is None: + return None + pose = _tf_buffer.get(anchor, image.frame_id, image.ts) + if pose is None: + return None + import rerun as rr + + return [ + ( + self.origin, + rr.Transform3D( + translation=[pose.translation.x, pose.translation.y, pose.translation.z], + rotation=pose.rotation.to_rerun(), + parent_frame=f"tf#/{anchor}", + ), + ), + (_camera_entity(self.origin), image.to_rerun()), + ] From 4fb8cf6dcf858e11027224ff6b3255e6a008fd9d Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 10 Aug 2026 14:40:45 -0700 Subject: [PATCH 25/26] fix(blueprints): register OdometryPath in all_blueprints --- dimos/robot/all_blueprints.py | 1 + 1 file changed, 1 insertion(+) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index a56aacc225..e8ac3af2bb 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -252,6 +252,7 @@ "object-tracker2-d": "dimos.perception.experimental.object_tracker_2d.ObjectTracker2D", "object-tracker3-d": "dimos.perception.experimental.object_tracker_3d.ObjectTracker3D", "object-tracking": "dimos.perception.experimental.object_tracker.ObjectTracking", + "odometry-path": "dimos.mapping.odometry_path.OdometryPath", "osm-skill": "dimos.agents.skills.osm.OsmSkill", "path-follower": "dimos.navigation.cmu_nav.modules.path_follower.path_follower.PathFollower", "path-following-coordinator": "dimos.control.path_following_coordinator.PathFollowingCoordinator", From e644e5e1d58393fe09a49faca6c9499cbe4977a2 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 10 Aug 2026 14:40:53 -0700 Subject: [PATCH 26/26] feat(spot): let spot-replay pull its recording from LFS by name Routes an explicit --db-path through resolve_db_path (the same helper go2 replay uses), so a name that is not on disk is downloaded from LFS instead of erroring. Makes `dimos run spot-replay --db-path=spot_small_loop.db` a one-liner that works on a fresh clone. --- .../robot/bosdyn/spot/blueprints/spot_replay.py | 2 ++ dimos/experimental/robot/bosdyn/spot/replay.py | 6 ++++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/dimos/experimental/robot/bosdyn/spot/blueprints/spot_replay.py b/dimos/experimental/robot/bosdyn/spot/blueprints/spot_replay.py index 83a79383d0..3f9fd629a0 100644 --- a/dimos/experimental/robot/bosdyn/spot/blueprints/spot_replay.py +++ b/dimos/experimental/robot/bosdyn/spot/blueprints/spot_replay.py @@ -21,6 +21,8 @@ anchors to its optical frame. Usage: + # the shared recording, pulled from LFS on first run: + dimos run spot-replay --db-path=spot_small_loop.db # newest *.db under ~/datasets/spot: dimos run spot-replay # a specific recording: diff --git a/dimos/experimental/robot/bosdyn/spot/replay.py b/dimos/experimental/robot/bosdyn/spot/replay.py index e2a271b1e2..5085640f3a 100644 --- a/dimos/experimental/robot/bosdyn/spot/replay.py +++ b/dimos/experimental/robot/bosdyn/spot/replay.py @@ -36,6 +36,7 @@ CAMERA_STREAM_SUFFIXES, FRONT_CAMERA_ROTATE_UPRIGHT, ) +from dimos.memory2.replay import resolve_db_path from dimos.memory2.store.sqlite import SqliteStore from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform @@ -69,7 +70,8 @@ class SpotReplayConfig(ModuleConfig): """Where to read the recording from and how to play it back.""" - # Explicit recording path. Empty -> newest ``*.db`` in ``dataset_dir``. + # An on-disk path, or the name of a dataset to pull from LFS (e.g. + # ``spot_small_loop.db``). Empty -> newest ``*.db`` in ``dataset_dir``. db_path: str = "" dataset_dir: str = "~/datasets/spot" @@ -110,7 +112,7 @@ class SpotReplay(Module): def _resolve_db_path(self) -> Path: if self.config.db_path: - return Path(self.config.db_path).expanduser() + return resolve_db_path(Path(self.config.db_path).expanduser()) directory = Path(self.config.dataset_dir).expanduser() recordings = sorted(directory.glob("*.db"), key=lambda path: path.stat().st_mtime) if not recordings: