From c2b429759ce1c024350d9edd41ac1fd186370ab1 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Fri, 24 Jul 2026 04:37:28 +0800 Subject: [PATCH 01/33] Add pluggable simulation providers --- dimos/core/global_config.py | 1 + .../blueprints/basic/unitree_g1_groot_wbc.py | 23 +++++-- dimos/simulation/providers.py | 63 +++++++++++++++++++ dimos/simulation/test_providers.py | 42 +++++++++++++ 4 files changed, 125 insertions(+), 4 deletions(-) create mode 100644 dimos/simulation/providers.py create mode 100644 dimos/simulation/test_providers.py diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index ec150c9bc9..f6b90f2be5 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -53,6 +53,7 @@ class GlobalConfig(BaseSettings): can_port: str | None = None device_path: str | None = None # device path for real robot (e.g. /dev/ttyUSB0) simulation: str = "" + simulation_provider: str = "" replay: bool = False replay_db: str = "go2_short" new_memory: bool = False diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py index 7621eafc6d..9876effc71 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py @@ -79,6 +79,7 @@ g1_urdf_joint_state, g1_urdf_static_robot, ) +from dimos.simulation.providers import SimulationRequest, load_simulation_provider from dimos.simulation.scene_assets.spec import ScenePackage from dimos.utils.data import LfsPath from dimos.visualization.rerun.scene_package import scene_package_static_entities @@ -254,13 +255,27 @@ def _precomposed_g1_scene(package: ScenePackage) -> Path | None: ) return candidate - # Sim backend: MuJoCo engine via SHM. - _backend, _adapter_address = _scene_mujoco_backend() + if global_config.simulation_provider: + _provider = load_simulation_provider(global_config.simulation_provider) + _binding = _provider.build( + SimulationRequest( + robot_model="unitree_g1", + model_path=_ROBOT_ONLY_MJCF_PATH, + mesh_dir=_ROBOT_MESHDIR, + scene_package=global_config.scene_package, + ) + ) + _backend = _binding.backend + _adapter_address = _binding.adapter_address + _adapter_type = _binding.adapter_type + else: + _backend, _adapter_address = _scene_mujoco_backend() + _adapter_type = "sim_mujoco_g1" + # MujocoSimModule's ``odom`` Out is the sole producer of ``/odom`` # now - the coordinator no longer polls the whole-body adapter for # base pose (read_odom was dropped from the Protocol). autoconnect # maps ``(odom, PoseStamped)`` to ``/odom`` by default; no override. - _adapter_type = "sim_mujoco_g1" _tick_rate = 50.0 _auto_arm = True _auto_dry_run = False @@ -443,6 +458,7 @@ def _g1_real_costmap(grid: Any) -> Any: "world/camera_info": None, "world/depth_image": None, "world/depth_camera_info": None, + "world/lidar": None, "world/coordinator_joint_state": g1_urdf_joint_state(root_path=_G1_ROOT), "world/global_costmap": g1_costmap, "world/navigation_costmap": g1_costmap, @@ -470,7 +486,6 @@ def _g1_real_costmap(grid: Any) -> Any: _rerun_config["visual_override"]["world/global_costmap"] = _g1_real_costmap _rerun_config["visual_override"]["world/navigation_costmap"] = _g1_real_costmap # Raw scan is sensor-frame (LIO contract); the voxel map is the live view. - _rerun_config["visual_override"]["world/lidar"] = None def _viewer() -> Any: diff --git a/dimos/simulation/providers.py b/dimos/simulation/providers.py new file mode 100644 index 0000000000..98549f9783 --- /dev/null +++ b/dimos/simulation/providers.py @@ -0,0 +1,63 @@ +# 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. + +from __future__ import annotations + +from dataclasses import dataclass +import importlib.metadata as importlib_metadata +from pathlib import Path +from typing import Protocol, runtime_checkable + +from dimos.core.coordination.blueprints import Blueprint + +ENTRY_POINT_GROUP = "dimos.simulation.providers" + + +@dataclass(frozen=True) +class SimulationRequest: + robot_model: str + model_path: str | Path + mesh_dir: str | Path + scene_package: str | Path | None + + +@dataclass(frozen=True) +class SimulationBinding: + backend: Blueprint + adapter_type: str + adapter_address: str | Path + + +@runtime_checkable +class SimulationProvider(Protocol): + def build(self, request: SimulationRequest) -> SimulationBinding: ... + + +def load_simulation_provider(name: str) -> SimulationProvider: + matches = list(importlib_metadata.entry_points(group=ENTRY_POINT_GROUP, name=name)) + if not matches: + available = sorted( + entry_point.name + for entry_point in importlib_metadata.entry_points(group=ENTRY_POINT_GROUP) + ) + suffix = f" Available providers: {', '.join(available)}." if available else "" + raise ValueError(f"Simulation provider {name!r} is not installed.{suffix}") + if len(matches) > 1: + raise ValueError(f"Simulation provider {name!r} is registered more than once") + provider = matches[0].load() + if not isinstance(provider, SimulationProvider): + raise TypeError( + f"Simulation provider {name!r} must implement SimulationProvider, got {provider!r}" + ) + return provider diff --git a/dimos/simulation/test_providers.py b/dimos/simulation/test_providers.py new file mode 100644 index 0000000000..8d2cd4c6ed --- /dev/null +++ b/dimos/simulation/test_providers.py @@ -0,0 +1,42 @@ +# 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. + +from typing import Any + +import pytest + +from dimos.simulation import providers +from dimos.simulation.providers import SimulationBinding, SimulationRequest + + +class _Provider: + def build(self, request: SimulationRequest) -> SimulationBinding: + raise NotImplementedError + + +class _EntryPoint: + name = "test" + + def load(self) -> Any: + return _Provider() + + +def test_load_external_simulation_provider(monkeypatch: pytest.MonkeyPatch) -> None: + def entry_points(*, group: str, name: str | None = None) -> list[_EntryPoint]: + assert group == providers.ENTRY_POINT_GROUP + return [_EntryPoint()] if name in (None, "test") else [] + + monkeypatch.setattr(providers.importlib_metadata, "entry_points", entry_points) + + assert isinstance(providers.load_simulation_provider("test"), _Provider) From 410ee007c8f300eb90a3ddfbb3a97e723289fb0b Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Fri, 24 Jul 2026 14:43:56 +0800 Subject: [PATCH 02/33] Unify G1 real and simulated perception stacks --- .../g1/blueprints/basic/groot_wbc_platform.py | 94 ++++ .../blueprints/basic/unitree_g1_groot_wbc.py | 491 ++++-------------- dimos/simulation/providers.py | 30 ++ 3 files changed, 233 insertions(+), 382 deletions(-) create mode 100644 dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py diff --git a/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py b/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py new file mode 100644 index 0000000000..64dec1b8f1 --- /dev/null +++ b/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py @@ -0,0 +1,94 @@ +# 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. + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +from dimos.core.coordination.blueprints import Blueprint +from dimos.core.global_config import global_config +from dimos.simulation.providers import SimulationRequest, load_simulation_provider +from dimos.utils.data import LfsPath + +_ROBOT_ONLY_MJCF_PATH = Path(__file__).resolve().parents[2] / "assets" / "g1_29dof.xml" +_ROBOT_MESHDIR = LfsPath("g1_urdf/meshes") + + +@dataclass(frozen=True) +class G1GrootPlatform: + backend: Blueprint + adapter_type: str + adapter_address: str | Path + tick_rate: float + policy_decimation: int + auto_arm: bool + auto_dry_run: bool + ramp_seconds: float + n_workers: int + pointlio_config: dict[str, Any] + + +def resolve_g1_groot_platform() -> G1GrootPlatform: + if not global_config.simulation: + from dimos.robot.unitree.g1.wholebody_connection import G1WholeBodyConnection + + return G1GrootPlatform( + backend=G1WholeBodyConnection.blueprint(release_sport_mode=True), + adapter_type="transport_lcm", + adapter_address="", + tick_rate=100.0, + policy_decimation=2, + auto_arm=False, + auto_dry_run=True, + ramp_seconds=10.0, + n_workers=10, + pointlio_config={}, + ) + + if global_config.simulation != "mujoco": + raise ValueError("unitree-g1-groot-wbc only supports --simulation mujoco") + if not global_config.simulation_provider: + raise ValueError("unitree-g1-groot-wbc simulation requires --simulation-provider pimsim") + + provider = load_simulation_provider(global_config.simulation_provider) + binding = provider.build( + SimulationRequest( + robot_model="unitree_g1", + model_path=_ROBOT_ONLY_MJCF_PATH, + mesh_dir=_ROBOT_MESHDIR, + scene_package=global_config.scene_package, + ) + ) + mid360 = binding.require_device("mid360") + if mid360.protocol != "livox_mid360": + raise ValueError( + f"unitree-g1-groot-wbc requires a livox_mid360 device, got {mid360.protocol!r}" + ) + return G1GrootPlatform( + backend=binding.backend, + adapter_type=binding.adapter_type, + adapter_address=binding.adapter_address, + tick_rate=50.0, + policy_decimation=1, + auto_arm=True, + auto_dry_run=False, + ramp_seconds=0.0, + n_workers=10, + pointlio_config={ + "host_ip": mid360.host_address, + "lidar_ip": mid360.device_address, + }, + ) diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py index 9876effc71..49200645f7 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py @@ -12,36 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Unitree G1 GR00T whole-body-control blueprint. - -One blueprint, ``--simulation`` flag picks the backend: - -Real hardware (default): - G1WholeBodyConnection (DDS rt/lowstate <-> rt/lowcmd) + transport_lcm - whole-body adapter. 500 Hz tick. Safety profile: unarmed + dry-run on - start; activate explicitly through ControlCoordinator RPC after - verifying commands. The policy ramps from the current pose to its - bent-knee default over 10 s before taking torque control. The 14 arm - joints are held at the relaxed GR00T-trained default via a lower-priority - servo task. - -Sim (``--simulation``): - MujocoSimModule (in-process MuJoCo + SHM) + sim_mujoco_g1 adapter. - 50 Hz tick (matches the rate the policy was trained at). No arming - ramp and no dry-run. The 14 arm joints are still held with the same - lower-priority servo task as hardware so headless and viewer runs do not - depend on incidental startup timing. +"""Unitree G1 GR00T whole-body control, mapping, and navigation. + +The module graph above the hardware boundary is identical in real and simulated +runs. A simulation provider replaces the G1 and MID360 devices without replacing +PointLIO, mapping, navigation, control, or visualization. Usage: - dimos run unitree-g1-groot-wbc # real hardware - dimos --simulation mujoco run unitree-g1-groot-wbc # sim - dimos --simulation mujoco --scene-package none run unitree-g1-groot-wbc - dimos --simulation mujoco --scene-package office run unitree-g1-groot-wbc - dimos --simulation mujoco --scene-package supermarket run unitree-g1-groot-wbc - -Overrides (replace the old env-var dance): - dimos run unitree-g1-groot-wbc \\ - -o g1wholebodyconnection.network_interface=enp2s0 + dimos run unitree-g1-groot-wbc + dimos --simulation mujoco --simulation-provider pimsim \ + --scene-package office run unitree-g1-groot-wbc """ from __future__ import annotations @@ -62,9 +42,11 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.core.transport import LCMTransport +from dimos.hardware.sensors.lidar.pointlio.module import PointLio from dimos.hardware.whole_body.spec import WholeBodyConfig from dimos.mapping.costmapper import CostMapper from dimos.mapping.pointclouds.occupancy import HeightCostConfig +from dimos.mapping.ray_tracing.module import RayTracingVoxelMap from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.nav_msgs.Path import Path as NavPath from dimos.msgs.sensor_msgs.Imu import Imu @@ -72,305 +54,60 @@ from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.replanning_a_star.module import ReplanningAStarPlanner +from dimos.robot.unitree.g1.blueprints.basic.groot_wbc_platform import ( + resolve_g1_groot_platform, +) from dimos.robot.unitree.g1.config import G1 from dimos.robot.unitree.g1.g1_rerun import ( - G1_RERUN_ROOT, g1_costmap, g1_urdf_joint_state, g1_urdf_static_robot, ) -from dimos.simulation.providers import SimulationRequest, load_simulation_provider -from dimos.simulation.scene_assets.spec import ScenePackage from dimos.utils.data import LfsPath from dimos.visualization.rerun.scene_package import scene_package_static_entities from dimos.visualization.vis_module import vis_module -# Lazy data handles. LfsPath only triggers the LFS pull on first -# str()/open(); using ``get_data(...)`` at import time would block the -# whole CLI on a multi-GB download every time the module is imported. _GROOT_MODEL_DIR = LfsPath("groot") -_MJCF_PATH = LfsPath("mujoco_sim/g1_gear_wbc.xml") -_ROBOT_ONLY_MJCF_PATH = Path(__file__).resolve().parents[2] / "assets" / "g1_29dof.xml" -_ROBOT_MESHDIR = LfsPath("g1_urdf/meshes") - -_adapter_address: str | Path -_cmd_vel_topic = "/cmd_vel" if global_config.simulation else "/g1/cmd_vel" -_MUJOCO_LIDAR_CAMERAS = ( - "lidar_front_camera", - "lidar_left_camera", - "lidar_right_camera", -) -_MUJOCO_LIDAR_CAMERA = _MUJOCO_LIDAR_CAMERAS[0] -_G1_NUM_MOTORS = len(g1_joints) -# Robot geoms occupy groups 0/1. The legacy floor uses group 2, and cooked -# scene packages/entities use group 3, so lidar should render world geometry. -_MUJOCO_LIDAR_GEOM_GROUPS = (2, 3) -assert G1.height_clearance is not None and G1.width_clearance is not None -_MUJOCO_LIDAR_BASE_KWARGS: dict[str, Any] = { - "width": 320, - "height": 240, - "fps": 2, - "enable_color": False, - "enable_depth": False, - "enable_pointcloud": True, - "pointcloud_fps": 1.0, - "enable_mujoco_lidar": True, - "mujoco_lidar_geom_groups": list(_MUJOCO_LIDAR_GEOM_GROUPS), - "mujoco_lidar_raycast_width": 64, - "mujoco_lidar_raycast_height": 32, - "mujoco_lidar_robot_exclusion_radius": G1.width_clearance, -} -_G1_COMPOSED_MJB_KEY = "unitree-g1-groot-wbc_spawn_9p2_11p8_yaw_m1p57_static_only_lidar" -_G1_COMPOSED_MJB_ROBOT = "unitree-g1-groot-wbc" -_G1_COMPOSED_MJB_ENTITY_POLICY = "static-only" -_G1_NAV_VOXEL_RESOLUTION = 0.05 -# go2 nav_3d resolution; 0.05 saturates the raytracer on the Orin. -_G1_REAL_NAV_VOXEL_RESOLUTION = 0.08 -_G1_NAV_OVERHEAD_SAFETY_MARGIN = 0.2 -_G1_NAV_MAX_STEP_HEIGHT = 0.10 -_G1_NAV_ROTATION_DIAMETER = 0.8 -_G1_NAV_SAFE_RADIUS_MARGIN = 0.6 - - -def _mujoco_lidar_kwargs(camera_name: str, camera_names: tuple[str, ...]) -> dict[str, Any]: - return { - "camera_name": camera_name, - "mujoco_lidar_camera_names": list(camera_names), - **_MUJOCO_LIDAR_BASE_KWARGS, - } - - -if global_config.simulation and global_config.simulation != "mujoco": - raise ValueError("unitree-g1-groot-wbc only supports --simulation mujoco") - -if global_config.simulation == "mujoco": - from dimos.mapping.voxels import VoxelGridMapper - from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule - from dimos.simulation.engines.robot_sim_binding import ( - RobotSimSpec, - mjcf_joint_names_from_hardware, - ) +_NAV_VOXEL_RESOLUTION = 0.08 +_NAV_OVERHEAD_SAFETY_MARGIN = 0.2 +_NAV_MAX_STEP_HEIGHT = 0.10 +_NAV_ROTATION_DIAMETER = 0.8 +_NAV_SAFE_RADIUS_MARGIN = 0.6 +_RERUN_ROOT = "world/odometry/g1" +_URDF_PATH = Path(__file__).resolve().parents[2] / "g1.urdf" +_NOMINAL_PELVIS_Z = 0.74 +_pelvis_mid360_cache: list[Any] = [] - _g1_sim_joints = tuple(g1_joints) - _g1_sim_spec = RobotSimSpec( - robot_id="g1", - hardware_joints=_g1_sim_joints, - root_body_names=("pelvis",), - root_joint_names=("floating_base_joint",), - require_floating_base=True, - model_joint_names=mjcf_joint_names_from_hardware(_g1_sim_joints), - imu_gyro_names=( - "imu-pelvis-angular-velocity", - "imu-torso-angular-velocity", - "imu-angular-velocity", - "gyro_pelvis", - "imu_gyro", - ), - imu_accel_names=( - "imu-pelvis-linear-acceleration", - "imu-torso-linear-acceleration", - "imu-linear-acceleration", - "accelerometer_pelvis", - "imu_accel", - ), - require_imu=True, - ) +assert G1.height_clearance is not None and G1.width_clearance is not None - def _legacy_mujoco_backend() -> Any: - return MujocoSimModule.blueprint( - address=_MJCF_PATH, - headless=True, - dof=_G1_NUM_MOTORS, - **_mujoco_lidar_kwargs(_MUJOCO_LIDAR_CAMERA, _MUJOCO_LIDAR_CAMERAS), - inject_legacy_assets=True, - robot_sim_spec=_g1_sim_spec, - ) - - def _scene_mujoco_backend() -> tuple[Any, str | Path]: - if global_config.scene_package is None: - return _legacy_mujoco_backend(), _MJCF_PATH - - scene_path = Path(str(global_config.scene_package)).expanduser() - if scene_path.suffix.lower() == ".mjb": - if not scene_path.exists(): - raise FileNotFoundError(f"MuJoCo binary scene not found: {scene_path}") - return ( - MujocoSimModule.blueprint( - address=scene_path, - headless=True, - dof=_G1_NUM_MOTORS, - **_mujoco_lidar_kwargs(_MUJOCO_LIDAR_CAMERA, _MUJOCO_LIDAR_CAMERAS), - robot_sim_spec=_g1_sim_spec, - ), - scene_path, - ) - - from dimos.simulation.scenes.catalog import resolve_scene_package - - package = resolve_scene_package(global_config.scene_package) - if package is None: - return _legacy_mujoco_backend(), _MJCF_PATH - if package.mujoco_scene_path is None: - raise ValueError(f"scene package has no MuJoCo scene artifact: {package.metadata_path}") - - composed_scene = _precomposed_g1_scene(package) - if composed_scene is not None: - return ( - MujocoSimModule.blueprint( - address=composed_scene, - headless=True, - dof=_G1_NUM_MOTORS, - **_mujoco_lidar_kwargs(_MUJOCO_LIDAR_CAMERA, _MUJOCO_LIDAR_CAMERAS), - robot_sim_spec=_g1_sim_spec, - ), - composed_scene, - ) - - return ( - MujocoSimModule.blueprint( - scene_xml=package.mujoco_scene_path, - robot_mjcf=_ROBOT_ONLY_MJCF_PATH, - robot_meshdir=_ROBOT_MESHDIR, - robot_id="", - scene_entities=package.entities, - headless=True, - dof=_G1_NUM_MOTORS, - **_mujoco_lidar_kwargs(_MUJOCO_LIDAR_CAMERA, _MUJOCO_LIDAR_CAMERAS), - robot_sim_spec=_g1_sim_spec, - ), - _ROBOT_ONLY_MJCF_PATH, - ) - - def _precomposed_g1_scene(package: ScenePackage) -> Path | None: - candidate = package.mujoco_composed_binary_path( - key=_G1_COMPOSED_MJB_KEY, - robot=_G1_COMPOSED_MJB_ROBOT, - entity_policy=_G1_COMPOSED_MJB_ENTITY_POLICY, - ) - if candidate is None: - return None - if not candidate.exists(): - raise FileNotFoundError( - f"scene package declares a composed MuJoCo binary that is missing: {candidate}" - ) - return candidate - - if global_config.simulation_provider: - _provider = load_simulation_provider(global_config.simulation_provider) - _binding = _provider.build( - SimulationRequest( - robot_model="unitree_g1", - model_path=_ROBOT_ONLY_MJCF_PATH, - mesh_dir=_ROBOT_MESHDIR, - scene_package=global_config.scene_package, - ) - ) - _backend = _binding.backend - _adapter_address = _binding.adapter_address - _adapter_type = _binding.adapter_type - else: - _backend, _adapter_address = _scene_mujoco_backend() - _adapter_type = "sim_mujoco_g1" - - # MujocoSimModule's ``odom`` Out is the sole producer of ``/odom`` - # now - the coordinator no longer polls the whole-body adapter for - # base pose (read_odom was dropped from the Protocol). autoconnect - # maps ``(odom, PoseStamped)`` to ``/odom`` by default; no override. - _tick_rate = 50.0 - _auto_arm = True - _auto_dry_run = False - _default_ramp_seconds = 0.0 - _decimation: int | None = 1 - _n_workers = 2 # sim: keep the default worker count - _arm_holder = TaskConfig( - name="servo_arms", - type="servo", - joint_names=g1_arms, - priority=10, - auto_start=True, - params={"default_positions": ARM_DEFAULT_POSE}, - ) - _mapper = VoxelGridMapper.blueprint(emit_every=1) - _nav_stack = autoconnect( - _mapper, - CostMapper.blueprint( - config=HeightCostConfig( - resolution=_G1_NAV_VOXEL_RESOLUTION, - can_pass_under=G1.height_clearance + _G1_NAV_OVERHEAD_SAFETY_MARGIN, - can_climb=_G1_NAV_MAX_STEP_HEIGHT, - ), - initial_safe_radius_meters=G1.width_clearance + _G1_NAV_SAFE_RADIUS_MARGIN, +_platform = resolve_g1_groot_platform() + +_navigation = autoconnect( + PointLio.blueprint(**_platform.pointlio_config), + RayTracingVoxelMap.blueprint( + voxel_size=_NAV_VOXEL_RESOLUTION, + emit_every=0, + global_emit_every=4, + max_health=10, + graze_cos=0.85, + ), + CostMapper.blueprint( + config=HeightCostConfig( + resolution=_NAV_VOXEL_RESOLUTION, + can_pass_under=G1.height_clearance + _NAV_OVERHEAD_SAFETY_MARGIN, + can_climb=_NAV_MAX_STEP_HEIGHT, ), - ReplanningAStarPlanner.blueprint( - robot_width=G1.width_clearance, - robot_rotation_diameter=_G1_NAV_ROTATION_DIAMETER, - ), - MovementManager.blueprint(), - ) - _remappings = [ - (VoxelGridMapper, "lidar", "pointcloud"), - (ControlCoordinator, "twist_command", "cmd_vel"), - ] -else: - from dimos.hardware.sensors.lidar.pointlio.module import PointLio - from dimos.mapping.ray_tracing.module import RayTracingVoxelMap - from dimos.robot.unitree.g1.wholebody_connection import G1WholeBodyConnection - - # Real-hw backend: DDS connection module + transport_lcm adapter. - _backend = G1WholeBodyConnection.blueprint(release_sport_mode=True) - _adapter_type = "transport_lcm" - _adapter_address = "" - # The onboard Jetson can't sustain a 500 Hz tick; it collapses to ~90 Hz - # and starves the policy, so balance decays. - _tick_rate = 100.0 - # Real hardware: come up unarmed + dry-run; operator must click - # Activate (10 s ramp) after verifying commands. - _auto_arm = False - _auto_dry_run = True - _default_ramp_seconds = 10.0 - _decimation = 2 # 100 Hz tick / 2 = 50 Hz policy (training + sim rate). - # One process per heavy module; fewer workers starve the Rerun bridge. - _n_workers = 10 - # Real hardware needs the arms held -- kd damping alone would let - # them sag toward singular configurations between trajectories. - _arm_holder = TaskConfig( - name="servo_arms", - type="servo", - joint_names=g1_arms, - priority=10, - auto_start=True, - params={"default_positions": ARM_DEFAULT_POSE}, - ) - # Same nav middle as unitree-g1-nav-simple, fed by Point-LIO from the - # MID-360, executed through the coordinator's twist_command. - _nav_stack = autoconnect( - PointLio.blueprint(), - RayTracingVoxelMap.blueprint( - voxel_size=_G1_REAL_NAV_VOXEL_RESOLUTION, - emit_every=0, # no local_map consumer here - global_emit_every=4, # ~1 Hz global map; also paces the costmap - # Clearing matched to go2 nav_3d. - max_health=10, - graze_cos=0.85, - ), - CostMapper.blueprint( - config=HeightCostConfig( - resolution=_G1_REAL_NAV_VOXEL_RESOLUTION, - can_pass_under=G1.height_clearance + _G1_NAV_OVERHEAD_SAFETY_MARGIN, - can_climb=_G1_NAV_MAX_STEP_HEIGHT, - ), - initial_safe_radius_meters=G1.width_clearance + _G1_NAV_SAFE_RADIUS_MARGIN, - ), - ReplanningAStarPlanner.blueprint( - robot_width=G1.width_clearance, - robot_rotation_diameter=_G1_NAV_ROTATION_DIAMETER, - ), - MovementManager.blueprint(), - ) - _remappings = [(ControlCoordinator, "twist_command", "cmd_vel")] + initial_safe_radius_meters=G1.width_clearance + _NAV_SAFE_RADIUS_MARGIN, + ), + ReplanningAStarPlanner.blueprint( + robot_width=G1.width_clearance, + robot_rotation_diameter=_NAV_ROTATION_DIAMETER, + ), + MovementManager.blueprint(), +) -def _g1_groot_rerun_blueprint() -> Any: +def _rerun_blueprint() -> Any: import rerun as rr import rerun.blueprint as rrb @@ -387,86 +124,78 @@ def _g1_groot_rerun_blueprint() -> Any: ) -def _g1_nav_path(path: NavPath) -> Any: +def _nav_path(path: NavPath) -> Any: return path.to_rerun(z_offset=0.3) -# Mesh root: sim roots under the /odom transform; real hw under the LIO's -# /odometry, whose world frame is the lidar boot pose (ground ~1.2 m below 0). -_G1_ROOT = G1_RERUN_ROOT if global_config.simulation == "mujoco" else "world/odometry/g1" - -_G1_URDF_PATH = Path(__file__).resolve().parents[2] / "g1.urdf" -# Nominal standing pelvis height; matches G1GrootWBCTask's height_cmd. -_G1_NOMINAL_PELVIS_Z = 0.74 -_g1_pelvis_mid360_cache: list[Any] = [] - - -def _g1_pelvis_to_mid360() -> Any: - """Rest-pose pelvis->mid360_link transform from the G1 URDF (cached).""" - if not _g1_pelvis_mid360_cache: +def _pelvis_to_mid360() -> Any: + if not _pelvis_mid360_cache: from importlib import import_module import numpy as np - urdf = import_module("yourdfpy").URDF.load(str(_G1_URDF_PATH), load_meshes=False) + urdf = import_module("yourdfpy").URDF.load(str(_URDF_PATH), load_meshes=False) urdf.update_cfg(np.zeros(len(urdf.actuated_joint_names))) - _g1_pelvis_mid360_cache.append(urdf.get_transform("mid360_link", "pelvis")) - return _g1_pelvis_mid360_cache[0] + _pelvis_mid360_cache.append(urdf.get_transform("mid360_link", "pelvis")) + return _pelvis_mid360_cache[0] -def _g1_real_odometry_root(odom: Any) -> Any: - """Robot-mesh root: pelvis pose from the LIO's mid360 odometry (rest offset).""" +def _odometry_root(odometry: Any) -> Any: import numpy as np import rerun as rr from dimos.msgs.geometry_msgs.Quaternion import Quaternion - t_world_mid360 = np.eye(4) - # The MID-360 is mounted upside down (the URDF doesn't carry the flip): - # un-roll by Rx(pi) == diag(1, -1, -1). - t_world_mid360[:3, :3] = odom.orientation.to_rotation_matrix() @ np.diag([1.0, -1.0, -1.0]) - t_world_mid360[:3, 3] = (odom.x, odom.y, odom.z) - t_world_pelvis = t_world_mid360 @ np.linalg.inv(_g1_pelvis_to_mid360()) - q = Quaternion.from_rotation_matrix(t_world_pelvis[:3, :3]) + world_mid360 = np.eye(4) + world_mid360[:3, :3] = odometry.orientation.to_rotation_matrix() @ np.diag([1.0, -1.0, -1.0]) + world_mid360[:3, 3] = (odometry.x, odometry.y, odometry.z) + world_pelvis = world_mid360 @ np.linalg.inv(_pelvis_to_mid360()) + quaternion = Quaternion.from_rotation_matrix(world_pelvis[:3, :3]) return rr.Transform3D( - translation=t_world_pelvis[:3, 3].tolist(), - rotation=rr.Quaternion(xyzw=[q.x, q.y, q.z, q.w]), + translation=world_pelvis[:3, 3].tolist(), + rotation=rr.Quaternion(xyzw=[quaternion.x, quaternion.y, quaternion.z, quaternion.w]), ) -def _g1_real_ground_z() -> float: - """Ground height in the LIO boot frame: -(mount z + nominal pelvis z).""" - return -(float(_g1_pelvis_to_mid360()[2, 3]) + _G1_NOMINAL_PELVIS_Z) +def _ground_z() -> float: + return -(float(_pelvis_to_mid360()[2, 3]) + _NOMINAL_PELVIS_Z) + + +def _costmap(grid: Any) -> Any: + return g1_costmap(grid, z_offset=_ground_z() + 0.02) -def _g1_real_costmap(grid: Any) -> Any: - """Costmap rendered on the actual ground plane of the boot frame.""" - return g1_costmap(grid, z_offset=_g1_real_ground_z() + 0.02) +def _scene_root(rr: Any) -> Any: + return rr.Transform3D(translation=[0.0, 0.0, _ground_z()]) -_static_rerun_entities: dict[str, Any] = { - _G1_ROOT: g1_urdf_static_robot(root_path=_G1_ROOT), +_static_entities: dict[str, Any] = { + _RERUN_ROOT: g1_urdf_static_robot(root_path=_RERUN_ROOT), } -_static_rerun_entities.update(scene_package_static_entities(global_config.scene_package)) +_scene_entities = scene_package_static_entities( + global_config.scene_package, + entity_path="world/scene/visual", +) +if _scene_entities: + _static_entities["world/scene"] = _scene_root + _static_entities.update(_scene_entities) _rerun_config: dict[str, Any] = { - "blueprint": _g1_groot_rerun_blueprint, + "blueprint": _rerun_blueprint, "visual_override": { - # This blueprint uses raycast lidar, so suppress raw camera streams - # in Rerun. "world/color_image": None, "world/camera_info": None, "world/depth_image": None, "world/depth_camera_info": None, "world/lidar": None, - "world/coordinator_joint_state": g1_urdf_joint_state(root_path=_G1_ROOT), - "world/global_costmap": g1_costmap, - "world/navigation_costmap": g1_costmap, - "world/path": _g1_nav_path, + "world/coordinator_joint_state": g1_urdf_joint_state(root_path=_RERUN_ROOT), + "world/odometry": _odometry_root, + "world/global_costmap": _costmap, + "world/navigation_costmap": _costmap, + "world/path": _nav_path, }, "max_hz": { "world/coordinator_joint_state": 20.0, - # Raw state streams arrive at ~440 Hz; useful only as debug plots. "world/g1/imu": 10.0, "world/g1/motor_states": 10.0, "world/g1/motor_command": 10.0, @@ -474,33 +203,25 @@ def _g1_real_costmap(grid: Any) -> Any: "world/global_map": 1.0, "world/global_costmap": 2.0, "world/navigation_costmap": 2.0, - # The planner publishes an empty Path() immediately before the new - # planned path. Throttling this entity drops the real path. "world/path": 0, }, - "static": _static_rerun_entities, + "static": _static_entities, } -if global_config.simulation != "mujoco": - _rerun_config["visual_override"]["world/odometry"] = _g1_real_odometry_root - _rerun_config["visual_override"]["world/global_costmap"] = _g1_real_costmap - _rerun_config["visual_override"]["world/navigation_costmap"] = _g1_real_costmap - # Raw scan is sensor-frame (LIO contract); the voxel map is the live view. - def _viewer() -> Any: return vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config) _coordinator = ControlCoordinator.blueprint( - tick_rate=_tick_rate, + tick_rate=_platform.tick_rate, hardware=[ HardwareComponent( hardware_id="g1", hardware_type=HardwareType.WHOLE_BODY, joints=g1_joints, - adapter_type=_adapter_type, - address=_adapter_address, + adapter_type=_platform.adapter_type, + address=_platform.adapter_address, wb_config=WholeBodyConfig(kp=tuple(G1_GROOT_KP), kd=tuple(G1_GROOT_KD)), ), ], @@ -514,30 +235,36 @@ def _viewer() -> Any: params={ "model_path": _GROOT_MODEL_DIR, "hardware_id": "g1", - "auto_arm": _auto_arm, - "auto_dry_run": _auto_dry_run, - "default_ramp_seconds": _default_ramp_seconds, - "decimation": _decimation, + "auto_arm": _platform.auto_arm, + "auto_dry_run": _platform.auto_dry_run, + "default_ramp_seconds": _platform.ramp_seconds, + "decimation": _platform.policy_decimation, }, ), - *([_arm_holder] if _arm_holder is not None else []), + TaskConfig( + name="servo_arms", + type="servo", + joint_names=g1_arms, + priority=10, + auto_start=True, + params={"default_positions": ARM_DEFAULT_POSE}, + ), ], ).transports( { ("joint_command", JointState): LCMTransport("/g1/joint_command", JointState), - ("cmd_vel", Twist): LCMTransport(_cmd_vel_topic, Twist), - # Real-hw only: the transport_lcm adapter speaks to - # G1WholeBodyConnection over these topics. autoconnect already - # matches by (name, type) so sim doesn't need them -- they're - # harmless when the sim engine doesn't expose those ports. + ("cmd_vel", Twist): LCMTransport("/g1/cmd_vel", Twist), ("motor_states", JointState): LCMTransport("/g1/motor_states", JointState), ("imu", Imu): LCMTransport("/g1/imu", Imu), - ("motor_command", MotorCommandArray): LCMTransport("/g1/motor_command", MotorCommandArray), + ("motor_command", MotorCommandArray): LCMTransport( + "/g1/motor_command", + MotorCommandArray, + ), } ) unitree_g1_groot_wbc = ( - autoconnect(_backend, _coordinator, _nav_stack, _viewer()) - .remappings(cast("Any", _remappings)) - .global_config(robot_model="unitree_g1", n_workers=_n_workers) + autoconnect(_platform.backend, _coordinator, _navigation, _viewer()) + .remappings(cast("Any", [(ControlCoordinator, "twist_command", "cmd_vel")])) + .global_config(robot_model="unitree_g1", n_workers=_platform.n_workers) ) diff --git a/dimos/simulation/providers.py b/dimos/simulation/providers.py index 98549f9783..eebf65a187 100644 --- a/dimos/simulation/providers.py +++ b/dimos/simulation/providers.py @@ -32,11 +32,41 @@ class SimulationRequest: scene_package: str | Path | None +@dataclass(frozen=True) +class SimulationDeviceEndpoint: + device_id: str + protocol: str + host_address: str + device_address: str + + def __post_init__(self) -> None: + if not all( + value.strip() + for value in ( + self.device_id, + self.protocol, + self.host_address, + self.device_address, + ) + ): + raise ValueError("simulation device endpoint fields must not be empty") + + @dataclass(frozen=True) class SimulationBinding: backend: Blueprint adapter_type: str adapter_address: str | Path + devices: tuple[SimulationDeviceEndpoint, ...] = () + + def require_device(self, device_id: str) -> SimulationDeviceEndpoint: + matches = tuple(device for device in self.devices if device.device_id == device_id) + if len(matches) != 1: + raise ValueError( + f"simulation binding requires exactly one {device_id!r} device, " + f"found {len(matches)}" + ) + return matches[0] @runtime_checkable From 41aaa927650953b14f0f2cd40a5f899803e8f13d Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Fri, 24 Jul 2026 20:49:41 +0800 Subject: [PATCH 03/33] Relay native PointLIO output to Zenoh --- dimos/robot/all_blueprints.py | 1 + .../blueprints/basic/pointlio_zenoh_relay.py | 64 +++++++++++++++++++ .../blueprints/basic/unitree_g1_groot_wbc.py | 7 ++ 3 files changed, 72 insertions(+) create mode 100644 dimos/robot/unitree/g1/blueprints/basic/pointlio_zenoh_relay.py diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 9194f48e92..dbd5ea42d1 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -263,6 +263,7 @@ "phone-teleop-module": "dimos.teleop.phone.phone_teleop_module.PhoneTeleopModule", "pick-and-place-module": "dimos.manipulation.pick_and_place_module.PickAndPlaceModule", "point-lio": "dimos.hardware.sensors.lidar.pointlio.module.PointLio", + "point-lio-zenoh-relay": "dimos.robot.unitree.g1.blueprints.basic.pointlio_zenoh_relay.PointLioZenohRelay", "pointlio-recorder": "dimos.hardware.sensors.lidar.pointlio.recorder.PointlioRecorder", "quest-teleop-module": "dimos.teleop.quest.quest_teleop_module.QuestTeleopModule", "ray-tracing-voxel-map": "dimos.mapping.ray_tracing.module.RayTracingVoxelMap", diff --git a/dimos/robot/unitree/g1/blueprints/basic/pointlio_zenoh_relay.py b/dimos/robot/unitree/g1/blueprints/basic/pointlio_zenoh_relay.py new file mode 100644 index 0000000000..479fa517cc --- /dev/null +++ b/dimos/robot/unitree/g1/blueprints/basic/pointlio_zenoh_relay.py @@ -0,0 +1,64 @@ +# 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. + +from __future__ import annotations + +from reactivex.disposable import Disposable + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.protocol.pubsub.impl.lcmpubsub import LCMPubSubBase, Topic as LCMTopic +from dimos.protocol.pubsub.impl.zenohpubsub import ( + QOS_LATEST_WINS, + Topic as ZenohTopic, + ZenohPubSubBase, +) + +_LIDAR_CHANNEL = "dimos/lidar/sensor_msgs.PointCloud2" +_ODOMETRY_CHANNEL = "dimos/odometry/nav_msgs.Odometry" + + +class PointLioZenohRelay(Module): + config: ModuleConfig + + @rpc + def start(self) -> None: + super().start() + lcm = LCMPubSubBase() + zenoh = ZenohPubSubBase() + lcm.start() + zenoh.start() + + lidar = ZenohTopic("dimos/lidar", PointCloud2, qos=QOS_LATEST_WINS) + odometry = ZenohTopic("dimos/odometry", Odometry) + self.register_disposable( + Disposable( + lcm.subscribe( + LCMTopic(_LIDAR_CHANNEL), + lambda payload, _: zenoh.publish(lidar, payload), + ) + ) + ) + self.register_disposable( + Disposable( + lcm.subscribe( + LCMTopic(_ODOMETRY_CHANNEL), + lambda payload, _: zenoh.publish(odometry, payload), + ) + ) + ) + self.register_disposable(Disposable(lcm.stop)) + self.register_disposable(Disposable(zenoh.stop)) diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py index 49200645f7..fddaf4e9bc 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py @@ -57,6 +57,9 @@ from dimos.robot.unitree.g1.blueprints.basic.groot_wbc_platform import ( resolve_g1_groot_platform, ) +from dimos.robot.unitree.g1.blueprints.basic.pointlio_zenoh_relay import ( + PointLioZenohRelay, +) from dimos.robot.unitree.g1.config import G1 from dimos.robot.unitree.g1.g1_rerun import ( g1_costmap, @@ -81,8 +84,12 @@ assert G1.height_clearance is not None and G1.width_clearance is not None _platform = resolve_g1_groot_platform() +_pointlio_transport = ( + PointLioZenohRelay.blueprint() if global_config.transport == "zenoh" else autoconnect() +) _navigation = autoconnect( + _pointlio_transport, PointLio.blueprint(**_platform.pointlio_config), RayTracingVoxelMap.blueprint( voxel_size=_NAV_VOXEL_RESOLUTION, From 056cacc767413ecc4c0000491e803ea605628877 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Fri, 24 Jul 2026 21:02:23 +0800 Subject: [PATCH 04/33] Restore scene visual package alignment --- .../g1/blueprints/basic/unitree_g1_groot_wbc.py | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py index fddaf4e9bc..7daab92636 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py @@ -172,20 +172,10 @@ def _costmap(grid: Any) -> Any: return g1_costmap(grid, z_offset=_ground_z() + 0.02) -def _scene_root(rr: Any) -> Any: - return rr.Transform3D(translation=[0.0, 0.0, _ground_z()]) - - _static_entities: dict[str, Any] = { _RERUN_ROOT: g1_urdf_static_robot(root_path=_RERUN_ROOT), } -_scene_entities = scene_package_static_entities( - global_config.scene_package, - entity_path="world/scene/visual", -) -if _scene_entities: - _static_entities["world/scene"] = _scene_root - _static_entities.update(_scene_entities) +_static_entities.update(scene_package_static_entities(global_config.scene_package)) _rerun_config: dict[str, Any] = { "blueprint": _rerun_blueprint, From a9b02a29b3ccfb10b0a79f64467bf6a71cb605ff Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Fri, 24 Jul 2026 21:59:31 +0800 Subject: [PATCH 05/33] Restore provider-owned G1 localization --- .../g1/blueprints/basic/groot_wbc_platform.py | 34 +++++++++++------- .../blueprints/basic/unitree_g1_groot_wbc.py | 35 +++++++++---------- dimos/simulation/providers.py | 30 ---------------- 3 files changed, 37 insertions(+), 62 deletions(-) diff --git a/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py b/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py index 64dec1b8f1..e4cb743fa2 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py +++ b/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py @@ -12,13 +12,19 @@ # See the License for the specific language governing permissions and # limitations under the License. +"""Platform-owned control and localization inputs for the G1 GR00T stack. + +Hardware uses PointLIO for ``lidar`` and ``odometry``. A simulation provider +publishes those streams itself. Mapping and navigation remain outside this +boundary. +""" + from __future__ import annotations from dataclasses import dataclass from pathlib import Path -from typing import Any -from dimos.core.coordination.blueprints import Blueprint +from dimos.core.coordination.blueprints import Blueprint, autoconnect from dimos.core.global_config import global_config from dimos.simulation.providers import SimulationRequest, load_simulation_provider from dimos.utils.data import LfsPath @@ -30,6 +36,8 @@ @dataclass(frozen=True) class G1GrootPlatform: backend: Blueprint + localization_source: Blueprint + simulation: bool adapter_type: str adapter_address: str | Path tick_rate: float @@ -38,15 +46,23 @@ class G1GrootPlatform: auto_dry_run: bool ramp_seconds: float n_workers: int - pointlio_config: dict[str, Any] def resolve_g1_groot_platform() -> G1GrootPlatform: if not global_config.simulation: + from dimos.hardware.sensors.lidar.pointlio.module import PointLio + from dimos.robot.unitree.g1.blueprints.basic.pointlio_zenoh_relay import ( + PointLioZenohRelay, + ) from dimos.robot.unitree.g1.wholebody_connection import G1WholeBodyConnection + pointlio_transport = ( + PointLioZenohRelay.blueprint() if global_config.transport == "zenoh" else autoconnect() + ) return G1GrootPlatform( backend=G1WholeBodyConnection.blueprint(release_sport_mode=True), + localization_source=autoconnect(pointlio_transport, PointLio.blueprint()), + simulation=False, adapter_type="transport_lcm", adapter_address="", tick_rate=100.0, @@ -55,7 +71,6 @@ def resolve_g1_groot_platform() -> G1GrootPlatform: auto_dry_run=True, ramp_seconds=10.0, n_workers=10, - pointlio_config={}, ) if global_config.simulation != "mujoco": @@ -72,13 +87,10 @@ def resolve_g1_groot_platform() -> G1GrootPlatform: scene_package=global_config.scene_package, ) ) - mid360 = binding.require_device("mid360") - if mid360.protocol != "livox_mid360": - raise ValueError( - f"unitree-g1-groot-wbc requires a livox_mid360 device, got {mid360.protocol!r}" - ) return G1GrootPlatform( backend=binding.backend, + localization_source=autoconnect(), + simulation=True, adapter_type=binding.adapter_type, adapter_address=binding.adapter_address, tick_rate=50.0, @@ -87,8 +99,4 @@ def resolve_g1_groot_platform() -> G1GrootPlatform: auto_dry_run=False, ramp_seconds=0.0, n_workers=10, - pointlio_config={ - "host_ip": mid360.host_address, - "lidar_ip": mid360.device_address, - }, ) diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py index 7daab92636..6be968ec7f 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py @@ -14,9 +14,9 @@ """Unitree G1 GR00T whole-body control, mapping, and navigation. -The module graph above the hardware boundary is identical in real and simulated -runs. A simulation provider replaces the G1 and MID360 devices without replacing -PointLIO, mapping, navigation, control, or visualization. +Real hardware uses PointLIO to produce registered lidar and odometry. Simulation +provides the same typed localization boundary from simulator truth. Both feed +the same mapping, planning, control, and visualization modules. Usage: dimos run unitree-g1-groot-wbc @@ -42,7 +42,6 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.core.transport import LCMTransport -from dimos.hardware.sensors.lidar.pointlio.module import PointLio from dimos.hardware.whole_body.spec import WholeBodyConfig from dimos.mapping.costmapper import CostMapper from dimos.mapping.pointclouds.occupancy import HeightCostConfig @@ -57,9 +56,6 @@ from dimos.robot.unitree.g1.blueprints.basic.groot_wbc_platform import ( resolve_g1_groot_platform, ) -from dimos.robot.unitree.g1.blueprints.basic.pointlio_zenoh_relay import ( - PointLioZenohRelay, -) from dimos.robot.unitree.g1.config import G1 from dimos.robot.unitree.g1.g1_rerun import ( g1_costmap, @@ -84,13 +80,9 @@ assert G1.height_clearance is not None and G1.width_clearance is not None _platform = resolve_g1_groot_platform() -_pointlio_transport = ( - PointLioZenohRelay.blueprint() if global_config.transport == "zenoh" else autoconnect() -) _navigation = autoconnect( - _pointlio_transport, - PointLio.blueprint(**_platform.pointlio_config), + _platform.localization_source, RayTracingVoxelMap.blueprint( voxel_size=_NAV_VOXEL_RESOLUTION, emit_every=0, @@ -147,7 +139,7 @@ def _pelvis_to_mid360() -> Any: return _pelvis_mid360_cache[0] -def _odometry_root(odometry: Any) -> Any: +def _real_odometry_root(odometry: Any) -> Any: import numpy as np import rerun as rr @@ -164,12 +156,12 @@ def _odometry_root(odometry: Any) -> Any: ) -def _ground_z() -> float: +def _real_ground_z() -> float: return -(float(_pelvis_to_mid360()[2, 3]) + _NOMINAL_PELVIS_Z) -def _costmap(grid: Any) -> Any: - return g1_costmap(grid, z_offset=_ground_z() + 0.02) +def _real_costmap(grid: Any) -> Any: + return g1_costmap(grid, z_offset=_real_ground_z() + 0.02) _static_entities: dict[str, Any] = { @@ -178,6 +170,7 @@ def _costmap(grid: Any) -> Any: _static_entities.update(scene_package_static_entities(global_config.scene_package)) _rerun_config: dict[str, Any] = { + "memory_limit": "8GB" if _platform.simulation else "512MB", "blueprint": _rerun_blueprint, "visual_override": { "world/color_image": None, @@ -186,9 +179,8 @@ def _costmap(grid: Any) -> Any: "world/depth_camera_info": None, "world/lidar": None, "world/coordinator_joint_state": g1_urdf_joint_state(root_path=_RERUN_ROOT), - "world/odometry": _odometry_root, - "world/global_costmap": _costmap, - "world/navigation_costmap": _costmap, + "world/global_costmap": g1_costmap, + "world/navigation_costmap": g1_costmap, "world/path": _nav_path, }, "max_hz": { @@ -205,6 +197,11 @@ def _costmap(grid: Any) -> Any: "static": _static_entities, } +if not _platform.simulation: + _rerun_config["visual_override"]["world/odometry"] = _real_odometry_root + _rerun_config["visual_override"]["world/global_costmap"] = _real_costmap + _rerun_config["visual_override"]["world/navigation_costmap"] = _real_costmap + def _viewer() -> Any: return vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config) diff --git a/dimos/simulation/providers.py b/dimos/simulation/providers.py index eebf65a187..98549f9783 100644 --- a/dimos/simulation/providers.py +++ b/dimos/simulation/providers.py @@ -32,41 +32,11 @@ class SimulationRequest: scene_package: str | Path | None -@dataclass(frozen=True) -class SimulationDeviceEndpoint: - device_id: str - protocol: str - host_address: str - device_address: str - - def __post_init__(self) -> None: - if not all( - value.strip() - for value in ( - self.device_id, - self.protocol, - self.host_address, - self.device_address, - ) - ): - raise ValueError("simulation device endpoint fields must not be empty") - - @dataclass(frozen=True) class SimulationBinding: backend: Blueprint adapter_type: str adapter_address: str | Path - devices: tuple[SimulationDeviceEndpoint, ...] = () - - def require_device(self, device_id: str) -> SimulationDeviceEndpoint: - matches = tuple(device for device in self.devices if device.device_id == device_id) - if len(matches) != 1: - raise ValueError( - f"simulation binding requires exactly one {device_id!r} device, " - f"found {len(matches)}" - ) - return matches[0] @runtime_checkable From 235846e4423457a6c615af3222020e017c1cacc7 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Sat, 25 Jul 2026 00:19:03 +0800 Subject: [PATCH 06/33] Share PointLIO Zenoh relay --- .../sensors/lidar/pointlio/zenoh_relay.py} | 27 ++++++++++++++----- dimos/robot/all_blueprints.py | 2 +- .../g1/blueprints/basic/groot_wbc_platform.py | 8 +++--- .../blueprints/basic/unitree_g1_groot_wbc.py | 8 +++--- 4 files changed, 29 insertions(+), 16 deletions(-) rename dimos/{robot/unitree/g1/blueprints/basic/pointlio_zenoh_relay.py => hardware/sensors/lidar/pointlio/zenoh_relay.py} (69%) diff --git a/dimos/robot/unitree/g1/blueprints/basic/pointlio_zenoh_relay.py b/dimos/hardware/sensors/lidar/pointlio/zenoh_relay.py similarity index 69% rename from dimos/robot/unitree/g1/blueprints/basic/pointlio_zenoh_relay.py rename to dimos/hardware/sensors/lidar/pointlio/zenoh_relay.py index 479fa517cc..246faba7ed 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/pointlio_zenoh_relay.py +++ b/dimos/hardware/sensors/lidar/pointlio/zenoh_relay.py @@ -27,12 +27,14 @@ ZenohPubSubBase, ) -_LIDAR_CHANNEL = "dimos/lidar/sensor_msgs.PointCloud2" -_ODOMETRY_CHANNEL = "dimos/odometry/nav_msgs.Odometry" + +class PointLioZenohRelayConfig(ModuleConfig): + lidar_topic: str = "lidar" + odometry_topic: str = "odometry" class PointLioZenohRelay(Module): - config: ModuleConfig + config: PointLioZenohRelayConfig @rpc def start(self) -> None: @@ -42,12 +44,14 @@ def start(self) -> None: lcm.start() zenoh.start() - lidar = ZenohTopic("dimos/lidar", PointCloud2, qos=QOS_LATEST_WINS) - odometry = ZenohTopic("dimos/odometry", Odometry) + lidar_name = _topic_name(self.config.lidar_topic) + odometry_name = _topic_name(self.config.odometry_topic) + lidar = ZenohTopic(lidar_name, PointCloud2, qos=QOS_LATEST_WINS) + odometry = ZenohTopic(odometry_name, Odometry) self.register_disposable( Disposable( lcm.subscribe( - LCMTopic(_LIDAR_CHANNEL), + LCMTopic(f"{lidar_name}/{PointCloud2.msg_name}"), lambda payload, _: zenoh.publish(lidar, payload), ) ) @@ -55,10 +59,19 @@ def start(self) -> None: self.register_disposable( Disposable( lcm.subscribe( - LCMTopic(_ODOMETRY_CHANNEL), + LCMTopic(f"{odometry_name}/{Odometry.msg_name}"), lambda payload, _: zenoh.publish(odometry, payload), ) ) ) self.register_disposable(Disposable(lcm.stop)) self.register_disposable(Disposable(zenoh.stop)) + + +def _topic_name(topic: str) -> str: + name = topic.strip("/") + if name.startswith("dimos/"): + name = name.removeprefix("dimos/") + if not name: + raise ValueError("PointLIO relay topics cannot be empty") + return f"dimos/{name}" diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index dbd5ea42d1..8b95f28713 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -263,7 +263,7 @@ "phone-teleop-module": "dimos.teleop.phone.phone_teleop_module.PhoneTeleopModule", "pick-and-place-module": "dimos.manipulation.pick_and_place_module.PickAndPlaceModule", "point-lio": "dimos.hardware.sensors.lidar.pointlio.module.PointLio", - "point-lio-zenoh-relay": "dimos.robot.unitree.g1.blueprints.basic.pointlio_zenoh_relay.PointLioZenohRelay", + "point-lio-zenoh-relay": "dimos.hardware.sensors.lidar.pointlio.zenoh_relay.PointLioZenohRelay", "pointlio-recorder": "dimos.hardware.sensors.lidar.pointlio.recorder.PointlioRecorder", "quest-teleop-module": "dimos.teleop.quest.quest_teleop_module.QuestTeleopModule", "ray-tracing-voxel-map": "dimos.mapping.ray_tracing.module.RayTracingVoxelMap", diff --git a/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py b/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py index e4cb743fa2..0d1e20240a 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py +++ b/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py @@ -14,9 +14,9 @@ """Platform-owned control and localization inputs for the G1 GR00T stack. -Hardware uses PointLIO for ``lidar`` and ``odometry``. A simulation provider -publishes those streams itself. Mapping and navigation remain outside this -boundary. +Hardware and simulation both use PointLIO for ``lidar`` and ``odometry``. +Simulation providers supply the virtual sensor device and its world-frame +anchor. Mapping and navigation remain outside this boundary. """ from __future__ import annotations @@ -51,7 +51,7 @@ class G1GrootPlatform: def resolve_g1_groot_platform() -> G1GrootPlatform: if not global_config.simulation: from dimos.hardware.sensors.lidar.pointlio.module import PointLio - from dimos.robot.unitree.g1.blueprints.basic.pointlio_zenoh_relay import ( + from dimos.hardware.sensors.lidar.pointlio.zenoh_relay import ( PointLioZenohRelay, ) from dimos.robot.unitree.g1.wholebody_connection import G1WholeBodyConnection diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py index 6be968ec7f..69e7e51913 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py @@ -14,9 +14,9 @@ """Unitree G1 GR00T whole-body control, mapping, and navigation. -Real hardware uses PointLIO to produce registered lidar and odometry. Simulation -provides the same typed localization boundary from simulator truth. Both feed -the same mapping, planning, control, and visualization modules. +Real hardware and simulation use PointLIO to produce registered lidar and +odometry. Both feed the same mapping, planning, control, and visualization +modules. Usage: dimos run unitree-g1-groot-wbc @@ -179,6 +179,7 @@ def _real_costmap(grid: Any) -> Any: "world/depth_camera_info": None, "world/lidar": None, "world/coordinator_joint_state": g1_urdf_joint_state(root_path=_RERUN_ROOT), + "world/odometry": _real_odometry_root, "world/global_costmap": g1_costmap, "world/navigation_costmap": g1_costmap, "world/path": _nav_path, @@ -198,7 +199,6 @@ def _real_costmap(grid: Any) -> Any: } if not _platform.simulation: - _rerun_config["visual_override"]["world/odometry"] = _real_odometry_root _rerun_config["visual_override"]["world/global_costmap"] = _real_costmap _rerun_config["visual_override"]["world/navigation_costmap"] = _real_costmap From d22ad2f47fefe8aacc317433774369cab76d23e5 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Sat, 25 Jul 2026 00:50:19 +0800 Subject: [PATCH 07/33] Show G1 lidar in its public frame --- .../robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py index 69e7e51913..7234f2c531 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py @@ -177,7 +177,9 @@ def _real_costmap(grid: Any) -> Any: "world/camera_info": None, "world/depth_image": None, "world/depth_camera_info": None, - "world/lidar": None, + "world/pimsim/pointlio_lidar": None, + "world/pimsim/pointlio_odometry": None, + "world/localization_anchor": None, "world/coordinator_joint_state": g1_urdf_joint_state(root_path=_RERUN_ROOT), "world/odometry": _real_odometry_root, "world/global_costmap": g1_costmap, From 4f3fa155d44d89b48aa03721817c7494168420bb Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Sat, 25 Jul 2026 01:54:46 +0800 Subject: [PATCH 08/33] Tune Groot simulation visualization --- .../unitree/g1/blueprints/basic/groot_wbc_platform.py | 2 +- .../unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py | 9 +++++++-- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py b/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py index 0d1e20240a..69c9c5ed08 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py +++ b/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py @@ -98,5 +98,5 @@ def resolve_g1_groot_platform() -> G1GrootPlatform: auto_arm=True, auto_dry_run=False, ramp_seconds=0.0, - n_workers=10, + n_workers=12, ) diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py index 7234f2c531..60e646fd48 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py @@ -127,6 +127,10 @@ def _nav_path(path: NavPath) -> Any: return path.to_rerun(z_offset=0.3) +def _lidar_scan(cloud: Any) -> Any: + return cloud.to_rerun(voxel_size=0.03, colors=[80, 210, 255], mode="points") + + def _pelvis_to_mid360() -> Any: if not _pelvis_mid360_cache: from importlib import import_module @@ -170,16 +174,16 @@ def _real_costmap(grid: Any) -> Any: _static_entities.update(scene_package_static_entities(global_config.scene_package)) _rerun_config: dict[str, Any] = { - "memory_limit": "8GB" if _platform.simulation else "512MB", + "memory_limit": "1GB", "blueprint": _rerun_blueprint, "visual_override": { "world/color_image": None, "world/camera_info": None, "world/depth_image": None, "world/depth_camera_info": None, - "world/pimsim/pointlio_lidar": None, "world/pimsim/pointlio_odometry": None, "world/localization_anchor": None, + "world/lidar": _lidar_scan, "world/coordinator_joint_state": g1_urdf_joint_state(root_path=_RERUN_ROOT), "world/odometry": _real_odometry_root, "world/global_costmap": g1_costmap, @@ -192,6 +196,7 @@ def _real_costmap(grid: Any) -> Any: "world/g1/motor_states": 10.0, "world/g1/motor_command": 10.0, "world/odometry": 15.0, + "world/lidar": 2.0, "world/global_map": 1.0, "world/global_costmap": 2.0, "world/navigation_costmap": 2.0, From 5c89fc85affd7759588eca9fc2741c2459a5f0e9 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Sat, 25 Jul 2026 02:05:22 +0800 Subject: [PATCH 09/33] Keep coordinator RPC on the local control plane --- dimos/core/coordination/coordinator_rpc.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/dimos/core/coordination/coordinator_rpc.py b/dimos/core/coordination/coordinator_rpc.py index fd65b182ca..e63ab5a415 100644 --- a/dimos/core/coordination/coordinator_rpc.py +++ b/dimos/core/coordination/coordinator_rpc.py @@ -16,8 +16,7 @@ from typing import TYPE_CHECKING, Any -from dimos.core.global_config import global_config -from dimos.core.transport_factory import rpc_backend +from dimos.protocol.rpc.pubsubrpc import LCMRPC from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -38,7 +37,7 @@ def __init__(self, rpc: RPCSpec) -> None: def serve(cls, coordinator: RPCInspectable) -> CoordinatorRPC: """Publish `coordinator`'s @rpc methods under the `Coordinator/` prefix.""" cls._ensure_no_existing_service() - rpc = rpc_backend()() + rpc = LCMRPC() # start() before serve_module_rpc(): Zenoh's subscribe needs an open # session (acquired in start()), whereas LCM tolerates either order. rpc.start() @@ -48,7 +47,7 @@ def serve(cls, coordinator: RPCInspectable) -> CoordinatorRPC: @classmethod def connect(cls, *, timeout: float) -> CoordinatorRPC: """Attach to a running Coordinator, raising `TimeoutError` if none answers.""" - rpc = rpc_backend()() + rpc = LCMRPC() rpc.start() client = cls(rpc) try: @@ -79,7 +78,7 @@ def stop(self) -> None: @classmethod def _ensure_no_existing_service(cls) -> None: - probe = rpc_backend()() + probe = LCMRPC() probe.start() try: try: @@ -87,8 +86,8 @@ def _ensure_no_existing_service(cls) -> None: except TimeoutError: return raise RuntimeError( - f"another {cls.NAME} service is already running on the " - f"{global_config.transport} bus. Run `dimos stop` first." + f"another {cls.NAME} service is already running on the LCM bus. " + "Run `dimos stop` first." ) finally: probe.stop() From 61c549b2dfcd7c3b00fcb862959edcdf38e77d8f Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Sat, 25 Jul 2026 02:06:45 +0800 Subject: [PATCH 10/33] Revert "Keep coordinator RPC on the local control plane" This reverts commit 5c89fc85affd7759588eca9fc2741c2459a5f0e9. --- dimos/core/coordination/coordinator_rpc.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/dimos/core/coordination/coordinator_rpc.py b/dimos/core/coordination/coordinator_rpc.py index e63ab5a415..fd65b182ca 100644 --- a/dimos/core/coordination/coordinator_rpc.py +++ b/dimos/core/coordination/coordinator_rpc.py @@ -16,7 +16,8 @@ from typing import TYPE_CHECKING, Any -from dimos.protocol.rpc.pubsubrpc import LCMRPC +from dimos.core.global_config import global_config +from dimos.core.transport_factory import rpc_backend from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -37,7 +38,7 @@ def __init__(self, rpc: RPCSpec) -> None: def serve(cls, coordinator: RPCInspectable) -> CoordinatorRPC: """Publish `coordinator`'s @rpc methods under the `Coordinator/` prefix.""" cls._ensure_no_existing_service() - rpc = LCMRPC() + rpc = rpc_backend()() # start() before serve_module_rpc(): Zenoh's subscribe needs an open # session (acquired in start()), whereas LCM tolerates either order. rpc.start() @@ -47,7 +48,7 @@ def serve(cls, coordinator: RPCInspectable) -> CoordinatorRPC: @classmethod def connect(cls, *, timeout: float) -> CoordinatorRPC: """Attach to a running Coordinator, raising `TimeoutError` if none answers.""" - rpc = LCMRPC() + rpc = rpc_backend()() rpc.start() client = cls(rpc) try: @@ -78,7 +79,7 @@ def stop(self) -> None: @classmethod def _ensure_no_existing_service(cls) -> None: - probe = LCMRPC() + probe = rpc_backend()() probe.start() try: try: @@ -86,8 +87,8 @@ def _ensure_no_existing_service(cls) -> None: except TimeoutError: return raise RuntimeError( - f"another {cls.NAME} service is already running on the LCM bus. " - "Run `dimos stop` first." + f"another {cls.NAME} service is already running on the " + f"{global_config.transport} bus. Run `dimos stop` first." ) finally: probe.stop() From e9774b1ee2e36c773fee89ba0ae991366689c3f2 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Sat, 25 Jul 2026 17:06:11 +0800 Subject: [PATCH 11/33] Select the Go2 simulation provider from the ordinary blueprint - SimulationRequest model assets become optional so providers can resolve their own robot assets - SimulationBinding carries a provider rerun config that the ordinary G1 and Go2 blueprints merge into their own visualization - unitree-go2-basic resolves its platform through the simulation provider registry, mirroring the G1 GR00T blueprint --- .../g1/blueprints/basic/groot_wbc_platform.py | 4 ++ .../blueprints/basic/unitree_g1_groot_wbc.py | 12 +++- .../go2/blueprints/basic/go2_platform.py | 55 +++++++++++++++++++ .../go2/blueprints/basic/unitree_go2_basic.py | 18 +++++- dimos/simulation/providers.py | 11 ++-- 5 files changed, 92 insertions(+), 8 deletions(-) create mode 100644 dimos/robot/unitree/go2/blueprints/basic/go2_platform.py diff --git a/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py b/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py index 69c9c5ed08..c407726c6a 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py +++ b/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py @@ -23,6 +23,7 @@ from dataclasses import dataclass from pathlib import Path +from typing import Any from dimos.core.coordination.blueprints import Blueprint, autoconnect from dimos.core.global_config import global_config @@ -46,6 +47,7 @@ class G1GrootPlatform: auto_dry_run: bool ramp_seconds: float n_workers: int + rerun_config: dict[str, Any] def resolve_g1_groot_platform() -> G1GrootPlatform: @@ -71,6 +73,7 @@ def resolve_g1_groot_platform() -> G1GrootPlatform: auto_dry_run=True, ramp_seconds=10.0, n_workers=10, + rerun_config={}, ) if global_config.simulation != "mujoco": @@ -99,4 +102,5 @@ def resolve_g1_groot_platform() -> G1GrootPlatform: auto_dry_run=False, ramp_seconds=0.0, n_workers=12, + rerun_config=binding.rerun_config, ) diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py index 60e646fd48..0df24eed69 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py @@ -171,7 +171,8 @@ def _real_costmap(grid: Any) -> Any: _static_entities: dict[str, Any] = { _RERUN_ROOT: g1_urdf_static_robot(root_path=_RERUN_ROOT), } -_static_entities.update(scene_package_static_entities(global_config.scene_package)) +if not _platform.simulation: + _static_entities.update(scene_package_static_entities(global_config.scene_package)) _rerun_config: dict[str, Any] = { "memory_limit": "1GB", @@ -205,6 +206,15 @@ def _real_costmap(grid: Any) -> Any: "static": _static_entities, } +for _section in ("static", "visual_override", "max_hz"): + _rerun_config[_section] = { + **_rerun_config.get(_section, {}), + **_platform.rerun_config.get(_section, {}), + } +for _key, _value in _platform.rerun_config.items(): + if _key not in {"static", "visual_override", "max_hz"}: + _rerun_config[_key] = _value + if not _platform.simulation: _rerun_config["visual_override"]["world/global_costmap"] = _real_costmap _rerun_config["visual_override"]["world/navigation_costmap"] = _real_costmap diff --git a/dimos/robot/unitree/go2/blueprints/basic/go2_platform.py b/dimos/robot/unitree/go2/blueprints/basic/go2_platform.py new file mode 100644 index 0000000000..3e939e95a9 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/basic/go2_platform.py @@ -0,0 +1,55 @@ +# 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. + +from functools import cache + +from dimos.core.coordination.blueprints import Blueprint +from dimos.core.global_config import global_config +from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.simulation.providers import ( + SimulationBinding, + SimulationRequest, + load_simulation_provider, +) + + +def resolve_go2_platform() -> Blueprint: + if not global_config.simulation: + return GO2Connection.blueprint() + return _resolve_simulation_binding().backend + + +def resolve_go2_rerun_config() -> dict[str, object]: + if not global_config.simulation: + return {} + return _resolve_simulation_binding().rerun_config + + +@cache +def _resolve_simulation_binding() -> SimulationBinding: + if global_config.simulation != "mujoco": + raise ValueError("unitree-go2 only supports --simulation mujoco") + if not global_config.simulation_provider: + raise ValueError("unitree-go2 simulation requires --simulation-provider pimsim") + + provider = load_simulation_provider(global_config.simulation_provider) + return provider.build( + SimulationRequest( + robot_model="unitree_go2", + scene_package=global_config.scene_package, + ) + ) + + +__all__ = ["resolve_go2_platform", "resolve_go2_rerun_config"] diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py index c342c74f92..df437477e8 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py @@ -18,7 +18,10 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config -from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.robot.unitree.go2.blueprints.basic.go2_platform import ( + resolve_go2_platform, + resolve_go2_rerun_config, +) from dimos.visualization.vis_module import vis_module @@ -89,6 +92,7 @@ def _go2_rerun_blueprint() -> Any: "world/camera_info": _convert_camera_info, "world/global_map": _convert_global_map, "world/merged_map": _convert_global_map, + "world/global_costmap": _convert_navigation_costmap, "world/navigation_costmap": _convert_navigation_costmap, }, "max_hz": { @@ -103,6 +107,16 @@ def _go2_rerun_blueprint() -> Any: }, } +_provider_rerun_config = resolve_go2_rerun_config() +for _section in ("static", "visual_override", "max_hz"): + rerun_config[_section] = { + **rerun_config.get(_section, {}), + **_provider_rerun_config.get(_section, {}), + } +for _key, _value in _provider_rerun_config.items(): + if _key not in {"static", "visual_override", "max_hz"}: + rerun_config[_key] = _value + _with_vis = autoconnect( vis_module( viewer_backend=global_config.viewer, @@ -114,7 +128,7 @@ def _go2_rerun_blueprint() -> Any: unitree_go2_basic = ( autoconnect( _with_vis, - GO2Connection.blueprint(), + resolve_go2_platform(), ).global_config(n_workers=4, robot_model="unitree_go2") # we temporarily disabled sensor timestamps # and are derriving all timestmaps upon reception diff --git a/dimos/simulation/providers.py b/dimos/simulation/providers.py index 98549f9783..888ae342db 100644 --- a/dimos/simulation/providers.py +++ b/dimos/simulation/providers.py @@ -14,10 +14,10 @@ from __future__ import annotations -from dataclasses import dataclass +from dataclasses import dataclass, field import importlib.metadata as importlib_metadata from pathlib import Path -from typing import Protocol, runtime_checkable +from typing import Any, Protocol, runtime_checkable from dimos.core.coordination.blueprints import Blueprint @@ -27,9 +27,9 @@ @dataclass(frozen=True) class SimulationRequest: robot_model: str - model_path: str | Path - mesh_dir: str | Path - scene_package: str | Path | None + model_path: str | Path | None = None + mesh_dir: str | Path | None = None + scene_package: str | Path | None = None @dataclass(frozen=True) @@ -37,6 +37,7 @@ class SimulationBinding: backend: Blueprint adapter_type: str adapter_address: str | Path + rerun_config: dict[str, Any] = field(default_factory=dict) @runtime_checkable From b51231ce7f9fc7b99b56a1ae46cd28dea01d03f2 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Sat, 25 Jul 2026 17:06:26 +0800 Subject: [PATCH 12/33] Parameterize the DimSim agent tests over scene-control providers - SceneControl protocol with a shared load_scene_control resolver; DimSim stays built in, other simulators load from the dimos.simulation.scene_controls entry-point group - DIMOS_E2E_SIMULATOR selects the simulator (default pimsim) while the test bodies stay unchanged - The CLI harness maps pimsim onto --simulation mujoco with the pimsim provider and a scene package --- dimos/e2e_tests/conftest.py | 18 +++++-- dimos/e2e_tests/dimos_cli_call.py | 24 ++++++--- dimos/e2e_tests/scene_control.py | 51 +++++++++++++++++++ dimos/e2e_tests/test_dimsim_path_replaning.py | 35 +++++++------ dimos/e2e_tests/test_dimsim_spatial_memory.py | 11 +++- dimos/e2e_tests/test_dimsim_walk_forward.py | 6 +-- 6 files changed, 113 insertions(+), 32 deletions(-) create mode 100644 dimos/e2e_tests/scene_control.py diff --git a/dimos/e2e_tests/conftest.py b/dimos/e2e_tests/conftest.py index 36709963a4..50f026be74 100644 --- a/dimos/e2e_tests/conftest.py +++ b/dimos/e2e_tests/conftest.py @@ -13,6 +13,7 @@ # limitations under the License. from collections.abc import Callable, Generator, Iterator +import os import threading import time @@ -20,9 +21,9 @@ from dimos.core.transport import pLCMTransport from dimos.e2e_tests.conf_types import StartPersonTrack -from dimos.e2e_tests.dim_sim_client import DimSimClient from dimos.e2e_tests.dimos_cli_call import DimosCliCall from dimos.e2e_tests.lcm_spy import LcmSpy +from dimos.e2e_tests.scene_control import SceneControl, load_scene_control from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import make_vector3 @@ -74,10 +75,12 @@ def start_blueprint(mcp_port: int) -> Iterator[Callable[..., DimosCliCall]]: def set_name_and_start( *demo_args: str, simulator: str | None = None, + scene_package: str | None = None, ) -> DimosCliCall: dimos_robot_call.demo_args = list(demo_args) if simulator is not None: dimos_robot_call.simulator = simulator + dimos_robot_call.scene_package = scene_package dimos_robot_call.start() return dimos_robot_call @@ -162,15 +165,20 @@ def explore() -> None: @pytest.fixture -def dim_sim(): - client = DimSimClient() +def simulator_name() -> str: + return os.environ.get("DIMOS_E2E_SIMULATOR", "pimsim") + + +@pytest.fixture +def scene_control(simulator_name: str) -> Iterator[SceneControl]: + client = load_scene_control(simulator_name) client.start() yield client client.stop() @pytest.fixture -def spawn_wall_on_pose(lcm_spy: LcmSpy, dim_sim: DimSimClient): +def spawn_wall_on_pose(lcm_spy: LcmSpy, scene_control: SceneControl): """Spawn a dim_sim wall when the robot's /odom comes within `threshold` metres of `point`.""" odom_topic = "/odom#geometry_msgs.PoseStamped" stop_event = threading.Event() @@ -196,7 +204,7 @@ def worker(): with lcm_spy.topic_listener(odom_topic, on_odom): while not stop_event.is_set(): if triggered.wait(timeout=0.1): - dim_sim.add_wall(*wall) + scene_control.add_wall(*wall) return except BaseException as e: errors.append(e) diff --git a/dimos/e2e_tests/dimos_cli_call.py b/dimos/e2e_tests/dimos_cli_call.py index 83f7c5566f..541ab114cb 100644 --- a/dimos/e2e_tests/dimos_cli_call.py +++ b/dimos/e2e_tests/dimos_cli_call.py @@ -23,6 +23,7 @@ class DimosCliCall: demo_args: list[str] | None = None mcp_port: int | None = None simulator: str = "mujoco" + scene_package: str | None = None def __init__(self) -> None: self.process = None @@ -51,14 +52,21 @@ def start(self) -> None: global_overrides += ["--mcp-port", str(self.mcp_port)] env["MCPCLIENT__MCP_SERVER_URL"] = f"http://localhost:{self.mcp_port}/mcp" - self.process = subprocess.Popen( - [ - "dimos", - *global_overrides, + simulation_args = ["--simulation", self.simulator] + if self.simulator == "pimsim": + simulation_args = [ "--simulation", - self.simulator, - *args, - ], + "mujoco", + "--simulation-provider", + "pimsim", + "--scene-package", + self.scene_package or "dimsim-apartment", + "--viewer", + "none", + ] + + self.process = subprocess.Popen( + ["dimos", *global_overrides, *simulation_args, *args], start_new_session=True, env=env, ) @@ -66,6 +74,8 @@ def start(self) -> None: def stop(self) -> None: if self.process is None: return + if self.process.poll() is not None: + return try: # Send SIGTERM to the entire process group so child processes diff --git a/dimos/e2e_tests/scene_control.py b/dimos/e2e_tests/scene_control.py new file mode 100644 index 0000000000..8e95c68226 --- /dev/null +++ b/dimos/e2e_tests/scene_control.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. + +from __future__ import annotations + +from importlib.metadata import entry_points +from typing import Protocol + +from dimos.e2e_tests.dim_sim_client import DimSimClient + + +class SceneControl(Protocol): + def start(self) -> None: ... + + def stop(self) -> None: ... + + def set_agent_position(self, x: float, y: float, z: float = 0.4) -> None: ... + + def add_wall(self, x1: float, y1: float, x2: float, y2: float) -> None: ... + + def publish_goal(self, x: float, y: float) -> None: ... + + +def load_scene_control(simulator: str) -> SceneControl: + if simulator == "dimsim": + return DimSimClient() + matches = [ + entry + for entry in entry_points(group="dimos.simulation.scene_controls") + if entry.name == simulator + ] + if len(matches) != 1: + raise ValueError( + f"expected one scene-control provider for {simulator!r}, found {len(matches)}" + ) + client = matches[0].load()() + return client + + +__all__ = ["SceneControl", "load_scene_control"] diff --git a/dimos/e2e_tests/test_dimsim_path_replaning.py b/dimos/e2e_tests/test_dimsim_path_replaning.py index d222da30bd..11cd84c9da 100644 --- a/dimos/e2e_tests/test_dimsim_path_replaning.py +++ b/dimos/e2e_tests/test_dimsim_path_replaning.py @@ -17,31 +17,36 @@ @pytest.mark.self_hosted_large def test_path_replanning( - lcm_spy, start_blueprint, dim_sim, direct_cmd_vel_explorer, spawn_wall_on_pose + lcm_spy, + start_blueprint, + scene_control, + simulator_name, + direct_cmd_vel_explorer, + spawn_wall_on_pose, ) -> None: - start_blueprint( - "--dimsim-scene=empty", - "run", - "unitree-go2-agentic", - simulator="dimsim", + args = ( + ("--dimsim-scene=empty", "run", "unitree-go2-agentic") + if simulator_name == "dimsim" + else ("run", "unitree-go2-agentic") ) + start_blueprint(*args, simulator=simulator_name, scene_package="none") lcm_spy.save_topic("/rpc/McpClient/on_system_modules/res") lcm_spy.wait_for_saved_topic("/rpc/McpClient/on_system_modules/res", timeout=1200.0) - # robot spawns at (3, 2) + scene_control.set_agent_position(3, 2) # side wall - dim_sim.add_wall(2, -2.5, 12, -2.5) + scene_control.add_wall(2, -2.5, 12, -2.5) # other side wall - dim_sim.add_wall(2, 3.5, 12, 3.5) + scene_control.add_wall(2, 3.5, 12, 3.5) # back wall (behind robot) - dim_sim.add_wall(2, -2.5, 2, 3.5) + scene_control.add_wall(2, -2.5, 2, 3.5) # forward wall (far end) - dim_sim.add_wall(12, -2.5, 12, 3.5) + scene_control.add_wall(12, -2.5, 12, 3.5) # dividing wall at x=7 with doors at y=[-1.5,-0.5] and y=[1.5,2.5] - dim_sim.add_wall(7, -2.5, 7, -1.5) - dim_sim.add_wall(7, -0.5, 7, 1.5) - dim_sim.add_wall(7, 2.5, 7, 3.5) + scene_control.add_wall(7, -2.5, 7, -1.5) + scene_control.add_wall(7, -0.5, 7, 1.5) + scene_control.add_wall(7, 2.5, 7, 3.5) direct_cmd_vel_explorer.linear_speed = 0.8 direct_cmd_vel_explorer.follow_points([(10, 2), (2.5, 2), (3, 2)]) @@ -55,6 +60,6 @@ def test_path_replanning( wall=(7, 1.5, 7, 2.5), ) - dim_sim.publish_goal(10.913, 0.588) + scene_control.publish_goal(10.913, 0.588) lcm_spy.wait_until_odom_position(10.913, 0.588, threshold=1, timeout=120) diff --git a/dimos/e2e_tests/test_dimsim_spatial_memory.py b/dimos/e2e_tests/test_dimsim_spatial_memory.py index df2d2477f6..97926e998a 100644 --- a/dimos/e2e_tests/test_dimsim_spatial_memory.py +++ b/dimos/e2e_tests/test_dimsim_spatial_memory.py @@ -16,11 +16,18 @@ @pytest.mark.self_hosted_large -def test_go_to_the_bed(lcm_spy, start_blueprint, human_input, dim_sim, explore_house) -> None: +def test_go_to_the_bed( + lcm_spy, + start_blueprint, + human_input, + scene_control, + simulator_name, + explore_house, +) -> None: start_blueprint( "run", "unitree-go2-agentic", - simulator="dimsim", + simulator=simulator_name, ) lcm_spy.save_topic("/rpc/McpClient/on_system_modules/res") lcm_spy.wait_for_saved_topic("/rpc/McpClient/on_system_modules/res", timeout=1200.0) diff --git a/dimos/e2e_tests/test_dimsim_walk_forward.py b/dimos/e2e_tests/test_dimsim_walk_forward.py index fe8a73e94f..fd5d5e193b 100644 --- a/dimos/e2e_tests/test_dimsim_walk_forward.py +++ b/dimos/e2e_tests/test_dimsim_walk_forward.py @@ -16,7 +16,7 @@ @pytest.mark.self_hosted_large -def test_walk_forward(lcm_spy, start_blueprint, human_input, dim_sim) -> None: +def test_walk_forward(lcm_spy, start_blueprint, human_input, scene_control, simulator_name) -> None: start_blueprint( "run", "--disable", @@ -24,13 +24,13 @@ def test_walk_forward(lcm_spy, start_blueprint, human_input, dim_sim) -> None: "--disable", "security-module", "unitree-go2-agentic", - simulator="dimsim", + simulator=simulator_name, ) lcm_spy.save_topic("/rpc/McpClient/on_system_modules/res") lcm_spy.wait_for_saved_topic("/rpc/McpClient/on_system_modules/res", timeout=1200.0) origin_x, origin_y = 1, 2 - dim_sim.set_agent_position(origin_x, origin_y) + scene_control.set_agent_position(origin_x, origin_y) human_input("move forward 3 meter") From 67282524269766795b753208df2fb682661c5c6b Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Sat, 25 Jul 2026 17:19:18 +0800 Subject: [PATCH 13/33] Give the Go2 costmap the shared palette and explicit robot dimensions Live inspection of the running Go2 office session showed a healthy global costmap (5,410 nonzero and 2,363 lethal cells forming the office walls), so the wall-crossing planner defect is downstream of occupancy. The Go2 planner ran with robot_width=None, meaning no obstacle inflation at all, unlike the G1 composition. - Share one classic costmap palette between G1 and Go2 instead of the unrelated Accent colormap - Add the GO2 physical config from the published 0.70 x 0.31 x 0.40 m envelope and pass explicit width and rotation diameter to the planner - Make the Go2 height-cost configuration explicit, mirroring the G1 overhead-margin pattern Pending live goal-driven validation of the replanning behavior. --- dimos/robot/unitree/g1/g1_rerun.py | 13 +----- .../go2/blueprints/basic/unitree_go2_basic.py | 8 +--- .../go2/blueprints/smart/unitree_go2.py | 19 ++++++++- dimos/robot/unitree/go2/config.py | 41 +++++++++++++++++++ dimos/visualization/rerun/costmap.py | 39 ++++++++++++++++++ 5 files changed, 101 insertions(+), 19 deletions(-) create mode 100644 dimos/robot/unitree/go2/config.py create mode 100644 dimos/visualization/rerun/costmap.py diff --git a/dimos/robot/unitree/g1/g1_rerun.py b/dimos/robot/unitree/g1/g1_rerun.py index dca02403f6..0ccdbf17af 100644 --- a/dimos/robot/unitree/g1/g1_rerun.py +++ b/dimos/robot/unitree/g1/g1_rerun.py @@ -18,8 +18,7 @@ from typing import Any -import numpy as np - +from dimos.visualization.rerun.costmap import classic_costmap from dimos.visualization.rerun.urdf_robot import ( UrdfRobotJointStateRerunFactory, UrdfRobotStaticRerunFactory, @@ -28,14 +27,6 @@ G1_RERUN_ROOT = "world/odom/g1" G1_RERUN_URDF = "g1_urdf/g1.fixed.urdf" -# Classic costmap palette, indexed by grid value + 1: -# transparent unknown, blue free, orange occupied, red lethal. -_COSTMAP_LOOKUP_TABLE = np.zeros((102, 4), dtype=np.uint8) -_COSTMAP_LOOKUP_TABLE[0] = (0, 0, 0, 0) -_COSTMAP_LOOKUP_TABLE[1] = (72, 73, 129, 255) -_COSTMAP_LOOKUP_TABLE[2:101] = (255, 140, 0, 255) -_COSTMAP_LOOKUP_TABLE[101] = (220, 30, 30, 255) - def g1_costmap(grid: Any, z_offset: float = 0.02) -> Any: """Render an OccupancyGrid with the classic costmap palette. @@ -43,7 +34,7 @@ def g1_costmap(grid: Any, z_offset: float = 0.02) -> Any: The default z_offset lifts the mesh 2cm off the floor plane to avoid z-fighting with the ground. """ - return grid.to_rerun(color_lookup_table=_COSTMAP_LOOKUP_TABLE, z_offset=z_offset) + return classic_costmap(grid, z_offset=z_offset) def g1_urdf_static_robot(root_path: str = G1_RERUN_ROOT) -> UrdfRobotStaticRerunFactory: diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py index df437477e8..8ae8daa143 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py @@ -22,6 +22,7 @@ resolve_go2_platform, resolve_go2_rerun_config, ) +from dimos.visualization.rerun.costmap import classic_costmap from dimos.visualization.vis_module import vis_module @@ -37,12 +38,7 @@ def _convert_global_map(grid: Any) -> Any: def _convert_navigation_costmap(grid: Any) -> Any: - return grid.to_rerun( - colormap="Accent", - z_offset=0.015, - opacity=0.2, - background="#484981", - ) + return classic_costmap(grid, z_offset=0.015) def _static_base_link(rr: Any) -> list[Any]: diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py index 3351a3dc16..7c8a9c0f64 100644 --- a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py @@ -19,6 +19,7 @@ from dimos.core.stream import In from dimos.core.transport import LCMTransport from dimos.mapping.costmapper import CostMapper +from dimos.mapping.pointclouds.occupancy import HeightCostConfig from dimos.mapping.relocalization.module import RelocalizationModule from dimos.mapping.voxels import VoxelGridMapper from dimos.memory2.module import Recorder, RecorderConfig, pose_setter_for @@ -36,13 +37,27 @@ from dimos.perception.fiducial.marker_detection_stream_module import MarkerDetectionStreamModule from dimos.perception.fiducial.marker_tf_module import MarkerTfModule from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic +from dimos.robot.unitree.go2.config import GO2 from dimos.robot.unitree.go2.connection import GO2Connection +# Overhead margin added to the standing height before a gap counts as +# pass-under space, mirroring the G1 navigation composition. +_NAV_OVERHEAD_SAFETY_MARGIN = 0.2 +_NAV_MAX_STEP_HEIGHT = 0.15 + unitree_go2 = autoconnect( unitree_go2_basic, VoxelGridMapper.blueprint(emit_every=5), - CostMapper.blueprint(), - ReplanningAStarPlanner.blueprint(), + CostMapper.blueprint( + config=HeightCostConfig( + can_pass_under=GO2.height_clearance + _NAV_OVERHEAD_SAFETY_MARGIN, + can_climb=_NAV_MAX_STEP_HEIGHT, + ), + ), + ReplanningAStarPlanner.blueprint( + robot_width=GO2.width_clearance, + robot_rotation_diameter=GO2.rotation_diameter, + ), WavefrontFrontierExplorer.blueprint(), PatrollingModule.blueprint(), MovementManager.blueprint(), diff --git a/dimos/robot/unitree/go2/config.py b/dimos/robot/unitree/go2/config.py new file mode 100644 index 0000000000..9ab1d11141 --- /dev/null +++ b/dimos/robot/unitree/go2/config.py @@ -0,0 +1,41 @@ +# 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. + +from __future__ import annotations + +from dataclasses import dataclass + + +@dataclass(frozen=True) +class GO2Config: + """Physical metadata used by Go2 navigation blueprints. + + The Unitree Go2 standing envelope is 0.70 x 0.31 x 0.40 m; clearances + include the leg stance and a safety margin. + """ + + name: str + height_clearance: float + width_clearance: float + rotation_diameter: float + + +GO2 = GO2Config( + name="unitree_go2", + height_clearance=0.45, + width_clearance=0.5, + rotation_diameter=0.75, +) + +__all__ = ["GO2", "GO2Config"] diff --git a/dimos/visualization/rerun/costmap.py b/dimos/visualization/rerun/costmap.py new file mode 100644 index 0000000000..3bfc49efad --- /dev/null +++ b/dimos/visualization/rerun/costmap.py @@ -0,0 +1,39 @@ +# 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. + +from __future__ import annotations + +from typing import Any + +import numpy as np + +# Classic costmap palette, indexed by grid value + 1: +# transparent unknown, blue free, orange occupied, red lethal. +COSTMAP_LOOKUP_TABLE = np.zeros((102, 4), dtype=np.uint8) +COSTMAP_LOOKUP_TABLE[0] = (0, 0, 0, 0) +COSTMAP_LOOKUP_TABLE[1] = (72, 73, 129, 255) +COSTMAP_LOOKUP_TABLE[2:101] = (255, 140, 0, 255) +COSTMAP_LOOKUP_TABLE[101] = (220, 30, 30, 255) + + +def classic_costmap(grid: Any, z_offset: float = 0.02) -> Any: + """Render an OccupancyGrid with the classic costmap palette. + + The default z_offset lifts the mesh 2cm off the floor plane to avoid + z-fighting with the ground. + """ + return grid.to_rerun(color_lookup_table=COSTMAP_LOOKUP_TABLE, z_offset=z_offset) + + +__all__ = ["COSTMAP_LOOKUP_TABLE", "classic_costmap"] From 377027f4ca56e44d7038b1356a9629b5906d9be9 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Sat, 25 Jul 2026 18:37:09 +0800 Subject: [PATCH 14/33] Route local Zenoh sessions through a router --- dimos/core/coordination/coordinator_rpc.py | 9 +++- dimos/core/coordination/module_coordinator.py | 35 ++++++++++++-- dimos/core/native_module.py | 6 +++ dimos/protocol/service/test_zenohservice.py | 19 +++++++- dimos/protocol/service/zenohservice.py | 47 +++++++++++++++++-- docs/usage/transports/index.md | 2 + native/rust/dimos-module/src/zenoh.rs | 13 +++-- 7 files changed, 120 insertions(+), 11 deletions(-) diff --git a/dimos/core/coordination/coordinator_rpc.py b/dimos/core/coordination/coordinator_rpc.py index fd65b182ca..762a32d177 100644 --- a/dimos/core/coordination/coordinator_rpc.py +++ b/dimos/core/coordination/coordinator_rpc.py @@ -18,6 +18,8 @@ from dimos.core.global_config import global_config from dimos.core.transport_factory import rpc_backend +from dimos.protocol.rpc.pubsubrpc import ZenohRPC +from dimos.protocol.service.zenohservice import ZENOH_LOCAL_ROUTER_ENDPOINT from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -48,7 +50,12 @@ def serve(cls, coordinator: RPCInspectable) -> CoordinatorRPC: @classmethod def connect(cls, *, timeout: float) -> CoordinatorRPC: """Attach to a running Coordinator, raising `TimeoutError` if none answers.""" - rpc = rpc_backend()() + backend = rpc_backend() + rpc = ( + ZenohRPC(mode="client", connect=[ZENOH_LOCAL_ROUTER_ENDPOINT]) + if backend is ZenohRPC + else backend() + ) rpc.start() client = cls(rpc) try: diff --git a/dimos/core/coordination/module_coordinator.py b/dimos/core/coordination/module_coordinator.py index 3f6c48f3e3..05dec92f87 100644 --- a/dimos/core/coordination/module_coordinator.py +++ b/dimos/core/coordination/module_coordinator.py @@ -19,6 +19,7 @@ import dataclasses import importlib import inspect +import os import shutil import sys import threading @@ -40,6 +41,11 @@ pZenohTransport, ) from dimos.core.transport_factory import make_transport +from dimos.protocol.service.zenohservice import ( + ZENOH_LOCAL_ROUTER_ENDPOINT, + ZENOH_ROUTER_ENDPOINT_ENV, + ZenohRouter, +) from dimos.spec.utils import is_spec, spec_annotation_compliance, spec_structural_compliance from dimos.utils.generic import short_id from dimos.utils.logging_config import setup_logger @@ -86,13 +92,24 @@ def __init__( self._started = False self._modules_lock = threading.RLock() self._coordinator_rpc: CoordinatorRPC | None = None + self._zenoh_router: ZenohRouter | None = None + self._previous_zenoh_router_endpoint: str | None = None def start(self) -> None: from dimos.core.o3dpickle import register_picklers register_picklers() - for m in self._managers.values(): - m.start() + if self._global_config.transport == "zenoh": + self._zenoh_router = ZenohRouter() + self._zenoh_router.start() + self._previous_zenoh_router_endpoint = os.environ.get(ZENOH_ROUTER_ENDPOINT_ENV) + os.environ[ZENOH_ROUTER_ENDPOINT_ENV] = ZENOH_LOCAL_ROUTER_ENDPOINT + try: + for m in self._managers.values(): + m.start() + except BaseException: + self._stop_zenoh_router() + raise self._started = True def stop(self) -> None: @@ -115,9 +132,21 @@ def _stop_manager(m: WorkerManager) -> None: logger.error("Error stopping manager", manager=type(m).__name__, exc_info=True) safe_thread_map(tuple(self._managers.values()), _stop_manager) + self._stop_zenoh_router() + + def _stop_zenoh_router(self) -> None: + if self._zenoh_router is not None: + self._zenoh_router.stop() + self._zenoh_router = None + if self._global_config.transport != "zenoh": + return + if self._previous_zenoh_router_endpoint is None: + os.environ.pop(ZENOH_ROUTER_ENDPOINT_ENV, None) + else: + os.environ[ZENOH_ROUTER_ENDPOINT_ENV] = self._previous_zenoh_router_endpoint def start_rpc_service(self) -> None: - """Expose the coordinator's API as @rpc methods over LCM.""" + """Expose the coordinator's API over the configured RPC transport.""" if self._coordinator_rpc is not None: return self._coordinator_rpc = CoordinatorRPC.serve(self) diff --git a/dimos/core/native_module.py b/dimos/core/native_module.py index abf43f70fc..9aa234ce60 100644 --- a/dimos/core/native_module.py +++ b/dimos/core/native_module.py @@ -60,6 +60,10 @@ class MyCppModule(NativeModule): from dimos.core.core import rpc from dimos.core.global_config import global_config from dimos.core.module import Module, ModuleConfig +from dimos.protocol.service.zenohservice import ( + ZENOH_LOCAL_ROUTER_ENDPOINT, + ZENOH_ROUTER_ENDPOINT_ENV, +) from dimos.utils.logging_config import setup_logger if sys.platform.startswith("linux"): @@ -228,6 +232,8 @@ def start(self) -> None: # set transport so native modules know which one to spawn env["DIMOS_TRANSPORT"] = global_config.transport + if global_config.transport == "zenoh": + env[ZENOH_ROUTER_ENDPOINT_ENV] = ZENOH_LOCAL_ROUTER_ENDPOINT # set Rust logging to match Python level env["RUST_LOG"] = _PYTHON_TO_RUST_LEVELS.get( diff --git a/dimos/protocol/service/test_zenohservice.py b/dimos/protocol/service/test_zenohservice.py index 621c370485..c1406023b4 100644 --- a/dimos/protocol/service/test_zenohservice.py +++ b/dimos/protocol/service/test_zenohservice.py @@ -16,7 +16,13 @@ import pytest -from dimos.protocol.service.zenohservice import ZenohConfig, ZenohService, ZenohSessionPool +from dimos.protocol.service.zenohservice import ( + ZENOH_LOCAL_ROUTER_ENDPOINT, + ZENOH_ROUTER_ENDPOINT_ENV, + ZenohConfig, + ZenohService, + ZenohSessionPool, +) @pytest.fixture() @@ -33,6 +39,17 @@ def test_different_modes_produce_different_keys() -> None: assert peer.session_key != client.session_key +def test_default_config_uses_local_router_when_configured( + monkeypatch: pytest.MonkeyPatch, +) -> None: + monkeypatch.setenv(ZENOH_ROUTER_ENDPOINT_ENV, ZENOH_LOCAL_ROUTER_ENDPOINT) + + config = ZenohConfig() + + assert config.mode == "client" + assert config.connect == [ZENOH_LOCAL_ROUTER_ENDPOINT] + + def test_start_creates_session(session_pool) -> None: svc = ZenohService(session_pool=session_pool) svc.start() diff --git a/dimos/protocol/service/zenohservice.py b/dimos/protocol/service/zenohservice.py index 2cc491f9ae..66d977b6b7 100644 --- a/dimos/protocol/service/zenohservice.py +++ b/dimos/protocol/service/zenohservice.py @@ -15,9 +15,11 @@ from __future__ import annotations import json +import os import threading from typing import Any +from pydantic import Field import zenoh from dimos.protocol.service.spec import BaseConfig, Service @@ -27,11 +29,24 @@ logger = setup_logger() +ZENOH_ROUTER_ENDPOINT_ENV = "DIMOS_ZENOH_ROUTER_ENDPOINT" +ZENOH_LOCAL_ROUTER_ENDPOINT = "tcp/127.0.0.1:7447" +ZENOH_LOCAL_ROUTER_LISTEN = "tcp/[::]:7447" + + +def _default_mode() -> str: + return "client" if os.getenv(ZENOH_ROUTER_ENDPOINT_ENV) else "peer" + + +def _default_connect() -> list[str]: + endpoint = os.getenv(ZENOH_ROUTER_ENDPOINT_ENV) + return [endpoint] if endpoint else [] + class ZenohConfig(BaseConfig): - mode: str = "peer" - connect: list[str] = [] - listen: list[str] = [] + mode: str = Field(default_factory=_default_mode) + connect: list[str] = Field(default_factory=_default_connect) + listen: list[str] = Field(default_factory=list) @property def session_key(self) -> str: @@ -70,6 +85,29 @@ def close_all(self) -> None: default_session_pool = ZenohSessionPool() +class ZenohRouter: + def __init__(self, listen: str = ZENOH_LOCAL_ROUTER_LISTEN) -> None: + self._listen = listen + self._session: zenoh.Session | None = None + + def start(self) -> None: + config = zenoh.Config() + config.insert_json5("mode", '"router"') + config.insert_json5("listen/endpoints", json.dumps([self._listen])) + try: + self._session = zenoh.open(config) + logger.info("Local Zenoh router started", endpoint=self._listen) + except zenoh.ZError as exc: + if "Address already in use" not in str(exc): + raise + logger.info("Using existing local Zenoh router", endpoint=self._listen) + + def stop(self) -> None: + if self._session is not None: + self._session.close() + self._session = None + + class ZenohService(Service): config: ZenohConfig @@ -81,6 +119,9 @@ def __init__(self, *, session_pool: ZenohSessionPool | None = None, **kwargs: An self._session: zenoh.Session | None = None def start(self) -> None: + endpoint = os.getenv(ZENOH_ROUTER_ENDPOINT_ENV) + if endpoint and not self.config.model_fields_set: + self.config = ZenohConfig(mode="client", connect=[endpoint]) self._session = self._session_pool.acquire(self.config) super().start() diff --git a/docs/usage/transports/index.md b/docs/usage/transports/index.md index 8ec9fedaa2..3a2919d32a 100644 --- a/docs/usage/transports/index.md +++ b/docs/usage/transports/index.md @@ -351,6 +351,8 @@ Use Zenoh when: At the stream level, the transport wrappers are `ZenohTransport` and `pZenohTransport`. Install, defaults, and CLI versus environment overrides are in the [Zenoh quickstart](#zenoh-quickstart) above. +For a local `dimos run`, the coordinator starts or reuses one Zenoh router listening on port `7447`. Python workers, native modules, and local coordinator clients connect to it through `127.0.0.1`. This avoids an all-to-all peer mesh and keeps a local run independent of Wi-Fi, VPN, and Docker interface changes. The router remains reachable on other interfaces for explicitly configured remote participants. + Performance note: zenoh's session-to-session path (modules in different processes, the common case) benchmarks faster than LCM for small messages and for >=2MiB ones. Delivery *within* one shared session (co-located modules in one worker) is its slow path for 256KiB-1MiB messages (a few GiB/s); pin shared memory transports for heavy co-located streams. The benchmark has both cases (`Zenoh` = shared session, `ZenohPeers` = separate sessions). The Rerun bridge also follows the global transport. When `transport=zenoh`, the bridge listens on Zenoh and on LCM for TF data. diff --git a/native/rust/dimos-module/src/zenoh.rs b/native/rust/dimos-module/src/zenoh.rs index 9ff5209793..198ca89ce5 100644 --- a/native/rust/dimos-module/src/zenoh.rs +++ b/native/rust/dimos-module/src/zenoh.rs @@ -63,9 +63,16 @@ pub struct ZenohTransport { impl ZenohTransport { pub async fn new() -> io::Result { - let session = ::zenoh::open(::zenoh::Config::default()) - .await - .map_err(to_io)?; + let mut config = ::zenoh::Config::default(); + if let Ok(endpoint) = std::env::var("DIMOS_ZENOH_ROUTER_ENDPOINT") { + config.insert_json5("mode", r#""client""#).map_err(to_io)?; + let endpoints = serde_json::to_string(&[endpoint]) + .map_err(|error| io::Error::new(io::ErrorKind::InvalidData, error))?; + config + .insert_json5("connect/endpoints", &endpoints) + .map_err(to_io)?; + } + let session = ::zenoh::open(config).await.map_err(to_io)?; Ok(Self { session, qos: OnceLock::new(), From 854ee108249db5f98ca22e68ceebbdb907624db9 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Wed, 29 Jul 2026 12:47:53 +0800 Subject: [PATCH 15/33] Align simulator mapping and visualization boundaries --- .../g1/blueprints/basic/groot_wbc_platform.py | 6 +++--- .../g1/blueprints/basic/unitree_g1_groot_wbc.py | 11 ++++++++++- dimos/simulation/engines/mujoco_shm.py | 17 ++--------------- dimos/visualization/rerun/bridge.py | 12 +++++++++--- .../rerun/test_detection3d_bridge.py | 15 +++++++++++++++ 5 files changed, 39 insertions(+), 22 deletions(-) diff --git a/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py b/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py index c407726c6a..7abccf56ed 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py +++ b/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py @@ -14,9 +14,9 @@ """Platform-owned control and localization inputs for the G1 GR00T stack. -Hardware and simulation both use PointLIO for ``lidar`` and ``odometry``. -Simulation providers supply the virtual sensor device and its world-frame -anchor. Mapping and navigation remain outside this boundary. +Hardware PointLIO and simulation providers both supply ``lidar`` and +``odometry`` at the shared mapper boundary. Mapping and navigation remain +outside this boundary. """ from __future__ import annotations diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py index 0df24eed69..6b1f59a747 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py @@ -203,6 +203,11 @@ def _real_costmap(grid: Any) -> Any: "world/navigation_costmap": 2.0, "world/path": 0, }, + "latest_state": { + "world/global_map", + "world/global_costmap", + "world/navigation_costmap", + }, "static": _static_entities, } @@ -211,8 +216,12 @@ def _real_costmap(grid: Any) -> Any: **_rerun_config.get(_section, {}), **_platform.rerun_config.get(_section, {}), } +_rerun_config["latest_state"] = { + *_rerun_config["latest_state"], + *_platform.rerun_config.get("latest_state", set()), +} for _key, _value in _platform.rerun_config.items(): - if _key not in {"static", "visual_override", "max_hz"}: + if _key not in {"static", "visual_override", "max_hz", "latest_state"}: _rerun_config[_key] = _value if not _platform.simulation: diff --git a/dimos/simulation/engines/mujoco_shm.py b/dimos/simulation/engines/mujoco_shm.py index 7cb24e39a7..523c2e822b 100644 --- a/dimos/simulation/engines/mujoco_shm.py +++ b/dimos/simulation/engines/mujoco_shm.py @@ -28,7 +28,6 @@ from dataclasses import dataclass import hashlib -from multiprocessing import resource_tracker from multiprocessing.shared_memory import SharedMemory from pathlib import Path from typing import Any @@ -120,18 +119,6 @@ def _buffer_name(key: str, buffer: str) -> str: return f"{_NAME_PREFIX}_{key}_{buffer}" -def _unregister(shm: SharedMemory) -> SharedMemory: - """Detach ``shm`` from ``resource_tracker`` to silence spurious warnings. - - Same technique as ``dimos.simulation.mujoco.shared_memory._unregister``. - """ - try: - resource_tracker.unregister(shm._name, "shared_memory") # type: ignore[attr-defined] - except Exception: - pass - return shm - - @dataclass(frozen=True) class ManipShmSet: """Frozen set of named SharedMemory buffers for sim <-> adapter IPC. @@ -164,7 +151,7 @@ def create(cls, key: str) -> ManipShmSet: for buffer_name, size in _shm_sizes.items(): name = _buffer_name(key, buffer_name) try: - stale = _unregister(SharedMemory(name=name)) + stale = SharedMemory(name=name) stale.close() try: stale.unlink() @@ -182,7 +169,7 @@ def attach(cls, key: str) -> ManipShmSet: buffers: dict[str, SharedMemory] = {} for buffer_name in _shm_sizes: name = _buffer_name(key, buffer_name) - buffers[buffer_name] = _unregister(SharedMemory(name=name)) + buffers[buffer_name] = SharedMemory(name=name) return cls(**buffers) def as_list(self) -> list[SharedMemory]: diff --git a/dimos/visualization/rerun/bridge.py b/dimos/visualization/rerun/bridge.py index f968216526..e59a25fa91 100644 --- a/dimos/visualization/rerun/bridge.py +++ b/dimos/visualization/rerun/bridge.py @@ -213,6 +213,7 @@ class Config(ModuleConfig): ) static: dict[str, Callable[[Any], Any]] = field(default_factory=dict) max_hz: dict[str, float] = field(default_factory=dict) + latest_state: set[str] = field(default_factory=set) entity_prefix: str = "world" topic_to_entity: Callable[[Any], str] | None = None @@ -342,15 +343,20 @@ def _on_message(self, msg: Any, topic: Any) -> None: # TFMessage for example returns list of (entity_path, archetype) tuples if is_rerun_multi(rerun_data): for path, archetype in rerun_data: - rr.log(path, archetype) + rr.log(path, archetype, static=path in self.config.latest_state) else: - rr.log(entity_path, cast("Archetype", rerun_data)) + latest_state = entity_path in self.config.latest_state + rr.log(entity_path, cast("Archetype", rerun_data), static=latest_state) # if source msg carries a frame_id, attach the entity to that TF frame # should skip if archetype is a Transform3D if not isinstance(rerun_data, rr.Transform3D): frame_id = getattr(msg, "frame_id", None) if frame_id and self._frame_attached.get(entity_path) != frame_id: - rr.log(entity_path, rr.Transform3D(parent_frame=f"tf#/{frame_id}")) + rr.log( + entity_path, + rr.Transform3D(parent_frame=f"tf#/{frame_id}"), + static=latest_state, + ) self._frame_attached[entity_path] = frame_id @rpc diff --git a/dimos/visualization/rerun/test_detection3d_bridge.py b/dimos/visualization/rerun/test_detection3d_bridge.py index ff1d727e92..3ad1d674b8 100644 --- a/dimos/visualization/rerun/test_detection3d_bridge.py +++ b/dimos/visualization/rerun/test_detection3d_bridge.py @@ -77,3 +77,18 @@ def test_detection3darray_bridge_attaches_topic_entity_to_message_frame() -> Non transform = mock_log.call_args_list[1].args[1] assert isinstance(transform, rr.Transform3D) assert transform.parent_frame.as_arrow_array().to_pylist() == ["tf#/world"] + + +def test_latest_state_entity_overwrites_data_and_frame_attachment() -> None: + entity_path = "world/marker_detection/detections" + bridge = RerunBridgeModule(latest_state={entity_path}) + bridge._min_intervals = {} + + try: + with patch("dimos.visualization.rerun.bridge.rr.log") as mock_log: + bridge._on_message(_detection_array(), Topic("/marker_detection/detections")) + finally: + bridge.stop() + + assert mock_log.call_count == 2 + assert all(call.kwargs == {"static": True} for call in mock_log.call_args_list) From df56d7f2fd5c7a0433ab5a78a75297a8d6a38f4e Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Wed, 29 Jul 2026 23:48:23 +0800 Subject: [PATCH 16/33] Fix G1 apartment navigation clearance --- .../unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py index 6b1f59a747..774f2d8c8f 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py @@ -71,7 +71,7 @@ _NAV_OVERHEAD_SAFETY_MARGIN = 0.2 _NAV_MAX_STEP_HEIGHT = 0.10 _NAV_ROTATION_DIAMETER = 0.8 -_NAV_SAFE_RADIUS_MARGIN = 0.6 +_NAV_PATH_WIDTH_MARGIN = 1.1 _RERUN_ROOT = "world/odometry/g1" _URDF_PATH = Path(__file__).resolve().parents[2] / "g1.urdf" _NOMINAL_PELVIS_Z = 0.74 @@ -96,10 +96,9 @@ can_pass_under=G1.height_clearance + _NAV_OVERHEAD_SAFETY_MARGIN, can_climb=_NAV_MAX_STEP_HEIGHT, ), - initial_safe_radius_meters=G1.width_clearance + _NAV_SAFE_RADIUS_MARGIN, ), ReplanningAStarPlanner.blueprint( - robot_width=G1.width_clearance, + robot_width=G1.width_clearance / _NAV_PATH_WIDTH_MARGIN, robot_rotation_diameter=_NAV_ROTATION_DIAMETER, ), MovementManager.blueprint(), From 623bf81141c2350be941f0d0a5fa6a0f7e41af1d Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Fri, 31 Jul 2026 08:33:59 +0800 Subject: [PATCH 17/33] Make simulation E2E tests transport agnostic --- dimos/e2e_tests/conftest.py | 54 ++++++++++++--- dimos/e2e_tests/dim_sim_client.py | 15 ++-- dimos/e2e_tests/dimos_cli_call.py | 5 +- dimos/e2e_tests/lcm_spy.py | 69 +++++++++++++++---- dimos/e2e_tests/test_dimsim_path_replaning.py | 10 ++- dimos/e2e_tests/test_dimsim_walk_forward.py | 9 +++ .../go2/blueprints/basic/go2_platform.py | 4 +- .../mujoco/direct_cmd_vel_explorer.py | 19 +++-- 8 files changed, 147 insertions(+), 38 deletions(-) diff --git a/dimos/e2e_tests/conftest.py b/dimos/e2e_tests/conftest.py index 50f026be74..e5a4ed4636 100644 --- a/dimos/e2e_tests/conftest.py +++ b/dimos/e2e_tests/conftest.py @@ -19,7 +19,8 @@ import pytest -from dimos.core.transport import pLCMTransport +from dimos.core.global_config import global_config +from dimos.core.transport_factory import make_transport from dimos.e2e_tests.conf_types import StartPersonTrack from dimos.e2e_tests.dimos_cli_call import DimosCliCall from dimos.e2e_tests.lcm_spy import LcmSpy @@ -28,6 +29,11 @@ from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import make_vector3 from dimos.msgs.std_msgs.Bool import Bool +from dimos.protocol.service.zenohservice import ( + ZENOH_LOCAL_ROUTER_ENDPOINT, + ZENOH_ROUTER_ENDPOINT_ENV, + ZenohRouter, +) from dimos.simulation.mujoco.direct_cmd_vel_explorer import DirectCmdVelExplorer from dimos.simulation.mujoco.person_on_track import PersonTrackPublisher @@ -41,7 +47,23 @@ def _pose(x: float, y: float, theta: float) -> PoseStamped: @pytest.fixture -def lcm_spy() -> Iterator[LcmSpy]: +def transport_runtime(monkeypatch) -> Iterator[None]: + if global_config.transport != "zenoh": + yield + return + + router = ZenohRouter() + router.start() + monkeypatch.setenv(ZENOH_ROUTER_ENDPOINT_ENV, ZENOH_LOCAL_ROUTER_ENDPOINT) + try: + yield + finally: + router.stop() + + +@pytest.fixture +def lcm_spy(transport_runtime: None) -> Iterator[LcmSpy]: + del transport_runtime lcm_spy = LcmSpy() lcm_spy.start() yield lcm_spy @@ -68,7 +90,11 @@ def fun(*, points: list[tuple[float, float, float]], fail_message: str) -> None: @pytest.fixture -def start_blueprint(mcp_port: int) -> Iterator[Callable[..., DimosCliCall]]: +def start_blueprint( + mcp_port: int, + transport_runtime: None, +) -> Iterator[Callable[..., DimosCliCall]]: + del transport_runtime dimos_robot_call = DimosCliCall() dimos_robot_call.mcp_port = mcp_port @@ -90,16 +116,17 @@ def set_name_and_start( @pytest.fixture -def human_input(): - transport = pLCMTransport("/human_input") - transport.lcm.start() +def human_input(transport_runtime: None): + del transport_runtime + transport = make_transport("/human_input") + transport.start() def send_human_input(message: str) -> None: - transport.publish(message) + transport.broadcast(None, message) yield send_human_input - transport.lcm.stop() + transport.stop() @pytest.fixture @@ -130,7 +157,10 @@ def run_person_track() -> None: @pytest.fixture -def direct_cmd_vel_explorer() -> Generator[PersonTrackPublisher, None, None]: +def direct_cmd_vel_explorer( + transport_runtime: None, +) -> Generator[DirectCmdVelExplorer, None, None]: + del transport_runtime explorer = DirectCmdVelExplorer() explorer.start() yield explorer @@ -170,7 +200,11 @@ def simulator_name() -> str: @pytest.fixture -def scene_control(simulator_name: str) -> Iterator[SceneControl]: +def scene_control( + simulator_name: str, + transport_runtime: None, +) -> Iterator[SceneControl]: + del transport_runtime client = load_scene_control(simulator_name) client.start() yield client diff --git a/dimos/e2e_tests/dim_sim_client.py b/dimos/e2e_tests/dim_sim_client.py index 18b12074f5..9187e5f634 100644 --- a/dimos/e2e_tests/dim_sim_client.py +++ b/dimos/e2e_tests/dim_sim_client.py @@ -12,7 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos.core.transport import LCMTransport +from typing import cast + +from dimos.core.transport import PubSubTransport +from dimos.core.transport_factory import make_transport from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.simulation.dimsim.scene_client import SceneClient @@ -22,7 +25,10 @@ class DimSimClient: def __init__(self) -> None: self._client = None - self._goal_request: LCMTransport[PoseStamped] = LCMTransport("/goal_request", PoseStamped) + self._goal_request = cast( + "PubSubTransport[PoseStamped]", + make_transport("/goal_request", PoseStamped), + ) def start(self) -> None: # self.client should be started lazily to avoid starting the dimsim @@ -47,10 +53,11 @@ def add_wall(self, x1: float, y1: float, x2: float, y2: float) -> None: self.client.add_wall(y1, x1, y2, x2) def publish_goal(self, x: float, y: float) -> None: - self._goal_request.publish( + self._goal_request.broadcast( + None, PoseStamped( position=(x, y, 0), orientation=(0, 0, 0, 1), frame_id="world", - ) + ), ) diff --git a/dimos/e2e_tests/dimos_cli_call.py b/dimos/e2e_tests/dimos_cli_call.py index 541ab114cb..c19703934d 100644 --- a/dimos/e2e_tests/dimos_cli_call.py +++ b/dimos/e2e_tests/dimos_cli_call.py @@ -52,7 +52,8 @@ def start(self) -> None: global_overrides += ["--mcp-port", str(self.mcp_port)] env["MCPCLIENT__MCP_SERVER_URL"] = f"http://localhost:{self.mcp_port}/mcp" - simulation_args = ["--simulation", self.simulator] + viewer = os.environ.get("DIMOS_E2E_VIEWER", "none") + simulation_args = ["--simulation", self.simulator, "--viewer", viewer] if self.simulator == "pimsim": simulation_args = [ "--simulation", @@ -62,7 +63,7 @@ def start(self) -> None: "--scene-package", self.scene_package or "dimsim-apartment", "--viewer", - "none", + viewer, ] self.process = subprocess.Popen( diff --git a/dimos/e2e_tests/lcm_spy.py b/dimos/e2e_tests/lcm_spy.py index cec34324f3..2a7e281f7e 100644 --- a/dimos/e2e_tests/lcm_spy.py +++ b/dimos/e2e_tests/lcm_spy.py @@ -19,16 +19,15 @@ import threading from typing import Any -import lcm - +from dimos.core.transport import PubSubTransport +from dimos.core.transport_factory import make_transport from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.helpers import resolve_msg_type from dimos.msgs.protocol import DimosMsg -from dimos.protocol.service.lcmservice import LCMService from dimos.utils.testing.waiting import wait_until -class LcmSpy(LCMService): - l: lcm.LCM +class LcmSpy: messages: dict[str, list[bytes]] _messages_lock: threading.Lock _saved_topics: set[str] @@ -36,23 +35,33 @@ class LcmSpy(LCMService): _topic_listeners: dict[str, list[Callable[[bytes], None]]] _topic_listeners_lock: threading.Lock - def __init__(self, **kwargs: Any) -> None: - super().__init__(**kwargs) - self.l = lcm.LCM() + def __init__(self) -> None: self.messages = {} self._messages_lock = threading.Lock() self._saved_topics = set() self._saved_topics_lock = threading.Lock() self._topic_listeners = {} self._topic_listeners_lock = threading.Lock() + self._transports: dict[str, PubSubTransport[Any]] = {} + self._unsubscribers: dict[str, Callable[[], None]] = {} + self._publishers: dict[str, PubSubTransport[Any]] = {} + self._transports_lock = threading.Lock() def start(self) -> None: - super().start() - if self.l: - self.l.subscribe(".*", self.msg) + pass def stop(self) -> None: - super().stop() + with self._transports_lock: + unsubscribers = tuple(self._unsubscribers.values()) + transports = tuple(self._transports.values()) + publishers = tuple(self._publishers.values()) + self._unsubscribers.clear() + self._transports.clear() + self._publishers.clear() + for unsubscribe in unsubscribers: + unsubscribe() + for transport in (*transports, *publishers): + transport.stop() def msg(self, topic: str, data: bytes) -> None: with self._saved_topics_lock: @@ -67,15 +76,23 @@ def msg(self, topic: str, data: bytes) -> None: listener(data) def publish(self, topic: str, msg: Any) -> None: - self.l.publish(topic, msg.lcm_encode()) + with self._transports_lock: + transport = self._publishers.get(topic) + if transport is None: + name, msg_type = _parse_topic(topic, type(msg)) + transport = make_transport(name, msg_type) + self._publishers[topic] = transport + transport.broadcast(None, msg) def save_topic(self, topic: str) -> None: with self._saved_topics_lock: self._saved_topics.add(topic) + self._ensure_subscription(topic) def register_topic_listener(self, topic: str, listener: Callable[[bytes], None]) -> None: with self._topic_listeners_lock: self._topic_listeners.setdefault(topic, []).append(listener) + self._ensure_subscription(topic) def unregister_topic_listener(self, topic: str, listener: Callable[[bytes], None]) -> None: with self._topic_listeners_lock: @@ -171,3 +188,29 @@ def predicate(msg: PoseStamped) -> bool: f"Failed to get to position x={x}, y={y}", timeout, ) + + def _ensure_subscription(self, topic: str) -> None: + with self._transports_lock: + if topic in self._transports: + return + name, msg_type = _parse_topic(topic) + transport = make_transport(name, msg_type) + unsubscribe = transport.subscribe(lambda msg: self.msg(topic, _encode_message(msg))) + self._transports[topic] = transport + self._unsubscribers[topic] = unsubscribe + + +def _parse_topic(topic: str, default_type: type[Any] | None = None) -> tuple[str, type[Any] | None]: + if "#" not in topic: + return topic, default_type if hasattr(default_type, "lcm_encode") else None + name, type_name = topic.rsplit("#", 1) + msg_type = resolve_msg_type(type_name) + if msg_type is None: + raise ValueError(f"Unknown message type {type_name!r} in topic {topic!r}") + return name, msg_type + + +def _encode_message(message: Any) -> bytes: + if hasattr(message, "lcm_encode"): + return message.lcm_encode() + return pickle.dumps(message) diff --git a/dimos/e2e_tests/test_dimsim_path_replaning.py b/dimos/e2e_tests/test_dimsim_path_replaning.py index 11cd84c9da..38fc099f3d 100644 --- a/dimos/e2e_tests/test_dimsim_path_replaning.py +++ b/dimos/e2e_tests/test_dimsim_path_replaning.py @@ -14,6 +14,8 @@ import pytest +from dimos.msgs.std_msgs.Bool import Bool + @pytest.mark.self_hosted_large def test_path_replanning( @@ -62,4 +64,10 @@ def test_path_replanning( scene_control.publish_goal(10.913, 0.588) - lcm_spy.wait_until_odom_position(10.913, 0.588, threshold=1, timeout=120) + lcm_spy.wait_for_message_result( + "/goal_reached#std_msgs.Bool", + Bool, + predicate=bool, + fail_message="Planner did not complete the replanned route", + timeout=120, + ) diff --git a/dimos/e2e_tests/test_dimsim_walk_forward.py b/dimos/e2e_tests/test_dimsim_walk_forward.py index fd5d5e193b..c95039fb8f 100644 --- a/dimos/e2e_tests/test_dimsim_walk_forward.py +++ b/dimos/e2e_tests/test_dimsim_walk_forward.py @@ -17,7 +17,9 @@ @pytest.mark.self_hosted_large def test_walk_forward(lcm_spy, start_blueprint, human_input, scene_control, simulator_name) -> None: + scene_args = ("--dimsim-scene=empty",) if simulator_name == "dimsim" else () start_blueprint( + *scene_args, "run", "--disable", "spatial-memory", @@ -25,12 +27,19 @@ def test_walk_forward(lcm_spy, start_blueprint, human_input, scene_control, simu "security-module", "unitree-go2-agentic", simulator=simulator_name, + scene_package="none", ) lcm_spy.save_topic("/rpc/McpClient/on_system_modules/res") lcm_spy.wait_for_saved_topic("/rpc/McpClient/on_system_modules/res", timeout=1200.0) origin_x, origin_y = 1, 2 scene_control.set_agent_position(origin_x, origin_y) + lcm_spy.save_topic("/global_costmap#nav_msgs.OccupancyGrid") + scene_control.add_wall(-1, -1, 6, -1) + scene_control.add_wall(-1, 5, 6, 5) + scene_control.add_wall(-1, -1, -1, 5) + scene_control.add_wall(6, -1, 6, 5) + lcm_spy.wait_for_saved_topic("/global_costmap#nav_msgs.OccupancyGrid", timeout=30) human_input("move forward 3 meter") diff --git a/dimos/robot/unitree/go2/blueprints/basic/go2_platform.py b/dimos/robot/unitree/go2/blueprints/basic/go2_platform.py index 3e939e95a9..d1ef72c734 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/go2_platform.py +++ b/dimos/robot/unitree/go2/blueprints/basic/go2_platform.py @@ -25,13 +25,13 @@ def resolve_go2_platform() -> Blueprint: - if not global_config.simulation: + if global_config.simulation in ("", "dimsim"): return GO2Connection.blueprint() return _resolve_simulation_binding().backend def resolve_go2_rerun_config() -> dict[str, object]: - if not global_config.simulation: + if global_config.simulation in ("", "dimsim"): return {} return _resolve_simulation_binding().rerun_config diff --git a/dimos/simulation/mujoco/direct_cmd_vel_explorer.py b/dimos/simulation/mujoco/direct_cmd_vel_explorer.py index 81b7f62156..1b563f7ab5 100644 --- a/dimos/simulation/mujoco/direct_cmd_vel_explorer.py +++ b/dimos/simulation/mujoco/direct_cmd_vel_explorer.py @@ -14,9 +14,10 @@ import math import threading -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, cast -from dimos.core.transport import LCMTransport +from dimos.core.transport import PubSubTransport +from dimos.core.transport_factory import make_transport from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -35,15 +36,21 @@ def __init__( self.linear_speed = linear_speed self.rotation_speed = rotation_speed self._dt = 1.0 / publish_rate - self._cmd_vel: LCMTransport[Twist] | None = None - self._odom: LCMTransport[PoseStamped] | None = None + self._cmd_vel: PubSubTransport[Twist] | None = None + self._odom: PubSubTransport[PoseStamped] | None = None self._pose: PoseStamped | None = None self._new_pose = threading.Event() self._unsub: Callable[[], None] | None = None def start(self) -> None: - self._cmd_vel = LCMTransport("/cmd_vel", Twist) - self._odom = LCMTransport("/odom", PoseStamped) + self._cmd_vel = cast( + "PubSubTransport[Twist]", + make_transport("/cmd_vel", Twist), + ) + self._odom = cast( + "PubSubTransport[PoseStamped]", + make_transport("/odom", PoseStamped), + ) self._pose = None self._unsub = self._odom.subscribe(self._on_odom) From 071ebb30faaa3166b81beaa2b262b3caa42c406e Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Fri, 31 Jul 2026 10:30:12 +0800 Subject: [PATCH 18/33] Fix transport-agnostic agent readiness in E2E tests --- dimos/agents/mcp/mcp_client.py | 1 + dimos/agents/mcp/test_mcp_client_unit.py | 12 ++++++++++++ dimos/e2e_tests/conftest.py | 11 +++++++++++ dimos/e2e_tests/test_dimsim_path_replaning.py | 4 ++-- dimos/e2e_tests/test_dimsim_walk_forward.py | 12 +++++++++--- 5 files changed, 35 insertions(+), 5 deletions(-) diff --git a/dimos/agents/mcp/mcp_client.py b/dimos/agents/mcp/mcp_client.py index 859b15451b..600fcd7e9e 100644 --- a/dimos/agents/mcp/mcp_client.py +++ b/dimos/agents/mcp/mcp_client.py @@ -243,6 +243,7 @@ def on_system_modules(self, _modules: list[RPCClient]) -> None: ) if not self._thread.is_alive(): self._thread.start() + self.agent_idle.publish(True) @rpc def stop(self) -> None: diff --git a/dimos/agents/mcp/test_mcp_client_unit.py b/dimos/agents/mcp/test_mcp_client_unit.py index a49df130ff..f6d48d64f1 100644 --- a/dimos/agents/mcp/test_mcp_client_unit.py +++ b/dimos/agents/mcp/test_mcp_client_unit.py @@ -250,6 +250,18 @@ def test_on_system_modules_uses_responses_api_model( assert model.reasoning == {"effort": "medium", "summary": "auto"} +def test_on_system_modules_publishes_initial_idle( + configured_mcp_client: McpClient, +) -> None: + with ( + patch("dimos.agents.mcp.mcp_client.create_agent"), + patch.object(configured_mcp_client.agent_idle, "publish") as publish, + ): + configured_mcp_client.on_system_modules([]) + + publish.assert_called_once_with(True) + + @pytest.mark.parametrize("model_name", ["gpt-4o", "ollama:qwen3:8b", "huggingface:Qwen/Qwen3-8B"]) def test_on_system_modules_resolves_non_reasoning_models( configured_mcp_client: McpClient, model_name: str diff --git a/dimos/e2e_tests/conftest.py b/dimos/e2e_tests/conftest.py index e5a4ed4636..8198cee629 100644 --- a/dimos/e2e_tests/conftest.py +++ b/dimos/e2e_tests/conftest.py @@ -70,6 +70,17 @@ def lcm_spy(transport_runtime: None) -> Iterator[LcmSpy]: lcm_spy.stop() +@pytest.fixture +def wait_for_agent_ready(lcm_spy: LcmSpy) -> Callable[[float], None]: + topic = "/agent_idle" + lcm_spy.save_topic(topic) + + def wait(timeout: float = 120.0) -> None: + lcm_spy.wait_for_saved_topic(topic, timeout=timeout) + + return wait + + @pytest.fixture def follow_points(lcm_spy: LcmSpy): def fun(*, points: list[tuple[float, float, float]], fail_message: str) -> None: diff --git a/dimos/e2e_tests/test_dimsim_path_replaning.py b/dimos/e2e_tests/test_dimsim_path_replaning.py index 38fc099f3d..7fa3455600 100644 --- a/dimos/e2e_tests/test_dimsim_path_replaning.py +++ b/dimos/e2e_tests/test_dimsim_path_replaning.py @@ -21,6 +21,7 @@ def test_path_replanning( lcm_spy, start_blueprint, + wait_for_agent_ready, scene_control, simulator_name, direct_cmd_vel_explorer, @@ -32,8 +33,7 @@ def test_path_replanning( else ("run", "unitree-go2-agentic") ) start_blueprint(*args, simulator=simulator_name, scene_package="none") - lcm_spy.save_topic("/rpc/McpClient/on_system_modules/res") - lcm_spy.wait_for_saved_topic("/rpc/McpClient/on_system_modules/res", timeout=1200.0) + wait_for_agent_ready(timeout=1200.0) scene_control.set_agent_position(3, 2) diff --git a/dimos/e2e_tests/test_dimsim_walk_forward.py b/dimos/e2e_tests/test_dimsim_walk_forward.py index c95039fb8f..50d9c280ea 100644 --- a/dimos/e2e_tests/test_dimsim_walk_forward.py +++ b/dimos/e2e_tests/test_dimsim_walk_forward.py @@ -16,7 +16,14 @@ @pytest.mark.self_hosted_large -def test_walk_forward(lcm_spy, start_blueprint, human_input, scene_control, simulator_name) -> None: +def test_walk_forward( + lcm_spy, + start_blueprint, + wait_for_agent_ready, + human_input, + scene_control, + simulator_name, +) -> None: scene_args = ("--dimsim-scene=empty",) if simulator_name == "dimsim" else () start_blueprint( *scene_args, @@ -29,8 +36,7 @@ def test_walk_forward(lcm_spy, start_blueprint, human_input, scene_control, simu simulator=simulator_name, scene_package="none", ) - lcm_spy.save_topic("/rpc/McpClient/on_system_modules/res") - lcm_spy.wait_for_saved_topic("/rpc/McpClient/on_system_modules/res", timeout=1200.0) + wait_for_agent_ready(timeout=1200.0) origin_x, origin_y = 1, 2 scene_control.set_agent_position(origin_x, origin_y) From c569c638c336885c537bd7bb8d729dfb6bc8a924 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Mon, 3 Aug 2026 09:22:15 +0800 Subject: [PATCH 19/33] fix macOS CLIP inference in workers --- dimos/perception/experimental/image_embedding.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/dimos/perception/experimental/image_embedding.py b/dimos/perception/experimental/image_embedding.py index 78cd803d81..854076104d 100644 --- a/dimos/perception/experimental/image_embedding.py +++ b/dimos/perception/experimental/image_embedding.py @@ -80,13 +80,13 @@ def _initialize_model(self): # type: ignore[no-untyped-def] providers = ["CUDAExecutionProvider", "CPUExecutionProvider"] if sys.platform == "darwin": - # 2025-11-17 12:36:47.877215 [W:onnxruntime:, helper.cc:82 IsInputSupported] CoreML does not support input dim > 16384. Input:text_model.embeddings.token_embedding.weight, shape: {49408,512} - # 2025-11-17 12:36:47.878496 [W:onnxruntime:, coreml_execution_provider.cc:107 GetCapability] CoreMLExecutionProvider::GetCapability, number of partitions supported by CoreML: 88 number of nodes in the graph: 1504 number of nodes supported by CoreML: 933 - providers = ["CoreMLExecutionProvider"] + [ - each for each in providers if each != "CUDAExecutionProvider" - ] + # CoreML's Metal compiler connection is not reliable in forkserver workers. + providers = ["CPUExecutionProvider"] - self.model = ort.InferenceSession(str(model_id), providers=providers) + self.model = ort.InferenceSession( + str(model_id), + providers=providers, + ) actual_providers = self.model.get_providers() # type: ignore[attr-defined] self.processor = CLIPProcessor.from_pretrained(processor_id) From e76029dd5473619519c77c2c63b500261e6a1b26 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Mon, 3 Aug 2026 13:08:34 +0800 Subject: [PATCH 20/33] test(simulation): unify semantic navigation acceptance --- dimos/e2e_tests/conftest.py | 47 ++++---- dimos/e2e_tests/dim_sim_client.py | 14 +++ dimos/e2e_tests/lcm_spy.py | 39 ++++++- dimos/e2e_tests/scene_contract.py | 45 ++++++++ dimos/e2e_tests/scene_control.py | 9 +- dimos/e2e_tests/simulation_scenarios.py | 109 ++++++++++++++++++ dimos/e2e_tests/test_dimsim_spatial_memory.py | 100 +++++++++++++++- dimos/e2e_tests/test_lcm_spy.py | 32 +++++ dimos/e2e_tests/test_scene_contract.py | 43 +++++++ dimos/simulation/dimsim/scene_client.py | 32 +++++ docs/development/testing.md | 33 ++++++ misc/DimSim/README.md | 10 +- misc/DimSim/cli/cli.ts | 6 +- misc/DimSim/docs/evals.md | 23 ++-- misc/DimSim/evals/deno-client.ts | 2 +- .../scenes/apartment/evals/go-to-couch.js | 9 -- .../scenes/apartment/evals/go-to-kitchen.js | 9 -- .../DimSim/scenes/apartment/evals/go-to-tv.js | 9 -- misc/DimSim/src/sceneEditor.ts | 2 +- 19 files changed, 489 insertions(+), 84 deletions(-) create mode 100644 dimos/e2e_tests/scene_contract.py create mode 100644 dimos/e2e_tests/simulation_scenarios.py create mode 100644 dimos/e2e_tests/test_lcm_spy.py create mode 100644 dimos/e2e_tests/test_scene_contract.py delete mode 100644 misc/DimSim/scenes/apartment/evals/go-to-couch.js delete mode 100644 misc/DimSim/scenes/apartment/evals/go-to-kitchen.js delete mode 100644 misc/DimSim/scenes/apartment/evals/go-to-tv.js diff --git a/dimos/e2e_tests/conftest.py b/dimos/e2e_tests/conftest.py index 8198cee629..678ab8cffd 100644 --- a/dimos/e2e_tests/conftest.py +++ b/dimos/e2e_tests/conftest.py @@ -25,6 +25,7 @@ from dimos.e2e_tests.dimos_cli_call import DimosCliCall from dimos.e2e_tests.lcm_spy import LcmSpy from dimos.e2e_tests.scene_control import SceneControl, load_scene_control +from dimos.e2e_tests.simulation_scenarios import APARTMENT_EXPLORATION_ROUTE from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import make_vector3 @@ -81,6 +82,17 @@ def wait(timeout: float = 120.0) -> None: return wait +@pytest.fixture +def wait_for_robot_odometry(lcm_spy: LcmSpy) -> Callable[[float], None]: + topic = "/odom#geometry_msgs.PoseStamped" + lcm_spy.save_topic(topic) + + def wait(timeout: float = 60.0) -> None: + lcm_spy.wait_for_saved_topic(topic, timeout=timeout) + + return wait + + @pytest.fixture def follow_points(lcm_spy: LcmSpy): def fun(*, points: list[tuple[float, float, float]], fail_message: str) -> None: @@ -207,7 +219,13 @@ def explore() -> None: @pytest.fixture def simulator_name() -> str: - return os.environ.get("DIMOS_E2E_SIMULATOR", "pimsim") + simulator = os.environ.get("DIMOS_E2E_SIMULATOR", "pimsim") + if simulator == "dimsim" and global_config.transport != "lcm": + raise pytest.UsageError( + "native DimSim publishes its robot bridge over LCM; use " + "DIMOS_TRANSPORT=lcm for DIMOS_E2E_SIMULATOR=dimsim" + ) + return simulator @pytest.fixture @@ -271,34 +289,9 @@ def worker(): def explore_house( direct_cmd_vel_explorer: DirectCmdVelExplorer, ) -> Callable[[], None]: - points = [ - (3.881, 4.803), - (4.160, 1.615), - (1.596, 1.505), - (1.649, 0.137), - (-3.644, -0.064), - (-3.759, -2.661), - (-4.186, -4.830), - (-3.759, -2.661), - (-1.070, -3.285), - (-2.504, -2.452), - (-2.647, 5.243), - (-3.663, 3.591), - (-1.178, 1.974), - (-2.416, 2.629), - (-2.581, 0.164), - (1.834, 0.072), - (3.010, -3.883), - (1.756, -3.742), - (6.336, -4.077), - (8.264, -5.119), - (6.258, -0.964), - (6.453, 5.327), - ] - direct_cmd_vel_explorer.linear_speed = 0.5 def explore() -> None: - direct_cmd_vel_explorer.follow_points(points) + direct_cmd_vel_explorer.follow_points(list(APARTMENT_EXPLORATION_ROUTE)) return explore diff --git a/dimos/e2e_tests/dim_sim_client.py b/dimos/e2e_tests/dim_sim_client.py index 9187e5f634..8562bb4359 100644 --- a/dimos/e2e_tests/dim_sim_client.py +++ b/dimos/e2e_tests/dim_sim_client.py @@ -16,6 +16,7 @@ from dimos.core.transport import PubSubTransport from dimos.core.transport_factory import make_transport +from dimos.e2e_tests.scene_contract import PlanarBounds from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.simulation.dimsim.scene_client import SceneClient @@ -61,3 +62,16 @@ def publish_goal(self, x: float, y: float) -> None: frame_id="world", ), ) + + def semantic_object_bounds(self, query: str) -> PlanarBounds: + bounds = self.client.get_semantic_object_bounds(query) + minimum = bounds["min"] + maximum = bounds["max"] + # DimSim is Three.js Y-up. Its bridge publishes (z, x, y) as + # canonical DimOS (x, y, z), so apply the same mapping to the AABB. + return PlanarBounds( + min_x=float(minimum["z"]), + min_y=float(minimum["x"]), + max_x=float(maximum["z"]), + max_y=float(maximum["x"]), + ) diff --git a/dimos/e2e_tests/lcm_spy.py b/dimos/e2e_tests/lcm_spy.py index 2a7e281f7e..0d6775d014 100644 --- a/dimos/e2e_tests/lcm_spy.py +++ b/dimos/e2e_tests/lcm_spy.py @@ -17,10 +17,11 @@ import math import pickle import threading -from typing import Any +from typing import Any, cast from dimos.core.transport import PubSubTransport from dimos.core.transport_factory import make_transport +from dimos.e2e_tests.scene_contract import PlanarBounds from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.helpers import resolve_msg_type from dimos.msgs.protocol import DimosMsg @@ -173,6 +174,23 @@ def listener(msg: bytes) -> None: message=fail_message, ) + def wait_for_saved_message_result( + self, + topic: str, + type: type[DimosMsg], + predicate: Callable[[Any], bool], + fail_message: str, + timeout: float = 30.0, + ) -> None: + """Wait for a matching message saved since ``save_topic`` was called.""" + + def condition() -> bool: + with self._messages_lock: + messages = tuple(self.messages.get(topic, ())) + return any(predicate(type.lcm_decode(message)) for message in messages) + + wait_until(condition, timeout=timeout, message=fail_message) + def wait_until_odom_position( self, x: float, y: float, threshold: float = 1, timeout: float = 60 ) -> None: @@ -189,6 +207,23 @@ def predicate(msg: PoseStamped) -> bool: timeout, ) + def wait_until_odom_near_bounds( + self, + bounds: PlanarBounds, + max_distance: float, + timeout: float = 60.0, + ) -> None: + def predicate(msg: PoseStamped) -> bool: + return bounds.distance_to(msg.position.x, msg.position.y) <= max_distance + + self.wait_for_message_result( + "/odom#geometry_msgs.PoseStamped", + PoseStamped, + predicate, + f"Robot did not get within {max_distance} m of semantic target bounds {bounds}", + timeout, + ) + def _ensure_subscription(self, topic: str) -> None: with self._transports_lock: if topic in self._transports: @@ -212,5 +247,5 @@ def _parse_topic(topic: str, default_type: type[Any] | None = None) -> tuple[str def _encode_message(message: Any) -> bytes: if hasattr(message, "lcm_encode"): - return message.lcm_encode() + return cast("bytes", message.lcm_encode()) return pickle.dumps(message) diff --git a/dimos/e2e_tests/scene_contract.py b/dimos/e2e_tests/scene_contract.py new file mode 100644 index 0000000000..e35e429be2 --- /dev/null +++ b/dimos/e2e_tests/scene_contract.py @@ -0,0 +1,45 @@ +# 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. + +from __future__ import annotations + +from dataclasses import dataclass +import math + + +@dataclass(frozen=True) +class PlanarBounds: + """Axis-aligned bounds in the canonical DimOS world XY plane.""" + + min_x: float + min_y: float + max_x: float + max_y: float + + def __post_init__(self) -> None: + values = (self.min_x, self.min_y, self.max_x, self.max_y) + if not all(math.isfinite(value) for value in values): + raise ValueError("planar bounds must be finite") + if self.min_x > self.max_x or self.min_y > self.max_y: + raise ValueError("planar bounds minimum must not exceed maximum") + + def distance_to(self, x: float, y: float) -> float: + """Return planar distance from a point to the filled bounds.""" + + dx = max(self.min_x - x, 0.0, x - self.max_x) + dy = max(self.min_y - y, 0.0, y - self.max_y) + return math.hypot(dx, dy) + + +__all__ = ["PlanarBounds"] diff --git a/dimos/e2e_tests/scene_control.py b/dimos/e2e_tests/scene_control.py index 8e95c68226..8e1b56df58 100644 --- a/dimos/e2e_tests/scene_control.py +++ b/dimos/e2e_tests/scene_control.py @@ -15,9 +15,10 @@ from __future__ import annotations from importlib.metadata import entry_points -from typing import Protocol +from typing import Protocol, cast from dimos.e2e_tests.dim_sim_client import DimSimClient +from dimos.e2e_tests.scene_contract import PlanarBounds class SceneControl(Protocol): @@ -31,6 +32,8 @@ def add_wall(self, x1: float, y1: float, x2: float, y2: float) -> None: ... def publish_goal(self, x: float, y: float) -> None: ... + def semantic_object_bounds(self, query: str) -> PlanarBounds: ... + def load_scene_control(simulator: str) -> SceneControl: if simulator == "dimsim": @@ -45,7 +48,7 @@ def load_scene_control(simulator: str) -> SceneControl: f"expected one scene-control provider for {simulator!r}, found {len(matches)}" ) client = matches[0].load()() - return client + return cast("SceneControl", client) -__all__ = ["SceneControl", "load_scene_control"] +__all__ = ["PlanarBounds", "SceneControl", "load_scene_control"] diff --git a/dimos/e2e_tests/simulation_scenarios.py b/dimos/e2e_tests/simulation_scenarios.py new file mode 100644 index 0000000000..e5cf98a11c --- /dev/null +++ b/dimos/e2e_tests/simulation_scenarios.py @@ -0,0 +1,109 @@ +# 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. + +from __future__ import annotations + +from dataclasses import dataclass +import math + + +@dataclass(frozen=True) +class SemanticNavigationScenario: + """Provider-neutral contract for one semantic navigation task.""" + + scenario_id: str + command: str + target_query: str + max_target_distance_m: float + navigation_timeout_s: float = 180.0 + + def __post_init__(self) -> None: + if not self.scenario_id.strip(): + raise ValueError("scenario ID must not be empty") + if not self.command.strip() or not self.target_query.strip(): + raise ValueError("semantic navigation text must not be empty") + if not math.isfinite(self.max_target_distance_m) or self.max_target_distance_m <= 0: + raise ValueError("target distance must be finite and positive") + if not math.isfinite(self.navigation_timeout_s) or self.navigation_timeout_s <= 0: + raise ValueError("navigation timeout must be finite and positive") + + +# Canonical DimOS world-frame route used to populate spatial memory. Both +# simulator providers must present the apartment in this frame. +APARTMENT_EXPLORATION_ROUTE: tuple[tuple[float, float], ...] = ( + (3.881, 4.803), + (4.160, 1.615), + (1.596, 1.505), + (1.649, 0.137), + (-3.644, -0.064), + (-3.759, -2.661), + (-4.186, -4.830), + (-3.759, -2.661), + (-1.070, -3.285), + (-2.504, -2.452), + (-2.647, 5.243), + (-3.663, 3.591), + (-1.178, 1.974), + (-2.416, 2.629), + (-2.581, 0.164), + (1.834, 0.072), + (3.010, -3.883), + (1.756, -3.742), + (6.336, -4.077), + (8.264, -5.119), + (6.258, -0.964), + (6.453, 5.327), +) + +# The former browser-local workflows all started at Three.js (0, 0.5, 3), +# which is canonical DimOS (3, 0, 0.5). Use a slightly settled root height for +# both providers and restore this neutral start after exploration. +APARTMENT_TASK_START: tuple[float, float, float] = (3.0, 0.0, 0.52) + +GO_TO_BED = SemanticNavigationScenario( + scenario_id="bed", + command="go to the bed", + target_query="queen size bed", + max_target_distance_m=2.0, +) + +APARTMENT_SEMANTIC_NAVIGATION_SCENARIOS: tuple[SemanticNavigationScenario, ...] = ( + SemanticNavigationScenario( + scenario_id="couch", + command="go to the couch", + target_query="sectional", + max_target_distance_m=2.0, + ), + SemanticNavigationScenario( + scenario_id="kitchen", + command="go to the kitchen", + target_query="refrigerator", + max_target_distance_m=3.0, + ), + SemanticNavigationScenario( + scenario_id="television", + command="go to the TV", + target_query="television", + max_target_distance_m=2.0, + ), +) + + +__all__ = [ + "APARTMENT_EXPLORATION_ROUTE", + "APARTMENT_SEMANTIC_NAVIGATION_SCENARIOS", + "APARTMENT_TASK_START", + "GO_TO_BED", + "SemanticNavigationScenario", +] diff --git a/dimos/e2e_tests/test_dimsim_spatial_memory.py b/dimos/e2e_tests/test_dimsim_spatial_memory.py index 97926e998a..e472542eb0 100644 --- a/dimos/e2e_tests/test_dimsim_spatial_memory.py +++ b/dimos/e2e_tests/test_dimsim_spatial_memory.py @@ -14,11 +14,24 @@ import pytest +from dimos.e2e_tests.simulation_scenarios import ( + APARTMENT_SEMANTIC_NAVIGATION_SCENARIOS, + APARTMENT_TASK_START, + GO_TO_BED, + SemanticNavigationScenario, +) +from dimos.msgs.std_msgs.Bool import Bool -@pytest.mark.self_hosted_large -def test_go_to_the_bed( +_GOAL_REACHED_TOPIC = "/goal_reached#std_msgs.Bool" + + +def _run_semantic_navigation_scenario( + scenario: SemanticNavigationScenario, + *, lcm_spy, start_blueprint, + wait_for_agent_ready, + wait_for_robot_odometry, human_input, scene_control, simulator_name, @@ -29,11 +42,86 @@ def test_go_to_the_bed( "unitree-go2-agentic", simulator=simulator_name, ) - lcm_spy.save_topic("/rpc/McpClient/on_system_modules/res") - lcm_spy.wait_for_saved_topic("/rpc/McpClient/on_system_modules/res", timeout=1200.0) + wait_for_agent_ready(timeout=1200.0) + wait_for_robot_odometry(timeout=120.0) + target_bounds = scene_control.semantic_object_bounds(scenario.target_query) explore_house() + scene_control.set_agent_position(*APARTMENT_TASK_START) + lcm_spy.wait_until_odom_position( + APARTMENT_TASK_START[0], + APARTMENT_TASK_START[1], + threshold=0.25, + timeout=30.0, + ) + + # Subscribe before sending the task so an immediate completion cannot race + # the assertion. The semantic bounds remain test-only ground truth. + lcm_spy.save_topic(_GOAL_REACHED_TOPIC) + human_input(scenario.command) + lcm_spy.wait_for_saved_message_result( + _GOAL_REACHED_TOPIC, + Bool, + predicate=lambda message: message.data is True, + fail_message=f"Navigation did not report completion for {scenario.command!r}", + timeout=scenario.navigation_timeout_s, + ) + lcm_spy.wait_until_odom_near_bounds( + target_bounds, + max_distance=scenario.max_target_distance_m, + timeout=30.0, + ) - human_input("go to the bed") - lcm_spy.wait_until_odom_position(-3.567, -1.332, threshold=2, timeout=180) +@pytest.mark.self_hosted_large +def test_go_to_the_bed( + lcm_spy, + start_blueprint, + wait_for_agent_ready, + human_input, + scene_control, + simulator_name, + explore_house, + wait_for_robot_odometry, +) -> None: + _run_semantic_navigation_scenario( + GO_TO_BED, + lcm_spy=lcm_spy, + start_blueprint=start_blueprint, + wait_for_agent_ready=wait_for_agent_ready, + wait_for_robot_odometry=wait_for_robot_odometry, + human_input=human_input, + scene_control=scene_control, + simulator_name=simulator_name, + explore_house=explore_house, + ) + + +@pytest.mark.self_hosted_large +@pytest.mark.parametrize( + "scenario", + APARTMENT_SEMANTIC_NAVIGATION_SCENARIOS, + ids=lambda scenario: scenario.scenario_id, +) +def test_apartment_semantic_navigation( + scenario, + lcm_spy, + start_blueprint, + wait_for_agent_ready, + wait_for_robot_odometry, + human_input, + scene_control, + simulator_name, + explore_house, +) -> None: + _run_semantic_navigation_scenario( + scenario, + lcm_spy=lcm_spy, + start_blueprint=start_blueprint, + wait_for_agent_ready=wait_for_agent_ready, + wait_for_robot_odometry=wait_for_robot_odometry, + human_input=human_input, + scene_control=scene_control, + simulator_name=simulator_name, + explore_house=explore_house, + ) diff --git a/dimos/e2e_tests/test_lcm_spy.py b/dimos/e2e_tests/test_lcm_spy.py new file mode 100644 index 0000000000..55dc131efe --- /dev/null +++ b/dimos/e2e_tests/test_lcm_spy.py @@ -0,0 +1,32 @@ +# 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. + +from dimos.e2e_tests.lcm_spy import LcmSpy +from dimos.msgs.std_msgs.Bool import Bool + + +def test_wait_for_saved_message_result_matches_message_received_before_wait(mocker) -> None: + spy = LcmSpy() + mocker.patch.object(spy, "_ensure_subscription") + topic = "/goal_reached#std_msgs.Bool" + spy.save_topic(topic) + spy.msg(topic, Bool(True).lcm_encode()) + + spy.wait_for_saved_message_result( + topic, + Bool, + predicate=lambda message: message.data is True, + fail_message="missing completion", + timeout=0.1, + ) diff --git a/dimos/e2e_tests/test_scene_contract.py b/dimos/e2e_tests/test_scene_contract.py new file mode 100644 index 0000000000..21e71b8b97 --- /dev/null +++ b/dimos/e2e_tests/test_scene_contract.py @@ -0,0 +1,43 @@ +# 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. + +import pytest + +from dimos.e2e_tests.dim_sim_client import DimSimClient +from dimos.e2e_tests.scene_contract import PlanarBounds + + +def test_planar_bounds_distance_uses_nearest_point() -> None: + bounds = PlanarBounds(min_x=1.0, min_y=2.0, max_x=3.0, max_y=4.0) + + assert bounds.distance_to(2.0, 3.0) == 0.0 + assert bounds.distance_to(4.0, 5.0) == pytest.approx(2**0.5) + + +def test_dimsim_client_maps_browser_bounds_to_dimos_world(mocker) -> None: + mocker.patch("dimos.e2e_tests.dim_sim_client.make_transport") + browser_client = mocker.Mock() + browser_client.get_semantic_object_bounds.return_value = { + "min": {"x": -2.0, "y": 0.0, "z": 3.0}, + "max": {"x": 1.0, "y": 2.0, "z": 5.0}, + } + client = DimSimClient() + client._client = browser_client + + assert client.semantic_object_bounds("bed") == PlanarBounds( + min_x=3.0, + min_y=-2.0, + max_x=5.0, + max_y=1.0, + ) diff --git a/dimos/simulation/dimsim/scene_client.py b/dimos/simulation/dimsim/scene_client.py index d7234626c8..5dd555bac4 100644 --- a/dimos/simulation/dimsim/scene_client.py +++ b/dimos/simulation/dimsim/scene_client.py @@ -926,3 +926,35 @@ def get_agent_position(self) -> dict[str, Any]: return { x: p.x, y: p.y, z: p.z }; """ return cast("dict[str, Any]", self.exec(code)) + + def get_semantic_object_bounds(self, query: str) -> dict[str, Any]: + """Return the live Three.js world AABB for a semantic scene object.""" + + if not query.strip(): + raise ValueError("semantic object query must not be empty") + code = f""" +const normalize = (value) => String(value || "") + .toLowerCase() + .replace(/[^a-z0-9]+/g, " ") + .trim(); +const needle = normalize({json.dumps(query)}); +const entry = assets.find((asset) => + normalize(`${{asset.title || ""}} ${{asset.id || ""}}`).includes(needle) +); +if (!entry) return null; +const object = assetsGroup.getObjectByName(`asset:${{entry.id}}`); +if (!object) return null; +object.updateMatrixWorld(true); +const bounds = new THREE.Box3().setFromObject(object); +if (bounds.isEmpty()) return null; +return {{ + id: entry.id, + title: entry.title || null, + min: {{ x: bounds.min.x, y: bounds.min.y, z: bounds.min.z }}, + max: {{ x: bounds.max.x, y: bounds.max.y, z: bounds.max.z }}, +}}; +""" + result = self.exec(code) + if not isinstance(result, dict): + raise LookupError(f"semantic object not found in DimSim scene: {query!r}") + return result diff --git a/docs/development/testing.md b/docs/development/testing.md index 0de17adc77..868c073860 100644 --- a/docs/development/testing.md +++ b/docs/development/testing.md @@ -69,6 +69,39 @@ When writing or debugging a specific self-hosted test, override `-m` yourself to pytest -m self_hosted dimos/path/to/test_something.py ``` +### Cross-simulator system tests + +DimOS system acceptance scenarios live in `dimos/e2e_tests/` and run against +each supported simulator through the `SceneControl` provider contract. The +test owns the command, setup, public completion assertion, and scoring rule; +the provider owns only simulator-specific control and test-oracle queries. + +Select the provider without changing the test body: + +```bash +DIMOS_TRANSPORT=zenoh DIMOS_E2E_SIMULATOR=pimsim \ + pytest -o addopts='' -m self_hosted_large \ + dimos/e2e_tests/test_dimsim_spatial_memory.py + +DIMOS_TRANSPORT=lcm DIMOS_E2E_SIMULATOR=dimsim \ + pytest -o addopts='' -m self_hosted_large \ + dimos/e2e_tests/test_dimsim_spatial_memory.py +``` + +The scenario body and acceptance contract are provider-neutral, but native +DimSim's browser bridge is currently LCM-only. PimSim is the maintained Zenoh +path. Selecting native DimSim with Zenoh is rejected immediately instead of +waiting for odometry that cannot arrive. + +Semantic scene metadata may be used by the test after the task as ground truth +for scoring. It must not be published to the robot or substituted for its +camera, perception, mapping, memory, or navigation inputs. + +DimSim's JavaScript eval harness is a separate simulator-local layer for checks +that inherently require privileged Three.js or Rapier state. Its `task` field +is only a display label and is not delivered to DimOS, so agent and robot +workflows must not be owned or duplicated there. + ## Testing on a fresh Ubuntu install CI tests dimos with pre-built images and cached deps, so it can't catch gaps diff --git a/misc/DimSim/README.md b/misc/DimSim/README.md index affe85a50c..57723ece26 100644 --- a/misc/DimSim/README.md +++ b/misc/DimSim/README.md @@ -5,8 +5,8 @@ Browser-based 3D simulator (Three.js + Rapier) plus a Deno bridge that talks LCM ``` src/ — browser engine (vite-bundled) cli/ — Deno CLI + bridge server + headless launcher + LCM vendor -evals/ — eval harness (browser) + runner (Deno) + rubrics -scenes/ — user-authored scenes (JS) + per-scene eval workflows +evals/ — simulator-local harness (browser) + runner (Deno) + rubrics +scenes/ — user-authored scenes (JS) + optional engine-level workflows public/ — static assets (agent GLB, logo) docs/ — guides ``` @@ -26,7 +26,7 @@ On first run, `cli/cli.ts` will build `dist/` via Vite (dimsim ships its fronten - [docs/getting-started.md](docs/getting-started.md) — 5-minute tour - [docs/scenes.md](docs/scenes.md) — create + edit scenes -- [docs/evals.md](docs/evals.md) — write eval workflows +- [docs/evals.md](docs/evals.md) — simulator-local workflows vs DimOS system tests ## Install the CLI (optional) @@ -42,8 +42,8 @@ After install: ```bash dimsim dev --scene apartment # standalone dev server + browser dimsim eval list # list workflows under scenes/*/evals/ -dimsim eval go-to-couch # run one workflow against an open sim -dimsim eval --headless --scene apartment # full headless run (CI) +dimsim eval # run a simulator-local workflow +dimsim eval --headless --scene apartment --workflow ``` ## Build manually diff --git a/misc/DimSim/cli/cli.ts b/misc/DimSim/cli/cli.ts index 7a5d6a79c0..41038dfc9a 100644 --- a/misc/DimSim/cli/cli.ts +++ b/misc/DimSim/cli/cli.ts @@ -303,9 +303,9 @@ async function main() { // ── Eval ──────────────────────────────────────────────────────────── if (subcommand === "eval") { - // Positional workflow: `dimsim eval go-to-tv` is shorthand for - // `dimsim eval --workflow go-to-tv --connect`. Accepts either bare - // workflow name ("go-to-tv") or scene-qualified ("apartment/go-to-tv"). + // Positional workflow: `dimsim eval scene-smoke` is shorthand for + // `dimsim eval --workflow scene-smoke --connect`. Accepts either bare + // workflow name ("scene-smoke") or scene-qualified ("apartment/scene-smoke"). const positional = Deno.args[1] && !Deno.args[1].startsWith("--") ? Deno.args[1] : null; let posScene: string | undefined; let posWorkflow: string | undefined; diff --git a/misc/DimSim/docs/evals.md b/misc/DimSim/docs/evals.md index 79615045b6..c210002fe1 100644 --- a/misc/DimSim/docs/evals.md +++ b/misc/DimSim/docs/evals.md @@ -1,18 +1,22 @@ -# Evals +# Simulator-local evals -An eval workflow is one JS file at `scenes//evals/.js`. It imports `runEval` from `@dimsim/eval` and calls it. That's the whole authoring surface. +DimSim's JavaScript eval harness is for simulator-local engine and scene checks. It has privileged access to Three.js objects, Rapier, and the agent pose. The `task` field is a display and reporting label; it is **not** sent to a DimOS agent. + +End-to-end robot tasks belong in `dimos/e2e_tests/`. Those tests send commands through DimOS, observe public DimOS streams, and run unchanged against each simulator through the `SceneControl` provider contract. Do not duplicate a system acceptance scenario as a JavaScript workflow. + +A simulator-local workflow is one JS file at `scenes//evals/.js`. It imports `runEval` from `@dimsim/eval` and calls it. ## Create a new eval ```js -// scenes/apartment/evals/go-to-couch.js +// scenes/apartment/evals/sectional-bounds-smoke.js import { runEval } from '@dimsim/eval'; await runEval({ scene: 'apartment', - task: 'Go to the couch', + task: 'Sectional object-distance rubric resolves live bounds', timeoutSec: 30, - startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + startPose: { x: 4, y: 0.5, z: 1, yaw: 0 }, success: (ctx) => ctx.rubrics.objectDistance({ target: 'sectional', thresholdM: 2.0 }), }); ``` @@ -22,9 +26,9 @@ Drop the file under any scene's `evals/` folder and `dimsim eval list` picks it ## Run it ```bash -dimsim eval go-to-couch # against the open sim -dimsim eval --headless --scene apartment --workflow go-to-couch # standalone / CI -deno run -A misc/DimSim/scenes/apartment/evals/go-to-couch.js # direct execution +dimsim eval sectional-bounds-smoke +dimsim eval --headless --scene apartment --workflow sectional-bounds-smoke +deno run -A misc/DimSim/scenes/apartment/evals/sectional-bounds-smoke.js ``` All three end up at the same harness in the browser. Pick whichever fits the moment. @@ -34,7 +38,7 @@ All three end up at the same harness in the browser. Pick whichever fits the mom | Field | Required | Description | |---|---|---| | `scene` | ✓ | Scene name. Must match a directory under `scenes/`. | -| `task` | ✓ | Human-readable goal. Shown in the overlay + logged. | +| `task` | ✓ | Human-readable label shown in the overlay and logs. It is not delivered to DimOS. | | `success(ctx)` | ✓ | Returns `{passed, reason?, score?}`. Polled every 250 ms until it passes or timeout. | | `timeoutSec` | – | Default 120. Wall-clock cap. | | `startPose` | – | `{x, y, z, yaw?}`, applied before `setup`. Yaw in degrees. | @@ -89,6 +93,7 @@ You can spawn obstacles, change embodiments mid-eval, or set up multi-stage test ## Tips +- Use pytest for agent, perception, mapping, planning, transport, or robot behavior. Use this harness only when the assertion inherently needs browser-local scene or engine state. - One eval at a time. The harness is a singleton, so running two evals concurrently isn't supported. Use `--parallel N` with multiple browser pages for throughput. - Score is yours to define. Lower-is-better for distances, higher-is-better for coverage. CI consumers should not assume. - `startPose` yaw is in degrees, not radians. diff --git a/misc/DimSim/evals/deno-client.ts b/misc/DimSim/evals/deno-client.ts index 4bf67d1563..c3857be634 100644 --- a/misc/DimSim/evals/deno-client.ts +++ b/misc/DimSim/evals/deno-client.ts @@ -50,7 +50,7 @@ function _resolveWorkflowUrl(): string { `@dimsim/eval: workflow file must live under a 'scenes/' directory; got ${abs}`, ); } - return abs.slice(i); // e.g. "/scenes/apartment/evals/go-to-couch.js" + return abs.slice(i); // e.g. "/scenes/apartment/evals/scene-smoke.js" } /** Open the control WebSocket, race resolve / error / 5s timeout. */ diff --git a/misc/DimSim/scenes/apartment/evals/go-to-couch.js b/misc/DimSim/scenes/apartment/evals/go-to-couch.js deleted file mode 100644 index 30b36db952..0000000000 --- a/misc/DimSim/scenes/apartment/evals/go-to-couch.js +++ /dev/null @@ -1,9 +0,0 @@ -import { runEval } from '@dimsim/eval'; - -await runEval({ - scene: 'apartment', - task: 'Go to the couch', - timeoutSec: 30, - startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, - success: (ctx) => ctx.rubrics.objectDistance({ target: 'sectional', thresholdM: 2.0 }), -}); diff --git a/misc/DimSim/scenes/apartment/evals/go-to-kitchen.js b/misc/DimSim/scenes/apartment/evals/go-to-kitchen.js deleted file mode 100644 index 5165406385..0000000000 --- a/misc/DimSim/scenes/apartment/evals/go-to-kitchen.js +++ /dev/null @@ -1,9 +0,0 @@ -import { runEval } from '@dimsim/eval'; - -await runEval({ - scene: 'apartment', - task: 'Go to the kitchen', - timeoutSec: 30, - startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, - success: (ctx) => ctx.rubrics.objectDistance({ target: 'refrigerator', thresholdM: 3.0 }), -}); diff --git a/misc/DimSim/scenes/apartment/evals/go-to-tv.js b/misc/DimSim/scenes/apartment/evals/go-to-tv.js deleted file mode 100644 index c9800d80e5..0000000000 --- a/misc/DimSim/scenes/apartment/evals/go-to-tv.js +++ /dev/null @@ -1,9 +0,0 @@ -import { runEval } from '@dimsim/eval'; - -await runEval({ - scene: 'apartment', - task: 'Go to the TV', - timeoutSec: 30, - startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, - success: (ctx) => ctx.rubrics.objectDistance({ target: 'television', thresholdM: 2.0 }), -}); diff --git a/misc/DimSim/src/sceneEditor.ts b/misc/DimSim/src/sceneEditor.ts index 2736ed7430..d233248ac4 100644 --- a/misc/DimSim/src/sceneEditor.ts +++ b/misc/DimSim/src/sceneEditor.ts @@ -377,7 +377,7 @@ export class SceneEditor { // loadScript only ever serves bundled scene / eval scripts, which live under // /scenes/ and are plain .js / .mjs ES modules (e.g. /scenes/apartment/index.js, - // /scenes/apartment/evals/go-to-kitchen.js). Anything outside this allowlist is + // /scenes/apartment/evals/scene-smoke.js). Anything outside this allowlist is // refused so a malicious WS peer cannot turn loadScript into an SSRF primitive. static readonly _SCRIPT_PATH_ALLOWLIST = /^\/scenes\/[A-Za-z0-9._/-]+\.(?:js|mjs)$/; From 0abc1d8306a2e78fe14a66dac7c890762ecabed8 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Mon, 3 Aug 2026 16:31:52 +0800 Subject: [PATCH 21/33] feat: run xarm7 simulation through pimsim --- .../xarm/blueprints/simulation.py | 37 ++++++++++++++++--- 1 file changed, 31 insertions(+), 6 deletions(-) diff --git a/dimos/robot/manipulators/xarm/blueprints/simulation.py b/dimos/robot/manipulators/xarm/blueprints/simulation.py index fb6e21f09a..2af9bacab7 100644 --- a/dimos/robot/manipulators/xarm/blueprints/simulation.py +++ b/dimos/robot/manipulators/xarm/blueprints/simulation.py @@ -17,31 +17,56 @@ from __future__ import annotations from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config from dimos.manipulation.pick_and_place_module import PickAndPlaceModule from dimos.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule from dimos.robot.manipulators.common.blueprints import coordinator, trajectory_task from dimos.robot.manipulators.xarm.config import ( XARM7_SIM_PATH, make_xarm7_sim_hardware, - make_xarm7_sim_module_kwargs, make_xarm7_sim_robot_config, ) -from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule +from dimos.simulation.providers import ( + SimulationBinding, + SimulationRequest, + load_simulation_provider, +) from dimos.visualization.rerun.bridge import RerunBridgeModule -_xarm7_sim_hw = make_xarm7_sim_hardware(XARM7_SIM_PATH) + +def _resolve_xarm7_simulation() -> SimulationBinding: + binding = load_simulation_provider("pimsim").build( + SimulationRequest( + robot_model="xarm7", + model_path=XARM7_SIM_PATH, + ) + ) + if binding.adapter_type != "sim_mujoco": + raise ValueError("xarm-perception-sim requires a provider using the sim_mujoco adapter") + return binding + + +def _require_pimsim() -> str | None: + if global_config.simulation != "mujoco": + return "xarm-perception-sim requires --simulation mujoco" + if global_config.simulation_provider != "pimsim": + return "xarm-perception-sim requires --simulation-provider pimsim" + return None + + +_simulation = _resolve_xarm7_simulation() +_xarm7_sim_hw = make_xarm7_sim_hardware(_simulation.adapter_address) xarm_perception_sim = autoconnect( PickAndPlaceModule.blueprint( robots=[make_xarm7_sim_robot_config()], planning_timeout=10.0, - visualization={"backend": "meshcat"}, ), - MujocoSimModule.blueprint(**make_xarm7_sim_module_kwargs(XARM7_SIM_PATH)), + _simulation.backend, ObjectSceneRegistrationModule.blueprint(target_frame="world"), coordinator( hardware=[_xarm7_sim_hw], tasks=[trajectory_task(_xarm7_sim_hw)], ), RerunBridgeModule.blueprint(), -) +).requirements(_require_pimsim) From bfa5515e677ae121f25769dffb65d2df44e5ba0e Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Mon, 3 Aug 2026 17:31:18 +0800 Subject: [PATCH 22/33] feat: configure rerun for xarm simulation --- dimos/robot/manipulators/xarm/blueprints/simulation.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/dimos/robot/manipulators/xarm/blueprints/simulation.py b/dimos/robot/manipulators/xarm/blueprints/simulation.py index 2af9bacab7..f8044ac594 100644 --- a/dimos/robot/manipulators/xarm/blueprints/simulation.py +++ b/dimos/robot/manipulators/xarm/blueprints/simulation.py @@ -31,7 +31,7 @@ SimulationRequest, load_simulation_provider, ) -from dimos.visualization.rerun.bridge import RerunBridgeModule +from dimos.visualization.vis_module import vis_module def _resolve_xarm7_simulation() -> SimulationBinding: @@ -68,5 +68,8 @@ def _require_pimsim() -> str | None: hardware=[_xarm7_sim_hw], tasks=[trajectory_task(_xarm7_sim_hw)], ), - RerunBridgeModule.blueprint(), + vis_module( + viewer_backend=global_config.viewer, + rerun_config=_simulation.rerun_config, + ), ).requirements(_require_pimsim) From 8bac12528408e1b0cb25638c14832794bdd0336f Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Mon, 3 Aug 2026 22:27:43 +0800 Subject: [PATCH 23/33] refactor: move simulator control ABI to hardware boundary --- dimos/hardware/manipulators/sim/adapter.py | 2 +- .../manipulators/sim/test_shm_adapter.py | 2 +- dimos/hardware/simulation/shared_memory.py | 504 +++++++++++++++++ .../xarm/blueprints/simulation.py | 6 +- dimos/robot/manipulators/xarm/config.py | 2 + dimos/simulation/adapters/whole_body/g1.py | 8 +- dimos/simulation/engines/mujoco_shm.py | 516 ++---------------- dimos/simulation/engines/mujoco_sim_module.py | 10 +- 8 files changed, 574 insertions(+), 476 deletions(-) create mode 100644 dimos/hardware/simulation/shared_memory.py diff --git a/dimos/hardware/manipulators/sim/adapter.py b/dimos/hardware/manipulators/sim/adapter.py index 6b00bbf52e..d6e9977a88 100644 --- a/dimos/hardware/manipulators/sim/adapter.py +++ b/dimos/hardware/manipulators/sim/adapter.py @@ -27,7 +27,7 @@ JointLimits, ManipulatorInfo, ) -from dimos.simulation.engines.mujoco_shm import ( +from dimos.hardware.simulation.shared_memory import ( ManipShmReader, shm_key_from_path, ) diff --git a/dimos/hardware/manipulators/sim/test_shm_adapter.py b/dimos/hardware/manipulators/sim/test_shm_adapter.py index 5c1c0d17e2..f1e23504c6 100644 --- a/dimos/hardware/manipulators/sim/test_shm_adapter.py +++ b/dimos/hardware/manipulators/sim/test_shm_adapter.py @@ -23,7 +23,7 @@ import dimos.hardware.manipulators.sim.adapter as adapter_mod from dimos.hardware.manipulators.sim.adapter import ShmMujocoAdapter from dimos.hardware.manipulators.spec import ControlMode, ManipulatorAdapter -from dimos.simulation.engines.mujoco_shm import ManipShmWriter +from dimos.hardware.simulation.shared_memory import ManipShmWriter ARM_DOF = 7 diff --git a/dimos/hardware/simulation/shared_memory.py b/dimos/hardware/simulation/shared_memory.py new file mode 100644 index 0000000000..f6a934d94b --- /dev/null +++ b/dimos/hardware/simulation/shared_memory.py @@ -0,0 +1,504 @@ +# 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. + +"""Shared-memory buffers for sim-manipulator IPC. + +Layout for exchanging joint state and commands between ``MujocoSimModule`` +(which owns the physics engine) and ``ShmMujocoAdapter`` (which plugs into +ControlCoordinator). Modeled after ``dimos.simulation.mujoco.shared_memory`` +(the Go2 SHM pattern). + +Names are deterministic: both sides derive them from the resolved MJCF path, +so no name exchange over RPC is needed. The sim module creates the buffers +and signals ``ready``; the adapter attaches to them by name. +""" + +from __future__ import annotations + +from dataclasses import dataclass +import hashlib +from multiprocessing.shared_memory import SharedMemory +from pathlib import Path +from typing import Any + +import numpy as np +from numpy.typing import NDArray + +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +# Upper bound on joint count per sim. Manipulators use <=10; humanoids +# (Unitree G1: 29) push higher. 32 leaves headroom while keeping all +# per-joint buffers tiny (32 floats = 256 B). +MAX_JOINTS = 32 +_FLOAT_BYTES = 8 # float64 +_INT32_BYTES = 4 + +# IMU layout: quat (4) + gyro (3) + accel (3) = 10 floats. +_IMU_FLOATS = 10 + +_joint_array_size = MAX_JOINTS * _FLOAT_BYTES # float64 array + +# Element counts for control and sequence arrays. +_NUM_CTRL_FIELDS = 4 # [ready, stop, command_mode, num_joints] +_NUM_SEQ_COUNTERS = 12 # one per buffer type (manipulator + WB additions) + +# Buffer sizes (in bytes). +# Keys are short to stay under macOS PSHMNAMLEN (31 bytes). +_shm_sizes = { + # Manipulator-shared layout + "pos": _joint_array_size, + "vel": _joint_array_size, + "eff": _joint_array_size, + "pos_t": _joint_array_size, + "vel_t": _joint_array_size, + "grp": 2 * _FLOAT_BYTES, # [gripper_position, gripper_target] + # Whole-body additions (unused by manipulator path). + "imu": _IMU_FLOATS * _FLOAT_BYTES, # [w,x,y,z, gx,gy,gz, ax,ay,az] + "kp_t": _joint_array_size, # per-joint position-gain target + "kd_t": _joint_array_size, # per-joint velocity-gain target + "tau_t": _joint_array_size, # per-joint feedforward torque + # Bookkeeping + "seq": _NUM_SEQ_COUNTERS * _FLOAT_BYTES, # int64 counters + "ctl": _NUM_CTRL_FIELDS * _INT32_BYTES, # [ready, stop, command_mode, num_joints] +} + +# Sequence counter indices. +SEQ_POSITIONS = 0 +SEQ_VELOCITIES = 1 +SEQ_EFFORTS = 2 +SEQ_POSITION_CMD = 3 +SEQ_VELOCITY_CMD = 4 +SEQ_GRIPPER_STATE = 5 +SEQ_GRIPPER_CMD = 6 +# Whole-body additions +SEQ_IMU = 7 +SEQ_KP_CMD = 8 +SEQ_KD_CMD = 9 +SEQ_TAU_CMD = 10 + +# Control indices. +CTRL_READY = 0 +CTRL_STOP = 1 +CTRL_COMMAND_MODE = 2 +CTRL_NUM_JOINTS = 3 + +# Command modes. +CMD_MODE_POSITION = 0 +CMD_MODE_VELOCITY = 1 +# Whole-body PD-with-feedforward: ctrl = kp*(q_t - q) + kd*(0 - dq) + tau_t. +# Per-step kp/kd lets a policy retune gains online if it wants to. +CMD_MODE_PD_TAU = 2 + +_NAME_PREFIX = "dmjm" + + +def shm_key_from_path(config_path: Path | str) -> str: + """Derive a deterministic short key from a simulation model path. + + Both simulation provider and adapter compute the same key from the same path, + so SHM buffer names can be agreed upon without an RPC round-trip. + """ + resolved = str(Path(config_path).expanduser().resolve()) + return hashlib.md5(resolved.encode("utf-8")).hexdigest()[:12] + + +def _buffer_name(key: str, buffer: str) -> str: + return f"{_NAME_PREFIX}_{key}_{buffer}" + + +@dataclass(frozen=True) +class ManipShmSet: + """Frozen set of named SharedMemory buffers for sim <-> adapter IPC. + + Despite the name (kept for backward compat with existing manipulator + consumers), the layout now also covers whole-body needs: IMU, per-joint + PD gain commands, and per-joint feedforward torque commands. The + extra buffers are unused by the manipulator path. + """ + + pos: SharedMemory + vel: SharedMemory + eff: SharedMemory + pos_t: SharedMemory + vel_t: SharedMemory + grp: SharedMemory + # Whole-body additions + imu: SharedMemory + kp_t: SharedMemory + kd_t: SharedMemory + tau_t: SharedMemory + # Bookkeeping + seq: SharedMemory + ctl: SharedMemory + + @classmethod + def create(cls, key: str) -> ManipShmSet: + """Create new SHM buffers with deterministic names derived from *key*""" + buffers: dict[str, SharedMemory] = {} + for buffer_name, size in _shm_sizes.items(): + name = _buffer_name(key, buffer_name) + try: + stale = SharedMemory(name=name) + stale.close() + try: + stale.unlink() + logger.info("ManipShmSet: unlinked stale SHM", name=name) + except FileNotFoundError: + pass + except FileNotFoundError: + pass + buffers[buffer_name] = SharedMemory(create=True, size=size, name=name) + return cls(**buffers) + + @classmethod + def attach(cls, key: str) -> ManipShmSet: + """Attach to existing SHM buffers created by the sim side.""" + buffers: dict[str, SharedMemory] = {} + for buffer_name in _shm_sizes: + name = _buffer_name(key, buffer_name) + buffers[buffer_name] = SharedMemory(name=name) + return cls(**buffers) + + def as_list(self) -> list[SharedMemory]: + return [getattr(self, k) for k in _shm_sizes] + + +class ManipShmWriter: + """Sim-side handle: writes joint state, reads command targets. + Owned by the active simulation provider. Creates the SHM buffers on init and + unlinks them on cleanup. + """ + + shm: ManipShmSet + + def __init__(self, key: str) -> None: + self.shm = ManipShmSet.create(key) + self._last_pos_cmd_seq = 0 + self._last_vel_cmd_seq = 0 + self._last_gripper_cmd_seq = 0 + self._last_kp_cmd_seq = 0 + self._last_kd_cmd_seq = 0 + self._last_tau_cmd_seq = 0 + # Zero everything. + for buf in self.shm.as_list(): + np.ndarray((buf.size,), dtype=np.uint8, buffer=buf.buf)[:] = 0 + + def write_joint_state( + self, + positions: list[float], + velocities: list[float], + efforts: list[float], + ) -> None: + n = min(len(positions), MAX_JOINTS) + pos_arr = self._array(self.shm.pos, MAX_JOINTS, np.float64) + vel_arr = self._array(self.shm.vel, MAX_JOINTS, np.float64) + eff_arr = self._array(self.shm.eff, MAX_JOINTS, np.float64) + pos_arr[:n] = positions[:n] + vel_arr[:n] = velocities[:n] + eff_arr[:n] = efforts[:n] + self._increment_seq(SEQ_POSITIONS) + self._increment_seq(SEQ_VELOCITIES) + self._increment_seq(SEQ_EFFORTS) + + def write_gripper_state(self, position: float) -> None: + arr = self._array(self.shm.grp, 2, np.float64) + arr[0] = position + self._increment_seq(SEQ_GRIPPER_STATE) + + def read_position_command(self, num_joints: int) -> NDArray[np.float64] | None: + """Return a copy of position targets if a new command arrived since last call.""" + seq = self._get_seq(SEQ_POSITION_CMD) + if seq <= self._last_pos_cmd_seq: + return None + self._last_pos_cmd_seq = seq + arr = self._array(self.shm.pos_t, MAX_JOINTS, np.float64) + result: NDArray[np.float64] = arr[:num_joints].copy() + return result + + def read_velocity_command(self, num_joints: int) -> NDArray[np.float64] | None: + seq = self._get_seq(SEQ_VELOCITY_CMD) + if seq <= self._last_vel_cmd_seq: + return None + self._last_vel_cmd_seq = seq + arr = self._array(self.shm.vel_t, MAX_JOINTS, np.float64) + result: NDArray[np.float64] = arr[:num_joints].copy() + return result + + def read_gripper_command(self) -> float | None: + seq = self._get_seq(SEQ_GRIPPER_CMD) + if seq <= self._last_gripper_cmd_seq: + return None + self._last_gripper_cmd_seq = seq + arr = self._array(self.shm.grp, 2, np.float64) + return float(arr[1]) + + def read_command_mode(self) -> int: + return int(self._control()[CTRL_COMMAND_MODE]) + + # Whole-body additions + + def write_imu( + self, + quaternion: tuple[float, float, float, float], + gyroscope: tuple[float, float, float], + accelerometer: tuple[float, float, float], + ) -> None: + """Write IMU sample. Quaternion is (w, x, y, z).""" + arr = self._array(self.shm.imu, _IMU_FLOATS, np.float64) + arr[0:4] = quaternion + arr[4:7] = gyroscope + arr[7:10] = accelerometer + self._increment_seq(SEQ_IMU) + + def read_kp_command(self, num_joints: int) -> NDArray[np.float64] | None: + """Per-joint position-gain target if a new command landed since last call.""" + seq = self._get_seq(SEQ_KP_CMD) + if seq <= self._last_kp_cmd_seq: + return None + self._last_kp_cmd_seq = seq + arr = self._array(self.shm.kp_t, MAX_JOINTS, np.float64) + return arr[:num_joints].copy() + + def read_kd_command(self, num_joints: int) -> NDArray[np.float64] | None: + seq = self._get_seq(SEQ_KD_CMD) + if seq <= self._last_kd_cmd_seq: + return None + self._last_kd_cmd_seq = seq + arr = self._array(self.shm.kd_t, MAX_JOINTS, np.float64) + return arr[:num_joints].copy() + + def read_tau_command(self, num_joints: int) -> NDArray[np.float64] | None: + """Per-joint feedforward torque if a new command landed since last call.""" + seq = self._get_seq(SEQ_TAU_CMD) + if seq <= self._last_tau_cmd_seq: + return None + self._last_tau_cmd_seq = seq + arr = self._array(self.shm.tau_t, MAX_JOINTS, np.float64) + return arr[:num_joints].copy() + + def signal_ready(self, num_joints: int) -> None: + ctrl = self._control() + ctrl[CTRL_NUM_JOINTS] = num_joints + ctrl[CTRL_READY] = 1 + + def signal_stop(self) -> None: + self._control()[CTRL_STOP] = 1 + + def should_stop(self) -> bool: + return bool(self._control()[CTRL_STOP] == 1) + + def cleanup(self) -> None: + for shm in self.shm.as_list(): + try: + shm.close() + except FileNotFoundError: + pass # already detached + except OSError as exc: + logger.warning("SHM close failed", name=shm.name, error=str(exc)) + try: + shm.unlink() + except FileNotFoundError: + pass # already unlinked (e.g. cleanup called twice) + except OSError as exc: + logger.warning("SHM unlink failed", name=shm.name, error=str(exc)) + + def _array(self, buf: SharedMemory, n: int, dtype: Any) -> NDArray[Any]: + return np.ndarray((n,), dtype=dtype, buffer=buf.buf) + + def _control(self) -> NDArray[np.int32]: + return np.ndarray((_NUM_CTRL_FIELDS,), dtype=np.int32, buffer=self.shm.ctl.buf) + + def _increment_seq(self, index: int) -> None: + seq_arr = np.ndarray((_NUM_SEQ_COUNTERS,), dtype=np.int64, buffer=self.shm.seq.buf) + seq_arr[index] += 1 + + def _get_seq(self, index: int) -> int: + seq_arr = np.ndarray((_NUM_SEQ_COUNTERS,), dtype=np.int64, buffer=self.shm.seq.buf) + return int(seq_arr[index]) + + +class ManipShmReader: + """Adapter-side handle: reads joint state, writes command targets. + + Owned by ``ShmMujocoAdapter``. Attaches to existing buffers created by + the sim module; does not unlink them on cleanup. + """ + + shm: ManipShmSet + + def __init__(self, key: str) -> None: + self.shm = ManipShmSet.attach(key) + + def read_positions(self, num_joints: int) -> list[float]: + arr = np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.pos.buf) + return [float(x) for x in arr[:num_joints]] + + def read_velocities(self, num_joints: int) -> list[float]: + arr = np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.vel.buf) + return [float(x) for x in arr[:num_joints]] + + def read_efforts(self, num_joints: int) -> list[float]: + arr = np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.eff.buf) + return [float(x) for x in arr[:num_joints]] + + def read_gripper_position(self) -> float: + arr = np.ndarray((2,), dtype=np.float64, buffer=self.shm.grp.buf) + return float(arr[0]) + + def write_position_command(self, positions: list[float]) -> None: + n = min(len(positions), MAX_JOINTS) + arr = np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.pos_t.buf) + arr[:n] = positions[:n] + self._set_command_mode(CMD_MODE_POSITION) + self._increment_seq(SEQ_POSITION_CMD) + + def write_velocity_command(self, velocities: list[float]) -> None: + n = min(len(velocities), MAX_JOINTS) + arr = np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.vel_t.buf) + arr[:n] = velocities[:n] + self._set_command_mode(CMD_MODE_VELOCITY) + self._increment_seq(SEQ_VELOCITY_CMD) + + def write_gripper_command(self, position: float) -> None: + arr = np.ndarray((2,), dtype=np.float64, buffer=self.shm.grp.buf) + arr[1] = position + self._increment_seq(SEQ_GRIPPER_CMD) + + # Whole-body additions + + def read_imu( + self, + ) -> tuple[ + tuple[float, float, float, float], + tuple[float, float, float], + tuple[float, float, float], + ]: + """Read IMU sample: ((qw, qx, qy, qz), (gx, gy, gz), (ax, ay, az)).""" + arr = np.ndarray((_IMU_FLOATS,), dtype=np.float64, buffer=self.shm.imu.buf) + return ( + (float(arr[0]), float(arr[1]), float(arr[2]), float(arr[3])), + (float(arr[4]), float(arr[5]), float(arr[6])), + (float(arr[7]), float(arr[8]), float(arr[9])), + ) + + def write_kp_command(self, kp: list[float]) -> None: + """Per-joint position-gain target. Switches command mode to PD+tau.""" + n = min(len(kp), MAX_JOINTS) + arr = np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.kp_t.buf) + arr[:n] = kp[:n] + self._set_command_mode(CMD_MODE_PD_TAU) + self._increment_seq(SEQ_KP_CMD) + + def write_kd_command(self, kd: list[float]) -> None: + n = min(len(kd), MAX_JOINTS) + arr = np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.kd_t.buf) + arr[:n] = kd[:n] + self._set_command_mode(CMD_MODE_PD_TAU) + self._increment_seq(SEQ_KD_CMD) + + def write_tau_command(self, tau: list[float]) -> None: + """Per-joint feedforward torque, applied on top of PD.""" + n = min(len(tau), MAX_JOINTS) + arr = np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.tau_t.buf) + arr[:n] = tau[:n] + self._set_command_mode(CMD_MODE_PD_TAU) + self._increment_seq(SEQ_TAU_CMD) + + def write_pd_tau_command( + self, + positions: list[float], + kp: list[float], + kd: list[float], + tau: list[float], + ) -> None: + """Write a whole-body PD+tau command without transient mode flips. + + The sim engine runs in a different process, so setting position mode + first and PD mode later creates a small but real race. Write all arrays, + publish PD mode once, then bump the sequence counters. + """ + n_pos = min(len(positions), MAX_JOINTS) + n_kp = min(len(kp), MAX_JOINTS) + n_kd = min(len(kd), MAX_JOINTS) + n_tau = min(len(tau), MAX_JOINTS) + np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.pos_t.buf)[:n_pos] = positions[ + :n_pos + ] + np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.kp_t.buf)[:n_kp] = kp[:n_kp] + np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.kd_t.buf)[:n_kd] = kd[:n_kd] + np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.tau_t.buf)[:n_tau] = tau[:n_tau] + self._set_command_mode(CMD_MODE_PD_TAU) + self._increment_seq(SEQ_KP_CMD) + self._increment_seq(SEQ_KD_CMD) + self._increment_seq(SEQ_TAU_CMD) + # Position is the engine-side trigger for latching a new PD target, + # so publish it last after gains/torque are visible. + self._increment_seq(SEQ_POSITION_CMD) + + def is_ready(self) -> bool: + return bool(self._control()[CTRL_READY] == 1) + + def num_joints(self) -> int: + return int(self._control()[CTRL_NUM_JOINTS]) + + def signal_stop(self) -> None: + self._control()[CTRL_STOP] = 1 + + def cleanup(self) -> None: + for shm in self.shm.as_list(): + try: + shm.close() + except FileNotFoundError: + pass # already detached + except OSError as exc: + logger.warning("SHM close failed", name=shm.name, error=str(exc)) + + def _control(self) -> NDArray[np.int32]: + return np.ndarray((_NUM_CTRL_FIELDS,), dtype=np.int32, buffer=self.shm.ctl.buf) + + def _set_command_mode(self, mode: int) -> None: + self._control()[CTRL_COMMAND_MODE] = mode + + def _increment_seq(self, index: int) -> None: + seq_arr = np.ndarray((_NUM_SEQ_COUNTERS,), dtype=np.int64, buffer=self.shm.seq.buf) + seq_arr[index] += 1 + + +__all__ = [ + "CMD_MODE_PD_TAU", + "CMD_MODE_POSITION", + "CMD_MODE_VELOCITY", + "CTRL_COMMAND_MODE", + "CTRL_NUM_JOINTS", + "CTRL_READY", + "CTRL_STOP", + "MAX_JOINTS", + "SEQ_EFFORTS", + "SEQ_GRIPPER_CMD", + "SEQ_GRIPPER_STATE", + "SEQ_IMU", + "SEQ_KD_CMD", + "SEQ_KP_CMD", + "SEQ_POSITIONS", + "SEQ_POSITION_CMD", + "SEQ_TAU_CMD", + "SEQ_VELOCITIES", + "SEQ_VELOCITY_CMD", + "ManipShmReader", + "ManipShmSet", + "ManipShmWriter", + "shm_key_from_path", +] diff --git a/dimos/robot/manipulators/xarm/blueprints/simulation.py b/dimos/robot/manipulators/xarm/blueprints/simulation.py index f8044ac594..b44b76fcb5 100644 --- a/dimos/robot/manipulators/xarm/blueprints/simulation.py +++ b/dimos/robot/manipulators/xarm/blueprints/simulation.py @@ -22,7 +22,8 @@ from dimos.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule from dimos.robot.manipulators.common.blueprints import coordinator, trajectory_task from dimos.robot.manipulators.xarm.config import ( - XARM7_SIM_PATH, + XARM7_MODEL_PATH, + XARM7_TABLETOP_SCENE, make_xarm7_sim_hardware, make_xarm7_sim_robot_config, ) @@ -38,7 +39,8 @@ def _resolve_xarm7_simulation() -> SimulationBinding: binding = load_simulation_provider("pimsim").build( SimulationRequest( robot_model="xarm7", - model_path=XARM7_SIM_PATH, + model_path=XARM7_MODEL_PATH, + scene_package=XARM7_TABLETOP_SCENE, ) ) if binding.adapter_type != "sim_mujoco": diff --git a/dimos/robot/manipulators/xarm/config.py b/dimos/robot/manipulators/xarm/config.py index bb7d577927..c015bc0090 100644 --- a/dimos/robot/manipulators/xarm/config.py +++ b/dimos/robot/manipulators/xarm/config.py @@ -60,6 +60,8 @@ XARM7_FK_MODEL = LfsPath("xarm_description/urdf/xarm7/xarm7.urdf") XARM6_SIM_PATH = LfsPath("xarm6/scene.xml") XARM7_SIM_PATH = LfsPath("xarm7/scene.xml") +XARM7_MODEL_PATH = LfsPath("xarm7/xarm7.xml") +XARM7_TABLETOP_SCENE = "xarm-tabletop-v1" XARM_GRIPPER_PARAMS = { "gripper_joint": make_gripper_joints("arm")[0], "gripper_open_pos": 0.85, diff --git a/dimos/simulation/adapters/whole_body/g1.py b/dimos/simulation/adapters/whole_body/g1.py index d29e1b6585..6f17a5a183 100644 --- a/dimos/simulation/adapters/whole_body/g1.py +++ b/dimos/simulation/adapters/whole_body/g1.py @@ -29,16 +29,16 @@ import time from typing import Any +from dimos.hardware.simulation.shared_memory import ( + ManipShmReader, + shm_key_from_path, +) from dimos.hardware.whole_body.spec import ( POS_STOP, IMUState, MotorCommand, MotorState, ) -from dimos.simulation.engines.mujoco_shm import ( - ManipShmReader, - shm_key_from_path, -) from dimos.utils.logging_config import setup_logger logger = setup_logger() diff --git a/dimos/simulation/engines/mujoco_shm.py b/dimos/simulation/engines/mujoco_shm.py index 523c2e822b..33489d8f81 100644 --- a/dimos/simulation/engines/mujoco_shm.py +++ b/dimos/simulation/engines/mujoco_shm.py @@ -12,466 +12,56 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Shared-memory buffers for sim-manipulator IPC. - -Layout for exchanging joint state and commands between ``MujocoSimModule`` -(which owns the physics engine) and ``ShmMujocoAdapter`` (which plugs into -ControlCoordinator). Modeled after ``dimos.simulation.mujoco.shared_memory`` -(the Go2 SHM pattern). - -Names are deterministic: both sides derive them from the resolved MJCF path, -so no name exchange over RPC is needed. The sim module creates the buffers -and signals ``ready``; the adapter attaches to them by name. -""" - -from __future__ import annotations - -from dataclasses import dataclass -import hashlib -from multiprocessing.shared_memory import SharedMemory -from pathlib import Path -from typing import Any - -import numpy as np -from numpy.typing import NDArray - -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - -# Upper bound on joint count per sim. Manipulators use <=10; humanoids -# (Unitree G1: 29) push higher. 32 leaves headroom while keeping all -# per-joint buffers tiny (32 floats = 256 B). -MAX_JOINTS = 32 -_FLOAT_BYTES = 8 # float64 -_INT32_BYTES = 4 - -# IMU layout: quat (4) + gyro (3) + accel (3) = 10 floats. -_IMU_FLOATS = 10 - -_joint_array_size = MAX_JOINTS * _FLOAT_BYTES # float64 array - -# Element counts for control and sequence arrays. -_NUM_CTRL_FIELDS = 4 # [ready, stop, command_mode, num_joints] -_NUM_SEQ_COUNTERS = 12 # one per buffer type (manipulator + WB additions) - -# Buffer sizes (in bytes). -# Keys are short to stay under macOS PSHMNAMLEN (31 bytes). -_shm_sizes = { - # Manipulator-shared layout - "pos": _joint_array_size, - "vel": _joint_array_size, - "eff": _joint_array_size, - "pos_t": _joint_array_size, - "vel_t": _joint_array_size, - "grp": 2 * _FLOAT_BYTES, # [gripper_position, gripper_target] - # Whole-body additions (unused by manipulator path). - "imu": _IMU_FLOATS * _FLOAT_BYTES, # [w,x,y,z, gx,gy,gz, ax,ay,az] - "kp_t": _joint_array_size, # per-joint position-gain target - "kd_t": _joint_array_size, # per-joint velocity-gain target - "tau_t": _joint_array_size, # per-joint feedforward torque - # Bookkeeping - "seq": _NUM_SEQ_COUNTERS * _FLOAT_BYTES, # int64 counters - "ctl": _NUM_CTRL_FIELDS * _INT32_BYTES, # [ready, stop, command_mode, num_joints] -} - -# Sequence counter indices. -SEQ_POSITIONS = 0 -SEQ_VELOCITIES = 1 -SEQ_EFFORTS = 2 -SEQ_POSITION_CMD = 3 -SEQ_VELOCITY_CMD = 4 -SEQ_GRIPPER_STATE = 5 -SEQ_GRIPPER_CMD = 6 -# Whole-body additions -SEQ_IMU = 7 -SEQ_KP_CMD = 8 -SEQ_KD_CMD = 9 -SEQ_TAU_CMD = 10 - -# Control indices. -CTRL_READY = 0 -CTRL_STOP = 1 -CTRL_COMMAND_MODE = 2 -CTRL_NUM_JOINTS = 3 - -# Command modes. -CMD_MODE_POSITION = 0 -CMD_MODE_VELOCITY = 1 -# Whole-body PD-with-feedforward: ctrl = kp*(q_t - q) + kd*(0 - dq) + tau_t. -# Per-step kp/kd lets a policy retune gains online if it wants to. -CMD_MODE_PD_TAU = 2 - -_NAME_PREFIX = "dmjm" - - -def shm_key_from_path(config_path: Path | str) -> str: - """Derive a deterministic short key from an MJCF path. - - Both sim module and adapter compute the same key from the same path, - so SHM buffer names can be agreed upon without an RPC round-trip. - """ - resolved = str(Path(config_path).expanduser().resolve()) - return hashlib.md5(resolved.encode("utf-8")).hexdigest()[:12] - - -def _buffer_name(key: str, buffer: str) -> str: - return f"{_NAME_PREFIX}_{key}_{buffer}" - - -@dataclass(frozen=True) -class ManipShmSet: - """Frozen set of named SharedMemory buffers for sim <-> adapter IPC. - - Despite the name (kept for backward compat with existing manipulator - consumers), the layout now also covers whole-body needs: IMU, per-joint - PD gain commands, and per-joint feedforward torque commands. The - extra buffers are unused by the manipulator path. - """ - - pos: SharedMemory - vel: SharedMemory - eff: SharedMemory - pos_t: SharedMemory - vel_t: SharedMemory - grp: SharedMemory - # Whole-body additions - imu: SharedMemory - kp_t: SharedMemory - kd_t: SharedMemory - tau_t: SharedMemory - # Bookkeeping - seq: SharedMemory - ctl: SharedMemory - - @classmethod - def create(cls, key: str) -> ManipShmSet: - """Create new SHM buffers with deterministic names derived from *key*""" - buffers: dict[str, SharedMemory] = {} - for buffer_name, size in _shm_sizes.items(): - name = _buffer_name(key, buffer_name) - try: - stale = SharedMemory(name=name) - stale.close() - try: - stale.unlink() - logger.info("ManipShmSet: unlinked stale SHM", name=name) - except FileNotFoundError: - pass - except FileNotFoundError: - pass - buffers[buffer_name] = SharedMemory(create=True, size=size, name=name) - return cls(**buffers) - - @classmethod - def attach(cls, key: str) -> ManipShmSet: - """Attach to existing SHM buffers created by the sim side.""" - buffers: dict[str, SharedMemory] = {} - for buffer_name in _shm_sizes: - name = _buffer_name(key, buffer_name) - buffers[buffer_name] = SharedMemory(name=name) - return cls(**buffers) - - def as_list(self) -> list[SharedMemory]: - return [getattr(self, k) for k in _shm_sizes] - - -class ManipShmWriter: - """Sim-side handle: writes joint state, reads command targets. - Owned by ``MujocoSimModule``. Creates the SHM buffers on init and - unlinks them on cleanup. - """ - - shm: ManipShmSet - - def __init__(self, key: str) -> None: - self.shm = ManipShmSet.create(key) - self._last_pos_cmd_seq = 0 - self._last_vel_cmd_seq = 0 - self._last_gripper_cmd_seq = 0 - self._last_kp_cmd_seq = 0 - self._last_kd_cmd_seq = 0 - self._last_tau_cmd_seq = 0 - # Zero everything. - for buf in self.shm.as_list(): - np.ndarray((buf.size,), dtype=np.uint8, buffer=buf.buf)[:] = 0 - - def write_joint_state( - self, - positions: list[float], - velocities: list[float], - efforts: list[float], - ) -> None: - n = min(len(positions), MAX_JOINTS) - pos_arr = self._array(self.shm.pos, MAX_JOINTS, np.float64) - vel_arr = self._array(self.shm.vel, MAX_JOINTS, np.float64) - eff_arr = self._array(self.shm.eff, MAX_JOINTS, np.float64) - pos_arr[:n] = positions[:n] - vel_arr[:n] = velocities[:n] - eff_arr[:n] = efforts[:n] - self._increment_seq(SEQ_POSITIONS) - self._increment_seq(SEQ_VELOCITIES) - self._increment_seq(SEQ_EFFORTS) - - def write_gripper_state(self, position: float) -> None: - arr = self._array(self.shm.grp, 2, np.float64) - arr[0] = position - self._increment_seq(SEQ_GRIPPER_STATE) - - def read_position_command(self, num_joints: int) -> NDArray[np.float64] | None: - """Return a copy of position targets if a new command arrived since last call.""" - seq = self._get_seq(SEQ_POSITION_CMD) - if seq <= self._last_pos_cmd_seq: - return None - self._last_pos_cmd_seq = seq - arr = self._array(self.shm.pos_t, MAX_JOINTS, np.float64) - result: NDArray[np.float64] = arr[:num_joints].copy() - return result - - def read_velocity_command(self, num_joints: int) -> NDArray[np.float64] | None: - seq = self._get_seq(SEQ_VELOCITY_CMD) - if seq <= self._last_vel_cmd_seq: - return None - self._last_vel_cmd_seq = seq - arr = self._array(self.shm.vel_t, MAX_JOINTS, np.float64) - result: NDArray[np.float64] = arr[:num_joints].copy() - return result - - def read_gripper_command(self) -> float | None: - seq = self._get_seq(SEQ_GRIPPER_CMD) - if seq <= self._last_gripper_cmd_seq: - return None - self._last_gripper_cmd_seq = seq - arr = self._array(self.shm.grp, 2, np.float64) - return float(arr[1]) - - def read_command_mode(self) -> int: - return int(self._control()[CTRL_COMMAND_MODE]) - - # Whole-body additions - - def write_imu( - self, - quaternion: tuple[float, float, float, float], - gyroscope: tuple[float, float, float], - accelerometer: tuple[float, float, float], - ) -> None: - """Write IMU sample. Quaternion is (w, x, y, z).""" - arr = self._array(self.shm.imu, _IMU_FLOATS, np.float64) - arr[0:4] = quaternion - arr[4:7] = gyroscope - arr[7:10] = accelerometer - self._increment_seq(SEQ_IMU) - - def read_kp_command(self, num_joints: int) -> NDArray[np.float64] | None: - """Per-joint position-gain target if a new command landed since last call.""" - seq = self._get_seq(SEQ_KP_CMD) - if seq <= self._last_kp_cmd_seq: - return None - self._last_kp_cmd_seq = seq - arr = self._array(self.shm.kp_t, MAX_JOINTS, np.float64) - return arr[:num_joints].copy() - - def read_kd_command(self, num_joints: int) -> NDArray[np.float64] | None: - seq = self._get_seq(SEQ_KD_CMD) - if seq <= self._last_kd_cmd_seq: - return None - self._last_kd_cmd_seq = seq - arr = self._array(self.shm.kd_t, MAX_JOINTS, np.float64) - return arr[:num_joints].copy() - - def read_tau_command(self, num_joints: int) -> NDArray[np.float64] | None: - """Per-joint feedforward torque if a new command landed since last call.""" - seq = self._get_seq(SEQ_TAU_CMD) - if seq <= self._last_tau_cmd_seq: - return None - self._last_tau_cmd_seq = seq - arr = self._array(self.shm.tau_t, MAX_JOINTS, np.float64) - return arr[:num_joints].copy() - - def signal_ready(self, num_joints: int) -> None: - ctrl = self._control() - ctrl[CTRL_NUM_JOINTS] = num_joints - ctrl[CTRL_READY] = 1 - - def signal_stop(self) -> None: - self._control()[CTRL_STOP] = 1 - - def should_stop(self) -> bool: - return bool(self._control()[CTRL_STOP] == 1) - - def cleanup(self) -> None: - for shm in self.shm.as_list(): - try: - shm.close() - except FileNotFoundError: - pass # already detached - except OSError as exc: - logger.warning("SHM close failed", name=shm.name, error=str(exc)) - try: - shm.unlink() - except FileNotFoundError: - pass # already unlinked (e.g. cleanup called twice) - except OSError as exc: - logger.warning("SHM unlink failed", name=shm.name, error=str(exc)) - - def _array(self, buf: SharedMemory, n: int, dtype: Any) -> NDArray[Any]: - return np.ndarray((n,), dtype=dtype, buffer=buf.buf) - - def _control(self) -> NDArray[np.int32]: - return np.ndarray((_NUM_CTRL_FIELDS,), dtype=np.int32, buffer=self.shm.ctl.buf) - - def _increment_seq(self, index: int) -> None: - seq_arr = np.ndarray((_NUM_SEQ_COUNTERS,), dtype=np.int64, buffer=self.shm.seq.buf) - seq_arr[index] += 1 - - def _get_seq(self, index: int) -> int: - seq_arr = np.ndarray((_NUM_SEQ_COUNTERS,), dtype=np.int64, buffer=self.shm.seq.buf) - return int(seq_arr[index]) - - -class ManipShmReader: - """Adapter-side handle: reads joint state, writes command targets. - - Owned by ``ShmMujocoAdapter``. Attaches to existing buffers created by - the sim module; does not unlink them on cleanup. - """ - - shm: ManipShmSet - - def __init__(self, key: str) -> None: - self.shm = ManipShmSet.attach(key) - - def read_positions(self, num_joints: int) -> list[float]: - arr = np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.pos.buf) - return [float(x) for x in arr[:num_joints]] - - def read_velocities(self, num_joints: int) -> list[float]: - arr = np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.vel.buf) - return [float(x) for x in arr[:num_joints]] - - def read_efforts(self, num_joints: int) -> list[float]: - arr = np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.eff.buf) - return [float(x) for x in arr[:num_joints]] - - def read_gripper_position(self) -> float: - arr = np.ndarray((2,), dtype=np.float64, buffer=self.shm.grp.buf) - return float(arr[0]) - - def write_position_command(self, positions: list[float]) -> None: - n = min(len(positions), MAX_JOINTS) - arr = np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.pos_t.buf) - arr[:n] = positions[:n] - self._set_command_mode(CMD_MODE_POSITION) - self._increment_seq(SEQ_POSITION_CMD) - - def write_velocity_command(self, velocities: list[float]) -> None: - n = min(len(velocities), MAX_JOINTS) - arr = np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.vel_t.buf) - arr[:n] = velocities[:n] - self._set_command_mode(CMD_MODE_VELOCITY) - self._increment_seq(SEQ_VELOCITY_CMD) - - def write_gripper_command(self, position: float) -> None: - arr = np.ndarray((2,), dtype=np.float64, buffer=self.shm.grp.buf) - arr[1] = position - self._increment_seq(SEQ_GRIPPER_CMD) - - # Whole-body additions - - def read_imu( - self, - ) -> tuple[ - tuple[float, float, float, float], - tuple[float, float, float], - tuple[float, float, float], - ]: - """Read IMU sample: ((qw, qx, qy, qz), (gx, gy, gz), (ax, ay, az)).""" - arr = np.ndarray((_IMU_FLOATS,), dtype=np.float64, buffer=self.shm.imu.buf) - return ( - (float(arr[0]), float(arr[1]), float(arr[2]), float(arr[3])), - (float(arr[4]), float(arr[5]), float(arr[6])), - (float(arr[7]), float(arr[8]), float(arr[9])), - ) - - def write_kp_command(self, kp: list[float]) -> None: - """Per-joint position-gain target. Switches command mode to PD+tau.""" - n = min(len(kp), MAX_JOINTS) - arr = np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.kp_t.buf) - arr[:n] = kp[:n] - self._set_command_mode(CMD_MODE_PD_TAU) - self._increment_seq(SEQ_KP_CMD) - - def write_kd_command(self, kd: list[float]) -> None: - n = min(len(kd), MAX_JOINTS) - arr = np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.kd_t.buf) - arr[:n] = kd[:n] - self._set_command_mode(CMD_MODE_PD_TAU) - self._increment_seq(SEQ_KD_CMD) - - def write_tau_command(self, tau: list[float]) -> None: - """Per-joint feedforward torque, applied on top of PD.""" - n = min(len(tau), MAX_JOINTS) - arr = np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.tau_t.buf) - arr[:n] = tau[:n] - self._set_command_mode(CMD_MODE_PD_TAU) - self._increment_seq(SEQ_TAU_CMD) - - def write_pd_tau_command( - self, - positions: list[float], - kp: list[float], - kd: list[float], - tau: list[float], - ) -> None: - """Write a whole-body PD+tau command without transient mode flips. - - The sim engine runs in a different process, so setting position mode - first and PD mode later creates a small but real race. Write all arrays, - publish PD mode once, then bump the sequence counters. - """ - n_pos = min(len(positions), MAX_JOINTS) - n_kp = min(len(kp), MAX_JOINTS) - n_kd = min(len(kd), MAX_JOINTS) - n_tau = min(len(tau), MAX_JOINTS) - np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.pos_t.buf)[:n_pos] = positions[ - :n_pos - ] - np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.kp_t.buf)[:n_kp] = kp[:n_kp] - np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.kd_t.buf)[:n_kd] = kd[:n_kd] - np.ndarray((MAX_JOINTS,), dtype=np.float64, buffer=self.shm.tau_t.buf)[:n_tau] = tau[:n_tau] - self._set_command_mode(CMD_MODE_PD_TAU) - self._increment_seq(SEQ_KP_CMD) - self._increment_seq(SEQ_KD_CMD) - self._increment_seq(SEQ_TAU_CMD) - # Position is the engine-side trigger for latching a new PD target, - # so publish it last after gains/torque are visible. - self._increment_seq(SEQ_POSITION_CMD) - - def is_ready(self) -> bool: - return bool(self._control()[CTRL_READY] == 1) - - def num_joints(self) -> int: - return int(self._control()[CTRL_NUM_JOINTS]) - - def signal_stop(self) -> None: - self._control()[CTRL_STOP] = 1 - - def cleanup(self) -> None: - for shm in self.shm.as_list(): - try: - shm.close() - except FileNotFoundError: - pass # already detached - except OSError as exc: - logger.warning("SHM close failed", name=shm.name, error=str(exc)) - - def _control(self) -> NDArray[np.int32]: - return np.ndarray((_NUM_CTRL_FIELDS,), dtype=np.int32, buffer=self.shm.ctl.buf) - - def _set_command_mode(self, mode: int) -> None: - self._control()[CTRL_COMMAND_MODE] = mode - - def _increment_seq(self, index: int) -> None: - seq_arr = np.ndarray((_NUM_SEQ_COUNTERS,), dtype=np.int64, buffer=self.shm.seq.buf) - seq_arr[index] += 1 +"""Compatibility imports for the neutral simulation shared-memory boundary.""" + +from dimos.hardware.simulation.shared_memory import ( + CMD_MODE_PD_TAU, + CMD_MODE_POSITION, + CMD_MODE_VELOCITY, + CTRL_COMMAND_MODE, + CTRL_NUM_JOINTS, + CTRL_READY, + CTRL_STOP, + MAX_JOINTS, + SEQ_EFFORTS, + SEQ_GRIPPER_CMD, + SEQ_GRIPPER_STATE, + SEQ_IMU, + SEQ_KD_CMD, + SEQ_KP_CMD, + SEQ_POSITION_CMD, + SEQ_POSITIONS, + SEQ_TAU_CMD, + SEQ_VELOCITIES, + SEQ_VELOCITY_CMD, + ManipShmReader, + ManipShmSet, + ManipShmWriter, + shm_key_from_path, +) + +__all__ = [ + "CMD_MODE_PD_TAU", + "CMD_MODE_POSITION", + "CMD_MODE_VELOCITY", + "CTRL_COMMAND_MODE", + "CTRL_NUM_JOINTS", + "CTRL_READY", + "CTRL_STOP", + "MAX_JOINTS", + "SEQ_EFFORTS", + "SEQ_GRIPPER_CMD", + "SEQ_GRIPPER_STATE", + "SEQ_IMU", + "SEQ_KD_CMD", + "SEQ_KP_CMD", + "SEQ_POSITIONS", + "SEQ_POSITION_CMD", + "SEQ_TAU_CMD", + "SEQ_VELOCITIES", + "SEQ_VELOCITY_CMD", + "ManipShmReader", + "ManipShmSet", + "ManipShmWriter", + "shm_key_from_path", +] diff --git a/dimos/simulation/engines/mujoco_sim_module.py b/dimos/simulation/engines/mujoco_sim_module.py index 41151f722d..da468186ca 100644 --- a/dimos/simulation/engines/mujoco_sim_module.py +++ b/dimos/simulation/engines/mujoco_sim_module.py @@ -43,6 +43,11 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out from dimos.hardware.sensors.camera.spec import DepthCameraConfig, DepthCameraHardware +from dimos.hardware.simulation.shared_memory import ( + CMD_MODE_PD_TAU, + ManipShmWriter, + shm_key_from_path, +) from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform @@ -59,11 +64,6 @@ MujocoEngine, RaycastLidarConfig, ) -from dimos.simulation.engines.mujoco_shm import ( - CMD_MODE_PD_TAU, - ManipShmWriter, - shm_key_from_path, -) from dimos.simulation.engines.robot_sim_binding import RobotSimSpec from dimos.simulation.mujoco.constants import LIDAR_RESOLUTION, MAX_HEIGHT, MAX_RANGE, MIN_RANGE from dimos.spec import perception From 70e554c8cf79921d9ba99133ba0a5110d8692a15 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Tue, 4 Aug 2026 01:32:40 +0800 Subject: [PATCH 24/33] feat(manipulation): add simulation operator controls --- dimos/hardware/simulation/episode_control.py | 28 ++++ dimos/manipulation/manipulation_module.py | 12 ++ dimos/manipulation/test_manipulation_unit.py | 19 +++ dimos/manipulation/visualization/operator.py | 9 ++ dimos/manipulation/visualization/viser/gui.py | 84 +++++++++- .../visualization/viser/test_gui.py | 26 ++++ .../viser/test_viser_visualization.py | 24 ++- .../viser/test_visualizer_lifecycle.py | 57 +++++++ .../visualization/viser/visualizer.py | 146 +++++++++--------- .../xarm/blueprints/simulation.py | 1 + 10 files changed, 331 insertions(+), 75 deletions(-) create mode 100644 dimos/hardware/simulation/episode_control.py diff --git a/dimos/hardware/simulation/episode_control.py b/dimos/hardware/simulation/episode_control.py new file mode 100644 index 0000000000..ac0c1fb75d --- /dev/null +++ b/dimos/hardware/simulation/episode_control.py @@ -0,0 +1,28 @@ +# 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. + +"""Provider-neutral simulation episode controls.""" + +from typing import Protocol + +from dimos.spec.utils import Spec + + +class SimulationEpisodeControlSpec(Spec, Protocol): + """Optional control surface implemented by simulation providers.""" + + def reset_episode(self) -> bool: ... + + +__all__ = ["SimulationEpisodeControlSpec"] diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 137d8af72c..2c3a5d0b3c 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -40,6 +40,7 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out +from dimos.hardware.simulation.episode_control import SimulationEpisodeControlSpec from dimos.manipulation.execution_manager import ( ExecutionOutcome, ExecutionTarget, @@ -166,6 +167,7 @@ class ManipulationModule(Module): config: ManipulationModuleConfig _control_coordinator: ControlCoordinator + _episode_control: SimulationEpisodeControlSpec | None = None # Input: Joint state from coordinator (for world sync) coordinator_joint_state: In[JointState] @@ -472,8 +474,18 @@ def reset(self) -> SkillResult[ManipulationSkillError]: ) if self._state == ManipulationState.PLANNING: self._planning_epoch += 1 + plan = self._last_plan + self._last_plan = None self._state = ManipulationState.IDLE self._error_message = "" + if self._episode_control is not None and not self._episode_control.reset_episode(): + message = "Simulation provider failed to reset the episode" + with self._lock: + self._state = ManipulationState.FAULT + self._error_message = message + return SkillResult.fail("EXECUTION_FAILED", message) + if plan is not None: + self._dismiss_preview(plan.group_ids) return SkillResult.ok("Reset to IDLE — ready for new commands") @rpc diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 8c886aeafa..dd9eef008f 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -338,6 +338,25 @@ def test_reset_not_during_execution(self, module_factory): assert not result.is_success() assert result.error_code == "INVALID_STATE" + def test_reset_crosses_optional_episode_boundary_and_invalidates_plan( + self, module_factory + ) -> None: + module = module_factory() + episode_control = MagicMock() + episode_control.reset_episode.return_value = True + module._episode_control = episode_control + module._last_plan = GeneratedPlan( + trajectory=JointTrajectory(), + group_ids=("arm/manipulator",), + path=[], + ) + + result = module.reset() + + assert result.is_success() + episode_control.reset_episode.assert_called_once_with() + assert module._last_plan is None + def test_fail_sets_fault_state(self, module_factory): """_fail helper sets FAULT state and message.""" module = module_factory() diff --git a/dimos/manipulation/visualization/operator.py b/dimos/manipulation/visualization/operator.py index ac0ef65609..17b55650a9 100644 --- a/dimos/manipulation/visualization/operator.py +++ b/dimos/manipulation/visualization/operator.py @@ -207,6 +207,15 @@ def reset(self) -> bool: result = self._module.reset() return result.is_success() + def go_home(self, robot_name: RobotName | None = None) -> bool: + return self._module.go_home(robot_name).is_success() + + def open_gripper(self, robot_name: RobotName | None = None) -> bool: + return self._module.open_gripper(robot_name).is_success() + + def close_gripper(self, robot_name: RobotName | None = None) -> bool: + return self._module.close_gripper(robot_name).is_success() + def _validate_joint_request( self, request: JointTargetRequest ) -> tuple[tuple[PlanningGroup, ...] | None, TargetEvaluationResult | None]: diff --git a/dimos/manipulation/visualization/viser/gui.py b/dimos/manipulation/visualization/viser/gui.py index 02145d9fc5..d760a4ab86 100644 --- a/dimos/manipulation/visualization/viser/gui.py +++ b/dimos/manipulation/visualization/viser/gui.py @@ -14,7 +14,7 @@ from __future__ import annotations -from collections.abc import Mapping, MutableMapping, Sequence +from collections.abc import Callable, Mapping, MutableMapping, Sequence from typing import TypeAlias, cast from dimos.manipulation.planning.groups.models import PlanningGroup @@ -255,6 +255,15 @@ def get_error(self) -> str: def reset(self) -> bool: return self.operator.reset() + def go_home(self) -> bool: + return self.operator.go_home(self.state.selected_robot) + + def open_gripper(self) -> bool: + return self.operator.open_gripper(self.state.selected_robot) + + def close_gripper(self) -> bool: + return self.operator.close_gripper(self.state.selected_robot) + def evaluate_joint_target_set( self, group_ids: Sequence[PlanningGroupID], targets: Mapping[PlanningGroupID, JointState] ) -> TargetEvaluationResult: @@ -384,6 +393,7 @@ def _build_panel_controls(self, gui: GuiApi) -> None: "### Planning Groups\nActive planning groups for pose goals, planning, and joint edits." ) self._sync_group_selector(self.list_planning_groups()) + self._build_operator_controls(gui) self._handles["target_heading"] = gui.add_markdown("### Target") preset_dropdown = gui.add_dropdown( "Preset", @@ -421,6 +431,28 @@ def _build_panel_controls(self, gui: GuiApi) -> None: self._handles["joint_control_folder"] = joint_controls self._build_joint_sliders() + def _build_operator_controls(self, gui: GuiApi) -> None: + self._handles["operator_heading"] = gui.add_markdown("### Robot Controls") + actions: tuple[tuple[str, str, Callable[[], bool]], ...] = ( + ("reset", "Reset", self.reset), + ("go_home", "Go Home", self.go_home), + ("open_gripper", "Open Gripper", self.open_gripper), + ("close_gripper", "Close Gripper", self.close_gripper), + ) + for key, label, action in actions: + button = gui.add_button(label) + + def on_click( + _event: object, + action_key: str = key, + action_label: str = label, + callback: Callable[[], bool] = action, + ) -> None: + self._submit_operator_action(action_key, action_label, callback) + + button.on_click(on_click) + self._handles[key] = button + def _sync_group_selector(self, groups: list[PlanningGroup]) -> None: """Render source-order group toggle buttons without a robot dropdown.""" selected = set(self.state.selected_group_ids) @@ -786,11 +818,13 @@ def _preset_values_by_local_name(self, preset: str, robot_name: str) -> dict[str return self._local_values_for_robot(robot_name, state) def _remove_panel_handles(self) -> None: - for key, handle in list(self._handles.items()): + for key, handle in reversed(list(self._handles.items())): + self._handles.pop(key, None) + if key.startswith("ee_control:"): + continue remove = getattr(handle, "remove", None) if callable(remove): remove() - self._handles.pop(key, None) def _sync_preset_dropdown(self) -> None: handle = self._handles.get("preset") @@ -1119,6 +1153,11 @@ def _update_status_text(self) -> None: ) def _update_control_state(self) -> None: + operator_busy = self.state.action_status != ActionStatus.IDLE or ( + self.state.manipulation_state in {"PLANNING", "EXECUTING"} + ) + for key in ("reset", "go_home", "open_gripper", "close_gripper"): + self._set_disabled(key, operator_busy) self._set_disabled("plan", not self.state.can_plan()) self._set_disabled("preview", not self.state.can_preview()) self._set_disabled( @@ -1264,6 +1303,45 @@ def operation() -> None: operation, on_error=lambda message: self._set_operation_error(message, operation_id) ) + def _submit_operator_action( + self, + key: str, + label: str, + action: Callable[[], bool], + ) -> None: + if self._closed: + return + if self.state.action_status != ActionStatus.IDLE or self.state.manipulation_state in { + "PLANNING", + "EXECUTING", + }: + self._set_recoverable_error(f"Cannot {label.lower()} while manipulation is busy") + return + operation_id = self._next_operation_id() + self.state.action_status = ActionStatus.RUNNING + self.refresh() + + def operation() -> None: + if not self._operation_is_current(operation_id): + return + ok = action() + if not self._operation_is_current(operation_id): + return + if key == "reset" and ok: + self.state.plan_state = PanelPlanState() + if not ok: + self.state.error = self.get_error() or f"{label} failed" + self._finish_operation( + f"{key}={ok}", + clear_error=ok, + operation_id=operation_id, + ) + + self._operation_worker.submit( + operation, + on_error=lambda message: self._set_operation_error(message, operation_id), + ) + def _set_planning_mode(self, label: str) -> None: mode = PLANNING_MODES_BY_LABEL.get(label) if self._closed or mode is None or mode == self.state.planning_mode: diff --git a/dimos/manipulation/visualization/viser/test_gui.py b/dimos/manipulation/visualization/viser/test_gui.py index 9d8be1e168..2f09355d32 100644 --- a/dimos/manipulation/visualization/viser/test_gui.py +++ b/dimos/manipulation/visualization/viser/test_gui.py @@ -310,6 +310,32 @@ def test_gui_preview_enters_previewing_before_worker_runs( assert len(submissions) == 1 +def test_gui_operator_action_uses_operation_worker(monkeypatch: pytest.MonkeyPatch) -> None: + submissions: list[Callable[[], None]] = [] + calls: list[str] = [] + gui = make_gui() + gui._operation_worker.stop() + monkeypatch.setattr(gui, "_operation_worker", FakeOperationSubmitWorker(submissions)) + monkeypatch.setattr(gui, "refresh", lambda: None) + gui.state.manipulation_state = "IDLE" + + def reset() -> bool: + calls.append("reset") + return True + + gui._submit_operator_action("reset", "Reset", reset) + + assert gui.state.action_status == ActionStatus.RUNNING + assert calls == [] + assert len(submissions) == 1 + + submissions[0]() + + assert calls == ["reset"] + assert gui.state.action_status == ActionStatus.IDLE + assert gui.state.last_result == "reset=True" + + def test_gui_selection_change_clears_invalidated_preview( monkeypatch: pytest.MonkeyPatch, ) -> None: diff --git a/dimos/manipulation/visualization/viser/test_viser_visualization.py b/dimos/manipulation/visualization/viser/test_viser_visualization.py index 8e1428214f..e31b96ed7c 100644 --- a/dimos/manipulation/visualization/viser/test_viser_visualization.py +++ b/dimos/manipulation/visualization/viser/test_viser_visualization.py @@ -495,6 +495,10 @@ def test_panel_contract_group_order_defaults_and_controls( assert [button.label for button in server.gui.buttons] == [ "arm", "arm gripper", + "Reset", + "Go Home", + "Open Gripper", + "Close Gripper", "Plan", "Preview", "Execute", @@ -875,6 +879,12 @@ def test_group_controls_use_source_labels_and_active_colors( assert group_display_name(pose) == "arm" assert group_display_name(auxiliary) == "arm gripper" assert [button.label for button in server.gui.buttons[:2]] == ["arm", "arm gripper"] + assert [button.label for button in server.gui.buttons[2:6]] == [ + "Reset", + "Go Home", + "Open Gripper", + "Close Gripper", + ] assert [button.color for button in server.gui.buttons[:2]] == [ ACTIVE_GROUP_COLOR, INACTIVE_GROUP_COLOR, @@ -954,6 +964,10 @@ def test_panel_action_controls_are_present_in_source_order( _gui, _module, server = panel([selected], states("arm")) assert [button.label for button in server.gui.buttons[1:]] == [ + "Reset", + "Go Home", + "Open Gripper", + "Close Gripper", "Plan", "Preview", "Execute", @@ -1152,7 +1166,12 @@ def test_panel_disables_plan_preview_and_execute_until_a_feasible_target( selected = group("arm", "manipulator", ("j1",), pose=True) _gui, _module, server = panel([selected], states("arm")) - assert [button.disabled for button in server.gui.buttons[1:4]] == [True, True, True] + buttons = {button.label: button for button in server.gui.buttons} + assert [buttons[label].disabled for label in ("Plan", "Preview", "Execute")] == [ + True, + True, + True, + ] def test_panel_status_reports_target_and_plan_defaults( @@ -1406,6 +1425,9 @@ def test_transform_control_callback_preserves_pose_through_gui_and_backend( assert control.wxyz == (0.4, 0.1, 0.2, 0.3) assert request.pose_targets[selected.id] == gui.state.pose_targets[selected.id] gui.close() + assert control.removed is False + scene.close() + assert control.removed is True def test_joint_evaluation_updates_active_gizmo_from_computed_group_pose() -> None: diff --git a/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py b/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py index a6f7011d91..651b8ac60f 100644 --- a/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py +++ b/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py @@ -14,7 +14,9 @@ from __future__ import annotations +from concurrent.futures import ThreadPoolExecutor from pathlib import Path +import threading from types import SimpleNamespace import pytest @@ -189,6 +191,61 @@ def close(self) -> None: ] +def test_visualizer_concurrent_initialization_starts_one_runtime( + monkeypatch: pytest.MonkeyPatch, +) -> None: + start_barrier = threading.Barrier(2) + start_calls = 0 + + class FakeRuntime: + url = "http://localhost:8095" + + def __init__(self, config: ViserVisualizationConfig) -> None: + self.config = config + + def start(self) -> FakeServer: + nonlocal start_calls + start_calls += 1 + try: + start_barrier.wait(timeout=0.2) + except threading.BrokenBarrierError: + pass + return FakeServer() + + def close(self) -> None: + pass + + class FakeScene: + def __init__( + self, + server: FakeServer, + viser_urdf: type[FakeViserUrdf], + ) -> None: + pass + + def register_robot(self, robot_id: str, config: RobotModelConfig) -> None: + pass + + def close(self) -> None: + pass + + monkeypatch.setattr(visualizer_module, "ViserRuntime", FakeRuntime) + monkeypatch.setattr(visualizer_module, "ViserUrdf", FakeViserUrdf) + monkeypatch.setattr(visualizer_module, "ViserManipulationScene", FakeScene) + visualizer = ViserManipulationVisualizer( + config=ViserVisualizationConfig(panel_enabled=False), + ) + session = VisualizationSession(PlanningSceneInfo(robots={})) + + with ThreadPoolExecutor(max_workers=2) as executor: + futures = [executor.submit(visualizer.initialize, session) for _ in range(2)] + for future in futures: + future.result(timeout=1.0) + + assert start_calls == 1 + visualizer.close() + + def test_visualizer_closes_partial_startup_when_gui_start_fails( monkeypatch: pytest.MonkeyPatch, ) -> None: diff --git a/dimos/manipulation/visualization/viser/visualizer.py b/dimos/manipulation/visualization/viser/visualizer.py index 1b64601128..00b3df3795 100644 --- a/dimos/manipulation/visualization/viser/visualizer.py +++ b/dimos/manipulation/visualization/viser/visualizer.py @@ -16,6 +16,7 @@ from collections.abc import Sequence from contextlib import suppress +import threading from typing import TYPE_CHECKING from dimos.manipulation.visualization.viser.animation import ( @@ -79,55 +80,57 @@ def __init__( self._robot_names_by_id: dict[str, str] = {} self._robot_ids_by_name: dict[str, str] = {} self._configs_by_name: dict[str, RobotModelConfig] = {} + self._lifecycle_lock = threading.RLock() self._closed = False def _ensure_started(self) -> None: - if self._closed or self._runtime is not None: - return - runtime = ViserRuntime(self.config) - scene: ViserManipulationScene | None = None - gui: ViserPanelGui | None = None - try: - server = runtime.start() - apply_dimos_theme(server) - scene = ViserManipulationScene(server, ViserUrdf) - gui = ( - ViserPanelGui( - server, - self._session_scene, - self._operator, - self._current_states, - self.config, - scene, + with self._lifecycle_lock: + if self._closed or self._runtime is not None: + return + runtime = ViserRuntime(self.config) + scene: ViserManipulationScene | None = None + gui: ViserPanelGui | None = None + try: + server = runtime.start() + apply_dimos_theme(server) + scene = ViserManipulationScene(server, ViserUrdf) + gui = ( + ViserPanelGui( + server, + self._session_scene, + self._operator, + self._current_states, + self.config, + scene, + ) + if self.config.panel_enabled + and self._session_scene is not None + and self._operator is not None + else None ) - if self.config.panel_enabled - and self._session_scene is not None - and self._operator is not None - else None - ) - if gui is not None: - gui.start() - except Exception: - if gui is not None: - with suppress(Exception): - gui.close() - if scene is not None: + if gui is not None: + gui.start() + except Exception: + if gui is not None: + with suppress(Exception): + gui.close() + if scene is not None: + with suppress(Exception): + scene.close() with suppress(Exception): - scene.close() - with suppress(Exception): - runtime.close() - self._runtime = None - self._server = None - self._scene = None - self._gui = None - self._closed = True - raise - self._runtime = runtime - self._server = server - self._scene = scene - self._gui = gui - self._closed = False - logger.info(f"Viser manipulation visualization: {self.get_visualization_url()}") + runtime.close() + self._runtime = None + self._server = None + self._scene = None + self._gui = None + self._closed = True + raise + self._runtime = runtime + self._server = server + self._scene = scene + self._gui = gui + self._closed = False + logger.info(f"Viser manipulation visualization: {self.get_visualization_url()}") def initialize(self, session: VisualizationSession) -> None: """Initialize Viser robot visuals from a one-shot visualization session.""" @@ -317,30 +320,31 @@ def _baseline_values( return values if all(name in values for name in config.joint_names) else None def close(self) -> None: - if self._closed: - return - self._closed = True - errors: list[BaseException] = [] - try: - if self._gui is not None: - try: - self._gui.close() - except Exception as e: - errors.append(e) - if self._scene is not None: - try: - self._scene.close() - except Exception as e: - errors.append(e) - finally: - if self._runtime is not None: - try: - self._runtime.close() - except Exception as e: - errors.append(e) - self._runtime = None - self._server = None - self._scene = None - self._gui = None - if errors: - raise errors[0] + with self._lifecycle_lock: + if self._closed: + return + self._closed = True + errors: list[BaseException] = [] + try: + if self._gui is not None: + try: + self._gui.close() + except Exception as e: + errors.append(e) + if self._scene is not None: + try: + self._scene.close() + except Exception as e: + errors.append(e) + finally: + if self._runtime is not None: + try: + self._runtime.close() + except Exception as e: + errors.append(e) + self._runtime = None + self._server = None + self._scene = None + self._gui = None + if errors: + raise errors[0] diff --git a/dimos/robot/manipulators/xarm/blueprints/simulation.py b/dimos/robot/manipulators/xarm/blueprints/simulation.py index b44b76fcb5..5b2bd7ab91 100644 --- a/dimos/robot/manipulators/xarm/blueprints/simulation.py +++ b/dimos/robot/manipulators/xarm/blueprints/simulation.py @@ -63,6 +63,7 @@ def _require_pimsim() -> str | None: PickAndPlaceModule.blueprint( robots=[make_xarm7_sim_robot_config()], planning_timeout=10.0, + visualization={"backend": "viser"}, ), _simulation.backend, ObjectSceneRegistrationModule.blueprint(target_frame="world"), From 08e9013c59198f5b42d959880df2f7efbd68d4ee Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Tue, 4 Aug 2026 02:25:55 +0800 Subject: [PATCH 25/33] fix(manipulation): restore interactive target planning --- dimos/manipulation/manipulation_module.py | 51 +++++++++++- .../kinematics/drake_optimization_ik.py | 4 +- .../planning/kinematics/jacobian_ik.py | 4 +- .../planning/kinematics/pink_ik.py | 80 ++++++++++++++++++- .../planning/kinematics/test_pink_ik.py | 30 +++++++ dimos/manipulation/planning/spec/protocols.py | 14 +++- .../planning/world/roboplan_world.py | 9 +++ dimos/manipulation/test_manipulation_unit.py | 38 +++++++++ dimos/manipulation/test_roboplan_world.py | 22 +++++ dimos/manipulation/visualization/operator.py | 11 ++- .../visualization/test_operator.py | 23 +++++- dimos/manipulation/visualization/viser/gui.py | 60 +++++++++++--- .../manipulation/visualization/viser/state.py | 29 ++++++- .../visualization/viser/test_state.py | 48 +++++++++++ .../viser/test_viser_visualization.py | 8 ++ 15 files changed, 407 insertions(+), 24 deletions(-) diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 2c3a5d0b3c..a54b54a9e8 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -81,7 +81,11 @@ RobotName, WorldRobotID, ) -from dimos.manipulation.planning.spec.protocols import KinematicsSpec, PlannerSpec +from dimos.manipulation.planning.spec.protocols import ( + IKStepCallback, + KinematicsSpec, + PlannerSpec, +) from dimos.manipulation.planning.trajectory_generator.joint_trajectory_generator import ( JointTrajectoryGenerator, ) @@ -105,6 +109,10 @@ logger = setup_logger() +_INTERACTIVE_IK_POSITION_TOLERANCE_M = 0.02 +_INTERACTIVE_IK_ORIENTATION_TOLERANCE_RAD = math.pi +_INTERACTIVE_IK_MAX_ATTEMPTS = 1 + # Composite type aliases for readability (using semantic IDs from planning.spec) RobotEntry: TypeAlias = tuple[WorldRobotID, RobotModelConfig, JointTrajectoryGenerator] """(world_robot_id, config, trajectory_generator)""" @@ -875,6 +883,43 @@ def inverse_kinematics( check_collision: bool = True, ) -> IKResult: """Solve planning-group pose targets without planning a joint path.""" + return self._inverse_kinematics( + pose_targets=pose_targets, + auxiliary_group_ids=auxiliary_group_ids, + seed=seed, + check_collision=check_collision, + ) + + def inverse_kinematics_interactive( + self, + pose_targets: Mapping[PlanningGroupID, PoseStamped], + auxiliary_group_ids: Sequence[PlanningGroupID] = (), + seed: JointState | None = None, + on_step: IKStepCallback | None = None, + ) -> IKResult: + """Run bounded advisory IK for an in-process interactive target editor.""" + return self._inverse_kinematics( + pose_targets=pose_targets, + auxiliary_group_ids=auxiliary_group_ids, + seed=seed, + check_collision=True, + position_tolerance=_INTERACTIVE_IK_POSITION_TOLERANCE_M, + orientation_tolerance=_INTERACTIVE_IK_ORIENTATION_TOLERANCE_RAD, + max_attempts=_INTERACTIVE_IK_MAX_ATTEMPTS, + on_step=on_step, + ) + + def _inverse_kinematics( + self, + pose_targets: Mapping[PlanningGroupID, PoseStamped], + auxiliary_group_ids: Sequence[PlanningGroupID] = (), + seed: JointState | None = None, + check_collision: bool = True, + position_tolerance: float = 0.001, + orientation_tolerance: float = 0.01, + max_attempts: int = 10, + on_step: IKStepCallback | None = None, + ) -> IKResult: if self._kinematics is None or self._world_monitor is None: return IKResult(status=IKStatus.NO_SOLUTION, message="Planning not initialized") if not pose_targets: @@ -909,7 +954,11 @@ def inverse_kinematics( pose_targets=target_groups, auxiliary_groups=auxiliary_groups, seed=seed_state, + position_tolerance=position_tolerance, + orientation_tolerance=orientation_tolerance, check_collision=check_collision, + max_attempts=max_attempts, + on_step=on_step, ) @rpc diff --git a/dimos/manipulation/planning/kinematics/drake_optimization_ik.py b/dimos/manipulation/planning/kinematics/drake_optimization_ik.py index 91da25986c..e827473e4c 100644 --- a/dimos/manipulation/planning/kinematics/drake_optimization_ik.py +++ b/dimos/manipulation/planning/kinematics/drake_optimization_ik.py @@ -29,7 +29,7 @@ ) from dimos.manipulation.planning.spec.enums import IKStatus from dimos.manipulation.planning.spec.models import IKResult, WorldRobotID -from dimos.manipulation.planning.spec.protocols import WorldSpec +from dimos.manipulation.planning.spec.protocols import IKStepCallback, WorldSpec from dimos.manipulation.planning.utils.kinematics_utils import compute_pose_error from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Transform import Transform @@ -84,6 +84,7 @@ def solve( orientation_tolerance: float = 0.01, check_collision: bool = True, max_attempts: int = 10, + on_step: IKStepCallback | None = None, ) -> IKResult: """Solve IK with multiple random restarts, returning the best collision-free solution.""" error = self._validate_world(world) @@ -180,6 +181,7 @@ def solve_pose_targets( orientation_tolerance: float = 0.01, check_collision: bool = True, max_attempts: int = 10, + on_step: IKStepCallback | None = None, ) -> IKResult: """Solve a planning-group-scoped pose target with Drake IK.""" error = self._validate_world(world) diff --git a/dimos/manipulation/planning/kinematics/jacobian_ik.py b/dimos/manipulation/planning/kinematics/jacobian_ik.py index 4c4e16207a..3671d926ca 100644 --- a/dimos/manipulation/planning/kinematics/jacobian_ik.py +++ b/dimos/manipulation/planning/kinematics/jacobian_ik.py @@ -35,7 +35,7 @@ ) from dimos.manipulation.planning.spec.enums import IKStatus from dimos.manipulation.planning.spec.models import IKResult, WorldRobotID -from dimos.manipulation.planning.spec.protocols import WorldSpec +from dimos.manipulation.planning.spec.protocols import IKStepCallback, WorldSpec from dimos.manipulation.planning.utils.kinematics_utils import ( check_singularity, compute_error_twist, @@ -108,6 +108,7 @@ def solve( orientation_tolerance: float = 0.01, check_collision: bool = True, max_attempts: int = 10, + on_step: IKStepCallback | None = None, ) -> IKResult: """Solve IK with multiple random restarts. @@ -202,6 +203,7 @@ def solve_pose_targets( orientation_tolerance: float = 0.01, check_collision: bool = True, max_attempts: int = 10, + on_step: IKStepCallback | None = None, ) -> IKResult: """Solve a planning-group pose target using group FK/Jacobian.""" if not world.is_finalized: diff --git a/dimos/manipulation/planning/kinematics/pink_ik.py b/dimos/manipulation/planning/kinematics/pink_ik.py index 3456c2e27f..f1f07fa0ed 100644 --- a/dimos/manipulation/planning/kinematics/pink_ik.py +++ b/dimos/manipulation/planning/kinematics/pink_ik.py @@ -36,7 +36,7 @@ from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus from dimos.manipulation.planning.spec.models import IKResult, RobotName, WorldRobotID -from dimos.manipulation.planning.spec.protocols import WorldSpec +from dimos.manipulation.planning.spec.protocols import IKStepCallback, WorldSpec from dimos.manipulation.planning.utils.kinematics_utils import compute_pose_error from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -64,6 +64,11 @@ class _PinkModules: _MANIPULATION_EXTRA_HINT = "Install manipulation dependencies with: uv sync --extra manipulation." +_INTERACTIVE_STEP_STRIDE = 12 + + +class _IKSolveAbortedError(Exception): + """Stop a superseded interactive solve without treating it as an IK failure.""" @dataclass(frozen=True) @@ -118,6 +123,7 @@ def solve( orientation_tolerance: float = 0.01, check_collision: bool = True, max_attempts: int = 10, + on_step: IKStepCallback | None = None, ) -> IKResult: """Solve IK with Pink, returning the standard planning ``IKResult``.""" if not world.is_finalized: @@ -155,7 +161,11 @@ def solve( upper_limits=upper_limits, position_tolerance=position_tolerance, orientation_tolerance=orientation_tolerance, + attempt=attempt, + on_step=on_step, ) + except _IKSolveAbortedError: + return _failure(IKStatus.NO_SOLUTION, "Pink IK superseded by a newer target") except ValueError as exc: return _failure(IKStatus.NO_SOLUTION, f"Pink IK mapping failed: {exc}") except Exception as exc: @@ -189,6 +199,7 @@ def solve_pose_targets( orientation_tolerance: float = 0.01, check_collision: bool = True, max_attempts: int = 10, + on_step: IKStepCallback | None = None, ) -> IKResult: """Solve planning-group-scoped pose targets with Pink IK.""" if not world.is_finalized: @@ -256,6 +267,7 @@ def solve_pose_targets( return _failure(IKStatus.NO_SOLUTION, f"Pink IK model setup failed: {exc}") fallback_result: IKResult | None = None + progress_callback = self._group_progress_callback(groups, on_step) for attempt in range(max_attempts): current_positions = seed_positions.copy() if attempt > 0: @@ -274,6 +286,8 @@ def solve_pose_targets( position_tolerance=position_tolerance, orientation_tolerance=orientation_tolerance, locked_joint_positions=locked_positions, + attempt=attempt, + on_step=progress_callback, ) else: result = self._solve_multi( @@ -284,7 +298,11 @@ def solve_pose_targets( position_tolerance=position_tolerance, orientation_tolerance=orientation_tolerance, locked_joint_positions=locked_positions, + attempt=attempt, + on_step=progress_callback, ) + except _IKSolveAbortedError: + return _failure(IKStatus.NO_SOLUTION, "Pink IK superseded by a newer target") except ValueError as exc: return _failure(IKStatus.NO_SOLUTION, f"Pink IK mapping failed: {exc}") except Exception as exc: @@ -359,6 +377,40 @@ def solve_pose_targets( return _collision_failure(combined) return combined + @staticmethod + def _group_progress_callback( + groups: Sequence[PlanningGroup], on_step: IKStepCallback | None + ) -> IKStepCallback | None: + if on_step is None: + return None + + def report( + local_state: JointState, + position_error: float, + orientation_error: float, + attempt: int, + ) -> bool: + values = dict(zip(local_state.name, local_state.position, strict=True)) + names: list[str] = [] + positions: list[float] = [] + for group in groups: + for global_name, local_name in zip( + group.joint_names, group.local_joint_names, strict=True + ): + value = values.get(local_name, values.get(global_name)) + if value is None: + continue + names.append(global_name) + positions.append(float(value)) + return on_step( + JointState({"name": names, "position": positions}), + position_error, + orientation_error, + attempt, + ) + + return report + def _solve_multi( self, targets: Sequence[tuple[_PinkRobotContext, NDArray[np.float64]]], @@ -368,6 +420,8 @@ def _solve_multi( position_tolerance: float, orientation_tolerance: float, locked_joint_positions: Mapping[int, float] | None = None, + attempt: int = 0, + on_step: IKStepCallback | None = None, ) -> IKResult: robot_context = targets[0][0] pink = self._modules.pink @@ -397,6 +451,17 @@ def _solve_multi( ] final_position_error = max(error[0] for error in errors) final_orientation_error = max(error[1] for error in errors) + if on_step is not None and iteration % _INTERACTIVE_STEP_STRIDE == 0: + progress = JointState( + { + "name": robot_context.mapping.dimos_joint_names, + "position": self._q_to_dimos_positions( + robot_context, configuration.q + ).tolist(), + } + ) + if on_step(progress, final_position_error, final_orientation_error, attempt): + raise _IKSolveAbortedError if ( final_position_error <= position_tolerance and final_orientation_error <= orientation_tolerance @@ -448,6 +513,8 @@ def _solve_single( position_tolerance: float, orientation_tolerance: float, locked_joint_positions: Mapping[int, float] | None = None, + attempt: int = 0, + on_step: IKStepCallback | None = None, ) -> IKResult: pink = self._modules.pink pinocchio = self._modules.pinocchio @@ -478,6 +545,17 @@ def _solve_single( final_position_error, final_orientation_error = compute_pose_error( current_pose, target_model ) + if on_step is not None and iteration % _INTERACTIVE_STEP_STRIDE == 0: + progress = JointState( + { + "name": robot_context.mapping.dimos_joint_names, + "position": self._q_to_dimos_positions( + robot_context, configuration.q + ).tolist(), + } + ) + if on_step(progress, final_position_error, final_orientation_error, attempt): + raise _IKSolveAbortedError if ( final_position_error <= position_tolerance and final_orientation_error <= orientation_tolerance diff --git a/dimos/manipulation/planning/kinematics/test_pink_ik.py b/dimos/manipulation/planning/kinematics/test_pink_ik.py index d0324edb56..5b58dc7cae 100644 --- a/dimos/manipulation/planning/kinematics/test_pink_ik.py +++ b/dimos/manipulation/planning/kinematics/test_pink_ik.py @@ -582,6 +582,36 @@ def test_solve_pose_targets_uses_group_tip_and_filters_group_joints( assert world.joint_state_calls == 0 +def test_solve_pose_targets_aborts_superseded_interactive_search( + mocker: MockerFixture, +) -> None: + ik = _pink_ik(mocker, converge=False) + ik._robot_contexts = {("robot", "tool"): _context()} + world = _FakeWorld() + progress: list[JointState] = [] + + def abort(joints: JointState, _position: float, _orientation: float, _attempt: int) -> bool: + progress.append(joints) + return True + + result = ik.solve_pose_targets( + world=cast("Any", world), + pose_targets={ + world.groups["arm/manipulator"]: PoseStamped( + position=Vector3(0.1, 0.0, 0.0), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + ) + }, + seed=JointState({"name": ["arm/joint_a", "arm/joint_b"], "position": [0.0, 0.0]}), + on_step=abort, + ) + + assert result.status == IKStatus.NO_SOLUTION + assert result.message == "Pink IK superseded by a newer target" + assert len(progress) == 1 + assert progress[0].name == ["arm/joint_a", "arm/joint_b"] + + def test_solve_pose_targets_rejects_group_without_tip(mocker: MockerFixture) -> None: ik = _pink_ik(mocker) world = _FakeWorld() diff --git a/dimos/manipulation/planning/spec/protocols.py b/dimos/manipulation/planning/spec/protocols.py index ff33953025..d01390155b 100644 --- a/dimos/manipulation/planning/spec/protocols.py +++ b/dimos/manipulation/planning/spec/protocols.py @@ -20,8 +20,8 @@ from __future__ import annotations -from collections.abc import Mapping, Sequence -from typing import TYPE_CHECKING, Any, Protocol, runtime_checkable +from collections.abc import Callable, Mapping, Sequence +from typing import TYPE_CHECKING, Any, Protocol, TypeAlias, runtime_checkable if TYPE_CHECKING: from contextlib import AbstractContextManager @@ -47,6 +47,10 @@ from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory +IKStepCallback: TypeAlias = Callable[["JointState", float, float, int], bool] +"""Interactive IK progress hook: joints, position error, orientation error, attempt.""" + + @runtime_checkable class WorldSpec(Protocol): """Protocol for the world/scene backend. @@ -261,8 +265,9 @@ def solve( orientation_tolerance: float = 0.01, check_collision: bool = True, max_attempts: int = 10, + on_step: IKStepCallback | None = None, ) -> IKResult: - """Solve IK with optional collision checking.""" + """Solve IK with optional collision checking and interactive progress.""" ... def solve_pose_targets( @@ -275,8 +280,9 @@ def solve_pose_targets( orientation_tolerance: float = 0.01, check_collision: bool = True, max_attempts: int = 10, + on_step: IKStepCallback | None = None, ) -> IKResult: - """Solve planning-group-scoped pose targets.""" + """Solve planning-group-scoped pose targets with optional interactive progress.""" ... diff --git a/dimos/manipulation/planning/world/roboplan_world.py b/dimos/manipulation/planning/world/roboplan_world.py index 3f1580c7af..74f9315666 100644 --- a/dimos/manipulation/planning/world/roboplan_world.py +++ b/dimos/manipulation/planning/world/roboplan_world.py @@ -174,6 +174,7 @@ def add_obstacle(self, obstacle: Obstacle) -> str | None: return None snapshot = deepcopy(obstacle) self._add_obstacle_to_scene(snapshot, obstacle_id) + self._disable_obstacle_pair_collisions(obstacle_id) self._obstacles[obstacle_id] = snapshot return obstacle_id @@ -200,6 +201,7 @@ def update_obstacle(self, obstacle: Obstacle) -> bool: try: scene.removeGeometry(obstacle_id) self._add_obstacle_to_scene(snapshot, obstacle_id) + self._disable_obstacle_pair_collisions(obstacle_id) except Exception: self._usable = False raise @@ -1084,6 +1086,13 @@ def _add_obstacle_to_scene(self, obstacle: Obstacle, obstacle_id: str) -> None: return raise ValueError(f"Unsupported obstacle type: {obstacle.obstacle_type}") + def _disable_obstacle_pair_collisions(self, obstacle_id: str) -> None: + """Ignore environment contacts that cannot change with robot configuration.""" + scene = self._require_scene() + for other_id in self._obstacles: + if other_id != obstacle_id: + scene.setCollisions(obstacle_id, other_id, False) + def _validate_obstacle(self, obstacle: Obstacle, *, allow_empty_name: bool = False) -> None: validate_obstacle( obstacle, pose_to_matrix(obstacle.pose), allow_empty_name=allow_empty_name diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index dd9eef008f..739867fa95 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -611,6 +611,44 @@ def test_solve_ik_rpc_accepts_explicit_seed_without_current_state( assert kwargs["seed"] is explicit_seed module._world_monitor.current_global_joint_state.assert_not_called() + def test_interactive_ik_is_bounded_and_keeps_strict_rpc_defaults_separate( + self, robot_config, module_factory + ): + module = module_factory() + module._world_monitor = MagicMock() + module._world_monitor.world = MagicMock() + module._world_monitor.planning_groups = PlanningGroupRegistry([robot_config]) + module._kinematics = MagicMock() + expected = IKResult(status=IKStatus.NO_SOLUTION) + module._kinematics.solve_pose_targets.return_value = expected + seed = JointState( + name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], + position=[0.0, 0.0, 0.0], + ) + + def on_step( + _joints: JointState, _position: float, _orientation: float, _attempt: int + ) -> bool: + return False + + result = module.inverse_kinematics_interactive( + { + "test_arm/manipulator": PoseStamped( + frame_id="world", position=Vector3(), orientation=Quaternion() + ) + }, + seed=seed, + on_step=on_step, + ) + + assert result is expected + _, kwargs = module._kinematics.solve_pose_targets.call_args + assert kwargs["seed"] is seed + assert kwargs["position_tolerance"] == 0.02 + assert kwargs["orientation_tolerance"] == pytest.approx(3.141592653589793) + assert kwargs["max_attempts"] == 1 + assert kwargs["on_step"] is on_step + class TestPlanningGroupApis: """Test explicit planning-group API behavior.""" diff --git a/dimos/manipulation/test_roboplan_world.py b/dimos/manipulation/test_roboplan_world.py index 57e437041f..c7f354b615 100644 --- a/dimos/manipulation/test_roboplan_world.py +++ b/dimos/manipulation/test_roboplan_world.py @@ -686,6 +686,28 @@ def test_obstacle_mutation_updates_scene_and_stored_pose( assert world.get_obstacles() == [] +def test_environment_obstacles_do_not_collide_with_each_other( + fake_roboplan: None, robot_config: RobotModelConfig +) -> None: + world, _ = _make_world(fake_roboplan, robot_config) + tabletop = Obstacle( + name="tabletop", + obstacle_type=ObstacleType.BOX, + pose=PoseStamped(position=Vector3(0.45, 0.0, 0.12)), + dimensions=(0.3, 0.4, 0.02), + ) + table_leg = Obstacle( + name="table_leg", + obstacle_type=ObstacleType.CYLINDER, + pose=PoseStamped(position=Vector3(0.32, -0.18, 0.055)), + dimensions=(0.02, 0.11), + ) + + assert world.add_obstacle(tabletop) == "tabletop" + assert world.add_obstacle(table_leg) == "table_leg" + assert world._scene.collision_settings[("table_leg", "tabletop")] is False + + def test_obstacle_operations_require_finalization( fake_roboplan: None, robot_config: RobotModelConfig, diff --git a/dimos/manipulation/visualization/operator.py b/dimos/manipulation/visualization/operator.py index 17b55650a9..2119947ef2 100644 --- a/dimos/manipulation/visualization/operator.py +++ b/dimos/manipulation/visualization/operator.py @@ -24,6 +24,7 @@ from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.manipulation.planning.planners.config import CartesianPathConfig from dimos.manipulation.planning.spec.models import GeneratedPlan, PlanningGroupID, RobotName +from dimos.manipulation.planning.spec.protocols import IKStepCallback from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState @@ -112,16 +113,20 @@ def evaluate_joint_target(self, request: JointTargetRequest) -> TargetEvaluation return self._invalid(request.group_ids, "Incomplete robot target state") return self._evaluate_global_target(groups, JointState(request.target), complete) - def evaluate_pose_target(self, request: PoseTargetRequest) -> TargetEvaluationResult: + def evaluate_pose_target( + self, + request: PoseTargetRequest, + on_step: IKStepCallback | None = None, + ) -> TargetEvaluationResult: """Validate and evaluate explicit world-frame pose targets.""" group_ids, validation = self._validate_pose_request(request) if validation is not None: return validation - ik = self._module.inverse_kinematics( + ik = self._module.inverse_kinematics_interactive( pose_targets=dict(request.pose_targets), auxiliary_group_ids=request.auxiliary_group_ids, seed=JointState(request.seed) if request.seed is not None else None, - check_collision=True, + on_step=on_step, ) if not ik.is_success() or ik.joint_state is None: return TargetEvaluationResult( diff --git a/dimos/manipulation/visualization/test_operator.py b/dimos/manipulation/visualization/test_operator.py index a302fe9642..fb9fa706b7 100644 --- a/dimos/manipulation/visualization/test_operator.py +++ b/dimos/manipulation/visualization/test_operator.py @@ -28,6 +28,7 @@ PlanningGroupID, RobotName, ) +from dimos.manipulation.planning.spec.protocols import IKStepCallback from dimos.manipulation.visualization.operator import ( CartesianTargetRequest, JointTargetRequest, @@ -108,6 +109,7 @@ def __init__(self) -> None: dict[PlanningGroupID, PoseStamped], tuple[PlanningGroupID, ...], JointState | None ] ] = [] + self.ik_progress_callbacks: list[IKStepCallback | None] = [] self.plan_success = True self.preview_success = True self.execute_success = True @@ -152,6 +154,21 @@ def inverse_kinematics( message="ok", ) + def inverse_kinematics_interactive( + self, + pose_targets: dict[PlanningGroupID, PoseStamped], + auxiliary_group_ids: tuple[PlanningGroupID, ...] = (), + seed: JointState | None = None, + on_step: IKStepCallback | None = None, + ) -> IKResult: + self.ik_progress_callbacks.append(on_step) + return self.inverse_kinematics( + pose_targets, + auxiliary_group_ids=auxiliary_group_ids, + seed=seed, + check_collision=True, + ) + def plan_to_joint_targets(self, targets: dict[PlanningGroupID, JointState]) -> bool: self.plan_joint_targets.append(targets) return self.plan_success @@ -336,12 +353,16 @@ def test_pose_evaluation_accepts_world_frame_and_delegates_original_request() -> seed = JointState(name=["arm/j0", "arm/j1"], position=[0.0, 0.0]) request = PoseTargetRequest({"arm/manipulator": pose}, seed=seed) - result = operator.evaluate_pose_target(request) + def on_step(_joints: JointState, _position: float, _orientation: float, _attempt: int) -> bool: + return False + + result = operator.evaluate_pose_target(request, on_step=on_step) assert result.success is True assert result.target_joints is not None assert list(result.target_joints.name) == ["arm/j0", "arm/j1"] assert module.ik_calls == [({"arm/manipulator": pose}, (), seed)] + assert module.ik_progress_callbacks == [on_step] def test_pose_validation_rejects_frame_capability_and_seed_errors() -> None: diff --git a/dimos/manipulation/visualization/viser/gui.py b/dimos/manipulation/visualization/viser/gui.py index d760a4ab86..962013333c 100644 --- a/dimos/manipulation/visualization/viser/gui.py +++ b/dimos/manipulation/visualization/viser/gui.py @@ -21,6 +21,7 @@ from dimos.manipulation.planning.planners.config import RoboPlanCartesianPathConfig from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import PlanningGroupID, PlanningSceneInfo, RobotName +from dimos.manipulation.planning.spec.protocols import IKStepCallback from dimos.manipulation.visualization.operator import ( CartesianTargetRequest, JointTargetRequest, @@ -286,6 +287,7 @@ def evaluate_pose_target_set( pose_targets: Mapping[PlanningGroupID, Pose], auxiliary_group_ids: Sequence[PlanningGroupID] = (), seed: JointState | None = None, + on_step: IKStepCallback | None = None, ) -> TargetEvaluationResult: stamped = { group_id: PoseStamped( @@ -294,7 +296,8 @@ def evaluate_pose_target_set( for group_id, pose in pose_targets.items() } return self.operator.evaluate_pose_target( - PoseTargetRequest(stamped, tuple(auxiliary_group_ids), _copy_joint_state(seed)) + PoseTargetRequest(stamped, tuple(auxiliary_group_ids), _copy_joint_state(seed)), + on_step=on_step, ) def cancel(self) -> bool: @@ -956,7 +959,8 @@ def _on_transform_update( pose_targets=dict(self._active_pose_targets()), ) ) - self.refresh() + # A drag can emit tens of updates per second from the Viser client + # thread. The coalescing worker refreshes once for the accepted result. def _submit_joint_target_evaluation(self) -> None: targets = self._target_set_from_sliders() @@ -1037,13 +1041,32 @@ def _sync_target_ghost_visibility(self) -> None: self.scene.set_target_active(str(robot_id), str(robot_id) in active_robot_ids) def _handle_target_evaluation_request( - self, request: TargetEvaluationRequest + self, + request: TargetEvaluationRequest, + is_stale: Callable[[], bool], ) -> TargetEvaluationResult: if request.source == "cartesian": if not request.pose_targets: return TargetEvaluationResult(False, "INVALID", "No pose target") + + def on_step( + joints: JointState, + _position_error: float, + _orientation_error: float, + _attempt: int, + ) -> bool: + if is_stale(): + return True + targets = self._group_targets_from_joint_state(request.group_ids, joints) + if targets: + self._move_joint_target_visuals(targets) + return False + return self.evaluate_pose_target_set( - request.pose_targets, request.auxiliary_group_ids, request.joints + request.pose_targets, + request.auxiliary_group_ids, + request.joints, + on_step=on_step, ) if not request.joint_targets: return TargetEvaluationResult(False, "INVALID", "No joint target") @@ -1093,22 +1116,41 @@ def _sync_controls_from_targets(self) -> None: self._move_joint_target_visuals(self.state.group_joint_targets) def _split_target_joints_by_group(self, target_joints: JointState) -> None: + self.state.group_joint_targets.update( + self._group_targets_from_joint_state(self.state.selected_group_ids, target_joints) + ) + + def _group_targets_from_joint_state( + self, + group_ids: Sequence[PlanningGroupID], + target_joints: JointState, + ) -> dict[PlanningGroupID, JointState]: if len(target_joints.name) != len(target_joints.position): - return + return {} positions = { str(name): float(value) for name, value in zip(target_joints.name, target_joints.position, strict=True) } - for group_id in self.state.selected_group_ids: + targets: dict[PlanningGroupID, JointState] = {} + for group_id in group_ids: group = self._groups_by_id().get(group_id) - if group is None or any(str(name) not in positions for name in group.joint_names): + if group is None: continue - self.state.group_joint_targets[group_id] = JointState( + values = [ + positions.get(str(global_name), positions.get(str(local_name))) + for global_name, local_name in zip( + group.joint_names, group.local_joint_names, strict=True + ) + ] + if any(value is None for value in values): + continue + targets[group_id] = JointState( { "name": list(group.joint_names), - "position": [positions[str(name)] for name in group.joint_names], + "position": [float(value) for value in values if value is not None], } ) + return targets def _sync_pose_targets_from_group_poses(self) -> None: groups = self._groups_by_id() diff --git a/dimos/manipulation/visualization/viser/state.py b/dimos/manipulation/visualization/viser/state.py index 38c0503b70..ca12b6e339 100644 --- a/dimos/manipulation/visualization/viser/state.py +++ b/dimos/manipulation/visualization/viser/state.py @@ -15,8 +15,9 @@ from __future__ import annotations from collections.abc import Callable -from dataclasses import dataclass, field +from dataclasses import dataclass, field, replace from enum import Enum +from functools import partial import queue import threading from typing import Literal @@ -228,13 +229,14 @@ class TargetEvaluationWorker: def __init__( self, - handler: Callable[[TargetEvaluationRequest], TargetEvaluationResult], + handler: Callable[[TargetEvaluationRequest, Callable[[], bool]], TargetEvaluationResult], apply_result: Callable[[TargetEvaluationRequest, TargetEvaluationResult], None], ) -> None: self._handler = handler self._apply_result = apply_result self._requests: queue.Queue[TargetEvaluationRequest] = queue.Queue(maxsize=1) self._submit_lock = threading.Lock() + self._latest_request_key: tuple[int, int, tuple[PlanningGroupID, ...]] | None = None self._stop_event = threading.Event() self._thread: threading.Thread | None = None @@ -255,6 +257,7 @@ def stop(self, timeout: float | None = 2.0) -> None: def submit(self, request: TargetEvaluationRequest) -> None: with self._submit_lock: + self._latest_request_key = self._request_key(request) while True: try: self._requests.get_nowait() @@ -263,6 +266,8 @@ def submit(self, request: TargetEvaluationRequest) -> None: self._requests.put_nowait(request) def _run(self) -> None: + warm_seed_key: tuple[int, tuple[PlanningGroupID, ...]] | None = None + warm_seed: JointState | None = None while not self._stop_event.is_set(): try: request = self._requests.get(timeout=0.1) @@ -273,12 +278,30 @@ def _run(self) -> None: request = self._requests.get_nowait() except queue.Empty: break + request_seed_key = (request.selection_epoch, request.group_ids) + if warm_seed is not None and warm_seed_key == request_seed_key: + request = replace(request, joints=JointState(warm_seed)) try: - result = self._handler(request) + result = self._handler(request, partial(self._request_is_stale, request)) + if result.success and result.collision_free and result.target_joints is not None: + warm_seed_key = request_seed_key + warm_seed = JointState(result.target_joints) self._apply_result(request, result) except Exception: logger.warning("Target evaluation worker caught unhandled exception", exc_info=True) + @staticmethod + def _request_key( + request: TargetEvaluationRequest, + ) -> tuple[int, int, tuple[PlanningGroupID, ...]]: + return request.selection_epoch, request.sequence_id, request.group_ids + + def _request_is_stale(self, request: TargetEvaluationRequest) -> bool: + if self._stop_event.is_set(): + return True + with self._submit_lock: + return self._latest_request_key != self._request_key(request) + class OperationWorker: """Single-worker operation queue for Viser panel actions.""" diff --git a/dimos/manipulation/visualization/viser/test_state.py b/dimos/manipulation/visualization/viser/test_state.py index a9ec709b93..1b86fb89a2 100644 --- a/dimos/manipulation/visualization/viser/test_state.py +++ b/dimos/manipulation/visualization/viser/test_state.py @@ -18,6 +18,7 @@ import threading from dimos.manipulation.planning.spec.models import PlanningGroupID +from dimos.manipulation.visualization.operator import TargetEvaluationResult from dimos.manipulation.visualization.viser.state import ( ActionStatus, BackendConnectionStatus, @@ -25,9 +26,11 @@ PanelRuntime, PanelState, PlanStatus, + TargetEvaluationRequest, TargetEvaluationWorker, TargetStatus, ) +from dimos.msgs.sensor_msgs.JointState import JointState def test_panel_cannot_plan_from_fault_without_explicit_reset() -> None: @@ -117,6 +120,51 @@ def operation() -> None: assert operation_errors == ["Operation timed out after 0.0s"] +def test_target_worker_supersedes_inflight_work_and_reuses_collision_free_seed() -> None: + first_started = threading.Event() + newer_submitted = threading.Event() + second_applied = threading.Event() + handled: list[TargetEvaluationRequest] = [] + stale_checks: list[bool] = [] + + def handler( + request: TargetEvaluationRequest, is_stale: Callable[[], bool] + ) -> TargetEvaluationResult: + handled.append(request) + if request.sequence_id == 1: + first_started.set() + newer_submitted.wait(timeout=1.0) + stale_checks.append(is_stale()) + return TargetEvaluationResult( + True, + "FEASIBLE", + "", + True, + target_joints=JointState(name=["arm/joint"], position=[float(request.sequence_id)]), + ) + + def apply(request: TargetEvaluationRequest, _result: TargetEvaluationResult) -> None: + if request.sequence_id == 2: + second_applied.set() + + worker = TargetEvaluationWorker(handler, apply) + worker.start() + try: + group_ids = (PlanningGroupID("arm/manipulator"),) + worker.submit(TargetEvaluationRequest(1, "cartesian", group_ids=group_ids)) + assert first_started.wait(timeout=1.0) + worker.submit(TargetEvaluationRequest(2, "cartesian", group_ids=group_ids)) + newer_submitted.set() + assert second_applied.wait(timeout=1.0) + finally: + worker.stop() + + assert stale_checks == [True] + assert len(handled) == 2 + assert handled[1].joints is not None + assert handled[1].joints.position == [1.0] + + class FakeTargetEvaluationWorker(TargetEvaluationWorker): def __init__(self, calls: list[Callable[[], None]]) -> None: self.calls = calls diff --git a/dimos/manipulation/visualization/viser/test_viser_visualization.py b/dimos/manipulation/visualization/viser/test_viser_visualization.py index e31b96ed7c..9a0b65968c 100644 --- a/dimos/manipulation/visualization/viser/test_viser_visualization.py +++ b/dimos/manipulation/visualization/viser/test_viser_visualization.py @@ -1412,6 +1412,13 @@ def test_transform_control_callback_preserves_pose_through_gui_and_backend( submitted: list[TargetEvaluationRequest] = [] gui._worker.submit = submitted.append # type: ignore[method-assign] gui.start() + refresh_calls = 0 + + def count_refresh() -> None: + nonlocal refresh_calls + refresh_calls += 1 + + gui.refresh = count_refresh # type: ignore[method-assign] control = scene._handles[f"{selected.id}:ee_control"] control.position = (1.0, 2.0, 3.0) control.wxyz = (0.4, 0.1, 0.2, 0.3) @@ -1424,6 +1431,7 @@ def test_transform_control_callback_preserves_pose_through_gui_and_backend( assert control.position == (1.0, 2.0, 3.0) assert control.wxyz == (0.4, 0.1, 0.2, 0.3) assert request.pose_targets[selected.id] == gui.state.pose_targets[selected.id] + assert refresh_calls == 0 gui.close() assert control.removed is False scene.close() From d31c2b803f9e55efe9321c5944d0e0a796d0e816 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Tue, 4 Aug 2026 17:23:13 +0800 Subject: [PATCH 26/33] fix(manipulation): align planner with simulated arm base --- dimos/robot/manipulators/xarm/blueprints/simulation.py | 2 +- dimos/robot/manipulators/xarm/config.py | 9 +++++++-- dimos/simulation/providers.py | 2 ++ 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/dimos/robot/manipulators/xarm/blueprints/simulation.py b/dimos/robot/manipulators/xarm/blueprints/simulation.py index 5b2bd7ab91..7a306d010a 100644 --- a/dimos/robot/manipulators/xarm/blueprints/simulation.py +++ b/dimos/robot/manipulators/xarm/blueprints/simulation.py @@ -61,7 +61,7 @@ def _require_pimsim() -> str | None: xarm_perception_sim = autoconnect( PickAndPlaceModule.blueprint( - robots=[make_xarm7_sim_robot_config()], + robots=[make_xarm7_sim_robot_config(_simulation.robot_base_pose)], planning_timeout=10.0, visualization={"backend": "viser"}, ), diff --git a/dimos/robot/manipulators/xarm/config.py b/dimos/robot/manipulators/xarm/config.py index c015bc0090..02104a4b6d 100644 --- a/dimos/robot/manipulators/xarm/config.py +++ b/dimos/robot/manipulators/xarm/config.py @@ -28,6 +28,7 @@ from dimos.core.global_config import global_config from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.robot.manipulators._modeling import ( base_pose, coordinator_joint_mapping, @@ -71,13 +72,14 @@ XARM7_SIM_HOME = [0.0, -0.247, 0.0, 0.909, 0.0, 1.15644, 0.0] -def make_xarm7_sim_robot_config() -> RobotModelConfig: +def make_xarm7_sim_robot_config(robot_base_pose: PoseStamped) -> RobotModelConfig: return make_xarm7_model_config( name="arm", add_gripper=True, tf_extra_links=["link7"], home_joints=XARM7_SIM_HOME, pre_grasp_offset=0.05, + placement=robot_base_pose, ) @@ -236,6 +238,7 @@ def make_xarm_model_config( tf_extra_links: list[str] | None = None, home_joints: list[float] | None = None, pre_grasp_offset: float = 0.10, + placement: PoseStamped | None = None, ) -> RobotModelConfig: xacro_args = { "dof": str(dof), @@ -251,7 +254,9 @@ def make_xarm_model_config( return RobotModelConfig( name=name, model_path=XARM_MODEL_PATH, - base_pose=base_pose(x_offset, y_offset, z_offset, pitch), + base_pose=( + placement if placement is not None else base_pose(x_offset, y_offset, z_offset, pitch) + ), joint_names=local_joint_names, base_link="link_base", planning_groups=[ diff --git a/dimos/simulation/providers.py b/dimos/simulation/providers.py index 888ae342db..32b6a78bf2 100644 --- a/dimos/simulation/providers.py +++ b/dimos/simulation/providers.py @@ -20,6 +20,7 @@ from typing import Any, Protocol, runtime_checkable from dimos.core.coordination.blueprints import Blueprint +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped ENTRY_POINT_GROUP = "dimos.simulation.providers" @@ -38,6 +39,7 @@ class SimulationBinding: adapter_type: str adapter_address: str | Path rerun_config: dict[str, Any] = field(default_factory=dict) + robot_base_pose: PoseStamped = field(default_factory=PoseStamped) @runtime_checkable From 7dcbd36106897ed5cde8d0e615f6ec11306c446d Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Tue, 4 Aug 2026 17:18:37 +0800 Subject: [PATCH 27/33] fix: preserve runtime objects in blueprint config --- .../coordination/blueprint_config/parser.py | 3 ++- .../blueprint_config/test_parser.py | 22 ++++++++++++++++ .../coordination/blueprint_config/values.py | 25 +++++++++++++++++++ 3 files changed, 49 insertions(+), 1 deletion(-) diff --git a/dimos/core/coordination/blueprint_config/parser.py b/dimos/core/coordination/blueprint_config/parser.py index ce8338ec66..a97422040f 100644 --- a/dimos/core/coordination/blueprint_config/parser.py +++ b/dimos/core/coordination/blueprint_config/parser.py @@ -75,6 +75,7 @@ plain, plain_mapping, snapshot_mapping, + validated_model_values, ) from dimos.core.coordination.blueprints import ( Blueprint, @@ -421,7 +422,7 @@ def _validate_modules( raise BlueprintConfigError( format_validation_error(module.atom.name, error) ) from error - dumped = model.model_dump(mode="python", exclude_unset=True) + dumped = validated_model_values(model) dumped.pop("g", None) dumped.pop("instance_name", None) parsed[module.atom.name] = dumped diff --git a/dimos/core/coordination/blueprint_config/test_parser.py b/dimos/core/coordination/blueprint_config/test_parser.py index 6b24322489..bcfdf272fb 100644 --- a/dimos/core/coordination/blueprint_config/test_parser.py +++ b/dimos/core/coordination/blueprint_config/test_parser.py @@ -13,7 +13,9 @@ # limitations under the License. from collections.abc import Callable +from dataclasses import dataclass from pathlib import Path +import pickle from typing import Annotated, Any, Literal from pydantic import BaseModel, Field @@ -452,6 +454,14 @@ def __str__(self) -> str: return "Anchor:\n multi\n line" +@dataclass(frozen=True) +class CallableAnchor: + prefix: str + + def __call__(self, value: Any) -> str: + return f"{self.prefix}:{value}" + + class ArbitraryConfig(ModuleConfig): scaling: Anchor = Field(default_factory=Anchor) hybrid: Anchor | str = "fallback" @@ -490,6 +500,18 @@ def test_blueprint_pinned_arbitrary_value_survives_filtering() -> None: assert isinstance(parsed.module_kwargs("arbitrarymodule")["scaling"], Anchor) +def test_blueprint_pinned_callable_dataclass_survives_worker_serialization() -> None: + blueprint = ArbitraryModule.blueprint(handlers={"scene": CallableAnchor("render")}) + + parsed = BlueprintConfigParser(blueprint).parse(environ={}) + worker_kwargs = pickle.loads(pickle.dumps(parsed.module_kwargs("arbitrarymodule"))) + worker_config = ArbitraryConfig.model_validate(worker_kwargs) + handler = worker_config.handlers["scene"] + + assert isinstance(handler, CallableAnchor) + assert handler("apartment") == "render:apartment" + + def test_format_help_uses_nested_parent_default_instance() -> None: class NestedRequiredConfig(BaseModel): value: int diff --git a/dimos/core/coordination/blueprint_config/values.py b/dimos/core/coordination/blueprint_config/values.py index 39cab50442..b5654d6bbc 100644 --- a/dimos/core/coordination/blueprint_config/values.py +++ b/dimos/core/coordination/blueprint_config/values.py @@ -55,6 +55,31 @@ def plain(value: Any) -> Any: return _copy_opaque(value) +def validated_model_values(model: BaseModel) -> dict[str, Any]: + """Copy explicitly set validated fields without serializing runtime objects.""" + return { + name: _validated_value(getattr(model, name)) + for name in type(model).model_fields + if name in model.model_fields_set + } + + +def _validated_value(value: Any) -> Any: + if isinstance(value, BaseModel): + return validated_model_values(value) + if isinstance(value, Mapping): + return {_copy_opaque(key): _validated_value(item) for key, item in value.items()} + if isinstance(value, list): + return [_validated_value(item) for item in value] + if isinstance(value, tuple): + return tuple(_validated_value(item) for item in value) + if isinstance(value, set): + return {_validated_value(item) for item in value} + if isinstance(value, frozenset): + return frozenset(_validated_value(item) for item in value) + return _copy_opaque(value) + + def deep_merge(destination: dict[str, Any], incoming: Mapping[str, Any]) -> None: for key, value in incoming.items(): if key in destination and isinstance(destination[key], dict) and isinstance(value, Mapping): From c4d202b3bbf87bb559506392b531d8e75dabb34d Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Tue, 4 Aug 2026 19:08:23 +0800 Subject: [PATCH 28/33] Fix simulated xArm pick-and-place execution --- dimos/control/coordinator.py | 2 +- dimos/control/hardware_interface.py | 13 ++++ dimos/control/test_control.py | 21 +++++ dimos/manipulation/pick_and_place_module.py | 46 ++++++++--- .../manipulation/test_pick_and_place_unit.py | 76 ++++++++++++++++++- .../xarm/blueprints/simulation.py | 2 - 6 files changed, 147 insertions(+), 13 deletions(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index eeed8dead0..059d5c0699 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -913,7 +913,7 @@ def set_gripper_position(self, hardware_id: str, position: float) -> bool: if isinstance(hw, ConnectedTwistBase): logger.warning(f"Hardware '{hardware_id}' is a twist base, no gripper support") return False - return hw.adapter.write_gripper_position(position) + return hw.set_gripper_position(position) @rpc def get_gripper_position(self, hardware_id: str) -> float | None: diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 3a7c74f430..ef74c56e09 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -202,6 +202,19 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: return arm_ok and gripper_ok + def set_gripper_position(self, position: float) -> bool: + """Command the gripper and preserve that value across arm-only commands.""" + if not self._gripper_joints: + return False + if not self._initialized: + self._initialize_last_commanded() + if not self._adapter.write_gripper_position(position): + return False + normalized = self._physical_to_normalized(position) + for joint_name in self._gripper_joints: + self._last_commanded[joint_name] = normalized + return True + def _initialize_last_commanded(self) -> None: """Initialize last_commanded with current hardware positions.""" for _ in range(10): diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index 99834b090f..90449958c1 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -253,6 +253,27 @@ def make(**kwargs: Any) -> ControlCoordinator: class TestControlCoordinatorLifecycle: + def test_arm_trajectory_preserves_direct_gripper_command(self, make_coordinator, mock_adapter): + mock_adapter.read_gripper_position.return_value = 0.85 + mock_adapter.write_gripper_position.return_value = True + component = HardwareComponent( + hardware_id="arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("arm", 6), + gripper_joints=["arm/gripper"], + ) + hardware = ConnectedHardware(mock_adapter, component) + coordinator = make_coordinator() + coordinator._hardware = {"arm": hardware} + + assert coordinator.set_gripper_position("arm", 0.0) + assert hardware.write_command({"arm/joint1": 0.2}, ControlMode.POSITION) + + assert [call.args[0] for call in mock_adapter.write_gripper_position.call_args_list] == [ + 0.0, + 0.0, + ] + def test_dispatch_routes_ee_twist_only_to_matching_frame_id(self, make_coordinator): coordinator = make_coordinator() matching_task = RecordingTask("eef") diff --git a/dimos/manipulation/pick_and_place_module.py b/dimos/manipulation/pick_and_place_module.py index c788277b94..4926d80511 100644 --- a/dimos/manipulation/pick_and_place_module.py +++ b/dimos/manipulation/pick_and_place_module.py @@ -90,6 +90,7 @@ def __init__(self, **kwargs: Any) -> None: # The live detection cache is volatile (labels change every frame), # so pick/place use this stable snapshot instead. self._detection_snapshot: list[DetObject] = [] + self._held_object_id: str | None = None @rpc def start(self) -> None: @@ -300,11 +301,14 @@ def _generate_grasps_for_pick( cx, cy, cz = det.center.x, det.center.y, det.center.z xy_dist = (cx**2 + cy**2) ** 0.5 - # Distance-adaptive occlusion offset: - # Near (< 0.8m): small inset — grasp shifted well toward robot (front surface) - # Far (>= 0.8m): larger inset — less toward-robot shift (grasp closer to true center) - inset = 0.01 if xy_dist < _FAR_OCCLUSION_XY_THRESHOLD else 0.05 - gx, gy = self._occlusion_offset(det.center, det.size, inset=inset) + # Exact scene state has no single-viewpoint occlusion error to correct. + if det.identity_basis == "pimsim_scene": + inset = 0.0 + gx, gy = cx, cy + else: + # Near detections need more correction toward the visible surface. + inset = 0.01 if xy_dist < _FAR_OCCLUSION_XY_THRESHOLD else 0.05 + gx, gy = self._occlusion_offset(det.center, det.size, inset=inset) # For tall objects, grasp in the upper third instead of center # to avoid plunging deep and colliding with the object. @@ -486,8 +490,14 @@ def pick( pre_grasp_offset = config.pre_grasp_offset # 1. Generate grasps (uses already-cached detections — call scan_objects first) + target = self._find_object_in_detections(object_name, object_id) + if target is None: + return SkillResult.fail( + "GRASP_GENERATION_FAILED", + f"No grasp poses found for '{object_name}'. Object may not be detected.", + ) logger.info(f"Generating grasp poses for '{object_name}'...") - grasp_poses = self._generate_grasps_for_pick(object_name, object_id) + grasp_poses = self._generate_grasps_for_pick(object_name, target.object_id) if not grasp_poses: return SkillResult.fail( "GRASP_GENERATION_FAILED", @@ -515,7 +525,8 @@ def pick( # 3. Open gripper before approach logger.info("Opening gripper...") - self._set_gripper_position(0.85, rname) + if not self._set_gripper_position(0.85, rname): + return SkillResult.fail("GRIPPER_FAILED", "Failed to open gripper") time.sleep(0.5) # 4. Execute approach to pre-grasp @@ -524,16 +535,27 @@ def pick( return exec_result # 5. Move to grasp pose + target_removed = bool( + self._world_monitor and self._world_monitor.remove_object_obstacle(target.object_id) + ) logger.info("Moving to grasp position...") if not self.plan_to_pose(grasp_pose, rname): + if target_removed: + self.refresh_obstacles() return SkillResult.fail("PLANNING_FAILED", "Grasp pose planning failed") exec_result = self._preview_execute_wait(rname) if not exec_result.is_success(): + if target_removed: + self.refresh_obstacles() return exec_result # 6. Close gripper logger.info("Closing gripper...") - self._set_gripper_position(0.0, rname) + if not self._set_gripper_position(0.0, rname): + if target_removed: + self.refresh_obstacles() + return SkillResult.fail("GRIPPER_FAILED", "Failed to close gripper") + self._held_object_id = target.object_id time.sleep(1.5) # Wait for gripper to close # 7. Retract to pre-grasp @@ -624,17 +646,23 @@ def _place_with_orientation( # 3. Release logger.info("Releasing object...") - self._set_gripper_position(0.85, rname) + if not self._set_gripper_position(0.85, rname): + return SkillResult.fail("GRIPPER_FAILED", "Failed to open gripper") + self._held_object_id = None time.sleep(1.0) # 4. Retract logger.info("Retracting...") if not self.plan_to_pose(pre_place_pose, rname): + self.refresh_obstacles() return SkillResult.fail("PLANNING_FAILED", "Retract planning failed") exec_result = self._preview_execute_wait(rname) if not exec_result.is_success(): + self.refresh_obstacles() return exec_result + self.refresh_obstacles() + return SkillResult.ok(f"Place complete — object released at ({x:.3f}, {y:.3f}, {z:.3f})") @skill diff --git a/dimos/manipulation/test_pick_and_place_unit.py b/dimos/manipulation/test_pick_and_place_unit.py index 6ee984f0e2..19d60e6843 100644 --- a/dimos/manipulation/test_pick_and_place_unit.py +++ b/dimos/manipulation/test_pick_and_place_unit.py @@ -16,14 +16,18 @@ from __future__ import annotations -from unittest.mock import patch +from types import SimpleNamespace +from unittest.mock import MagicMock, call, patch import open3d as o3d import pytest +from dimos.agents.skill_result import SkillResult from dimos.core.module import ModuleBase from dimos.manipulation.pick_and_place_module import PickAndPlaceModule +from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 @@ -147,6 +151,17 @@ def test_grasp_orientation_far_differs_from_near(self): and abs(q_near.w - q_far.w) < 0.01 ) + def test_exact_simulation_detection_does_not_apply_occlusion_offset(self, module): + det = _make_det_object(name="can", center=(0.5, 0.0, 0.19)) + det.identity_basis = "pimsim_scene" + module._detection_snapshot = [det] + + grasps = module._generate_grasps_for_pick("can") + + assert grasps is not None + assert grasps[0].position.x == pytest.approx(0.5) + assert grasps[0].position.y == pytest.approx(0.0) + class TestPlaceBack: """Test place_back guard logic.""" @@ -158,3 +173,62 @@ def test_place_back_no_pick_pose_errors(self, module): assert not result.is_success() assert result.error_code == "NO_PRIOR_POSE" assert "pick" in result.message.lower() + + +class TestPickPlaceObstacleLifecycle: + def _prepare_motion(self, module: PickAndPlaceModule) -> None: + module._get_robot = MagicMock( + return_value=("arm", "robot_1", SimpleNamespace(pre_grasp_offset=0.1), None) + ) + module._lift_if_low = MagicMock(return_value=SkillResult.ok()) + module._preview_execute_wait = MagicMock(return_value=SkillResult.ok()) + module._set_gripper_position = MagicMock(return_value=True) + module._world_monitor = MagicMock() + module._world_monitor.remove_object_obstacle.return_value = True + + def test_pick_removes_target_for_contact_and_holds_gripper_state(self, module): + self._prepare_motion(module) + target = _make_det_object(name="can", object_id="pimsim:manip_can") + module._detection_snapshot = [target] + grasp = Pose(Vector3(0.5, 0.0, 0.25), Quaternion()) + module._generate_grasps_for_pick = MagicMock(return_value=[grasp]) + module.plan_to_pose = MagicMock(return_value=True) + + with patch("dimos.manipulation.pick_and_place_module.time.sleep"): + result = module.pick("can") + + assert result.is_success() + module._world_monitor.remove_object_obstacle.assert_called_once_with("pimsim:manip_can") + assert module._set_gripper_position.call_args_list == [call(0.85, "arm"), call(0.0, "arm")] + assert module._held_object_id == "pimsim:manip_can" + + def test_pick_restores_target_when_contact_plan_fails(self, module): + self._prepare_motion(module) + target = _make_det_object(name="can", object_id="pimsim:manip_can") + module._detection_snapshot = [target] + module._generate_grasps_for_pick = MagicMock( + return_value=[Pose(Vector3(0.5, 0.0, 0.25), Quaternion())] + ) + module.plan_to_pose = MagicMock(side_effect=[True, False]) + module.refresh_obstacles = MagicMock(return_value=[]) + + with patch("dimos.manipulation.pick_and_place_module.time.sleep"): + result = module.pick("can") + + assert not result.is_success() + module.refresh_obstacles.assert_called_once_with() + assert module._held_object_id is None + + def test_place_releases_then_restores_perception_obstacles(self, module): + self._prepare_motion(module) + module._held_object_id = "pimsim:manip_can" + module.plan_to_pose = MagicMock(return_value=True) + module.refresh_obstacles = MagicMock(return_value=[]) + + with patch("dimos.manipulation.pick_and_place_module.time.sleep"): + result = module.place(0.45, 0.1, 0.19) + + assert result.is_success() + module._set_gripper_position.assert_called_once_with(0.85, "arm") + module.refresh_obstacles.assert_called_once_with() + assert module._held_object_id is None diff --git a/dimos/robot/manipulators/xarm/blueprints/simulation.py b/dimos/robot/manipulators/xarm/blueprints/simulation.py index 7a306d010a..f1ca4ddc5e 100644 --- a/dimos/robot/manipulators/xarm/blueprints/simulation.py +++ b/dimos/robot/manipulators/xarm/blueprints/simulation.py @@ -19,7 +19,6 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.manipulation.pick_and_place_module import PickAndPlaceModule -from dimos.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule from dimos.robot.manipulators.common.blueprints import coordinator, trajectory_task from dimos.robot.manipulators.xarm.config import ( XARM7_MODEL_PATH, @@ -66,7 +65,6 @@ def _require_pimsim() -> str | None: visualization={"backend": "viser"}, ), _simulation.backend, - ObjectSceneRegistrationModule.blueprint(target_frame="world"), coordinator( hardware=[_xarm7_sim_hw], tasks=[trajectory_task(_xarm7_sim_hw)], From 0c236e61e82af7483a9c2199d9048664643be0ab Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Tue, 4 Aug 2026 21:23:40 +0800 Subject: [PATCH 29/33] Add full-fidelity mesh scene cooking --- .../scene_cooking/browser/visuals.py | 14 +- dimos/experimental/scene_cooking/cook.py | 63 +++++--- .../scene_cooking/package_config.py | 37 ++++- .../scene_cooking/source_assets/inspect.py | 147 ++++++++++++------ .../scene_cooking/test_cooking.py | 55 +++++++ 5 files changed, 241 insertions(+), 75 deletions(-) diff --git a/dimos/experimental/scene_cooking/browser/visuals.py b/dimos/experimental/scene_cooking/browser/visuals.py index 7407f9caca..3bad6d20fb 100644 --- a/dimos/experimental/scene_cooking/browser/visuals.py +++ b/dimos/experimental/scene_cooking/browser/visuals.py @@ -306,13 +306,11 @@ def _export_with_gltfpack( "-o", str(target), "-mm", - "-si", - str(spec.simplify_ratio), - "-se", - str(spec.simplify_error), "-r", str(report_path), ] + if spec.simplify_ratio < 1.0: + args.extend(["-si", str(spec.simplify_ratio), "-se", str(spec.simplify_error)]) if not spec.quantize: args.append("-noq") if spec.use_gpu_instancing: @@ -417,6 +415,14 @@ def _validate_output( output_stats: dict[str, Any], spec: BrowserVisualSpec, ) -> None: + if spec.preserve_geometry: + source_triangles = int(source_stats.get("expanded_triangle_count") or 0) + output_triangles = int(output_stats.get("expanded_triangle_count") or 0) + if source_triangles != output_triangles: + raise RuntimeError( + "geometry-preserving visual cook changed expanded triangle count from " + f"{source_triangles} to {output_triangles}" + ) source_vertices = int(source_stats.get("vertex_count") or 0) output_vertices = int(output_stats.get("vertex_count") or 0) if source_vertices <= 0 or output_vertices <= 0: diff --git a/dimos/experimental/scene_cooking/cook.py b/dimos/experimental/scene_cooking/cook.py index 6666512d53..2a3e79506d 100644 --- a/dimos/experimental/scene_cooking/cook.py +++ b/dimos/experimental/scene_cooking/cook.py @@ -29,7 +29,10 @@ from typing import Any from dimos.experimental.scene_cooking.browser.collision import cook_browser_collision -from dimos.experimental.scene_cooking.browser.visuals import cook_browser_visual +from dimos.experimental.scene_cooking.browser.visuals import ( + BrowserVisualCookResult, + cook_browser_visual, +) from dimos.experimental.scene_cooking.entities.collision import ( COLLISION_DIR_NAME, cook_entity_collision_hulls, @@ -60,7 +63,7 @@ SCENE_PACKAGE_DIR = get_data_dir("scene_packages") _PACKAGE_KEY_LEN = 12 -_COOK_VERSION = 4 +_COOK_VERSION = 5 #: Cap on entity id samples recorded in cook stats -- diagnostics only, not #: the full entity list (that lives in ``scene.meta.json``). _ENTITY_ID_SAMPLE_CAP = 100 @@ -74,6 +77,7 @@ def cook_scene_package( collision_spec: CollisionSpec | None = None, cook_sidecar: SceneCookSidecar | None = None, visual_spec: BrowserVisualSpec | None = None, + visual_specs: tuple[BrowserVisualSpec, ...] | None = None, browser_collision_spec: BrowserCollisionSpec | None = None, mujoco_spec: MujocoSceneSpec | None = None, rebake: bool = False, @@ -90,13 +94,23 @@ def cook_scene_package( raise FileNotFoundError(f"scene source not found: {source}") align = alignment or SceneMeshAlignment() - visual = visual_spec or BrowserVisualSpec() + if visual_spec is not None and visual_specs is not None: + raise ValueError("pass visual_spec or visual_specs, not both") + visuals = ( + tuple(visual_specs) if visual_specs is not None else (visual_spec or BrowserVisualSpec(),) + ) + visual_targets = [visual.target_key for visual in visuals] + if len(visual_targets) != len(set(visual_targets)): + raise ValueError("visual_specs must use unique targets") + artifact_names = [visual.artifact_name for visual in visuals if visual.enabled] + if len(artifact_names) != len(set(artifact_names)): + raise ValueError("enabled visual_specs must use unique output names") browser_collision = browser_collision_spec or BrowserCollisionSpec() mujoco = mujoco_spec or MujocoSceneSpec() cook_spec = SceneCookSpec( source_path=source, alignment=align, - browser_visual=visual, + browser_visuals=visuals, browser_collision=browser_collision, mujoco=mujoco, ) @@ -144,7 +158,7 @@ def cook_scene_package( visual_source = cook_source # Only invoke Blender when at least one entity actually extracts from # the source mesh; pure-synthetic sidecars (manip rigs) don't need it. - needs_blender = visual.enabled and any( + needs_blender = any(visual.enabled for visual in visuals) and any( entity.visual_path is not None for entity in plan.entities ) if needs_blender: @@ -169,24 +183,28 @@ def cook_scene_package( if hull_counts: stats["entity_collision"]["hulls_per_entity"] = hull_counts - visual_result = cook_browser_visual( - visual_source, - browser_dir, - spec=visual, - rebake=rebake, - ) - if visual_result is not None: - visual_stats = { + visual_results: dict[str, BrowserVisualCookResult] = {} + visual_stats_by_target: dict[str, dict[str, Any]] = {} + for visual in visuals: + visual_result = cook_browser_visual( + visual_source, + browser_dir, + spec=visual, + rebake=rebake, + ) + if visual_result is None: + continue + visual_results[visual.target_key] = visual_result + visual_stats_by_target[visual.target_key] = { "target": visual.target_key, "tool": visual_result.tool, **visual_result.stats, } - stats["browser_visual"] = { - **visual_stats, - } - stats["browser_visuals"] = { - visual.target_key: visual_stats, - } + if visual_stats_by_target: + stats["browser_visuals"] = visual_stats_by_target + stats["browser_visual"] = visual_stats_by_target.get( + "rerun", next(iter(visual_stats_by_target.values())) + ) browser_collision_result = cook_browser_collision( cook_source, @@ -219,12 +237,15 @@ def cook_scene_package( stats["mujoco"]["binary_path"] = str(mujoco_binary_path) stats["mujoco"]["binary"] = binary_stats + primary_visual = visual_results.get("rerun") + if primary_visual is None and visual_results: + primary_visual = next(iter(visual_results.values())) package = ScenePackage( package_dir=package_dir, source_path=source, alignment=align, - visual_path=visual_result.path if visual_result else None, - browser_visuals={visual.target_key: visual_result.path} if visual_result else {}, + visual_path=primary_visual.path if primary_visual else None, + browser_visuals={target: result.path for target, result in visual_results.items()}, browser_collision_path=browser_collision_result.path if browser_collision_result else None, objects_path=browser_collision_result.objects_path if browser_collision_result else None, mujoco_scene_path=mujoco_scene_path, diff --git a/dimos/experimental/scene_cooking/package_config.py b/dimos/experimental/scene_cooking/package_config.py index 0956083b61..d536462e6b 100644 --- a/dimos/experimental/scene_cooking/package_config.py +++ b/dimos/experimental/scene_cooking/package_config.py @@ -38,6 +38,7 @@ class BrowserVisualSpec: normalize_textures: bool = True quantize: bool = False use_gpu_instancing: bool = False + preserve_geometry: bool = False demote_required_extensions: tuple[str, ...] = ("KHR_texture_transform",) max_meshes: int = 200 max_materials: int = 50 @@ -45,6 +46,21 @@ class BrowserVisualSpec: max_vertices: int = 750_000 max_vertex_growth_ratio: float = 1.25 + def __post_init__(self) -> None: + if not 0.0 < self.simplify_ratio <= 1.0: + raise ValueError("visual simplify_ratio must satisfy 0 < ratio <= 1") + if self.preserve_geometry and ( + self.simplify_ratio != 1.0 + or self.quantize + or self.texture_format is not None + or self.max_texture_size is not None + or self.normalize_textures + ): + raise ValueError( + "geometry-preserving visuals cannot simplify, quantize, resize, " + "convert, or normalize textures" + ) + @property def target_key(self) -> str: return self.target.strip().lower() @@ -57,6 +73,23 @@ def artifact_name(self) -> str: #: Per-target overrides layered on top of ``BrowserVisualSpec``'s defaults. #: "rerun" has no entry here -- its values *are* the dataclass defaults. _BROWSER_VISUAL_PROFILES: dict[str, dict[str, Any]] = { + "mesh": { + "optimizer": "gltfpack", + "simplify_ratio": 1.0, + "simplify_error": 0.0, + "texture_format": None, + "max_texture_size": None, + "normalize_textures": False, + "quantize": False, + "use_gpu_instancing": True, + "preserve_geometry": True, + "demote_required_extensions": (), + "max_meshes": 1_000, + "max_materials": 500, + "max_textures": 2_000, + "max_vertices": 2_000_000, + "max_vertex_growth_ratio": 1.0, + }, "babylon": { "optimizer": "gltfpack", "simplify_ratio": 0.3, @@ -131,6 +164,8 @@ class SceneCookSpec: source_path: Path alignment: SceneMeshAlignment = field(default_factory=SceneMeshAlignment) - browser_visual: BrowserVisualSpec = field(default_factory=BrowserVisualSpec) + browser_visuals: tuple[BrowserVisualSpec, ...] = field( + default_factory=lambda: (BrowserVisualSpec(),) + ) browser_collision: BrowserCollisionSpec = field(default_factory=BrowserCollisionSpec) mujoco: MujocoSceneSpec = field(default_factory=MujocoSceneSpec) diff --git a/dimos/experimental/scene_cooking/source_assets/inspect.py b/dimos/experimental/scene_cooking/source_assets/inspect.py index e9221edd65..59d4e441c8 100644 --- a/dimos/experimental/scene_cooking/source_assets/inspect.py +++ b/dimos/experimental/scene_cooking/source_assets/inspect.py @@ -17,11 +17,14 @@ from __future__ import annotations from dataclasses import asdict, dataclass +import json from pathlib import Path from typing import Any import numpy as np +from dimos.experimental.scene_cooking.source_assets.glb import read_glb + @dataclass(frozen=True) class SceneAssetStats: @@ -34,6 +37,11 @@ class SceneAssetStats: texture_count: int = 0 vertex_count: int = 0 triangle_count: int = 0 + primitive_count: int = 0 + draw_count: int = 0 + instance_count: int = 0 + expanded_triangle_count: int = 0 + extensions_used: tuple[str, ...] = () def to_json_dict(self) -> dict[str, Any]: return asdict(self) @@ -54,68 +62,109 @@ def inspect_scene_asset(path: str | Path) -> SceneAssetStats: def _inspect_gltf(path: Path) -> SceneAssetStats: - import trimesh - - loaded: Any = trimesh.load(str(path)) - if isinstance(loaded, trimesh.Trimesh): - # visual may be ColorVisuals (no material) or TextureVisuals. - material = getattr(loaded.visual, "material", None) - material_count = 1 if material is not None else 0 - return SceneAssetStats( - path=str(path), - bytes=path.stat().st_size, - format=path.suffix.lower().lstrip("."), - mesh_count=1, - node_count=1, - material_count=material_count, - texture_count=_count_material_textures([material]), - vertex_count=len(loaded.vertices), - triangle_count=len(loaded.faces), - ) - - scene = loaded - mesh_count = len(getattr(scene, "geometry", {})) - node_count = len(getattr(scene.graph, "nodes_geometry", [])) - materials = [] + gltf = read_glb(path)[0] if path.suffix.lower() == ".glb" else json.loads(path.read_text()) + accessors = gltf.get("accessors", []) + meshes = gltf.get("meshes", []) + nodes = gltf.get("nodes", []) + if ( + not isinstance(accessors, list) + or not isinstance(meshes, list) + or not isinstance(nodes, list) + ): + raise RuntimeError(f"invalid glTF scene structure: {path}") + + mesh_triangle_counts: list[int] = [] + mesh_primitive_counts: list[int] = [] vertex_count = 0 triangle_count = 0 - for geom in scene.geometry.values(): - if not isinstance(geom, trimesh.Trimesh): + primitive_count = 0 + for mesh in meshes: + primitives = mesh.get("primitives", []) if isinstance(mesh, dict) else [] + mesh_triangles = 0 + for primitive in primitives: + if not isinstance(primitive, dict): + continue + attributes = primitive.get("attributes", {}) + position_index = attributes.get("POSITION") if isinstance(attributes, dict) else None + if isinstance(position_index, int): + vertex_count += _accessor_count(accessors, position_index, path) + element_index = primitive.get("indices", position_index) + element_count = ( + _accessor_count(accessors, element_index, path) + if isinstance(element_index, int) + else 0 + ) + triangles = _triangle_count(int(primitive.get("mode", 4)), element_count) + mesh_triangles += triangles + triangle_count += triangles + primitive_count += 1 + mesh_triangle_counts.append(mesh_triangles) + mesh_primitive_counts.append(len(primitives)) + + node_count = 0 + draw_count = 0 + instance_count = 0 + expanded_triangle_count = 0 + for node in nodes: + mesh_index = node.get("mesh") if isinstance(node, dict) else None + if not isinstance(mesh_index, int): continue - vertex_count += len(geom.vertices) - triangle_count += len(geom.faces) - materials.append(getattr(geom.visual, "material", None)) - material_keys = {repr(material) for material in materials if material is not None} + if mesh_index < 0 or mesh_index >= len(meshes): + raise RuntimeError(f"glTF node references missing mesh {mesh_index}: {path}") + count = _node_instance_count(node, accessors, path) + node_count += 1 + draw_count += mesh_primitive_counts[mesh_index] + instance_count += count + expanded_triangle_count += mesh_triangle_counts[mesh_index] * count + return SceneAssetStats( path=str(path), bytes=path.stat().st_size, format=path.suffix.lower().lstrip("."), - mesh_count=mesh_count, + mesh_count=len(meshes), node_count=node_count, - material_count=len(material_keys), - texture_count=_count_material_textures(materials), + material_count=len(gltf.get("materials", [])), + texture_count=len(gltf.get("textures", [])), vertex_count=vertex_count, triangle_count=triangle_count, + primitive_count=primitive_count, + draw_count=draw_count, + instance_count=instance_count, + expanded_triangle_count=expanded_triangle_count, + extensions_used=tuple(sorted(str(value) for value in gltf.get("extensionsUsed", []))), ) -def _count_material_textures(materials: list[Any]) -> int: - textures: set[int] = set() - for material in materials: - if material is None: - continue - for name in ( - "baseColorTexture", - "metallicRoughnessTexture", - "normalTexture", - "emissiveTexture", - "occlusionTexture", - "image", - ): - image = getattr(material, name, None) - if image is not None: - textures.add(id(image)) - return len(textures) +def _accessor_count(accessors: list[Any], index: int, path: Path) -> int: + if index < 0 or index >= len(accessors) or not isinstance(accessors[index], dict): + raise RuntimeError(f"glTF references missing accessor {index}: {path}") + return int(accessors[index].get("count", 0)) + + +def _triangle_count(mode: int, element_count: int) -> int: + if mode == 4: + return element_count // 3 + if mode in {5, 6}: + return max(0, element_count - 2) + return 0 + + +def _node_instance_count(node: dict[str, Any], accessors: list[Any], path: Path) -> int: + extensions = node.get("extensions", {}) + instancing = extensions.get("EXT_mesh_gpu_instancing") if isinstance(extensions, dict) else None + if not isinstance(instancing, dict): + return 1 + attributes = instancing.get("attributes") + if not isinstance(attributes, dict) or not attributes: + raise RuntimeError(f"empty EXT_mesh_gpu_instancing attributes: {path}") + counts = { + _accessor_count(accessors, index, path) + for index in attributes.values() + if isinstance(index, int) + } + if len(counts) != 1: + raise RuntimeError(f"inconsistent EXT_mesh_gpu_instancing accessor counts: {path}") + return counts.pop() def _inspect_usd(path: Path) -> SceneAssetStats: diff --git a/dimos/experimental/scene_cooking/test_cooking.py b/dimos/experimental/scene_cooking/test_cooking.py index e37ab0847f..1cc0da96c3 100644 --- a/dimos/experimental/scene_cooking/test_cooking.py +++ b/dimos/experimental/scene_cooking/test_cooking.py @@ -24,6 +24,7 @@ from dimos.experimental.scene_cooking import planning as plan_module from dimos.experimental.scene_cooking.package_config import browser_visual_spec_for_target from dimos.experimental.scene_cooking.sidecar import SceneCookSidecar +from dimos.experimental.scene_cooking.source_assets.inspect import inspect_scene_asset from dimos.experimental.scene_cooking.source_assets.mesh import ScenePrimMesh from dimos.simulation.scene_assets.spec import ( ARTIFACT_FRAMES, @@ -190,6 +191,7 @@ def test_load_scene_package_tolerates_missing_objects_sidecar(tmp_path: Path) -> def test_browser_visual_profiles_are_backend_specific() -> None: rerun = browser_visual_spec_for_target("rerun") babylon = browser_visual_spec_for_target("babylon") + mesh = browser_visual_spec_for_target("mesh") assert rerun.artifact_name == "visual.rerun.glb" assert rerun.quantize is False @@ -201,6 +203,59 @@ def test_browser_visual_profiles_are_backend_specific() -> None: assert babylon.normalize_textures is False assert babylon.demote_required_extensions == () + assert mesh.artifact_name == "visual.mesh.glb" + assert mesh.simplify_ratio == 1.0 + assert mesh.quantize is False + assert mesh.normalize_textures is False + assert mesh.texture_format is None + assert mesh.max_texture_size is None + assert mesh.use_gpu_instancing is True + assert mesh.preserve_geometry is True + + +def test_scene_asset_inspection_counts_gpu_instances(tmp_path: Path) -> None: + path = tmp_path / "instanced.gltf" + path.write_text( + json.dumps( + { + "asset": {"version": "2.0"}, + "accessors": [ + {"count": 3}, + {"count": 3}, + {"count": 4}, + ], + "meshes": [ + { + "primitives": [ + { + "attributes": {"POSITION": 0}, + "indices": 1, + } + ] + } + ], + "nodes": [ + { + "mesh": 0, + "extensions": { + "EXT_mesh_gpu_instancing": {"attributes": {"TRANSLATION": 2}} + }, + } + ], + "extensionsUsed": ["EXT_mesh_gpu_instancing"], + } + ) + ) + + stats = inspect_scene_asset(path) + + assert stats.node_count == 1 + assert stats.draw_count == 1 + assert stats.instance_count == 4 + assert stats.triangle_count == 1 + assert stats.expanded_triangle_count == 4 + assert stats.extensions_used == ("EXT_mesh_gpu_instancing",) + def test_extract_scene_objects_emits_per_prim_aabb() -> None: # Inlined so importing this test module doesn't pull heavy open3d/trimesh From 3ad0cdc5dde55b8a1ec0cfa6bf85da7ca21aa906 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Tue, 4 Aug 2026 22:01:28 +0800 Subject: [PATCH 30/33] Use world-frame mapping for simulated G1 --- .../g1/blueprints/basic/groot_wbc_platform.py | 6 ++-- .../blueprints/basic/unitree_g1_groot_wbc.py | 33 +++++++++++++------ 2 files changed, 26 insertions(+), 13 deletions(-) diff --git a/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py b/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py index 7abccf56ed..6db4c417b9 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py +++ b/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py @@ -14,9 +14,9 @@ """Platform-owned control and localization inputs for the G1 GR00T stack. -Hardware PointLIO and simulation providers both supply ``lidar`` and -``odometry`` at the shared mapper boundary. Mapping and navigation remain -outside this boundary. +Hardware supplies local-frame lidar plus sensor odometry through PointLIO. +Simulation providers supply world-frame lidar plus base odometry. Mapping and +navigation remain outside this boundary. """ from __future__ import annotations diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py index 4b6c3ce743..36e07063fd 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py @@ -14,9 +14,9 @@ """Unitree G1 GR00T whole-body control, mapping, and navigation. -Real hardware and simulation use PointLIO to produce registered lidar and -odometry. Both feed the same mapping, planning, control, and visualization -modules. +Hardware registers local-frame lidar through PointLIO and the ray-tracing +mapper. PimSim supplies world-frame lidar and base odometry directly, matching +the stable Go2 simulation path. Both paths converge at the global-map boundary. Usage: dimos run unitree-g1-groot-wbc # real hardware @@ -51,6 +51,7 @@ from dimos.mapping.costmapper import CostMapper from dimos.mapping.pointclouds.occupancy import HeightCostConfig from dimos.mapping.ray_tracing.module import RayTracingVoxelMap +from dimos.mapping.voxels.module import VoxelGridMapper from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.nav_msgs.Path import Path as NavPath from dimos.msgs.sensor_msgs.Imu import Imu @@ -77,7 +78,8 @@ _NAV_MAX_STEP_HEIGHT = 0.10 _NAV_ROTATION_DIAMETER = 0.8 _NAV_PATH_WIDTH_MARGIN = 1.1 -_RERUN_ROOT = "world/odometry/g1" +_HARDWARE_RERUN_ROOT = "world/odometry/g1" +_SIMULATION_RERUN_ROOT = "world/odom/g1" _URDF_PATH = Path(__file__).resolve().parents[2] / "g1.urdf" _NOMINAL_PELVIS_Z = 0.74 _pelvis_mid360_cache: list[Any] = [] @@ -94,16 +96,27 @@ class _G1GrootCoordinator(ControlCoordinator): _platform = resolve_g1_groot_platform() +_RERUN_ROOT = _SIMULATION_RERUN_ROOT if _platform.simulation else _HARDWARE_RERUN_ROOT +_ODOMETRY_ENTITY = "world/odom" if _platform.simulation else "world/odometry" -_navigation = autoconnect( - _platform.localization_source, - RayTracingVoxelMap.blueprint( +_mapper = ( + VoxelGridMapper.blueprint( + voxel_size=_NAV_VOXEL_RESOLUTION, + emit_every=5, + ) + if _platform.simulation + else RayTracingVoxelMap.blueprint( voxel_size=_NAV_VOXEL_RESOLUTION, emit_every=0, global_emit_every=4, max_health=10, graze_cos=0.85, - ), + ) +) + +_navigation = autoconnect( + _platform.localization_source, + _mapper, CostMapper.blueprint( config=HeightCostConfig( resolution=_NAV_VOXEL_RESOLUTION, @@ -199,7 +212,6 @@ def _real_costmap(grid: Any) -> Any: "world/localization_anchor": None, "world/lidar": _lidar_scan, _G1_JOINTS_ENTITY: g1_urdf_joint_state(root_path=_RERUN_ROOT), - "world/odometry": _real_odometry_root, "world/global_costmap": g1_costmap, "world/navigation_costmap": g1_costmap, "world/path": _nav_path, @@ -209,7 +221,7 @@ def _real_costmap(grid: Any) -> Any: "world/g1/imu": 10.0, "world/g1/motor_states": 10.0, "world/g1/motor_command": 10.0, - "world/odometry": 15.0, + _ODOMETRY_ENTITY: 15.0, "world/lidar": 2.0, "world/global_map": 1.0, "world/global_costmap": 2.0, @@ -238,6 +250,7 @@ def _real_costmap(grid: Any) -> Any: _rerun_config[_key] = _value if not _platform.simulation: + _rerun_config["visual_override"]["world/odometry"] = _real_odometry_root _rerun_config["visual_override"]["world/global_costmap"] = _real_costmap _rerun_config["visual_override"]["world/navigation_costmap"] = _real_costmap From 6f2c826f5fdc8198ca8c1a1d04e6f1ab94c4f184 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Tue, 4 Aug 2026 23:24:26 +0800 Subject: [PATCH 31/33] feat(sim): publish updated PimSim scene packages --- README.md | 12 ++++++++++++ data/.lfs/scene_packages.tar.gz | 4 ++-- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index fc7bd0013f..7e37ef4ea1 100644 --- a/README.md +++ b/README.md @@ -180,6 +180,18 @@ dimos --simulation run unitree-go2 dimos --simulation run unitree-g1-sim ``` +PimSim environments are stored in Git LFS. From a source checkout, fetch the +scene package before the first PimSim run; DimOS extracts it into +`data/scene_packages/` on first use. + +```bash +git lfs install +git lfs pull --include="data/.lfs/scene_packages.tar.gz" --exclude="" +``` + +See [Large File Management](docs/development/large_file_management.md) for the +data packaging and update workflow. + ```bash # Control a real robot (Unitree quadruped over WebRTC) export ROBOT_IP= diff --git a/data/.lfs/scene_packages.tar.gz b/data/.lfs/scene_packages.tar.gz index 09c023313a..f0c800fcef 100644 --- a/data/.lfs/scene_packages.tar.gz +++ b/data/.lfs/scene_packages.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:e362afc6c6f712b07e570dacb06e9a6c6fb2a29db6a9970556bd7acd233f2064 -size 1671132987 +oid sha256:8f97ea60e828466c438c5c63ba58600f192fa54f849d865051eae2cdf68cca86 +size 4078444330 From d01b00b0823baa1ef0e4e127c9e892ea1b198deb Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Wed, 5 Aug 2026 18:55:17 +0800 Subject: [PATCH 32/33] refactor(sim): narrow integration to provider contract --- README.md | 12 - data/.lfs/scene_packages.tar.gz | 4 +- dimos/agents/mcp/mcp_client.py | 1 - dimos/agents/mcp/test_mcp_client_unit.py | 12 - dimos/control/coordinator.py | 2 +- dimos/control/hardware_interface.py | 13 - dimos/control/test_control.py | 21 - dimos/e2e_tests/conftest.py | 126 ++--- dimos/e2e_tests/dim_sim_client.py | 29 +- dimos/e2e_tests/dimos_cli_call.py | 25 +- dimos/e2e_tests/lcm_spy.py | 106 +--- dimos/e2e_tests/scene_contract.py | 45 -- dimos/e2e_tests/scene_control.py | 54 -- dimos/e2e_tests/simulation_scenarios.py | 109 ---- dimos/e2e_tests/test_dimsim_path_replaning.py | 49 +- dimos/e2e_tests/test_dimsim_spatial_memory.py | 109 +--- dimos/e2e_tests/test_dimsim_walk_forward.py | 25 +- dimos/e2e_tests/test_lcm_spy.py | 32 -- dimos/e2e_tests/test_scene_contract.py | 43 -- .../scene_cooking/browser/visuals.py | 14 +- dimos/experimental/scene_cooking/cook.py | 63 +-- .../scene_cooking/package_config.py | 37 +- .../scene_cooking/source_assets/inspect.py | 147 ++--- .../scene_cooking/test_cooking.py | 55 -- .../sensors/lidar/pointlio/zenoh_relay.py | 77 --- dimos/hardware/simulation/episode_control.py | 28 - dimos/manipulation/manipulation_module.py | 63 +-- dimos/manipulation/pick_and_place_module.py | 46 +- .../kinematics/drake_optimization_ik.py | 4 +- .../planning/kinematics/jacobian_ik.py | 4 +- .../planning/kinematics/pink_ik.py | 80 +-- .../planning/kinematics/test_pink_ik.py | 30 -- dimos/manipulation/planning/spec/protocols.py | 14 +- .../planning/world/roboplan_world.py | 9 - dimos/manipulation/test_manipulation_unit.py | 57 -- .../manipulation/test_pick_and_place_unit.py | 76 +-- dimos/manipulation/test_roboplan_world.py | 22 - dimos/manipulation/visualization/operator.py | 20 +- .../visualization/test_operator.py | 23 +- dimos/manipulation/visualization/viser/gui.py | 144 +---- .../manipulation/visualization/viser/state.py | 29 +- .../visualization/viser/test_gui.py | 26 - .../visualization/viser/test_state.py | 48 -- .../viser/test_viser_visualization.py | 32 +- .../viser/test_visualizer_lifecycle.py | 57 -- .../visualization/viser/visualizer.py | 146 +++-- .../experimental/image_embedding.py | 12 +- dimos/robot/all_blueprints.py | 1 - .../xarm/blueprints/simulation.py | 53 +- dimos/robot/manipulators/xarm/config.py | 11 +- .../g1/blueprints/basic/groot_wbc_platform.py | 106 ---- .../blueprints/basic/unitree_g1_groot_wbc.py | 506 +++++++++++++----- dimos/robot/unitree/g1/g1_rerun.py | 13 +- .../go2/blueprints/basic/go2_platform.py | 55 -- .../go2/blueprints/basic/unitree_go2_basic.py | 26 +- .../go2/blueprints/smart/unitree_go2.py | 19 +- dimos/robot/unitree/go2/config.py | 41 -- dimos/simulation/dimsim/scene_client.py | 32 -- .../mujoco/direct_cmd_vel_explorer.py | 19 +- dimos/visualization/rerun/bridge.py | 12 +- dimos/visualization/rerun/costmap.py | 39 -- .../rerun/test_detection3d_bridge.py | 15 - docs/development/testing.md | 33 -- misc/DimSim/README.md | 10 +- misc/DimSim/cli/cli.ts | 6 +- misc/DimSim/docs/evals.md | 23 +- misc/DimSim/evals/deno-client.ts | 2 +- .../scenes/apartment/evals/go-to-couch.js | 9 + .../scenes/apartment/evals/go-to-kitchen.js | 9 + .../DimSim/scenes/apartment/evals/go-to-tv.js | 9 + misc/DimSim/src/sceneEditor.ts | 2 +- 71 files changed, 757 insertions(+), 2474 deletions(-) delete mode 100644 dimos/e2e_tests/scene_contract.py delete mode 100644 dimos/e2e_tests/scene_control.py delete mode 100644 dimos/e2e_tests/simulation_scenarios.py delete mode 100644 dimos/e2e_tests/test_lcm_spy.py delete mode 100644 dimos/e2e_tests/test_scene_contract.py delete mode 100644 dimos/hardware/sensors/lidar/pointlio/zenoh_relay.py delete mode 100644 dimos/hardware/simulation/episode_control.py delete mode 100644 dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py delete mode 100644 dimos/robot/unitree/go2/blueprints/basic/go2_platform.py delete mode 100644 dimos/robot/unitree/go2/config.py delete mode 100644 dimos/visualization/rerun/costmap.py create mode 100644 misc/DimSim/scenes/apartment/evals/go-to-couch.js create mode 100644 misc/DimSim/scenes/apartment/evals/go-to-kitchen.js create mode 100644 misc/DimSim/scenes/apartment/evals/go-to-tv.js diff --git a/README.md b/README.md index 7e37ef4ea1..fc7bd0013f 100644 --- a/README.md +++ b/README.md @@ -180,18 +180,6 @@ dimos --simulation run unitree-go2 dimos --simulation run unitree-g1-sim ``` -PimSim environments are stored in Git LFS. From a source checkout, fetch the -scene package before the first PimSim run; DimOS extracts it into -`data/scene_packages/` on first use. - -```bash -git lfs install -git lfs pull --include="data/.lfs/scene_packages.tar.gz" --exclude="" -``` - -See [Large File Management](docs/development/large_file_management.md) for the -data packaging and update workflow. - ```bash # Control a real robot (Unitree quadruped over WebRTC) export ROBOT_IP= diff --git a/data/.lfs/scene_packages.tar.gz b/data/.lfs/scene_packages.tar.gz index f0c800fcef..09c023313a 100644 --- a/data/.lfs/scene_packages.tar.gz +++ b/data/.lfs/scene_packages.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:8f97ea60e828466c438c5c63ba58600f192fa54f849d865051eae2cdf68cca86 -size 4078444330 +oid sha256:e362afc6c6f712b07e570dacb06e9a6c6fb2a29db6a9970556bd7acd233f2064 +size 1671132987 diff --git a/dimos/agents/mcp/mcp_client.py b/dimos/agents/mcp/mcp_client.py index 600fcd7e9e..859b15451b 100644 --- a/dimos/agents/mcp/mcp_client.py +++ b/dimos/agents/mcp/mcp_client.py @@ -243,7 +243,6 @@ def on_system_modules(self, _modules: list[RPCClient]) -> None: ) if not self._thread.is_alive(): self._thread.start() - self.agent_idle.publish(True) @rpc def stop(self) -> None: diff --git a/dimos/agents/mcp/test_mcp_client_unit.py b/dimos/agents/mcp/test_mcp_client_unit.py index f6d48d64f1..a49df130ff 100644 --- a/dimos/agents/mcp/test_mcp_client_unit.py +++ b/dimos/agents/mcp/test_mcp_client_unit.py @@ -250,18 +250,6 @@ def test_on_system_modules_uses_responses_api_model( assert model.reasoning == {"effort": "medium", "summary": "auto"} -def test_on_system_modules_publishes_initial_idle( - configured_mcp_client: McpClient, -) -> None: - with ( - patch("dimos.agents.mcp.mcp_client.create_agent"), - patch.object(configured_mcp_client.agent_idle, "publish") as publish, - ): - configured_mcp_client.on_system_modules([]) - - publish.assert_called_once_with(True) - - @pytest.mark.parametrize("model_name", ["gpt-4o", "ollama:qwen3:8b", "huggingface:Qwen/Qwen3-8B"]) def test_on_system_modules_resolves_non_reasoning_models( configured_mcp_client: McpClient, model_name: str diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 059d5c0699..eeed8dead0 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -913,7 +913,7 @@ def set_gripper_position(self, hardware_id: str, position: float) -> bool: if isinstance(hw, ConnectedTwistBase): logger.warning(f"Hardware '{hardware_id}' is a twist base, no gripper support") return False - return hw.set_gripper_position(position) + return hw.adapter.write_gripper_position(position) @rpc def get_gripper_position(self, hardware_id: str) -> float | None: diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index ef74c56e09..3a7c74f430 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -202,19 +202,6 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: return arm_ok and gripper_ok - def set_gripper_position(self, position: float) -> bool: - """Command the gripper and preserve that value across arm-only commands.""" - if not self._gripper_joints: - return False - if not self._initialized: - self._initialize_last_commanded() - if not self._adapter.write_gripper_position(position): - return False - normalized = self._physical_to_normalized(position) - for joint_name in self._gripper_joints: - self._last_commanded[joint_name] = normalized - return True - def _initialize_last_commanded(self) -> None: """Initialize last_commanded with current hardware positions.""" for _ in range(10): diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index a1b1cb7736..cbd3dd9f48 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -253,27 +253,6 @@ def make(**kwargs: Any) -> ControlCoordinator: class TestControlCoordinatorLifecycle: - def test_arm_trajectory_preserves_direct_gripper_command(self, make_coordinator, mock_adapter): - mock_adapter.read_gripper_position.return_value = 0.85 - mock_adapter.write_gripper_position.return_value = True - component = HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 6), - gripper_joints=["arm/gripper"], - ) - hardware = ConnectedHardware(mock_adapter, component) - coordinator = make_coordinator() - coordinator._hardware = {"arm": hardware} - - assert coordinator.set_gripper_position("arm", 0.0) - assert hardware.write_command({"arm/joint1": 0.2}, ControlMode.POSITION) - - assert [call.args[0] for call in mock_adapter.write_gripper_position.call_args_list] == [ - 0.0, - 0.0, - ] - def test_dispatch_routes_ee_twist_only_to_matching_frame_id(self, make_coordinator): coordinator = make_coordinator() matching_task = RecordingTask("eef") diff --git a/dimos/e2e_tests/conftest.py b/dimos/e2e_tests/conftest.py index 678ab8cffd..36709963a4 100644 --- a/dimos/e2e_tests/conftest.py +++ b/dimos/e2e_tests/conftest.py @@ -13,28 +13,20 @@ # limitations under the License. from collections.abc import Callable, Generator, Iterator -import os import threading import time import pytest -from dimos.core.global_config import global_config -from dimos.core.transport_factory import make_transport +from dimos.core.transport import pLCMTransport from dimos.e2e_tests.conf_types import StartPersonTrack +from dimos.e2e_tests.dim_sim_client import DimSimClient from dimos.e2e_tests.dimos_cli_call import DimosCliCall from dimos.e2e_tests.lcm_spy import LcmSpy -from dimos.e2e_tests.scene_control import SceneControl, load_scene_control -from dimos.e2e_tests.simulation_scenarios import APARTMENT_EXPLORATION_ROUTE from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import make_vector3 from dimos.msgs.std_msgs.Bool import Bool -from dimos.protocol.service.zenohservice import ( - ZENOH_LOCAL_ROUTER_ENDPOINT, - ZENOH_ROUTER_ENDPOINT_ENV, - ZenohRouter, -) from dimos.simulation.mujoco.direct_cmd_vel_explorer import DirectCmdVelExplorer from dimos.simulation.mujoco.person_on_track import PersonTrackPublisher @@ -48,51 +40,13 @@ def _pose(x: float, y: float, theta: float) -> PoseStamped: @pytest.fixture -def transport_runtime(monkeypatch) -> Iterator[None]: - if global_config.transport != "zenoh": - yield - return - - router = ZenohRouter() - router.start() - monkeypatch.setenv(ZENOH_ROUTER_ENDPOINT_ENV, ZENOH_LOCAL_ROUTER_ENDPOINT) - try: - yield - finally: - router.stop() - - -@pytest.fixture -def lcm_spy(transport_runtime: None) -> Iterator[LcmSpy]: - del transport_runtime +def lcm_spy() -> Iterator[LcmSpy]: lcm_spy = LcmSpy() lcm_spy.start() yield lcm_spy lcm_spy.stop() -@pytest.fixture -def wait_for_agent_ready(lcm_spy: LcmSpy) -> Callable[[float], None]: - topic = "/agent_idle" - lcm_spy.save_topic(topic) - - def wait(timeout: float = 120.0) -> None: - lcm_spy.wait_for_saved_topic(topic, timeout=timeout) - - return wait - - -@pytest.fixture -def wait_for_robot_odometry(lcm_spy: LcmSpy) -> Callable[[float], None]: - topic = "/odom#geometry_msgs.PoseStamped" - lcm_spy.save_topic(topic) - - def wait(timeout: float = 60.0) -> None: - lcm_spy.wait_for_saved_topic(topic, timeout=timeout) - - return wait - - @pytest.fixture def follow_points(lcm_spy: LcmSpy): def fun(*, points: list[tuple[float, float, float]], fail_message: str) -> None: @@ -113,23 +67,17 @@ def fun(*, points: list[tuple[float, float, float]], fail_message: str) -> None: @pytest.fixture -def start_blueprint( - mcp_port: int, - transport_runtime: None, -) -> Iterator[Callable[..., DimosCliCall]]: - del transport_runtime +def start_blueprint(mcp_port: int) -> Iterator[Callable[..., DimosCliCall]]: dimos_robot_call = DimosCliCall() dimos_robot_call.mcp_port = mcp_port def set_name_and_start( *demo_args: str, simulator: str | None = None, - scene_package: str | None = None, ) -> DimosCliCall: dimos_robot_call.demo_args = list(demo_args) if simulator is not None: dimos_robot_call.simulator = simulator - dimos_robot_call.scene_package = scene_package dimos_robot_call.start() return dimos_robot_call @@ -139,17 +87,16 @@ def set_name_and_start( @pytest.fixture -def human_input(transport_runtime: None): - del transport_runtime - transport = make_transport("/human_input") - transport.start() +def human_input(): + transport = pLCMTransport("/human_input") + transport.lcm.start() def send_human_input(message: str) -> None: - transport.broadcast(None, message) + transport.publish(message) yield send_human_input - transport.stop() + transport.lcm.stop() @pytest.fixture @@ -180,10 +127,7 @@ def run_person_track() -> None: @pytest.fixture -def direct_cmd_vel_explorer( - transport_runtime: None, -) -> Generator[DirectCmdVelExplorer, None, None]: - del transport_runtime +def direct_cmd_vel_explorer() -> Generator[PersonTrackPublisher, None, None]: explorer = DirectCmdVelExplorer() explorer.start() yield explorer @@ -218,30 +162,15 @@ def explore() -> None: @pytest.fixture -def simulator_name() -> str: - simulator = os.environ.get("DIMOS_E2E_SIMULATOR", "pimsim") - if simulator == "dimsim" and global_config.transport != "lcm": - raise pytest.UsageError( - "native DimSim publishes its robot bridge over LCM; use " - "DIMOS_TRANSPORT=lcm for DIMOS_E2E_SIMULATOR=dimsim" - ) - return simulator - - -@pytest.fixture -def scene_control( - simulator_name: str, - transport_runtime: None, -) -> Iterator[SceneControl]: - del transport_runtime - client = load_scene_control(simulator_name) +def dim_sim(): + client = DimSimClient() client.start() yield client client.stop() @pytest.fixture -def spawn_wall_on_pose(lcm_spy: LcmSpy, scene_control: SceneControl): +def spawn_wall_on_pose(lcm_spy: LcmSpy, dim_sim: DimSimClient): """Spawn a dim_sim wall when the robot's /odom comes within `threshold` metres of `point`.""" odom_topic = "/odom#geometry_msgs.PoseStamped" stop_event = threading.Event() @@ -267,7 +196,7 @@ def worker(): with lcm_spy.topic_listener(odom_topic, on_odom): while not stop_event.is_set(): if triggered.wait(timeout=0.1): - scene_control.add_wall(*wall) + dim_sim.add_wall(*wall) return except BaseException as e: errors.append(e) @@ -289,9 +218,34 @@ def worker(): def explore_house( direct_cmd_vel_explorer: DirectCmdVelExplorer, ) -> Callable[[], None]: + points = [ + (3.881, 4.803), + (4.160, 1.615), + (1.596, 1.505), + (1.649, 0.137), + (-3.644, -0.064), + (-3.759, -2.661), + (-4.186, -4.830), + (-3.759, -2.661), + (-1.070, -3.285), + (-2.504, -2.452), + (-2.647, 5.243), + (-3.663, 3.591), + (-1.178, 1.974), + (-2.416, 2.629), + (-2.581, 0.164), + (1.834, 0.072), + (3.010, -3.883), + (1.756, -3.742), + (6.336, -4.077), + (8.264, -5.119), + (6.258, -0.964), + (6.453, 5.327), + ] + direct_cmd_vel_explorer.linear_speed = 0.5 def explore() -> None: - direct_cmd_vel_explorer.follow_points(list(APARTMENT_EXPLORATION_ROUTE)) + direct_cmd_vel_explorer.follow_points(points) return explore diff --git a/dimos/e2e_tests/dim_sim_client.py b/dimos/e2e_tests/dim_sim_client.py index 8562bb4359..18b12074f5 100644 --- a/dimos/e2e_tests/dim_sim_client.py +++ b/dimos/e2e_tests/dim_sim_client.py @@ -12,11 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import cast - -from dimos.core.transport import PubSubTransport -from dimos.core.transport_factory import make_transport -from dimos.e2e_tests.scene_contract import PlanarBounds +from dimos.core.transport import LCMTransport from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.simulation.dimsim.scene_client import SceneClient @@ -26,10 +22,7 @@ class DimSimClient: def __init__(self) -> None: self._client = None - self._goal_request = cast( - "PubSubTransport[PoseStamped]", - make_transport("/goal_request", PoseStamped), - ) + self._goal_request: LCMTransport[PoseStamped] = LCMTransport("/goal_request", PoseStamped) def start(self) -> None: # self.client should be started lazily to avoid starting the dimsim @@ -54,24 +47,10 @@ def add_wall(self, x1: float, y1: float, x2: float, y2: float) -> None: self.client.add_wall(y1, x1, y2, x2) def publish_goal(self, x: float, y: float) -> None: - self._goal_request.broadcast( - None, + self._goal_request.publish( PoseStamped( position=(x, y, 0), orientation=(0, 0, 0, 1), frame_id="world", - ), - ) - - def semantic_object_bounds(self, query: str) -> PlanarBounds: - bounds = self.client.get_semantic_object_bounds(query) - minimum = bounds["min"] - maximum = bounds["max"] - # DimSim is Three.js Y-up. Its bridge publishes (z, x, y) as - # canonical DimOS (x, y, z), so apply the same mapping to the AABB. - return PlanarBounds( - min_x=float(minimum["z"]), - min_y=float(minimum["x"]), - max_x=float(maximum["z"]), - max_y=float(maximum["x"]), + ) ) diff --git a/dimos/e2e_tests/dimos_cli_call.py b/dimos/e2e_tests/dimos_cli_call.py index 26e97a33a1..3a4fdfb732 100644 --- a/dimos/e2e_tests/dimos_cli_call.py +++ b/dimos/e2e_tests/dimos_cli_call.py @@ -24,7 +24,6 @@ class DimosCliCall: mcp_port: int | None = None # None: no --simulation flag (e.g. replay runs via --robot-ip fake). simulator: str | None = "mujoco" - scene_package: str | None = None def __init__(self) -> None: self.process = None @@ -56,25 +55,15 @@ def start(self) -> None: if self.mcp_port is not None: global_overrides += ["--mcp-port", str(self.mcp_port)] env["MCPCLIENT__MCP_SERVER_URL"] = f"http://localhost:{self.mcp_port}/mcp" - - viewer = os.environ.get("DIMOS_E2E_VIEWER", "none") - if "--viewer" not in global_overrides: - global_overrides += ["--viewer", viewer] - - if self.simulator == "pimsim": - global_overrides += [ - "--simulation", - "mujoco", - "--simulation-provider", - "pimsim", - "--scene-package", - self.scene_package or "dimsim-apartment", - ] - elif self.simulator is not None: + if self.simulator is not None: global_overrides += ["--simulation", self.simulator] self.process = subprocess.Popen( - ["dimos", *global_overrides, *args], + [ + "dimos", + *global_overrides, + *args, + ], start_new_session=True, env=env, ) @@ -84,7 +73,7 @@ def stop(self) -> None: # call reaps the process its pgid can be recycled, and a second # killpg could hit an unrelated process group. process, self.process = self.process, None - if process is None or process.poll() is not None: + if process is None: return try: diff --git a/dimos/e2e_tests/lcm_spy.py b/dimos/e2e_tests/lcm_spy.py index 0d6775d014..cec34324f3 100644 --- a/dimos/e2e_tests/lcm_spy.py +++ b/dimos/e2e_tests/lcm_spy.py @@ -17,18 +17,18 @@ import math import pickle import threading -from typing import Any, cast +from typing import Any + +import lcm -from dimos.core.transport import PubSubTransport -from dimos.core.transport_factory import make_transport -from dimos.e2e_tests.scene_contract import PlanarBounds from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.helpers import resolve_msg_type from dimos.msgs.protocol import DimosMsg +from dimos.protocol.service.lcmservice import LCMService from dimos.utils.testing.waiting import wait_until -class LcmSpy: +class LcmSpy(LCMService): + l: lcm.LCM messages: dict[str, list[bytes]] _messages_lock: threading.Lock _saved_topics: set[str] @@ -36,33 +36,23 @@ class LcmSpy: _topic_listeners: dict[str, list[Callable[[bytes], None]]] _topic_listeners_lock: threading.Lock - def __init__(self) -> None: + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self.l = lcm.LCM() self.messages = {} self._messages_lock = threading.Lock() self._saved_topics = set() self._saved_topics_lock = threading.Lock() self._topic_listeners = {} self._topic_listeners_lock = threading.Lock() - self._transports: dict[str, PubSubTransport[Any]] = {} - self._unsubscribers: dict[str, Callable[[], None]] = {} - self._publishers: dict[str, PubSubTransport[Any]] = {} - self._transports_lock = threading.Lock() def start(self) -> None: - pass + super().start() + if self.l: + self.l.subscribe(".*", self.msg) def stop(self) -> None: - with self._transports_lock: - unsubscribers = tuple(self._unsubscribers.values()) - transports = tuple(self._transports.values()) - publishers = tuple(self._publishers.values()) - self._unsubscribers.clear() - self._transports.clear() - self._publishers.clear() - for unsubscribe in unsubscribers: - unsubscribe() - for transport in (*transports, *publishers): - transport.stop() + super().stop() def msg(self, topic: str, data: bytes) -> None: with self._saved_topics_lock: @@ -77,23 +67,15 @@ def msg(self, topic: str, data: bytes) -> None: listener(data) def publish(self, topic: str, msg: Any) -> None: - with self._transports_lock: - transport = self._publishers.get(topic) - if transport is None: - name, msg_type = _parse_topic(topic, type(msg)) - transport = make_transport(name, msg_type) - self._publishers[topic] = transport - transport.broadcast(None, msg) + self.l.publish(topic, msg.lcm_encode()) def save_topic(self, topic: str) -> None: with self._saved_topics_lock: self._saved_topics.add(topic) - self._ensure_subscription(topic) def register_topic_listener(self, topic: str, listener: Callable[[bytes], None]) -> None: with self._topic_listeners_lock: self._topic_listeners.setdefault(topic, []).append(listener) - self._ensure_subscription(topic) def unregister_topic_listener(self, topic: str, listener: Callable[[bytes], None]) -> None: with self._topic_listeners_lock: @@ -174,23 +156,6 @@ def listener(msg: bytes) -> None: message=fail_message, ) - def wait_for_saved_message_result( - self, - topic: str, - type: type[DimosMsg], - predicate: Callable[[Any], bool], - fail_message: str, - timeout: float = 30.0, - ) -> None: - """Wait for a matching message saved since ``save_topic`` was called.""" - - def condition() -> bool: - with self._messages_lock: - messages = tuple(self.messages.get(topic, ())) - return any(predicate(type.lcm_decode(message)) for message in messages) - - wait_until(condition, timeout=timeout, message=fail_message) - def wait_until_odom_position( self, x: float, y: float, threshold: float = 1, timeout: float = 60 ) -> None: @@ -206,46 +171,3 @@ def predicate(msg: PoseStamped) -> bool: f"Failed to get to position x={x}, y={y}", timeout, ) - - def wait_until_odom_near_bounds( - self, - bounds: PlanarBounds, - max_distance: float, - timeout: float = 60.0, - ) -> None: - def predicate(msg: PoseStamped) -> bool: - return bounds.distance_to(msg.position.x, msg.position.y) <= max_distance - - self.wait_for_message_result( - "/odom#geometry_msgs.PoseStamped", - PoseStamped, - predicate, - f"Robot did not get within {max_distance} m of semantic target bounds {bounds}", - timeout, - ) - - def _ensure_subscription(self, topic: str) -> None: - with self._transports_lock: - if topic in self._transports: - return - name, msg_type = _parse_topic(topic) - transport = make_transport(name, msg_type) - unsubscribe = transport.subscribe(lambda msg: self.msg(topic, _encode_message(msg))) - self._transports[topic] = transport - self._unsubscribers[topic] = unsubscribe - - -def _parse_topic(topic: str, default_type: type[Any] | None = None) -> tuple[str, type[Any] | None]: - if "#" not in topic: - return topic, default_type if hasattr(default_type, "lcm_encode") else None - name, type_name = topic.rsplit("#", 1) - msg_type = resolve_msg_type(type_name) - if msg_type is None: - raise ValueError(f"Unknown message type {type_name!r} in topic {topic!r}") - return name, msg_type - - -def _encode_message(message: Any) -> bytes: - if hasattr(message, "lcm_encode"): - return cast("bytes", message.lcm_encode()) - return pickle.dumps(message) diff --git a/dimos/e2e_tests/scene_contract.py b/dimos/e2e_tests/scene_contract.py deleted file mode 100644 index e35e429be2..0000000000 --- a/dimos/e2e_tests/scene_contract.py +++ /dev/null @@ -1,45 +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. - -from __future__ import annotations - -from dataclasses import dataclass -import math - - -@dataclass(frozen=True) -class PlanarBounds: - """Axis-aligned bounds in the canonical DimOS world XY plane.""" - - min_x: float - min_y: float - max_x: float - max_y: float - - def __post_init__(self) -> None: - values = (self.min_x, self.min_y, self.max_x, self.max_y) - if not all(math.isfinite(value) for value in values): - raise ValueError("planar bounds must be finite") - if self.min_x > self.max_x or self.min_y > self.max_y: - raise ValueError("planar bounds minimum must not exceed maximum") - - def distance_to(self, x: float, y: float) -> float: - """Return planar distance from a point to the filled bounds.""" - - dx = max(self.min_x - x, 0.0, x - self.max_x) - dy = max(self.min_y - y, 0.0, y - self.max_y) - return math.hypot(dx, dy) - - -__all__ = ["PlanarBounds"] diff --git a/dimos/e2e_tests/scene_control.py b/dimos/e2e_tests/scene_control.py deleted file mode 100644 index 8e1b56df58..0000000000 --- a/dimos/e2e_tests/scene_control.py +++ /dev/null @@ -1,54 +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. - -from __future__ import annotations - -from importlib.metadata import entry_points -from typing import Protocol, cast - -from dimos.e2e_tests.dim_sim_client import DimSimClient -from dimos.e2e_tests.scene_contract import PlanarBounds - - -class SceneControl(Protocol): - def start(self) -> None: ... - - def stop(self) -> None: ... - - def set_agent_position(self, x: float, y: float, z: float = 0.4) -> None: ... - - def add_wall(self, x1: float, y1: float, x2: float, y2: float) -> None: ... - - def publish_goal(self, x: float, y: float) -> None: ... - - def semantic_object_bounds(self, query: str) -> PlanarBounds: ... - - -def load_scene_control(simulator: str) -> SceneControl: - if simulator == "dimsim": - return DimSimClient() - matches = [ - entry - for entry in entry_points(group="dimos.simulation.scene_controls") - if entry.name == simulator - ] - if len(matches) != 1: - raise ValueError( - f"expected one scene-control provider for {simulator!r}, found {len(matches)}" - ) - client = matches[0].load()() - return cast("SceneControl", client) - - -__all__ = ["PlanarBounds", "SceneControl", "load_scene_control"] diff --git a/dimos/e2e_tests/simulation_scenarios.py b/dimos/e2e_tests/simulation_scenarios.py deleted file mode 100644 index e5cf98a11c..0000000000 --- a/dimos/e2e_tests/simulation_scenarios.py +++ /dev/null @@ -1,109 +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. - -from __future__ import annotations - -from dataclasses import dataclass -import math - - -@dataclass(frozen=True) -class SemanticNavigationScenario: - """Provider-neutral contract for one semantic navigation task.""" - - scenario_id: str - command: str - target_query: str - max_target_distance_m: float - navigation_timeout_s: float = 180.0 - - def __post_init__(self) -> None: - if not self.scenario_id.strip(): - raise ValueError("scenario ID must not be empty") - if not self.command.strip() or not self.target_query.strip(): - raise ValueError("semantic navigation text must not be empty") - if not math.isfinite(self.max_target_distance_m) or self.max_target_distance_m <= 0: - raise ValueError("target distance must be finite and positive") - if not math.isfinite(self.navigation_timeout_s) or self.navigation_timeout_s <= 0: - raise ValueError("navigation timeout must be finite and positive") - - -# Canonical DimOS world-frame route used to populate spatial memory. Both -# simulator providers must present the apartment in this frame. -APARTMENT_EXPLORATION_ROUTE: tuple[tuple[float, float], ...] = ( - (3.881, 4.803), - (4.160, 1.615), - (1.596, 1.505), - (1.649, 0.137), - (-3.644, -0.064), - (-3.759, -2.661), - (-4.186, -4.830), - (-3.759, -2.661), - (-1.070, -3.285), - (-2.504, -2.452), - (-2.647, 5.243), - (-3.663, 3.591), - (-1.178, 1.974), - (-2.416, 2.629), - (-2.581, 0.164), - (1.834, 0.072), - (3.010, -3.883), - (1.756, -3.742), - (6.336, -4.077), - (8.264, -5.119), - (6.258, -0.964), - (6.453, 5.327), -) - -# The former browser-local workflows all started at Three.js (0, 0.5, 3), -# which is canonical DimOS (3, 0, 0.5). Use a slightly settled root height for -# both providers and restore this neutral start after exploration. -APARTMENT_TASK_START: tuple[float, float, float] = (3.0, 0.0, 0.52) - -GO_TO_BED = SemanticNavigationScenario( - scenario_id="bed", - command="go to the bed", - target_query="queen size bed", - max_target_distance_m=2.0, -) - -APARTMENT_SEMANTIC_NAVIGATION_SCENARIOS: tuple[SemanticNavigationScenario, ...] = ( - SemanticNavigationScenario( - scenario_id="couch", - command="go to the couch", - target_query="sectional", - max_target_distance_m=2.0, - ), - SemanticNavigationScenario( - scenario_id="kitchen", - command="go to the kitchen", - target_query="refrigerator", - max_target_distance_m=3.0, - ), - SemanticNavigationScenario( - scenario_id="television", - command="go to the TV", - target_query="television", - max_target_distance_m=2.0, - ), -) - - -__all__ = [ - "APARTMENT_EXPLORATION_ROUTE", - "APARTMENT_SEMANTIC_NAVIGATION_SCENARIOS", - "APARTMENT_TASK_START", - "GO_TO_BED", - "SemanticNavigationScenario", -] diff --git a/dimos/e2e_tests/test_dimsim_path_replaning.py b/dimos/e2e_tests/test_dimsim_path_replaning.py index 7fa3455600..d222da30bd 100644 --- a/dimos/e2e_tests/test_dimsim_path_replaning.py +++ b/dimos/e2e_tests/test_dimsim_path_replaning.py @@ -14,41 +14,34 @@ import pytest -from dimos.msgs.std_msgs.Bool import Bool - @pytest.mark.self_hosted_large def test_path_replanning( - lcm_spy, - start_blueprint, - wait_for_agent_ready, - scene_control, - simulator_name, - direct_cmd_vel_explorer, - spawn_wall_on_pose, + lcm_spy, start_blueprint, dim_sim, direct_cmd_vel_explorer, spawn_wall_on_pose ) -> None: - args = ( - ("--dimsim-scene=empty", "run", "unitree-go2-agentic") - if simulator_name == "dimsim" - else ("run", "unitree-go2-agentic") + start_blueprint( + "--dimsim-scene=empty", + "run", + "unitree-go2-agentic", + simulator="dimsim", ) - start_blueprint(*args, simulator=simulator_name, scene_package="none") - wait_for_agent_ready(timeout=1200.0) + lcm_spy.save_topic("/rpc/McpClient/on_system_modules/res") + lcm_spy.wait_for_saved_topic("/rpc/McpClient/on_system_modules/res", timeout=1200.0) - scene_control.set_agent_position(3, 2) + # robot spawns at (3, 2) # side wall - scene_control.add_wall(2, -2.5, 12, -2.5) + dim_sim.add_wall(2, -2.5, 12, -2.5) # other side wall - scene_control.add_wall(2, 3.5, 12, 3.5) + dim_sim.add_wall(2, 3.5, 12, 3.5) # back wall (behind robot) - scene_control.add_wall(2, -2.5, 2, 3.5) + dim_sim.add_wall(2, -2.5, 2, 3.5) # forward wall (far end) - scene_control.add_wall(12, -2.5, 12, 3.5) + dim_sim.add_wall(12, -2.5, 12, 3.5) # dividing wall at x=7 with doors at y=[-1.5,-0.5] and y=[1.5,2.5] - scene_control.add_wall(7, -2.5, 7, -1.5) - scene_control.add_wall(7, -0.5, 7, 1.5) - scene_control.add_wall(7, 2.5, 7, 3.5) + dim_sim.add_wall(7, -2.5, 7, -1.5) + dim_sim.add_wall(7, -0.5, 7, 1.5) + dim_sim.add_wall(7, 2.5, 7, 3.5) direct_cmd_vel_explorer.linear_speed = 0.8 direct_cmd_vel_explorer.follow_points([(10, 2), (2.5, 2), (3, 2)]) @@ -62,12 +55,6 @@ def test_path_replanning( wall=(7, 1.5, 7, 2.5), ) - scene_control.publish_goal(10.913, 0.588) + dim_sim.publish_goal(10.913, 0.588) - lcm_spy.wait_for_message_result( - "/goal_reached#std_msgs.Bool", - Bool, - predicate=bool, - fail_message="Planner did not complete the replanned route", - timeout=120, - ) + lcm_spy.wait_until_odom_position(10.913, 0.588, threshold=1, timeout=120) diff --git a/dimos/e2e_tests/test_dimsim_spatial_memory.py b/dimos/e2e_tests/test_dimsim_spatial_memory.py index e472542eb0..df2d2477f6 100644 --- a/dimos/e2e_tests/test_dimsim_spatial_memory.py +++ b/dimos/e2e_tests/test_dimsim_spatial_memory.py @@ -14,114 +14,19 @@ import pytest -from dimos.e2e_tests.simulation_scenarios import ( - APARTMENT_SEMANTIC_NAVIGATION_SCENARIOS, - APARTMENT_TASK_START, - GO_TO_BED, - SemanticNavigationScenario, -) -from dimos.msgs.std_msgs.Bool import Bool -_GOAL_REACHED_TOPIC = "/goal_reached#std_msgs.Bool" - - -def _run_semantic_navigation_scenario( - scenario: SemanticNavigationScenario, - *, - lcm_spy, - start_blueprint, - wait_for_agent_ready, - wait_for_robot_odometry, - human_input, - scene_control, - simulator_name, - explore_house, -) -> None: +@pytest.mark.self_hosted_large +def test_go_to_the_bed(lcm_spy, start_blueprint, human_input, dim_sim, explore_house) -> None: start_blueprint( "run", "unitree-go2-agentic", - simulator=simulator_name, + simulator="dimsim", ) - wait_for_agent_ready(timeout=1200.0) - wait_for_robot_odometry(timeout=120.0) + lcm_spy.save_topic("/rpc/McpClient/on_system_modules/res") + lcm_spy.wait_for_saved_topic("/rpc/McpClient/on_system_modules/res", timeout=1200.0) - target_bounds = scene_control.semantic_object_bounds(scenario.target_query) explore_house() - scene_control.set_agent_position(*APARTMENT_TASK_START) - lcm_spy.wait_until_odom_position( - APARTMENT_TASK_START[0], - APARTMENT_TASK_START[1], - threshold=0.25, - timeout=30.0, - ) - - # Subscribe before sending the task so an immediate completion cannot race - # the assertion. The semantic bounds remain test-only ground truth. - lcm_spy.save_topic(_GOAL_REACHED_TOPIC) - human_input(scenario.command) - lcm_spy.wait_for_saved_message_result( - _GOAL_REACHED_TOPIC, - Bool, - predicate=lambda message: message.data is True, - fail_message=f"Navigation did not report completion for {scenario.command!r}", - timeout=scenario.navigation_timeout_s, - ) - lcm_spy.wait_until_odom_near_bounds( - target_bounds, - max_distance=scenario.max_target_distance_m, - timeout=30.0, - ) + human_input("go to the bed") -@pytest.mark.self_hosted_large -def test_go_to_the_bed( - lcm_spy, - start_blueprint, - wait_for_agent_ready, - human_input, - scene_control, - simulator_name, - explore_house, - wait_for_robot_odometry, -) -> None: - _run_semantic_navigation_scenario( - GO_TO_BED, - lcm_spy=lcm_spy, - start_blueprint=start_blueprint, - wait_for_agent_ready=wait_for_agent_ready, - wait_for_robot_odometry=wait_for_robot_odometry, - human_input=human_input, - scene_control=scene_control, - simulator_name=simulator_name, - explore_house=explore_house, - ) - - -@pytest.mark.self_hosted_large -@pytest.mark.parametrize( - "scenario", - APARTMENT_SEMANTIC_NAVIGATION_SCENARIOS, - ids=lambda scenario: scenario.scenario_id, -) -def test_apartment_semantic_navigation( - scenario, - lcm_spy, - start_blueprint, - wait_for_agent_ready, - wait_for_robot_odometry, - human_input, - scene_control, - simulator_name, - explore_house, -) -> None: - _run_semantic_navigation_scenario( - scenario, - lcm_spy=lcm_spy, - start_blueprint=start_blueprint, - wait_for_agent_ready=wait_for_agent_ready, - wait_for_robot_odometry=wait_for_robot_odometry, - human_input=human_input, - scene_control=scene_control, - simulator_name=simulator_name, - explore_house=explore_house, - ) + lcm_spy.wait_until_odom_position(-3.567, -1.332, threshold=2, timeout=180) diff --git a/dimos/e2e_tests/test_dimsim_walk_forward.py b/dimos/e2e_tests/test_dimsim_walk_forward.py index 50d9c280ea..fe8a73e94f 100644 --- a/dimos/e2e_tests/test_dimsim_walk_forward.py +++ b/dimos/e2e_tests/test_dimsim_walk_forward.py @@ -16,36 +16,21 @@ @pytest.mark.self_hosted_large -def test_walk_forward( - lcm_spy, - start_blueprint, - wait_for_agent_ready, - human_input, - scene_control, - simulator_name, -) -> None: - scene_args = ("--dimsim-scene=empty",) if simulator_name == "dimsim" else () +def test_walk_forward(lcm_spy, start_blueprint, human_input, dim_sim) -> None: start_blueprint( - *scene_args, "run", "--disable", "spatial-memory", "--disable", "security-module", "unitree-go2-agentic", - simulator=simulator_name, - scene_package="none", + simulator="dimsim", ) - wait_for_agent_ready(timeout=1200.0) + lcm_spy.save_topic("/rpc/McpClient/on_system_modules/res") + lcm_spy.wait_for_saved_topic("/rpc/McpClient/on_system_modules/res", timeout=1200.0) origin_x, origin_y = 1, 2 - scene_control.set_agent_position(origin_x, origin_y) - lcm_spy.save_topic("/global_costmap#nav_msgs.OccupancyGrid") - scene_control.add_wall(-1, -1, 6, -1) - scene_control.add_wall(-1, 5, 6, 5) - scene_control.add_wall(-1, -1, -1, 5) - scene_control.add_wall(6, -1, 6, 5) - lcm_spy.wait_for_saved_topic("/global_costmap#nav_msgs.OccupancyGrid", timeout=30) + dim_sim.set_agent_position(origin_x, origin_y) human_input("move forward 3 meter") diff --git a/dimos/e2e_tests/test_lcm_spy.py b/dimos/e2e_tests/test_lcm_spy.py deleted file mode 100644 index 55dc131efe..0000000000 --- a/dimos/e2e_tests/test_lcm_spy.py +++ /dev/null @@ -1,32 +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. - -from dimos.e2e_tests.lcm_spy import LcmSpy -from dimos.msgs.std_msgs.Bool import Bool - - -def test_wait_for_saved_message_result_matches_message_received_before_wait(mocker) -> None: - spy = LcmSpy() - mocker.patch.object(spy, "_ensure_subscription") - topic = "/goal_reached#std_msgs.Bool" - spy.save_topic(topic) - spy.msg(topic, Bool(True).lcm_encode()) - - spy.wait_for_saved_message_result( - topic, - Bool, - predicate=lambda message: message.data is True, - fail_message="missing completion", - timeout=0.1, - ) diff --git a/dimos/e2e_tests/test_scene_contract.py b/dimos/e2e_tests/test_scene_contract.py deleted file mode 100644 index 21e71b8b97..0000000000 --- a/dimos/e2e_tests/test_scene_contract.py +++ /dev/null @@ -1,43 +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. - -import pytest - -from dimos.e2e_tests.dim_sim_client import DimSimClient -from dimos.e2e_tests.scene_contract import PlanarBounds - - -def test_planar_bounds_distance_uses_nearest_point() -> None: - bounds = PlanarBounds(min_x=1.0, min_y=2.0, max_x=3.0, max_y=4.0) - - assert bounds.distance_to(2.0, 3.0) == 0.0 - assert bounds.distance_to(4.0, 5.0) == pytest.approx(2**0.5) - - -def test_dimsim_client_maps_browser_bounds_to_dimos_world(mocker) -> None: - mocker.patch("dimos.e2e_tests.dim_sim_client.make_transport") - browser_client = mocker.Mock() - browser_client.get_semantic_object_bounds.return_value = { - "min": {"x": -2.0, "y": 0.0, "z": 3.0}, - "max": {"x": 1.0, "y": 2.0, "z": 5.0}, - } - client = DimSimClient() - client._client = browser_client - - assert client.semantic_object_bounds("bed") == PlanarBounds( - min_x=3.0, - min_y=-2.0, - max_x=5.0, - max_y=1.0, - ) diff --git a/dimos/experimental/scene_cooking/browser/visuals.py b/dimos/experimental/scene_cooking/browser/visuals.py index 3bad6d20fb..7407f9caca 100644 --- a/dimos/experimental/scene_cooking/browser/visuals.py +++ b/dimos/experimental/scene_cooking/browser/visuals.py @@ -306,11 +306,13 @@ def _export_with_gltfpack( "-o", str(target), "-mm", + "-si", + str(spec.simplify_ratio), + "-se", + str(spec.simplify_error), "-r", str(report_path), ] - if spec.simplify_ratio < 1.0: - args.extend(["-si", str(spec.simplify_ratio), "-se", str(spec.simplify_error)]) if not spec.quantize: args.append("-noq") if spec.use_gpu_instancing: @@ -415,14 +417,6 @@ def _validate_output( output_stats: dict[str, Any], spec: BrowserVisualSpec, ) -> None: - if spec.preserve_geometry: - source_triangles = int(source_stats.get("expanded_triangle_count") or 0) - output_triangles = int(output_stats.get("expanded_triangle_count") or 0) - if source_triangles != output_triangles: - raise RuntimeError( - "geometry-preserving visual cook changed expanded triangle count from " - f"{source_triangles} to {output_triangles}" - ) source_vertices = int(source_stats.get("vertex_count") or 0) output_vertices = int(output_stats.get("vertex_count") or 0) if source_vertices <= 0 or output_vertices <= 0: diff --git a/dimos/experimental/scene_cooking/cook.py b/dimos/experimental/scene_cooking/cook.py index 2a3e79506d..6666512d53 100644 --- a/dimos/experimental/scene_cooking/cook.py +++ b/dimos/experimental/scene_cooking/cook.py @@ -29,10 +29,7 @@ from typing import Any from dimos.experimental.scene_cooking.browser.collision import cook_browser_collision -from dimos.experimental.scene_cooking.browser.visuals import ( - BrowserVisualCookResult, - cook_browser_visual, -) +from dimos.experimental.scene_cooking.browser.visuals import cook_browser_visual from dimos.experimental.scene_cooking.entities.collision import ( COLLISION_DIR_NAME, cook_entity_collision_hulls, @@ -63,7 +60,7 @@ SCENE_PACKAGE_DIR = get_data_dir("scene_packages") _PACKAGE_KEY_LEN = 12 -_COOK_VERSION = 5 +_COOK_VERSION = 4 #: Cap on entity id samples recorded in cook stats -- diagnostics only, not #: the full entity list (that lives in ``scene.meta.json``). _ENTITY_ID_SAMPLE_CAP = 100 @@ -77,7 +74,6 @@ def cook_scene_package( collision_spec: CollisionSpec | None = None, cook_sidecar: SceneCookSidecar | None = None, visual_spec: BrowserVisualSpec | None = None, - visual_specs: tuple[BrowserVisualSpec, ...] | None = None, browser_collision_spec: BrowserCollisionSpec | None = None, mujoco_spec: MujocoSceneSpec | None = None, rebake: bool = False, @@ -94,23 +90,13 @@ def cook_scene_package( raise FileNotFoundError(f"scene source not found: {source}") align = alignment or SceneMeshAlignment() - if visual_spec is not None and visual_specs is not None: - raise ValueError("pass visual_spec or visual_specs, not both") - visuals = ( - tuple(visual_specs) if visual_specs is not None else (visual_spec or BrowserVisualSpec(),) - ) - visual_targets = [visual.target_key for visual in visuals] - if len(visual_targets) != len(set(visual_targets)): - raise ValueError("visual_specs must use unique targets") - artifact_names = [visual.artifact_name for visual in visuals if visual.enabled] - if len(artifact_names) != len(set(artifact_names)): - raise ValueError("enabled visual_specs must use unique output names") + visual = visual_spec or BrowserVisualSpec() browser_collision = browser_collision_spec or BrowserCollisionSpec() mujoco = mujoco_spec or MujocoSceneSpec() cook_spec = SceneCookSpec( source_path=source, alignment=align, - browser_visuals=visuals, + browser_visual=visual, browser_collision=browser_collision, mujoco=mujoco, ) @@ -158,7 +144,7 @@ def cook_scene_package( visual_source = cook_source # Only invoke Blender when at least one entity actually extracts from # the source mesh; pure-synthetic sidecars (manip rigs) don't need it. - needs_blender = any(visual.enabled for visual in visuals) and any( + needs_blender = visual.enabled and any( entity.visual_path is not None for entity in plan.entities ) if needs_blender: @@ -183,28 +169,24 @@ def cook_scene_package( if hull_counts: stats["entity_collision"]["hulls_per_entity"] = hull_counts - visual_results: dict[str, BrowserVisualCookResult] = {} - visual_stats_by_target: dict[str, dict[str, Any]] = {} - for visual in visuals: - visual_result = cook_browser_visual( - visual_source, - browser_dir, - spec=visual, - rebake=rebake, - ) - if visual_result is None: - continue - visual_results[visual.target_key] = visual_result - visual_stats_by_target[visual.target_key] = { + visual_result = cook_browser_visual( + visual_source, + browser_dir, + spec=visual, + rebake=rebake, + ) + if visual_result is not None: + visual_stats = { "target": visual.target_key, "tool": visual_result.tool, **visual_result.stats, } - if visual_stats_by_target: - stats["browser_visuals"] = visual_stats_by_target - stats["browser_visual"] = visual_stats_by_target.get( - "rerun", next(iter(visual_stats_by_target.values())) - ) + stats["browser_visual"] = { + **visual_stats, + } + stats["browser_visuals"] = { + visual.target_key: visual_stats, + } browser_collision_result = cook_browser_collision( cook_source, @@ -237,15 +219,12 @@ def cook_scene_package( stats["mujoco"]["binary_path"] = str(mujoco_binary_path) stats["mujoco"]["binary"] = binary_stats - primary_visual = visual_results.get("rerun") - if primary_visual is None and visual_results: - primary_visual = next(iter(visual_results.values())) package = ScenePackage( package_dir=package_dir, source_path=source, alignment=align, - visual_path=primary_visual.path if primary_visual else None, - browser_visuals={target: result.path for target, result in visual_results.items()}, + visual_path=visual_result.path if visual_result else None, + browser_visuals={visual.target_key: visual_result.path} if visual_result else {}, browser_collision_path=browser_collision_result.path if browser_collision_result else None, objects_path=browser_collision_result.objects_path if browser_collision_result else None, mujoco_scene_path=mujoco_scene_path, diff --git a/dimos/experimental/scene_cooking/package_config.py b/dimos/experimental/scene_cooking/package_config.py index d536462e6b..0956083b61 100644 --- a/dimos/experimental/scene_cooking/package_config.py +++ b/dimos/experimental/scene_cooking/package_config.py @@ -38,7 +38,6 @@ class BrowserVisualSpec: normalize_textures: bool = True quantize: bool = False use_gpu_instancing: bool = False - preserve_geometry: bool = False demote_required_extensions: tuple[str, ...] = ("KHR_texture_transform",) max_meshes: int = 200 max_materials: int = 50 @@ -46,21 +45,6 @@ class BrowserVisualSpec: max_vertices: int = 750_000 max_vertex_growth_ratio: float = 1.25 - def __post_init__(self) -> None: - if not 0.0 < self.simplify_ratio <= 1.0: - raise ValueError("visual simplify_ratio must satisfy 0 < ratio <= 1") - if self.preserve_geometry and ( - self.simplify_ratio != 1.0 - or self.quantize - or self.texture_format is not None - or self.max_texture_size is not None - or self.normalize_textures - ): - raise ValueError( - "geometry-preserving visuals cannot simplify, quantize, resize, " - "convert, or normalize textures" - ) - @property def target_key(self) -> str: return self.target.strip().lower() @@ -73,23 +57,6 @@ def artifact_name(self) -> str: #: Per-target overrides layered on top of ``BrowserVisualSpec``'s defaults. #: "rerun" has no entry here -- its values *are* the dataclass defaults. _BROWSER_VISUAL_PROFILES: dict[str, dict[str, Any]] = { - "mesh": { - "optimizer": "gltfpack", - "simplify_ratio": 1.0, - "simplify_error": 0.0, - "texture_format": None, - "max_texture_size": None, - "normalize_textures": False, - "quantize": False, - "use_gpu_instancing": True, - "preserve_geometry": True, - "demote_required_extensions": (), - "max_meshes": 1_000, - "max_materials": 500, - "max_textures": 2_000, - "max_vertices": 2_000_000, - "max_vertex_growth_ratio": 1.0, - }, "babylon": { "optimizer": "gltfpack", "simplify_ratio": 0.3, @@ -164,8 +131,6 @@ class SceneCookSpec: source_path: Path alignment: SceneMeshAlignment = field(default_factory=SceneMeshAlignment) - browser_visuals: tuple[BrowserVisualSpec, ...] = field( - default_factory=lambda: (BrowserVisualSpec(),) - ) + browser_visual: BrowserVisualSpec = field(default_factory=BrowserVisualSpec) browser_collision: BrowserCollisionSpec = field(default_factory=BrowserCollisionSpec) mujoco: MujocoSceneSpec = field(default_factory=MujocoSceneSpec) diff --git a/dimos/experimental/scene_cooking/source_assets/inspect.py b/dimos/experimental/scene_cooking/source_assets/inspect.py index 59d4e441c8..e9221edd65 100644 --- a/dimos/experimental/scene_cooking/source_assets/inspect.py +++ b/dimos/experimental/scene_cooking/source_assets/inspect.py @@ -17,14 +17,11 @@ from __future__ import annotations from dataclasses import asdict, dataclass -import json from pathlib import Path from typing import Any import numpy as np -from dimos.experimental.scene_cooking.source_assets.glb import read_glb - @dataclass(frozen=True) class SceneAssetStats: @@ -37,11 +34,6 @@ class SceneAssetStats: texture_count: int = 0 vertex_count: int = 0 triangle_count: int = 0 - primitive_count: int = 0 - draw_count: int = 0 - instance_count: int = 0 - expanded_triangle_count: int = 0 - extensions_used: tuple[str, ...] = () def to_json_dict(self) -> dict[str, Any]: return asdict(self) @@ -62,109 +54,68 @@ def inspect_scene_asset(path: str | Path) -> SceneAssetStats: def _inspect_gltf(path: Path) -> SceneAssetStats: - gltf = read_glb(path)[0] if path.suffix.lower() == ".glb" else json.loads(path.read_text()) - accessors = gltf.get("accessors", []) - meshes = gltf.get("meshes", []) - nodes = gltf.get("nodes", []) - if ( - not isinstance(accessors, list) - or not isinstance(meshes, list) - or not isinstance(nodes, list) - ): - raise RuntimeError(f"invalid glTF scene structure: {path}") - - mesh_triangle_counts: list[int] = [] - mesh_primitive_counts: list[int] = [] + import trimesh + + loaded: Any = trimesh.load(str(path)) + if isinstance(loaded, trimesh.Trimesh): + # visual may be ColorVisuals (no material) or TextureVisuals. + material = getattr(loaded.visual, "material", None) + material_count = 1 if material is not None else 0 + return SceneAssetStats( + path=str(path), + bytes=path.stat().st_size, + format=path.suffix.lower().lstrip("."), + mesh_count=1, + node_count=1, + material_count=material_count, + texture_count=_count_material_textures([material]), + vertex_count=len(loaded.vertices), + triangle_count=len(loaded.faces), + ) + + scene = loaded + mesh_count = len(getattr(scene, "geometry", {})) + node_count = len(getattr(scene.graph, "nodes_geometry", [])) + materials = [] vertex_count = 0 triangle_count = 0 - primitive_count = 0 - for mesh in meshes: - primitives = mesh.get("primitives", []) if isinstance(mesh, dict) else [] - mesh_triangles = 0 - for primitive in primitives: - if not isinstance(primitive, dict): - continue - attributes = primitive.get("attributes", {}) - position_index = attributes.get("POSITION") if isinstance(attributes, dict) else None - if isinstance(position_index, int): - vertex_count += _accessor_count(accessors, position_index, path) - element_index = primitive.get("indices", position_index) - element_count = ( - _accessor_count(accessors, element_index, path) - if isinstance(element_index, int) - else 0 - ) - triangles = _triangle_count(int(primitive.get("mode", 4)), element_count) - mesh_triangles += triangles - triangle_count += triangles - primitive_count += 1 - mesh_triangle_counts.append(mesh_triangles) - mesh_primitive_counts.append(len(primitives)) - - node_count = 0 - draw_count = 0 - instance_count = 0 - expanded_triangle_count = 0 - for node in nodes: - mesh_index = node.get("mesh") if isinstance(node, dict) else None - if not isinstance(mesh_index, int): + for geom in scene.geometry.values(): + if not isinstance(geom, trimesh.Trimesh): continue - if mesh_index < 0 or mesh_index >= len(meshes): - raise RuntimeError(f"glTF node references missing mesh {mesh_index}: {path}") - count = _node_instance_count(node, accessors, path) - node_count += 1 - draw_count += mesh_primitive_counts[mesh_index] - instance_count += count - expanded_triangle_count += mesh_triangle_counts[mesh_index] * count - + vertex_count += len(geom.vertices) + triangle_count += len(geom.faces) + materials.append(getattr(geom.visual, "material", None)) + material_keys = {repr(material) for material in materials if material is not None} return SceneAssetStats( path=str(path), bytes=path.stat().st_size, format=path.suffix.lower().lstrip("."), - mesh_count=len(meshes), + mesh_count=mesh_count, node_count=node_count, - material_count=len(gltf.get("materials", [])), - texture_count=len(gltf.get("textures", [])), + material_count=len(material_keys), + texture_count=_count_material_textures(materials), vertex_count=vertex_count, triangle_count=triangle_count, - primitive_count=primitive_count, - draw_count=draw_count, - instance_count=instance_count, - expanded_triangle_count=expanded_triangle_count, - extensions_used=tuple(sorted(str(value) for value in gltf.get("extensionsUsed", []))), ) -def _accessor_count(accessors: list[Any], index: int, path: Path) -> int: - if index < 0 or index >= len(accessors) or not isinstance(accessors[index], dict): - raise RuntimeError(f"glTF references missing accessor {index}: {path}") - return int(accessors[index].get("count", 0)) - - -def _triangle_count(mode: int, element_count: int) -> int: - if mode == 4: - return element_count // 3 - if mode in {5, 6}: - return max(0, element_count - 2) - return 0 - - -def _node_instance_count(node: dict[str, Any], accessors: list[Any], path: Path) -> int: - extensions = node.get("extensions", {}) - instancing = extensions.get("EXT_mesh_gpu_instancing") if isinstance(extensions, dict) else None - if not isinstance(instancing, dict): - return 1 - attributes = instancing.get("attributes") - if not isinstance(attributes, dict) or not attributes: - raise RuntimeError(f"empty EXT_mesh_gpu_instancing attributes: {path}") - counts = { - _accessor_count(accessors, index, path) - for index in attributes.values() - if isinstance(index, int) - } - if len(counts) != 1: - raise RuntimeError(f"inconsistent EXT_mesh_gpu_instancing accessor counts: {path}") - return counts.pop() +def _count_material_textures(materials: list[Any]) -> int: + textures: set[int] = set() + for material in materials: + if material is None: + continue + for name in ( + "baseColorTexture", + "metallicRoughnessTexture", + "normalTexture", + "emissiveTexture", + "occlusionTexture", + "image", + ): + image = getattr(material, name, None) + if image is not None: + textures.add(id(image)) + return len(textures) def _inspect_usd(path: Path) -> SceneAssetStats: diff --git a/dimos/experimental/scene_cooking/test_cooking.py b/dimos/experimental/scene_cooking/test_cooking.py index 1cc0da96c3..e37ab0847f 100644 --- a/dimos/experimental/scene_cooking/test_cooking.py +++ b/dimos/experimental/scene_cooking/test_cooking.py @@ -24,7 +24,6 @@ from dimos.experimental.scene_cooking import planning as plan_module from dimos.experimental.scene_cooking.package_config import browser_visual_spec_for_target from dimos.experimental.scene_cooking.sidecar import SceneCookSidecar -from dimos.experimental.scene_cooking.source_assets.inspect import inspect_scene_asset from dimos.experimental.scene_cooking.source_assets.mesh import ScenePrimMesh from dimos.simulation.scene_assets.spec import ( ARTIFACT_FRAMES, @@ -191,7 +190,6 @@ def test_load_scene_package_tolerates_missing_objects_sidecar(tmp_path: Path) -> def test_browser_visual_profiles_are_backend_specific() -> None: rerun = browser_visual_spec_for_target("rerun") babylon = browser_visual_spec_for_target("babylon") - mesh = browser_visual_spec_for_target("mesh") assert rerun.artifact_name == "visual.rerun.glb" assert rerun.quantize is False @@ -203,59 +201,6 @@ def test_browser_visual_profiles_are_backend_specific() -> None: assert babylon.normalize_textures is False assert babylon.demote_required_extensions == () - assert mesh.artifact_name == "visual.mesh.glb" - assert mesh.simplify_ratio == 1.0 - assert mesh.quantize is False - assert mesh.normalize_textures is False - assert mesh.texture_format is None - assert mesh.max_texture_size is None - assert mesh.use_gpu_instancing is True - assert mesh.preserve_geometry is True - - -def test_scene_asset_inspection_counts_gpu_instances(tmp_path: Path) -> None: - path = tmp_path / "instanced.gltf" - path.write_text( - json.dumps( - { - "asset": {"version": "2.0"}, - "accessors": [ - {"count": 3}, - {"count": 3}, - {"count": 4}, - ], - "meshes": [ - { - "primitives": [ - { - "attributes": {"POSITION": 0}, - "indices": 1, - } - ] - } - ], - "nodes": [ - { - "mesh": 0, - "extensions": { - "EXT_mesh_gpu_instancing": {"attributes": {"TRANSLATION": 2}} - }, - } - ], - "extensionsUsed": ["EXT_mesh_gpu_instancing"], - } - ) - ) - - stats = inspect_scene_asset(path) - - assert stats.node_count == 1 - assert stats.draw_count == 1 - assert stats.instance_count == 4 - assert stats.triangle_count == 1 - assert stats.expanded_triangle_count == 4 - assert stats.extensions_used == ("EXT_mesh_gpu_instancing",) - def test_extract_scene_objects_emits_per_prim_aabb() -> None: # Inlined so importing this test module doesn't pull heavy open3d/trimesh diff --git a/dimos/hardware/sensors/lidar/pointlio/zenoh_relay.py b/dimos/hardware/sensors/lidar/pointlio/zenoh_relay.py deleted file mode 100644 index 246faba7ed..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/zenoh_relay.py +++ /dev/null @@ -1,77 +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. - -from __future__ import annotations - -from reactivex.disposable import Disposable - -from dimos.core.core import rpc -from dimos.core.module import Module, ModuleConfig -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.protocol.pubsub.impl.lcmpubsub import LCMPubSubBase, Topic as LCMTopic -from dimos.protocol.pubsub.impl.zenohpubsub import ( - QOS_LATEST_WINS, - Topic as ZenohTopic, - ZenohPubSubBase, -) - - -class PointLioZenohRelayConfig(ModuleConfig): - lidar_topic: str = "lidar" - odometry_topic: str = "odometry" - - -class PointLioZenohRelay(Module): - config: PointLioZenohRelayConfig - - @rpc - def start(self) -> None: - super().start() - lcm = LCMPubSubBase() - zenoh = ZenohPubSubBase() - lcm.start() - zenoh.start() - - lidar_name = _topic_name(self.config.lidar_topic) - odometry_name = _topic_name(self.config.odometry_topic) - lidar = ZenohTopic(lidar_name, PointCloud2, qos=QOS_LATEST_WINS) - odometry = ZenohTopic(odometry_name, Odometry) - self.register_disposable( - Disposable( - lcm.subscribe( - LCMTopic(f"{lidar_name}/{PointCloud2.msg_name}"), - lambda payload, _: zenoh.publish(lidar, payload), - ) - ) - ) - self.register_disposable( - Disposable( - lcm.subscribe( - LCMTopic(f"{odometry_name}/{Odometry.msg_name}"), - lambda payload, _: zenoh.publish(odometry, payload), - ) - ) - ) - self.register_disposable(Disposable(lcm.stop)) - self.register_disposable(Disposable(zenoh.stop)) - - -def _topic_name(topic: str) -> str: - name = topic.strip("/") - if name.startswith("dimos/"): - name = name.removeprefix("dimos/") - if not name: - raise ValueError("PointLIO relay topics cannot be empty") - return f"dimos/{name}" diff --git a/dimos/hardware/simulation/episode_control.py b/dimos/hardware/simulation/episode_control.py deleted file mode 100644 index ac0c1fb75d..0000000000 --- a/dimos/hardware/simulation/episode_control.py +++ /dev/null @@ -1,28 +0,0 @@ -# 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. - -"""Provider-neutral simulation episode controls.""" - -from typing import Protocol - -from dimos.spec.utils import Spec - - -class SimulationEpisodeControlSpec(Spec, Protocol): - """Optional control surface implemented by simulation providers.""" - - def reset_episode(self) -> bool: ... - - -__all__ = ["SimulationEpisodeControlSpec"] diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index a54b54a9e8..137d8af72c 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -40,7 +40,6 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.hardware.simulation.episode_control import SimulationEpisodeControlSpec from dimos.manipulation.execution_manager import ( ExecutionOutcome, ExecutionTarget, @@ -81,11 +80,7 @@ RobotName, WorldRobotID, ) -from dimos.manipulation.planning.spec.protocols import ( - IKStepCallback, - KinematicsSpec, - PlannerSpec, -) +from dimos.manipulation.planning.spec.protocols import KinematicsSpec, PlannerSpec from dimos.manipulation.planning.trajectory_generator.joint_trajectory_generator import ( JointTrajectoryGenerator, ) @@ -109,10 +104,6 @@ logger = setup_logger() -_INTERACTIVE_IK_POSITION_TOLERANCE_M = 0.02 -_INTERACTIVE_IK_ORIENTATION_TOLERANCE_RAD = math.pi -_INTERACTIVE_IK_MAX_ATTEMPTS = 1 - # Composite type aliases for readability (using semantic IDs from planning.spec) RobotEntry: TypeAlias = tuple[WorldRobotID, RobotModelConfig, JointTrajectoryGenerator] """(world_robot_id, config, trajectory_generator)""" @@ -175,7 +166,6 @@ class ManipulationModule(Module): config: ManipulationModuleConfig _control_coordinator: ControlCoordinator - _episode_control: SimulationEpisodeControlSpec | None = None # Input: Joint state from coordinator (for world sync) coordinator_joint_state: In[JointState] @@ -482,18 +472,8 @@ def reset(self) -> SkillResult[ManipulationSkillError]: ) if self._state == ManipulationState.PLANNING: self._planning_epoch += 1 - plan = self._last_plan - self._last_plan = None self._state = ManipulationState.IDLE self._error_message = "" - if self._episode_control is not None and not self._episode_control.reset_episode(): - message = "Simulation provider failed to reset the episode" - with self._lock: - self._state = ManipulationState.FAULT - self._error_message = message - return SkillResult.fail("EXECUTION_FAILED", message) - if plan is not None: - self._dismiss_preview(plan.group_ids) return SkillResult.ok("Reset to IDLE — ready for new commands") @rpc @@ -883,43 +863,6 @@ def inverse_kinematics( check_collision: bool = True, ) -> IKResult: """Solve planning-group pose targets without planning a joint path.""" - return self._inverse_kinematics( - pose_targets=pose_targets, - auxiliary_group_ids=auxiliary_group_ids, - seed=seed, - check_collision=check_collision, - ) - - def inverse_kinematics_interactive( - self, - pose_targets: Mapping[PlanningGroupID, PoseStamped], - auxiliary_group_ids: Sequence[PlanningGroupID] = (), - seed: JointState | None = None, - on_step: IKStepCallback | None = None, - ) -> IKResult: - """Run bounded advisory IK for an in-process interactive target editor.""" - return self._inverse_kinematics( - pose_targets=pose_targets, - auxiliary_group_ids=auxiliary_group_ids, - seed=seed, - check_collision=True, - position_tolerance=_INTERACTIVE_IK_POSITION_TOLERANCE_M, - orientation_tolerance=_INTERACTIVE_IK_ORIENTATION_TOLERANCE_RAD, - max_attempts=_INTERACTIVE_IK_MAX_ATTEMPTS, - on_step=on_step, - ) - - def _inverse_kinematics( - self, - pose_targets: Mapping[PlanningGroupID, PoseStamped], - auxiliary_group_ids: Sequence[PlanningGroupID] = (), - seed: JointState | None = None, - check_collision: bool = True, - position_tolerance: float = 0.001, - orientation_tolerance: float = 0.01, - max_attempts: int = 10, - on_step: IKStepCallback | None = None, - ) -> IKResult: if self._kinematics is None or self._world_monitor is None: return IKResult(status=IKStatus.NO_SOLUTION, message="Planning not initialized") if not pose_targets: @@ -954,11 +897,7 @@ def _inverse_kinematics( pose_targets=target_groups, auxiliary_groups=auxiliary_groups, seed=seed_state, - position_tolerance=position_tolerance, - orientation_tolerance=orientation_tolerance, check_collision=check_collision, - max_attempts=max_attempts, - on_step=on_step, ) @rpc diff --git a/dimos/manipulation/pick_and_place_module.py b/dimos/manipulation/pick_and_place_module.py index 4926d80511..c788277b94 100644 --- a/dimos/manipulation/pick_and_place_module.py +++ b/dimos/manipulation/pick_and_place_module.py @@ -90,7 +90,6 @@ def __init__(self, **kwargs: Any) -> None: # The live detection cache is volatile (labels change every frame), # so pick/place use this stable snapshot instead. self._detection_snapshot: list[DetObject] = [] - self._held_object_id: str | None = None @rpc def start(self) -> None: @@ -301,14 +300,11 @@ def _generate_grasps_for_pick( cx, cy, cz = det.center.x, det.center.y, det.center.z xy_dist = (cx**2 + cy**2) ** 0.5 - # Exact scene state has no single-viewpoint occlusion error to correct. - if det.identity_basis == "pimsim_scene": - inset = 0.0 - gx, gy = cx, cy - else: - # Near detections need more correction toward the visible surface. - inset = 0.01 if xy_dist < _FAR_OCCLUSION_XY_THRESHOLD else 0.05 - gx, gy = self._occlusion_offset(det.center, det.size, inset=inset) + # Distance-adaptive occlusion offset: + # Near (< 0.8m): small inset — grasp shifted well toward robot (front surface) + # Far (>= 0.8m): larger inset — less toward-robot shift (grasp closer to true center) + inset = 0.01 if xy_dist < _FAR_OCCLUSION_XY_THRESHOLD else 0.05 + gx, gy = self._occlusion_offset(det.center, det.size, inset=inset) # For tall objects, grasp in the upper third instead of center # to avoid plunging deep and colliding with the object. @@ -490,14 +486,8 @@ def pick( pre_grasp_offset = config.pre_grasp_offset # 1. Generate grasps (uses already-cached detections — call scan_objects first) - target = self._find_object_in_detections(object_name, object_id) - if target is None: - return SkillResult.fail( - "GRASP_GENERATION_FAILED", - f"No grasp poses found for '{object_name}'. Object may not be detected.", - ) logger.info(f"Generating grasp poses for '{object_name}'...") - grasp_poses = self._generate_grasps_for_pick(object_name, target.object_id) + grasp_poses = self._generate_grasps_for_pick(object_name, object_id) if not grasp_poses: return SkillResult.fail( "GRASP_GENERATION_FAILED", @@ -525,8 +515,7 @@ def pick( # 3. Open gripper before approach logger.info("Opening gripper...") - if not self._set_gripper_position(0.85, rname): - return SkillResult.fail("GRIPPER_FAILED", "Failed to open gripper") + self._set_gripper_position(0.85, rname) time.sleep(0.5) # 4. Execute approach to pre-grasp @@ -535,27 +524,16 @@ def pick( return exec_result # 5. Move to grasp pose - target_removed = bool( - self._world_monitor and self._world_monitor.remove_object_obstacle(target.object_id) - ) logger.info("Moving to grasp position...") if not self.plan_to_pose(grasp_pose, rname): - if target_removed: - self.refresh_obstacles() return SkillResult.fail("PLANNING_FAILED", "Grasp pose planning failed") exec_result = self._preview_execute_wait(rname) if not exec_result.is_success(): - if target_removed: - self.refresh_obstacles() return exec_result # 6. Close gripper logger.info("Closing gripper...") - if not self._set_gripper_position(0.0, rname): - if target_removed: - self.refresh_obstacles() - return SkillResult.fail("GRIPPER_FAILED", "Failed to close gripper") - self._held_object_id = target.object_id + self._set_gripper_position(0.0, rname) time.sleep(1.5) # Wait for gripper to close # 7. Retract to pre-grasp @@ -646,23 +624,17 @@ def _place_with_orientation( # 3. Release logger.info("Releasing object...") - if not self._set_gripper_position(0.85, rname): - return SkillResult.fail("GRIPPER_FAILED", "Failed to open gripper") - self._held_object_id = None + self._set_gripper_position(0.85, rname) time.sleep(1.0) # 4. Retract logger.info("Retracting...") if not self.plan_to_pose(pre_place_pose, rname): - self.refresh_obstacles() return SkillResult.fail("PLANNING_FAILED", "Retract planning failed") exec_result = self._preview_execute_wait(rname) if not exec_result.is_success(): - self.refresh_obstacles() return exec_result - self.refresh_obstacles() - return SkillResult.ok(f"Place complete — object released at ({x:.3f}, {y:.3f}, {z:.3f})") @skill diff --git a/dimos/manipulation/planning/kinematics/drake_optimization_ik.py b/dimos/manipulation/planning/kinematics/drake_optimization_ik.py index e827473e4c..91da25986c 100644 --- a/dimos/manipulation/planning/kinematics/drake_optimization_ik.py +++ b/dimos/manipulation/planning/kinematics/drake_optimization_ik.py @@ -29,7 +29,7 @@ ) from dimos.manipulation.planning.spec.enums import IKStatus from dimos.manipulation.planning.spec.models import IKResult, WorldRobotID -from dimos.manipulation.planning.spec.protocols import IKStepCallback, WorldSpec +from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.utils.kinematics_utils import compute_pose_error from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Transform import Transform @@ -84,7 +84,6 @@ def solve( orientation_tolerance: float = 0.01, check_collision: bool = True, max_attempts: int = 10, - on_step: IKStepCallback | None = None, ) -> IKResult: """Solve IK with multiple random restarts, returning the best collision-free solution.""" error = self._validate_world(world) @@ -181,7 +180,6 @@ def solve_pose_targets( orientation_tolerance: float = 0.01, check_collision: bool = True, max_attempts: int = 10, - on_step: IKStepCallback | None = None, ) -> IKResult: """Solve a planning-group-scoped pose target with Drake IK.""" error = self._validate_world(world) diff --git a/dimos/manipulation/planning/kinematics/jacobian_ik.py b/dimos/manipulation/planning/kinematics/jacobian_ik.py index 3671d926ca..4c4e16207a 100644 --- a/dimos/manipulation/planning/kinematics/jacobian_ik.py +++ b/dimos/manipulation/planning/kinematics/jacobian_ik.py @@ -35,7 +35,7 @@ ) from dimos.manipulation.planning.spec.enums import IKStatus from dimos.manipulation.planning.spec.models import IKResult, WorldRobotID -from dimos.manipulation.planning.spec.protocols import IKStepCallback, WorldSpec +from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.utils.kinematics_utils import ( check_singularity, compute_error_twist, @@ -108,7 +108,6 @@ def solve( orientation_tolerance: float = 0.01, check_collision: bool = True, max_attempts: int = 10, - on_step: IKStepCallback | None = None, ) -> IKResult: """Solve IK with multiple random restarts. @@ -203,7 +202,6 @@ def solve_pose_targets( orientation_tolerance: float = 0.01, check_collision: bool = True, max_attempts: int = 10, - on_step: IKStepCallback | None = None, ) -> IKResult: """Solve a planning-group pose target using group FK/Jacobian.""" if not world.is_finalized: diff --git a/dimos/manipulation/planning/kinematics/pink_ik.py b/dimos/manipulation/planning/kinematics/pink_ik.py index f1f07fa0ed..3456c2e27f 100644 --- a/dimos/manipulation/planning/kinematics/pink_ik.py +++ b/dimos/manipulation/planning/kinematics/pink_ik.py @@ -36,7 +36,7 @@ from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus from dimos.manipulation.planning.spec.models import IKResult, RobotName, WorldRobotID -from dimos.manipulation.planning.spec.protocols import IKStepCallback, WorldSpec +from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.utils.kinematics_utils import compute_pose_error from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -64,11 +64,6 @@ class _PinkModules: _MANIPULATION_EXTRA_HINT = "Install manipulation dependencies with: uv sync --extra manipulation." -_INTERACTIVE_STEP_STRIDE = 12 - - -class _IKSolveAbortedError(Exception): - """Stop a superseded interactive solve without treating it as an IK failure.""" @dataclass(frozen=True) @@ -123,7 +118,6 @@ def solve( orientation_tolerance: float = 0.01, check_collision: bool = True, max_attempts: int = 10, - on_step: IKStepCallback | None = None, ) -> IKResult: """Solve IK with Pink, returning the standard planning ``IKResult``.""" if not world.is_finalized: @@ -161,11 +155,7 @@ def solve( upper_limits=upper_limits, position_tolerance=position_tolerance, orientation_tolerance=orientation_tolerance, - attempt=attempt, - on_step=on_step, ) - except _IKSolveAbortedError: - return _failure(IKStatus.NO_SOLUTION, "Pink IK superseded by a newer target") except ValueError as exc: return _failure(IKStatus.NO_SOLUTION, f"Pink IK mapping failed: {exc}") except Exception as exc: @@ -199,7 +189,6 @@ def solve_pose_targets( orientation_tolerance: float = 0.01, check_collision: bool = True, max_attempts: int = 10, - on_step: IKStepCallback | None = None, ) -> IKResult: """Solve planning-group-scoped pose targets with Pink IK.""" if not world.is_finalized: @@ -267,7 +256,6 @@ def solve_pose_targets( return _failure(IKStatus.NO_SOLUTION, f"Pink IK model setup failed: {exc}") fallback_result: IKResult | None = None - progress_callback = self._group_progress_callback(groups, on_step) for attempt in range(max_attempts): current_positions = seed_positions.copy() if attempt > 0: @@ -286,8 +274,6 @@ def solve_pose_targets( position_tolerance=position_tolerance, orientation_tolerance=orientation_tolerance, locked_joint_positions=locked_positions, - attempt=attempt, - on_step=progress_callback, ) else: result = self._solve_multi( @@ -298,11 +284,7 @@ def solve_pose_targets( position_tolerance=position_tolerance, orientation_tolerance=orientation_tolerance, locked_joint_positions=locked_positions, - attempt=attempt, - on_step=progress_callback, ) - except _IKSolveAbortedError: - return _failure(IKStatus.NO_SOLUTION, "Pink IK superseded by a newer target") except ValueError as exc: return _failure(IKStatus.NO_SOLUTION, f"Pink IK mapping failed: {exc}") except Exception as exc: @@ -377,40 +359,6 @@ def solve_pose_targets( return _collision_failure(combined) return combined - @staticmethod - def _group_progress_callback( - groups: Sequence[PlanningGroup], on_step: IKStepCallback | None - ) -> IKStepCallback | None: - if on_step is None: - return None - - def report( - local_state: JointState, - position_error: float, - orientation_error: float, - attempt: int, - ) -> bool: - values = dict(zip(local_state.name, local_state.position, strict=True)) - names: list[str] = [] - positions: list[float] = [] - for group in groups: - for global_name, local_name in zip( - group.joint_names, group.local_joint_names, strict=True - ): - value = values.get(local_name, values.get(global_name)) - if value is None: - continue - names.append(global_name) - positions.append(float(value)) - return on_step( - JointState({"name": names, "position": positions}), - position_error, - orientation_error, - attempt, - ) - - return report - def _solve_multi( self, targets: Sequence[tuple[_PinkRobotContext, NDArray[np.float64]]], @@ -420,8 +368,6 @@ def _solve_multi( position_tolerance: float, orientation_tolerance: float, locked_joint_positions: Mapping[int, float] | None = None, - attempt: int = 0, - on_step: IKStepCallback | None = None, ) -> IKResult: robot_context = targets[0][0] pink = self._modules.pink @@ -451,17 +397,6 @@ def _solve_multi( ] final_position_error = max(error[0] for error in errors) final_orientation_error = max(error[1] for error in errors) - if on_step is not None and iteration % _INTERACTIVE_STEP_STRIDE == 0: - progress = JointState( - { - "name": robot_context.mapping.dimos_joint_names, - "position": self._q_to_dimos_positions( - robot_context, configuration.q - ).tolist(), - } - ) - if on_step(progress, final_position_error, final_orientation_error, attempt): - raise _IKSolveAbortedError if ( final_position_error <= position_tolerance and final_orientation_error <= orientation_tolerance @@ -513,8 +448,6 @@ def _solve_single( position_tolerance: float, orientation_tolerance: float, locked_joint_positions: Mapping[int, float] | None = None, - attempt: int = 0, - on_step: IKStepCallback | None = None, ) -> IKResult: pink = self._modules.pink pinocchio = self._modules.pinocchio @@ -545,17 +478,6 @@ def _solve_single( final_position_error, final_orientation_error = compute_pose_error( current_pose, target_model ) - if on_step is not None and iteration % _INTERACTIVE_STEP_STRIDE == 0: - progress = JointState( - { - "name": robot_context.mapping.dimos_joint_names, - "position": self._q_to_dimos_positions( - robot_context, configuration.q - ).tolist(), - } - ) - if on_step(progress, final_position_error, final_orientation_error, attempt): - raise _IKSolveAbortedError if ( final_position_error <= position_tolerance and final_orientation_error <= orientation_tolerance diff --git a/dimos/manipulation/planning/kinematics/test_pink_ik.py b/dimos/manipulation/planning/kinematics/test_pink_ik.py index 5b58dc7cae..d0324edb56 100644 --- a/dimos/manipulation/planning/kinematics/test_pink_ik.py +++ b/dimos/manipulation/planning/kinematics/test_pink_ik.py @@ -582,36 +582,6 @@ def test_solve_pose_targets_uses_group_tip_and_filters_group_joints( assert world.joint_state_calls == 0 -def test_solve_pose_targets_aborts_superseded_interactive_search( - mocker: MockerFixture, -) -> None: - ik = _pink_ik(mocker, converge=False) - ik._robot_contexts = {("robot", "tool"): _context()} - world = _FakeWorld() - progress: list[JointState] = [] - - def abort(joints: JointState, _position: float, _orientation: float, _attempt: int) -> bool: - progress.append(joints) - return True - - result = ik.solve_pose_targets( - world=cast("Any", world), - pose_targets={ - world.groups["arm/manipulator"]: PoseStamped( - position=Vector3(0.1, 0.0, 0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - ) - }, - seed=JointState({"name": ["arm/joint_a", "arm/joint_b"], "position": [0.0, 0.0]}), - on_step=abort, - ) - - assert result.status == IKStatus.NO_SOLUTION - assert result.message == "Pink IK superseded by a newer target" - assert len(progress) == 1 - assert progress[0].name == ["arm/joint_a", "arm/joint_b"] - - def test_solve_pose_targets_rejects_group_without_tip(mocker: MockerFixture) -> None: ik = _pink_ik(mocker) world = _FakeWorld() diff --git a/dimos/manipulation/planning/spec/protocols.py b/dimos/manipulation/planning/spec/protocols.py index d01390155b..ff33953025 100644 --- a/dimos/manipulation/planning/spec/protocols.py +++ b/dimos/manipulation/planning/spec/protocols.py @@ -20,8 +20,8 @@ from __future__ import annotations -from collections.abc import Callable, Mapping, Sequence -from typing import TYPE_CHECKING, Any, Protocol, TypeAlias, runtime_checkable +from collections.abc import Mapping, Sequence +from typing import TYPE_CHECKING, Any, Protocol, runtime_checkable if TYPE_CHECKING: from contextlib import AbstractContextManager @@ -47,10 +47,6 @@ from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -IKStepCallback: TypeAlias = Callable[["JointState", float, float, int], bool] -"""Interactive IK progress hook: joints, position error, orientation error, attempt.""" - - @runtime_checkable class WorldSpec(Protocol): """Protocol for the world/scene backend. @@ -265,9 +261,8 @@ def solve( orientation_tolerance: float = 0.01, check_collision: bool = True, max_attempts: int = 10, - on_step: IKStepCallback | None = None, ) -> IKResult: - """Solve IK with optional collision checking and interactive progress.""" + """Solve IK with optional collision checking.""" ... def solve_pose_targets( @@ -280,9 +275,8 @@ def solve_pose_targets( orientation_tolerance: float = 0.01, check_collision: bool = True, max_attempts: int = 10, - on_step: IKStepCallback | None = None, ) -> IKResult: - """Solve planning-group-scoped pose targets with optional interactive progress.""" + """Solve planning-group-scoped pose targets.""" ... diff --git a/dimos/manipulation/planning/world/roboplan_world.py b/dimos/manipulation/planning/world/roboplan_world.py index 74f9315666..3f1580c7af 100644 --- a/dimos/manipulation/planning/world/roboplan_world.py +++ b/dimos/manipulation/planning/world/roboplan_world.py @@ -174,7 +174,6 @@ def add_obstacle(self, obstacle: Obstacle) -> str | None: return None snapshot = deepcopy(obstacle) self._add_obstacle_to_scene(snapshot, obstacle_id) - self._disable_obstacle_pair_collisions(obstacle_id) self._obstacles[obstacle_id] = snapshot return obstacle_id @@ -201,7 +200,6 @@ def update_obstacle(self, obstacle: Obstacle) -> bool: try: scene.removeGeometry(obstacle_id) self._add_obstacle_to_scene(snapshot, obstacle_id) - self._disable_obstacle_pair_collisions(obstacle_id) except Exception: self._usable = False raise @@ -1086,13 +1084,6 @@ def _add_obstacle_to_scene(self, obstacle: Obstacle, obstacle_id: str) -> None: return raise ValueError(f"Unsupported obstacle type: {obstacle.obstacle_type}") - def _disable_obstacle_pair_collisions(self, obstacle_id: str) -> None: - """Ignore environment contacts that cannot change with robot configuration.""" - scene = self._require_scene() - for other_id in self._obstacles: - if other_id != obstacle_id: - scene.setCollisions(obstacle_id, other_id, False) - def _validate_obstacle(self, obstacle: Obstacle, *, allow_empty_name: bool = False) -> None: validate_obstacle( obstacle, pose_to_matrix(obstacle.pose), allow_empty_name=allow_empty_name diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 739867fa95..8c886aeafa 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -338,25 +338,6 @@ def test_reset_not_during_execution(self, module_factory): assert not result.is_success() assert result.error_code == "INVALID_STATE" - def test_reset_crosses_optional_episode_boundary_and_invalidates_plan( - self, module_factory - ) -> None: - module = module_factory() - episode_control = MagicMock() - episode_control.reset_episode.return_value = True - module._episode_control = episode_control - module._last_plan = GeneratedPlan( - trajectory=JointTrajectory(), - group_ids=("arm/manipulator",), - path=[], - ) - - result = module.reset() - - assert result.is_success() - episode_control.reset_episode.assert_called_once_with() - assert module._last_plan is None - def test_fail_sets_fault_state(self, module_factory): """_fail helper sets FAULT state and message.""" module = module_factory() @@ -611,44 +592,6 @@ def test_solve_ik_rpc_accepts_explicit_seed_without_current_state( assert kwargs["seed"] is explicit_seed module._world_monitor.current_global_joint_state.assert_not_called() - def test_interactive_ik_is_bounded_and_keeps_strict_rpc_defaults_separate( - self, robot_config, module_factory - ): - module = module_factory() - module._world_monitor = MagicMock() - module._world_monitor.world = MagicMock() - module._world_monitor.planning_groups = PlanningGroupRegistry([robot_config]) - module._kinematics = MagicMock() - expected = IKResult(status=IKStatus.NO_SOLUTION) - module._kinematics.solve_pose_targets.return_value = expected - seed = JointState( - name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], - position=[0.0, 0.0, 0.0], - ) - - def on_step( - _joints: JointState, _position: float, _orientation: float, _attempt: int - ) -> bool: - return False - - result = module.inverse_kinematics_interactive( - { - "test_arm/manipulator": PoseStamped( - frame_id="world", position=Vector3(), orientation=Quaternion() - ) - }, - seed=seed, - on_step=on_step, - ) - - assert result is expected - _, kwargs = module._kinematics.solve_pose_targets.call_args - assert kwargs["seed"] is seed - assert kwargs["position_tolerance"] == 0.02 - assert kwargs["orientation_tolerance"] == pytest.approx(3.141592653589793) - assert kwargs["max_attempts"] == 1 - assert kwargs["on_step"] is on_step - class TestPlanningGroupApis: """Test explicit planning-group API behavior.""" diff --git a/dimos/manipulation/test_pick_and_place_unit.py b/dimos/manipulation/test_pick_and_place_unit.py index 19d60e6843..6ee984f0e2 100644 --- a/dimos/manipulation/test_pick_and_place_unit.py +++ b/dimos/manipulation/test_pick_and_place_unit.py @@ -16,18 +16,14 @@ from __future__ import annotations -from types import SimpleNamespace -from unittest.mock import MagicMock, call, patch +from unittest.mock import patch import open3d as o3d import pytest -from dimos.agents.skill_result import SkillResult from dimos.core.module import ModuleBase from dimos.manipulation.pick_and_place_module import PickAndPlaceModule -from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 @@ -151,17 +147,6 @@ def test_grasp_orientation_far_differs_from_near(self): and abs(q_near.w - q_far.w) < 0.01 ) - def test_exact_simulation_detection_does_not_apply_occlusion_offset(self, module): - det = _make_det_object(name="can", center=(0.5, 0.0, 0.19)) - det.identity_basis = "pimsim_scene" - module._detection_snapshot = [det] - - grasps = module._generate_grasps_for_pick("can") - - assert grasps is not None - assert grasps[0].position.x == pytest.approx(0.5) - assert grasps[0].position.y == pytest.approx(0.0) - class TestPlaceBack: """Test place_back guard logic.""" @@ -173,62 +158,3 @@ def test_place_back_no_pick_pose_errors(self, module): assert not result.is_success() assert result.error_code == "NO_PRIOR_POSE" assert "pick" in result.message.lower() - - -class TestPickPlaceObstacleLifecycle: - def _prepare_motion(self, module: PickAndPlaceModule) -> None: - module._get_robot = MagicMock( - return_value=("arm", "robot_1", SimpleNamespace(pre_grasp_offset=0.1), None) - ) - module._lift_if_low = MagicMock(return_value=SkillResult.ok()) - module._preview_execute_wait = MagicMock(return_value=SkillResult.ok()) - module._set_gripper_position = MagicMock(return_value=True) - module._world_monitor = MagicMock() - module._world_monitor.remove_object_obstacle.return_value = True - - def test_pick_removes_target_for_contact_and_holds_gripper_state(self, module): - self._prepare_motion(module) - target = _make_det_object(name="can", object_id="pimsim:manip_can") - module._detection_snapshot = [target] - grasp = Pose(Vector3(0.5, 0.0, 0.25), Quaternion()) - module._generate_grasps_for_pick = MagicMock(return_value=[grasp]) - module.plan_to_pose = MagicMock(return_value=True) - - with patch("dimos.manipulation.pick_and_place_module.time.sleep"): - result = module.pick("can") - - assert result.is_success() - module._world_monitor.remove_object_obstacle.assert_called_once_with("pimsim:manip_can") - assert module._set_gripper_position.call_args_list == [call(0.85, "arm"), call(0.0, "arm")] - assert module._held_object_id == "pimsim:manip_can" - - def test_pick_restores_target_when_contact_plan_fails(self, module): - self._prepare_motion(module) - target = _make_det_object(name="can", object_id="pimsim:manip_can") - module._detection_snapshot = [target] - module._generate_grasps_for_pick = MagicMock( - return_value=[Pose(Vector3(0.5, 0.0, 0.25), Quaternion())] - ) - module.plan_to_pose = MagicMock(side_effect=[True, False]) - module.refresh_obstacles = MagicMock(return_value=[]) - - with patch("dimos.manipulation.pick_and_place_module.time.sleep"): - result = module.pick("can") - - assert not result.is_success() - module.refresh_obstacles.assert_called_once_with() - assert module._held_object_id is None - - def test_place_releases_then_restores_perception_obstacles(self, module): - self._prepare_motion(module) - module._held_object_id = "pimsim:manip_can" - module.plan_to_pose = MagicMock(return_value=True) - module.refresh_obstacles = MagicMock(return_value=[]) - - with patch("dimos.manipulation.pick_and_place_module.time.sleep"): - result = module.place(0.45, 0.1, 0.19) - - assert result.is_success() - module._set_gripper_position.assert_called_once_with(0.85, "arm") - module.refresh_obstacles.assert_called_once_with() - assert module._held_object_id is None diff --git a/dimos/manipulation/test_roboplan_world.py b/dimos/manipulation/test_roboplan_world.py index c7f354b615..57e437041f 100644 --- a/dimos/manipulation/test_roboplan_world.py +++ b/dimos/manipulation/test_roboplan_world.py @@ -686,28 +686,6 @@ def test_obstacle_mutation_updates_scene_and_stored_pose( assert world.get_obstacles() == [] -def test_environment_obstacles_do_not_collide_with_each_other( - fake_roboplan: None, robot_config: RobotModelConfig -) -> None: - world, _ = _make_world(fake_roboplan, robot_config) - tabletop = Obstacle( - name="tabletop", - obstacle_type=ObstacleType.BOX, - pose=PoseStamped(position=Vector3(0.45, 0.0, 0.12)), - dimensions=(0.3, 0.4, 0.02), - ) - table_leg = Obstacle( - name="table_leg", - obstacle_type=ObstacleType.CYLINDER, - pose=PoseStamped(position=Vector3(0.32, -0.18, 0.055)), - dimensions=(0.02, 0.11), - ) - - assert world.add_obstacle(tabletop) == "tabletop" - assert world.add_obstacle(table_leg) == "table_leg" - assert world._scene.collision_settings[("table_leg", "tabletop")] is False - - def test_obstacle_operations_require_finalization( fake_roboplan: None, robot_config: RobotModelConfig, diff --git a/dimos/manipulation/visualization/operator.py b/dimos/manipulation/visualization/operator.py index 2119947ef2..ac0ef65609 100644 --- a/dimos/manipulation/visualization/operator.py +++ b/dimos/manipulation/visualization/operator.py @@ -24,7 +24,6 @@ from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.manipulation.planning.planners.config import CartesianPathConfig from dimos.manipulation.planning.spec.models import GeneratedPlan, PlanningGroupID, RobotName -from dimos.manipulation.planning.spec.protocols import IKStepCallback from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState @@ -113,20 +112,16 @@ def evaluate_joint_target(self, request: JointTargetRequest) -> TargetEvaluation return self._invalid(request.group_ids, "Incomplete robot target state") return self._evaluate_global_target(groups, JointState(request.target), complete) - def evaluate_pose_target( - self, - request: PoseTargetRequest, - on_step: IKStepCallback | None = None, - ) -> TargetEvaluationResult: + def evaluate_pose_target(self, request: PoseTargetRequest) -> TargetEvaluationResult: """Validate and evaluate explicit world-frame pose targets.""" group_ids, validation = self._validate_pose_request(request) if validation is not None: return validation - ik = self._module.inverse_kinematics_interactive( + ik = self._module.inverse_kinematics( pose_targets=dict(request.pose_targets), auxiliary_group_ids=request.auxiliary_group_ids, seed=JointState(request.seed) if request.seed is not None else None, - on_step=on_step, + check_collision=True, ) if not ik.is_success() or ik.joint_state is None: return TargetEvaluationResult( @@ -212,15 +207,6 @@ def reset(self) -> bool: result = self._module.reset() return result.is_success() - def go_home(self, robot_name: RobotName | None = None) -> bool: - return self._module.go_home(robot_name).is_success() - - def open_gripper(self, robot_name: RobotName | None = None) -> bool: - return self._module.open_gripper(robot_name).is_success() - - def close_gripper(self, robot_name: RobotName | None = None) -> bool: - return self._module.close_gripper(robot_name).is_success() - def _validate_joint_request( self, request: JointTargetRequest ) -> tuple[tuple[PlanningGroup, ...] | None, TargetEvaluationResult | None]: diff --git a/dimos/manipulation/visualization/test_operator.py b/dimos/manipulation/visualization/test_operator.py index fb9fa706b7..a302fe9642 100644 --- a/dimos/manipulation/visualization/test_operator.py +++ b/dimos/manipulation/visualization/test_operator.py @@ -28,7 +28,6 @@ PlanningGroupID, RobotName, ) -from dimos.manipulation.planning.spec.protocols import IKStepCallback from dimos.manipulation.visualization.operator import ( CartesianTargetRequest, JointTargetRequest, @@ -109,7 +108,6 @@ def __init__(self) -> None: dict[PlanningGroupID, PoseStamped], tuple[PlanningGroupID, ...], JointState | None ] ] = [] - self.ik_progress_callbacks: list[IKStepCallback | None] = [] self.plan_success = True self.preview_success = True self.execute_success = True @@ -154,21 +152,6 @@ def inverse_kinematics( message="ok", ) - def inverse_kinematics_interactive( - self, - pose_targets: dict[PlanningGroupID, PoseStamped], - auxiliary_group_ids: tuple[PlanningGroupID, ...] = (), - seed: JointState | None = None, - on_step: IKStepCallback | None = None, - ) -> IKResult: - self.ik_progress_callbacks.append(on_step) - return self.inverse_kinematics( - pose_targets, - auxiliary_group_ids=auxiliary_group_ids, - seed=seed, - check_collision=True, - ) - def plan_to_joint_targets(self, targets: dict[PlanningGroupID, JointState]) -> bool: self.plan_joint_targets.append(targets) return self.plan_success @@ -353,16 +336,12 @@ def test_pose_evaluation_accepts_world_frame_and_delegates_original_request() -> seed = JointState(name=["arm/j0", "arm/j1"], position=[0.0, 0.0]) request = PoseTargetRequest({"arm/manipulator": pose}, seed=seed) - def on_step(_joints: JointState, _position: float, _orientation: float, _attempt: int) -> bool: - return False - - result = operator.evaluate_pose_target(request, on_step=on_step) + result = operator.evaluate_pose_target(request) assert result.success is True assert result.target_joints is not None assert list(result.target_joints.name) == ["arm/j0", "arm/j1"] assert module.ik_calls == [({"arm/manipulator": pose}, (), seed)] - assert module.ik_progress_callbacks == [on_step] def test_pose_validation_rejects_frame_capability_and_seed_errors() -> None: diff --git a/dimos/manipulation/visualization/viser/gui.py b/dimos/manipulation/visualization/viser/gui.py index 962013333c..02145d9fc5 100644 --- a/dimos/manipulation/visualization/viser/gui.py +++ b/dimos/manipulation/visualization/viser/gui.py @@ -14,14 +14,13 @@ from __future__ import annotations -from collections.abc import Callable, Mapping, MutableMapping, Sequence +from collections.abc import Mapping, MutableMapping, Sequence from typing import TypeAlias, cast from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.manipulation.planning.planners.config import RoboPlanCartesianPathConfig from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import PlanningGroupID, PlanningSceneInfo, RobotName -from dimos.manipulation.planning.spec.protocols import IKStepCallback from dimos.manipulation.visualization.operator import ( CartesianTargetRequest, JointTargetRequest, @@ -256,15 +255,6 @@ def get_error(self) -> str: def reset(self) -> bool: return self.operator.reset() - def go_home(self) -> bool: - return self.operator.go_home(self.state.selected_robot) - - def open_gripper(self) -> bool: - return self.operator.open_gripper(self.state.selected_robot) - - def close_gripper(self) -> bool: - return self.operator.close_gripper(self.state.selected_robot) - def evaluate_joint_target_set( self, group_ids: Sequence[PlanningGroupID], targets: Mapping[PlanningGroupID, JointState] ) -> TargetEvaluationResult: @@ -287,7 +277,6 @@ def evaluate_pose_target_set( pose_targets: Mapping[PlanningGroupID, Pose], auxiliary_group_ids: Sequence[PlanningGroupID] = (), seed: JointState | None = None, - on_step: IKStepCallback | None = None, ) -> TargetEvaluationResult: stamped = { group_id: PoseStamped( @@ -296,8 +285,7 @@ def evaluate_pose_target_set( for group_id, pose in pose_targets.items() } return self.operator.evaluate_pose_target( - PoseTargetRequest(stamped, tuple(auxiliary_group_ids), _copy_joint_state(seed)), - on_step=on_step, + PoseTargetRequest(stamped, tuple(auxiliary_group_ids), _copy_joint_state(seed)) ) def cancel(self) -> bool: @@ -396,7 +384,6 @@ def _build_panel_controls(self, gui: GuiApi) -> None: "### Planning Groups\nActive planning groups for pose goals, planning, and joint edits." ) self._sync_group_selector(self.list_planning_groups()) - self._build_operator_controls(gui) self._handles["target_heading"] = gui.add_markdown("### Target") preset_dropdown = gui.add_dropdown( "Preset", @@ -434,28 +421,6 @@ def _build_panel_controls(self, gui: GuiApi) -> None: self._handles["joint_control_folder"] = joint_controls self._build_joint_sliders() - def _build_operator_controls(self, gui: GuiApi) -> None: - self._handles["operator_heading"] = gui.add_markdown("### Robot Controls") - actions: tuple[tuple[str, str, Callable[[], bool]], ...] = ( - ("reset", "Reset", self.reset), - ("go_home", "Go Home", self.go_home), - ("open_gripper", "Open Gripper", self.open_gripper), - ("close_gripper", "Close Gripper", self.close_gripper), - ) - for key, label, action in actions: - button = gui.add_button(label) - - def on_click( - _event: object, - action_key: str = key, - action_label: str = label, - callback: Callable[[], bool] = action, - ) -> None: - self._submit_operator_action(action_key, action_label, callback) - - button.on_click(on_click) - self._handles[key] = button - def _sync_group_selector(self, groups: list[PlanningGroup]) -> None: """Render source-order group toggle buttons without a robot dropdown.""" selected = set(self.state.selected_group_ids) @@ -821,13 +786,11 @@ def _preset_values_by_local_name(self, preset: str, robot_name: str) -> dict[str return self._local_values_for_robot(robot_name, state) def _remove_panel_handles(self) -> None: - for key, handle in reversed(list(self._handles.items())): - self._handles.pop(key, None) - if key.startswith("ee_control:"): - continue + for key, handle in list(self._handles.items()): remove = getattr(handle, "remove", None) if callable(remove): remove() + self._handles.pop(key, None) def _sync_preset_dropdown(self) -> None: handle = self._handles.get("preset") @@ -959,8 +922,7 @@ def _on_transform_update( pose_targets=dict(self._active_pose_targets()), ) ) - # A drag can emit tens of updates per second from the Viser client - # thread. The coalescing worker refreshes once for the accepted result. + self.refresh() def _submit_joint_target_evaluation(self) -> None: targets = self._target_set_from_sliders() @@ -1041,32 +1003,13 @@ def _sync_target_ghost_visibility(self) -> None: self.scene.set_target_active(str(robot_id), str(robot_id) in active_robot_ids) def _handle_target_evaluation_request( - self, - request: TargetEvaluationRequest, - is_stale: Callable[[], bool], + self, request: TargetEvaluationRequest ) -> TargetEvaluationResult: if request.source == "cartesian": if not request.pose_targets: return TargetEvaluationResult(False, "INVALID", "No pose target") - - def on_step( - joints: JointState, - _position_error: float, - _orientation_error: float, - _attempt: int, - ) -> bool: - if is_stale(): - return True - targets = self._group_targets_from_joint_state(request.group_ids, joints) - if targets: - self._move_joint_target_visuals(targets) - return False - return self.evaluate_pose_target_set( - request.pose_targets, - request.auxiliary_group_ids, - request.joints, - on_step=on_step, + request.pose_targets, request.auxiliary_group_ids, request.joints ) if not request.joint_targets: return TargetEvaluationResult(False, "INVALID", "No joint target") @@ -1116,41 +1059,22 @@ def _sync_controls_from_targets(self) -> None: self._move_joint_target_visuals(self.state.group_joint_targets) def _split_target_joints_by_group(self, target_joints: JointState) -> None: - self.state.group_joint_targets.update( - self._group_targets_from_joint_state(self.state.selected_group_ids, target_joints) - ) - - def _group_targets_from_joint_state( - self, - group_ids: Sequence[PlanningGroupID], - target_joints: JointState, - ) -> dict[PlanningGroupID, JointState]: if len(target_joints.name) != len(target_joints.position): - return {} + return positions = { str(name): float(value) for name, value in zip(target_joints.name, target_joints.position, strict=True) } - targets: dict[PlanningGroupID, JointState] = {} - for group_id in group_ids: + for group_id in self.state.selected_group_ids: group = self._groups_by_id().get(group_id) - if group is None: + if group is None or any(str(name) not in positions for name in group.joint_names): continue - values = [ - positions.get(str(global_name), positions.get(str(local_name))) - for global_name, local_name in zip( - group.joint_names, group.local_joint_names, strict=True - ) - ] - if any(value is None for value in values): - continue - targets[group_id] = JointState( + self.state.group_joint_targets[group_id] = JointState( { "name": list(group.joint_names), - "position": [float(value) for value in values if value is not None], + "position": [positions[str(name)] for name in group.joint_names], } ) - return targets def _sync_pose_targets_from_group_poses(self) -> None: groups = self._groups_by_id() @@ -1195,11 +1119,6 @@ def _update_status_text(self) -> None: ) def _update_control_state(self) -> None: - operator_busy = self.state.action_status != ActionStatus.IDLE or ( - self.state.manipulation_state in {"PLANNING", "EXECUTING"} - ) - for key in ("reset", "go_home", "open_gripper", "close_gripper"): - self._set_disabled(key, operator_busy) self._set_disabled("plan", not self.state.can_plan()) self._set_disabled("preview", not self.state.can_preview()) self._set_disabled( @@ -1345,45 +1264,6 @@ def operation() -> None: operation, on_error=lambda message: self._set_operation_error(message, operation_id) ) - def _submit_operator_action( - self, - key: str, - label: str, - action: Callable[[], bool], - ) -> None: - if self._closed: - return - if self.state.action_status != ActionStatus.IDLE or self.state.manipulation_state in { - "PLANNING", - "EXECUTING", - }: - self._set_recoverable_error(f"Cannot {label.lower()} while manipulation is busy") - return - operation_id = self._next_operation_id() - self.state.action_status = ActionStatus.RUNNING - self.refresh() - - def operation() -> None: - if not self._operation_is_current(operation_id): - return - ok = action() - if not self._operation_is_current(operation_id): - return - if key == "reset" and ok: - self.state.plan_state = PanelPlanState() - if not ok: - self.state.error = self.get_error() or f"{label} failed" - self._finish_operation( - f"{key}={ok}", - clear_error=ok, - operation_id=operation_id, - ) - - self._operation_worker.submit( - operation, - on_error=lambda message: self._set_operation_error(message, operation_id), - ) - def _set_planning_mode(self, label: str) -> None: mode = PLANNING_MODES_BY_LABEL.get(label) if self._closed or mode is None or mode == self.state.planning_mode: diff --git a/dimos/manipulation/visualization/viser/state.py b/dimos/manipulation/visualization/viser/state.py index ca12b6e339..38c0503b70 100644 --- a/dimos/manipulation/visualization/viser/state.py +++ b/dimos/manipulation/visualization/viser/state.py @@ -15,9 +15,8 @@ from __future__ import annotations from collections.abc import Callable -from dataclasses import dataclass, field, replace +from dataclasses import dataclass, field from enum import Enum -from functools import partial import queue import threading from typing import Literal @@ -229,14 +228,13 @@ class TargetEvaluationWorker: def __init__( self, - handler: Callable[[TargetEvaluationRequest, Callable[[], bool]], TargetEvaluationResult], + handler: Callable[[TargetEvaluationRequest], TargetEvaluationResult], apply_result: Callable[[TargetEvaluationRequest, TargetEvaluationResult], None], ) -> None: self._handler = handler self._apply_result = apply_result self._requests: queue.Queue[TargetEvaluationRequest] = queue.Queue(maxsize=1) self._submit_lock = threading.Lock() - self._latest_request_key: tuple[int, int, tuple[PlanningGroupID, ...]] | None = None self._stop_event = threading.Event() self._thread: threading.Thread | None = None @@ -257,7 +255,6 @@ def stop(self, timeout: float | None = 2.0) -> None: def submit(self, request: TargetEvaluationRequest) -> None: with self._submit_lock: - self._latest_request_key = self._request_key(request) while True: try: self._requests.get_nowait() @@ -266,8 +263,6 @@ def submit(self, request: TargetEvaluationRequest) -> None: self._requests.put_nowait(request) def _run(self) -> None: - warm_seed_key: tuple[int, tuple[PlanningGroupID, ...]] | None = None - warm_seed: JointState | None = None while not self._stop_event.is_set(): try: request = self._requests.get(timeout=0.1) @@ -278,30 +273,12 @@ def _run(self) -> None: request = self._requests.get_nowait() except queue.Empty: break - request_seed_key = (request.selection_epoch, request.group_ids) - if warm_seed is not None and warm_seed_key == request_seed_key: - request = replace(request, joints=JointState(warm_seed)) try: - result = self._handler(request, partial(self._request_is_stale, request)) - if result.success and result.collision_free and result.target_joints is not None: - warm_seed_key = request_seed_key - warm_seed = JointState(result.target_joints) + result = self._handler(request) self._apply_result(request, result) except Exception: logger.warning("Target evaluation worker caught unhandled exception", exc_info=True) - @staticmethod - def _request_key( - request: TargetEvaluationRequest, - ) -> tuple[int, int, tuple[PlanningGroupID, ...]]: - return request.selection_epoch, request.sequence_id, request.group_ids - - def _request_is_stale(self, request: TargetEvaluationRequest) -> bool: - if self._stop_event.is_set(): - return True - with self._submit_lock: - return self._latest_request_key != self._request_key(request) - class OperationWorker: """Single-worker operation queue for Viser panel actions.""" diff --git a/dimos/manipulation/visualization/viser/test_gui.py b/dimos/manipulation/visualization/viser/test_gui.py index 2f09355d32..9d8be1e168 100644 --- a/dimos/manipulation/visualization/viser/test_gui.py +++ b/dimos/manipulation/visualization/viser/test_gui.py @@ -310,32 +310,6 @@ def test_gui_preview_enters_previewing_before_worker_runs( assert len(submissions) == 1 -def test_gui_operator_action_uses_operation_worker(monkeypatch: pytest.MonkeyPatch) -> None: - submissions: list[Callable[[], None]] = [] - calls: list[str] = [] - gui = make_gui() - gui._operation_worker.stop() - monkeypatch.setattr(gui, "_operation_worker", FakeOperationSubmitWorker(submissions)) - monkeypatch.setattr(gui, "refresh", lambda: None) - gui.state.manipulation_state = "IDLE" - - def reset() -> bool: - calls.append("reset") - return True - - gui._submit_operator_action("reset", "Reset", reset) - - assert gui.state.action_status == ActionStatus.RUNNING - assert calls == [] - assert len(submissions) == 1 - - submissions[0]() - - assert calls == ["reset"] - assert gui.state.action_status == ActionStatus.IDLE - assert gui.state.last_result == "reset=True" - - def test_gui_selection_change_clears_invalidated_preview( monkeypatch: pytest.MonkeyPatch, ) -> None: diff --git a/dimos/manipulation/visualization/viser/test_state.py b/dimos/manipulation/visualization/viser/test_state.py index 1b86fb89a2..a9ec709b93 100644 --- a/dimos/manipulation/visualization/viser/test_state.py +++ b/dimos/manipulation/visualization/viser/test_state.py @@ -18,7 +18,6 @@ import threading from dimos.manipulation.planning.spec.models import PlanningGroupID -from dimos.manipulation.visualization.operator import TargetEvaluationResult from dimos.manipulation.visualization.viser.state import ( ActionStatus, BackendConnectionStatus, @@ -26,11 +25,9 @@ PanelRuntime, PanelState, PlanStatus, - TargetEvaluationRequest, TargetEvaluationWorker, TargetStatus, ) -from dimos.msgs.sensor_msgs.JointState import JointState def test_panel_cannot_plan_from_fault_without_explicit_reset() -> None: @@ -120,51 +117,6 @@ def operation() -> None: assert operation_errors == ["Operation timed out after 0.0s"] -def test_target_worker_supersedes_inflight_work_and_reuses_collision_free_seed() -> None: - first_started = threading.Event() - newer_submitted = threading.Event() - second_applied = threading.Event() - handled: list[TargetEvaluationRequest] = [] - stale_checks: list[bool] = [] - - def handler( - request: TargetEvaluationRequest, is_stale: Callable[[], bool] - ) -> TargetEvaluationResult: - handled.append(request) - if request.sequence_id == 1: - first_started.set() - newer_submitted.wait(timeout=1.0) - stale_checks.append(is_stale()) - return TargetEvaluationResult( - True, - "FEASIBLE", - "", - True, - target_joints=JointState(name=["arm/joint"], position=[float(request.sequence_id)]), - ) - - def apply(request: TargetEvaluationRequest, _result: TargetEvaluationResult) -> None: - if request.sequence_id == 2: - second_applied.set() - - worker = TargetEvaluationWorker(handler, apply) - worker.start() - try: - group_ids = (PlanningGroupID("arm/manipulator"),) - worker.submit(TargetEvaluationRequest(1, "cartesian", group_ids=group_ids)) - assert first_started.wait(timeout=1.0) - worker.submit(TargetEvaluationRequest(2, "cartesian", group_ids=group_ids)) - newer_submitted.set() - assert second_applied.wait(timeout=1.0) - finally: - worker.stop() - - assert stale_checks == [True] - assert len(handled) == 2 - assert handled[1].joints is not None - assert handled[1].joints.position == [1.0] - - class FakeTargetEvaluationWorker(TargetEvaluationWorker): def __init__(self, calls: list[Callable[[], None]]) -> None: self.calls = calls diff --git a/dimos/manipulation/visualization/viser/test_viser_visualization.py b/dimos/manipulation/visualization/viser/test_viser_visualization.py index 9a0b65968c..8e1428214f 100644 --- a/dimos/manipulation/visualization/viser/test_viser_visualization.py +++ b/dimos/manipulation/visualization/viser/test_viser_visualization.py @@ -495,10 +495,6 @@ def test_panel_contract_group_order_defaults_and_controls( assert [button.label for button in server.gui.buttons] == [ "arm", "arm gripper", - "Reset", - "Go Home", - "Open Gripper", - "Close Gripper", "Plan", "Preview", "Execute", @@ -879,12 +875,6 @@ def test_group_controls_use_source_labels_and_active_colors( assert group_display_name(pose) == "arm" assert group_display_name(auxiliary) == "arm gripper" assert [button.label for button in server.gui.buttons[:2]] == ["arm", "arm gripper"] - assert [button.label for button in server.gui.buttons[2:6]] == [ - "Reset", - "Go Home", - "Open Gripper", - "Close Gripper", - ] assert [button.color for button in server.gui.buttons[:2]] == [ ACTIVE_GROUP_COLOR, INACTIVE_GROUP_COLOR, @@ -964,10 +954,6 @@ def test_panel_action_controls_are_present_in_source_order( _gui, _module, server = panel([selected], states("arm")) assert [button.label for button in server.gui.buttons[1:]] == [ - "Reset", - "Go Home", - "Open Gripper", - "Close Gripper", "Plan", "Preview", "Execute", @@ -1166,12 +1152,7 @@ def test_panel_disables_plan_preview_and_execute_until_a_feasible_target( selected = group("arm", "manipulator", ("j1",), pose=True) _gui, _module, server = panel([selected], states("arm")) - buttons = {button.label: button for button in server.gui.buttons} - assert [buttons[label].disabled for label in ("Plan", "Preview", "Execute")] == [ - True, - True, - True, - ] + assert [button.disabled for button in server.gui.buttons[1:4]] == [True, True, True] def test_panel_status_reports_target_and_plan_defaults( @@ -1412,13 +1393,6 @@ def test_transform_control_callback_preserves_pose_through_gui_and_backend( submitted: list[TargetEvaluationRequest] = [] gui._worker.submit = submitted.append # type: ignore[method-assign] gui.start() - refresh_calls = 0 - - def count_refresh() -> None: - nonlocal refresh_calls - refresh_calls += 1 - - gui.refresh = count_refresh # type: ignore[method-assign] control = scene._handles[f"{selected.id}:ee_control"] control.position = (1.0, 2.0, 3.0) control.wxyz = (0.4, 0.1, 0.2, 0.3) @@ -1431,11 +1405,7 @@ def count_refresh() -> None: assert control.position == (1.0, 2.0, 3.0) assert control.wxyz == (0.4, 0.1, 0.2, 0.3) assert request.pose_targets[selected.id] == gui.state.pose_targets[selected.id] - assert refresh_calls == 0 gui.close() - assert control.removed is False - scene.close() - assert control.removed is True def test_joint_evaluation_updates_active_gizmo_from_computed_group_pose() -> None: diff --git a/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py b/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py index 651b8ac60f..a6f7011d91 100644 --- a/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py +++ b/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py @@ -14,9 +14,7 @@ from __future__ import annotations -from concurrent.futures import ThreadPoolExecutor from pathlib import Path -import threading from types import SimpleNamespace import pytest @@ -191,61 +189,6 @@ def close(self) -> None: ] -def test_visualizer_concurrent_initialization_starts_one_runtime( - monkeypatch: pytest.MonkeyPatch, -) -> None: - start_barrier = threading.Barrier(2) - start_calls = 0 - - class FakeRuntime: - url = "http://localhost:8095" - - def __init__(self, config: ViserVisualizationConfig) -> None: - self.config = config - - def start(self) -> FakeServer: - nonlocal start_calls - start_calls += 1 - try: - start_barrier.wait(timeout=0.2) - except threading.BrokenBarrierError: - pass - return FakeServer() - - def close(self) -> None: - pass - - class FakeScene: - def __init__( - self, - server: FakeServer, - viser_urdf: type[FakeViserUrdf], - ) -> None: - pass - - def register_robot(self, robot_id: str, config: RobotModelConfig) -> None: - pass - - def close(self) -> None: - pass - - monkeypatch.setattr(visualizer_module, "ViserRuntime", FakeRuntime) - monkeypatch.setattr(visualizer_module, "ViserUrdf", FakeViserUrdf) - monkeypatch.setattr(visualizer_module, "ViserManipulationScene", FakeScene) - visualizer = ViserManipulationVisualizer( - config=ViserVisualizationConfig(panel_enabled=False), - ) - session = VisualizationSession(PlanningSceneInfo(robots={})) - - with ThreadPoolExecutor(max_workers=2) as executor: - futures = [executor.submit(visualizer.initialize, session) for _ in range(2)] - for future in futures: - future.result(timeout=1.0) - - assert start_calls == 1 - visualizer.close() - - def test_visualizer_closes_partial_startup_when_gui_start_fails( monkeypatch: pytest.MonkeyPatch, ) -> None: diff --git a/dimos/manipulation/visualization/viser/visualizer.py b/dimos/manipulation/visualization/viser/visualizer.py index 00b3df3795..1b64601128 100644 --- a/dimos/manipulation/visualization/viser/visualizer.py +++ b/dimos/manipulation/visualization/viser/visualizer.py @@ -16,7 +16,6 @@ from collections.abc import Sequence from contextlib import suppress -import threading from typing import TYPE_CHECKING from dimos.manipulation.visualization.viser.animation import ( @@ -80,57 +79,55 @@ def __init__( self._robot_names_by_id: dict[str, str] = {} self._robot_ids_by_name: dict[str, str] = {} self._configs_by_name: dict[str, RobotModelConfig] = {} - self._lifecycle_lock = threading.RLock() self._closed = False def _ensure_started(self) -> None: - with self._lifecycle_lock: - if self._closed or self._runtime is not None: - return - runtime = ViserRuntime(self.config) - scene: ViserManipulationScene | None = None - gui: ViserPanelGui | None = None - try: - server = runtime.start() - apply_dimos_theme(server) - scene = ViserManipulationScene(server, ViserUrdf) - gui = ( - ViserPanelGui( - server, - self._session_scene, - self._operator, - self._current_states, - self.config, - scene, - ) - if self.config.panel_enabled - and self._session_scene is not None - and self._operator is not None - else None + if self._closed or self._runtime is not None: + return + runtime = ViserRuntime(self.config) + scene: ViserManipulationScene | None = None + gui: ViserPanelGui | None = None + try: + server = runtime.start() + apply_dimos_theme(server) + scene = ViserManipulationScene(server, ViserUrdf) + gui = ( + ViserPanelGui( + server, + self._session_scene, + self._operator, + self._current_states, + self.config, + scene, ) - if gui is not None: - gui.start() - except Exception: - if gui is not None: - with suppress(Exception): - gui.close() - if scene is not None: - with suppress(Exception): - scene.close() + if self.config.panel_enabled + and self._session_scene is not None + and self._operator is not None + else None + ) + if gui is not None: + gui.start() + except Exception: + if gui is not None: + with suppress(Exception): + gui.close() + if scene is not None: with suppress(Exception): - runtime.close() - self._runtime = None - self._server = None - self._scene = None - self._gui = None - self._closed = True - raise - self._runtime = runtime - self._server = server - self._scene = scene - self._gui = gui - self._closed = False - logger.info(f"Viser manipulation visualization: {self.get_visualization_url()}") + scene.close() + with suppress(Exception): + runtime.close() + self._runtime = None + self._server = None + self._scene = None + self._gui = None + self._closed = True + raise + self._runtime = runtime + self._server = server + self._scene = scene + self._gui = gui + self._closed = False + logger.info(f"Viser manipulation visualization: {self.get_visualization_url()}") def initialize(self, session: VisualizationSession) -> None: """Initialize Viser robot visuals from a one-shot visualization session.""" @@ -320,31 +317,30 @@ def _baseline_values( return values if all(name in values for name in config.joint_names) else None def close(self) -> None: - with self._lifecycle_lock: - if self._closed: - return - self._closed = True - errors: list[BaseException] = [] - try: - if self._gui is not None: - try: - self._gui.close() - except Exception as e: - errors.append(e) - if self._scene is not None: - try: - self._scene.close() - except Exception as e: - errors.append(e) - finally: - if self._runtime is not None: - try: - self._runtime.close() - except Exception as e: - errors.append(e) - self._runtime = None - self._server = None - self._scene = None - self._gui = None - if errors: - raise errors[0] + if self._closed: + return + self._closed = True + errors: list[BaseException] = [] + try: + if self._gui is not None: + try: + self._gui.close() + except Exception as e: + errors.append(e) + if self._scene is not None: + try: + self._scene.close() + except Exception as e: + errors.append(e) + finally: + if self._runtime is not None: + try: + self._runtime.close() + except Exception as e: + errors.append(e) + self._runtime = None + self._server = None + self._scene = None + self._gui = None + if errors: + raise errors[0] diff --git a/dimos/perception/experimental/image_embedding.py b/dimos/perception/experimental/image_embedding.py index 854076104d..78cd803d81 100644 --- a/dimos/perception/experimental/image_embedding.py +++ b/dimos/perception/experimental/image_embedding.py @@ -80,13 +80,13 @@ def _initialize_model(self): # type: ignore[no-untyped-def] providers = ["CUDAExecutionProvider", "CPUExecutionProvider"] if sys.platform == "darwin": - # CoreML's Metal compiler connection is not reliable in forkserver workers. - providers = ["CPUExecutionProvider"] + # 2025-11-17 12:36:47.877215 [W:onnxruntime:, helper.cc:82 IsInputSupported] CoreML does not support input dim > 16384. Input:text_model.embeddings.token_embedding.weight, shape: {49408,512} + # 2025-11-17 12:36:47.878496 [W:onnxruntime:, coreml_execution_provider.cc:107 GetCapability] CoreMLExecutionProvider::GetCapability, number of partitions supported by CoreML: 88 number of nodes in the graph: 1504 number of nodes supported by CoreML: 933 + providers = ["CoreMLExecutionProvider"] + [ + each for each in providers if each != "CUDAExecutionProvider" + ] - self.model = ort.InferenceSession( - str(model_id), - providers=providers, - ) + self.model = ort.InferenceSession(str(model_id), providers=providers) actual_providers = self.model.get_providers() # type: ignore[attr-defined] self.processor = CLIPProcessor.from_pretrained(processor_id) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index b00bbd88ec..c67b10b549 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -257,7 +257,6 @@ "phone-teleop-module": "dimos.teleop.phone.phone_teleop_module.PhoneTeleopModule", "pick-and-place-module": "dimos.manipulation.pick_and_place_module.PickAndPlaceModule", "point-lio": "dimos.hardware.sensors.lidar.pointlio.module.PointLio", - "point-lio-zenoh-relay": "dimos.hardware.sensors.lidar.pointlio.zenoh_relay.PointLioZenohRelay", "pointlio-recorder": "dimos.hardware.sensors.lidar.pointlio.recorder.PointlioRecorder", "quest-teleop-module": "dimos.teleop.quest.quest_teleop_module.QuestTeleopModule", "ray-tracing-voxel-map": "dimos.mapping.ray_tracing.module.RayTracingVoxelMap", diff --git a/dimos/robot/manipulators/xarm/blueprints/simulation.py b/dimos/robot/manipulators/xarm/blueprints/simulation.py index f1ca4ddc5e..fb6e21f09a 100644 --- a/dimos/robot/manipulators/xarm/blueprints/simulation.py +++ b/dimos/robot/manipulators/xarm/blueprints/simulation.py @@ -17,60 +17,31 @@ from __future__ import annotations from dimos.core.coordination.blueprints import autoconnect -from dimos.core.global_config import global_config from dimos.manipulation.pick_and_place_module import PickAndPlaceModule +from dimos.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule from dimos.robot.manipulators.common.blueprints import coordinator, trajectory_task from dimos.robot.manipulators.xarm.config import ( - XARM7_MODEL_PATH, - XARM7_TABLETOP_SCENE, + XARM7_SIM_PATH, make_xarm7_sim_hardware, + make_xarm7_sim_module_kwargs, make_xarm7_sim_robot_config, ) -from dimos.simulation.providers import ( - SimulationBinding, - SimulationRequest, - load_simulation_provider, -) -from dimos.visualization.vis_module import vis_module - - -def _resolve_xarm7_simulation() -> SimulationBinding: - binding = load_simulation_provider("pimsim").build( - SimulationRequest( - robot_model="xarm7", - model_path=XARM7_MODEL_PATH, - scene_package=XARM7_TABLETOP_SCENE, - ) - ) - if binding.adapter_type != "sim_mujoco": - raise ValueError("xarm-perception-sim requires a provider using the sim_mujoco adapter") - return binding - +from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule +from dimos.visualization.rerun.bridge import RerunBridgeModule -def _require_pimsim() -> str | None: - if global_config.simulation != "mujoco": - return "xarm-perception-sim requires --simulation mujoco" - if global_config.simulation_provider != "pimsim": - return "xarm-perception-sim requires --simulation-provider pimsim" - return None - - -_simulation = _resolve_xarm7_simulation() -_xarm7_sim_hw = make_xarm7_sim_hardware(_simulation.adapter_address) +_xarm7_sim_hw = make_xarm7_sim_hardware(XARM7_SIM_PATH) xarm_perception_sim = autoconnect( PickAndPlaceModule.blueprint( - robots=[make_xarm7_sim_robot_config(_simulation.robot_base_pose)], + robots=[make_xarm7_sim_robot_config()], planning_timeout=10.0, - visualization={"backend": "viser"}, + visualization={"backend": "meshcat"}, ), - _simulation.backend, + MujocoSimModule.blueprint(**make_xarm7_sim_module_kwargs(XARM7_SIM_PATH)), + ObjectSceneRegistrationModule.blueprint(target_frame="world"), coordinator( hardware=[_xarm7_sim_hw], tasks=[trajectory_task(_xarm7_sim_hw)], ), - vis_module( - viewer_backend=global_config.viewer, - rerun_config=_simulation.rerun_config, - ), -).requirements(_require_pimsim) + RerunBridgeModule.blueprint(), +) diff --git a/dimos/robot/manipulators/xarm/config.py b/dimos/robot/manipulators/xarm/config.py index 50839f5067..0906610545 100644 --- a/dimos/robot/manipulators/xarm/config.py +++ b/dimos/robot/manipulators/xarm/config.py @@ -28,7 +28,6 @@ from dimos.core.global_config import global_config from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.robot.manipulators._modeling import ( base_pose, coordinator_joint_mapping, @@ -59,8 +58,6 @@ XARM_PACKAGE_PATHS: dict[str, Path] = {"xarm_description": LfsPath("xarm_description")} XARM6_SIM_PATH = LfsPath("xarm6/scene.xml") XARM7_SIM_PATH = LfsPath("xarm7/scene.xml") -XARM7_MODEL_PATH = LfsPath("xarm7/xarm7.xml") -XARM7_TABLETOP_SCENE = "xarm-tabletop-v1" XARM_GRIPPER_PARAMS = { "gripper_joint": make_gripper_joints("arm")[0], "gripper_open_pos": 0.85, @@ -69,14 +66,13 @@ XARM7_SIM_HOME = [0.0, -0.247, 0.0, 0.909, 0.0, 1.15644, 0.0] -def make_xarm7_sim_robot_config(robot_base_pose: PoseStamped) -> RobotModelConfig: +def make_xarm7_sim_robot_config() -> RobotModelConfig: return make_xarm7_model_config( name="arm", add_gripper=True, tf_extra_links=["link7"], home_joints=XARM7_SIM_HOME, pre_grasp_offset=0.05, - placement=robot_base_pose, ) @@ -235,7 +231,6 @@ def make_xarm_model_config( tf_extra_links: list[str] | None = None, home_joints: list[float] | None = None, pre_grasp_offset: float = 0.10, - placement: PoseStamped | None = None, ) -> RobotModelConfig: xacro_args = { "dof": str(dof), @@ -251,9 +246,7 @@ def make_xarm_model_config( return RobotModelConfig( name=name, model_path=XARM_MODEL_PATH, - base_pose=( - placement if placement is not None else base_pose(x_offset, y_offset, z_offset, pitch) - ), + base_pose=base_pose(x_offset, y_offset, z_offset, pitch), joint_names=local_joint_names, base_link="link_base", planning_groups=[ diff --git a/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py b/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py deleted file mode 100644 index 6db4c417b9..0000000000 --- a/dimos/robot/unitree/g1/blueprints/basic/groot_wbc_platform.py +++ /dev/null @@ -1,106 +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. - -"""Platform-owned control and localization inputs for the G1 GR00T stack. - -Hardware supplies local-frame lidar plus sensor odometry through PointLIO. -Simulation providers supply world-frame lidar plus base odometry. Mapping and -navigation remain outside this boundary. -""" - -from __future__ import annotations - -from dataclasses import dataclass -from pathlib import Path -from typing import Any - -from dimos.core.coordination.blueprints import Blueprint, autoconnect -from dimos.core.global_config import global_config -from dimos.simulation.providers import SimulationRequest, load_simulation_provider -from dimos.utils.data import LfsPath - -_ROBOT_ONLY_MJCF_PATH = Path(__file__).resolve().parents[2] / "assets" / "g1_29dof.xml" -_ROBOT_MESHDIR = LfsPath("g1_urdf/meshes") - - -@dataclass(frozen=True) -class G1GrootPlatform: - backend: Blueprint - localization_source: Blueprint - simulation: bool - adapter_type: str - adapter_address: str | Path - tick_rate: float - policy_decimation: int - auto_arm: bool - auto_dry_run: bool - ramp_seconds: float - n_workers: int - rerun_config: dict[str, Any] - - -def resolve_g1_groot_platform() -> G1GrootPlatform: - if not global_config.simulation: - from dimos.hardware.sensors.lidar.pointlio.module import PointLio - from dimos.hardware.sensors.lidar.pointlio.zenoh_relay import ( - PointLioZenohRelay, - ) - from dimos.robot.unitree.g1.wholebody_connection import G1WholeBodyConnection - - pointlio_transport = ( - PointLioZenohRelay.blueprint() if global_config.transport == "zenoh" else autoconnect() - ) - return G1GrootPlatform( - backend=G1WholeBodyConnection.blueprint(release_sport_mode=True), - localization_source=autoconnect(pointlio_transport, PointLio.blueprint()), - simulation=False, - adapter_type="transport_lcm", - adapter_address="", - tick_rate=100.0, - policy_decimation=2, - auto_arm=False, - auto_dry_run=True, - ramp_seconds=10.0, - n_workers=10, - rerun_config={}, - ) - - if global_config.simulation != "mujoco": - raise ValueError("unitree-g1-groot-wbc only supports --simulation mujoco") - if not global_config.simulation_provider: - raise ValueError("unitree-g1-groot-wbc simulation requires --simulation-provider pimsim") - - provider = load_simulation_provider(global_config.simulation_provider) - binding = provider.build( - SimulationRequest( - robot_model="unitree_g1", - model_path=_ROBOT_ONLY_MJCF_PATH, - mesh_dir=_ROBOT_MESHDIR, - scene_package=global_config.scene_package, - ) - ) - return G1GrootPlatform( - backend=binding.backend, - localization_source=autoconnect(), - simulation=True, - adapter_type=binding.adapter_type, - adapter_address=binding.adapter_address, - tick_rate=50.0, - policy_decimation=1, - auto_arm=True, - auto_dry_run=False, - ramp_seconds=0.0, - n_workers=12, - rerun_config=binding.rerun_config, - ) diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py index 36e07063fd..b19967d533 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py @@ -12,16 +12,32 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Unitree G1 GR00T whole-body control, mapping, and navigation. - -Hardware registers local-frame lidar through PointLIO and the ray-tracing -mapper. PimSim supplies world-frame lidar and base odometry directly, matching -the stable Go2 simulation path. Both paths converge at the global-map boundary. +"""Unitree G1 GR00T whole-body-control blueprint. + +One blueprint, ``--simulation`` flag picks the backend: + +Real hardware (default): + G1WholeBodyConnection (DDS rt/lowstate <-> rt/lowcmd) + transport_lcm + whole-body adapter. 500 Hz tick. Safety profile: unarmed + dry-run on + start; activate explicitly through ControlCoordinator RPC after + verifying commands. The policy ramps from the current pose to its + bent-knee default over 10 s before taking torque control. The 14 arm + joints are held at the relaxed GR00T-trained default via a lower-priority + servo task. + +Sim (``--simulation``): + MujocoSimModule (in-process MuJoCo + SHM) + sim_mujoco_g1 adapter. + 50 Hz tick (matches the rate the policy was trained at). No arming + ramp and no dry-run. The 14 arm joints are still held with the same + lower-priority servo task as hardware so headless and viewer runs do not + depend on incidental startup timing. Usage: dimos run unitree-g1-groot-wbc # real hardware - dimos --simulation mujoco --simulation-provider pimsim \ - --scene-package office run unitree-g1-groot-wbc + dimos --simulation mujoco run unitree-g1-groot-wbc # sim + dimos --simulation mujoco --scene-package none run unitree-g1-groot-wbc + dimos --simulation mujoco --scene-package office run unitree-g1-groot-wbc + dimos --simulation mujoco --scene-package supermarket run unitree-g1-groot-wbc Overrides (replace the old env-var dance): dimos run unitree-g1-groot-wbc \\ @@ -50,8 +66,6 @@ from dimos.hardware.whole_body.spec import WholeBodyConfig from dimos.mapping.costmapper import CostMapper from dimos.mapping.pointclouds.occupancy import HeightCostConfig -from dimos.mapping.ray_tracing.module import RayTracingVoxelMap -from dimos.mapping.voxels.module import VoxelGridMapper from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.nav_msgs.Path import Path as NavPath from dimos.msgs.sensor_msgs.Imu import Imu @@ -59,80 +73,319 @@ from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.replanning_a_star.module import ReplanningAStarPlanner -from dimos.robot.unitree.g1.blueprints.basic.groot_wbc_platform import ( - resolve_g1_groot_platform, -) from dimos.robot.unitree.g1.config import G1 from dimos.robot.unitree.g1.g1_rerun import ( + G1_RERUN_ROOT, g1_costmap, g1_urdf_joint_state, g1_urdf_static_robot, ) +from dimos.simulation.providers import SimulationRequest, load_simulation_provider +from dimos.simulation.scene_assets.spec import ScenePackage from dimos.utils.data import LfsPath from dimos.visualization.rerun.scene_package import scene_package_static_entities from dimos.visualization.vis_module import vis_module +# Lazy data handles. LfsPath only triggers the LFS pull on first +# str()/open(); using ``get_data(...)`` at import time would block the +# whole CLI on a multi-GB download every time the module is imported. _GROOT_MODEL_DIR = LfsPath("groot") -_NAV_VOXEL_RESOLUTION = 0.08 -_NAV_OVERHEAD_SAFETY_MARGIN = 0.2 -_NAV_MAX_STEP_HEIGHT = 0.10 -_NAV_ROTATION_DIAMETER = 0.8 -_NAV_PATH_WIDTH_MARGIN = 1.1 -_HARDWARE_RERUN_ROOT = "world/odometry/g1" -_SIMULATION_RERUN_ROOT = "world/odom/g1" -_URDF_PATH = Path(__file__).resolve().parents[2] / "g1.urdf" -_NOMINAL_PELVIS_Z = 0.74 -_pelvis_mid360_cache: list[Any] = [] - +_MJCF_PATH = LfsPath("mujoco_sim/g1_gear_wbc.xml") +_ROBOT_ONLY_MJCF_PATH = Path(__file__).resolve().parents[2] / "assets" / "g1_29dof.xml" +_ROBOT_MESHDIR = LfsPath("g1_urdf/meshes") + +_adapter_address: str | Path +_using_simulation_provider = False +_provider_rerun_config: dict[str, Any] = {} +_cmd_vel_topic = "/cmd_vel" if global_config.simulation else "/g1/cmd_vel" +_MUJOCO_LIDAR_CAMERAS = ( + "lidar_front_camera", + "lidar_left_camera", + "lidar_right_camera", +) +_MUJOCO_LIDAR_CAMERA = _MUJOCO_LIDAR_CAMERAS[0] +_G1_NUM_MOTORS = len(g1_joints) +# Robot geoms occupy groups 0/1. The legacy floor uses group 2, and cooked +# scene packages/entities use group 3, so lidar should render world geometry. +_MUJOCO_LIDAR_GEOM_GROUPS = (2, 3) assert G1.height_clearance is not None and G1.width_clearance is not None +_MUJOCO_LIDAR_BASE_KWARGS: dict[str, Any] = { + "width": 320, + "height": 240, + "fps": 2, + "enable_color": False, + "enable_depth": False, + "enable_pointcloud": True, + "pointcloud_fps": 1.0, + "enable_mujoco_lidar": True, + "mujoco_lidar_geom_groups": list(_MUJOCO_LIDAR_GEOM_GROUPS), + "mujoco_lidar_raycast_width": 64, + "mujoco_lidar_raycast_height": 32, + "mujoco_lidar_robot_exclusion_radius": G1.width_clearance, +} +_G1_COMPOSED_MJB_KEY = "unitree-g1-groot-wbc_spawn_9p2_11p8_yaw_m1p57_static_only_lidar" +_G1_COMPOSED_MJB_ROBOT = "unitree-g1-groot-wbc" +_G1_COMPOSED_MJB_ENTITY_POLICY = "static-only" +_G1_NAV_VOXEL_RESOLUTION = 0.05 +# go2 nav_3d resolution; 0.05 saturates the raytracer on the Orin. +_G1_REAL_NAV_VOXEL_RESOLUTION = 0.08 +_G1_NAV_OVERHEAD_SAFETY_MARGIN = 0.2 +_G1_NAV_MAX_STEP_HEIGHT = 0.10 +_G1_NAV_ROTATION_DIAMETER = 0.8 +_G1_NAV_SAFE_RADIUS_MARGIN = 0.6 class _G1GrootCoordinator(ControlCoordinator): g1_joints: Out[JointState] +# Per-robot joint stream. Namespaced like the rest of the g1 wire topics, which +# also fixes its Rerun entity path (`world/` + topic). _G1_JOINTS_TOPIC = "/g1/joints" _G1_JOINTS_ENTITY = f"world{_G1_JOINTS_TOPIC}" -_platform = resolve_g1_groot_platform() -_RERUN_ROOT = _SIMULATION_RERUN_ROOT if _platform.simulation else _HARDWARE_RERUN_ROOT -_ODOMETRY_ENTITY = "world/odom" if _platform.simulation else "world/odometry" +def _mujoco_lidar_kwargs(camera_name: str, camera_names: tuple[str, ...]) -> dict[str, Any]: + return { + "camera_name": camera_name, + "mujoco_lidar_camera_names": list(camera_names), + **_MUJOCO_LIDAR_BASE_KWARGS, + } + + +if global_config.simulation and global_config.simulation != "mujoco": + raise ValueError("unitree-g1-groot-wbc only supports --simulation mujoco") -_mapper = ( - VoxelGridMapper.blueprint( - voxel_size=_NAV_VOXEL_RESOLUTION, - emit_every=5, +if global_config.simulation == "mujoco": + from dimos.mapping.voxels.module import VoxelGridMapper + from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule + from dimos.simulation.engines.robot_sim_binding import ( + RobotSimSpec, + mjcf_joint_names_from_hardware, ) - if _platform.simulation - else RayTracingVoxelMap.blueprint( - voxel_size=_NAV_VOXEL_RESOLUTION, - emit_every=0, - global_emit_every=4, - max_health=10, - graze_cos=0.85, + + _g1_sim_joints = tuple(g1_joints) + _g1_sim_spec = RobotSimSpec( + robot_id="g1", + hardware_joints=_g1_sim_joints, + root_body_names=("pelvis",), + root_joint_names=("floating_base_joint",), + require_floating_base=True, + model_joint_names=mjcf_joint_names_from_hardware(_g1_sim_joints), + imu_gyro_names=( + "imu-pelvis-angular-velocity", + "imu-torso-angular-velocity", + "imu-angular-velocity", + "gyro_pelvis", + "imu_gyro", + ), + imu_accel_names=( + "imu-pelvis-linear-acceleration", + "imu-torso-linear-acceleration", + "imu-linear-acceleration", + "accelerometer_pelvis", + "imu_accel", + ), + require_imu=True, ) -) -_navigation = autoconnect( - _platform.localization_source, - _mapper, - CostMapper.blueprint( - config=HeightCostConfig( - resolution=_NAV_VOXEL_RESOLUTION, - can_pass_under=G1.height_clearance + _NAV_OVERHEAD_SAFETY_MARGIN, - can_climb=_NAV_MAX_STEP_HEIGHT, + def _legacy_mujoco_backend() -> Any: + return MujocoSimModule.blueprint( + address=_MJCF_PATH, + headless=True, + dof=_G1_NUM_MOTORS, + **_mujoco_lidar_kwargs(_MUJOCO_LIDAR_CAMERA, _MUJOCO_LIDAR_CAMERAS), + inject_legacy_assets=True, + robot_sim_spec=_g1_sim_spec, + ) + + def _scene_mujoco_backend() -> tuple[Any, str | Path]: + if global_config.scene_package is None: + return _legacy_mujoco_backend(), _MJCF_PATH + + scene_path = Path(str(global_config.scene_package)).expanduser() + if scene_path.suffix.lower() == ".mjb": + if not scene_path.exists(): + raise FileNotFoundError(f"MuJoCo binary scene not found: {scene_path}") + return ( + MujocoSimModule.blueprint( + address=scene_path, + headless=True, + dof=_G1_NUM_MOTORS, + **_mujoco_lidar_kwargs(_MUJOCO_LIDAR_CAMERA, _MUJOCO_LIDAR_CAMERAS), + robot_sim_spec=_g1_sim_spec, + ), + scene_path, + ) + + from dimos.simulation.scenes.catalog import resolve_scene_package + + package = resolve_scene_package(global_config.scene_package) + if package is None: + return _legacy_mujoco_backend(), _MJCF_PATH + if package.mujoco_scene_path is None: + raise ValueError(f"scene package has no MuJoCo scene artifact: {package.metadata_path}") + + composed_scene = _precomposed_g1_scene(package) + if composed_scene is not None: + return ( + MujocoSimModule.blueprint( + address=composed_scene, + headless=True, + dof=_G1_NUM_MOTORS, + **_mujoco_lidar_kwargs(_MUJOCO_LIDAR_CAMERA, _MUJOCO_LIDAR_CAMERAS), + robot_sim_spec=_g1_sim_spec, + ), + composed_scene, + ) + + return ( + MujocoSimModule.blueprint( + scene_xml=package.mujoco_scene_path, + robot_mjcf=_ROBOT_ONLY_MJCF_PATH, + robot_meshdir=_ROBOT_MESHDIR, + robot_id="", + scene_entities=package.entities, + headless=True, + dof=_G1_NUM_MOTORS, + **_mujoco_lidar_kwargs(_MUJOCO_LIDAR_CAMERA, _MUJOCO_LIDAR_CAMERAS), + robot_sim_spec=_g1_sim_spec, + ), + _ROBOT_ONLY_MJCF_PATH, + ) + + def _precomposed_g1_scene(package: ScenePackage) -> Path | None: + candidate = package.mujoco_composed_binary_path( + key=_G1_COMPOSED_MJB_KEY, + robot=_G1_COMPOSED_MJB_ROBOT, + entity_policy=_G1_COMPOSED_MJB_ENTITY_POLICY, + ) + if candidate is None: + return None + if not candidate.exists(): + raise FileNotFoundError( + f"scene package declares a composed MuJoCo binary that is missing: {candidate}" + ) + return candidate + + if global_config.simulation_provider: + _using_simulation_provider = True + _provider = load_simulation_provider(global_config.simulation_provider) + _binding = _provider.build( + SimulationRequest( + robot_model="unitree_g1", + model_path=_ROBOT_ONLY_MJCF_PATH, + mesh_dir=_ROBOT_MESHDIR, + scene_package=global_config.scene_package, + ) + ) + _backend = _binding.backend + _adapter_address = _binding.adapter_address + _adapter_type = _binding.adapter_type + _provider_rerun_config = _binding.rerun_config + else: + # Legacy in-repo MuJoCo backend. + _backend, _adapter_address = _scene_mujoco_backend() + _adapter_type = "sim_mujoco_g1" + + # MujocoSimModule's ``odom`` Out is the sole producer of ``/odom`` + # now - the coordinator no longer polls the whole-body adapter for + # base pose (read_odom was dropped from the Protocol). autoconnect + # maps ``(odom, PoseStamped)`` to ``/odom`` by default; no override. + _tick_rate = 50.0 + _auto_arm = True + _auto_dry_run = False + _default_ramp_seconds = 0.0 + _decimation: int | None = 1 + _n_workers = 12 if _using_simulation_provider else 2 + _arm_holder = TaskConfig( + name="servo_arms", + type="servo", + joint_names=g1_arms, + priority=10, + auto_start=True, + params={"default_positions": ARM_DEFAULT_POSE}, + ) + _mapper = VoxelGridMapper.blueprint(emit_every=1) + _nav_stack = autoconnect( + _mapper, + CostMapper.blueprint( + config=HeightCostConfig( + resolution=_G1_NAV_VOXEL_RESOLUTION, + can_pass_under=G1.height_clearance + _G1_NAV_OVERHEAD_SAFETY_MARGIN, + can_climb=_G1_NAV_MAX_STEP_HEIGHT, + ), + initial_safe_radius_meters=G1.width_clearance + _G1_NAV_SAFE_RADIUS_MARGIN, ), - ), - ReplanningAStarPlanner.blueprint( - robot_width=G1.width_clearance / _NAV_PATH_WIDTH_MARGIN, - robot_rotation_diameter=_NAV_ROTATION_DIAMETER, - ), - MovementManager.blueprint(), -) + ReplanningAStarPlanner.blueprint( + robot_width=G1.width_clearance, + robot_rotation_diameter=_G1_NAV_ROTATION_DIAMETER, + ), + MovementManager.blueprint(), + ) + _remappings = [(_G1GrootCoordinator, "twist_command", "cmd_vel")] + if not _using_simulation_provider: + _remappings.insert(0, (VoxelGridMapper, "lidar", "pointcloud")) +else: + from dimos.hardware.sensors.lidar.pointlio.module import PointLio + from dimos.mapping.ray_tracing.module import RayTracingVoxelMap + from dimos.robot.unitree.g1.wholebody_connection import G1WholeBodyConnection + + # Real-hw backend: DDS connection module + transport_lcm adapter. + _backend = G1WholeBodyConnection.blueprint(release_sport_mode=True) + _adapter_type = "transport_lcm" + _adapter_address = "" + # The onboard Jetson can't sustain a 500 Hz tick; it collapses to ~90 Hz + # and starves the policy, so balance decays. + _tick_rate = 100.0 + # Real hardware: come up unarmed + dry-run; operator must click + # Activate (10 s ramp) after verifying commands. + _auto_arm = False + _auto_dry_run = True + _default_ramp_seconds = 10.0 + _decimation = 2 # 100 Hz tick / 2 = 50 Hz policy (training + sim rate). + # One process per heavy module; fewer workers starve the Rerun bridge. + _n_workers = 10 + # Real hardware needs the arms held -- kd damping alone would let + # them sag toward singular configurations between trajectories. + _arm_holder = TaskConfig( + name="servo_arms", + type="servo", + joint_names=g1_arms, + priority=10, + auto_start=True, + params={"default_positions": ARM_DEFAULT_POSE}, + ) + # Same nav middle as unitree-g1-nav-simple, fed by Point-LIO from the + # MID-360, executed through the coordinator's twist_command. + _nav_stack = autoconnect( + PointLio.blueprint(), + RayTracingVoxelMap.blueprint( + voxel_size=_G1_REAL_NAV_VOXEL_RESOLUTION, + emit_every=0, # no local_map consumer here + global_emit_every=4, # ~1 Hz global map; also paces the costmap + # Clearing matched to go2 nav_3d. + max_health=10, + graze_cos=0.85, + ), + CostMapper.blueprint( + config=HeightCostConfig( + resolution=_G1_REAL_NAV_VOXEL_RESOLUTION, + can_pass_under=G1.height_clearance + _G1_NAV_OVERHEAD_SAFETY_MARGIN, + can_climb=_G1_NAV_MAX_STEP_HEIGHT, + ), + initial_safe_radius_meters=G1.width_clearance + _G1_NAV_SAFE_RADIUS_MARGIN, + ), + ReplanningAStarPlanner.blueprint( + robot_width=G1.width_clearance, + robot_rotation_diameter=_G1_NAV_ROTATION_DIAMETER, + ), + MovementManager.blueprint(), + ) + _remappings = [(_G1GrootCoordinator, "twist_command", "cmd_vel")] -def _rerun_blueprint() -> Any: +def _g1_groot_rerun_blueprint() -> Any: import rerun as rr import rerun.blueprint as rrb @@ -149,110 +402,115 @@ def _rerun_blueprint() -> Any: ) -def _nav_path(path: NavPath) -> Any: +def _g1_nav_path(path: NavPath) -> Any: return path.to_rerun(z_offset=0.3) -def _lidar_scan(cloud: Any) -> Any: - return cloud.to_rerun(voxel_size=0.03, colors=[80, 210, 255], mode="points") +# Mesh root: sim roots under the /odom transform; real hw under the LIO's +# /odometry, whose world frame is the lidar boot pose (ground ~1.2 m below 0). +_G1_ROOT = G1_RERUN_ROOT if global_config.simulation == "mujoco" else "world/odometry/g1" +_G1_URDF_PATH = Path(__file__).resolve().parents[2] / "g1.urdf" +# Nominal standing pelvis height; matches G1GrootWBCTask's height_cmd. +_G1_NOMINAL_PELVIS_Z = 0.74 +_g1_pelvis_mid360_cache: list[Any] = [] -def _pelvis_to_mid360() -> Any: - if not _pelvis_mid360_cache: + +def _g1_pelvis_to_mid360() -> Any: + """Rest-pose pelvis->mid360_link transform from the G1 URDF (cached).""" + if not _g1_pelvis_mid360_cache: from importlib import import_module import numpy as np - urdf = import_module("yourdfpy").URDF.load(str(_URDF_PATH), load_meshes=False) + urdf = import_module("yourdfpy").URDF.load(str(_G1_URDF_PATH), load_meshes=False) urdf.update_cfg(np.zeros(len(urdf.actuated_joint_names))) - _pelvis_mid360_cache.append(urdf.get_transform("mid360_link", "pelvis")) - return _pelvis_mid360_cache[0] + _g1_pelvis_mid360_cache.append(urdf.get_transform("mid360_link", "pelvis")) + return _g1_pelvis_mid360_cache[0] -def _real_odometry_root(odometry: Any) -> Any: +def _g1_real_odometry_root(odom: Any) -> Any: + """Robot-mesh root: pelvis pose from the LIO's mid360 odometry (rest offset).""" import numpy as np import rerun as rr from dimos.msgs.geometry_msgs.Quaternion import Quaternion - world_mid360 = np.eye(4) - world_mid360[:3, :3] = odometry.orientation.to_rotation_matrix() @ np.diag([1.0, -1.0, -1.0]) - world_mid360[:3, 3] = (odometry.x, odometry.y, odometry.z) - world_pelvis = world_mid360 @ np.linalg.inv(_pelvis_to_mid360()) - quaternion = Quaternion.from_rotation_matrix(world_pelvis[:3, :3]) + t_world_mid360 = np.eye(4) + # The MID-360 is mounted upside down (the URDF doesn't carry the flip): + # un-roll by Rx(pi) == diag(1, -1, -1). + t_world_mid360[:3, :3] = odom.orientation.to_rotation_matrix() @ np.diag([1.0, -1.0, -1.0]) + t_world_mid360[:3, 3] = (odom.x, odom.y, odom.z) + t_world_pelvis = t_world_mid360 @ np.linalg.inv(_g1_pelvis_to_mid360()) + q = Quaternion.from_rotation_matrix(t_world_pelvis[:3, :3]) return rr.Transform3D( - translation=world_pelvis[:3, 3].tolist(), - rotation=rr.Quaternion(xyzw=[quaternion.x, quaternion.y, quaternion.z, quaternion.w]), + translation=t_world_pelvis[:3, 3].tolist(), + rotation=rr.Quaternion(xyzw=[q.x, q.y, q.z, q.w]), ) -def _real_ground_z() -> float: - return -(float(_pelvis_to_mid360()[2, 3]) + _NOMINAL_PELVIS_Z) +def _g1_real_ground_z() -> float: + """Ground height in the LIO boot frame: -(mount z + nominal pelvis z).""" + return -(float(_g1_pelvis_to_mid360()[2, 3]) + _G1_NOMINAL_PELVIS_Z) -def _real_costmap(grid: Any) -> Any: - return g1_costmap(grid, z_offset=_real_ground_z() + 0.02) +def _g1_real_costmap(grid: Any) -> Any: + """Costmap rendered on the actual ground plane of the boot frame.""" + return g1_costmap(grid, z_offset=_g1_real_ground_z() + 0.02) -_static_entities: dict[str, Any] = { - _RERUN_ROOT: g1_urdf_static_robot(root_path=_RERUN_ROOT), +_static_rerun_entities: dict[str, Any] = { + _G1_ROOT: g1_urdf_static_robot(root_path=_G1_ROOT), } -if not _platform.simulation: - _static_entities.update(scene_package_static_entities(global_config.scene_package)) +if not _using_simulation_provider: + _static_rerun_entities.update(scene_package_static_entities(global_config.scene_package)) _rerun_config: dict[str, Any] = { - "memory_limit": "1GB", - "blueprint": _rerun_blueprint, + "blueprint": _g1_groot_rerun_blueprint, "visual_override": { + # This blueprint uses raycast lidar, so suppress raw camera streams + # in Rerun. "world/color_image": None, "world/camera_info": None, "world/depth_image": None, "world/depth_camera_info": None, - "world/pimsim/pointlio_odometry": None, - "world/localization_anchor": None, - "world/lidar": _lidar_scan, - _G1_JOINTS_ENTITY: g1_urdf_joint_state(root_path=_RERUN_ROOT), + _G1_JOINTS_ENTITY: g1_urdf_joint_state(root_path=_G1_ROOT), "world/global_costmap": g1_costmap, "world/navigation_costmap": g1_costmap, - "world/path": _nav_path, + "world/path": _g1_nav_path, }, "max_hz": { _G1_JOINTS_ENTITY: 20.0, + # Raw state streams arrive at ~440 Hz; useful only as debug plots. "world/g1/imu": 10.0, "world/g1/motor_states": 10.0, "world/g1/motor_command": 10.0, - _ODOMETRY_ENTITY: 15.0, - "world/lidar": 2.0, + "world/odometry": 15.0, "world/global_map": 1.0, "world/global_costmap": 2.0, "world/navigation_costmap": 2.0, + # The planner publishes an empty Path() immediately before the new + # planned path. Throttling this entity drops the real path. "world/path": 0, }, - "latest_state": { - "world/global_map", - "world/global_costmap", - "world/navigation_costmap", - }, - "static": _static_entities, + "static": _static_rerun_entities, } for _section in ("static", "visual_override", "max_hz"): _rerun_config[_section] = { **_rerun_config.get(_section, {}), - **_platform.rerun_config.get(_section, {}), + **_provider_rerun_config.get(_section, {}), } -_rerun_config["latest_state"] = { - *_rerun_config["latest_state"], - *_platform.rerun_config.get("latest_state", set()), -} -for _key, _value in _platform.rerun_config.items(): - if _key not in {"static", "visual_override", "max_hz", "latest_state"}: +for _key, _value in _provider_rerun_config.items(): + if _key not in {"static", "visual_override", "max_hz"}: _rerun_config[_key] = _value -if not _platform.simulation: - _rerun_config["visual_override"]["world/odometry"] = _real_odometry_root - _rerun_config["visual_override"]["world/global_costmap"] = _real_costmap - _rerun_config["visual_override"]["world/navigation_costmap"] = _real_costmap +if global_config.simulation != "mujoco": + _rerun_config["visual_override"]["world/odometry"] = _g1_real_odometry_root + _rerun_config["visual_override"]["world/global_costmap"] = _g1_real_costmap + _rerun_config["visual_override"]["world/navigation_costmap"] = _g1_real_costmap + # Raw scan is sensor-frame (LIO contract); the voxel map is the live view. + _rerun_config["visual_override"]["world/lidar"] = None def _viewer() -> Any: @@ -262,14 +520,14 @@ def _viewer() -> Any: _coordinator = _G1GrootCoordinator.blueprint( instance_name="ControlCoordinator", publish_robot_joint_states=True, - tick_rate=_platform.tick_rate, + tick_rate=_tick_rate, hardware=[ HardwareComponent( hardware_id="g1", hardware_type=HardwareType.WHOLE_BODY, joints=g1_joints, - adapter_type=_platform.adapter_type, - address=_platform.adapter_address, + adapter_type=_adapter_type, + address=_adapter_address, wb_config=WholeBodyConfig(kp=tuple(G1_GROOT_KP), kd=tuple(G1_GROOT_KD)), ), ], @@ -283,37 +541,31 @@ def _viewer() -> Any: params={ "model_path": _GROOT_MODEL_DIR, "hardware_id": "g1", - "auto_arm": _platform.auto_arm, - "auto_dry_run": _platform.auto_dry_run, - "default_ramp_seconds": _platform.ramp_seconds, - "decimation": _platform.policy_decimation, + "auto_arm": _auto_arm, + "auto_dry_run": _auto_dry_run, + "default_ramp_seconds": _default_ramp_seconds, + "decimation": _decimation, }, ), - TaskConfig( - name="servo_arms", - type="servo", - joint_names=g1_arms, - priority=10, - auto_start=True, - params={"default_positions": ARM_DEFAULT_POSE}, - ), + *([_arm_holder] if _arm_holder is not None else []), ], ).transports( { ("joint_command", JointState): LCMTransport("/g1/joint_command", JointState), ("g1_joints", JointState): LCMTransport(_G1_JOINTS_TOPIC, JointState), - ("cmd_vel", Twist): LCMTransport("/g1/cmd_vel", Twist), + ("cmd_vel", Twist): LCMTransport(_cmd_vel_topic, Twist), + # Real-hw only: the transport_lcm adapter speaks to + # G1WholeBodyConnection over these topics. autoconnect already + # matches by (name, type) so sim doesn't need them -- they're + # harmless when the sim engine doesn't expose those ports. ("motor_states", JointState): LCMTransport("/g1/motor_states", JointState), ("imu", Imu): LCMTransport("/g1/imu", Imu), - ("motor_command", MotorCommandArray): LCMTransport( - "/g1/motor_command", - MotorCommandArray, - ), + ("motor_command", MotorCommandArray): LCMTransport("/g1/motor_command", MotorCommandArray), } ) unitree_g1_groot_wbc = ( - autoconnect(_platform.backend, _coordinator, _navigation, _viewer()) - .remappings(cast("Any", [(_G1GrootCoordinator, "twist_command", "cmd_vel")])) - .global_config(robot_model="unitree_g1", n_workers=_platform.n_workers) + autoconnect(_backend, _coordinator, _nav_stack, _viewer()) + .remappings(cast("Any", _remappings)) + .global_config(robot_model="unitree_g1", n_workers=_n_workers) ) diff --git a/dimos/robot/unitree/g1/g1_rerun.py b/dimos/robot/unitree/g1/g1_rerun.py index 0ccdbf17af..dca02403f6 100644 --- a/dimos/robot/unitree/g1/g1_rerun.py +++ b/dimos/robot/unitree/g1/g1_rerun.py @@ -18,7 +18,8 @@ from typing import Any -from dimos.visualization.rerun.costmap import classic_costmap +import numpy as np + from dimos.visualization.rerun.urdf_robot import ( UrdfRobotJointStateRerunFactory, UrdfRobotStaticRerunFactory, @@ -27,6 +28,14 @@ G1_RERUN_ROOT = "world/odom/g1" G1_RERUN_URDF = "g1_urdf/g1.fixed.urdf" +# Classic costmap palette, indexed by grid value + 1: +# transparent unknown, blue free, orange occupied, red lethal. +_COSTMAP_LOOKUP_TABLE = np.zeros((102, 4), dtype=np.uint8) +_COSTMAP_LOOKUP_TABLE[0] = (0, 0, 0, 0) +_COSTMAP_LOOKUP_TABLE[1] = (72, 73, 129, 255) +_COSTMAP_LOOKUP_TABLE[2:101] = (255, 140, 0, 255) +_COSTMAP_LOOKUP_TABLE[101] = (220, 30, 30, 255) + def g1_costmap(grid: Any, z_offset: float = 0.02) -> Any: """Render an OccupancyGrid with the classic costmap palette. @@ -34,7 +43,7 @@ def g1_costmap(grid: Any, z_offset: float = 0.02) -> Any: The default z_offset lifts the mesh 2cm off the floor plane to avoid z-fighting with the ground. """ - return classic_costmap(grid, z_offset=z_offset) + return grid.to_rerun(color_lookup_table=_COSTMAP_LOOKUP_TABLE, z_offset=z_offset) def g1_urdf_static_robot(root_path: str = G1_RERUN_ROOT) -> UrdfRobotStaticRerunFactory: diff --git a/dimos/robot/unitree/go2/blueprints/basic/go2_platform.py b/dimos/robot/unitree/go2/blueprints/basic/go2_platform.py deleted file mode 100644 index d1ef72c734..0000000000 --- a/dimos/robot/unitree/go2/blueprints/basic/go2_platform.py +++ /dev/null @@ -1,55 +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. - -from functools import cache - -from dimos.core.coordination.blueprints import Blueprint -from dimos.core.global_config import global_config -from dimos.robot.unitree.go2.connection import GO2Connection -from dimos.simulation.providers import ( - SimulationBinding, - SimulationRequest, - load_simulation_provider, -) - - -def resolve_go2_platform() -> Blueprint: - if global_config.simulation in ("", "dimsim"): - return GO2Connection.blueprint() - return _resolve_simulation_binding().backend - - -def resolve_go2_rerun_config() -> dict[str, object]: - if global_config.simulation in ("", "dimsim"): - return {} - return _resolve_simulation_binding().rerun_config - - -@cache -def _resolve_simulation_binding() -> SimulationBinding: - if global_config.simulation != "mujoco": - raise ValueError("unitree-go2 only supports --simulation mujoco") - if not global_config.simulation_provider: - raise ValueError("unitree-go2 simulation requires --simulation-provider pimsim") - - provider = load_simulation_provider(global_config.simulation_provider) - return provider.build( - SimulationRequest( - robot_model="unitree_go2", - scene_package=global_config.scene_package, - ) - ) - - -__all__ = ["resolve_go2_platform", "resolve_go2_rerun_config"] diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py index 8ae8daa143..c342c74f92 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py @@ -18,11 +18,7 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config -from dimos.robot.unitree.go2.blueprints.basic.go2_platform import ( - resolve_go2_platform, - resolve_go2_rerun_config, -) -from dimos.visualization.rerun.costmap import classic_costmap +from dimos.robot.unitree.go2.connection import GO2Connection from dimos.visualization.vis_module import vis_module @@ -38,7 +34,12 @@ def _convert_global_map(grid: Any) -> Any: def _convert_navigation_costmap(grid: Any) -> Any: - return classic_costmap(grid, z_offset=0.015) + return grid.to_rerun( + colormap="Accent", + z_offset=0.015, + opacity=0.2, + background="#484981", + ) def _static_base_link(rr: Any) -> list[Any]: @@ -88,7 +89,6 @@ def _go2_rerun_blueprint() -> Any: "world/camera_info": _convert_camera_info, "world/global_map": _convert_global_map, "world/merged_map": _convert_global_map, - "world/global_costmap": _convert_navigation_costmap, "world/navigation_costmap": _convert_navigation_costmap, }, "max_hz": { @@ -103,16 +103,6 @@ def _go2_rerun_blueprint() -> Any: }, } -_provider_rerun_config = resolve_go2_rerun_config() -for _section in ("static", "visual_override", "max_hz"): - rerun_config[_section] = { - **rerun_config.get(_section, {}), - **_provider_rerun_config.get(_section, {}), - } -for _key, _value in _provider_rerun_config.items(): - if _key not in {"static", "visual_override", "max_hz"}: - rerun_config[_key] = _value - _with_vis = autoconnect( vis_module( viewer_backend=global_config.viewer, @@ -124,7 +114,7 @@ def _go2_rerun_blueprint() -> Any: unitree_go2_basic = ( autoconnect( _with_vis, - resolve_go2_platform(), + GO2Connection.blueprint(), ).global_config(n_workers=4, robot_model="unitree_go2") # we temporarily disabled sensor timestamps # and are derriving all timestmaps upon reception diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py index eedf08073c..4e41e0e675 100644 --- a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py @@ -19,7 +19,6 @@ from dimos.core.stream import In from dimos.core.transport import LCMTransport from dimos.mapping.costmapper import CostMapper -from dimos.mapping.pointclouds.occupancy import HeightCostConfig from dimos.mapping.relocalization.module import RelocalizationModule from dimos.mapping.voxels.module import VoxelGridMapper from dimos.memory2.module import Recorder, RecorderConfig, pose_setter_for @@ -37,27 +36,13 @@ from dimos.perception.fiducial.marker_detection_stream_module import MarkerDetectionStreamModule from dimos.perception.fiducial.marker_tf_module import MarkerTfModule from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic -from dimos.robot.unitree.go2.config import GO2 from dimos.robot.unitree.go2.connection import GO2Connection -# Overhead margin added to the standing height before a gap counts as -# pass-under space, mirroring the G1 navigation composition. -_NAV_OVERHEAD_SAFETY_MARGIN = 0.2 -_NAV_MAX_STEP_HEIGHT = 0.15 - unitree_go2 = autoconnect( unitree_go2_basic, VoxelGridMapper.blueprint(emit_every=5), - CostMapper.blueprint( - config=HeightCostConfig( - can_pass_under=GO2.height_clearance + _NAV_OVERHEAD_SAFETY_MARGIN, - can_climb=_NAV_MAX_STEP_HEIGHT, - ), - ), - ReplanningAStarPlanner.blueprint( - robot_width=GO2.width_clearance, - robot_rotation_diameter=GO2.rotation_diameter, - ), + CostMapper.blueprint(), + ReplanningAStarPlanner.blueprint(), WavefrontFrontierExplorer.blueprint(), PatrollingModule.blueprint(), MovementManager.blueprint(), diff --git a/dimos/robot/unitree/go2/config.py b/dimos/robot/unitree/go2/config.py deleted file mode 100644 index 9ab1d11141..0000000000 --- a/dimos/robot/unitree/go2/config.py +++ /dev/null @@ -1,41 +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. - -from __future__ import annotations - -from dataclasses import dataclass - - -@dataclass(frozen=True) -class GO2Config: - """Physical metadata used by Go2 navigation blueprints. - - The Unitree Go2 standing envelope is 0.70 x 0.31 x 0.40 m; clearances - include the leg stance and a safety margin. - """ - - name: str - height_clearance: float - width_clearance: float - rotation_diameter: float - - -GO2 = GO2Config( - name="unitree_go2", - height_clearance=0.45, - width_clearance=0.5, - rotation_diameter=0.75, -) - -__all__ = ["GO2", "GO2Config"] diff --git a/dimos/simulation/dimsim/scene_client.py b/dimos/simulation/dimsim/scene_client.py index 5dd555bac4..d7234626c8 100644 --- a/dimos/simulation/dimsim/scene_client.py +++ b/dimos/simulation/dimsim/scene_client.py @@ -926,35 +926,3 @@ def get_agent_position(self) -> dict[str, Any]: return { x: p.x, y: p.y, z: p.z }; """ return cast("dict[str, Any]", self.exec(code)) - - def get_semantic_object_bounds(self, query: str) -> dict[str, Any]: - """Return the live Three.js world AABB for a semantic scene object.""" - - if not query.strip(): - raise ValueError("semantic object query must not be empty") - code = f""" -const normalize = (value) => String(value || "") - .toLowerCase() - .replace(/[^a-z0-9]+/g, " ") - .trim(); -const needle = normalize({json.dumps(query)}); -const entry = assets.find((asset) => - normalize(`${{asset.title || ""}} ${{asset.id || ""}}`).includes(needle) -); -if (!entry) return null; -const object = assetsGroup.getObjectByName(`asset:${{entry.id}}`); -if (!object) return null; -object.updateMatrixWorld(true); -const bounds = new THREE.Box3().setFromObject(object); -if (bounds.isEmpty()) return null; -return {{ - id: entry.id, - title: entry.title || null, - min: {{ x: bounds.min.x, y: bounds.min.y, z: bounds.min.z }}, - max: {{ x: bounds.max.x, y: bounds.max.y, z: bounds.max.z }}, -}}; -""" - result = self.exec(code) - if not isinstance(result, dict): - raise LookupError(f"semantic object not found in DimSim scene: {query!r}") - return result diff --git a/dimos/simulation/mujoco/direct_cmd_vel_explorer.py b/dimos/simulation/mujoco/direct_cmd_vel_explorer.py index 1b563f7ab5..81b7f62156 100644 --- a/dimos/simulation/mujoco/direct_cmd_vel_explorer.py +++ b/dimos/simulation/mujoco/direct_cmd_vel_explorer.py @@ -14,10 +14,9 @@ import math import threading -from typing import TYPE_CHECKING, cast +from typing import TYPE_CHECKING -from dimos.core.transport import PubSubTransport -from dimos.core.transport_factory import make_transport +from dimos.core.transport import LCMTransport from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -36,21 +35,15 @@ def __init__( self.linear_speed = linear_speed self.rotation_speed = rotation_speed self._dt = 1.0 / publish_rate - self._cmd_vel: PubSubTransport[Twist] | None = None - self._odom: PubSubTransport[PoseStamped] | None = None + self._cmd_vel: LCMTransport[Twist] | None = None + self._odom: LCMTransport[PoseStamped] | None = None self._pose: PoseStamped | None = None self._new_pose = threading.Event() self._unsub: Callable[[], None] | None = None def start(self) -> None: - self._cmd_vel = cast( - "PubSubTransport[Twist]", - make_transport("/cmd_vel", Twist), - ) - self._odom = cast( - "PubSubTransport[PoseStamped]", - make_transport("/odom", PoseStamped), - ) + self._cmd_vel = LCMTransport("/cmd_vel", Twist) + self._odom = LCMTransport("/odom", PoseStamped) self._pose = None self._unsub = self._odom.subscribe(self._on_odom) diff --git a/dimos/visualization/rerun/bridge.py b/dimos/visualization/rerun/bridge.py index 8fba9641a1..25f16e0271 100644 --- a/dimos/visualization/rerun/bridge.py +++ b/dimos/visualization/rerun/bridge.py @@ -223,7 +223,6 @@ class Config(ModuleConfig): visual_override: dict[Glob | str, VisualOverride | None] = field(default_factory=dict) static: dict[str, Callable[[Any], Any]] = field(default_factory=dict) max_hz: dict[str, float] = field(default_factory=dict) - latest_state: set[str] = field(default_factory=set) entity_prefix: str = "world" topic_to_entity: Callable[[Any], str] | None = None @@ -353,20 +352,15 @@ def _on_message(self, msg: Any, topic: Any) -> None: # TFMessage for example returns list of (entity_path, archetype) tuples if is_rerun_multi(rerun_data): for path, archetype in rerun_data: - rr.log(path, archetype, static=path in self.config.latest_state) + rr.log(path, archetype) else: - latest_state = entity_path in self.config.latest_state - rr.log(entity_path, cast("Archetype", rerun_data), static=latest_state) + rr.log(entity_path, cast("Archetype", rerun_data)) # if source msg carries a frame_id, attach the entity to that TF frame # should skip if archetype is a Transform3D if not isinstance(rerun_data, rr.Transform3D): frame_id = getattr(msg, "frame_id", None) if frame_id and self._frame_attached.get(entity_path) != frame_id: - rr.log( - entity_path, - rr.Transform3D(parent_frame=f"tf#/{frame_id}"), - static=latest_state, - ) + rr.log(entity_path, rr.Transform3D(parent_frame=f"tf#/{frame_id}")) self._frame_attached[entity_path] = frame_id @rpc diff --git a/dimos/visualization/rerun/costmap.py b/dimos/visualization/rerun/costmap.py deleted file mode 100644 index 3bfc49efad..0000000000 --- a/dimos/visualization/rerun/costmap.py +++ /dev/null @@ -1,39 +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. - -from __future__ import annotations - -from typing import Any - -import numpy as np - -# Classic costmap palette, indexed by grid value + 1: -# transparent unknown, blue free, orange occupied, red lethal. -COSTMAP_LOOKUP_TABLE = np.zeros((102, 4), dtype=np.uint8) -COSTMAP_LOOKUP_TABLE[0] = (0, 0, 0, 0) -COSTMAP_LOOKUP_TABLE[1] = (72, 73, 129, 255) -COSTMAP_LOOKUP_TABLE[2:101] = (255, 140, 0, 255) -COSTMAP_LOOKUP_TABLE[101] = (220, 30, 30, 255) - - -def classic_costmap(grid: Any, z_offset: float = 0.02) -> Any: - """Render an OccupancyGrid with the classic costmap palette. - - The default z_offset lifts the mesh 2cm off the floor plane to avoid - z-fighting with the ground. - """ - return grid.to_rerun(color_lookup_table=COSTMAP_LOOKUP_TABLE, z_offset=z_offset) - - -__all__ = ["COSTMAP_LOOKUP_TABLE", "classic_costmap"] diff --git a/dimos/visualization/rerun/test_detection3d_bridge.py b/dimos/visualization/rerun/test_detection3d_bridge.py index 92202773e8..8385c6dfbf 100644 --- a/dimos/visualization/rerun/test_detection3d_bridge.py +++ b/dimos/visualization/rerun/test_detection3d_bridge.py @@ -77,18 +77,3 @@ def test_detection3darray_bridge_attaches_topic_entity_to_message_frame() -> Non transform = mock_log.call_args_list[1].args[1] assert isinstance(transform, rr.Transform3D) assert transform.parent_frame.as_arrow_array().to_pylist() == ["tf#/world"] - - -def test_latest_state_entity_overwrites_data_and_frame_attachment() -> None: - entity_path = "world/marker_detection/detections" - bridge = RerunBridgeModule(latest_state={entity_path}) - bridge._min_intervals = {} - - try: - with patch("dimos.visualization.rerun.bridge.rr.log") as mock_log: - bridge._on_message(_detection_array(), Topic("/marker_detection/detections")) - finally: - bridge.stop() - - assert mock_log.call_count == 2 - assert all(call.kwargs == {"static": True} for call in mock_log.call_args_list) diff --git a/docs/development/testing.md b/docs/development/testing.md index 868c073860..0de17adc77 100644 --- a/docs/development/testing.md +++ b/docs/development/testing.md @@ -69,39 +69,6 @@ When writing or debugging a specific self-hosted test, override `-m` yourself to pytest -m self_hosted dimos/path/to/test_something.py ``` -### Cross-simulator system tests - -DimOS system acceptance scenarios live in `dimos/e2e_tests/` and run against -each supported simulator through the `SceneControl` provider contract. The -test owns the command, setup, public completion assertion, and scoring rule; -the provider owns only simulator-specific control and test-oracle queries. - -Select the provider without changing the test body: - -```bash -DIMOS_TRANSPORT=zenoh DIMOS_E2E_SIMULATOR=pimsim \ - pytest -o addopts='' -m self_hosted_large \ - dimos/e2e_tests/test_dimsim_spatial_memory.py - -DIMOS_TRANSPORT=lcm DIMOS_E2E_SIMULATOR=dimsim \ - pytest -o addopts='' -m self_hosted_large \ - dimos/e2e_tests/test_dimsim_spatial_memory.py -``` - -The scenario body and acceptance contract are provider-neutral, but native -DimSim's browser bridge is currently LCM-only. PimSim is the maintained Zenoh -path. Selecting native DimSim with Zenoh is rejected immediately instead of -waiting for odometry that cannot arrive. - -Semantic scene metadata may be used by the test after the task as ground truth -for scoring. It must not be published to the robot or substituted for its -camera, perception, mapping, memory, or navigation inputs. - -DimSim's JavaScript eval harness is a separate simulator-local layer for checks -that inherently require privileged Three.js or Rapier state. Its `task` field -is only a display label and is not delivered to DimOS, so agent and robot -workflows must not be owned or duplicated there. - ## Testing on a fresh Ubuntu install CI tests dimos with pre-built images and cached deps, so it can't catch gaps diff --git a/misc/DimSim/README.md b/misc/DimSim/README.md index 57723ece26..affe85a50c 100644 --- a/misc/DimSim/README.md +++ b/misc/DimSim/README.md @@ -5,8 +5,8 @@ Browser-based 3D simulator (Three.js + Rapier) plus a Deno bridge that talks LCM ``` src/ — browser engine (vite-bundled) cli/ — Deno CLI + bridge server + headless launcher + LCM vendor -evals/ — simulator-local harness (browser) + runner (Deno) + rubrics -scenes/ — user-authored scenes (JS) + optional engine-level workflows +evals/ — eval harness (browser) + runner (Deno) + rubrics +scenes/ — user-authored scenes (JS) + per-scene eval workflows public/ — static assets (agent GLB, logo) docs/ — guides ``` @@ -26,7 +26,7 @@ On first run, `cli/cli.ts` will build `dist/` via Vite (dimsim ships its fronten - [docs/getting-started.md](docs/getting-started.md) — 5-minute tour - [docs/scenes.md](docs/scenes.md) — create + edit scenes -- [docs/evals.md](docs/evals.md) — simulator-local workflows vs DimOS system tests +- [docs/evals.md](docs/evals.md) — write eval workflows ## Install the CLI (optional) @@ -42,8 +42,8 @@ After install: ```bash dimsim dev --scene apartment # standalone dev server + browser dimsim eval list # list workflows under scenes/*/evals/ -dimsim eval # run a simulator-local workflow -dimsim eval --headless --scene apartment --workflow +dimsim eval go-to-couch # run one workflow against an open sim +dimsim eval --headless --scene apartment # full headless run (CI) ``` ## Build manually diff --git a/misc/DimSim/cli/cli.ts b/misc/DimSim/cli/cli.ts index 41038dfc9a..7a5d6a79c0 100644 --- a/misc/DimSim/cli/cli.ts +++ b/misc/DimSim/cli/cli.ts @@ -303,9 +303,9 @@ async function main() { // ── Eval ──────────────────────────────────────────────────────────── if (subcommand === "eval") { - // Positional workflow: `dimsim eval scene-smoke` is shorthand for - // `dimsim eval --workflow scene-smoke --connect`. Accepts either bare - // workflow name ("scene-smoke") or scene-qualified ("apartment/scene-smoke"). + // Positional workflow: `dimsim eval go-to-tv` is shorthand for + // `dimsim eval --workflow go-to-tv --connect`. Accepts either bare + // workflow name ("go-to-tv") or scene-qualified ("apartment/go-to-tv"). const positional = Deno.args[1] && !Deno.args[1].startsWith("--") ? Deno.args[1] : null; let posScene: string | undefined; let posWorkflow: string | undefined; diff --git a/misc/DimSim/docs/evals.md b/misc/DimSim/docs/evals.md index c210002fe1..79615045b6 100644 --- a/misc/DimSim/docs/evals.md +++ b/misc/DimSim/docs/evals.md @@ -1,22 +1,18 @@ -# Simulator-local evals +# Evals -DimSim's JavaScript eval harness is for simulator-local engine and scene checks. It has privileged access to Three.js objects, Rapier, and the agent pose. The `task` field is a display and reporting label; it is **not** sent to a DimOS agent. - -End-to-end robot tasks belong in `dimos/e2e_tests/`. Those tests send commands through DimOS, observe public DimOS streams, and run unchanged against each simulator through the `SceneControl` provider contract. Do not duplicate a system acceptance scenario as a JavaScript workflow. - -A simulator-local workflow is one JS file at `scenes//evals/.js`. It imports `runEval` from `@dimsim/eval` and calls it. +An eval workflow is one JS file at `scenes//evals/.js`. It imports `runEval` from `@dimsim/eval` and calls it. That's the whole authoring surface. ## Create a new eval ```js -// scenes/apartment/evals/sectional-bounds-smoke.js +// scenes/apartment/evals/go-to-couch.js import { runEval } from '@dimsim/eval'; await runEval({ scene: 'apartment', - task: 'Sectional object-distance rubric resolves live bounds', + task: 'Go to the couch', timeoutSec: 30, - startPose: { x: 4, y: 0.5, z: 1, yaw: 0 }, + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, success: (ctx) => ctx.rubrics.objectDistance({ target: 'sectional', thresholdM: 2.0 }), }); ``` @@ -26,9 +22,9 @@ Drop the file under any scene's `evals/` folder and `dimsim eval list` picks it ## Run it ```bash -dimsim eval sectional-bounds-smoke -dimsim eval --headless --scene apartment --workflow sectional-bounds-smoke -deno run -A misc/DimSim/scenes/apartment/evals/sectional-bounds-smoke.js +dimsim eval go-to-couch # against the open sim +dimsim eval --headless --scene apartment --workflow go-to-couch # standalone / CI +deno run -A misc/DimSim/scenes/apartment/evals/go-to-couch.js # direct execution ``` All three end up at the same harness in the browser. Pick whichever fits the moment. @@ -38,7 +34,7 @@ All three end up at the same harness in the browser. Pick whichever fits the mom | Field | Required | Description | |---|---|---| | `scene` | ✓ | Scene name. Must match a directory under `scenes/`. | -| `task` | ✓ | Human-readable label shown in the overlay and logs. It is not delivered to DimOS. | +| `task` | ✓ | Human-readable goal. Shown in the overlay + logged. | | `success(ctx)` | ✓ | Returns `{passed, reason?, score?}`. Polled every 250 ms until it passes or timeout. | | `timeoutSec` | – | Default 120. Wall-clock cap. | | `startPose` | – | `{x, y, z, yaw?}`, applied before `setup`. Yaw in degrees. | @@ -93,7 +89,6 @@ You can spawn obstacles, change embodiments mid-eval, or set up multi-stage test ## Tips -- Use pytest for agent, perception, mapping, planning, transport, or robot behavior. Use this harness only when the assertion inherently needs browser-local scene or engine state. - One eval at a time. The harness is a singleton, so running two evals concurrently isn't supported. Use `--parallel N` with multiple browser pages for throughput. - Score is yours to define. Lower-is-better for distances, higher-is-better for coverage. CI consumers should not assume. - `startPose` yaw is in degrees, not radians. diff --git a/misc/DimSim/evals/deno-client.ts b/misc/DimSim/evals/deno-client.ts index c3857be634..4bf67d1563 100644 --- a/misc/DimSim/evals/deno-client.ts +++ b/misc/DimSim/evals/deno-client.ts @@ -50,7 +50,7 @@ function _resolveWorkflowUrl(): string { `@dimsim/eval: workflow file must live under a 'scenes/' directory; got ${abs}`, ); } - return abs.slice(i); // e.g. "/scenes/apartment/evals/scene-smoke.js" + return abs.slice(i); // e.g. "/scenes/apartment/evals/go-to-couch.js" } /** Open the control WebSocket, race resolve / error / 5s timeout. */ diff --git a/misc/DimSim/scenes/apartment/evals/go-to-couch.js b/misc/DimSim/scenes/apartment/evals/go-to-couch.js new file mode 100644 index 0000000000..30b36db952 --- /dev/null +++ b/misc/DimSim/scenes/apartment/evals/go-to-couch.js @@ -0,0 +1,9 @@ +import { runEval } from '@dimsim/eval'; + +await runEval({ + scene: 'apartment', + task: 'Go to the couch', + timeoutSec: 30, + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + success: (ctx) => ctx.rubrics.objectDistance({ target: 'sectional', thresholdM: 2.0 }), +}); diff --git a/misc/DimSim/scenes/apartment/evals/go-to-kitchen.js b/misc/DimSim/scenes/apartment/evals/go-to-kitchen.js new file mode 100644 index 0000000000..5165406385 --- /dev/null +++ b/misc/DimSim/scenes/apartment/evals/go-to-kitchen.js @@ -0,0 +1,9 @@ +import { runEval } from '@dimsim/eval'; + +await runEval({ + scene: 'apartment', + task: 'Go to the kitchen', + timeoutSec: 30, + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + success: (ctx) => ctx.rubrics.objectDistance({ target: 'refrigerator', thresholdM: 3.0 }), +}); diff --git a/misc/DimSim/scenes/apartment/evals/go-to-tv.js b/misc/DimSim/scenes/apartment/evals/go-to-tv.js new file mode 100644 index 0000000000..c9800d80e5 --- /dev/null +++ b/misc/DimSim/scenes/apartment/evals/go-to-tv.js @@ -0,0 +1,9 @@ +import { runEval } from '@dimsim/eval'; + +await runEval({ + scene: 'apartment', + task: 'Go to the TV', + timeoutSec: 30, + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + success: (ctx) => ctx.rubrics.objectDistance({ target: 'television', thresholdM: 2.0 }), +}); diff --git a/misc/DimSim/src/sceneEditor.ts b/misc/DimSim/src/sceneEditor.ts index d233248ac4..2736ed7430 100644 --- a/misc/DimSim/src/sceneEditor.ts +++ b/misc/DimSim/src/sceneEditor.ts @@ -377,7 +377,7 @@ export class SceneEditor { // loadScript only ever serves bundled scene / eval scripts, which live under // /scenes/ and are plain .js / .mjs ES modules (e.g. /scenes/apartment/index.js, - // /scenes/apartment/evals/scene-smoke.js). Anything outside this allowlist is + // /scenes/apartment/evals/go-to-kitchen.js). Anything outside this allowlist is // refused so a malicious WS peer cannot turn loadScript into an SSRF primitive. static readonly _SCRIPT_PATH_ALLOWLIST = /^\/scenes\/[A-Za-z0-9._/-]+\.(?:js|mjs)$/; From 4634598635760ffda292effd6f5c451468824255 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Wed, 5 Aug 2026 22:17:51 +0800 Subject: [PATCH 33/33] fix(sim): restore PimSim Go2 startup --- .../go2/blueprints/basic/go2_platform.py | 55 ++++++++++++++++ .../go2/blueprints/basic/test_go2_platform.py | 64 +++++++++++++++++++ .../go2/blueprints/basic/unitree_go2_basic.py | 17 ++++- dimos/visualization/rerun/bridge.py | 12 +++- .../rerun/test_detection3d_bridge.py | 15 +++++ 5 files changed, 158 insertions(+), 5 deletions(-) create mode 100644 dimos/robot/unitree/go2/blueprints/basic/go2_platform.py create mode 100644 dimos/robot/unitree/go2/blueprints/basic/test_go2_platform.py diff --git a/dimos/robot/unitree/go2/blueprints/basic/go2_platform.py b/dimos/robot/unitree/go2/blueprints/basic/go2_platform.py new file mode 100644 index 0000000000..d1ef72c734 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/basic/go2_platform.py @@ -0,0 +1,55 @@ +# 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. + +from functools import cache + +from dimos.core.coordination.blueprints import Blueprint +from dimos.core.global_config import global_config +from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.simulation.providers import ( + SimulationBinding, + SimulationRequest, + load_simulation_provider, +) + + +def resolve_go2_platform() -> Blueprint: + if global_config.simulation in ("", "dimsim"): + return GO2Connection.blueprint() + return _resolve_simulation_binding().backend + + +def resolve_go2_rerun_config() -> dict[str, object]: + if global_config.simulation in ("", "dimsim"): + return {} + return _resolve_simulation_binding().rerun_config + + +@cache +def _resolve_simulation_binding() -> SimulationBinding: + if global_config.simulation != "mujoco": + raise ValueError("unitree-go2 only supports --simulation mujoco") + if not global_config.simulation_provider: + raise ValueError("unitree-go2 simulation requires --simulation-provider pimsim") + + provider = load_simulation_provider(global_config.simulation_provider) + return provider.build( + SimulationRequest( + robot_model="unitree_go2", + scene_package=global_config.scene_package, + ) + ) + + +__all__ = ["resolve_go2_platform", "resolve_go2_rerun_config"] diff --git a/dimos/robot/unitree/go2/blueprints/basic/test_go2_platform.py b/dimos/robot/unitree/go2/blueprints/basic/test_go2_platform.py new file mode 100644 index 0000000000..491696bde3 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/basic/test_go2_platform.py @@ -0,0 +1,64 @@ +# 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. + +from collections.abc import Iterator +from pathlib import Path + +import pytest + +from dimos.core.coordination.blueprints import Blueprint +from dimos.core.global_config import global_config +from dimos.robot.unitree.go2.blueprints.basic import go2_platform +from dimos.simulation.providers import SimulationBinding, SimulationRequest + + +@pytest.fixture +def pimsim_go2_config(monkeypatch: pytest.MonkeyPatch) -> Iterator[None]: + monkeypatch.setattr(global_config, "simulation", "mujoco") + monkeypatch.setattr(global_config, "simulation_provider", "pimsim") + monkeypatch.setattr(global_config, "scene_package", "dimsim-apartment") + go2_platform._resolve_simulation_binding.cache_clear() + yield + go2_platform._resolve_simulation_binding.cache_clear() + + +def test_go2_platform_uses_requested_simulation_provider( + pimsim_go2_config: None, + mocker, +) -> None: + del pimsim_go2_config + backend = Blueprint(blueprints=()) + binding = SimulationBinding( + backend=backend, + adapter_type="pimsim_go2", + adapter_address=Path("/tmp/pimsim-go2"), + rerun_config={"static": {"world/scene": "scene"}}, + ) + provider = mocker.Mock() + provider.build.return_value = binding + load_provider = mocker.patch.object( + go2_platform, + "load_simulation_provider", + return_value=provider, + ) + + assert go2_platform.resolve_go2_platform() is backend + assert go2_platform.resolve_go2_rerun_config() == binding.rerun_config + load_provider.assert_called_once_with("pimsim") + provider.build.assert_called_once_with( + SimulationRequest( + robot_model="unitree_go2", + scene_package="dimsim-apartment", + ) + ) diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py index c342c74f92..b850f654d7 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py @@ -18,7 +18,10 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config -from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.robot.unitree.go2.blueprints.basic.go2_platform import ( + resolve_go2_platform, + resolve_go2_rerun_config, +) from dimos.visualization.vis_module import vis_module @@ -103,6 +106,16 @@ def _go2_rerun_blueprint() -> Any: }, } +_provider_rerun_config = resolve_go2_rerun_config() +for _section in ("static", "visual_override", "max_hz"): + rerun_config[_section] = { + **rerun_config.get(_section, {}), + **_provider_rerun_config.get(_section, {}), + } +for _key, _value in _provider_rerun_config.items(): + if _key not in {"static", "visual_override", "max_hz"}: + rerun_config[_key] = _value + _with_vis = autoconnect( vis_module( viewer_backend=global_config.viewer, @@ -114,7 +127,7 @@ def _go2_rerun_blueprint() -> Any: unitree_go2_basic = ( autoconnect( _with_vis, - GO2Connection.blueprint(), + resolve_go2_platform(), ).global_config(n_workers=4, robot_model="unitree_go2") # we temporarily disabled sensor timestamps # and are derriving all timestmaps upon reception diff --git a/dimos/visualization/rerun/bridge.py b/dimos/visualization/rerun/bridge.py index 25f16e0271..8fba9641a1 100644 --- a/dimos/visualization/rerun/bridge.py +++ b/dimos/visualization/rerun/bridge.py @@ -223,6 +223,7 @@ class Config(ModuleConfig): visual_override: dict[Glob | str, VisualOverride | None] = field(default_factory=dict) static: dict[str, Callable[[Any], Any]] = field(default_factory=dict) max_hz: dict[str, float] = field(default_factory=dict) + latest_state: set[str] = field(default_factory=set) entity_prefix: str = "world" topic_to_entity: Callable[[Any], str] | None = None @@ -352,15 +353,20 @@ def _on_message(self, msg: Any, topic: Any) -> None: # TFMessage for example returns list of (entity_path, archetype) tuples if is_rerun_multi(rerun_data): for path, archetype in rerun_data: - rr.log(path, archetype) + rr.log(path, archetype, static=path in self.config.latest_state) else: - rr.log(entity_path, cast("Archetype", rerun_data)) + latest_state = entity_path in self.config.latest_state + rr.log(entity_path, cast("Archetype", rerun_data), static=latest_state) # if source msg carries a frame_id, attach the entity to that TF frame # should skip if archetype is a Transform3D if not isinstance(rerun_data, rr.Transform3D): frame_id = getattr(msg, "frame_id", None) if frame_id and self._frame_attached.get(entity_path) != frame_id: - rr.log(entity_path, rr.Transform3D(parent_frame=f"tf#/{frame_id}")) + rr.log( + entity_path, + rr.Transform3D(parent_frame=f"tf#/{frame_id}"), + static=latest_state, + ) self._frame_attached[entity_path] = frame_id @rpc diff --git a/dimos/visualization/rerun/test_detection3d_bridge.py b/dimos/visualization/rerun/test_detection3d_bridge.py index 8385c6dfbf..46f8b4b86f 100644 --- a/dimos/visualization/rerun/test_detection3d_bridge.py +++ b/dimos/visualization/rerun/test_detection3d_bridge.py @@ -77,3 +77,18 @@ def test_detection3darray_bridge_attaches_topic_entity_to_message_frame() -> Non transform = mock_log.call_args_list[1].args[1] assert isinstance(transform, rr.Transform3D) assert transform.parent_frame.as_arrow_array().to_pylist() == ["tf#/world"] + + +def test_latest_state_entity_overwrites_data_and_frame_attachment() -> None: + entity_path = "world/marker_detection/detections" + bridge = RerunBridgeModule(latest_state={entity_path}) + bridge._min_intervals = {} + + try: + with patch("rerun.log") as mock_log: + bridge._on_message(_detection_array(), Topic("/marker_detection/detections")) + finally: + bridge.stop() + + assert mock_log.call_count == 2 + assert all(call.kwargs == {"static": True} for call in mock_log.call_args_list)