feat: add MujocoCamera module for sim-based perception

dimos/hardware/manipulators/sim/adapter.py

@@ -23,7 +23,7 @@
 from pathlib import Path
 from typing import TYPE_CHECKING, Any
 
-from dimos.simulation.engines.mujoco_engine import MujocoEngine
+from dimos.simulation.engines.mujoco_engine import MujocoEngine, get_or_create_engine
 from dimos.simulation.manipulators.sim_manip_interface import SimManipInterface
 
 if TYPE_CHECKING:
@@ -41,11 +41,13 @@ def __init__(
         dof: int = 7,
         address: str | None = None,
         headless: bool = True,
+        engine: MujocoEngine | None = None,
         **_: Any,
     ) -> None:
-        if address is None:
-            raise ValueError("address (MJCF XML path) is required for sim_mujoco adapter")
-        engine = MujocoEngine(config_path=Path(address), headless=headless)
+        if engine is None:
+            if address is None:
+                raise ValueError("address (MJCF XML path) is required for sim_mujoco adapter")
+            engine = get_or_create_engine(config_path=Path(address), headless=headless)
 
         # Detect gripper from engine joints
         gripper_idx = None

dimos/manipulation/blueprints.py

@@ -184,6 +184,8 @@ def _make_xarm7_config(
     add_gripper: bool = False,
     gripper_hardware_id: str | None = None,
     tf_extra_links: list[str] | None = None,
+    pre_grasp_offset: float = 0.10,
+    home_joints: list[float] | None = None,
 ) -> RobotModelConfig:
     """Create XArm7 robot config.
 
@@ -227,8 +229,8 @@ def _make_xarm7_config(
         coordinator_task_name=coordinator_task,
         gripper_hardware_id=gripper_hardware_id,
         tf_extra_links=tf_extra_links or [],
-        # Home configuration: arm extended forward, elbow up (safe observe pose)
-        home_joints=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        pre_grasp_offset=pre_grasp_offset,
+        home_joints=home_joints or [0.0] * 7,
     )
 
 
@@ -492,6 +494,68 @@ def _make_piper_config(
 )
 
 
+# Sim perception: MuJoCo camera replaces RealSense for sim-based pick-and-place
+# Both the sim adapter and camera resolve the same MujocoEngine via the registry
+# (keyed by MJCF path), so they share physics state.
+# The engine is created lazily when the adapter connects / camera starts.
+
+from dimos.control.blueprints._hardware import XARM7_SIM_PATH, sim_xarm7
+from dimos.control.coordinator import TaskConfig as TaskConfig
+from dimos.simulation.sensors.mujoco_camera import MujocoCamera
+from dimos.visualization.rerun.bridge import RerunBridgeModule, _resolve_viewer_mode
+
+xarm_perception_sim = autoconnect(
+    PickAndPlaceModule.blueprint(
+        robots=[
+            _make_xarm7_config(
+                "arm",
+                joint_prefix="arm_",
+                coordinator_task="traj_arm",
+                add_gripper=True,
+                gripper_hardware_id="arm",
+                tf_extra_links=["link7"],
+                pre_grasp_offset=0.05,
+                home_joints=[0.0, 0.0, 0.0, 0.0, 0.0, -0.7, 0.0],
+            ),
+        ],
+        planning_timeout=10.0,
+        enable_viz=True,
+    ),
+    MujocoCamera.blueprint(
+        address=str(XARM7_SIM_PATH),
+        camera_name="wrist_camera",
+        base_frame_id="link7",
+    ),
+    ObjectSceneRegistrationModule.blueprint(target_frame="world"),
+    ControlCoordinator.blueprint(
+        tick_rate=100.0,
+        publish_joint_state=True,
+        joint_state_frame_id="coordinator",
+        hardware=[sim_xarm7("arm", headless=False, gripper=True)],
+        tasks=[
+            TaskConfig(
+                name="traj_arm",
+                type="trajectory",
+                joint_names=[f"arm_joint{i + 1}" for i in range(7)],
+                priority=10,
+            ),
+        ],
+    ),
+    RerunBridgeModule.blueprint(viewer_mode=_resolve_viewer_mode()),
+).transports(
+    {
+        ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState),
+    }
+)
+
+
+xarm_perception_sim_agent = autoconnect(
+    xarm_perception_sim,
+    McpServer.blueprint(),
+    McpClient.blueprint(system_prompt=_MANIPULATION_AGENT_SYSTEM_PROMPT),
+)
+
+
 __all__ = [
     "PIPER_GRIPPER_COLLISION_EXCLUSIONS",
     "XARM_GRIPPER_COLLISION_EXCLUSIONS",
@@ -501,4 +565,6 @@ def _make_piper_config(
     "xarm7_planner_coordinator_agent",
     "xarm_perception",
     "xarm_perception_agent",
+    "xarm_perception_sim",
+    "xarm_perception_sim_agent",
 ]

dimos/manipulation/manipulation_module.py

@@ -471,6 +471,14 @@ def _plan_path_only(
         if start is None:
             return self._fail("No joint state")
 
+        # Trim goal to planner DOF (e.g. strip gripper joint from coordinator state)
+        planner_dof = len(start.position)
+        if len(goal.position) > planner_dof:
+            goal = JointState(
+                name=list(goal.name[:planner_dof]) if goal.name else [],
+                position=list(goal.position[:planner_dof]),
+            )
+
         result = self._planner.plan_joint_path(
             world=self._world_monitor.world,
             robot_id=robot_id,

dimos/manipulation/planning/spec/config.py

@@ -16,7 +16,6 @@
 
 from __future__ import annotations
 
-from collections.abc import Sequence
 from pathlib import Path
 
 from pydantic import Field
@@ -74,7 +73,7 @@ class RobotModelConfig(ModuleConfig):
     coordinator_task_name: str | None = None
     gripper_hardware_id: str | None = None
     # TF publishing for extra links (e.g., camera mount)
-    tf_extra_links: Sequence[str] = ()
+    tf_extra_links: list[str] = Field(default_factory=list)
     # Home/observe joint configuration for go_home skill
     home_joints: list[float] | None = None
     # Pre-grasp offset distance in meters (along approach direction)

dimos/msgs/sensor_msgs/CameraInfo.py

@@ -90,6 +90,38 @@ def __init__(
         self.roi_width = 0
         self.roi_do_rectify = False
 
+    @classmethod
+    def from_intrinsics(
+        cls,
+        fx: float,
+        fy: float,
+        cx: float,
+        cy: float,
+        width: int,
+        height: int,
+        frame_id: str = "",
+    ) -> CameraInfo:
+        """Create CameraInfo from pinhole intrinsics (no distortion).
+
+        Args:
+            fx: Focal length x (pixels)
+            fy: Focal length y (pixels)
+            cx: Principal point x (pixels)
+            cy: Principal point y (pixels)
+            width: Image width
+            height: Image height
+            frame_id: Frame ID
+        """
+        return cls(
+            height=height,
+            width=width,
+            distortion_model="plumb_bob",
+            D=[0.0, 0.0, 0.0, 0.0, 0.0],
+            K=[fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0],
+            P=[fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0],
+            frame_id=frame_id,
+        )
+
     def with_ts(self, ts: float) -> CameraInfo:
         """Return a copy of this CameraInfo with the given timestamp.
 

