From 07c18f6a1015196d4612d64d2e109f666c5bdfb0 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Thu, 15 Jan 2026 13:45:36 -0800 Subject: [PATCH 01/43] add original files --- dimos/simulation/manipulators/__init__.py | 19 ++ .../manipulators/mujoco_sim_backend.py | 234 +++++++++++++++ dimos/simulation/manipulators/simulation.py | 277 +++++++++++++++++ .../manipulators/xarm_sim_driver.py | 283 ++++++++++++++++++ dimos/simulation/sim_blueprints.py | 46 +++ 5 files changed, 859 insertions(+) create mode 100644 dimos/simulation/manipulators/__init__.py create mode 100644 dimos/simulation/manipulators/mujoco_sim_backend.py create mode 100644 dimos/simulation/manipulators/simulation.py create mode 100644 dimos/simulation/manipulators/xarm_sim_driver.py create mode 100644 dimos/simulation/sim_blueprints.py diff --git a/dimos/simulation/manipulators/__init__.py b/dimos/simulation/manipulators/__init__.py new file mode 100644 index 0000000000..1c1d932910 --- /dev/null +++ b/dimos/simulation/manipulators/__init__.py @@ -0,0 +1,19 @@ +# Copyright 2025 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. + +"""MuJoCo simulation backend utilities.""" + +from dimos.simulation.manipulators.mujoco_sim_backend import MujocoSimBackend + +__all__ = ["MujocoSimBackend"] diff --git a/dimos/simulation/manipulators/mujoco_sim_backend.py b/dimos/simulation/manipulators/mujoco_sim_backend.py new file mode 100644 index 0000000000..2c6dfd173d --- /dev/null +++ b/dimos/simulation/manipulators/mujoco_sim_backend.py @@ -0,0 +1,234 @@ +# 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 pathlib import Path +import threading +import time + +import mujoco +import mujoco.viewer as viewer +from robot_descriptions.loaders.mujoco import load_robot_description + +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class MujocoSimBackend: + """ + Base class for MuJoCo simulation backend. + """ + + def __init__( + self, + robot: str, + config_path: str | None = None, + headless: bool = False, + ): + """ + Initialize the MuJoCo simulation backend. + + Args: + robot: Robot description (from robot_descriptions library) name (e.g., "piper", "xarm7_mj_description"). + config_path: Path to a MuJoCo XML or a folder containing scene.xml. + headless: Run without launching the MuJoCo GUI viewer. + """ + self._robot_name = robot + self._headless = headless + + if config_path: + resolved = Path(config_path).expanduser() + xml_path = resolved / "scene.xml" if resolved.is_dir() else resolved + if not xml_path.exists(): + raise FileNotFoundError(f"MuJoCo XML not found: {xml_path}") + self._model = mujoco.MjModel.from_xml_path(str(xml_path)) + else: + self._model = load_robot_description(robot) + self._data = mujoco.MjData(self._model) + self._num_joints = self._model.nq + timestep = float(self._model.opt.timestep) + self._control_frequency = 1.0 / timestep if timestep > 0.0 else 100.0 + + self._connected: bool = False + + self._lock = threading.Lock() + self._stop_event = threading.Event() + self._sim_thread: threading.Thread | None = None + + self._joint_positions = [0.0] * self._num_joints + self._joint_velocities = [0.0] * self._num_joints + self._joint_efforts = [0.0] * self._num_joints + + self._joint_position_targets = [0.0] * self._num_joints + + for i in range(min(self._num_joints, self._model.nq)): + current_pos = float(self._data.qpos[i]) + self._joint_position_targets[i] = current_pos + self._joint_positions[i] = current_pos + + def _apply_control(self) -> None: + """ + Apply control commands to MuJoCo actuators. + + Default implementation applies joint position targets directly to actuators. + """ + with self._lock: + pos_targets = list(self._joint_position_targets) + n_act = min(self._num_joints, self._model.nu) + with self._lock: + for i in range(n_act): + self._data.ctrl[i] = pos_targets[i] + + def _update_joint_state(self) -> None: + """ + Update internal joint state from MuJoCo simulation. + + Default implementation mirrors MuJoCo state directly. + """ + with self._lock: + n_q = min(self._num_joints, self._model.nq) + n_v = min(self._num_joints, self._model.nv) + n_act = min(self._num_joints, self._model.nu) + for i in range(n_q): + self._joint_positions[i] = float(self._data.qpos[i]) + for i in range(n_v): + self._joint_velocities[i] = float(self._data.qvel[i]) + for i in range(n_act): + self._joint_efforts[i] = float(self._data.qfrc_actuator[i]) + + def connect(self) -> None: + """Connect to simulation and start the simulation loop.""" + logger.info(f"{self.__class__.__name__}: connect()") + with self._lock: + self._connected = True + self._stop_event.clear() + + if self._sim_thread is None or not self._sim_thread.is_alive(): + self._sim_thread = threading.Thread( + target=self._sim_loop, + name=f"{self.__class__.__name__}Sim", + daemon=True, + ) + self._sim_thread.start() + + def disconnect(self) -> None: + """Disconnect from simulation and stop the simulation loop.""" + logger.info(f"{self.__class__.__name__}: disconnect()") + with self._lock: + self._connected = False + + self._stop_event.set() + if self._sim_thread and self._sim_thread.is_alive(): + self._sim_thread.join(timeout=2.0) + self._sim_thread = None + + def _sim_loop(self) -> None: + """ + Main simulation loop running MuJoCo. + + This method: + 1. Launches the MuJoCo viewer + 2. Runs the simulation at the specified control frequency + 3. Calls `_apply_control()` to apply control commands + 4. Steps the simulation + 5. Calls `_update_joint_state()` to update internal state + """ + logger.info(f"{self.__class__.__name__}: sim loop started") + dt = 1.0 / self._control_frequency + + def _step_once(sync_viewer: bool) -> None: + loop_start = time.time() + self._apply_control() + mujoco.mj_step(self._model, self._data) + if sync_viewer: + m_viewer.sync() + self._update_joint_state() + + elapsed = time.time() - loop_start + sleep_time = dt - elapsed + if sleep_time > 0: + time.sleep(sleep_time) + + if self._headless: + while not self._stop_event.is_set(): + _step_once(sync_viewer=False) + else: + with viewer.launch_passive( + self._model, self._data, show_left_ui=False, show_right_ui=False + ) as m_viewer: + while m_viewer.is_running() and not self._stop_event.is_set(): + _step_once(sync_viewer=True) + + logger.info(f"{self.__class__.__name__}: sim loop stopped") + + @property + def connected(self) -> bool: + """Whether the backend is connected to simulation.""" + with self._lock: + return self._connected + + @property + def num_joints(self) -> int: + """Number of joints in the robot.""" + return self._num_joints + + @property + def model(self) -> mujoco.MjModel: + """MuJoCo model (read-only).""" + return self._model + + @property + def data(self) -> mujoco.MjData: + """MuJoCo data (read-only).""" + return self._data + + @property + def joint_positions(self) -> list[float]: + """Current joint positions in radians (thread-safe copy).""" + with self._lock: + return list(self._joint_positions) + + @property + def joint_velocities(self) -> list[float]: + """Current joint velocities in rad/s (thread-safe copy).""" + with self._lock: + return list(self._joint_velocities) + + @property + def joint_efforts(self) -> list[float]: + """Current joint efforts/torques (thread-safe copy).""" + with self._lock: + return list(self._joint_efforts) + + @property + def control_frequency(self) -> float: + """Control loop frequency derived from the MuJoCo model.""" + return self._control_frequency + + def set_joint_position_targets(self, positions: list[float]) -> None: + """Set joint position targets in radians.""" + with self._lock: + limit = min(len(positions), self._num_joints) + for i in range(limit): + self._joint_position_targets[i] = float(positions[i]) + + def hold_current_position(self) -> None: + """Lock joints at their current positions.""" + with self._lock: + for i in range(min(self._num_joints, self._model.nq)): + current_pos = float(self._data.qpos[i]) + self._joint_position_targets[i] = current_pos diff --git a/dimos/simulation/manipulators/simulation.py b/dimos/simulation/manipulators/simulation.py new file mode 100644 index 0000000000..d78775259c --- /dev/null +++ b/dimos/simulation/manipulators/simulation.py @@ -0,0 +1,277 @@ +# 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. + +"""Robot-agnostic MuJoCo simulation driver.""" + +import logging +import math +from typing import Any + +from dimos.hardware.manipulators.base import ( + BaseManipulatorDriver, + StandardMotionComponent, + StandardServoComponent, + StandardStatusComponent, +) +from dimos.hardware.manipulators.base.sdk_interface import BaseManipulatorSDK, ManipulatorInfo +from dimos.simulation.manipulators import MujocoSimBackend + +logger = logging.getLogger(__name__) + + +class SimSDKWrapper(BaseManipulatorSDK): + """SDK wrapper for a generic MuJoCo simulation backend.""" + + def __init__(self) -> None: + self.logger = logging.getLogger(self.__class__.__name__) + self.native_sdk: MujocoSimBackend | None = None + self.dof = 0 + self._connected = False + self._servos_enabled = False + self._mode = 0 + self._state = 0 + self._robot = "unknown" + + def connect(self, config: dict[str, Any]) -> bool: + """Connect to the MuJoCo simulation backend.""" + try: + robot = config.get("robot") + if not robot: + raise ValueError("robot is required for MuJoCo simulation loading") + self._robot = str(robot) + config_path = config.get("config_path") + headless = bool(config.get("headless", False)) + + self.logger.info("Connecting to MuJoCo Sim...") + self.native_sdk = MujocoSimBackend( + robot=self._robot, + config_path=config_path, + headless=headless, + ) + self.native_sdk.connect() + + if self.native_sdk.connected: + self._connected = True + self._servos_enabled = True + self._state = 0 + self._mode = 0 + self.dof = int(self.native_sdk.num_joints) + self.logger.info("Successfully connected to MuJoCo Sim", extra={"dof": self.dof}) + return True + + self.logger.error("Failed to connect to MuJoCo Sim") + return False + except Exception as exc: + self.logger.error(f"Sim connection failed: {exc}") + return False + + def disconnect(self) -> None: + """Disconnect from simulation.""" + if self.native_sdk: + try: + self.native_sdk.disconnect() + finally: + self._connected = False + self.native_sdk = None + + def is_connected(self) -> bool: + return bool(self._connected and self.native_sdk and self.native_sdk.connected) + + def get_joint_positions(self) -> list[float]: + return self.native_sdk.joint_positions[: self.dof] if self.native_sdk else [] + + def get_joint_velocities(self) -> list[float]: + return self.native_sdk.joint_velocities[: self.dof] if self.native_sdk else [] + + def get_joint_efforts(self) -> list[float]: + return self.native_sdk.joint_efforts[: self.dof] if self.native_sdk else [] + + def set_joint_positions( + self, + positions: list[float], + _velocity: float = 1.0, + _acceleration: float = 1.0, + _wait: bool = False, + ) -> bool: + _ = _velocity + _ = _acceleration + _ = _wait + if not self._servos_enabled or not self.native_sdk: + return False + self._mode = 0 + self.native_sdk.set_joint_position_targets(positions[: self.dof]) + return True + + def set_joint_velocities(self, velocities: list[float]) -> bool: + if not self._servos_enabled or not self.native_sdk: + return False + self._mode = 1 + dt = 1.0 / self.native_sdk.control_frequency + current = self.native_sdk.joint_positions + targets = [current[i] + velocities[i] * dt for i in range(min(len(velocities), self.dof))] + self.native_sdk.set_joint_position_targets(targets) + return True + + def set_joint_efforts(self, efforts: list[float]) -> bool: + self.logger.warning("Torque control not supported in MuJoCo Sim bridge") + _ = efforts + return False + + def stop_motion(self) -> bool: + if not self.native_sdk: + return False + self.native_sdk.hold_current_position() + self._state = 0 + return True + + def enable_servos(self) -> bool: + self._servos_enabled = True + self._state = 0 + return True + + def disable_servos(self) -> bool: + self._servos_enabled = False + return True + + def are_servos_enabled(self) -> bool: + return self._servos_enabled + + def get_robot_state(self) -> dict[str, Any]: + velocities = self.native_sdk.joint_velocities[: self.dof] if self.native_sdk else [] + is_moving = any(abs(v) > 1e-4 for v in velocities) + self._state = 1 if is_moving else 0 + return { + "state": self._state, + "mode": self._mode, + "error_code": 0, + "is_moving": is_moving, + } + + def get_error_code(self) -> int: + return 0 + + def get_error_message(self) -> str: + return "" + + def clear_errors(self) -> bool: + self._state = 0 + return True + + def emergency_stop(self) -> bool: + if self.native_sdk: + self.native_sdk.hold_current_position() + self._state = 3 + return True + + def get_info(self) -> ManipulatorInfo: + return ManipulatorInfo( + vendor="MuJoCo", + model=self._robot, + dof=self.dof, + firmware_version=None, + serial_number=None, + ) + + def get_joint_limits(self) -> tuple[list[float], list[float]]: + if not self.native_sdk: + return ([], []) + ranges = getattr(self.native_sdk.model, "jnt_range", None) + if ranges is None or len(ranges) == 0: + lower = [-math.pi] * self.dof + upper = [math.pi] * self.dof + return (lower, upper) + limit = min(len(ranges), self.dof) + lower = [float(ranges[i][0]) for i in range(limit)] + upper = [float(ranges[i][1]) for i in range(limit)] + if limit < self.dof: + lower.extend([-math.pi] * (self.dof - limit)) + upper.extend([math.pi] * (self.dof - limit)) + return (lower, upper) + + def get_velocity_limits(self) -> list[float]: + max_vel_rad = math.radians(180.0) + return [max_vel_rad] * self.dof + + def get_acceleration_limits(self) -> list[float]: + max_acc_rad = math.radians(1145.0) + return [max_acc_rad] * self.dof + + +class SimDriver(BaseManipulatorDriver): + """Generic manipulator driver backed by MuJoCo.""" + + def __init__(self, **kwargs: Any) -> None: + config: dict[str, Any] = kwargs.pop("config", {}) + + driver_params = [ + "robot", + "config_path", + "headless", + ] + for param in driver_params: + if param in kwargs: + config[param] = kwargs.pop(param) + + logger.info(f"Initializing SimDriver with config: {config}") + + sdk = SimSDKWrapper() + components = [ + StandardMotionComponent(sdk), + StandardServoComponent(sdk), + StandardStatusComponent(sdk), + ] + + kwargs.pop("sdk", None) + kwargs.pop("components", None) + kwargs.pop("name", None) + + super().__init__( + sdk=sdk, + components=components, + config=config, + name="SimDriver", + **kwargs, + ) + + logger.info("SimDriver initialized successfully") + + +def get_blueprint() -> dict[str, Any]: + return { + "name": "SimDriver", + "class": SimDriver, + "config": { + "robot": None, + "config_path": None, + "headless": False, + }, + "inputs": { + "joint_position_command": "JointCommand", + "joint_velocity_command": "JointCommand", + }, + "outputs": { + "joint_state": "JointState", + "robot_state": "RobotState", + }, + } + + +simulation = SimDriver.blueprint + +__all__ = [ + "SimDriver", + "SimSDKWrapper", + "get_blueprint", + "simulation", +] diff --git a/dimos/simulation/manipulators/xarm_sim_driver.py b/dimos/simulation/manipulators/xarm_sim_driver.py new file mode 100644 index 0000000000..292018e11b --- /dev/null +++ b/dimos/simulation/manipulators/xarm_sim_driver.py @@ -0,0 +1,283 @@ +# 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. + +"""MuJoCo-native xArm simulation driver and SDK wrapper.""" + +import logging +import math +from typing import Any + +from dimos.hardware.manipulators.base import ( + BaseManipulatorDriver, + StandardMotionComponent, + StandardServoComponent, + StandardStatusComponent, +) +from dimos.hardware.manipulators.base.sdk_interface import BaseManipulatorSDK, ManipulatorInfo +from dimos.simulation.manipulators import MujocoSimBackend + +logger = logging.getLogger(__name__) + + +class XArmSimSDKWrapper(BaseManipulatorSDK): + """SDK wrapper for xArm simulation using the MuJoCo simulation backend.""" + + def __init__(self) -> None: + self.logger = logging.getLogger(self.__class__.__name__) + self.native_sdk: MujocoSimBackend | None = None + self.dof = 7 + self._connected = False + self._servos_enabled = False + self._mode = 0 + self._state = 0 + + def connect(self, config: dict[str, Any]) -> bool: + """Connect to the MuJoCo simulation backend.""" + try: + robot = config.get("robot") + if not robot: + raise ValueError("robot is required for MuJoCo simulation loading") + config_path = config.get("config_path") + headless = bool(config.get("headless", False)) + + self.logger.info("Connecting to MuJoCo Sim...") + self.native_sdk = MujocoSimBackend( + robot=robot, + config_path=config_path, + headless=headless, + ) + self.native_sdk.connect() + + if self.native_sdk.connected: + self._connected = True + self._servos_enabled = True + self._state = 0 + self._mode = 0 + self.dof = int(config.get("dof", self.native_sdk.num_joints)) + self.logger.info(f"Successfully connected to MuJoCo Sim (DOF: {self.dof})") + return True + + self.logger.error("Failed to connect to XArm Sim") + return False + except Exception as exc: + self.logger.error(f"Sim connection failed: {exc}") + return False + + def disconnect(self) -> None: + """Disconnect from simulation.""" + if self.native_sdk: + try: + self.native_sdk.disconnect() + finally: + self._connected = False + self.native_sdk = None + + def is_connected(self) -> bool: + return bool(self._connected and self.native_sdk and self.native_sdk.connected) + + def get_joint_positions(self) -> list[float]: + return self.native_sdk.joint_positions[: self.dof] + + def get_joint_velocities(self) -> list[float]: + return self.native_sdk.joint_velocities[: self.dof] + + def get_joint_efforts(self) -> list[float]: + return self.native_sdk.joint_efforts[: self.dof] + + def set_joint_positions( + self, + positions: list[float], + _velocity: float = 1.0, + _acceleration: float = 1.0, + _wait: bool = False, + ) -> bool: + _ = _velocity + _ = _acceleration + _ = _wait + if not self._servos_enabled: + return False + self._mode = 0 + self.native_sdk.set_joint_position_targets(positions[: self.dof]) + return True + + def set_joint_velocities(self, velocities: list[float]) -> bool: + if not self._servos_enabled: + return False + self._mode = 1 + dt = 1.0 / self.native_sdk.control_frequency + current = self.native_sdk.joint_positions + targets = [current[i] + velocities[i] * dt for i in range(min(len(velocities), self.dof))] + self.native_sdk.set_joint_position_targets(targets) + return True + + def set_joint_efforts(self, efforts: list[float]) -> bool: + self.logger.warning("Torque control not supported in MuJoCo Sim bridge") + _ = efforts + return False + + def stop_motion(self) -> bool: + self.native_sdk.hold_current_position() + self._state = 0 + return True + + def enable_servos(self) -> bool: + self._servos_enabled = True + self._state = 0 + return True + + def disable_servos(self) -> bool: + self._servos_enabled = False + return True + + def are_servos_enabled(self) -> bool: + return self._servos_enabled + + def get_robot_state(self) -> dict[str, Any]: + is_moving = any(abs(v) > 1e-4 for v in self.native_sdk.joint_velocities[: self.dof]) + self._state = 1 if is_moving else 0 + return { + "state": self._state, + "mode": self._mode, + "error_code": 0, + "warn_code": 0, + "is_moving": is_moving, + "cmd_num": 0, + } + + def get_error_code(self) -> int: + return 0 + + def get_error_message(self) -> str: + return "" + + def clear_errors(self) -> bool: + self._state = 0 + return True + + def emergency_stop(self) -> bool: + self._state = 3 + self.native_sdk.hold_current_position() + return True + + def get_info(self) -> ManipulatorInfo: + return ManipulatorInfo( + vendor="UFACTORY", + model=f"xArm{self.dof}", + dof=self.dof, + firmware_version=None, + serial_number=None, + ) + + def get_joint_limits(self) -> tuple[list[float], list[float]]: + if self.dof == 7: + lower_deg = [-360, -118, -360, -233, -360, -97, -360] + upper_deg = [360, 118, 360, 11, 360, 180, 360] + elif self.dof == 6: + lower_deg = [-360, -118, -225, -11, -360, -97] + upper_deg = [360, 118, 11, 225, 360, 180] + else: + lower_deg = [-360, -118, -225, -97, -360] + upper_deg = [360, 118, 11, 180, 360] + + lower_rad = [math.radians(d) for d in lower_deg[: self.dof]] + upper_rad = [math.radians(d) for d in upper_deg[: self.dof]] + return (lower_rad, upper_rad) + + def get_velocity_limits(self) -> list[float]: + max_vel_rad = math.radians(180.0) + return [max_vel_rad] * self.dof + + def get_acceleration_limits(self) -> list[float]: + max_acc_rad = math.radians(1145.0) + return [max_acc_rad] * self.dof + + +class XArmSimDriver(BaseManipulatorDriver): + """xArm driver backed by the MuJoCo simulation bridge.""" + + def __init__(self, **kwargs: Any) -> None: + config: dict[str, Any] = kwargs.pop("config", {}) + + driver_params = [ + "dof", + "has_gripper", + "has_force_torque", + "control_rate", + "monitor_rate", + "robot", + "config_path", + "headless", + ] + for param in driver_params: + if param in kwargs: + config[param] = kwargs.pop(param) + + logger.info(f"Initializing XArmSimDriver with config: {config}") + + sdk = XArmSimSDKWrapper() + sdk.dof = int(config.get("dof", sdk.dof)) + components = [ + StandardMotionComponent(sdk), + StandardServoComponent(sdk), + StandardStatusComponent(sdk), + ] + + kwargs.pop("sdk", None) + kwargs.pop("components", None) + kwargs.pop("name", None) + + super().__init__( + sdk=sdk, + components=components, + config=config, + name="XArmSimDriver", + **kwargs, + ) + + logger.info("XArmSimDriver initialized successfully") + + +def get_blueprint() -> dict[str, Any]: + return { + "name": "XArmSimDriver", + "class": XArmSimDriver, + "config": { + "dof": 7, + "has_gripper": False, + "has_force_torque": False, + "control_rate": 100, + "monitor_rate": 10, + "robot": None, + "config_path": None, + "headless": False, + }, + "inputs": { + "joint_position_command": "JointCommand", + "joint_velocity_command": "JointCommand", + }, + "outputs": { + "joint_state": "JointState", + "robot_state": "RobotState", + }, + } + + +xarm_sim_driver = XArmSimDriver.blueprint + +__all__ = [ + "XArmSimDriver", + "XArmSimSDKWrapper", + "get_blueprint", + "xarm_sim_driver", +] diff --git a/dimos/simulation/sim_blueprints.py b/dimos/simulation/sim_blueprints.py new file mode 100644 index 0000000000..f5ad7ead73 --- /dev/null +++ b/dimos/simulation/sim_blueprints.py @@ -0,0 +1,46 @@ +# 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.core.blueprints import autoconnect +from dimos.core.transport import LCMTransport +from dimos.msgs.sensor_msgs import ( # type: ignore[attr-defined] + JointCommand, + JointState, + RobotState, +) +from dimos.msgs.trajectory_msgs import JointTrajectory +from dimos.simulation.manipulators.simulation import simulation + +xarm7_trajectory_sim = simulation( + robot="xarm7_mj_description", + config_path=None, + headless=False, +).transports( + { + ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), + ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), + ("joint_position_command", JointCommand): LCMTransport( + "/xarm/joint_position_command", JointCommand + ), + ("trajectory", JointTrajectory): LCMTransport("/trajectory", JointTrajectory), + } +) + + +__all__ = [ + "xarm7_trajectory_sim", +] + +if __name__ == "__main__": + xarm7_trajectory_sim.build().loop() From 4821255aced7e33020815b25de6771c1670e17f5 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Thu, 15 Jan 2026 13:56:36 -0800 Subject: [PATCH 02/43] rebased with dev for new controller orchestrator arch --- dimos/simulation/manipulators/sim_backend.py | 257 ++++++++++++++++ dimos/simulation/manipulators/sim_driver.py | 241 +++++++++++++++ dimos/simulation/manipulators/simulation.py | 277 ------------------ .../manipulators/xarm_sim_driver.py | 258 ++++------------ dimos/simulation/sim_blueprints.py | 4 +- 5 files changed, 554 insertions(+), 483 deletions(-) create mode 100644 dimos/simulation/manipulators/sim_backend.py create mode 100644 dimos/simulation/manipulators/sim_driver.py delete mode 100644 dimos/simulation/manipulators/simulation.py diff --git a/dimos/simulation/manipulators/sim_backend.py b/dimos/simulation/manipulators/sim_backend.py new file mode 100644 index 0000000000..ab8184d7fd --- /dev/null +++ b/dimos/simulation/manipulators/sim_backend.py @@ -0,0 +1,257 @@ +# 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. + +"""Robot-agnostic MuJoCo simulation backend.""" + +import logging +import math + +from dimos.hardware.manipulators.spec import ControlMode, JointLimits, ManipulatorInfo +from dimos.simulation.manipulators import MujocoSimBackend + + +class SimBackend: + """Backend wrapper for a generic MuJoCo simulation.""" + + def __init__( + self, + robot: str, + config_path: str | None = None, + headless: bool = False, + dof: int | None = None, + ) -> None: + self.logger = logging.getLogger(self.__class__.__name__) + self._robot = robot + self._config_path = config_path + self._headless = headless + self._native: MujocoSimBackend | None = None + self._dof = int(dof) if dof is not None else 0 + self._connected = False + self._servos_enabled = False + self._control_mode = ControlMode.POSITION + self._error_code = 0 + self._error_message = "" + + # ========================================================================= + # Connection + # ========================================================================= + + def connect(self) -> bool: + """Connect to the MuJoCo simulation backend.""" + if not self._robot: + raise ValueError("robot is required for MuJoCo simulation loading") + try: + self.logger.info("Connecting to MuJoCo Sim...") + self._native = MujocoSimBackend( + robot=self._robot, + config_path=self._config_path, + headless=self._headless, + ) + self._native.connect() + + if self._native.connected: + self._connected = True + self._servos_enabled = True + if self._dof <= 0: + self._dof = int(self._native.num_joints) + self.logger.info("Successfully connected to MuJoCo Sim", extra={"dof": self._dof}) + return True + + self.logger.error("Failed to connect to MuJoCo Sim") + return False + except Exception as exc: + self.logger.error(f"Sim connection failed: {exc}") + return False + + def disconnect(self) -> None: + """Disconnect from simulation.""" + if self._native: + try: + self._native.disconnect() + finally: + self._connected = False + self._native = None + + def is_connected(self) -> bool: + return bool(self._connected and self._native and self._native.connected) + + # ========================================================================= + # Info + # ========================================================================= + + def get_info(self) -> ManipulatorInfo: + return ManipulatorInfo( + vendor="MuJoCo", + model=self._robot, + dof=self._dof, + firmware_version=None, + serial_number=None, + ) + + def get_dof(self) -> int: + return self._dof + + def get_limits(self) -> JointLimits: + if not self._native: + lower = [-math.pi] * self._dof + upper = [math.pi] * self._dof + max_vel_rad = math.radians(180.0) + return JointLimits( + position_lower=lower, + position_upper=upper, + velocity_max=[max_vel_rad] * self._dof, + ) + ranges = getattr(self._native.model, "jnt_range", None) + if ranges is None or len(ranges) == 0: + lower = [-math.pi] * self._dof + upper = [math.pi] * self._dof + else: + limit = min(len(ranges), self._dof) + lower = [float(ranges[i][0]) for i in range(limit)] + upper = [float(ranges[i][1]) for i in range(limit)] + if limit < self._dof: + lower.extend([-math.pi] * (self._dof - limit)) + upper.extend([math.pi] * (self._dof - limit)) + max_vel_rad = math.radians(180.0) + return JointLimits( + position_lower=lower, + position_upper=upper, + velocity_max=[max_vel_rad] * self._dof, + ) + + # ========================================================================= + # Control Mode + # ========================================================================= + + def set_control_mode(self, mode: ControlMode) -> bool: + self._control_mode = mode + return True + + def get_control_mode(self) -> ControlMode: + return self._control_mode + + # ========================================================================= + # State Reading + # ========================================================================= + + def read_joint_positions(self) -> list[float]: + if self._native: + return self._native.joint_positions[: self._dof] + return [0.0] * self._dof + + def read_joint_velocities(self) -> list[float]: + if self._native: + return self._native.joint_velocities[: self._dof] + return [0.0] * self._dof + + def read_joint_efforts(self) -> list[float]: + if self._native: + return self._native.joint_efforts[: self._dof] + return [0.0] * self._dof + + def read_state(self) -> dict[str, int]: + velocities = self.read_joint_velocities() + is_moving = any(abs(v) > 1e-4 for v in velocities) + mode_int = list(ControlMode).index(self._control_mode) + return { + "state": 1 if is_moving else 0, + "mode": mode_int, + } + + def read_error(self) -> tuple[int, str]: + return self._error_code, self._error_message + + # ========================================================================= + # Motion Control (Joint Space) + # ========================================================================= + + def write_joint_positions( + self, + positions: list[float], + velocity: float = 1.0, + ) -> bool: + _ = velocity + if not self._servos_enabled or not self._native: + return False + self._control_mode = ControlMode.POSITION + self._native.set_joint_position_targets(positions[: self._dof]) + return True + + def write_joint_velocities(self, velocities: list[float]) -> bool: + if not self._servos_enabled or not self._native: + return False + self._control_mode = ControlMode.VELOCITY + dt = 1.0 / self._native.control_frequency + current = self._native.joint_positions + targets = [current[i] + velocities[i] * dt for i in range(min(len(velocities), self._dof))] + self._native.set_joint_position_targets(targets) + return True + + def write_stop(self) -> bool: + if not self._native: + return False + self._native.hold_current_position() + return True + + # ========================================================================= + # Servo Control + # ========================================================================= + + def write_enable(self, enable: bool) -> bool: + self._servos_enabled = enable + return True + + def read_enabled(self) -> bool: + return self._servos_enabled + + def write_clear_errors(self) -> bool: + self._error_code = 0 + self._error_message = "" + return True + + # ========================================================================= + # Optional Interfaces + # ========================================================================= + + def read_cartesian_position(self) -> dict[str, float] | None: + return None + + def write_cartesian_position( + self, + pose: dict[str, float], + velocity: float = 1.0, + ) -> bool: + _ = pose + _ = velocity + return False + + def read_gripper_position(self) -> float | None: + return None + + def write_gripper_position(self, position: float) -> bool: + _ = position + return False + + def read_force_torque(self) -> list[float] | None: + return None + + +class SimSDKWrapper(SimBackend): + """Backward-compatible alias for SimBackend.""" + + +__all__ = [ + "SimBackend", + "SimSDKWrapper", +] diff --git a/dimos/simulation/manipulators/sim_driver.py b/dimos/simulation/manipulators/sim_driver.py new file mode 100644 index 0000000000..ac25007139 --- /dev/null +++ b/dimos/simulation/manipulators/sim_driver.py @@ -0,0 +1,241 @@ +# 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. + +"""Robot-agnostic MuJoCo simulation driver module.""" + +from dataclasses import dataclass +import threading +import time +from typing import Any + +from reactivex.disposable import Disposable + +from dimos.core import In, Module, Out, rpc +from dimos.core.module import ModuleConfig +from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState +from dimos.simulation.manipulators.sim_backend import SimBackend + + +@dataclass +class SimDriverConfig(ModuleConfig): + robot: str | None = None + config_path: str | None = None + headless: bool = False + + +class SimDriver(Module[SimDriverConfig]): + """Module wrapper for MuJoCo simulation backends.""" + + default_config = SimDriverConfig + config: SimDriverConfig + + joint_state: Out[JointState] + robot_state: Out[RobotState] + joint_position_command: In[JointCommand] + joint_velocity_command: In[JointCommand] + + def __init__(self, *args: Any, **kwargs: Any) -> None: + super().__init__(*args, **kwargs) + self._backend = self._create_backend() + self._control_rate = 100.0 + self._monitor_rate = 100.0 + self._joint_prefix = "joint" + self._stop_event = threading.Event() + self._control_thread: threading.Thread | None = None + self._monitor_thread: threading.Thread | None = None + self._command_lock = threading.Lock() + self._pending_positions: list[float] | None = None + self._pending_velocities: list[float] | None = None + + def _create_backend(self) -> SimBackend: + return SimBackend( + robot=self.config.robot or "", + config_path=self.config.config_path, + headless=self.config.headless, + ) + + @rpc + def start(self) -> None: + super().start() + if not self._backend.connect(): + raise RuntimeError("Failed to connect to MuJoCo simulation backend") + self._backend.write_enable(True) + + try: + if ( + self.joint_position_command.connection is not None + or self.joint_position_command._transport is not None + ): + self._disposables.add( + Disposable( + self.joint_position_command.subscribe(self._on_joint_position_command) + ) + ) + except Exception as exc: + import logging + + logging.getLogger(self.__class__.__name__).warning( + f"Failed to subscribe joint_position_command: {exc}" + ) + + try: + if ( + self.joint_velocity_command.connection is not None + or self.joint_velocity_command._transport is not None + ): + self._disposables.add( + Disposable( + self.joint_velocity_command.subscribe(self._on_joint_velocity_command) + ) + ) + except Exception as exc: + import logging + + logging.getLogger(self.__class__.__name__).warning( + f"Failed to subscribe joint_velocity_command: {exc}" + ) + + self._stop_event.clear() + self._control_thread = threading.Thread( + target=self._control_loop, + daemon=True, + name=f"{self.__class__.__name__}-control", + ) + self._monitor_thread = threading.Thread( + target=self._monitor_loop, + daemon=True, + name=f"{self.__class__.__name__}-monitor", + ) + self._control_thread.start() + self._monitor_thread.start() + + @rpc + def stop(self) -> None: + self._stop_event.set() + if self._control_thread and self._control_thread.is_alive(): + self._control_thread.join(timeout=2.0) + if self._monitor_thread and self._monitor_thread.is_alive(): + self._monitor_thread.join(timeout=2.0) + self._backend.disconnect() + super().stop() + + @rpc + def enable_servos(self) -> bool: + return self._backend.write_enable(True) + + @rpc + def disable_servos(self) -> bool: + return self._backend.write_enable(False) + + @rpc + def clear_errors(self) -> bool: + return self._backend.write_clear_errors() + + @rpc + def emergency_stop(self) -> bool: + return self._backend.write_stop() + + def _on_joint_position_command(self, msg: JointCommand) -> None: + with self._command_lock: + self._pending_positions = list(msg.positions) + self._pending_velocities = None + + def _on_joint_velocity_command(self, msg: JointCommand) -> None: + with self._command_lock: + self._pending_velocities = list(msg.positions) + self._pending_positions = None + + def _control_loop(self) -> None: + period = 1.0 / max(self._control_rate, 1.0) + while not self._stop_event.is_set(): + with self._command_lock: + positions = ( + None if self._pending_positions is None else list(self._pending_positions) + ) + velocities = ( + None if self._pending_velocities is None else list(self._pending_velocities) + ) + + if positions is not None: + self._backend.write_joint_positions(positions) + elif velocities is not None: + self._backend.write_joint_velocities(velocities) + + time.sleep(period) + + def _monitor_loop(self) -> None: + period = 1.0 / max(self._monitor_rate, 1.0) + names = [f"{self._joint_prefix}{i + 1}" for i in range(self._backend.get_dof())] + while not self._stop_event.is_set(): + positions = self._backend.read_joint_positions() + velocities = self._backend.read_joint_velocities() + efforts = self._backend.read_joint_efforts() + state = self._backend.read_state() + error_code, _ = self._backend.read_error() + + self.joint_state.publish( + JointState( + frame_id=self.frame_id, + name=names, + position=positions, + velocity=velocities, + effort=efforts, + ) + ) + self.robot_state.publish( + RobotState( + state=state.get("state", 0), + mode=state.get("mode", 0), + error_code=error_code, + warn_code=0, + cmdnum=0, + mt_brake=0, + mt_able=1 if self._backend.read_enabled() else 0, + tcp_pose=[], + tcp_offset=[], + joints=[float(p) for p in positions], + ) + ) + time.sleep(period) + + +def get_blueprint() -> dict[str, Any]: + return { + "name": "SimDriver", + "class": SimDriver, + "config": { + "robot": None, + "config_path": None, + "headless": False, + }, + "inputs": { + "joint_position_command": "JointCommand", + "joint_velocity_command": "JointCommand", + }, + "outputs": { + "joint_state": "JointState", + "robot_state": "RobotState", + }, + } + + +simulation = SimDriver.blueprint + + +__all__ = [ + "SimDriver", + "SimDriverConfig", + "get_blueprint", + "simulation", +] diff --git a/dimos/simulation/manipulators/simulation.py b/dimos/simulation/manipulators/simulation.py deleted file mode 100644 index d78775259c..0000000000 --- a/dimos/simulation/manipulators/simulation.py +++ /dev/null @@ -1,277 +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. - -"""Robot-agnostic MuJoCo simulation driver.""" - -import logging -import math -from typing import Any - -from dimos.hardware.manipulators.base import ( - BaseManipulatorDriver, - StandardMotionComponent, - StandardServoComponent, - StandardStatusComponent, -) -from dimos.hardware.manipulators.base.sdk_interface import BaseManipulatorSDK, ManipulatorInfo -from dimos.simulation.manipulators import MujocoSimBackend - -logger = logging.getLogger(__name__) - - -class SimSDKWrapper(BaseManipulatorSDK): - """SDK wrapper for a generic MuJoCo simulation backend.""" - - def __init__(self) -> None: - self.logger = logging.getLogger(self.__class__.__name__) - self.native_sdk: MujocoSimBackend | None = None - self.dof = 0 - self._connected = False - self._servos_enabled = False - self._mode = 0 - self._state = 0 - self._robot = "unknown" - - def connect(self, config: dict[str, Any]) -> bool: - """Connect to the MuJoCo simulation backend.""" - try: - robot = config.get("robot") - if not robot: - raise ValueError("robot is required for MuJoCo simulation loading") - self._robot = str(robot) - config_path = config.get("config_path") - headless = bool(config.get("headless", False)) - - self.logger.info("Connecting to MuJoCo Sim...") - self.native_sdk = MujocoSimBackend( - robot=self._robot, - config_path=config_path, - headless=headless, - ) - self.native_sdk.connect() - - if self.native_sdk.connected: - self._connected = True - self._servos_enabled = True - self._state = 0 - self._mode = 0 - self.dof = int(self.native_sdk.num_joints) - self.logger.info("Successfully connected to MuJoCo Sim", extra={"dof": self.dof}) - return True - - self.logger.error("Failed to connect to MuJoCo Sim") - return False - except Exception as exc: - self.logger.error(f"Sim connection failed: {exc}") - return False - - def disconnect(self) -> None: - """Disconnect from simulation.""" - if self.native_sdk: - try: - self.native_sdk.disconnect() - finally: - self._connected = False - self.native_sdk = None - - def is_connected(self) -> bool: - return bool(self._connected and self.native_sdk and self.native_sdk.connected) - - def get_joint_positions(self) -> list[float]: - return self.native_sdk.joint_positions[: self.dof] if self.native_sdk else [] - - def get_joint_velocities(self) -> list[float]: - return self.native_sdk.joint_velocities[: self.dof] if self.native_sdk else [] - - def get_joint_efforts(self) -> list[float]: - return self.native_sdk.joint_efforts[: self.dof] if self.native_sdk else [] - - def set_joint_positions( - self, - positions: list[float], - _velocity: float = 1.0, - _acceleration: float = 1.0, - _wait: bool = False, - ) -> bool: - _ = _velocity - _ = _acceleration - _ = _wait - if not self._servos_enabled or not self.native_sdk: - return False - self._mode = 0 - self.native_sdk.set_joint_position_targets(positions[: self.dof]) - return True - - def set_joint_velocities(self, velocities: list[float]) -> bool: - if not self._servos_enabled or not self.native_sdk: - return False - self._mode = 1 - dt = 1.0 / self.native_sdk.control_frequency - current = self.native_sdk.joint_positions - targets = [current[i] + velocities[i] * dt for i in range(min(len(velocities), self.dof))] - self.native_sdk.set_joint_position_targets(targets) - return True - - def set_joint_efforts(self, efforts: list[float]) -> bool: - self.logger.warning("Torque control not supported in MuJoCo Sim bridge") - _ = efforts - return False - - def stop_motion(self) -> bool: - if not self.native_sdk: - return False - self.native_sdk.hold_current_position() - self._state = 0 - return True - - def enable_servos(self) -> bool: - self._servos_enabled = True - self._state = 0 - return True - - def disable_servos(self) -> bool: - self._servos_enabled = False - return True - - def are_servos_enabled(self) -> bool: - return self._servos_enabled - - def get_robot_state(self) -> dict[str, Any]: - velocities = self.native_sdk.joint_velocities[: self.dof] if self.native_sdk else [] - is_moving = any(abs(v) > 1e-4 for v in velocities) - self._state = 1 if is_moving else 0 - return { - "state": self._state, - "mode": self._mode, - "error_code": 0, - "is_moving": is_moving, - } - - def get_error_code(self) -> int: - return 0 - - def get_error_message(self) -> str: - return "" - - def clear_errors(self) -> bool: - self._state = 0 - return True - - def emergency_stop(self) -> bool: - if self.native_sdk: - self.native_sdk.hold_current_position() - self._state = 3 - return True - - def get_info(self) -> ManipulatorInfo: - return ManipulatorInfo( - vendor="MuJoCo", - model=self._robot, - dof=self.dof, - firmware_version=None, - serial_number=None, - ) - - def get_joint_limits(self) -> tuple[list[float], list[float]]: - if not self.native_sdk: - return ([], []) - ranges = getattr(self.native_sdk.model, "jnt_range", None) - if ranges is None or len(ranges) == 0: - lower = [-math.pi] * self.dof - upper = [math.pi] * self.dof - return (lower, upper) - limit = min(len(ranges), self.dof) - lower = [float(ranges[i][0]) for i in range(limit)] - upper = [float(ranges[i][1]) for i in range(limit)] - if limit < self.dof: - lower.extend([-math.pi] * (self.dof - limit)) - upper.extend([math.pi] * (self.dof - limit)) - return (lower, upper) - - def get_velocity_limits(self) -> list[float]: - max_vel_rad = math.radians(180.0) - return [max_vel_rad] * self.dof - - def get_acceleration_limits(self) -> list[float]: - max_acc_rad = math.radians(1145.0) - return [max_acc_rad] * self.dof - - -class SimDriver(BaseManipulatorDriver): - """Generic manipulator driver backed by MuJoCo.""" - - def __init__(self, **kwargs: Any) -> None: - config: dict[str, Any] = kwargs.pop("config", {}) - - driver_params = [ - "robot", - "config_path", - "headless", - ] - for param in driver_params: - if param in kwargs: - config[param] = kwargs.pop(param) - - logger.info(f"Initializing SimDriver with config: {config}") - - sdk = SimSDKWrapper() - components = [ - StandardMotionComponent(sdk), - StandardServoComponent(sdk), - StandardStatusComponent(sdk), - ] - - kwargs.pop("sdk", None) - kwargs.pop("components", None) - kwargs.pop("name", None) - - super().__init__( - sdk=sdk, - components=components, - config=config, - name="SimDriver", - **kwargs, - ) - - logger.info("SimDriver initialized successfully") - - -def get_blueprint() -> dict[str, Any]: - return { - "name": "SimDriver", - "class": SimDriver, - "config": { - "robot": None, - "config_path": None, - "headless": False, - }, - "inputs": { - "joint_position_command": "JointCommand", - "joint_velocity_command": "JointCommand", - }, - "outputs": { - "joint_state": "JointState", - "robot_state": "RobotState", - }, - } - - -simulation = SimDriver.blueprint - -__all__ = [ - "SimDriver", - "SimSDKWrapper", - "get_blueprint", - "simulation", -] diff --git a/dimos/simulation/manipulators/xarm_sim_driver.py b/dimos/simulation/manipulators/xarm_sim_driver.py index 292018e11b..fc240b4004 100644 --- a/dimos/simulation/manipulators/xarm_sim_driver.py +++ b/dimos/simulation/manipulators/xarm_sim_driver.py @@ -12,240 +12,92 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""MuJoCo-native xArm simulation driver and SDK wrapper.""" +"""MuJoCo-native xArm simulation backend and module wrapper.""" -import logging +from dataclasses import dataclass import math from typing import Any -from dimos.hardware.manipulators.base import ( - BaseManipulatorDriver, - StandardMotionComponent, - StandardServoComponent, - StandardStatusComponent, -) -from dimos.hardware.manipulators.base.sdk_interface import BaseManipulatorSDK, ManipulatorInfo -from dimos.simulation.manipulators import MujocoSimBackend +from dimos.core import rpc +from dimos.hardware.manipulators.spec import JointLimits, ManipulatorInfo +from dimos.simulation.manipulators.sim_backend import SimBackend +from dimos.simulation.manipulators.sim_driver import SimDriver, SimDriverConfig -logger = logging.getLogger(__name__) +class XArmSimBackend(SimBackend): + """Backend wrapper for xArm simulation using the MuJoCo simulation backend.""" -class XArmSimSDKWrapper(BaseManipulatorSDK): - """SDK wrapper for xArm simulation using the MuJoCo simulation backend.""" - - def __init__(self) -> None: - self.logger = logging.getLogger(self.__class__.__name__) - self.native_sdk: MujocoSimBackend | None = None - self.dof = 7 - self._connected = False - self._servos_enabled = False - self._mode = 0 - self._state = 0 - - def connect(self, config: dict[str, Any]) -> bool: - """Connect to the MuJoCo simulation backend.""" - try: - robot = config.get("robot") - if not robot: - raise ValueError("robot is required for MuJoCo simulation loading") - config_path = config.get("config_path") - headless = bool(config.get("headless", False)) - - self.logger.info("Connecting to MuJoCo Sim...") - self.native_sdk = MujocoSimBackend( - robot=robot, - config_path=config_path, - headless=headless, - ) - self.native_sdk.connect() - - if self.native_sdk.connected: - self._connected = True - self._servos_enabled = True - self._state = 0 - self._mode = 0 - self.dof = int(config.get("dof", self.native_sdk.num_joints)) - self.logger.info(f"Successfully connected to MuJoCo Sim (DOF: {self.dof})") - return True - - self.logger.error("Failed to connect to XArm Sim") - return False - except Exception as exc: - self.logger.error(f"Sim connection failed: {exc}") - return False - - def disconnect(self) -> None: - """Disconnect from simulation.""" - if self.native_sdk: - try: - self.native_sdk.disconnect() - finally: - self._connected = False - self.native_sdk = None - - def is_connected(self) -> bool: - return bool(self._connected and self.native_sdk and self.native_sdk.connected) - - def get_joint_positions(self) -> list[float]: - return self.native_sdk.joint_positions[: self.dof] - - def get_joint_velocities(self) -> list[float]: - return self.native_sdk.joint_velocities[: self.dof] - - def get_joint_efforts(self) -> list[float]: - return self.native_sdk.joint_efforts[: self.dof] - - def set_joint_positions( + def __init__( self, - positions: list[float], - _velocity: float = 1.0, - _acceleration: float = 1.0, - _wait: bool = False, - ) -> bool: - _ = _velocity - _ = _acceleration - _ = _wait - if not self._servos_enabled: - return False - self._mode = 0 - self.native_sdk.set_joint_position_targets(positions[: self.dof]) - return True - - def set_joint_velocities(self, velocities: list[float]) -> bool: - if not self._servos_enabled: - return False - self._mode = 1 - dt = 1.0 / self.native_sdk.control_frequency - current = self.native_sdk.joint_positions - targets = [current[i] + velocities[i] * dt for i in range(min(len(velocities), self.dof))] - self.native_sdk.set_joint_position_targets(targets) - return True - - def set_joint_efforts(self, efforts: list[float]) -> bool: - self.logger.warning("Torque control not supported in MuJoCo Sim bridge") - _ = efforts - return False - - def stop_motion(self) -> bool: - self.native_sdk.hold_current_position() - self._state = 0 - return True - - def enable_servos(self) -> bool: - self._servos_enabled = True - self._state = 0 - return True - - def disable_servos(self) -> bool: - self._servos_enabled = False - return True - - def are_servos_enabled(self) -> bool: - return self._servos_enabled - - def get_robot_state(self) -> dict[str, Any]: - is_moving = any(abs(v) > 1e-4 for v in self.native_sdk.joint_velocities[: self.dof]) - self._state = 1 if is_moving else 0 - return { - "state": self._state, - "mode": self._mode, - "error_code": 0, - "warn_code": 0, - "is_moving": is_moving, - "cmd_num": 0, - } - - def get_error_code(self) -> int: - return 0 - - def get_error_message(self) -> str: - return "" - - def clear_errors(self) -> bool: - self._state = 0 - return True - - def emergency_stop(self) -> bool: - self._state = 3 - self.native_sdk.hold_current_position() - return True + robot: str, + dof: int = 7, + config_path: str | None = None, + headless: bool = False, + ) -> None: + super().__init__( + robot=robot, + config_path=config_path, + headless=headless, + dof=dof, + ) def get_info(self) -> ManipulatorInfo: return ManipulatorInfo( vendor="UFACTORY", - model=f"xArm{self.dof}", - dof=self.dof, + model=f"xArm{self._dof}", + dof=self._dof, firmware_version=None, serial_number=None, ) - def get_joint_limits(self) -> tuple[list[float], list[float]]: - if self.dof == 7: + def get_limits(self) -> JointLimits: + if self._dof == 7: lower_deg = [-360, -118, -360, -233, -360, -97, -360] upper_deg = [360, 118, 360, 11, 360, 180, 360] - elif self.dof == 6: + elif self._dof == 6: lower_deg = [-360, -118, -225, -11, -360, -97] upper_deg = [360, 118, 11, 225, 360, 180] else: lower_deg = [-360, -118, -225, -97, -360] upper_deg = [360, 118, 11, 180, 360] - lower_rad = [math.radians(d) for d in lower_deg[: self.dof]] - upper_rad = [math.radians(d) for d in upper_deg[: self.dof]] - return (lower_rad, upper_rad) - - def get_velocity_limits(self) -> list[float]: + lower_rad = [math.radians(d) for d in lower_deg[: self._dof]] + upper_rad = [math.radians(d) for d in upper_deg[: self._dof]] max_vel_rad = math.radians(180.0) - return [max_vel_rad] * self.dof - - def get_acceleration_limits(self) -> list[float]: - max_acc_rad = math.radians(1145.0) - return [max_acc_rad] * self.dof + return JointLimits( + position_lower=lower_rad, + position_upper=upper_rad, + velocity_max=[max_vel_rad] * self._dof, + ) -class XArmSimDriver(BaseManipulatorDriver): - """xArm driver backed by the MuJoCo simulation bridge.""" +class XArmSimSDKWrapper(XArmSimBackend): + """Backward-compatible alias for XArmSimBackend.""" - def __init__(self, **kwargs: Any) -> None: - config: dict[str, Any] = kwargs.pop("config", {}) - driver_params = [ - "dof", - "has_gripper", - "has_force_torque", - "control_rate", - "monitor_rate", - "robot", - "config_path", - "headless", - ] - for param in driver_params: - if param in kwargs: - config[param] = kwargs.pop(param) +@dataclass +class XArmSimDriverConfig(SimDriverConfig): + pass - logger.info(f"Initializing XArmSimDriver with config: {config}") - sdk = XArmSimSDKWrapper() - sdk.dof = int(config.get("dof", sdk.dof)) - components = [ - StandardMotionComponent(sdk), - StandardServoComponent(sdk), - StandardStatusComponent(sdk), - ] +class XArmSimDriver(SimDriver): + """xArm simulation driver module using MuJoCo backend.""" - kwargs.pop("sdk", None) - kwargs.pop("components", None) - kwargs.pop("name", None) + default_config = XArmSimDriverConfig + config: XArmSimDriverConfig - super().__init__( - sdk=sdk, - components=components, - config=config, - name="XArmSimDriver", - **kwargs, + def _create_backend(self) -> SimBackend: + return XArmSimBackend( + robot=self.config.robot or "", + config_path=self.config.config_path, + headless=self.config.headless, ) - logger.info("XArmSimDriver initialized successfully") + @rpc + def start(self) -> None: + if not self.config.robot: + raise ValueError("robot is required for XArmSimDriver") + super().start() def get_blueprint() -> dict[str, Any]: @@ -253,11 +105,6 @@ def get_blueprint() -> dict[str, Any]: "name": "XArmSimDriver", "class": XArmSimDriver, "config": { - "dof": 7, - "has_gripper": False, - "has_force_torque": False, - "control_rate": 100, - "monitor_rate": 10, "robot": None, "config_path": None, "headless": False, @@ -275,8 +122,11 @@ def get_blueprint() -> dict[str, Any]: xarm_sim_driver = XArmSimDriver.blueprint + __all__ = [ + "XArmSimBackend", "XArmSimDriver", + "XArmSimDriverConfig", "XArmSimSDKWrapper", "get_blueprint", "xarm_sim_driver", diff --git a/dimos/simulation/sim_blueprints.py b/dimos/simulation/sim_blueprints.py index f5ad7ead73..e1b68c3861 100644 --- a/dimos/simulation/sim_blueprints.py +++ b/dimos/simulation/sim_blueprints.py @@ -20,12 +20,12 @@ RobotState, ) from dimos.msgs.trajectory_msgs import JointTrajectory -from dimos.simulation.manipulators.simulation import simulation +from dimos.simulation.manipulators.sim_driver import simulation xarm7_trajectory_sim = simulation( robot="xarm7_mj_description", config_path=None, - headless=False, + headless=True, ).transports( { ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), From b9adc9b2b5e0482285b925f2c38d47caae822583 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Thu, 15 Jan 2026 17:03:23 -0800 Subject: [PATCH 03/43] clean up code + comments --- dimos/simulation/manipulators/__init__.py | 10 +++- .../{sim_driver.py => mujoco_driver.py} | 28 +++++------ ...m_backend.py => mujoco_manip_interface.py} | 46 ++++--------------- .../manipulators/mujoco_sim_backend.py | 27 +++-------- .../manipulators/xarm_sim_driver.py | 14 +++--- dimos/simulation/sim_blueprints.py | 4 +- dimos/simulation/subscribe_topic.py | 31 +++++++++++++ 7 files changed, 79 insertions(+), 81 deletions(-) rename dimos/simulation/manipulators/{sim_driver.py => mujoco_driver.py} (93%) rename dimos/simulation/manipulators/{sim_backend.py => mujoco_manip_interface.py} (81%) create mode 100644 dimos/simulation/subscribe_topic.py diff --git a/dimos/simulation/manipulators/__init__.py b/dimos/simulation/manipulators/__init__.py index 1c1d932910..a907f25653 100644 --- a/dimos/simulation/manipulators/__init__.py +++ b/dimos/simulation/manipulators/__init__.py @@ -14,6 +14,12 @@ """MuJoCo simulation backend utilities.""" -from dimos.simulation.manipulators.mujoco_sim_backend import MujocoSimBackend +from dimos.simulation.manipulators.mujoco_driver import MujocoDriver +from dimos.simulation.manipulators.mujoco_manip_interface import ( + MujocoManipInterface, +) -__all__ = ["MujocoSimBackend"] +__all__ = [ + "MujocoDriver", + "MujocoManipInterface", +] diff --git a/dimos/simulation/manipulators/sim_driver.py b/dimos/simulation/manipulators/mujoco_driver.py similarity index 93% rename from dimos/simulation/manipulators/sim_driver.py rename to dimos/simulation/manipulators/mujoco_driver.py index ac25007139..f20a4bb7b7 100644 --- a/dimos/simulation/manipulators/sim_driver.py +++ b/dimos/simulation/manipulators/mujoco_driver.py @@ -24,21 +24,21 @@ from dimos.core import In, Module, Out, rpc from dimos.core.module import ModuleConfig from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState -from dimos.simulation.manipulators.sim_backend import SimBackend +from dimos.simulation.manipulators.mujoco_manip_interface import MujocoManipInterface @dataclass -class SimDriverConfig(ModuleConfig): +class MujocoDriverConfig(ModuleConfig): robot: str | None = None config_path: str | None = None headless: bool = False -class SimDriver(Module[SimDriverConfig]): - """Module wrapper for MuJoCo simulation backends.""" +class MujocoDriver(Module[MujocoDriverConfig]): + """Module wrapper for MuJoCo manipulator simulation.""" - default_config = SimDriverConfig - config: SimDriverConfig + default_config = MujocoDriverConfig + config: MujocoDriverConfig joint_state: Out[JointState] robot_state: Out[RobotState] @@ -58,8 +58,8 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: self._pending_positions: list[float] | None = None self._pending_velocities: list[float] | None = None - def _create_backend(self) -> SimBackend: - return SimBackend( + def _create_backend(self) -> MujocoManipInterface: + return MujocoManipInterface( robot=self.config.robot or "", config_path=self.config.config_path, headless=self.config.headless, @@ -212,8 +212,8 @@ def _monitor_loop(self) -> None: def get_blueprint() -> dict[str, Any]: return { - "name": "SimDriver", - "class": SimDriver, + "name": "MujocoDriver", + "class": MujocoDriver, "config": { "robot": None, "config_path": None, @@ -230,12 +230,12 @@ def get_blueprint() -> dict[str, Any]: } -simulation = SimDriver.blueprint +mujoco_sim = MujocoDriver.blueprint __all__ = [ - "SimDriver", - "SimDriverConfig", + "MujocoDriver", + "MujocoDriverConfig", "get_blueprint", - "simulation", + "mujoco_sim", ] diff --git a/dimos/simulation/manipulators/sim_backend.py b/dimos/simulation/manipulators/mujoco_manip_interface.py similarity index 81% rename from dimos/simulation/manipulators/sim_backend.py rename to dimos/simulation/manipulators/mujoco_manip_interface.py index ab8184d7fd..14b6e9e6bb 100644 --- a/dimos/simulation/manipulators/sim_backend.py +++ b/dimos/simulation/manipulators/mujoco_manip_interface.py @@ -12,17 +12,19 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Robot-agnostic MuJoCo simulation backend.""" +"""MuJoCo simulation adapter wrapper.""" import logging import math from dimos.hardware.manipulators.spec import ControlMode, JointLimits, ManipulatorInfo -from dimos.simulation.manipulators import MujocoSimBackend +from dimos.simulation.manipulators.mujoco_sim_backend import MujocoSimBackend -class SimBackend: - """Backend wrapper for a generic MuJoCo simulation.""" +class MujocoManipInterface: + """ + Adapter wrapper around a MuJoCo simulation backend to provide a uniform interface for all manipulators. + """ def __init__( self, @@ -43,10 +45,6 @@ def __init__( self._error_code = 0 self._error_message = "" - # ========================================================================= - # Connection - # ========================================================================= - def connect(self) -> bool: """Connect to the MuJoCo simulation backend.""" if not self._robot: @@ -86,10 +84,6 @@ def disconnect(self) -> None: def is_connected(self) -> bool: return bool(self._connected and self._native and self._native.connected) - # ========================================================================= - # Info - # ========================================================================= - def get_info(self) -> ManipulatorInfo: return ManipulatorInfo( vendor="MuJoCo", @@ -130,10 +124,6 @@ def get_limits(self) -> JointLimits: velocity_max=[max_vel_rad] * self._dof, ) - # ========================================================================= - # Control Mode - # ========================================================================= - def set_control_mode(self, mode: ControlMode) -> bool: self._control_mode = mode return True @@ -141,10 +131,6 @@ def set_control_mode(self, mode: ControlMode) -> bool: def get_control_mode(self) -> ControlMode: return self._control_mode - # ========================================================================= - # State Reading - # ========================================================================= - def read_joint_positions(self) -> list[float]: if self._native: return self._native.joint_positions[: self._dof] @@ -172,10 +158,6 @@ def read_state(self) -> dict[str, int]: def read_error(self) -> tuple[int, str]: return self._error_code, self._error_message - # ========================================================================= - # Motion Control (Joint Space) - # ========================================================================= - def write_joint_positions( self, positions: list[float], @@ -204,10 +186,6 @@ def write_stop(self) -> bool: self._native.hold_current_position() return True - # ========================================================================= - # Servo Control - # ========================================================================= - def write_enable(self, enable: bool) -> bool: self._servos_enabled = enable return True @@ -220,10 +198,6 @@ def write_clear_errors(self) -> bool: self._error_message = "" return True - # ========================================================================= - # Optional Interfaces - # ========================================================================= - def read_cartesian_position(self) -> dict[str, float] | None: return None @@ -247,11 +221,11 @@ def read_force_torque(self) -> list[float] | None: return None -class SimSDKWrapper(SimBackend): - """Backward-compatible alias for SimBackend.""" +class MujocoManipSDKWrapper(MujocoManipInterface): + """Backward-compatible alias for MujocoManipInterface.""" __all__ = [ - "SimBackend", - "SimSDKWrapper", + "MujocoManipInterface", + "MujocoManipSDKWrapper", ] diff --git a/dimos/simulation/manipulators/mujoco_sim_backend.py b/dimos/simulation/manipulators/mujoco_sim_backend.py index 2c6dfd173d..a931e6c37a 100644 --- a/dimos/simulation/manipulators/mujoco_sim_backend.py +++ b/dimos/simulation/manipulators/mujoco_sim_backend.py @@ -31,6 +31,10 @@ class MujocoSimBackend: """ Base class for MuJoCo simulation backend. + + - starts mujoco simulation engine + - loads robot/environment into simulation + - applies control commands """ def __init__( @@ -40,8 +44,6 @@ def __init__( headless: bool = False, ): """ - Initialize the MuJoCo simulation backend. - Args: robot: Robot description (from robot_descriptions library) name (e.g., "piper", "xarm7_mj_description"). config_path: Path to a MuJoCo XML or a folder containing scene.xml. @@ -50,7 +52,7 @@ def __init__( self._robot_name = robot self._headless = headless - if config_path: + if config_path: # given config dir/file resolved = Path(config_path).expanduser() xml_path = resolved / "scene.xml" if resolved.is_dir() else resolved if not xml_path.exists(): @@ -58,6 +60,7 @@ def __init__( self._model = mujoco.MjModel.from_xml_path(str(xml_path)) else: self._model = load_robot_description(robot) + self._data = mujoco.MjData(self._model) self._num_joints = self._model.nq timestep = float(self._model.opt.timestep) @@ -83,8 +86,6 @@ def __init__( def _apply_control(self) -> None: """ Apply control commands to MuJoCo actuators. - - Default implementation applies joint position targets directly to actuators. """ with self._lock: pos_targets = list(self._joint_position_targets) @@ -96,8 +97,6 @@ def _apply_control(self) -> None: def _update_joint_state(self) -> None: """ Update internal joint state from MuJoCo simulation. - - Default implementation mirrors MuJoCo state directly. """ with self._lock: n_q = min(self._num_joints, self._model.nq) @@ -139,18 +138,11 @@ def disconnect(self) -> None: def _sim_loop(self) -> None: """ Main simulation loop running MuJoCo. - - This method: - 1. Launches the MuJoCo viewer - 2. Runs the simulation at the specified control frequency - 3. Calls `_apply_control()` to apply control commands - 4. Steps the simulation - 5. Calls `_update_joint_state()` to update internal state """ logger.info(f"{self.__class__.__name__}: sim loop started") dt = 1.0 / self._control_frequency - def _step_once(sync_viewer: bool) -> None: + def _step_once(sync_viewer: bool) -> None: # helper to step sim loop_start = time.time() self._apply_control() mujoco.mj_step(self._model, self._data) @@ -191,11 +183,6 @@ def model(self) -> mujoco.MjModel: """MuJoCo model (read-only).""" return self._model - @property - def data(self) -> mujoco.MjData: - """MuJoCo data (read-only).""" - return self._data - @property def joint_positions(self) -> list[float]: """Current joint positions in radians (thread-safe copy).""" diff --git a/dimos/simulation/manipulators/xarm_sim_driver.py b/dimos/simulation/manipulators/xarm_sim_driver.py index fc240b4004..f588b2b1cd 100644 --- a/dimos/simulation/manipulators/xarm_sim_driver.py +++ b/dimos/simulation/manipulators/xarm_sim_driver.py @@ -20,12 +20,12 @@ from dimos.core import rpc from dimos.hardware.manipulators.spec import JointLimits, ManipulatorInfo -from dimos.simulation.manipulators.sim_backend import SimBackend -from dimos.simulation.manipulators.sim_driver import SimDriver, SimDriverConfig +from dimos.simulation.manipulators.mujoco_driver import MujocoDriver, MujocoDriverConfig +from dimos.simulation.manipulators.mujoco_manip_interface import MujocoManipInterface -class XArmSimBackend(SimBackend): - """Backend wrapper for xArm simulation using the MuJoCo simulation backend.""" +class XArmSimBackend(MujocoManipInterface): + """xArm specific implementation of the MuJoCo manipulator interface.""" def __init__( self, @@ -76,17 +76,17 @@ class XArmSimSDKWrapper(XArmSimBackend): @dataclass -class XArmSimDriverConfig(SimDriverConfig): +class XArmSimDriverConfig(MujocoDriverConfig): pass -class XArmSimDriver(SimDriver): +class XArmSimDriver(MujocoDriver): """xArm simulation driver module using MuJoCo backend.""" default_config = XArmSimDriverConfig config: XArmSimDriverConfig - def _create_backend(self) -> SimBackend: + def _create_backend(self) -> MujocoManipInterface: return XArmSimBackend( robot=self.config.robot or "", config_path=self.config.config_path, diff --git a/dimos/simulation/sim_blueprints.py b/dimos/simulation/sim_blueprints.py index e1b68c3861..8cf7d1171e 100644 --- a/dimos/simulation/sim_blueprints.py +++ b/dimos/simulation/sim_blueprints.py @@ -20,9 +20,9 @@ RobotState, ) from dimos.msgs.trajectory_msgs import JointTrajectory -from dimos.simulation.manipulators.sim_driver import simulation +from dimos.simulation.manipulators.mujoco_driver import mujoco_sim -xarm7_trajectory_sim = simulation( +xarm7_trajectory_sim = mujoco_sim( robot="xarm7_mj_description", config_path=None, headless=True, diff --git a/dimos/simulation/subscribe_topic.py b/dimos/simulation/subscribe_topic.py new file mode 100644 index 0000000000..e674fbf63b --- /dev/null +++ b/dimos/simulation/subscribe_topic.py @@ -0,0 +1,31 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import time + +from dimos.core.transport import LCMTransport +from dimos.msgs.sensor_msgs import JointState + +t = LCMTransport("/xarm/joint_states", JointState) + + +def cb(msg: JointState): + # print("names:", msg.name) + print("pos :", " ".join(f"{x:.4f}" for x in msg.position)) + + +t.subscribe(cb) + +while True: + time.sleep(0.1) From ac1c647839c9620bf7182f4d8b039cf52e8ea006 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Thu, 15 Jan 2026 17:24:11 -0800 Subject: [PATCH 04/43] tests for testing --- dimos/simulation/test_publish_topic.py | 24 +++++++++++++++++++ ...cribe_topic.py => test_subscribe_topic.py} | 0 2 files changed, 24 insertions(+) create mode 100644 dimos/simulation/test_publish_topic.py rename dimos/simulation/{subscribe_topic.py => test_subscribe_topic.py} (100%) diff --git a/dimos/simulation/test_publish_topic.py b/dimos/simulation/test_publish_topic.py new file mode 100644 index 0000000000..ba035b5b37 --- /dev/null +++ b/dimos/simulation/test_publish_topic.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python3 +# 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. + +from dimos.core.transport import LCMTransport +from dimos.msgs.sensor_msgs import JointCommand + +TOPIC = "/xarm/joint_position_command" +POSITIONS = [1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] + +transport = LCMTransport(TOPIC, JointCommand) +message = JointCommand(positions=POSITIONS) +transport.broadcast(None, message) diff --git a/dimos/simulation/subscribe_topic.py b/dimos/simulation/test_subscribe_topic.py similarity index 100% rename from dimos/simulation/subscribe_topic.py rename to dimos/simulation/test_subscribe_topic.py From ca898c5489b25316a9640a0be365fe5ce4fd2d33 Mon Sep 17 00:00:00 2001 From: Jing Cao <38737296+jca0@users.noreply.github.com> Date: Thu, 15 Jan 2026 23:41:35 -0800 Subject: [PATCH 05/43] address race condition Co-authored-by: greptile-apps[bot] <165735046+greptile-apps[bot]@users.noreply.github.com> --- dimos/simulation/manipulators/mujoco_manip_interface.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/dimos/simulation/manipulators/mujoco_manip_interface.py b/dimos/simulation/manipulators/mujoco_manip_interface.py index 14b6e9e6bb..2e6b4bcd95 100644 --- a/dimos/simulation/manipulators/mujoco_manip_interface.py +++ b/dimos/simulation/manipulators/mujoco_manip_interface.py @@ -175,10 +175,12 @@ def write_joint_velocities(self, velocities: list[float]) -> bool: return False self._control_mode = ControlMode.VELOCITY dt = 1.0 / self._native.control_frequency - current = self._native.joint_positions - targets = [current[i] + velocities[i] * dt for i in range(min(len(velocities), self._dof))] + with self._native._lock: + current = list(self._native._joint_positions) + targets = [current[i] + velocities[i] * dt for i in range(min(len(velocities), self._dof))] self._native.set_joint_position_targets(targets) return True + return True def write_stop(self) -> bool: if not self._native: From 8116d1f19dc5e3046cc7d7cd04b9f7f82da87f82 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Thu, 15 Jan 2026 23:42:18 -0800 Subject: [PATCH 06/43] fix mypy errors --- dimos/simulation/manipulators/mujoco_sim_backend.py | 8 +++++--- dimos/simulation/sim_blueprints.py | 2 +- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/dimos/simulation/manipulators/mujoco_sim_backend.py b/dimos/simulation/manipulators/mujoco_sim_backend.py index a931e6c37a..45eeebea95 100644 --- a/dimos/simulation/manipulators/mujoco_sim_backend.py +++ b/dimos/simulation/manipulators/mujoco_sim_backend.py @@ -20,8 +20,10 @@ import time import mujoco -import mujoco.viewer as viewer -from robot_descriptions.loaders.mujoco import load_robot_description +import mujoco.viewer as viewer # type: ignore[import-untyped] +from robot_descriptions.loaders.mujoco import ( + load_robot_description, # type: ignore[import-not-found] +) from dimos.utils.logging_config import setup_logger @@ -62,7 +64,7 @@ def __init__( self._model = load_robot_description(robot) self._data = mujoco.MjData(self._model) - self._num_joints = self._model.nq + self._num_joints: int = int(self._model.nq) timestep = float(self._model.opt.timestep) self._control_frequency = 1.0 / timestep if timestep > 0.0 else 100.0 diff --git a/dimos/simulation/sim_blueprints.py b/dimos/simulation/sim_blueprints.py index 8cf7d1171e..2dbe3e1a39 100644 --- a/dimos/simulation/sim_blueprints.py +++ b/dimos/simulation/sim_blueprints.py @@ -25,7 +25,7 @@ xarm7_trajectory_sim = mujoco_sim( robot="xarm7_mj_description", config_path=None, - headless=True, + headless=False, ).transports( { ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), From 346134804bbdcff1b9059b6bac0de7bbd4411904 Mon Sep 17 00:00:00 2001 From: Jing Cao <38737296+jca0@users.noreply.github.com> Date: Thu, 15 Jan 2026 23:50:17 -0800 Subject: [PATCH 07/43] syntax error :( Co-authored-by: greptile-apps[bot] <165735046+greptile-apps[bot]@users.noreply.github.com> --- dimos/simulation/manipulators/mujoco_manip_interface.py | 1 - 1 file changed, 1 deletion(-) diff --git a/dimos/simulation/manipulators/mujoco_manip_interface.py b/dimos/simulation/manipulators/mujoco_manip_interface.py index 2e6b4bcd95..2025403bfb 100644 --- a/dimos/simulation/manipulators/mujoco_manip_interface.py +++ b/dimos/simulation/manipulators/mujoco_manip_interface.py @@ -180,7 +180,6 @@ def write_joint_velocities(self, velocities: list[float]) -> bool: targets = [current[i] + velocities[i] * dt for i in range(min(len(velocities), self._dof))] self._native.set_joint_position_targets(targets) return True - return True def write_stop(self) -> bool: if not self._native: From f46ec174c5f287f7827cd34702e37a8c48c8642d Mon Sep 17 00:00:00 2001 From: jca0 <38737296+jca0@users.noreply.github.com> Date: Fri, 16 Jan 2026 07:52:28 +0000 Subject: [PATCH 08/43] CI code cleanup --- dimos/simulation/manipulators/mujoco_manip_interface.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/dimos/simulation/manipulators/mujoco_manip_interface.py b/dimos/simulation/manipulators/mujoco_manip_interface.py index 2025403bfb..1151fee50b 100644 --- a/dimos/simulation/manipulators/mujoco_manip_interface.py +++ b/dimos/simulation/manipulators/mujoco_manip_interface.py @@ -177,7 +177,9 @@ def write_joint_velocities(self, velocities: list[float]) -> bool: dt = 1.0 / self._native.control_frequency with self._native._lock: current = list(self._native._joint_positions) - targets = [current[i] + velocities[i] * dt for i in range(min(len(velocities), self._dof))] + targets = [ + current[i] + velocities[i] * dt for i in range(min(len(velocities), self._dof)) + ] self._native.set_joint_position_targets(targets) return True From 97508eeeb02066505e3b98780e131a45a6f6aa71 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 16 Jan 2026 16:44:07 -0800 Subject: [PATCH 09/43] skeleton for modular sim arch --- dimos/simulation/engines/__init__.py | 8 +++ dimos/simulation/engines/base.py | 97 ++++++++++++++++++++++++++++ dimos/simulation/registry.py | 79 ++++++++++++++++++++++ 3 files changed, 184 insertions(+) create mode 100644 dimos/simulation/engines/__init__.py create mode 100644 dimos/simulation/engines/base.py create mode 100644 dimos/simulation/registry.py diff --git a/dimos/simulation/engines/__init__.py b/dimos/simulation/engines/__init__.py new file mode 100644 index 0000000000..ab8b53230f --- /dev/null +++ b/dimos/simulation/engines/__init__.py @@ -0,0 +1,8 @@ +"""Simulation engines for manipulator backends.""" + +from dimos.simulation.engines.base import RobotSpec, SimulationEngine + +__all__ = [ + "RobotSpec", + "SimulationEngine", +] diff --git a/dimos/simulation/engines/base.py b/dimos/simulation/engines/base.py new file mode 100644 index 0000000000..f707486a2e --- /dev/null +++ b/dimos/simulation/engines/base.py @@ -0,0 +1,97 @@ +# 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. + +"""Base interfaces for simulator engines and robot specs.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from dataclasses import dataclass +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from dimos.hardware.manipulators.spec import JointLimits + + +@dataclass(frozen=True) +class RobotSpec: + """Robot description metadata for simulation engines.""" + + name: str + engine: str + asset: str | None = None + dof: int | None = None + joint_names: list[str] | None = None + limits: JointLimits | None = None + vendor: str | None = None + model: str | None = None + + +class SimulationEngine(ABC): + """Abstract base class for a simulator engine instance.""" + + def __init__(self, spec: RobotSpec, config_path: str | None, headless: bool) -> None: + self._spec = spec + self._config_path = config_path + self._headless = headless + + @property + def spec(self) -> RobotSpec: + return self._spec + + @property + def config_path(self) -> str | None: + return self._config_path + + @property + def headless(self) -> bool: + return self._headless + + @abstractmethod + def connect(self) -> None: + """Connect to simulation and start the engine.""" + + @abstractmethod + def disconnect(self) -> None: + """Disconnect from simulation and stop the engine.""" + + @property + @abstractmethod + def connected(self) -> bool: + """Whether the engine is connected.""" + + @property + @abstractmethod + def num_joints(self) -> int: + """Number of joints for the loaded robot.""" + + @abstractmethod + def read_joint_positions(self) -> list[float]: + """Read joint positions in radians.""" + + @abstractmethod + def read_joint_velocities(self) -> list[float]: + """Read joint velocities in rad/s.""" + + @abstractmethod + def read_joint_efforts(self) -> list[float]: + """Read joint efforts in Nm.""" + + @abstractmethod + def write_joint_positions(self, positions: list[float]) -> None: + """Command joint positions in radians.""" + + @abstractmethod + def write_joint_velocities(self, velocities: list[float]) -> None: + """Command joint velocities in rad/s.""" diff --git a/dimos/simulation/registry.py b/dimos/simulation/registry.py new file mode 100644 index 0000000000..91ed29fdff --- /dev/null +++ b/dimos/simulation/registry.py @@ -0,0 +1,79 @@ +# 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. + +"""Registry for simulation engines and robot specs.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from dimos.simulation.engines.base import RobotSpec, SimulationEngine + + +class SimulationRegistry: + """Lookup table for simulator engines and robot specs.""" + + def __init__(self) -> None: + self._engines: dict[str, type[SimulationEngine]] = {} + self._robots: dict[str, RobotSpec] = {} + + def register_engine(self, name: str, engine_cls: type[SimulationEngine]) -> None: + key = name.lower() + self._engines[key] = engine_cls + + def register_robot(self, name: str, spec: RobotSpec) -> None: + key = name.lower() + self._robots[key] = spec + + def get_engine(self, name: str) -> type[SimulationEngine]: + key = name.lower() + if key not in self._engines: + raise KeyError(f"Unknown simulation engine: {name}") + return self._engines[key] + + def get_robot(self, name: str) -> RobotSpec: + key = name.lower() + if key not in self._robots: + raise KeyError(f"Unknown robot spec: {name}") + return self._robots[key] + + def create_engine( + self, + engine: str, + robot: str, + config_path: str | None, + headless: bool, + ) -> SimulationEngine: + spec = self.get_robot(robot) + if spec.engine and spec.engine.lower() != engine.lower(): + raise ValueError( + f"Robot '{spec.name}' registered for engine '{spec.engine}', got '{engine}'." + ) + engine_cls = self.get_engine(engine) + return engine_cls(spec=spec, config_path=config_path, headless=headless) + + def list_engines(self) -> list[str]: + return sorted(self._engines.keys()) + + def list_robots(self) -> list[str]: + return sorted(self._robots.keys()) + + +registry = SimulationRegistry() + +__all__ = [ + "SimulationRegistry", + "registry", +] From 89f462ace4eae6c30799bc24ef83de11b4e75c72 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 16 Jan 2026 16:57:45 -0800 Subject: [PATCH 10/43] mujoco engine/bringup script --- dimos/simulation/engines/mujoco_engine.py | 219 ++++++++++++++++++++++ 1 file changed, 219 insertions(+) create mode 100644 dimos/simulation/engines/mujoco_engine.py diff --git a/dimos/simulation/engines/mujoco_engine.py b/dimos/simulation/engines/mujoco_engine.py new file mode 100644 index 0000000000..950b11d4c6 --- /dev/null +++ b/dimos/simulation/engines/mujoco_engine.py @@ -0,0 +1,219 @@ +# 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. + +"""MuJoCo simulation engine implementation.""" + +from __future__ import annotations + +from pathlib import Path +import threading +import time + +import mujoco +import mujoco.viewer as viewer # type: ignore[import-untyped] +from robot_descriptions.loaders.mujoco import ( + load_robot_description, # type: ignore[import-not-found] +) + +from dimos.simulation.engines.base import RobotSpec, SimulationEngine +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class MujocoEngine(SimulationEngine): + """ + MuJoCo simulation engine. + + - starts MuJoCo simulation engine + - loads robot/environment into simulation + - applies control commands + """ + + def __init__(self, spec: RobotSpec, config_path: str | None, headless: bool) -> None: + super().__init__(spec=spec, config_path=config_path, headless=headless) + + self._robot_name = spec.asset or spec.name + if not self._robot_name: + raise ValueError("robot name or asset is required for MuJoCo simulation loading") + + if config_path: + resolved = Path(config_path).expanduser() + xml_path = resolved / "scene.xml" if resolved.is_dir() else resolved + if not xml_path.exists(): + raise FileNotFoundError(f"MuJoCo XML not found: {xml_path}") + self._model = mujoco.MjModel.from_xml_path(str(xml_path)) + else: + self._model = load_robot_description(self._robot_name) + + self._data = mujoco.MjData(self._model) + self._num_joints = int(self._model.nq) + timestep = float(self._model.opt.timestep) + self._control_frequency = 1.0 / timestep if timestep > 0.0 else 100.0 + + self._connected = False + self._lock = threading.Lock() + self._stop_event = threading.Event() + self._sim_thread: threading.Thread | None = None + + self._joint_positions = [0.0] * self._num_joints + self._joint_velocities = [0.0] * self._num_joints + self._joint_efforts = [0.0] * self._num_joints + + self._joint_position_targets = [0.0] * self._num_joints + + for i in range(min(self._num_joints, self._model.nq)): + current_pos = float(self._data.qpos[i]) + self._joint_position_targets[i] = current_pos + self._joint_positions[i] = current_pos + + def _apply_control(self) -> None: + with self._lock: + pos_targets = list(self._joint_position_targets) + n_act = min(self._num_joints, self._model.nu) + with self._lock: + for i in range(n_act): + self._data.ctrl[i] = pos_targets[i] + + def _update_joint_state(self) -> None: + with self._lock: + n_q = min(self._num_joints, self._model.nq) + n_v = min(self._num_joints, self._model.nv) + n_act = min(self._num_joints, self._model.nu) + for i in range(n_q): + self._joint_positions[i] = float(self._data.qpos[i]) + for i in range(n_v): + self._joint_velocities[i] = float(self._data.qvel[i]) + for i in range(n_act): + self._joint_efforts[i] = float(self._data.qfrc_actuator[i]) + + def connect(self) -> None: + logger.info(f"{self.__class__.__name__}: connect()") + with self._lock: + self._connected = True + self._stop_event.clear() + + if self._sim_thread is None or not self._sim_thread.is_alive(): + self._sim_thread = threading.Thread( + target=self._sim_loop, + name=f"{self.__class__.__name__}Sim", + daemon=True, + ) + self._sim_thread.start() + + def disconnect(self) -> None: + logger.info(f"{self.__class__.__name__}: disconnect()") + with self._lock: + self._connected = False + + self._stop_event.set() + if self._sim_thread and self._sim_thread.is_alive(): + self._sim_thread.join(timeout=2.0) + self._sim_thread = None + + def _sim_loop(self) -> None: + logger.info(f"{self.__class__.__name__}: sim loop started") + dt = 1.0 / self._control_frequency + + def _step_once(sync_viewer: bool) -> None: + loop_start = time.time() + self._apply_control() + mujoco.mj_step(self._model, self._data) + if sync_viewer: + m_viewer.sync() + self._update_joint_state() + + elapsed = time.time() - loop_start + sleep_time = dt - elapsed + if sleep_time > 0: + time.sleep(sleep_time) + + if self._headless: + while not self._stop_event.is_set(): + _step_once(sync_viewer=False) + else: + with viewer.launch_passive( + self._model, self._data, show_left_ui=False, show_right_ui=False + ) as m_viewer: + while m_viewer.is_running() and not self._stop_event.is_set(): + _step_once(sync_viewer=True) + + logger.info(f"{self.__class__.__name__}: sim loop stopped") + + @property + def connected(self) -> bool: + with self._lock: + return self._connected + + @property + def num_joints(self) -> int: + return self._num_joints + + @property + def model(self) -> mujoco.MjModel: + return self._model + + @property + def joint_positions(self) -> list[float]: + with self._lock: + return list(self._joint_positions) + + @property + def joint_velocities(self) -> list[float]: + with self._lock: + return list(self._joint_velocities) + + @property + def joint_efforts(self) -> list[float]: + with self._lock: + return list(self._joint_efforts) + + @property + def control_frequency(self) -> float: + return self._control_frequency + + def read_joint_positions(self) -> list[float]: + return self.joint_positions + + def read_joint_velocities(self) -> list[float]: + return self.joint_velocities + + def read_joint_efforts(self) -> list[float]: + return self.joint_efforts + + def write_joint_positions(self, positions: list[float]) -> None: + with self._lock: + limit = min(len(positions), self._num_joints) + for i in range(limit): + self._joint_position_targets[i] = float(positions[i]) + + def write_joint_velocities(self, velocities: list[float]) -> None: + dt = 1.0 / self._control_frequency + with self._lock: + limit = min(len(velocities), self._num_joints) + for i in range(limit): + self._joint_position_targets[i] = ( + self._joint_positions[i] + float(velocities[i]) * dt + ) + + def hold_current_position(self) -> None: + with self._lock: + for i in range(min(self._num_joints, self._model.nq)): + current_pos = float(self._data.qpos[i]) + self._joint_position_targets[i] = current_pos + + +__all__ = [ + "MujocoEngine", +] From 6073d80594f27a93224ed0e982a11b6f8a3904b7 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 16 Jan 2026 17:02:22 -0800 Subject: [PATCH 11/43] manipulation interface that plugs into different sims --- .../manipulators/sim_manip_interface.py | 188 ++++++++++++++++++ 1 file changed, 188 insertions(+) create mode 100644 dimos/simulation/manipulators/sim_manip_interface.py diff --git a/dimos/simulation/manipulators/sim_manip_interface.py b/dimos/simulation/manipulators/sim_manip_interface.py new file mode 100644 index 0000000000..6b6cd7220e --- /dev/null +++ b/dimos/simulation/manipulators/sim_manip_interface.py @@ -0,0 +1,188 @@ +# 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. + +"""Simulation-agnostic manipulator interface.""" + +from __future__ import annotations + +import logging +import math +from typing import TYPE_CHECKING + +from dimos.hardware.manipulators.spec import ControlMode, JointLimits, ManipulatorInfo + +if TYPE_CHECKING: + from dimos.simulation.engines.base import RobotSpec, SimulationEngine + + +class SimManipInterface: + """Adapter wrapper around a simulation engine to provide a uniform manipulator API.""" + + def __init__(self, engine: SimulationEngine, spec: RobotSpec) -> None: + self.logger = logging.getLogger(self.__class__.__name__) + self._engine = engine + self._spec = spec + self._dof = int(spec.dof) if spec.dof is not None else 0 + self._connected = False + self._servos_enabled = False + self._control_mode = ControlMode.POSITION + self._error_code = 0 + self._error_message = "" + + def connect(self) -> bool: + """Connect to the simulation engine.""" + try: + self.logger.info("Connecting to simulation engine...") + self._engine.connect() + if self._engine.connected: + self._connected = True + self._servos_enabled = True + if self._dof <= 0: + self._dof = int(self._engine.num_joints) + self.logger.info("Successfully connected to simulation", extra={"dof": self._dof}) + return True + self.logger.error("Failed to connect to simulation engine") + return False + except Exception as exc: + self.logger.error(f"Sim connection failed: {exc}") + return False + + def disconnect(self) -> None: + """Disconnect from simulation.""" + try: + self._engine.disconnect() + finally: + self._connected = False + + def is_connected(self) -> bool: + return bool(self._connected and self._engine.connected) + + def get_info(self) -> ManipulatorInfo: + vendor = self._spec.vendor or "Simulation" + model = self._spec.model or self._spec.name + dof = self.get_dof() + return ManipulatorInfo( + vendor=vendor, + model=model, + dof=dof, + firmware_version=None, + serial_number=None, + ) + + def get_dof(self) -> int: + return self._dof + + def get_limits(self) -> JointLimits: + if self._spec.limits is not None: + return self._spec.limits + lower = [-math.pi] * self._dof + upper = [math.pi] * self._dof + max_vel_rad = math.radians(180.0) + return JointLimits( + position_lower=lower, + position_upper=upper, + velocity_max=[max_vel_rad] * self._dof, + ) + + def set_control_mode(self, mode: ControlMode) -> bool: + self._control_mode = mode + return True + + def get_control_mode(self) -> ControlMode: + return self._control_mode + + def read_joint_positions(self) -> list[float]: + positions = self._engine.read_joint_positions() + return positions[: self._dof] + + def read_joint_velocities(self) -> list[float]: + velocities = self._engine.read_joint_velocities() + return velocities[: self._dof] + + def read_joint_efforts(self) -> list[float]: + efforts = self._engine.read_joint_efforts() + return efforts[: self._dof] + + def read_state(self) -> dict[str, int]: + velocities = self.read_joint_velocities() + is_moving = any(abs(v) > 1e-4 for v in velocities) + mode_int = list(ControlMode).index(self._control_mode) + return { + "state": 1 if is_moving else 0, + "mode": mode_int, + } + + def read_error(self) -> tuple[int, str]: + return self._error_code, self._error_message + + def write_joint_positions(self, positions: list[float], velocity: float = 1.0) -> bool: + _ = velocity + if not self._servos_enabled: + return False + self._control_mode = ControlMode.POSITION + self._engine.write_joint_positions(positions[: self._dof]) + return True + + def write_joint_velocities(self, velocities: list[float]) -> bool: + if not self._servos_enabled: + return False + self._control_mode = ControlMode.VELOCITY + self._engine.write_joint_velocities(velocities[: self._dof]) + return True + + def write_stop(self) -> bool: + if hasattr(self._engine, "hold_current_position"): + self._engine.hold_current_position() # type: ignore[attr-defined] + return True + self._engine.write_joint_velocities([0.0] * self._dof) + return True + + def write_enable(self, enable: bool) -> bool: + self._servos_enabled = enable + return True + + def read_enabled(self) -> bool: + return self._servos_enabled + + def write_clear_errors(self) -> bool: + self._error_code = 0 + self._error_message = "" + return True + + def read_cartesian_position(self) -> dict[str, float] | None: + return None + + def write_cartesian_position( + self, + pose: dict[str, float], + velocity: float = 1.0, + ) -> bool: + _ = pose + _ = velocity + return False + + def read_gripper_position(self) -> float | None: + return None + + def write_gripper_position(self, position: float) -> bool: + _ = position + return False + + def read_force_torque(self) -> list[float] | None: + return None + + +__all__ = [ + "SimManipInterface", +] From 328a0193bca6b81ebecad91ffa41c60e142c7d54 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 16 Jan 2026 17:13:13 -0800 Subject: [PATCH 12/43] simulation model that supports any sim engine --- dimos/simulation/manipulators/sim_module.py | 278 ++++++++++++++++++++ 1 file changed, 278 insertions(+) create mode 100644 dimos/simulation/manipulators/sim_module.py diff --git a/dimos/simulation/manipulators/sim_module.py b/dimos/simulation/manipulators/sim_module.py new file mode 100644 index 0000000000..ff043c21c7 --- /dev/null +++ b/dimos/simulation/manipulators/sim_module.py @@ -0,0 +1,278 @@ +# 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. + +"""Simulator-agnostic manipulator simulation module.""" + +from __future__ import annotations + +from dataclasses import dataclass +import threading +import time +from typing import TYPE_CHECKING, Any + +from reactivex.disposable import Disposable + +from dimos.core import In, Module, Out, rpc +from dimos.core.module import ModuleConfig +from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState +from dimos.simulation.manipulators.sim_manip_interface import SimManipInterface +from dimos.simulation.registry import registry + +if TYPE_CHECKING: + from dimos.simulation.engines.base import RobotSpec + + +@dataclass +class SimulationModuleConfig(ModuleConfig): + engine: str | None = None + robot: str | None = None + config_path: str | None = None + headless: bool = False + + +class SimulationModule(Module[SimulationModuleConfig]): + """Module wrapper for manipulator simulation across engines.""" + + default_config = SimulationModuleConfig + config: SimulationModuleConfig + + joint_state: Out[JointState] + robot_state: Out[RobotState] + joint_position_command: In[JointCommand] + joint_velocity_command: In[JointCommand] + + def __init__(self, *args: Any, **kwargs: Any) -> None: + super().__init__(*args, **kwargs) + self._backend: SimManipInterface | None = None + self._spec: RobotSpec | None = None + self._control_rate = 100.0 + self._monitor_rate = 100.0 + self._joint_prefix = "joint" + self._stop_event = threading.Event() + self._control_thread: threading.Thread | None = None + self._monitor_thread: threading.Thread | None = None + self._command_lock = threading.Lock() + self._pending_positions: list[float] | None = None + self._pending_velocities: list[float] | None = None + + def _create_backend(self) -> SimManipInterface: + if not self.config.engine: + raise ValueError("engine is required for SimulationModule") + if not self.config.robot: + raise ValueError("robot is required for SimulationModule") + engine = registry.create_engine( + engine=self.config.engine, + robot=self.config.robot, + config_path=self.config.config_path, + headless=self.config.headless, + ) + self._spec = engine.spec + return SimManipInterface(engine=engine, spec=engine.spec) + + @rpc + def start(self) -> None: + super().start() + if self._backend is None: + self._backend = self._create_backend() + if not self._backend.connect(): + raise RuntimeError("Failed to connect to simulation backend") + self._backend.write_enable(True) + + try: + if ( + self.joint_position_command.connection is not None + or self.joint_position_command._transport is not None + ): + self._disposables.add( + Disposable( + self.joint_position_command.subscribe(self._on_joint_position_command) + ) + ) + except Exception as exc: + import logging + + logging.getLogger(self.__class__.__name__).warning( + f"Failed to subscribe joint_position_command: {exc}" + ) + + try: + if ( + self.joint_velocity_command.connection is not None + or self.joint_velocity_command._transport is not None + ): + self._disposables.add( + Disposable( + self.joint_velocity_command.subscribe(self._on_joint_velocity_command) + ) + ) + except Exception as exc: + import logging + + logging.getLogger(self.__class__.__name__).warning( + f"Failed to subscribe joint_velocity_command: {exc}" + ) + + self._stop_event.clear() + self._control_thread = threading.Thread( + target=self._control_loop, + daemon=True, + name=f"{self.__class__.__name__}-control", + ) + self._monitor_thread = threading.Thread( + target=self._monitor_loop, + daemon=True, + name=f"{self.__class__.__name__}-monitor", + ) + self._control_thread.start() + self._monitor_thread.start() + + @rpc + def stop(self) -> None: + self._stop_event.set() + if self._control_thread and self._control_thread.is_alive(): + self._control_thread.join(timeout=2.0) + if self._monitor_thread and self._monitor_thread.is_alive(): + self._monitor_thread.join(timeout=2.0) + if self._backend: + self._backend.disconnect() + super().stop() + + @rpc + def enable_servos(self) -> bool: + if not self._backend: + return False + return self._backend.write_enable(True) + + @rpc + def disable_servos(self) -> bool: + if not self._backend: + return False + return self._backend.write_enable(False) + + @rpc + def clear_errors(self) -> bool: + if not self._backend: + return False + return self._backend.write_clear_errors() + + @rpc + def emergency_stop(self) -> bool: + if not self._backend: + return False + return self._backend.write_stop() + + def _on_joint_position_command(self, msg: JointCommand) -> None: + with self._command_lock: + self._pending_positions = list(msg.positions) + self._pending_velocities = None + + def _on_joint_velocity_command(self, msg: JointCommand) -> None: + with self._command_lock: + self._pending_velocities = list(msg.positions) + self._pending_positions = None + + def _control_loop(self) -> None: + period = 1.0 / max(self._control_rate, 1.0) + while not self._stop_event.is_set(): + with self._command_lock: + positions = ( + None if self._pending_positions is None else list(self._pending_positions) + ) + velocities = ( + None if self._pending_velocities is None else list(self._pending_velocities) + ) + + if self._backend: + if positions is not None: + self._backend.write_joint_positions(positions) + elif velocities is not None: + self._backend.write_joint_velocities(velocities) + + time.sleep(period) + + def _monitor_loop(self) -> None: + period = 1.0 / max(self._monitor_rate, 1.0) + while not self._stop_event.is_set(): + if not self._backend: + time.sleep(period) + continue + dof = self._backend.get_dof() + names = self._resolve_joint_names(dof) + positions = self._backend.read_joint_positions() + velocities = self._backend.read_joint_velocities() + efforts = self._backend.read_joint_efforts() + state = self._backend.read_state() + error_code, _ = self._backend.read_error() + + self.joint_state.publish( + JointState( + frame_id=self.frame_id, + name=names, + position=positions, + velocity=velocities, + effort=efforts, + ) + ) + self.robot_state.publish( + RobotState( + state=state.get("state", 0), + mode=state.get("mode", 0), + error_code=error_code, + warn_code=0, + cmdnum=0, + mt_brake=0, + mt_able=1 if self._backend.read_enabled() else 0, + tcp_pose=[], + tcp_offset=[], + joints=[float(p) for p in positions], + ) + ) + time.sleep(period) + + def _resolve_joint_names(self, dof: int) -> list[str]: + if self._spec and self._spec.joint_names: + if len(self._spec.joint_names) >= dof: + return list(self._spec.joint_names[:dof]) + return [f"{self._joint_prefix}{i + 1}" for i in range(dof)] + + +def get_blueprint() -> dict[str, Any]: + return { + "name": "SimulationModule", + "class": SimulationModule, + "config": { + "engine": None, + "robot": None, + "config_path": None, + "headless": False, + }, + "inputs": { + "joint_position_command": "JointCommand", + "joint_velocity_command": "JointCommand", + }, + "outputs": { + "joint_state": "JointState", + "robot_state": "RobotState", + }, + } + + +simulation_module = SimulationModule.blueprint + +__all__ = [ + "SimulationModule", + "SimulationModuleConfig", + "get_blueprint", + "simulation_module", +] From c153c13ed38900ba38b665e29fdc4d9275d6f2f3 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 16 Jan 2026 17:19:54 -0800 Subject: [PATCH 13/43] add support for xarm in sim --- dimos/simulation/__init__.py | 4 + dimos/simulation/manipulators/__init__.py | 12 ++- dimos/simulation/plugins/__init__.py | 12 +++ dimos/simulation/plugins/mujoco_xarm.py | 91 +++++++++++++++++++++++ dimos/simulation/sim_blueprints.py | 2 + 5 files changed, 120 insertions(+), 1 deletion(-) create mode 100644 dimos/simulation/plugins/__init__.py create mode 100644 dimos/simulation/plugins/mujoco_xarm.py diff --git a/dimos/simulation/__init__.py b/dimos/simulation/__init__.py index 1a68191a36..f24b65cee4 100644 --- a/dimos/simulation/__init__.py +++ b/dimos/simulation/__init__.py @@ -1,3 +1,7 @@ +from dimos.simulation.plugins import register_all + +register_all() + # Try to import Isaac Sim components try: from .isaac import IsaacSimulator, IsaacStream diff --git a/dimos/simulation/manipulators/__init__.py b/dimos/simulation/manipulators/__init__.py index a907f25653..600251b421 100644 --- a/dimos/simulation/manipulators/__init__.py +++ b/dimos/simulation/manipulators/__init__.py @@ -12,14 +12,24 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""MuJoCo simulation backend utilities.""" +"""Simulation manipulator utilities.""" from dimos.simulation.manipulators.mujoco_driver import MujocoDriver from dimos.simulation.manipulators.mujoco_manip_interface import ( MujocoManipInterface, ) +from dimos.simulation.manipulators.sim_manip_interface import SimManipInterface +from dimos.simulation.manipulators.sim_module import ( + SimulationModule, + SimulationModuleConfig, + simulation_module, +) __all__ = [ "MujocoDriver", "MujocoManipInterface", + "SimManipInterface", + "SimulationModule", + "SimulationModuleConfig", + "simulation_module", ] diff --git a/dimos/simulation/plugins/__init__.py b/dimos/simulation/plugins/__init__.py new file mode 100644 index 0000000000..b5c200e780 --- /dev/null +++ b/dimos/simulation/plugins/__init__.py @@ -0,0 +1,12 @@ +"""Simulation engine and robot registration plugins.""" + +from dimos.simulation.plugins.mujoco_xarm import register as register_mujoco_xarm + + +def register_all() -> None: + register_mujoco_xarm() + + +__all__ = [ + "register_all", +] diff --git a/dimos/simulation/plugins/mujoco_xarm.py b/dimos/simulation/plugins/mujoco_xarm.py new file mode 100644 index 0000000000..8c8eb30fd3 --- /dev/null +++ b/dimos/simulation/plugins/mujoco_xarm.py @@ -0,0 +1,91 @@ +# 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. + +"""MuJoCo engine registration with xArm robot specs.""" + +from __future__ import annotations + +import math + +from dimos.hardware.manipulators.spec import JointLimits +from dimos.simulation.engines.base import RobotSpec +from dimos.simulation.engines.mujoco_engine import MujocoEngine +from dimos.simulation.registry import registry + + +def _xarm_limits(dof: int) -> JointLimits: + if dof == 7: + lower_deg = [-360, -118, -360, -233, -360, -97, -360] + upper_deg = [360, 118, 360, 11, 360, 180, 360] + elif dof == 6: + lower_deg = [-360, -118, -225, -11, -360, -97] + upper_deg = [360, 118, 11, 225, 360, 180] + else: + lower_deg = [-360, -118, -225, -97, -360] + upper_deg = [360, 118, 11, 180, 360] + + lower_rad = [math.radians(d) for d in lower_deg[:dof]] + upper_rad = [math.radians(d) for d in upper_deg[:dof]] + max_vel_rad = math.radians(180.0) + return JointLimits( + position_lower=lower_rad, + position_upper=upper_rad, + velocity_max=[max_vel_rad] * dof, + ) + + +def register() -> None: + registry.register_engine("mujoco", MujocoEngine) + + registry.register_robot( + "xarm7", + RobotSpec( + name="xarm7", + engine="mujoco", + asset="xarm7_mj_description", + dof=7, + vendor="UFACTORY", + model="xArm7", + limits=_xarm_limits(7), + ), + ) + registry.register_robot( + "xarm6", + RobotSpec( + name="xarm6", + engine="mujoco", + asset="xarm6_mj_description", + dof=6, + vendor="UFACTORY", + model="xArm6", + limits=_xarm_limits(6), + ), + ) + registry.register_robot( + "xarm5", + RobotSpec( + name="xarm5", + engine="mujoco", + asset="xarm5_mj_description", + dof=5, + vendor="UFACTORY", + model="xArm5", + limits=_xarm_limits(5), + ), + ) + + +__all__ = [ + "register", +] diff --git a/dimos/simulation/sim_blueprints.py b/dimos/simulation/sim_blueprints.py index 2dbe3e1a39..c46632f356 100644 --- a/dimos/simulation/sim_blueprints.py +++ b/dimos/simulation/sim_blueprints.py @@ -21,6 +21,7 @@ ) from dimos.msgs.trajectory_msgs import JointTrajectory from dimos.simulation.manipulators.mujoco_driver import mujoco_sim +from dimos.simulation.manipulators.sim_module import simulation_module xarm7_trajectory_sim = mujoco_sim( robot="xarm7_mj_description", @@ -39,6 +40,7 @@ __all__ = [ + "simulation_module", "xarm7_trajectory_sim", ] From 6d9e1354586228ee2698f40ad44a79e09d9f84a3 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 16 Jan 2026 19:18:33 -0800 Subject: [PATCH 14/43] clean up code --- dimos/simulation/manipulators/__init__.py | 4 ++-- dimos/simulation/manipulators/sim_module.py | 6 +++--- dimos/simulation/sim_blueprints.py | 12 ++++++------ 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/dimos/simulation/manipulators/__init__.py b/dimos/simulation/manipulators/__init__.py index 600251b421..ea18753d8f 100644 --- a/dimos/simulation/manipulators/__init__.py +++ b/dimos/simulation/manipulators/__init__.py @@ -22,7 +22,7 @@ from dimos.simulation.manipulators.sim_module import ( SimulationModule, SimulationModuleConfig, - simulation_module, + simulation, ) __all__ = [ @@ -31,5 +31,5 @@ "SimManipInterface", "SimulationModule", "SimulationModuleConfig", - "simulation_module", + "simulation", ] diff --git a/dimos/simulation/manipulators/sim_module.py b/dimos/simulation/manipulators/sim_module.py index ff043c21c7..5a236b2303 100644 --- a/dimos/simulation/manipulators/sim_module.py +++ b/dimos/simulation/manipulators/sim_module.py @@ -249,7 +249,7 @@ def _resolve_joint_names(self, dof: int) -> list[str]: def get_blueprint() -> dict[str, Any]: return { - "name": "SimulationModule", + "name": "simulation", "class": SimulationModule, "config": { "engine": None, @@ -268,11 +268,11 @@ def get_blueprint() -> dict[str, Any]: } -simulation_module = SimulationModule.blueprint +simulation = SimulationModule.blueprint __all__ = [ "SimulationModule", "SimulationModuleConfig", "get_blueprint", - "simulation_module", + "simulation", ] diff --git a/dimos/simulation/sim_blueprints.py b/dimos/simulation/sim_blueprints.py index c46632f356..451aa0a96c 100644 --- a/dimos/simulation/sim_blueprints.py +++ b/dimos/simulation/sim_blueprints.py @@ -20,13 +20,13 @@ RobotState, ) from dimos.msgs.trajectory_msgs import JointTrajectory -from dimos.simulation.manipulators.mujoco_driver import mujoco_sim -from dimos.simulation.manipulators.sim_module import simulation_module +from dimos.simulation.manipulators.sim_module import simulation -xarm7_trajectory_sim = mujoco_sim( - robot="xarm7_mj_description", +xarm7_trajectory_sim = simulation( + engine="mujoco", + robot="xarm7", config_path=None, - headless=False, + headless=True, ).transports( { ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), @@ -40,7 +40,7 @@ __all__ = [ - "simulation_module", + "simulation", "xarm7_trajectory_sim", ] From 86a3c06e83853ccf4c9614cfc4405edf71f908d9 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 16 Jan 2026 19:19:04 -0800 Subject: [PATCH 15/43] more cleanup --- dimos/simulation/manipulators/__init__.py | 6 - .../simulation/manipulators/mujoco_driver.py | 241 ------------------ .../manipulators/mujoco_manip_interface.py | 234 ----------------- .../manipulators/mujoco_sim_backend.py | 223 ---------------- .../manipulators/xarm_sim_driver.py | 133 ---------- dimos/simulation/test_publish_topic.py | 24 -- dimos/simulation/test_subscribe_topic.py | 31 --- 7 files changed, 892 deletions(-) delete mode 100644 dimos/simulation/manipulators/mujoco_driver.py delete mode 100644 dimos/simulation/manipulators/mujoco_manip_interface.py delete mode 100644 dimos/simulation/manipulators/mujoco_sim_backend.py delete mode 100644 dimos/simulation/manipulators/xarm_sim_driver.py delete mode 100644 dimos/simulation/test_publish_topic.py delete mode 100644 dimos/simulation/test_subscribe_topic.py diff --git a/dimos/simulation/manipulators/__init__.py b/dimos/simulation/manipulators/__init__.py index ea18753d8f..85ee0a1683 100644 --- a/dimos/simulation/manipulators/__init__.py +++ b/dimos/simulation/manipulators/__init__.py @@ -14,10 +14,6 @@ """Simulation manipulator utilities.""" -from dimos.simulation.manipulators.mujoco_driver import MujocoDriver -from dimos.simulation.manipulators.mujoco_manip_interface import ( - MujocoManipInterface, -) from dimos.simulation.manipulators.sim_manip_interface import SimManipInterface from dimos.simulation.manipulators.sim_module import ( SimulationModule, @@ -26,8 +22,6 @@ ) __all__ = [ - "MujocoDriver", - "MujocoManipInterface", "SimManipInterface", "SimulationModule", "SimulationModuleConfig", diff --git a/dimos/simulation/manipulators/mujoco_driver.py b/dimos/simulation/manipulators/mujoco_driver.py deleted file mode 100644 index f20a4bb7b7..0000000000 --- a/dimos/simulation/manipulators/mujoco_driver.py +++ /dev/null @@ -1,241 +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. - -"""Robot-agnostic MuJoCo simulation driver module.""" - -from dataclasses import dataclass -import threading -import time -from typing import Any - -from reactivex.disposable import Disposable - -from dimos.core import In, Module, Out, rpc -from dimos.core.module import ModuleConfig -from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState -from dimos.simulation.manipulators.mujoco_manip_interface import MujocoManipInterface - - -@dataclass -class MujocoDriverConfig(ModuleConfig): - robot: str | None = None - config_path: str | None = None - headless: bool = False - - -class MujocoDriver(Module[MujocoDriverConfig]): - """Module wrapper for MuJoCo manipulator simulation.""" - - default_config = MujocoDriverConfig - config: MujocoDriverConfig - - joint_state: Out[JointState] - robot_state: Out[RobotState] - joint_position_command: In[JointCommand] - joint_velocity_command: In[JointCommand] - - def __init__(self, *args: Any, **kwargs: Any) -> None: - super().__init__(*args, **kwargs) - self._backend = self._create_backend() - self._control_rate = 100.0 - self._monitor_rate = 100.0 - self._joint_prefix = "joint" - self._stop_event = threading.Event() - self._control_thread: threading.Thread | None = None - self._monitor_thread: threading.Thread | None = None - self._command_lock = threading.Lock() - self._pending_positions: list[float] | None = None - self._pending_velocities: list[float] | None = None - - def _create_backend(self) -> MujocoManipInterface: - return MujocoManipInterface( - robot=self.config.robot or "", - config_path=self.config.config_path, - headless=self.config.headless, - ) - - @rpc - def start(self) -> None: - super().start() - if not self._backend.connect(): - raise RuntimeError("Failed to connect to MuJoCo simulation backend") - self._backend.write_enable(True) - - try: - if ( - self.joint_position_command.connection is not None - or self.joint_position_command._transport is not None - ): - self._disposables.add( - Disposable( - self.joint_position_command.subscribe(self._on_joint_position_command) - ) - ) - except Exception as exc: - import logging - - logging.getLogger(self.__class__.__name__).warning( - f"Failed to subscribe joint_position_command: {exc}" - ) - - try: - if ( - self.joint_velocity_command.connection is not None - or self.joint_velocity_command._transport is not None - ): - self._disposables.add( - Disposable( - self.joint_velocity_command.subscribe(self._on_joint_velocity_command) - ) - ) - except Exception as exc: - import logging - - logging.getLogger(self.__class__.__name__).warning( - f"Failed to subscribe joint_velocity_command: {exc}" - ) - - self._stop_event.clear() - self._control_thread = threading.Thread( - target=self._control_loop, - daemon=True, - name=f"{self.__class__.__name__}-control", - ) - self._monitor_thread = threading.Thread( - target=self._monitor_loop, - daemon=True, - name=f"{self.__class__.__name__}-monitor", - ) - self._control_thread.start() - self._monitor_thread.start() - - @rpc - def stop(self) -> None: - self._stop_event.set() - if self._control_thread and self._control_thread.is_alive(): - self._control_thread.join(timeout=2.0) - if self._monitor_thread and self._monitor_thread.is_alive(): - self._monitor_thread.join(timeout=2.0) - self._backend.disconnect() - super().stop() - - @rpc - def enable_servos(self) -> bool: - return self._backend.write_enable(True) - - @rpc - def disable_servos(self) -> bool: - return self._backend.write_enable(False) - - @rpc - def clear_errors(self) -> bool: - return self._backend.write_clear_errors() - - @rpc - def emergency_stop(self) -> bool: - return self._backend.write_stop() - - def _on_joint_position_command(self, msg: JointCommand) -> None: - with self._command_lock: - self._pending_positions = list(msg.positions) - self._pending_velocities = None - - def _on_joint_velocity_command(self, msg: JointCommand) -> None: - with self._command_lock: - self._pending_velocities = list(msg.positions) - self._pending_positions = None - - def _control_loop(self) -> None: - period = 1.0 / max(self._control_rate, 1.0) - while not self._stop_event.is_set(): - with self._command_lock: - positions = ( - None if self._pending_positions is None else list(self._pending_positions) - ) - velocities = ( - None if self._pending_velocities is None else list(self._pending_velocities) - ) - - if positions is not None: - self._backend.write_joint_positions(positions) - elif velocities is not None: - self._backend.write_joint_velocities(velocities) - - time.sleep(period) - - def _monitor_loop(self) -> None: - period = 1.0 / max(self._monitor_rate, 1.0) - names = [f"{self._joint_prefix}{i + 1}" for i in range(self._backend.get_dof())] - while not self._stop_event.is_set(): - positions = self._backend.read_joint_positions() - velocities = self._backend.read_joint_velocities() - efforts = self._backend.read_joint_efforts() - state = self._backend.read_state() - error_code, _ = self._backend.read_error() - - self.joint_state.publish( - JointState( - frame_id=self.frame_id, - name=names, - position=positions, - velocity=velocities, - effort=efforts, - ) - ) - self.robot_state.publish( - RobotState( - state=state.get("state", 0), - mode=state.get("mode", 0), - error_code=error_code, - warn_code=0, - cmdnum=0, - mt_brake=0, - mt_able=1 if self._backend.read_enabled() else 0, - tcp_pose=[], - tcp_offset=[], - joints=[float(p) for p in positions], - ) - ) - time.sleep(period) - - -def get_blueprint() -> dict[str, Any]: - return { - "name": "MujocoDriver", - "class": MujocoDriver, - "config": { - "robot": None, - "config_path": None, - "headless": False, - }, - "inputs": { - "joint_position_command": "JointCommand", - "joint_velocity_command": "JointCommand", - }, - "outputs": { - "joint_state": "JointState", - "robot_state": "RobotState", - }, - } - - -mujoco_sim = MujocoDriver.blueprint - - -__all__ = [ - "MujocoDriver", - "MujocoDriverConfig", - "get_blueprint", - "mujoco_sim", -] diff --git a/dimos/simulation/manipulators/mujoco_manip_interface.py b/dimos/simulation/manipulators/mujoco_manip_interface.py deleted file mode 100644 index 1151fee50b..0000000000 --- a/dimos/simulation/manipulators/mujoco_manip_interface.py +++ /dev/null @@ -1,234 +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. - -"""MuJoCo simulation adapter wrapper.""" - -import logging -import math - -from dimos.hardware.manipulators.spec import ControlMode, JointLimits, ManipulatorInfo -from dimos.simulation.manipulators.mujoco_sim_backend import MujocoSimBackend - - -class MujocoManipInterface: - """ - Adapter wrapper around a MuJoCo simulation backend to provide a uniform interface for all manipulators. - """ - - def __init__( - self, - robot: str, - config_path: str | None = None, - headless: bool = False, - dof: int | None = None, - ) -> None: - self.logger = logging.getLogger(self.__class__.__name__) - self._robot = robot - self._config_path = config_path - self._headless = headless - self._native: MujocoSimBackend | None = None - self._dof = int(dof) if dof is not None else 0 - self._connected = False - self._servos_enabled = False - self._control_mode = ControlMode.POSITION - self._error_code = 0 - self._error_message = "" - - def connect(self) -> bool: - """Connect to the MuJoCo simulation backend.""" - if not self._robot: - raise ValueError("robot is required for MuJoCo simulation loading") - try: - self.logger.info("Connecting to MuJoCo Sim...") - self._native = MujocoSimBackend( - robot=self._robot, - config_path=self._config_path, - headless=self._headless, - ) - self._native.connect() - - if self._native.connected: - self._connected = True - self._servos_enabled = True - if self._dof <= 0: - self._dof = int(self._native.num_joints) - self.logger.info("Successfully connected to MuJoCo Sim", extra={"dof": self._dof}) - return True - - self.logger.error("Failed to connect to MuJoCo Sim") - return False - except Exception as exc: - self.logger.error(f"Sim connection failed: {exc}") - return False - - def disconnect(self) -> None: - """Disconnect from simulation.""" - if self._native: - try: - self._native.disconnect() - finally: - self._connected = False - self._native = None - - def is_connected(self) -> bool: - return bool(self._connected and self._native and self._native.connected) - - def get_info(self) -> ManipulatorInfo: - return ManipulatorInfo( - vendor="MuJoCo", - model=self._robot, - dof=self._dof, - firmware_version=None, - serial_number=None, - ) - - def get_dof(self) -> int: - return self._dof - - def get_limits(self) -> JointLimits: - if not self._native: - lower = [-math.pi] * self._dof - upper = [math.pi] * self._dof - max_vel_rad = math.radians(180.0) - return JointLimits( - position_lower=lower, - position_upper=upper, - velocity_max=[max_vel_rad] * self._dof, - ) - ranges = getattr(self._native.model, "jnt_range", None) - if ranges is None or len(ranges) == 0: - lower = [-math.pi] * self._dof - upper = [math.pi] * self._dof - else: - limit = min(len(ranges), self._dof) - lower = [float(ranges[i][0]) for i in range(limit)] - upper = [float(ranges[i][1]) for i in range(limit)] - if limit < self._dof: - lower.extend([-math.pi] * (self._dof - limit)) - upper.extend([math.pi] * (self._dof - limit)) - max_vel_rad = math.radians(180.0) - return JointLimits( - position_lower=lower, - position_upper=upper, - velocity_max=[max_vel_rad] * self._dof, - ) - - def set_control_mode(self, mode: ControlMode) -> bool: - self._control_mode = mode - return True - - def get_control_mode(self) -> ControlMode: - return self._control_mode - - def read_joint_positions(self) -> list[float]: - if self._native: - return self._native.joint_positions[: self._dof] - return [0.0] * self._dof - - def read_joint_velocities(self) -> list[float]: - if self._native: - return self._native.joint_velocities[: self._dof] - return [0.0] * self._dof - - def read_joint_efforts(self) -> list[float]: - if self._native: - return self._native.joint_efforts[: self._dof] - return [0.0] * self._dof - - def read_state(self) -> dict[str, int]: - velocities = self.read_joint_velocities() - is_moving = any(abs(v) > 1e-4 for v in velocities) - mode_int = list(ControlMode).index(self._control_mode) - return { - "state": 1 if is_moving else 0, - "mode": mode_int, - } - - def read_error(self) -> tuple[int, str]: - return self._error_code, self._error_message - - def write_joint_positions( - self, - positions: list[float], - velocity: float = 1.0, - ) -> bool: - _ = velocity - if not self._servos_enabled or not self._native: - return False - self._control_mode = ControlMode.POSITION - self._native.set_joint_position_targets(positions[: self._dof]) - return True - - def write_joint_velocities(self, velocities: list[float]) -> bool: - if not self._servos_enabled or not self._native: - return False - self._control_mode = ControlMode.VELOCITY - dt = 1.0 / self._native.control_frequency - with self._native._lock: - current = list(self._native._joint_positions) - targets = [ - current[i] + velocities[i] * dt for i in range(min(len(velocities), self._dof)) - ] - self._native.set_joint_position_targets(targets) - return True - - def write_stop(self) -> bool: - if not self._native: - return False - self._native.hold_current_position() - return True - - def write_enable(self, enable: bool) -> bool: - self._servos_enabled = enable - return True - - def read_enabled(self) -> bool: - return self._servos_enabled - - def write_clear_errors(self) -> bool: - self._error_code = 0 - self._error_message = "" - return True - - def read_cartesian_position(self) -> dict[str, float] | None: - return None - - def write_cartesian_position( - self, - pose: dict[str, float], - velocity: float = 1.0, - ) -> bool: - _ = pose - _ = velocity - return False - - def read_gripper_position(self) -> float | None: - return None - - def write_gripper_position(self, position: float) -> bool: - _ = position - return False - - def read_force_torque(self) -> list[float] | None: - return None - - -class MujocoManipSDKWrapper(MujocoManipInterface): - """Backward-compatible alias for MujocoManipInterface.""" - - -__all__ = [ - "MujocoManipInterface", - "MujocoManipSDKWrapper", -] diff --git a/dimos/simulation/manipulators/mujoco_sim_backend.py b/dimos/simulation/manipulators/mujoco_sim_backend.py deleted file mode 100644 index 45eeebea95..0000000000 --- a/dimos/simulation/manipulators/mujoco_sim_backend.py +++ /dev/null @@ -1,223 +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 pathlib import Path -import threading -import time - -import mujoco -import mujoco.viewer as viewer # type: ignore[import-untyped] -from robot_descriptions.loaders.mujoco import ( - load_robot_description, # type: ignore[import-not-found] -) - -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -class MujocoSimBackend: - """ - Base class for MuJoCo simulation backend. - - - starts mujoco simulation engine - - loads robot/environment into simulation - - applies control commands - """ - - def __init__( - self, - robot: str, - config_path: str | None = None, - headless: bool = False, - ): - """ - Args: - robot: Robot description (from robot_descriptions library) name (e.g., "piper", "xarm7_mj_description"). - config_path: Path to a MuJoCo XML or a folder containing scene.xml. - headless: Run without launching the MuJoCo GUI viewer. - """ - self._robot_name = robot - self._headless = headless - - if config_path: # given config dir/file - resolved = Path(config_path).expanduser() - xml_path = resolved / "scene.xml" if resolved.is_dir() else resolved - if not xml_path.exists(): - raise FileNotFoundError(f"MuJoCo XML not found: {xml_path}") - self._model = mujoco.MjModel.from_xml_path(str(xml_path)) - else: - self._model = load_robot_description(robot) - - self._data = mujoco.MjData(self._model) - self._num_joints: int = int(self._model.nq) - timestep = float(self._model.opt.timestep) - self._control_frequency = 1.0 / timestep if timestep > 0.0 else 100.0 - - self._connected: bool = False - - self._lock = threading.Lock() - self._stop_event = threading.Event() - self._sim_thread: threading.Thread | None = None - - self._joint_positions = [0.0] * self._num_joints - self._joint_velocities = [0.0] * self._num_joints - self._joint_efforts = [0.0] * self._num_joints - - self._joint_position_targets = [0.0] * self._num_joints - - for i in range(min(self._num_joints, self._model.nq)): - current_pos = float(self._data.qpos[i]) - self._joint_position_targets[i] = current_pos - self._joint_positions[i] = current_pos - - def _apply_control(self) -> None: - """ - Apply control commands to MuJoCo actuators. - """ - with self._lock: - pos_targets = list(self._joint_position_targets) - n_act = min(self._num_joints, self._model.nu) - with self._lock: - for i in range(n_act): - self._data.ctrl[i] = pos_targets[i] - - def _update_joint_state(self) -> None: - """ - Update internal joint state from MuJoCo simulation. - """ - with self._lock: - n_q = min(self._num_joints, self._model.nq) - n_v = min(self._num_joints, self._model.nv) - n_act = min(self._num_joints, self._model.nu) - for i in range(n_q): - self._joint_positions[i] = float(self._data.qpos[i]) - for i in range(n_v): - self._joint_velocities[i] = float(self._data.qvel[i]) - for i in range(n_act): - self._joint_efforts[i] = float(self._data.qfrc_actuator[i]) - - def connect(self) -> None: - """Connect to simulation and start the simulation loop.""" - logger.info(f"{self.__class__.__name__}: connect()") - with self._lock: - self._connected = True - self._stop_event.clear() - - if self._sim_thread is None or not self._sim_thread.is_alive(): - self._sim_thread = threading.Thread( - target=self._sim_loop, - name=f"{self.__class__.__name__}Sim", - daemon=True, - ) - self._sim_thread.start() - - def disconnect(self) -> None: - """Disconnect from simulation and stop the simulation loop.""" - logger.info(f"{self.__class__.__name__}: disconnect()") - with self._lock: - self._connected = False - - self._stop_event.set() - if self._sim_thread and self._sim_thread.is_alive(): - self._sim_thread.join(timeout=2.0) - self._sim_thread = None - - def _sim_loop(self) -> None: - """ - Main simulation loop running MuJoCo. - """ - logger.info(f"{self.__class__.__name__}: sim loop started") - dt = 1.0 / self._control_frequency - - def _step_once(sync_viewer: bool) -> None: # helper to step sim - loop_start = time.time() - self._apply_control() - mujoco.mj_step(self._model, self._data) - if sync_viewer: - m_viewer.sync() - self._update_joint_state() - - elapsed = time.time() - loop_start - sleep_time = dt - elapsed - if sleep_time > 0: - time.sleep(sleep_time) - - if self._headless: - while not self._stop_event.is_set(): - _step_once(sync_viewer=False) - else: - with viewer.launch_passive( - self._model, self._data, show_left_ui=False, show_right_ui=False - ) as m_viewer: - while m_viewer.is_running() and not self._stop_event.is_set(): - _step_once(sync_viewer=True) - - logger.info(f"{self.__class__.__name__}: sim loop stopped") - - @property - def connected(self) -> bool: - """Whether the backend is connected to simulation.""" - with self._lock: - return self._connected - - @property - def num_joints(self) -> int: - """Number of joints in the robot.""" - return self._num_joints - - @property - def model(self) -> mujoco.MjModel: - """MuJoCo model (read-only).""" - return self._model - - @property - def joint_positions(self) -> list[float]: - """Current joint positions in radians (thread-safe copy).""" - with self._lock: - return list(self._joint_positions) - - @property - def joint_velocities(self) -> list[float]: - """Current joint velocities in rad/s (thread-safe copy).""" - with self._lock: - return list(self._joint_velocities) - - @property - def joint_efforts(self) -> list[float]: - """Current joint efforts/torques (thread-safe copy).""" - with self._lock: - return list(self._joint_efforts) - - @property - def control_frequency(self) -> float: - """Control loop frequency derived from the MuJoCo model.""" - return self._control_frequency - - def set_joint_position_targets(self, positions: list[float]) -> None: - """Set joint position targets in radians.""" - with self._lock: - limit = min(len(positions), self._num_joints) - for i in range(limit): - self._joint_position_targets[i] = float(positions[i]) - - def hold_current_position(self) -> None: - """Lock joints at their current positions.""" - with self._lock: - for i in range(min(self._num_joints, self._model.nq)): - current_pos = float(self._data.qpos[i]) - self._joint_position_targets[i] = current_pos diff --git a/dimos/simulation/manipulators/xarm_sim_driver.py b/dimos/simulation/manipulators/xarm_sim_driver.py deleted file mode 100644 index f588b2b1cd..0000000000 --- a/dimos/simulation/manipulators/xarm_sim_driver.py +++ /dev/null @@ -1,133 +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. - -"""MuJoCo-native xArm simulation backend and module wrapper.""" - -from dataclasses import dataclass -import math -from typing import Any - -from dimos.core import rpc -from dimos.hardware.manipulators.spec import JointLimits, ManipulatorInfo -from dimos.simulation.manipulators.mujoco_driver import MujocoDriver, MujocoDriverConfig -from dimos.simulation.manipulators.mujoco_manip_interface import MujocoManipInterface - - -class XArmSimBackend(MujocoManipInterface): - """xArm specific implementation of the MuJoCo manipulator interface.""" - - def __init__( - self, - robot: str, - dof: int = 7, - config_path: str | None = None, - headless: bool = False, - ) -> None: - super().__init__( - robot=robot, - config_path=config_path, - headless=headless, - dof=dof, - ) - - def get_info(self) -> ManipulatorInfo: - return ManipulatorInfo( - vendor="UFACTORY", - model=f"xArm{self._dof}", - dof=self._dof, - firmware_version=None, - serial_number=None, - ) - - def get_limits(self) -> JointLimits: - if self._dof == 7: - lower_deg = [-360, -118, -360, -233, -360, -97, -360] - upper_deg = [360, 118, 360, 11, 360, 180, 360] - elif self._dof == 6: - lower_deg = [-360, -118, -225, -11, -360, -97] - upper_deg = [360, 118, 11, 225, 360, 180] - else: - lower_deg = [-360, -118, -225, -97, -360] - upper_deg = [360, 118, 11, 180, 360] - - lower_rad = [math.radians(d) for d in lower_deg[: self._dof]] - upper_rad = [math.radians(d) for d in upper_deg[: self._dof]] - max_vel_rad = math.radians(180.0) - return JointLimits( - position_lower=lower_rad, - position_upper=upper_rad, - velocity_max=[max_vel_rad] * self._dof, - ) - - -class XArmSimSDKWrapper(XArmSimBackend): - """Backward-compatible alias for XArmSimBackend.""" - - -@dataclass -class XArmSimDriverConfig(MujocoDriverConfig): - pass - - -class XArmSimDriver(MujocoDriver): - """xArm simulation driver module using MuJoCo backend.""" - - default_config = XArmSimDriverConfig - config: XArmSimDriverConfig - - def _create_backend(self) -> MujocoManipInterface: - return XArmSimBackend( - robot=self.config.robot or "", - config_path=self.config.config_path, - headless=self.config.headless, - ) - - @rpc - def start(self) -> None: - if not self.config.robot: - raise ValueError("robot is required for XArmSimDriver") - super().start() - - -def get_blueprint() -> dict[str, Any]: - return { - "name": "XArmSimDriver", - "class": XArmSimDriver, - "config": { - "robot": None, - "config_path": None, - "headless": False, - }, - "inputs": { - "joint_position_command": "JointCommand", - "joint_velocity_command": "JointCommand", - }, - "outputs": { - "joint_state": "JointState", - "robot_state": "RobotState", - }, - } - - -xarm_sim_driver = XArmSimDriver.blueprint - - -__all__ = [ - "XArmSimBackend", - "XArmSimDriver", - "XArmSimDriverConfig", - "XArmSimSDKWrapper", - "get_blueprint", - "xarm_sim_driver", -] diff --git a/dimos/simulation/test_publish_topic.py b/dimos/simulation/test_publish_topic.py deleted file mode 100644 index ba035b5b37..0000000000 --- a/dimos/simulation/test_publish_topic.py +++ /dev/null @@ -1,24 +0,0 @@ -#!/usr/bin/env python3 -# 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. - -from dimos.core.transport import LCMTransport -from dimos.msgs.sensor_msgs import JointCommand - -TOPIC = "/xarm/joint_position_command" -POSITIONS = [1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] - -transport = LCMTransport(TOPIC, JointCommand) -message = JointCommand(positions=POSITIONS) -transport.broadcast(None, message) diff --git a/dimos/simulation/test_subscribe_topic.py b/dimos/simulation/test_subscribe_topic.py deleted file mode 100644 index e674fbf63b..0000000000 --- a/dimos/simulation/test_subscribe_topic.py +++ /dev/null @@ -1,31 +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. - -import time - -from dimos.core.transport import LCMTransport -from dimos.msgs.sensor_msgs import JointState - -t = LCMTransport("/xarm/joint_states", JointState) - - -def cb(msg: JointState): - # print("names:", msg.name) - print("pos :", " ".join(f"{x:.4f}" for x in msg.position)) - - -t.subscribe(cb) - -while True: - time.sleep(0.1) From 412164052cd842082ff29310a8b8903d00868c5b Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Sat, 17 Jan 2026 22:13:11 -0800 Subject: [PATCH 16/43] mypy error fix --- dimos/simulation/engines/mujoco_engine.py | 4 +--- pyproject.toml | 1 + 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/dimos/simulation/engines/mujoco_engine.py b/dimos/simulation/engines/mujoco_engine.py index 950b11d4c6..31c0a0c0c9 100644 --- a/dimos/simulation/engines/mujoco_engine.py +++ b/dimos/simulation/engines/mujoco_engine.py @@ -22,9 +22,7 @@ import mujoco import mujoco.viewer as viewer # type: ignore[import-untyped] -from robot_descriptions.loaders.mujoco import ( - load_robot_description, # type: ignore[import-not-found] -) +from robot_descriptions.loaders.mujoco import load_robot_description from dimos.simulation.engines.base import RobotSpec, SimulationEngine from dimos.utils.logging_config import setup_logger diff --git a/pyproject.toml b/pyproject.toml index d565f5ebe1..28d6114813 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -316,6 +316,7 @@ module = [ "tf2_msgs.*", "mujoco", "mujoco_playground.*", + "robot_descriptions.*", "etils", "xarm.*", "dimos_lcm.*", From a2b66f7c77dade3a2f378fbb21f173f53c773781 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Thu, 22 Jan 2026 16:51:30 -0800 Subject: [PATCH 17/43] delete joint limits --- dimos/simulation/plugins/mujoco_xarm.py | 27 ------------------------- 1 file changed, 27 deletions(-) diff --git a/dimos/simulation/plugins/mujoco_xarm.py b/dimos/simulation/plugins/mujoco_xarm.py index 8c8eb30fd3..35c2f68511 100644 --- a/dimos/simulation/plugins/mujoco_xarm.py +++ b/dimos/simulation/plugins/mujoco_xarm.py @@ -16,35 +16,11 @@ from __future__ import annotations -import math - -from dimos.hardware.manipulators.spec import JointLimits from dimos.simulation.engines.base import RobotSpec from dimos.simulation.engines.mujoco_engine import MujocoEngine from dimos.simulation.registry import registry -def _xarm_limits(dof: int) -> JointLimits: - if dof == 7: - lower_deg = [-360, -118, -360, -233, -360, -97, -360] - upper_deg = [360, 118, 360, 11, 360, 180, 360] - elif dof == 6: - lower_deg = [-360, -118, -225, -11, -360, -97] - upper_deg = [360, 118, 11, 225, 360, 180] - else: - lower_deg = [-360, -118, -225, -97, -360] - upper_deg = [360, 118, 11, 180, 360] - - lower_rad = [math.radians(d) for d in lower_deg[:dof]] - upper_rad = [math.radians(d) for d in upper_deg[:dof]] - max_vel_rad = math.radians(180.0) - return JointLimits( - position_lower=lower_rad, - position_upper=upper_rad, - velocity_max=[max_vel_rad] * dof, - ) - - def register() -> None: registry.register_engine("mujoco", MujocoEngine) @@ -57,7 +33,6 @@ def register() -> None: dof=7, vendor="UFACTORY", model="xArm7", - limits=_xarm_limits(7), ), ) registry.register_robot( @@ -69,7 +44,6 @@ def register() -> None: dof=6, vendor="UFACTORY", model="xArm6", - limits=_xarm_limits(6), ), ) registry.register_robot( @@ -81,7 +55,6 @@ def register() -> None: dof=5, vendor="UFACTORY", model="xArm5", - limits=_xarm_limits(5), ), ) From 91cd6a3965dd8ff309037fef35ed6b4763c4e705 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Thu, 22 Jan 2026 19:09:45 -0800 Subject: [PATCH 18/43] remove get_blueprint() and set constant --- dimos/simulation/manipulators/sim_module.py | 29 +++------------------ 1 file changed, 4 insertions(+), 25 deletions(-) diff --git a/dimos/simulation/manipulators/sim_module.py b/dimos/simulation/manipulators/sim_module.py index 5a236b2303..b94b9c18be 100644 --- a/dimos/simulation/manipulators/sim_module.py +++ b/dimos/simulation/manipulators/sim_module.py @@ -52,6 +52,8 @@ class SimulationModule(Module[SimulationModuleConfig]): joint_position_command: In[JointCommand] joint_velocity_command: In[JointCommand] + MIN_CONTROL_RATE = 1.0 + def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) self._backend: SimManipInterface | None = None @@ -183,7 +185,7 @@ def _on_joint_velocity_command(self, msg: JointCommand) -> None: self._pending_positions = None def _control_loop(self) -> None: - period = 1.0 / max(self._control_rate, 1.0) + period = 1.0 / max(self._control_rate, self.MIN_CONTROL_RATE) while not self._stop_event.is_set(): with self._command_lock: positions = ( @@ -198,11 +200,10 @@ def _control_loop(self) -> None: self._backend.write_joint_positions(positions) elif velocities is not None: self._backend.write_joint_velocities(velocities) - time.sleep(period) def _monitor_loop(self) -> None: - period = 1.0 / max(self._monitor_rate, 1.0) + period = 1.0 / max(self._monitor_rate, self.MIN_CONTROL_RATE) while not self._stop_event.is_set(): if not self._backend: time.sleep(period) @@ -247,32 +248,10 @@ def _resolve_joint_names(self, dof: int) -> list[str]: return [f"{self._joint_prefix}{i + 1}" for i in range(dof)] -def get_blueprint() -> dict[str, Any]: - return { - "name": "simulation", - "class": SimulationModule, - "config": { - "engine": None, - "robot": None, - "config_path": None, - "headless": False, - }, - "inputs": { - "joint_position_command": "JointCommand", - "joint_velocity_command": "JointCommand", - }, - "outputs": { - "joint_state": "JointState", - "robot_state": "RobotState", - }, - } - - simulation = SimulationModule.blueprint __all__ = [ "SimulationModule", "SimulationModuleConfig", - "get_blueprint", "simulation", ] From bcbe1be55c03f4f1fb9f80268ede5704b38f2f9b Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 23 Jan 2026 15:06:58 -0800 Subject: [PATCH 19/43] sim module takes in xml file and parses it for robot properties --- dimos/simulation/__init__.py | 4 - dimos/simulation/engines/__init__.py | 3 +- dimos/simulation/engines/base.py | 33 +-- dimos/simulation/engines/mujoco_engine.py | 105 ++++--- .../manipulators/sim_manip_interface.py | 28 +- dimos/simulation/manipulators/sim_module.py | 34 +-- dimos/simulation/manipulators/xml_parser.py | 274 ++++++++++++++++++ dimos/simulation/plugins/__init__.py | 12 - dimos/simulation/plugins/mujoco_xarm.py | 64 ---- dimos/simulation/registry.py | 79 ----- dimos/simulation/sim_blueprints.py | 5 +- pyproject.toml | 1 - 12 files changed, 388 insertions(+), 254 deletions(-) create mode 100644 dimos/simulation/manipulators/xml_parser.py delete mode 100644 dimos/simulation/plugins/__init__.py delete mode 100644 dimos/simulation/plugins/mujoco_xarm.py delete mode 100644 dimos/simulation/registry.py diff --git a/dimos/simulation/__init__.py b/dimos/simulation/__init__.py index f24b65cee4..1a68191a36 100644 --- a/dimos/simulation/__init__.py +++ b/dimos/simulation/__init__.py @@ -1,7 +1,3 @@ -from dimos.simulation.plugins import register_all - -register_all() - # Try to import Isaac Sim components try: from .isaac import IsaacSimulator, IsaacStream diff --git a/dimos/simulation/engines/__init__.py b/dimos/simulation/engines/__init__.py index ab8b53230f..4f4ecbbfdd 100644 --- a/dimos/simulation/engines/__init__.py +++ b/dimos/simulation/engines/__init__.py @@ -1,8 +1,7 @@ """Simulation engines for manipulator backends.""" -from dimos.simulation.engines.base import RobotSpec, SimulationEngine +from dimos.simulation.engines.base import SimulationEngine __all__ = [ - "RobotSpec", "SimulationEngine", ] diff --git a/dimos/simulation/engines/base.py b/dimos/simulation/engines/base.py index f707486a2e..89dd4fb4e3 100644 --- a/dimos/simulation/engines/base.py +++ b/dimos/simulation/engines/base.py @@ -12,44 +12,20 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Base interfaces for simulator engines and robot specs.""" +"""Base interfaces for simulator engines.""" from __future__ import annotations from abc import ABC, abstractmethod -from dataclasses import dataclass -from typing import TYPE_CHECKING - -if TYPE_CHECKING: - from dimos.hardware.manipulators.spec import JointLimits - - -@dataclass(frozen=True) -class RobotSpec: - """Robot description metadata for simulation engines.""" - - name: str - engine: str - asset: str | None = None - dof: int | None = None - joint_names: list[str] | None = None - limits: JointLimits | None = None - vendor: str | None = None - model: str | None = None class SimulationEngine(ABC): """Abstract base class for a simulator engine instance.""" - def __init__(self, spec: RobotSpec, config_path: str | None, headless: bool) -> None: - self._spec = spec + def __init__(self, config_path: str | None, headless: bool) -> None: self._config_path = config_path self._headless = headless - @property - def spec(self) -> RobotSpec: - return self._spec - @property def config_path(self) -> str | None: return self._config_path @@ -76,6 +52,11 @@ def connected(self) -> bool: def num_joints(self) -> int: """Number of joints for the loaded robot.""" + @property + @abstractmethod + def joint_names(self) -> list[str]: + """Joint names for the loaded robot.""" + @abstractmethod def read_joint_positions(self) -> list[float]: """Read joint positions in radians.""" diff --git a/dimos/simulation/engines/mujoco_engine.py b/dimos/simulation/engines/mujoco_engine.py index 31c0a0c0c9..deac563fbd 100644 --- a/dimos/simulation/engines/mujoco_engine.py +++ b/dimos/simulation/engines/mujoco_engine.py @@ -22,9 +22,9 @@ import mujoco import mujoco.viewer as viewer # type: ignore[import-untyped] -from robot_descriptions.loaders.mujoco import load_robot_description -from dimos.simulation.engines.base import RobotSpec, SimulationEngine +from dimos.simulation.engines.base import SimulationEngine +from dimos.simulation.manipulators.xml_parser import JointMapping, build_joint_mappings from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -39,24 +39,17 @@ class MujocoEngine(SimulationEngine): - applies control commands """ - def __init__(self, spec: RobotSpec, config_path: str | None, headless: bool) -> None: - super().__init__(spec=spec, config_path=config_path, headless=headless) + def __init__(self, config_path: str | None, headless: bool) -> None: + super().__init__(config_path=config_path, headless=headless) - self._robot_name = spec.asset or spec.name - if not self._robot_name: - raise ValueError("robot name or asset is required for MuJoCo simulation loading") - - if config_path: - resolved = Path(config_path).expanduser() - xml_path = resolved / "scene.xml" if resolved.is_dir() else resolved - if not xml_path.exists(): - raise FileNotFoundError(f"MuJoCo XML not found: {xml_path}") - self._model = mujoco.MjModel.from_xml_path(str(xml_path)) - else: - self._model = load_robot_description(self._robot_name) + xml_path = self._resolve_xml_path(config_path) + self._model = mujoco.MjModel.from_xml_path(str(xml_path)) + self._xml_path = xml_path self._data = mujoco.MjData(self._model) - self._num_joints = int(self._model.nq) + self._joint_mappings = build_joint_mappings(self._xml_path, self._model) + self._joint_names = [mapping.name for mapping in self._joint_mappings] + self._num_joints = len(self._joint_names) timestep = float(self._model.opt.timestep) self._control_frequency = 1.0 / timestep if timestep > 0.0 else 100.0 @@ -70,31 +63,70 @@ def __init__(self, spec: RobotSpec, config_path: str | None, headless: bool) -> self._joint_efforts = [0.0] * self._num_joints self._joint_position_targets = [0.0] * self._num_joints - - for i in range(min(self._num_joints, self._model.nq)): - current_pos = float(self._data.qpos[i]) + for i, mapping in enumerate(self._joint_mappings): + current_pos = self._current_position(mapping) self._joint_position_targets[i] = current_pos self._joint_positions[i] = current_pos + def _resolve_xml_path(self, config_path: str | None) -> Path: + if not config_path: + raise ValueError("config_path is required for MuJoCo simulation loading") + resolved = Path(config_path).expanduser() + xml_path = resolved / "scene.xml" if resolved.is_dir() else resolved + if not xml_path.exists(): + raise FileNotFoundError(f"MuJoCo XML not found: {xml_path}") + return xml_path + + def _current_position(self, mapping: JointMapping) -> float: + if mapping.joint_id is not None and mapping.qpos_adr is not None: + return float(self._data.qpos[mapping.qpos_adr]) + if mapping.tendon_qpos_adrs: + return float( + sum(self._data.qpos[adr] for adr in mapping.tendon_qpos_adrs) + / len(mapping.tendon_qpos_adrs) + ) + if mapping.actuator_id is not None: + return float(self._data.actuator_length[mapping.actuator_id]) + return 0.0 + def _apply_control(self) -> None: with self._lock: pos_targets = list(self._joint_position_targets) - n_act = min(self._num_joints, self._model.nu) with self._lock: - for i in range(n_act): - self._data.ctrl[i] = pos_targets[i] + for i, mapping in enumerate(self._joint_mappings): + if mapping.actuator_id is None: + continue + if i < len(pos_targets): + self._data.ctrl[mapping.actuator_id] = pos_targets[i] def _update_joint_state(self) -> None: with self._lock: - n_q = min(self._num_joints, self._model.nq) - n_v = min(self._num_joints, self._model.nv) - n_act = min(self._num_joints, self._model.nu) - for i in range(n_q): - self._joint_positions[i] = float(self._data.qpos[i]) - for i in range(n_v): - self._joint_velocities[i] = float(self._data.qvel[i]) - for i in range(n_act): - self._joint_efforts[i] = float(self._data.qfrc_actuator[i]) + for i, mapping in enumerate(self._joint_mappings): + if mapping.joint_id is not None: + if mapping.qpos_adr is not None: + self._joint_positions[i] = float(self._data.qpos[mapping.qpos_adr]) + if mapping.dof_adr is not None: + self._joint_velocities[i] = float(self._data.qvel[mapping.dof_adr]) + self._joint_efforts[i] = float(self._data.qfrc_actuator[mapping.dof_adr]) + continue + + if mapping.tendon_qpos_adrs: + pos_sum = sum(self._data.qpos[adr] for adr in mapping.tendon_qpos_adrs) + count = len(mapping.tendon_qpos_adrs) + self._joint_positions[i] = float(pos_sum / count) + if mapping.tendon_dof_adrs: + vel_sum = sum(self._data.qvel[adr] for adr in mapping.tendon_dof_adrs) + self._joint_velocities[i] = float(vel_sum / len(mapping.tendon_dof_adrs)) + else: + self._joint_velocities[i] = 0.0 + elif mapping.actuator_id is not None: + self._joint_positions[i] = float( + self._data.actuator_length[mapping.actuator_id] + ) + self._joint_velocities[i] = 0.0 + + if mapping.actuator_id is not None: + self._joint_efforts[i] = float(self._data.actuator_force[mapping.actuator_id]) def connect(self) -> None: logger.info(f"{self.__class__.__name__}: connect()") @@ -158,6 +190,10 @@ def connected(self) -> bool: def num_joints(self) -> int: return self._num_joints + @property + def joint_names(self) -> list[str]: + return list(self._joint_names) + @property def model(self) -> mujoco.MjModel: return self._model @@ -207,9 +243,8 @@ def write_joint_velocities(self, velocities: list[float]) -> None: def hold_current_position(self) -> None: with self._lock: - for i in range(min(self._num_joints, self._model.nq)): - current_pos = float(self._data.qpos[i]) - self._joint_position_targets[i] = current_pos + for i, mapping in enumerate(self._joint_mappings): + self._joint_position_targets[i] = self._current_position(mapping) __all__ = [ diff --git a/dimos/simulation/manipulators/sim_manip_interface.py b/dimos/simulation/manipulators/sim_manip_interface.py index 6b6cd7220e..478c022480 100644 --- a/dimos/simulation/manipulators/sim_manip_interface.py +++ b/dimos/simulation/manipulators/sim_manip_interface.py @@ -23,17 +23,17 @@ from dimos.hardware.manipulators.spec import ControlMode, JointLimits, ManipulatorInfo if TYPE_CHECKING: - from dimos.simulation.engines.base import RobotSpec, SimulationEngine + from dimos.simulation.engines.base import SimulationEngine class SimManipInterface: """Adapter wrapper around a simulation engine to provide a uniform manipulator API.""" - def __init__(self, engine: SimulationEngine, spec: RobotSpec) -> None: + def __init__(self, engine: SimulationEngine) -> None: self.logger = logging.getLogger(self.__class__.__name__) self._engine = engine - self._spec = spec - self._dof = int(spec.dof) if spec.dof is not None else 0 + self._joint_names = list(engine.joint_names) + self._dof = len(self._joint_names) self._connected = False self._servos_enabled = False self._control_mode = ControlMode.POSITION @@ -48,9 +48,12 @@ def connect(self) -> bool: if self._engine.connected: self._connected = True self._servos_enabled = True - if self._dof <= 0: - self._dof = int(self._engine.num_joints) - self.logger.info("Successfully connected to simulation", extra={"dof": self._dof}) + self._joint_names = list(self._engine.joint_names) + self._dof = len(self._joint_names) + self.logger.info( + "Successfully connected to simulation", + extra={"dof": self._dof}, + ) return True self.logger.error("Failed to connect to simulation engine") return False @@ -69,9 +72,9 @@ def is_connected(self) -> bool: return bool(self._connected and self._engine.connected) def get_info(self) -> ManipulatorInfo: - vendor = self._spec.vendor or "Simulation" - model = self._spec.model or self._spec.name - dof = self.get_dof() + vendor = "Simulation" + model = "Simulation" + dof = self._dof return ManipulatorInfo( vendor=vendor, model=model, @@ -83,9 +86,10 @@ def get_info(self) -> ManipulatorInfo: def get_dof(self) -> int: return self._dof + def get_joint_names(self) -> list[str]: + return list(self._joint_names) + def get_limits(self) -> JointLimits: - if self._spec.limits is not None: - return self._spec.limits lower = [-math.pi] * self._dof upper = [math.pi] * self._dof max_vel_rad = math.radians(180.0) diff --git a/dimos/simulation/manipulators/sim_module.py b/dimos/simulation/manipulators/sim_module.py index b94b9c18be..82e862b0a2 100644 --- a/dimos/simulation/manipulators/sim_module.py +++ b/dimos/simulation/manipulators/sim_module.py @@ -19,24 +19,20 @@ from dataclasses import dataclass import threading import time -from typing import TYPE_CHECKING, Any +from typing import Any from reactivex.disposable import Disposable from dimos.core import In, Module, Out, rpc from dimos.core.module import ModuleConfig from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState +from dimos.simulation.engines.mujoco_engine import MujocoEngine from dimos.simulation.manipulators.sim_manip_interface import SimManipInterface -from dimos.simulation.registry import registry - -if TYPE_CHECKING: - from dimos.simulation.engines.base import RobotSpec @dataclass class SimulationModuleConfig(ModuleConfig): engine: str | None = None - robot: str | None = None config_path: str | None = None headless: bool = False @@ -53,11 +49,13 @@ class SimulationModule(Module[SimulationModuleConfig]): joint_velocity_command: In[JointCommand] MIN_CONTROL_RATE = 1.0 + _ENGINES = { + "mujoco": MujocoEngine, + } def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) self._backend: SimManipInterface | None = None - self._spec: RobotSpec | None = None self._control_rate = 100.0 self._monitor_rate = 100.0 self._joint_prefix = "joint" @@ -71,16 +69,17 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: def _create_backend(self) -> SimManipInterface: if not self.config.engine: raise ValueError("engine is required for SimulationModule") - if not self.config.robot: - raise ValueError("robot is required for SimulationModule") - engine = registry.create_engine( - engine=self.config.engine, - robot=self.config.robot, + if not self.config.config_path: + raise ValueError("config_path is required for SimulationModule") + engine_key = self.config.engine.lower() + if engine_key not in self._ENGINES: + raise ValueError(f"Unknown simulation engine: {self.config.engine}") + engine_cls = self._ENGINES[engine_key] + engine = engine_cls( config_path=self.config.config_path, headless=self.config.headless, ) - self._spec = engine.spec - return SimManipInterface(engine=engine, spec=engine.spec) + return SimManipInterface(engine=engine) @rpc def start(self) -> None: @@ -242,9 +241,10 @@ def _monitor_loop(self) -> None: time.sleep(period) def _resolve_joint_names(self, dof: int) -> list[str]: - if self._spec and self._spec.joint_names: - if len(self._spec.joint_names) >= dof: - return list(self._spec.joint_names[:dof]) + if self._backend: + names = self._backend.get_joint_names() + if len(names) >= dof: + return list(names[:dof]) return [f"{self._joint_prefix}{i + 1}" for i in range(dof)] diff --git a/dimos/simulation/manipulators/xml_parser.py b/dimos/simulation/manipulators/xml_parser.py new file mode 100644 index 0000000000..24b1bbce15 --- /dev/null +++ b/dimos/simulation/manipulators/xml_parser.py @@ -0,0 +1,274 @@ +# 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. + +"""MuJoCo XML parsing helpers for joint/actuator metadata.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import TYPE_CHECKING +import xml.etree.ElementTree as ET + +import mujoco + +if TYPE_CHECKING: + from pathlib import Path + + +@dataclass(frozen=True) +class JointMapping: + name: str + joint_id: int | None + actuator_id: int | None + qpos_adr: int | None + dof_adr: int | None + tendon_qpos_adrs: tuple[int, ...] + tendon_dof_adrs: tuple[int, ...] + + +@dataclass(frozen=True) +class _ActuatorSpec: + name: str + joint: str | None + tendon: str | None + + +def build_joint_mappings(xml_path: Path, model: mujoco.MjModel) -> list[JointMapping]: + specs = _parse_actuator_specs(xml_path) + if specs: + return _build_joint_mappings_from_specs(specs, model) + if int(model.nu) > 0: + return _build_joint_mappings_from_actuators(model) + return _build_joint_mappings_from_model(model) + + +def _parse_actuator_specs(xml_path: Path) -> list[_ActuatorSpec]: + return _collect_actuator_specs(xml_path.resolve(), seen=set()) + + +def _collect_actuator_specs(xml_path: Path, seen: set[Path]) -> list[_ActuatorSpec]: + if xml_path in seen: + return [] + seen.add(xml_path) + + root = ET.parse(xml_path).getroot() + base_dir = xml_path.parent + specs: list[_ActuatorSpec] = [] + + def walk(node: ET.Element) -> None: + for child in node: + if child.tag == "include": + include_file = child.attrib.get("file") + if include_file: + include_path = (base_dir / include_file).resolve() + specs.extend(_collect_actuator_specs(include_path, seen)) + continue + if child.tag == "actuator": + specs.extend(_parse_actuator_block(child)) + continue + walk(child) + + walk(root) + return specs + + +def _parse_actuator_block(actuator_elem: ET.Element) -> list[_ActuatorSpec]: + specs: list[_ActuatorSpec] = [] + for child in actuator_elem: + joint = child.attrib.get("joint") + tendon = child.attrib.get("tendon") + if not joint and not tendon: + continue + name = child.attrib.get("name") or joint or tendon or "actuator" + specs.append(_ActuatorSpec(name=name, joint=joint, tendon=tendon)) + return specs + + +def _build_joint_mappings_from_specs( + specs: list[_ActuatorSpec], + model: mujoco.MjModel, +) -> list[JointMapping]: + mappings: list[JointMapping] = [] + for spec in specs: + if spec.joint: + mappings.append(_mapping_for_joint(spec, model)) + elif spec.tendon: + mappings.append(_mapping_for_tendon(spec, model)) + return mappings + + +def _mapping_for_joint(spec: _ActuatorSpec, model: mujoco.MjModel) -> JointMapping: + joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, spec.joint) + if joint_id < 0: + raise ValueError(f"Unknown joint '{spec.joint}' in MuJoCo model") + actuator_id = _find_actuator_id_for_joint(model, joint_id, spec.name) + joint_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, joint_id) or spec.name + return JointMapping( + name=joint_name, + joint_id=joint_id, + actuator_id=actuator_id, + qpos_adr=int(model.jnt_qposadr[joint_id]), + dof_adr=int(model.jnt_dofadr[joint_id]), + tendon_qpos_adrs=(), + tendon_dof_adrs=(), + ) + + +def _mapping_for_tendon(spec: _ActuatorSpec, model: mujoco.MjModel) -> JointMapping: + tendon_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_TENDON, spec.tendon) + if tendon_id < 0: + raise ValueError(f"Unknown tendon '{spec.tendon}' in MuJoCo model") + actuator_id = _find_actuator_id_for_tendon(model, tendon_id, spec.name) + joint_ids = _tendon_joint_ids(model, tendon_id) + return JointMapping( + name=spec.name or spec.tendon, + joint_id=None, + actuator_id=actuator_id, + qpos_adr=None, + dof_adr=None, + tendon_qpos_adrs=tuple(int(model.jnt_qposadr[joint_id]) for joint_id in joint_ids), + tendon_dof_adrs=tuple(int(model.jnt_dofadr[joint_id]) for joint_id in joint_ids), + ) + + +def _find_actuator_id_for_joint( + model: mujoco.MjModel, + joint_id: int, + actuator_name: str | None, +) -> int | None: + if actuator_name: + act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, actuator_name) + if act_id >= 0: + return int(act_id) + for act_id in range(int(model.nu)): + trn_type = int(model.actuator_trntype[act_id]) + if trn_type != int(mujoco.mjtTrn.mjTRN_JOINT): + continue + if int(model.actuator_trnid[act_id, 0]) == joint_id: + return act_id + return None + + +def _find_actuator_id_for_tendon( + model: mujoco.MjModel, + tendon_id: int, + actuator_name: str | None, +) -> int | None: + if actuator_name: + act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, actuator_name) + if act_id >= 0: + return int(act_id) + for act_id in range(int(model.nu)): + trn_type = int(model.actuator_trntype[act_id]) + if trn_type != int(mujoco.mjtTrn.mjTRN_TENDON): + continue + if int(model.actuator_trnid[act_id, 0]) == tendon_id: + return act_id + return None + + +def _tendon_joint_ids(model: mujoco.MjModel, tendon_id: int) -> tuple[int, ...]: + adr = int(model.tendon_adr[tendon_id]) + num = int(model.tendon_num[tendon_id]) + joint_ids: list[int] = [] + for wrap_id in range(adr, adr + num): + wrap_type = int(model.wrap_type[wrap_id]) + if wrap_type == int(mujoco.mjtWrap.mjWRAP_JOINT): + joint_ids.append(int(model.wrap_objid[wrap_id])) + return tuple(joint_ids) + + +def _build_joint_mappings_from_actuators(model: mujoco.MjModel) -> list[JointMapping]: + mappings: list[JointMapping] = [] + for actuator_id in range(int(model.nu)): + actuator_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, actuator_id) + name = actuator_name or f"actuator{actuator_id}" + trn_type = int(model.actuator_trntype[actuator_id]) + if trn_type == int(mujoco.mjtTrn.mjTRN_JOINT): + joint_id = int(model.actuator_trnid[actuator_id, 0]) + joint_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, joint_id) + mappings.append( + JointMapping( + name=joint_name or name, + joint_id=joint_id, + actuator_id=actuator_id, + qpos_adr=int(model.jnt_qposadr[joint_id]), + dof_adr=int(model.jnt_dofadr[joint_id]), + tendon_qpos_adrs=(), + tendon_dof_adrs=(), + ) + ) + continue + + if trn_type == int(mujoco.mjtTrn.mjTRN_TENDON): + tendon_id = int(model.actuator_trnid[actuator_id, 0]) + tendon_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_TENDON, tendon_id) + if not actuator_name and tendon_name: + name = tendon_name + joint_ids = _tendon_joint_ids(model, tendon_id) + mappings.append( + JointMapping( + name=name, + joint_id=None, + actuator_id=actuator_id, + qpos_adr=None, + dof_adr=None, + tendon_qpos_adrs=tuple( + int(model.jnt_qposadr[joint_id]) for joint_id in joint_ids + ), + tendon_dof_adrs=tuple( + int(model.jnt_dofadr[joint_id]) for joint_id in joint_ids + ), + ) + ) + continue + + mappings.append( + JointMapping( + name=name, + joint_id=None, + actuator_id=actuator_id, + qpos_adr=None, + dof_adr=None, + tendon_qpos_adrs=(), + tendon_dof_adrs=(), + ) + ) + + return mappings + + +def _build_joint_mappings_from_model(model: mujoco.MjModel) -> list[JointMapping]: + mappings: list[JointMapping] = [] + for joint_id in range(int(model.njnt)): + jnt_type = int(model.jnt_type[joint_id]) + if jnt_type not in ( + int(mujoco.mjtJoint.mjJNT_HINGE), + int(mujoco.mjtJoint.mjJNT_SLIDE), + ): + continue + joint_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, joint_id) + name = joint_name or f"joint{joint_id}" + mappings.append( + JointMapping( + name=name, + joint_id=joint_id, + actuator_id=None, + qpos_adr=int(model.jnt_qposadr[joint_id]), + dof_adr=int(model.jnt_dofadr[joint_id]), + tendon_qpos_adrs=(), + tendon_dof_adrs=(), + ) + ) + return mappings diff --git a/dimos/simulation/plugins/__init__.py b/dimos/simulation/plugins/__init__.py deleted file mode 100644 index b5c200e780..0000000000 --- a/dimos/simulation/plugins/__init__.py +++ /dev/null @@ -1,12 +0,0 @@ -"""Simulation engine and robot registration plugins.""" - -from dimos.simulation.plugins.mujoco_xarm import register as register_mujoco_xarm - - -def register_all() -> None: - register_mujoco_xarm() - - -__all__ = [ - "register_all", -] diff --git a/dimos/simulation/plugins/mujoco_xarm.py b/dimos/simulation/plugins/mujoco_xarm.py deleted file mode 100644 index 35c2f68511..0000000000 --- a/dimos/simulation/plugins/mujoco_xarm.py +++ /dev/null @@ -1,64 +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. - -"""MuJoCo engine registration with xArm robot specs.""" - -from __future__ import annotations - -from dimos.simulation.engines.base import RobotSpec -from dimos.simulation.engines.mujoco_engine import MujocoEngine -from dimos.simulation.registry import registry - - -def register() -> None: - registry.register_engine("mujoco", MujocoEngine) - - registry.register_robot( - "xarm7", - RobotSpec( - name="xarm7", - engine="mujoco", - asset="xarm7_mj_description", - dof=7, - vendor="UFACTORY", - model="xArm7", - ), - ) - registry.register_robot( - "xarm6", - RobotSpec( - name="xarm6", - engine="mujoco", - asset="xarm6_mj_description", - dof=6, - vendor="UFACTORY", - model="xArm6", - ), - ) - registry.register_robot( - "xarm5", - RobotSpec( - name="xarm5", - engine="mujoco", - asset="xarm5_mj_description", - dof=5, - vendor="UFACTORY", - model="xArm5", - ), - ) - - -__all__ = [ - "register", -] diff --git a/dimos/simulation/registry.py b/dimos/simulation/registry.py deleted file mode 100644 index 91ed29fdff..0000000000 --- a/dimos/simulation/registry.py +++ /dev/null @@ -1,79 +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. - -"""Registry for simulation engines and robot specs.""" - -from __future__ import annotations - -from typing import TYPE_CHECKING - -if TYPE_CHECKING: - from dimos.simulation.engines.base import RobotSpec, SimulationEngine - - -class SimulationRegistry: - """Lookup table for simulator engines and robot specs.""" - - def __init__(self) -> None: - self._engines: dict[str, type[SimulationEngine]] = {} - self._robots: dict[str, RobotSpec] = {} - - def register_engine(self, name: str, engine_cls: type[SimulationEngine]) -> None: - key = name.lower() - self._engines[key] = engine_cls - - def register_robot(self, name: str, spec: RobotSpec) -> None: - key = name.lower() - self._robots[key] = spec - - def get_engine(self, name: str) -> type[SimulationEngine]: - key = name.lower() - if key not in self._engines: - raise KeyError(f"Unknown simulation engine: {name}") - return self._engines[key] - - def get_robot(self, name: str) -> RobotSpec: - key = name.lower() - if key not in self._robots: - raise KeyError(f"Unknown robot spec: {name}") - return self._robots[key] - - def create_engine( - self, - engine: str, - robot: str, - config_path: str | None, - headless: bool, - ) -> SimulationEngine: - spec = self.get_robot(robot) - if spec.engine and spec.engine.lower() != engine.lower(): - raise ValueError( - f"Robot '{spec.name}' registered for engine '{spec.engine}', got '{engine}'." - ) - engine_cls = self.get_engine(engine) - return engine_cls(spec=spec, config_path=config_path, headless=headless) - - def list_engines(self) -> list[str]: - return sorted(self._engines.keys()) - - def list_robots(self) -> list[str]: - return sorted(self._robots.keys()) - - -registry = SimulationRegistry() - -__all__ = [ - "SimulationRegistry", - "registry", -] diff --git a/dimos/simulation/sim_blueprints.py b/dimos/simulation/sim_blueprints.py index 451aa0a96c..5b4a541adc 100644 --- a/dimos/simulation/sim_blueprints.py +++ b/dimos/simulation/sim_blueprints.py @@ -12,6 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +from pathlib import Path + from dimos.core.blueprints import autoconnect from dimos.core.transport import LCMTransport from dimos.msgs.sensor_msgs import ( # type: ignore[attr-defined] @@ -24,8 +26,7 @@ xarm7_trajectory_sim = simulation( engine="mujoco", - robot="xarm7", - config_path=None, + config_path="dimos/simulation/assets/xarm7/scene.xml", headless=True, ).transports( { diff --git a/pyproject.toml b/pyproject.toml index 28d6114813..d565f5ebe1 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -316,7 +316,6 @@ module = [ "tf2_msgs.*", "mujoco", "mujoco_playground.*", - "robot_descriptions.*", "etils", "xarm.*", "dimos_lcm.*", From f392af0ec03e56d58c6838759c29924d6830d47f Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 23 Jan 2026 15:11:23 -0800 Subject: [PATCH 20/43] Track xarm7 assets with LFS --- .gitattributes | 1 + dimos/simulation/assets/xarm7/CHANGELOG.md | 3 +++ dimos/simulation/assets/xarm7/LICENSE | 3 +++ dimos/simulation/assets/xarm7/README.md | 3 +++ dimos/simulation/assets/xarm7/assets/base_link.stl | 3 +++ dimos/simulation/assets/xarm7/assets/end_tool.stl | 3 +++ dimos/simulation/assets/xarm7/assets/left_finger.stl | 3 +++ dimos/simulation/assets/xarm7/assets/left_inner_knuckle.stl | 3 +++ dimos/simulation/assets/xarm7/assets/left_outer_knuckle.stl | 3 +++ dimos/simulation/assets/xarm7/assets/link1.stl | 3 +++ dimos/simulation/assets/xarm7/assets/link2.stl | 3 +++ dimos/simulation/assets/xarm7/assets/link3.stl | 3 +++ dimos/simulation/assets/xarm7/assets/link4.stl | 3 +++ dimos/simulation/assets/xarm7/assets/link5.stl | 3 +++ dimos/simulation/assets/xarm7/assets/link6.stl | 3 +++ dimos/simulation/assets/xarm7/assets/link7.stl | 3 +++ dimos/simulation/assets/xarm7/assets/link_base.stl | 3 +++ dimos/simulation/assets/xarm7/assets/right_finger.stl | 3 +++ dimos/simulation/assets/xarm7/assets/right_inner_knuckle.stl | 3 +++ dimos/simulation/assets/xarm7/assets/right_outer_knuckle.stl | 3 +++ dimos/simulation/assets/xarm7/hand.xml | 3 +++ dimos/simulation/assets/xarm7/scene.xml | 3 +++ dimos/simulation/assets/xarm7/xarm7.png | 3 +++ dimos/simulation/assets/xarm7/xarm7.xml | 3 +++ dimos/simulation/assets/xarm7/xarm7_nohand.xml | 3 +++ 25 files changed, 73 insertions(+) create mode 100644 dimos/simulation/assets/xarm7/CHANGELOG.md create mode 100644 dimos/simulation/assets/xarm7/LICENSE create mode 100644 dimos/simulation/assets/xarm7/README.md create mode 100644 dimos/simulation/assets/xarm7/assets/base_link.stl create mode 100644 dimos/simulation/assets/xarm7/assets/end_tool.stl create mode 100644 dimos/simulation/assets/xarm7/assets/left_finger.stl create mode 100644 dimos/simulation/assets/xarm7/assets/left_inner_knuckle.stl create mode 100644 dimos/simulation/assets/xarm7/assets/left_outer_knuckle.stl create mode 100644 dimos/simulation/assets/xarm7/assets/link1.stl create mode 100644 dimos/simulation/assets/xarm7/assets/link2.stl create mode 100644 dimos/simulation/assets/xarm7/assets/link3.stl create mode 100644 dimos/simulation/assets/xarm7/assets/link4.stl create mode 100644 dimos/simulation/assets/xarm7/assets/link5.stl create mode 100644 dimos/simulation/assets/xarm7/assets/link6.stl create mode 100644 dimos/simulation/assets/xarm7/assets/link7.stl create mode 100644 dimos/simulation/assets/xarm7/assets/link_base.stl create mode 100644 dimos/simulation/assets/xarm7/assets/right_finger.stl create mode 100644 dimos/simulation/assets/xarm7/assets/right_inner_knuckle.stl create mode 100644 dimos/simulation/assets/xarm7/assets/right_outer_knuckle.stl create mode 100644 dimos/simulation/assets/xarm7/hand.xml create mode 100644 dimos/simulation/assets/xarm7/scene.xml create mode 100644 dimos/simulation/assets/xarm7/xarm7.png create mode 100644 dimos/simulation/assets/xarm7/xarm7.xml create mode 100644 dimos/simulation/assets/xarm7/xarm7_nohand.xml diff --git a/.gitattributes b/.gitattributes index 8f05eb707f..e06f5016fd 100644 --- a/.gitattributes +++ b/.gitattributes @@ -16,3 +16,4 @@ *.mov filter=lfs diff=lfs merge=lfs -text binary *.gif filter=lfs diff=lfs merge=lfs -text binary *.foxe filter=lfs diff=lfs merge=lfs -text binary +dimos/simulation/assets/xarm7/** filter=lfs diff=lfs merge=lfs -text diff --git a/dimos/simulation/assets/xarm7/CHANGELOG.md b/dimos/simulation/assets/xarm7/CHANGELOG.md new file mode 100644 index 0000000000..e2b63f9633 --- /dev/null +++ b/dimos/simulation/assets/xarm7/CHANGELOG.md @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1ffbf4c8599f2cf371a488ee878d4025f6500b999640de12546b631f37f8896e +size 351 diff --git a/dimos/simulation/assets/xarm7/LICENSE b/dimos/simulation/assets/xarm7/LICENSE new file mode 100644 index 0000000000..4cf88471da --- /dev/null +++ b/dimos/simulation/assets/xarm7/LICENSE @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4fe659325706d0fc41592c8743d085a0bdf778056249f2a108943d22be916fb2 +size 1522 diff --git a/dimos/simulation/assets/xarm7/README.md b/dimos/simulation/assets/xarm7/README.md new file mode 100644 index 0000000000..1ea70d040b --- /dev/null +++ b/dimos/simulation/assets/xarm7/README.md @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:42f9df53f08a54251e832a6454107fe9eec52581d250212856e345f06b53ca76 +size 1117 diff --git a/dimos/simulation/assets/xarm7/assets/base_link.stl b/dimos/simulation/assets/xarm7/assets/base_link.stl new file mode 100644 index 0000000000..ea9d15f635 --- /dev/null +++ b/dimos/simulation/assets/xarm7/assets/base_link.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cdaa4cff22f7c9cff05c6a8ed32f94fd2b11a69d37dd97a159ccee2d8dd32f13 +size 1211434 diff --git a/dimos/simulation/assets/xarm7/assets/end_tool.stl b/dimos/simulation/assets/xarm7/assets/end_tool.stl new file mode 100644 index 0000000000..e9255ec68a --- /dev/null +++ b/dimos/simulation/assets/xarm7/assets/end_tool.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e06112781cdd615b7df536569f50addb445a6f4b4acced6d53b0742018b6c768 +size 13084 diff --git a/dimos/simulation/assets/xarm7/assets/left_finger.stl b/dimos/simulation/assets/xarm7/assets/left_finger.stl new file mode 100644 index 0000000000..6bf4e50254 --- /dev/null +++ b/dimos/simulation/assets/xarm7/assets/left_finger.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a44756eb72f9c214cb37e61dc209cd7073fdff3e4271a7423476ef6fd090d2d4 +size 242684 diff --git a/dimos/simulation/assets/xarm7/assets/left_inner_knuckle.stl b/dimos/simulation/assets/xarm7/assets/left_inner_knuckle.stl new file mode 100644 index 0000000000..817c7e1d46 --- /dev/null +++ b/dimos/simulation/assets/xarm7/assets/left_inner_knuckle.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e8e48692ad26837bb3d6a97582c89784d09948fc09bfe4e5a59017859ff04dac +size 366284 diff --git a/dimos/simulation/assets/xarm7/assets/left_outer_knuckle.stl b/dimos/simulation/assets/xarm7/assets/left_outer_knuckle.stl new file mode 100644 index 0000000000..010c0f3ba0 --- /dev/null +++ b/dimos/simulation/assets/xarm7/assets/left_outer_knuckle.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:501665812b08d67e764390db781e839adc6896a9540301d60adf606f57648921 +size 22284 diff --git a/dimos/simulation/assets/xarm7/assets/link1.stl b/dimos/simulation/assets/xarm7/assets/link1.stl new file mode 100644 index 0000000000..f2b676f29d --- /dev/null +++ b/dimos/simulation/assets/xarm7/assets/link1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:34b541122df84d2ef5fcb91b715eb19659dc15ad8d44a191dde481f780265636 +size 184184 diff --git a/dimos/simulation/assets/xarm7/assets/link2.stl b/dimos/simulation/assets/xarm7/assets/link2.stl new file mode 100644 index 0000000000..bf93580c69 --- /dev/null +++ b/dimos/simulation/assets/xarm7/assets/link2.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:61e641cd47c169ecef779683332e00e4914db729bf02dfb61bfbe69351827455 +size 225584 diff --git a/dimos/simulation/assets/xarm7/assets/link3.stl b/dimos/simulation/assets/xarm7/assets/link3.stl new file mode 100644 index 0000000000..d316d233fe --- /dev/null +++ b/dimos/simulation/assets/xarm7/assets/link3.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9e2798e7946dd70046c95455d5ba96392d0b54a6069caba91dc4ca66e1379b42 +size 237084 diff --git a/dimos/simulation/assets/xarm7/assets/link4.stl b/dimos/simulation/assets/xarm7/assets/link4.stl new file mode 100644 index 0000000000..f6d5fe94ad --- /dev/null +++ b/dimos/simulation/assets/xarm7/assets/link4.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c757fee95f873191a0633c355c07a360032960771cabbd7593a6cdb0f1ffb089 +size 243684 diff --git a/dimos/simulation/assets/xarm7/assets/link5.stl b/dimos/simulation/assets/xarm7/assets/link5.stl new file mode 100644 index 0000000000..e037b8b995 --- /dev/null +++ b/dimos/simulation/assets/xarm7/assets/link5.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:715ad5787c5dab57589937fd47289882707b5e1eb997e340d567785b02f4ec90 +size 229084 diff --git a/dimos/simulation/assets/xarm7/assets/link6.stl b/dimos/simulation/assets/xarm7/assets/link6.stl new file mode 100644 index 0000000000..198c53008a --- /dev/null +++ b/dimos/simulation/assets/xarm7/assets/link6.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:85b320aa420497827223d16d492bba8de091173374e361396fc7a5dad7bdb0cb +size 399384 diff --git a/dimos/simulation/assets/xarm7/assets/link7.stl b/dimos/simulation/assets/xarm7/assets/link7.stl new file mode 100644 index 0000000000..ce9a39ac78 --- /dev/null +++ b/dimos/simulation/assets/xarm7/assets/link7.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:97115d848fbf802cb770cd9be639ae2af993103b9d9bbb0c50c943c738a36f18 +size 231684 diff --git a/dimos/simulation/assets/xarm7/assets/link_base.stl b/dimos/simulation/assets/xarm7/assets/link_base.stl new file mode 100644 index 0000000000..58b35e2344 --- /dev/null +++ b/dimos/simulation/assets/xarm7/assets/link_base.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cf52275a913a0977595f5d12373f06583513e26fe18b8cf2827c74da94a4f0be +size 304784 diff --git a/dimos/simulation/assets/xarm7/assets/right_finger.stl b/dimos/simulation/assets/xarm7/assets/right_finger.stl new file mode 100644 index 0000000000..03f26e9ac8 --- /dev/null +++ b/dimos/simulation/assets/xarm7/assets/right_finger.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c5dee87c7f37baf554b8456ebfe0b3e8ed0b22b8938bd1add6505c2ad6d32c7d +size 242684 diff --git a/dimos/simulation/assets/xarm7/assets/right_inner_knuckle.stl b/dimos/simulation/assets/xarm7/assets/right_inner_knuckle.stl new file mode 100644 index 0000000000..8586f344c2 --- /dev/null +++ b/dimos/simulation/assets/xarm7/assets/right_inner_knuckle.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b41dd2c2c550281bf78d7cc6fa117b14786700e5c453560a0cb5fd6dfa0ffb3e +size 366284 diff --git a/dimos/simulation/assets/xarm7/assets/right_outer_knuckle.stl b/dimos/simulation/assets/xarm7/assets/right_outer_knuckle.stl new file mode 100644 index 0000000000..ae7afc25f6 --- /dev/null +++ b/dimos/simulation/assets/xarm7/assets/right_outer_knuckle.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:75ca1107d0a42a0f03802a9a49cab48419b31851ee8935f8f1ca06be1c1c91e8 +size 22284 diff --git a/dimos/simulation/assets/xarm7/hand.xml b/dimos/simulation/assets/xarm7/hand.xml new file mode 100644 index 0000000000..edb00ac880 --- /dev/null +++ b/dimos/simulation/assets/xarm7/hand.xml @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2a15029be4490510e5b6f4eeffcc5b67fcee5410ca567899d29ab6f9229c03ee +size 5950 diff --git a/dimos/simulation/assets/xarm7/scene.xml b/dimos/simulation/assets/xarm7/scene.xml new file mode 100644 index 0000000000..e696365892 --- /dev/null +++ b/dimos/simulation/assets/xarm7/scene.xml @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8c6f6a1b28c73126c347fff7d9b8b26223fc608b59c15e339f41948439024bbf +size 1529 diff --git a/dimos/simulation/assets/xarm7/xarm7.png b/dimos/simulation/assets/xarm7/xarm7.png new file mode 100644 index 0000000000..0f324578a0 --- /dev/null +++ b/dimos/simulation/assets/xarm7/xarm7.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:636e9e7a4396b54580231399dd64bb35d817814599eb160cede5886c63516228 +size 1692813 diff --git a/dimos/simulation/assets/xarm7/xarm7.xml b/dimos/simulation/assets/xarm7/xarm7.xml new file mode 100644 index 0000000000..f653a089a4 --- /dev/null +++ b/dimos/simulation/assets/xarm7/xarm7.xml @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a81ba21ead4ad662542976d97beeab3bf07c7e9f689180123ea786008c3a942a +size 10764 diff --git a/dimos/simulation/assets/xarm7/xarm7_nohand.xml b/dimos/simulation/assets/xarm7/xarm7_nohand.xml new file mode 100644 index 0000000000..658b641268 --- /dev/null +++ b/dimos/simulation/assets/xarm7/xarm7_nohand.xml @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:90bcc0e2fbbe5fa95dbbafb74e56bd0106ecc6f5767a4dc3e7c511016efc023c +size 4756 From 19f886a6dfaf207f7eb540f76aa327cb7fd0998b Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 23 Jan 2026 15:36:20 -0800 Subject: [PATCH 21/43] make blueprint runnable --- dimos/robot/all_blueprints.py | 3 ++ dimos/simulation/manipulators/__init__.py | 37 +++++++++++++++++++---- 2 files changed, 34 insertions(+), 6 deletions(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index f4524b8ff2..650b023526 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -53,6 +53,8 @@ "demo-google-maps-skill": "dimos.agents.skills.demo_google_maps_skill:demo_google_maps_skill", "demo-object-scene-registration": "dimos.perception.demo_object_scene_registration:demo_object_scene_registration", "demo-error-on-name-conflicts": "dimos.robot.unitree_webrtc.demo_error_on_name_conflicts:blueprint", + # Simulation blueprints + "simulation-xarm7": "dimos.simulation.sim_blueprints:xarm7_trajectory_sim", } @@ -84,6 +86,7 @@ "web_input": "dimos.agents.cli.web", # Control orchestrator module "control_orchestrator": "dimos.control.orchestrator", + "simulation": "dimos.simulation.manipulators.sim_module", } diff --git a/dimos/simulation/manipulators/__init__.py b/dimos/simulation/manipulators/__init__.py index 85ee0a1683..816de0a18d 100644 --- a/dimos/simulation/manipulators/__init__.py +++ b/dimos/simulation/manipulators/__init__.py @@ -14,12 +14,17 @@ """Simulation manipulator utilities.""" -from dimos.simulation.manipulators.sim_manip_interface import SimManipInterface -from dimos.simulation.manipulators.sim_module import ( - SimulationModule, - SimulationModuleConfig, - simulation, -) +from __future__ import annotations + +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from dimos.simulation.manipulators.sim_manip_interface import SimManipInterface + from dimos.simulation.manipulators.sim_module import ( + SimulationModule, + SimulationModuleConfig, + simulation, + ) __all__ = [ "SimManipInterface", @@ -27,3 +32,23 @@ "SimulationModuleConfig", "simulation", ] + + +def __getattr__(name: str): # type: ignore[no-untyped-def] + if name == "SimManipInterface": + from dimos.simulation.manipulators.sim_manip_interface import SimManipInterface + + return SimManipInterface + if name in {"SimulationModule", "SimulationModuleConfig", "simulation"}: + from dimos.simulation.manipulators.sim_module import ( + SimulationModule, + SimulationModuleConfig, + simulation, + ) + + return { + "SimulationModule": SimulationModule, + "SimulationModuleConfig": SimulationModuleConfig, + "simulation": simulation, + }[name] + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") From c3d0c2b00fdafd446cd002112b1f722db379ccfa Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 23 Jan 2026 16:07:43 -0800 Subject: [PATCH 22/43] e2e test and unit tests for sim module --- dimos/e2e_tests/test_simulation_module.py | 58 ++++++++++ .../manipulators/test_sim_module.py | 105 ++++++++++++++++++ 2 files changed, 163 insertions(+) create mode 100644 dimos/e2e_tests/test_simulation_module.py create mode 100644 dimos/simulation/manipulators/test_sim_module.py diff --git a/dimos/e2e_tests/test_simulation_module.py b/dimos/e2e_tests/test_simulation_module.py new file mode 100644 index 0000000000..c94fd7b97f --- /dev/null +++ b/dimos/e2e_tests/test_simulation_module.py @@ -0,0 +1,58 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""End-to-end tests for the simulation module.""" + +import os + +import pytest + +from dimos.msgs.sensor_msgs import JointCommand, JointState + + +def _positions_within_tolerance( + positions: list[float], + target: list[float], + tolerance: float, +) -> bool: + if len(positions) < len(target): + return False + return all(abs(positions[i] - target[i]) <= tolerance for i in range(len(target))) + + +@pytest.mark.skipif(bool(os.getenv("CI")), reason="LCM doesn't work in CI.") +class TestSimulationModuleE2E: + def test_xarm7_joint_command_updates_joint_state(self, lcm_spy, start_blueprint) -> None: + joint_state_topic = "/xarm/joint_states#sensor_msgs.JointState" + joint_command_topic = "/xarm/joint_position_command#sensor_msgs.JointCommand" + lcm_spy.save_topic(joint_state_topic) + + start_blueprint("simulation-xarm7") + lcm_spy.wait_for_saved_topic(joint_state_topic, timeout=15.0) + + target_positions = [0.2, -0.2, 0.1, -0.1, 0.15, -0.15, 0.05] + lcm_spy.publish(joint_command_topic, JointCommand(positions=target_positions)) + + tolerance = 0.03 + lcm_spy.wait_for_message_result( + joint_state_topic, + JointState, + predicate=lambda msg: _positions_within_tolerance( + list(msg.position), + target_positions, + tolerance, + ), + fail_message=("joint_state did not reach commanded positions within tolerance"), + timeout=10.0, + ) diff --git a/dimos/simulation/manipulators/test_sim_module.py b/dimos/simulation/manipulators/test_sim_module.py new file mode 100644 index 0000000000..89835f2e2a --- /dev/null +++ b/dimos/simulation/manipulators/test_sim_module.py @@ -0,0 +1,105 @@ +# 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 threading +import time + +import pytest + +from dimos.simulation.manipulators.sim_module import SimulationModule + + +class _DummyRPC: + def serve_module_rpc(self, _module) -> None: # type: ignore[no-untyped-def] + return None + + def start(self) -> None: + return None + + def stop(self) -> None: + return None + + +class _FakeBackend: + def __init__(self) -> None: + self._names = ["joint1", "joint2", "joint3"] + + def get_dof(self) -> int: + return len(self._names) + + def get_joint_names(self) -> list[str]: + return list(self._names) + + def read_joint_positions(self) -> list[float]: + return [0.1, 0.2, 0.3] + + def read_joint_velocities(self) -> list[float]: + return [0.0, 0.0, 0.0] + + def read_joint_efforts(self) -> list[float]: + return [0.0, 0.0, 0.0] + + def read_state(self) -> dict[str, int]: + return {"state": 1, "mode": 2} + + def read_error(self) -> tuple[int, str]: + return 0, "" + + def read_enabled(self) -> bool: + return True + + def disconnect(self) -> None: + return None + + +def _run_single_monitor_iteration(module: SimulationModule, monkeypatch) -> None: # type: ignore[no-untyped-def] + def _sleep_once(_: float) -> None: + module._stop_event.set() + raise StopIteration + + monkeypatch.setattr(time, "sleep", _sleep_once) + with pytest.raises(StopIteration): + module._monitor_loop() + + +def test_simulation_module_publishes_joint_state(monkeypatch) -> None: + module = SimulationModule(rpc_transport=_DummyRPC) + module._backend = _FakeBackend() # type: ignore[assignment] + module._stop_event = threading.Event() + + joint_states: list[object] = [] + module.joint_state.subscribe(joint_states.append) + try: + _run_single_monitor_iteration(module, monkeypatch) + finally: + module.stop() + + assert len(joint_states) == 1 + assert joint_states[0].name == ["joint1", "joint2", "joint3"] + + +def test_simulation_module_publishes_robot_state(monkeypatch) -> None: + module = SimulationModule(rpc_transport=_DummyRPC) + module._backend = _FakeBackend() # type: ignore[assignment] + module._stop_event = threading.Event() + + robot_states: list[object] = [] + module.robot_state.subscribe(robot_states.append) + try: + _run_single_monitor_iteration(module, monkeypatch) + finally: + module.stop() + + assert len(robot_states) == 1 + assert robot_states[0].state == 1 From 90c8297a97fcdddf0500f720a8998e12f8ae4e22 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 23 Jan 2026 18:51:21 -0800 Subject: [PATCH 23/43] move xarm7 arm to data folder --- .gitattributes | 1 - data/.lfs/xarm7.tar.gz | 3 +++ dimos/simulation/assets/xarm7/CHANGELOG.md | 3 --- dimos/simulation/assets/xarm7/LICENSE | 3 --- dimos/simulation/assets/xarm7/README.md | 3 --- dimos/simulation/assets/xarm7/assets/base_link.stl | 3 --- dimos/simulation/assets/xarm7/assets/end_tool.stl | 3 --- dimos/simulation/assets/xarm7/assets/left_finger.stl | 3 --- .../simulation/assets/xarm7/assets/left_inner_knuckle.stl | 3 --- .../simulation/assets/xarm7/assets/left_outer_knuckle.stl | 3 --- dimos/simulation/assets/xarm7/assets/link1.stl | 3 --- dimos/simulation/assets/xarm7/assets/link2.stl | 3 --- dimos/simulation/assets/xarm7/assets/link3.stl | 3 --- dimos/simulation/assets/xarm7/assets/link4.stl | 3 --- dimos/simulation/assets/xarm7/assets/link5.stl | 3 --- dimos/simulation/assets/xarm7/assets/link6.stl | 3 --- dimos/simulation/assets/xarm7/assets/link7.stl | 3 --- dimos/simulation/assets/xarm7/assets/link_base.stl | 3 --- dimos/simulation/assets/xarm7/assets/right_finger.stl | 3 --- .../simulation/assets/xarm7/assets/right_inner_knuckle.stl | 3 --- .../simulation/assets/xarm7/assets/right_outer_knuckle.stl | 3 --- dimos/simulation/assets/xarm7/hand.xml | 3 --- dimos/simulation/assets/xarm7/scene.xml | 3 --- dimos/simulation/assets/xarm7/xarm7.png | 3 --- dimos/simulation/assets/xarm7/xarm7.xml | 3 --- dimos/simulation/assets/xarm7/xarm7_nohand.xml | 3 --- dimos/simulation/sim_blueprints.py | 7 +++---- 27 files changed, 6 insertions(+), 77 deletions(-) create mode 100644 data/.lfs/xarm7.tar.gz delete mode 100644 dimos/simulation/assets/xarm7/CHANGELOG.md delete mode 100644 dimos/simulation/assets/xarm7/LICENSE delete mode 100644 dimos/simulation/assets/xarm7/README.md delete mode 100644 dimos/simulation/assets/xarm7/assets/base_link.stl delete mode 100644 dimos/simulation/assets/xarm7/assets/end_tool.stl delete mode 100644 dimos/simulation/assets/xarm7/assets/left_finger.stl delete mode 100644 dimos/simulation/assets/xarm7/assets/left_inner_knuckle.stl delete mode 100644 dimos/simulation/assets/xarm7/assets/left_outer_knuckle.stl delete mode 100644 dimos/simulation/assets/xarm7/assets/link1.stl delete mode 100644 dimos/simulation/assets/xarm7/assets/link2.stl delete mode 100644 dimos/simulation/assets/xarm7/assets/link3.stl delete mode 100644 dimos/simulation/assets/xarm7/assets/link4.stl delete mode 100644 dimos/simulation/assets/xarm7/assets/link5.stl delete mode 100644 dimos/simulation/assets/xarm7/assets/link6.stl delete mode 100644 dimos/simulation/assets/xarm7/assets/link7.stl delete mode 100644 dimos/simulation/assets/xarm7/assets/link_base.stl delete mode 100644 dimos/simulation/assets/xarm7/assets/right_finger.stl delete mode 100644 dimos/simulation/assets/xarm7/assets/right_inner_knuckle.stl delete mode 100644 dimos/simulation/assets/xarm7/assets/right_outer_knuckle.stl delete mode 100644 dimos/simulation/assets/xarm7/hand.xml delete mode 100644 dimos/simulation/assets/xarm7/scene.xml delete mode 100644 dimos/simulation/assets/xarm7/xarm7.png delete mode 100644 dimos/simulation/assets/xarm7/xarm7.xml delete mode 100644 dimos/simulation/assets/xarm7/xarm7_nohand.xml diff --git a/.gitattributes b/.gitattributes index e06f5016fd..8f05eb707f 100644 --- a/.gitattributes +++ b/.gitattributes @@ -16,4 +16,3 @@ *.mov filter=lfs diff=lfs merge=lfs -text binary *.gif filter=lfs diff=lfs merge=lfs -text binary *.foxe filter=lfs diff=lfs merge=lfs -text binary -dimos/simulation/assets/xarm7/** filter=lfs diff=lfs merge=lfs -text diff --git a/data/.lfs/xarm7.tar.gz b/data/.lfs/xarm7.tar.gz new file mode 100644 index 0000000000..b19d8d919a --- /dev/null +++ b/data/.lfs/xarm7.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:dc5c96439cc415d7d7b1296363b5684354aaef22c7dbe8e50bce81183401511c +size 6297600 diff --git a/dimos/simulation/assets/xarm7/CHANGELOG.md b/dimos/simulation/assets/xarm7/CHANGELOG.md deleted file mode 100644 index e2b63f9633..0000000000 --- a/dimos/simulation/assets/xarm7/CHANGELOG.md +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1ffbf4c8599f2cf371a488ee878d4025f6500b999640de12546b631f37f8896e -size 351 diff --git a/dimos/simulation/assets/xarm7/LICENSE b/dimos/simulation/assets/xarm7/LICENSE deleted file mode 100644 index 4cf88471da..0000000000 --- a/dimos/simulation/assets/xarm7/LICENSE +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4fe659325706d0fc41592c8743d085a0bdf778056249f2a108943d22be916fb2 -size 1522 diff --git a/dimos/simulation/assets/xarm7/README.md b/dimos/simulation/assets/xarm7/README.md deleted file mode 100644 index 1ea70d040b..0000000000 --- a/dimos/simulation/assets/xarm7/README.md +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:42f9df53f08a54251e832a6454107fe9eec52581d250212856e345f06b53ca76 -size 1117 diff --git a/dimos/simulation/assets/xarm7/assets/base_link.stl b/dimos/simulation/assets/xarm7/assets/base_link.stl deleted file mode 100644 index ea9d15f635..0000000000 --- a/dimos/simulation/assets/xarm7/assets/base_link.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cdaa4cff22f7c9cff05c6a8ed32f94fd2b11a69d37dd97a159ccee2d8dd32f13 -size 1211434 diff --git a/dimos/simulation/assets/xarm7/assets/end_tool.stl b/dimos/simulation/assets/xarm7/assets/end_tool.stl deleted file mode 100644 index e9255ec68a..0000000000 --- a/dimos/simulation/assets/xarm7/assets/end_tool.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e06112781cdd615b7df536569f50addb445a6f4b4acced6d53b0742018b6c768 -size 13084 diff --git a/dimos/simulation/assets/xarm7/assets/left_finger.stl b/dimos/simulation/assets/xarm7/assets/left_finger.stl deleted file mode 100644 index 6bf4e50254..0000000000 --- a/dimos/simulation/assets/xarm7/assets/left_finger.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a44756eb72f9c214cb37e61dc209cd7073fdff3e4271a7423476ef6fd090d2d4 -size 242684 diff --git a/dimos/simulation/assets/xarm7/assets/left_inner_knuckle.stl b/dimos/simulation/assets/xarm7/assets/left_inner_knuckle.stl deleted file mode 100644 index 817c7e1d46..0000000000 --- a/dimos/simulation/assets/xarm7/assets/left_inner_knuckle.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e8e48692ad26837bb3d6a97582c89784d09948fc09bfe4e5a59017859ff04dac -size 366284 diff --git a/dimos/simulation/assets/xarm7/assets/left_outer_knuckle.stl b/dimos/simulation/assets/xarm7/assets/left_outer_knuckle.stl deleted file mode 100644 index 010c0f3ba0..0000000000 --- a/dimos/simulation/assets/xarm7/assets/left_outer_knuckle.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:501665812b08d67e764390db781e839adc6896a9540301d60adf606f57648921 -size 22284 diff --git a/dimos/simulation/assets/xarm7/assets/link1.stl b/dimos/simulation/assets/xarm7/assets/link1.stl deleted file mode 100644 index f2b676f29d..0000000000 --- a/dimos/simulation/assets/xarm7/assets/link1.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:34b541122df84d2ef5fcb91b715eb19659dc15ad8d44a191dde481f780265636 -size 184184 diff --git a/dimos/simulation/assets/xarm7/assets/link2.stl b/dimos/simulation/assets/xarm7/assets/link2.stl deleted file mode 100644 index bf93580c69..0000000000 --- a/dimos/simulation/assets/xarm7/assets/link2.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:61e641cd47c169ecef779683332e00e4914db729bf02dfb61bfbe69351827455 -size 225584 diff --git a/dimos/simulation/assets/xarm7/assets/link3.stl b/dimos/simulation/assets/xarm7/assets/link3.stl deleted file mode 100644 index d316d233fe..0000000000 --- a/dimos/simulation/assets/xarm7/assets/link3.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9e2798e7946dd70046c95455d5ba96392d0b54a6069caba91dc4ca66e1379b42 -size 237084 diff --git a/dimos/simulation/assets/xarm7/assets/link4.stl b/dimos/simulation/assets/xarm7/assets/link4.stl deleted file mode 100644 index f6d5fe94ad..0000000000 --- a/dimos/simulation/assets/xarm7/assets/link4.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c757fee95f873191a0633c355c07a360032960771cabbd7593a6cdb0f1ffb089 -size 243684 diff --git a/dimos/simulation/assets/xarm7/assets/link5.stl b/dimos/simulation/assets/xarm7/assets/link5.stl deleted file mode 100644 index e037b8b995..0000000000 --- a/dimos/simulation/assets/xarm7/assets/link5.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:715ad5787c5dab57589937fd47289882707b5e1eb997e340d567785b02f4ec90 -size 229084 diff --git a/dimos/simulation/assets/xarm7/assets/link6.stl b/dimos/simulation/assets/xarm7/assets/link6.stl deleted file mode 100644 index 198c53008a..0000000000 --- a/dimos/simulation/assets/xarm7/assets/link6.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:85b320aa420497827223d16d492bba8de091173374e361396fc7a5dad7bdb0cb -size 399384 diff --git a/dimos/simulation/assets/xarm7/assets/link7.stl b/dimos/simulation/assets/xarm7/assets/link7.stl deleted file mode 100644 index ce9a39ac78..0000000000 --- a/dimos/simulation/assets/xarm7/assets/link7.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:97115d848fbf802cb770cd9be639ae2af993103b9d9bbb0c50c943c738a36f18 -size 231684 diff --git a/dimos/simulation/assets/xarm7/assets/link_base.stl b/dimos/simulation/assets/xarm7/assets/link_base.stl deleted file mode 100644 index 58b35e2344..0000000000 --- a/dimos/simulation/assets/xarm7/assets/link_base.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cf52275a913a0977595f5d12373f06583513e26fe18b8cf2827c74da94a4f0be -size 304784 diff --git a/dimos/simulation/assets/xarm7/assets/right_finger.stl b/dimos/simulation/assets/xarm7/assets/right_finger.stl deleted file mode 100644 index 03f26e9ac8..0000000000 --- a/dimos/simulation/assets/xarm7/assets/right_finger.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c5dee87c7f37baf554b8456ebfe0b3e8ed0b22b8938bd1add6505c2ad6d32c7d -size 242684 diff --git a/dimos/simulation/assets/xarm7/assets/right_inner_knuckle.stl b/dimos/simulation/assets/xarm7/assets/right_inner_knuckle.stl deleted file mode 100644 index 8586f344c2..0000000000 --- a/dimos/simulation/assets/xarm7/assets/right_inner_knuckle.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b41dd2c2c550281bf78d7cc6fa117b14786700e5c453560a0cb5fd6dfa0ffb3e -size 366284 diff --git a/dimos/simulation/assets/xarm7/assets/right_outer_knuckle.stl b/dimos/simulation/assets/xarm7/assets/right_outer_knuckle.stl deleted file mode 100644 index ae7afc25f6..0000000000 --- a/dimos/simulation/assets/xarm7/assets/right_outer_knuckle.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:75ca1107d0a42a0f03802a9a49cab48419b31851ee8935f8f1ca06be1c1c91e8 -size 22284 diff --git a/dimos/simulation/assets/xarm7/hand.xml b/dimos/simulation/assets/xarm7/hand.xml deleted file mode 100644 index edb00ac880..0000000000 --- a/dimos/simulation/assets/xarm7/hand.xml +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2a15029be4490510e5b6f4eeffcc5b67fcee5410ca567899d29ab6f9229c03ee -size 5950 diff --git a/dimos/simulation/assets/xarm7/scene.xml b/dimos/simulation/assets/xarm7/scene.xml deleted file mode 100644 index e696365892..0000000000 --- a/dimos/simulation/assets/xarm7/scene.xml +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8c6f6a1b28c73126c347fff7d9b8b26223fc608b59c15e339f41948439024bbf -size 1529 diff --git a/dimos/simulation/assets/xarm7/xarm7.png b/dimos/simulation/assets/xarm7/xarm7.png deleted file mode 100644 index 0f324578a0..0000000000 --- a/dimos/simulation/assets/xarm7/xarm7.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:636e9e7a4396b54580231399dd64bb35d817814599eb160cede5886c63516228 -size 1692813 diff --git a/dimos/simulation/assets/xarm7/xarm7.xml b/dimos/simulation/assets/xarm7/xarm7.xml deleted file mode 100644 index f653a089a4..0000000000 --- a/dimos/simulation/assets/xarm7/xarm7.xml +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a81ba21ead4ad662542976d97beeab3bf07c7e9f689180123ea786008c3a942a -size 10764 diff --git a/dimos/simulation/assets/xarm7/xarm7_nohand.xml b/dimos/simulation/assets/xarm7/xarm7_nohand.xml deleted file mode 100644 index 658b641268..0000000000 --- a/dimos/simulation/assets/xarm7/xarm7_nohand.xml +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:90bcc0e2fbbe5fa95dbbafb74e56bd0106ecc6f5767a4dc3e7c511016efc023c -size 4756 diff --git a/dimos/simulation/sim_blueprints.py b/dimos/simulation/sim_blueprints.py index 5b4a541adc..3b0fbc1c48 100644 --- a/dimos/simulation/sim_blueprints.py +++ b/dimos/simulation/sim_blueprints.py @@ -12,8 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from pathlib import Path - from dimos.core.blueprints import autoconnect from dimos.core.transport import LCMTransport from dimos.msgs.sensor_msgs import ( # type: ignore[attr-defined] @@ -23,11 +21,12 @@ ) from dimos.msgs.trajectory_msgs import JointTrajectory from dimos.simulation.manipulators.sim_module import simulation +from dimos.utils.data import get_data xarm7_trajectory_sim = simulation( engine="mujoco", - config_path="dimos/simulation/assets/xarm7/scene.xml", - headless=True, + config_path=str(get_data("xarm7") / "scene.xml"), + headless=False, ).transports( { ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), From b61e4db833bca2b6c2b8a821e77e6ce9f0f3df14 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 23 Jan 2026 19:03:36 -0800 Subject: [PATCH 24/43] move xml parser to utils folder --- dimos/simulation/engines/mujoco_engine.py | 2 +- dimos/simulation/{manipulators => utils}/xml_parser.py | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename dimos/simulation/{manipulators => utils}/xml_parser.py (100%) diff --git a/dimos/simulation/engines/mujoco_engine.py b/dimos/simulation/engines/mujoco_engine.py index deac563fbd..352ff7227c 100644 --- a/dimos/simulation/engines/mujoco_engine.py +++ b/dimos/simulation/engines/mujoco_engine.py @@ -24,7 +24,7 @@ import mujoco.viewer as viewer # type: ignore[import-untyped] from dimos.simulation.engines.base import SimulationEngine -from dimos.simulation.manipulators.xml_parser import JointMapping, build_joint_mappings +from dimos.simulation.utils.xml_parser import JointMapping, build_joint_mappings from dimos.utils.logging_config import setup_logger logger = setup_logger() diff --git a/dimos/simulation/manipulators/xml_parser.py b/dimos/simulation/utils/xml_parser.py similarity index 100% rename from dimos/simulation/manipulators/xml_parser.py rename to dimos/simulation/utils/xml_parser.py From 460f24addba1f1b175c481261ec6a9b9069189c5 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 23 Jan 2026 19:09:05 -0800 Subject: [PATCH 25/43] remove 'mujoco' from sim module --- dimos/simulation/engines/__init__.py | 22 +++++++++++++++++++++ dimos/simulation/manipulators/sim_module.py | 10 ++-------- 2 files changed, 24 insertions(+), 8 deletions(-) diff --git a/dimos/simulation/engines/__init__.py b/dimos/simulation/engines/__init__.py index 4f4ecbbfdd..559c4f872d 100644 --- a/dimos/simulation/engines/__init__.py +++ b/dimos/simulation/engines/__init__.py @@ -1,7 +1,29 @@ """Simulation engines for manipulator backends.""" +from __future__ import annotations + +import importlib + from dimos.simulation.engines.base import SimulationEngine +_ENGINE_REGISTRY: dict[str, str] = { + "mujoco": "dimos.simulation.engines.mujoco_engine:MujocoEngine", +} + + +def get_engine(engine_name: str) -> type[SimulationEngine]: + key = engine_name.lower() + if key not in _ENGINE_REGISTRY: + raise ValueError(f"Unknown simulation engine: {engine_name}") + module_path, class_name = _ENGINE_REGISTRY[key].split(":") + module = importlib.import_module(module_path) + engine_cls = getattr(module, class_name) + if not issubclass(engine_cls, SimulationEngine): + raise TypeError(f"{engine_cls} is not a SimulationEngine") + return engine_cls + + __all__ = [ "SimulationEngine", + "get_engine", ] diff --git a/dimos/simulation/manipulators/sim_module.py b/dimos/simulation/manipulators/sim_module.py index 82e862b0a2..6ad839b0a4 100644 --- a/dimos/simulation/manipulators/sim_module.py +++ b/dimos/simulation/manipulators/sim_module.py @@ -26,7 +26,7 @@ from dimos.core import In, Module, Out, rpc from dimos.core.module import ModuleConfig from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState -from dimos.simulation.engines.mujoco_engine import MujocoEngine +from dimos.simulation.engines import get_engine from dimos.simulation.manipulators.sim_manip_interface import SimManipInterface @@ -49,9 +49,6 @@ class SimulationModule(Module[SimulationModuleConfig]): joint_velocity_command: In[JointCommand] MIN_CONTROL_RATE = 1.0 - _ENGINES = { - "mujoco": MujocoEngine, - } def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) @@ -71,10 +68,7 @@ def _create_backend(self) -> SimManipInterface: raise ValueError("engine is required for SimulationModule") if not self.config.config_path: raise ValueError("config_path is required for SimulationModule") - engine_key = self.config.engine.lower() - if engine_key not in self._ENGINES: - raise ValueError(f"Unknown simulation engine: {self.config.engine}") - engine_cls = self._ENGINES[engine_key] + engine_cls = get_engine(self.config.engine) engine = engine_cls( config_path=self.config.config_path, headless=self.config.headless, From ca4afc91be25bde14ff270834094cb23149c2933 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 23 Jan 2026 20:16:23 -0800 Subject: [PATCH 26/43] no time.sleeps for clock. monotonic time instead --- dimos/simulation/manipulators/sim_module.py | 80 ++++++++++++--------- 1 file changed, 47 insertions(+), 33 deletions(-) diff --git a/dimos/simulation/manipulators/sim_module.py b/dimos/simulation/manipulators/sim_module.py index 6ad839b0a4..aa3d510f73 100644 --- a/dimos/simulation/manipulators/sim_module.py +++ b/dimos/simulation/manipulators/sim_module.py @@ -179,6 +179,7 @@ def _on_joint_velocity_command(self, msg: JointCommand) -> None: def _control_loop(self) -> None: period = 1.0 / max(self._control_rate, self.MIN_CONTROL_RATE) + next_tick = time.monotonic() # monotonic time used to avoid time drift while not self._stop_event.is_set(): with self._command_lock: positions = ( @@ -193,46 +194,59 @@ def _control_loop(self) -> None: self._backend.write_joint_positions(positions) elif velocities is not None: self._backend.write_joint_velocities(velocities) - time.sleep(period) + next_tick += period + sleep_for = next_tick - time.monotonic() + if sleep_for > 0: + if self._stop_event.wait(sleep_for): + break + else: + next_tick = time.monotonic() def _monitor_loop(self) -> None: period = 1.0 / max(self._monitor_rate, self.MIN_CONTROL_RATE) + next_tick = time.monotonic() # monotonic time used to avoid time drift while not self._stop_event.is_set(): if not self._backend: - time.sleep(period) - continue - dof = self._backend.get_dof() - names = self._resolve_joint_names(dof) - positions = self._backend.read_joint_positions() - velocities = self._backend.read_joint_velocities() - efforts = self._backend.read_joint_efforts() - state = self._backend.read_state() - error_code, _ = self._backend.read_error() - - self.joint_state.publish( - JointState( - frame_id=self.frame_id, - name=names, - position=positions, - velocity=velocities, - effort=efforts, + pass + else: + dof = self._backend.get_dof() + names = self._resolve_joint_names(dof) + positions = self._backend.read_joint_positions() + velocities = self._backend.read_joint_velocities() + efforts = self._backend.read_joint_efforts() + state = self._backend.read_state() + error_code, _ = self._backend.read_error() + + self.joint_state.publish( + JointState( + frame_id=self.frame_id, + name=names, + position=positions, + velocity=velocities, + effort=efforts, + ) ) - ) - self.robot_state.publish( - RobotState( - state=state.get("state", 0), - mode=state.get("mode", 0), - error_code=error_code, - warn_code=0, - cmdnum=0, - mt_brake=0, - mt_able=1 if self._backend.read_enabled() else 0, - tcp_pose=[], - tcp_offset=[], - joints=[float(p) for p in positions], + self.robot_state.publish( + RobotState( + state=state.get("state", 0), + mode=state.get("mode", 0), + error_code=error_code, + warn_code=0, + cmdnum=0, + mt_brake=0, + mt_able=1 if self._backend.read_enabled() else 0, + tcp_pose=[], + tcp_offset=[], + joints=[float(p) for p in positions], + ) ) - ) - time.sleep(period) + next_tick += period + sleep_for = next_tick - time.monotonic() + if sleep_for > 0: + if self._stop_event.wait(sleep_for): + break + else: + next_tick = time.monotonic() def _resolve_joint_names(self, dof: int) -> list[str]: if self._backend: From 2d118e428e0690174c12ea865646b7aae5ca9ac5 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 23 Jan 2026 20:22:03 -0800 Subject: [PATCH 27/43] connect() and disconnect() return bool --- dimos/simulation/engines/base.py | 4 +- dimos/simulation/engines/mujoco_engine.py | 55 +++++++++++-------- .../manipulators/sim_manip_interface.py | 8 ++- 3 files changed, 39 insertions(+), 28 deletions(-) diff --git a/dimos/simulation/engines/base.py b/dimos/simulation/engines/base.py index 89dd4fb4e3..91b5af1d75 100644 --- a/dimos/simulation/engines/base.py +++ b/dimos/simulation/engines/base.py @@ -35,11 +35,11 @@ def headless(self) -> bool: return self._headless @abstractmethod - def connect(self) -> None: + def connect(self) -> bool: """Connect to simulation and start the engine.""" @abstractmethod - def disconnect(self) -> None: + def disconnect(self) -> bool: """Disconnect from simulation and stop the engine.""" @property diff --git a/dimos/simulation/engines/mujoco_engine.py b/dimos/simulation/engines/mujoco_engine.py index 352ff7227c..08e463c9b5 100644 --- a/dimos/simulation/engines/mujoco_engine.py +++ b/dimos/simulation/engines/mujoco_engine.py @@ -128,29 +128,38 @@ def _update_joint_state(self) -> None: if mapping.actuator_id is not None: self._joint_efforts[i] = float(self._data.actuator_force[mapping.actuator_id]) - def connect(self) -> None: - logger.info(f"{self.__class__.__name__}: connect()") - with self._lock: - self._connected = True - self._stop_event.clear() - - if self._sim_thread is None or not self._sim_thread.is_alive(): - self._sim_thread = threading.Thread( - target=self._sim_loop, - name=f"{self.__class__.__name__}Sim", - daemon=True, - ) - self._sim_thread.start() - - def disconnect(self) -> None: - logger.info(f"{self.__class__.__name__}: disconnect()") - with self._lock: - self._connected = False - - self._stop_event.set() - if self._sim_thread and self._sim_thread.is_alive(): - self._sim_thread.join(timeout=2.0) - self._sim_thread = None + def connect(self) -> bool: + try: + logger.info(f"{self.__class__.__name__}: connect()") + with self._lock: + self._connected = True + self._stop_event.clear() + + if self._sim_thread is None or not self._sim_thread.is_alive(): + self._sim_thread = threading.Thread( + target=self._sim_loop, + name=f"{self.__class__.__name__}Sim", + daemon=True, + ) + self._sim_thread.start() + return True + except Exception as e: + logger.error(f"{self.__class__.__name__}: connect() failed: {e}") + return False + + def disconnect(self) -> bool: + try: + logger.info(f"{self.__class__.__name__}: disconnect()") + with self._lock: + self._connected = False + self._stop_event.set() + if self._sim_thread and self._sim_thread.is_alive(): + self._sim_thread.join(timeout=2.0) + self._sim_thread = None + return True + except Exception as e: + logger.error(f"{self.__class__.__name__}: disconnect() failed: {e}") + return False def _sim_loop(self) -> None: logger.info(f"{self.__class__.__name__}: sim loop started") diff --git a/dimos/simulation/manipulators/sim_manip_interface.py b/dimos/simulation/manipulators/sim_manip_interface.py index 478c022480..21634f9fbe 100644 --- a/dimos/simulation/manipulators/sim_manip_interface.py +++ b/dimos/simulation/manipulators/sim_manip_interface.py @@ -44,7 +44,9 @@ def connect(self) -> bool: """Connect to the simulation engine.""" try: self.logger.info("Connecting to simulation engine...") - self._engine.connect() + if not self._engine.connect(): + self.logger.error("Failed to connect to simulation engine") + return False if self._engine.connected: self._connected = True self._servos_enabled = True @@ -61,10 +63,10 @@ def connect(self) -> bool: self.logger.error(f"Sim connection failed: {exc}") return False - def disconnect(self) -> None: + def disconnect(self) -> bool: """Disconnect from simulation.""" try: - self._engine.disconnect() + return self._engine.disconnect() finally: self._connected = False From 7fec092765f054bfed008250614b3a2907b45711 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 23 Jan 2026 22:32:00 -0800 Subject: [PATCH 28/43] consolidate write commands into one and fix velocity control --- dimos/simulation/engines/base.py | 12 +++--- dimos/simulation/engines/mujoco_engine.py | 48 ++++++++++++++++++----- 2 files changed, 45 insertions(+), 15 deletions(-) diff --git a/dimos/simulation/engines/base.py b/dimos/simulation/engines/base.py index 91b5af1d75..cba1d6e6c4 100644 --- a/dimos/simulation/engines/base.py +++ b/dimos/simulation/engines/base.py @@ -17,6 +17,10 @@ from __future__ import annotations from abc import ABC, abstractmethod +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from dimos.msgs.sensor_msgs import JointState class SimulationEngine(ABC): @@ -70,9 +74,5 @@ def read_joint_efforts(self) -> list[float]: """Read joint efforts in Nm.""" @abstractmethod - def write_joint_positions(self, positions: list[float]) -> None: - """Command joint positions in radians.""" - - @abstractmethod - def write_joint_velocities(self, velocities: list[float]) -> None: - """Command joint velocities in rad/s.""" + def write_joint_command(self, command: JointState) -> None: + """Command joints using a JointState message.""" diff --git a/dimos/simulation/engines/mujoco_engine.py b/dimos/simulation/engines/mujoco_engine.py index 08e463c9b5..738ea6c1a5 100644 --- a/dimos/simulation/engines/mujoco_engine.py +++ b/dimos/simulation/engines/mujoco_engine.py @@ -19,6 +19,7 @@ from pathlib import Path import threading import time +from typing import TYPE_CHECKING import mujoco import mujoco.viewer as viewer # type: ignore[import-untyped] @@ -27,6 +28,9 @@ from dimos.simulation.utils.xml_parser import JointMapping, build_joint_mappings from dimos.utils.logging_config import setup_logger +if TYPE_CHECKING: + from dimos.msgs.sensor_msgs import JointState + logger = setup_logger() @@ -63,6 +67,9 @@ def __init__(self, config_path: str | None, headless: bool) -> None: self._joint_efforts = [0.0] * self._num_joints self._joint_position_targets = [0.0] * self._num_joints + self._joint_velocity_targets = [0.0] * self._num_joints + self._joint_effort_targets = [0.0] * self._num_joints + self._command_mode = "position" for i, mapping in enumerate(self._joint_mappings): current_pos = self._current_position(mapping) self._joint_position_targets[i] = current_pos @@ -91,13 +98,18 @@ def _current_position(self, mapping: JointMapping) -> float: def _apply_control(self) -> None: with self._lock: - pos_targets = list(self._joint_position_targets) + if self._command_mode == "effort": + targets = list(self._joint_effort_targets) + elif self._command_mode == "velocity": + targets = list(self._joint_velocity_targets) + elif self._command_mode == "position": + targets = list(self._joint_position_targets) with self._lock: for i, mapping in enumerate(self._joint_mappings): if mapping.actuator_id is None: continue - if i < len(pos_targets): - self._data.ctrl[mapping.actuator_id] = pos_targets[i] + if i < len(targets): + self._data.ctrl[mapping.actuator_id] = targets[i] def _update_joint_state(self) -> None: with self._lock: @@ -235,23 +247,41 @@ def read_joint_velocities(self) -> list[float]: def read_joint_efforts(self) -> list[float]: return self.joint_efforts - def write_joint_positions(self, positions: list[float]) -> None: + def write_joint_command(self, command: JointState) -> None: + if command.position: + self._command_mode = "position" + self._set_position_targets(command.position) + return + if command.velocity: + self._command_mode = "velocity" + self._set_velocity_targets(command.velocity) + return + if command.effort: + self._command_mode = "effort" + self._set_effort_targets(command.effort) + return + + def _set_position_targets(self, positions: list[float]) -> None: with self._lock: limit = min(len(positions), self._num_joints) for i in range(limit): self._joint_position_targets[i] = float(positions[i]) - def write_joint_velocities(self, velocities: list[float]) -> None: - dt = 1.0 / self._control_frequency + def _set_velocity_targets(self, velocities: list[float]) -> None: with self._lock: limit = min(len(velocities), self._num_joints) for i in range(limit): - self._joint_position_targets[i] = ( - self._joint_positions[i] + float(velocities[i]) * dt - ) + self._joint_velocity_targets[i] = float(velocities[i]) + + def _set_effort_targets(self, efforts: list[float]) -> None: + with self._lock: + limit = min(len(efforts), self._num_joints) + for i in range(limit): + self._joint_effort_targets[i] = float(efforts[i]) def hold_current_position(self) -> None: with self._lock: + self._command_mode = "position" for i, mapping in enumerate(self._joint_mappings): self._joint_position_targets[i] = self._current_position(mapping) From 104c4fe6de927c97e96ef92717e2daaf27306e07 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 23 Jan 2026 22:46:26 -0800 Subject: [PATCH 29/43] config_path changed to Path type and mandatory --- dimos/simulation/engines/mujoco_engine.py | 11 ++++---- dimos/simulation/manipulators/sim_module.py | 30 ++++++++++++--------- dimos/simulation/sim_blueprints.py | 2 +- 3 files changed, 24 insertions(+), 19 deletions(-) diff --git a/dimos/simulation/engines/mujoco_engine.py b/dimos/simulation/engines/mujoco_engine.py index 738ea6c1a5..8736afd63a 100644 --- a/dimos/simulation/engines/mujoco_engine.py +++ b/dimos/simulation/engines/mujoco_engine.py @@ -16,7 +16,6 @@ from __future__ import annotations -from pathlib import Path import threading import time from typing import TYPE_CHECKING @@ -29,6 +28,8 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: + from pathlib import Path + from dimos.msgs.sensor_msgs import JointState logger = setup_logger() @@ -43,7 +44,7 @@ class MujocoEngine(SimulationEngine): - applies control commands """ - def __init__(self, config_path: str | None, headless: bool) -> None: + def __init__(self, config_path: Path, headless: bool) -> None: super().__init__(config_path=config_path, headless=headless) xml_path = self._resolve_xml_path(config_path) @@ -75,10 +76,10 @@ def __init__(self, config_path: str | None, headless: bool) -> None: self._joint_position_targets[i] = current_pos self._joint_positions[i] = current_pos - def _resolve_xml_path(self, config_path: str | None) -> Path: - if not config_path: + def _resolve_xml_path(self, config_path: Path) -> Path: + if config_path is None: raise ValueError("config_path is required for MuJoCo simulation loading") - resolved = Path(config_path).expanduser() + resolved = config_path.expanduser() xml_path = resolved / "scene.xml" if resolved.is_dir() else resolved if not xml_path.exists(): raise FileNotFoundError(f"MuJoCo XML not found: {xml_path}") diff --git a/dimos/simulation/manipulators/sim_module.py b/dimos/simulation/manipulators/sim_module.py index aa3d510f73..6945f1626e 100644 --- a/dimos/simulation/manipulators/sim_module.py +++ b/dimos/simulation/manipulators/sim_module.py @@ -194,6 +194,20 @@ def _control_loop(self) -> None: self._backend.write_joint_positions(positions) elif velocities is not None: self._backend.write_joint_velocities(velocities) + dof = self._backend.get_dof() + names = self._resolve_joint_names(dof) + positions = self._backend.read_joint_positions() + velocities = self._backend.read_joint_velocities() + efforts = self._backend.read_joint_efforts() + self.joint_state.publish( + JointState( + frame_id=self.frame_id, + name=names, + position=positions, + velocity=velocities, + effort=efforts, + ) + ) next_tick += period sleep_for = next_tick - time.monotonic() if sleep_for > 0: @@ -210,22 +224,12 @@ def _monitor_loop(self) -> None: pass else: dof = self._backend.get_dof() - names = self._resolve_joint_names(dof) + self._resolve_joint_names(dof) positions = self._backend.read_joint_positions() - velocities = self._backend.read_joint_velocities() - efforts = self._backend.read_joint_efforts() + self._backend.read_joint_velocities() + self._backend.read_joint_efforts() state = self._backend.read_state() error_code, _ = self._backend.read_error() - - self.joint_state.publish( - JointState( - frame_id=self.frame_id, - name=names, - position=positions, - velocity=velocities, - effort=efforts, - ) - ) self.robot_state.publish( RobotState( state=state.get("state", 0), diff --git a/dimos/simulation/sim_blueprints.py b/dimos/simulation/sim_blueprints.py index 3b0fbc1c48..55320cf76f 100644 --- a/dimos/simulation/sim_blueprints.py +++ b/dimos/simulation/sim_blueprints.py @@ -25,7 +25,7 @@ xarm7_trajectory_sim = simulation( engine="mujoco", - config_path=str(get_data("xarm7") / "scene.xml"), + config_path=get_data("xarm7") / "scene.xml", headless=False, ).transports( { From e62a2490b4db02a7facafd9292a19d4eb9fa3d4c Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 23 Jan 2026 23:00:24 -0800 Subject: [PATCH 30/43] engine typing --- dimos/simulation/engines/__init__.py | 22 +++++++++------------ dimos/simulation/manipulators/sim_module.py | 15 +++++++------- 2 files changed, 16 insertions(+), 21 deletions(-) diff --git a/dimos/simulation/engines/__init__.py b/dimos/simulation/engines/__init__.py index 559c4f872d..d437f9a7cd 100644 --- a/dimos/simulation/engines/__init__.py +++ b/dimos/simulation/engines/__init__.py @@ -2,28 +2,24 @@ from __future__ import annotations -import importlib +from typing import Literal from dimos.simulation.engines.base import SimulationEngine +from dimos.simulation.engines.mujoco_engine import MujocoEngine -_ENGINE_REGISTRY: dict[str, str] = { - "mujoco": "dimos.simulation.engines.mujoco_engine:MujocoEngine", +EngineType = Literal["mujoco"] + +_ENGINES: dict[EngineType, type[SimulationEngine]] = { + "mujoco": MujocoEngine, } -def get_engine(engine_name: str) -> type[SimulationEngine]: - key = engine_name.lower() - if key not in _ENGINE_REGISTRY: - raise ValueError(f"Unknown simulation engine: {engine_name}") - module_path, class_name = _ENGINE_REGISTRY[key].split(":") - module = importlib.import_module(module_path) - engine_cls = getattr(module, class_name) - if not issubclass(engine_cls, SimulationEngine): - raise TypeError(f"{engine_cls} is not a SimulationEngine") - return engine_cls +def get_engine(engine_name: EngineType) -> type[SimulationEngine]: + return _ENGINES[engine_name] __all__ = [ + "EngineType", "SimulationEngine", "get_engine", ] diff --git a/dimos/simulation/manipulators/sim_module.py b/dimos/simulation/manipulators/sim_module.py index 6945f1626e..e3db1146e8 100644 --- a/dimos/simulation/manipulators/sim_module.py +++ b/dimos/simulation/manipulators/sim_module.py @@ -19,21 +19,24 @@ from dataclasses import dataclass import threading import time -from typing import Any +from typing import TYPE_CHECKING, Any from reactivex.disposable import Disposable from dimos.core import In, Module, Out, rpc from dimos.core.module import ModuleConfig from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState -from dimos.simulation.engines import get_engine +from dimos.simulation.engines import EngineType, get_engine from dimos.simulation.manipulators.sim_manip_interface import SimManipInterface +if TYPE_CHECKING: + from pathlib import Path + @dataclass class SimulationModuleConfig(ModuleConfig): - engine: str | None = None - config_path: str | None = None + engine: EngineType + config_path: Path headless: bool = False @@ -64,10 +67,6 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: self._pending_velocities: list[float] | None = None def _create_backend(self) -> SimManipInterface: - if not self.config.engine: - raise ValueError("engine is required for SimulationModule") - if not self.config.config_path: - raise ValueError("config_path is required for SimulationModule") engine_cls = get_engine(self.config.engine) engine = engine_cls( config_path=self.config.config_path, From 22d71ec2928ea91682f9e3cd50ef84b0cd041456 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 23 Jan 2026 23:17:25 -0800 Subject: [PATCH 31/43] typing fix, no default so set kw_only=True --- dimos/simulation/manipulators/sim_module.py | 41 ++++----------------- 1 file changed, 7 insertions(+), 34 deletions(-) diff --git a/dimos/simulation/manipulators/sim_module.py b/dimos/simulation/manipulators/sim_module.py index e3db1146e8..5d203f1ef0 100644 --- a/dimos/simulation/manipulators/sim_module.py +++ b/dimos/simulation/manipulators/sim_module.py @@ -33,7 +33,7 @@ from pathlib import Path -@dataclass +@dataclass(kw_only=True) class SimulationModuleConfig(ModuleConfig): engine: EngineType config_path: Path @@ -83,39 +83,12 @@ def start(self) -> None: raise RuntimeError("Failed to connect to simulation backend") self._backend.write_enable(True) - try: - if ( - self.joint_position_command.connection is not None - or self.joint_position_command._transport is not None - ): - self._disposables.add( - Disposable( - self.joint_position_command.subscribe(self._on_joint_position_command) - ) - ) - except Exception as exc: - import logging - - logging.getLogger(self.__class__.__name__).warning( - f"Failed to subscribe joint_position_command: {exc}" - ) - - try: - if ( - self.joint_velocity_command.connection is not None - or self.joint_velocity_command._transport is not None - ): - self._disposables.add( - Disposable( - self.joint_velocity_command.subscribe(self._on_joint_velocity_command) - ) - ) - except Exception as exc: - import logging - - logging.getLogger(self.__class__.__name__).warning( - f"Failed to subscribe joint_velocity_command: {exc}" - ) + self._disposables.add( + Disposable(self.joint_position_command.subscribe(self._on_joint_position_command)) + ) + self._disposables.add( + Disposable(self.joint_velocity_command.subscribe(self._on_joint_velocity_command)) + ) self._stop_event.clear() self._control_thread = threading.Thread( From f06fb46ede7aa4cb7f1524fa0c0552d0f4df3203 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 23 Jan 2026 23:35:00 -0800 Subject: [PATCH 32/43] add hold_current_position() to base --- dimos/simulation/engines/base.py | 4 ++++ .../manipulators/sim_manip_interface.py | 20 +++++++++++-------- 2 files changed, 16 insertions(+), 8 deletions(-) diff --git a/dimos/simulation/engines/base.py b/dimos/simulation/engines/base.py index cba1d6e6c4..e928519019 100644 --- a/dimos/simulation/engines/base.py +++ b/dimos/simulation/engines/base.py @@ -76,3 +76,7 @@ def read_joint_efforts(self) -> list[float]: @abstractmethod def write_joint_command(self, command: JointState) -> None: """Command joints using a JointState message.""" + + @abstractmethod + def hold_current_position(self) -> None: + """Hold current joint positions.""" diff --git a/dimos/simulation/manipulators/sim_manip_interface.py b/dimos/simulation/manipulators/sim_manip_interface.py index 21634f9fbe..121f54d2b0 100644 --- a/dimos/simulation/manipulators/sim_manip_interface.py +++ b/dimos/simulation/manipulators/sim_manip_interface.py @@ -21,6 +21,7 @@ from typing import TYPE_CHECKING from dimos.hardware.manipulators.spec import ControlMode, JointLimits, ManipulatorInfo +from dimos.msgs.sensor_msgs import JointState if TYPE_CHECKING: from dimos.simulation.engines.base import SimulationEngine @@ -132,26 +133,29 @@ def read_state(self) -> dict[str, int]: def read_error(self) -> tuple[int, str]: return self._error_code, self._error_message - def write_joint_positions(self, positions: list[float], velocity: float = 1.0) -> bool: - _ = velocity + def write_joint_positions(self, positions: list[float]) -> bool: if not self._servos_enabled: return False self._control_mode = ControlMode.POSITION - self._engine.write_joint_positions(positions[: self._dof]) + self._engine.write_joint_command(JointState(position=positions[: self._dof])) return True def write_joint_velocities(self, velocities: list[float]) -> bool: if not self._servos_enabled: return False self._control_mode = ControlMode.VELOCITY - self._engine.write_joint_velocities(velocities[: self._dof]) + self._engine.write_joint_command(JointState(velocity=velocities[: self._dof])) + return True + + def write_joint_efforts(self, efforts: list[float]) -> bool: + if not self._servos_enabled: + return False + self._control_mode = ControlMode.TORQUE + self._engine.write_joint_command(JointState(effort=efforts[: self._dof])) return True def write_stop(self) -> bool: - if hasattr(self._engine, "hold_current_position"): - self._engine.hold_current_position() # type: ignore[attr-defined] - return True - self._engine.write_joint_velocities([0.0] * self._dof) + self._engine.hold_current_position() return True def write_enable(self, enable: bool) -> bool: From 7a6662ddb673b01b0462da6087392b7203c947e5 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 23 Jan 2026 23:37:44 -0800 Subject: [PATCH 33/43] fix locks and make sure array length is as expected --- dimos/simulation/engines/mujoco_engine.py | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/dimos/simulation/engines/mujoco_engine.py b/dimos/simulation/engines/mujoco_engine.py index 8736afd63a..6dc8e46595 100644 --- a/dimos/simulation/engines/mujoco_engine.py +++ b/dimos/simulation/engines/mujoco_engine.py @@ -105,7 +105,6 @@ def _apply_control(self) -> None: targets = list(self._joint_velocity_targets) elif self._command_mode == "position": targets = list(self._joint_position_targets) - with self._lock: for i, mapping in enumerate(self._joint_mappings): if mapping.actuator_id is None: continue @@ -263,21 +262,30 @@ def write_joint_command(self, command: JointState) -> None: return def _set_position_targets(self, positions: list[float]) -> None: + if len(positions) > self._num_joints: + raise ValueError( + f"Position command has {len(positions)} joints, expected at most {self._num_joints}" + ) with self._lock: - limit = min(len(positions), self._num_joints) - for i in range(limit): + for i in range(len(positions)): self._joint_position_targets[i] = float(positions[i]) def _set_velocity_targets(self, velocities: list[float]) -> None: + if len(velocities) > self._num_joints: + raise ValueError( + f"Velocity command has {len(velocities)} joints, expected at most {self._num_joints}" + ) with self._lock: - limit = min(len(velocities), self._num_joints) - for i in range(limit): + for i in range(len(velocities)): self._joint_velocity_targets[i] = float(velocities[i]) def _set_effort_targets(self, efforts: list[float]) -> None: + if len(efforts) > self._num_joints: + raise ValueError( + f"Effort command has {len(efforts)} joints, expected at most {self._num_joints}" + ) with self._lock: - limit = min(len(efforts), self._num_joints) - for i in range(limit): + for i in range(len(efforts)): self._joint_effort_targets[i] = float(efforts[i]) def hold_current_position(self) -> None: From bc98dd357fa3896584a4d026c063e9b0ea5bc543 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 23 Jan 2026 23:53:05 -0800 Subject: [PATCH 34/43] revise e2e tests to spin up blueprint --- dimos/e2e_tests/test_simulation_module.py | 32 ++++++++++++++++++++++- dimos/simulation/sim_blueprints.py | 2 +- 2 files changed, 32 insertions(+), 2 deletions(-) diff --git a/dimos/e2e_tests/test_simulation_module.py b/dimos/e2e_tests/test_simulation_module.py index c94fd7b97f..2c60466438 100644 --- a/dimos/e2e_tests/test_simulation_module.py +++ b/dimos/e2e_tests/test_simulation_module.py @@ -18,7 +18,7 @@ import pytest -from dimos.msgs.sensor_msgs import JointCommand, JointState +from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState def _positions_within_tolerance( @@ -33,6 +33,36 @@ def _positions_within_tolerance( @pytest.mark.skipif(bool(os.getenv("CI")), reason="LCM doesn't work in CI.") class TestSimulationModuleE2E: + def test_xarm7_joint_state_published(self, lcm_spy, start_blueprint) -> None: + joint_state_topic = "/xarm/joint_states#sensor_msgs.JointState" + lcm_spy.save_topic(joint_state_topic) + + start_blueprint("simulation-xarm7") + lcm_spy.wait_for_saved_topic(joint_state_topic, timeout=15.0) + + with lcm_spy._messages_lock: + raw_joint_state = lcm_spy.messages[joint_state_topic][0] + + joint_state = JointState.lcm_decode(raw_joint_state) + assert len(joint_state.name) == 8 + assert len(joint_state.position) == 8 + assert "joint1" in joint_state.name + assert "joint7" in joint_state.name + assert "gripper" in joint_state.name + + def test_xarm7_robot_state_published(self, lcm_spy, start_blueprint) -> None: + robot_state_topic = "/xarm/robot_state#sensor_msgs.RobotState" + lcm_spy.save_topic(robot_state_topic) + + start_blueprint("simulation-xarm7") + lcm_spy.wait_for_saved_topic(robot_state_topic, timeout=15.0) + + with lcm_spy._messages_lock: + raw_robot_state = lcm_spy.messages[robot_state_topic][0] + + robot_state = RobotState.lcm_decode(raw_robot_state) + assert robot_state.mt_able in (0, 1) + def test_xarm7_joint_command_updates_joint_state(self, lcm_spy, start_blueprint) -> None: joint_state_topic = "/xarm/joint_states#sensor_msgs.JointState" joint_command_topic = "/xarm/joint_position_command#sensor_msgs.JointCommand" diff --git a/dimos/simulation/sim_blueprints.py b/dimos/simulation/sim_blueprints.py index 55320cf76f..6d0a2b7a7c 100644 --- a/dimos/simulation/sim_blueprints.py +++ b/dimos/simulation/sim_blueprints.py @@ -26,7 +26,7 @@ xarm7_trajectory_sim = simulation( engine="mujoco", config_path=get_data("xarm7") / "scene.xml", - headless=False, + headless=True, ).transports( { ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), From 499ede526909288255804f9934ab5a5f2dd6800a Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Fri, 23 Jan 2026 23:58:28 -0800 Subject: [PATCH 35/43] fix sim module tests --- .../manipulators/test_sim_module.py | 30 +++++++++++++++---- 1 file changed, 24 insertions(+), 6 deletions(-) diff --git a/dimos/simulation/manipulators/test_sim_module.py b/dimos/simulation/manipulators/test_sim_module.py index 89835f2e2a..334e2ce85f 100644 --- a/dimos/simulation/manipulators/test_sim_module.py +++ b/dimos/simulation/manipulators/test_sim_module.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +from pathlib import Path import threading -import time import pytest @@ -64,24 +64,38 @@ def disconnect(self) -> None: def _run_single_monitor_iteration(module: SimulationModule, monkeypatch) -> None: # type: ignore[no-untyped-def] - def _sleep_once(_: float) -> None: + def _wait_once(_: float) -> bool: module._stop_event.set() raise StopIteration - monkeypatch.setattr(time, "sleep", _sleep_once) + monkeypatch.setattr(module._stop_event, "wait", _wait_once) with pytest.raises(StopIteration): module._monitor_loop() +def _run_single_control_iteration(module: SimulationModule, monkeypatch) -> None: # type: ignore[no-untyped-def] + def _wait_once(_: float) -> bool: + module._stop_event.set() + raise StopIteration + + monkeypatch.setattr(module._stop_event, "wait", _wait_once) + with pytest.raises(StopIteration): + module._control_loop() + + def test_simulation_module_publishes_joint_state(monkeypatch) -> None: - module = SimulationModule(rpc_transport=_DummyRPC) + module = SimulationModule( + engine="mujoco", + config_path=Path("."), + rpc_transport=_DummyRPC, + ) module._backend = _FakeBackend() # type: ignore[assignment] module._stop_event = threading.Event() joint_states: list[object] = [] module.joint_state.subscribe(joint_states.append) try: - _run_single_monitor_iteration(module, monkeypatch) + _run_single_control_iteration(module, monkeypatch) finally: module.stop() @@ -90,7 +104,11 @@ def test_simulation_module_publishes_joint_state(monkeypatch) -> None: def test_simulation_module_publishes_robot_state(monkeypatch) -> None: - module = SimulationModule(rpc_transport=_DummyRPC) + module = SimulationModule( + engine="mujoco", + config_path=Path("."), + rpc_transport=_DummyRPC, + ) module._backend = _FakeBackend() # type: ignore[assignment] module._stop_event = threading.Event() From 2c5266903e09a77564e24c8dfba3eb88a5c6bc4a Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Sat, 24 Jan 2026 00:47:21 -0800 Subject: [PATCH 36/43] fix mypy errors --- dimos/simulation/engines/base.py | 6 ++++-- dimos/simulation/utils/xml_parser.py | 5 ++++- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/dimos/simulation/engines/base.py b/dimos/simulation/engines/base.py index e928519019..d450614c62 100644 --- a/dimos/simulation/engines/base.py +++ b/dimos/simulation/engines/base.py @@ -20,18 +20,20 @@ from typing import TYPE_CHECKING if TYPE_CHECKING: + from pathlib import Path + from dimos.msgs.sensor_msgs import JointState class SimulationEngine(ABC): """Abstract base class for a simulator engine instance.""" - def __init__(self, config_path: str | None, headless: bool) -> None: + def __init__(self, config_path: Path, headless: bool) -> None: self._config_path = config_path self._headless = headless @property - def config_path(self) -> str | None: + def config_path(self) -> Path: return self._config_path @property diff --git a/dimos/simulation/utils/xml_parser.py b/dimos/simulation/utils/xml_parser.py index 24b1bbce15..052657ea95 100644 --- a/dimos/simulation/utils/xml_parser.py +++ b/dimos/simulation/utils/xml_parser.py @@ -126,13 +126,16 @@ def _mapping_for_joint(spec: _ActuatorSpec, model: mujoco.MjModel) -> JointMappi def _mapping_for_tendon(spec: _ActuatorSpec, model: mujoco.MjModel) -> JointMapping: + name = spec.name or spec.tendon + if not name: + raise ValueError("Tendon actuator is missing a name and tendon reference") tendon_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_TENDON, spec.tendon) if tendon_id < 0: raise ValueError(f"Unknown tendon '{spec.tendon}' in MuJoCo model") actuator_id = _find_actuator_id_for_tendon(model, tendon_id, spec.name) joint_ids = _tendon_joint_ids(model, tendon_id) return JointMapping( - name=spec.name or spec.tendon, + name=name, joint_id=None, actuator_id=actuator_id, qpos_adr=None, From 645a5f02f0aae729a09eff89a1f6c3d0aa885b13 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Sat, 24 Jan 2026 00:52:57 -0800 Subject: [PATCH 37/43] fetch LFS when module starts --- dimos/simulation/manipulators/sim_module.py | 10 ++++++++-- dimos/simulation/sim_blueprints.py | 4 +++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/dimos/simulation/manipulators/sim_module.py b/dimos/simulation/manipulators/sim_module.py index 5d203f1ef0..4f1bb986d3 100644 --- a/dimos/simulation/manipulators/sim_module.py +++ b/dimos/simulation/manipulators/sim_module.py @@ -30,13 +30,14 @@ from dimos.simulation.manipulators.sim_manip_interface import SimManipInterface if TYPE_CHECKING: + from collections.abc import Callable from pathlib import Path @dataclass(kw_only=True) class SimulationModuleConfig(ModuleConfig): engine: EngineType - config_path: Path + config_path: Path | Callable[[], Path] headless: bool = False @@ -68,8 +69,13 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: def _create_backend(self) -> SimManipInterface: engine_cls = get_engine(self.config.engine) + config_path = ( + self.config.config_path() + if callable(self.config.config_path) + else self.config.config_path + ) engine = engine_cls( - config_path=self.config.config_path, + config_path=config_path, headless=self.config.headless, ) return SimManipInterface(engine=engine) diff --git a/dimos/simulation/sim_blueprints.py b/dimos/simulation/sim_blueprints.py index 6d0a2b7a7c..f2175515b2 100644 --- a/dimos/simulation/sim_blueprints.py +++ b/dimos/simulation/sim_blueprints.py @@ -12,6 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +from pathlib import Path + from dimos.core.blueprints import autoconnect from dimos.core.transport import LCMTransport from dimos.msgs.sensor_msgs import ( # type: ignore[attr-defined] @@ -25,7 +27,7 @@ xarm7_trajectory_sim = simulation( engine="mujoco", - config_path=get_data("xarm7") / "scene.xml", + config_path=lambda: get_data("xarm7") / "scene.xml", headless=True, ).transports( { From 9f5f919bd1e60ee148e079df2a7070b6e056617b Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Sat, 24 Jan 2026 15:16:55 -0800 Subject: [PATCH 38/43] clean up --- dimos/e2e_tests/test_simulation_module.py | 3 --- dimos/simulation/manipulators/sim_manip_interface.py | 8 +++++--- dimos/simulation/sim_blueprints.py | 3 ++- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/dimos/e2e_tests/test_simulation_module.py b/dimos/e2e_tests/test_simulation_module.py index 2c60466438..bfeef4110d 100644 --- a/dimos/e2e_tests/test_simulation_module.py +++ b/dimos/e2e_tests/test_simulation_module.py @@ -46,9 +46,6 @@ def test_xarm7_joint_state_published(self, lcm_spy, start_blueprint) -> None: joint_state = JointState.lcm_decode(raw_joint_state) assert len(joint_state.name) == 8 assert len(joint_state.position) == 8 - assert "joint1" in joint_state.name - assert "joint7" in joint_state.name - assert "gripper" in joint_state.name def test_xarm7_robot_state_published(self, lcm_spy, start_blueprint) -> None: robot_state_topic = "/xarm/robot_state#sensor_msgs.RobotState" diff --git a/dimos/simulation/manipulators/sim_manip_interface.py b/dimos/simulation/manipulators/sim_manip_interface.py index 121f54d2b0..c829f0c864 100644 --- a/dimos/simulation/manipulators/sim_manip_interface.py +++ b/dimos/simulation/manipulators/sim_manip_interface.py @@ -68,8 +68,10 @@ def disconnect(self) -> bool: """Disconnect from simulation.""" try: return self._engine.disconnect() - finally: + except Exception as exc: self._connected = False + self.logger.error(f"Sim disconnection failed: {exc}") + return False def is_connected(self) -> bool: return bool(self._connected and self._engine.connected) @@ -178,8 +180,8 @@ def write_cartesian_position( pose: dict[str, float], velocity: float = 1.0, ) -> bool: - _ = pose - _ = velocity + _pose = pose + _velocity = velocity return False def read_gripper_position(self) -> float | None: diff --git a/dimos/simulation/sim_blueprints.py b/dimos/simulation/sim_blueprints.py index f2175515b2..6b5c69cad7 100644 --- a/dimos/simulation/sim_blueprints.py +++ b/dimos/simulation/sim_blueprints.py @@ -27,7 +27,8 @@ xarm7_trajectory_sim = simulation( engine="mujoco", - config_path=lambda: get_data("xarm7") / "scene.xml", + config_path=lambda: get_data("xarm7") + / "scene.xml", # avoid triggering LFS downloads during tests headless=True, ).transports( { From 224575295dd85e7f0c26fe2cce2fc8fb5cc602a2 Mon Sep 17 00:00:00 2001 From: Jing Cao <38737296+jca0@users.noreply.github.com> Date: Sun, 25 Jan 2026 03:16:20 -0800 Subject: [PATCH 39/43] Update test_simulation_module.py Co-authored-by: Paul Nechifor --- dimos/e2e_tests/test_simulation_module.py | 1 + 1 file changed, 1 insertion(+) diff --git a/dimos/e2e_tests/test_simulation_module.py b/dimos/e2e_tests/test_simulation_module.py index bfeef4110d..32ac01a183 100644 --- a/dimos/e2e_tests/test_simulation_module.py +++ b/dimos/e2e_tests/test_simulation_module.py @@ -32,6 +32,7 @@ def _positions_within_tolerance( @pytest.mark.skipif(bool(os.getenv("CI")), reason="LCM doesn't work in CI.") +@pytest.mark.e2e class TestSimulationModuleE2E: def test_xarm7_joint_state_published(self, lcm_spy, start_blueprint) -> None: joint_state_topic = "/xarm/joint_states#sensor_msgs.JointState" From 64f5979adbbc9001fc1bf71528363790835d8a64 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Sun, 25 Jan 2026 22:13:19 -0800 Subject: [PATCH 40/43] remove duplicated blueprint --- dimos/robot/all_blueprints.py | 1 - 1 file changed, 1 deletion(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 4f5243a5eb..e8b99a9203 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -30,7 +30,6 @@ "demo-google-maps-skill": "dimos.agents.skills.demo_google_maps_skill:demo_google_maps_skill", "demo-gps-nav": "dimos.agents.skills.demo_gps_nav:demo_gps_nav", "demo-object-scene-registration": "dimos.perception.demo_object_scene_registration:demo_object_scene_registration", - "demo-error-on-name-conflicts": "dimos.robot.unitree_webrtc.demo_error_on_name_conflicts:blueprint", "simulation-xarm7": "dimos.simulation.sim_blueprints:xarm7_trajectory_sim", "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", From 001a053e2f11d0077947e020e6bdcfcaaffe3320 Mon Sep 17 00:00:00 2001 From: Jing Cao Date: Sun, 25 Jan 2026 22:17:01 -0800 Subject: [PATCH 41/43] run all blueprints generation test --- dimos/robot/all_blueprints.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index e8b99a9203..29a103e55e 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -30,7 +30,6 @@ "demo-google-maps-skill": "dimos.agents.skills.demo_google_maps_skill:demo_google_maps_skill", "demo-gps-nav": "dimos.agents.skills.demo_gps_nav:demo_gps_nav", "demo-object-scene-registration": "dimos.perception.demo_object_scene_registration:demo_object_scene_registration", - "simulation-xarm7": "dimos.simulation.sim_blueprints:xarm7_trajectory_sim", "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", "unitree-g1": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1", @@ -54,6 +53,7 @@ "unitree-go2-spatial": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:unitree_go2_spatial", "unitree-go2-temporal-memory": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:unitree_go2_temporal_memory", "unitree-go2-vlm-stream-test": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:unitree_go2_vlm_stream_test", + "xarm7-trajectory-sim": "dimos.simulation.sim_blueprints:xarm7_trajectory_sim", } @@ -89,6 +89,7 @@ "replanning_a_star_planner": "dimos.navigation.replanning_a_star.module", "rerun_scene_wiring": "dimos.dashboard.rerun_scene_wiring", "ros_nav": "dimos.navigation.rosnav", + "simulation": "dimos.simulation.manipulators.sim_module", "spatial_memory": "dimos.perception.spatial_perception", "speak_skill": "dimos.agents.skills.speak_skill", "temporal_memory": "dimos.perception.experimental.temporal_memory.temporal_memory", @@ -99,8 +100,6 @@ "voxel_mapper": "dimos.mapping.voxels", "wavefront_frontier_explorer": "dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector", "web_input": "dimos.agents.cli.web", - "control_orchestrator": "dimos.control.orchestrator", - "simulation": "dimos.simulation.manipulators.sim_module", "websocket_vis": "dimos.web.websocket_vis.websocket_vis_module", "zed_camera": "dimos.hardware.sensors.camera.zed.camera", } From a527d072ca2faadc771802c4269b604f622696b8 Mon Sep 17 00:00:00 2001 From: spomichter <12108168+spomichter@users.noreply.github.com> Date: Thu, 12 Feb 2026 14:25:10 +0000 Subject: [PATCH 42/43] CI code cleanup --- dimos/simulation/sim_blueprints.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/dimos/simulation/sim_blueprints.py b/dimos/simulation/sim_blueprints.py index 6b5c69cad7..5733b19ef2 100644 --- a/dimos/simulation/sim_blueprints.py +++ b/dimos/simulation/sim_blueprints.py @@ -12,9 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from pathlib import Path -from dimos.core.blueprints import autoconnect from dimos.core.transport import LCMTransport from dimos.msgs.sensor_msgs import ( # type: ignore[attr-defined] JointCommand, From de58166198100c1bf9c4de01cc8afa5cad30c131 Mon Sep 17 00:00:00 2001 From: stash Date: Thu, 12 Feb 2026 07:10:29 -0800 Subject: [PATCH 43/43] fixed all blueprints --- dimos/robot/all_blueprints.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 8b30066275..c33e56116b 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -61,10 +61,10 @@ "unitree-go2-spatial": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:unitree_go2_spatial", "unitree-go2-temporal-memory": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:unitree_go2_temporal_memory", "unitree-go2-vlm-stream-test": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:unitree_go2_vlm_stream_test", - "xarm7-trajectory-sim": "dimos.simulation.sim_blueprints:xarm7_trajectory_sim", "xarm-perception": "dimos.manipulation.manipulation_blueprints:xarm_perception", "xarm6-planner-only": "dimos.manipulation.manipulation_blueprints:xarm6_planner_only", "xarm7-planner-coordinator": "dimos.manipulation.manipulation_blueprints:xarm7_planner_coordinator", + "xarm7-trajectory-sim": "dimos.simulation.sim_blueprints:xarm7_trajectory_sim", }