dimos/msgs/sensor_msgs/PointCloud2.py

@@ -100,9 +100,10 @@ def __getstate__(self) -> dict[str, object]:
         # Remove non-picklable objects
         del state["_pcd_tensor"]
         state["_pcd_legacy_cache"] = None
-        # Remove cached_property entries that hold unpicklable Open3D types
-        state.pop("oriented_bounding_box", None)
-        state.pop("axis_aligned_bounding_box", None)
+        # Remove all cached_property entries
+        for key in list(state):
+            if isinstance(getattr(type(self), key, None), functools.cached_property):
+                del state[key]
         return state
 
     def __setstate__(self, state: dict[str, object]) -> None:

dimos/robot/all_blueprints.py

@@ -91,6 +91,8 @@
     "unity-sim": "dimos.simulation.unity.blueprint:unity_sim",
     "xarm-perception": "dimos.manipulation.blueprints:xarm_perception",
     "xarm-perception-agent": "dimos.manipulation.blueprints:xarm_perception_agent",
+    "xarm-perception-sim": "dimos.manipulation.blueprints:xarm_perception_sim",
+    "xarm-perception-sim-agent": "dimos.manipulation.blueprints:xarm_perception_sim_agent",
     "xarm6-planner-only": "dimos.manipulation.blueprints:xarm6_planner_only",
     "xarm7-planner-coordinator": "dimos.manipulation.blueprints:xarm7_planner_coordinator",
     "xarm7-planner-coordinator-agent": "dimos.manipulation.blueprints:xarm7_planner_coordinator_agent",
@@ -137,6 +139,7 @@
     "mock-b1-connection-module": "dimos.robot.unitree.b1.connection.MockB1ConnectionModule",
     "module-a": "dimos.robot.unitree.demo_error_on_name_conflicts.ModuleA",
     "module-b": "dimos.robot.unitree.demo_error_on_name_conflicts.ModuleB",
+    "mujoco-camera": "dimos.simulation.sensors.mujoco_camera.MujocoCamera",
     "navigation-module": "dimos.robot.unitree.rosnav.NavigationModule",
     "navigation-skill-container": "dimos.agents.skills.navigation.NavigationSkillContainer",
     "object-db-module": "dimos.perception.detection.moduleDB.ObjectDBModule",

dimos/simulation/conftest.py

@@ -0,0 +1,131 @@
+# Copyright 2025-2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Shared test fixtures for simulation tests."""
+
+from __future__ import annotations
+
+from pathlib import Path
+from unittest.mock import MagicMock, patch
+
+import numpy as np
+import pytest
+
+from dimos.simulation.engines.mujoco_engine import _clear_registry
+from dimos.simulation.utils.xml_parser import JointMapping
+
+ARM_DOF = 7
+
+
+def _make_joint_mapping(name: str, idx: int) -> JointMapping:
+    """Create a JointMapping for a simple revolute joint."""
+    return JointMapping(
+        name=name,
+        joint_id=idx,
+        actuator_id=idx,
+        qpos_adr=idx,
+        dof_adr=idx,
+        tendon_qpos_adrs=(),
+        tendon_dof_adrs=(),
+    )
+
+
+def _make_gripper_mapping(name: str, idx: int) -> JointMapping:
+    """Create a JointMapping for a tendon-driven gripper."""
+    return JointMapping(
+        name=name,
+        joint_id=None,
+        actuator_id=idx,
+        qpos_adr=None,
+        dof_adr=None,
+        tendon_qpos_adrs=(idx, idx + 1),
+        tendon_dof_adrs=(idx, idx + 1),
+    )
+
+
+def _patch_mujoco_engine(n_joints: int = ARM_DOF):
+    """Patch only the MuJoCo C-library and filesystem boundaries.
+
+    Mocks ``_resolve_xml_path``, ``MjModel.from_xml_path``, ``MjData``, and
+    ``build_joint_mappings`` — the rest of ``MujocoEngine.__init__`` runs as-is.
+    """
+    mappings = [_make_joint_mapping(f"joint{i}", i) for i in range(ARM_DOF)]
+    if n_joints > ARM_DOF:
+        mappings.append(_make_gripper_mapping(f"joint{ARM_DOF}", ARM_DOF))
+
+    fake_model = MagicMock()
+    fake_model.opt.timestep = 0.002
+    fake_model.nu = n_joints
+    fake_model.nq = n_joints
+    fake_model.njnt = n_joints
+    fake_model.actuator_ctrlrange = np.array(
+        [[-6.28, 6.28]] * ARM_DOF + ([[0.0, 255.0]] if n_joints > ARM_DOF else [])
+    )
+    fake_model.jnt_range = np.array(
+        [[-6.28, 6.28]] * ARM_DOF + ([[0.0, 0.85]] if n_joints > ARM_DOF else [])
+    )
+    fake_model.jnt_qposadr = np.arange(n_joints)
+
+    fake_data = MagicMock()
+    fake_data.qpos = np.zeros(n_joints + 4)  # extra for tendon qpos addresses
+    fake_data.actuator_length = np.zeros(n_joints)
+
+    patches = [
+        patch(
+            "dimos.simulation.engines.mujoco_engine.MujocoEngine._resolve_xml_path",
+            return_value=Path("/fake/scene.xml"),
+        ),
+        patch(
+            "dimos.simulation.engines.mujoco_engine.mujoco.MjModel.from_xml_path",
+            return_value=fake_model,
+        ),
+        patch("dimos.simulation.engines.mujoco_engine.mujoco.MjData", return_value=fake_data),
+        patch("dimos.simulation.engines.mujoco_engine.build_joint_mappings", return_value=mappings),
+    ]
+    return patches
+
+
+@pytest.fixture(autouse=True)
+def clean_engine_registry():
+    """Clear the engine registry before and after each simulation test."""
+    _clear_registry()
+    yield
+    _clear_registry()
+
+
+@pytest.fixture
+def patched_mujoco_engine():
+    """Start MuJoCo engine patches for the test, stop on teardown.
+
+    Usage:
+        def test_something(patched_mujoco_engine):
+            engine = get_or_create_engine(config_path=Path("/fake/scene.xml"))
+    """
+    patches = _patch_mujoco_engine(ARM_DOF)
+    for p in patches:
+        p.start()
+    yield
+    for p in patches:
+        p.stop()
+
+
+@pytest.fixture
+def patched_mujoco_engine_with_gripper():
+    """Same as patched_mujoco_engine but with ARM_DOF+1 joints (gripper)."""
+    patches = _patch_mujoco_engine(ARM_DOF + 1)
+    for p in patches:
+        p.start()
+    yield
+    for p in patches:
+        p.stop()