feat(sim): add xArm GraspGenX simulation

XARM_GRASP_SIM_README.md

@@ -0,0 +1,16 @@
+# xArm Grasp Simulation
+
+This branch tests learned xArm grasps in MuJoCo with ground-truth object poses.
+
+1. Clone the branch, initialize LFS assets, and install the needed extras:
+   `git lfs pull && uv sync --extra agents --extra manipulation --extra sim --extra graspgenx --inexact`
+2. Use a Linux host with a working MuJoCo renderer; for headless NVIDIA hosts, configure EGL for that host.
+3. Clear stale DimOS shared-memory segments before a new run: `rm -f /dev/shm/dmjm_*`.
+4. Start the simulation: `uv run dimos run xarm-grasp-sim-agent`.
+5. In a second terminal, run `uv run dimos humancli`, then ask it to scan and pick an object, for example `Pick up the can.`
+6. Watch `uv run dimos log -f` and the Viser UI for the pick phases and candidate-rejection details.
+
+The test scene uses simulator ground truth instead of camera perception.  During a pick,
+all object collision obstacles are temporarily suppressed to isolate grasp execution; the
+table remains a collision obstacle.  A pregrasp success followed by a grasp failure points
+at approach, contact, gripper closure, or verification rather than scene-perception failure.

dimos/control/coordinator.py

@@ -913,7 +913,7 @@ def set_gripper_position(self, hardware_id: str, position: float) -> bool:
             if isinstance(hw, ConnectedTwistBase):
                 logger.warning(f"Hardware '{hardware_id}' is a twist base, no gripper support")
                 return False
-            return hw.adapter.write_gripper_position(position)
+            return hw.set_gripper_position(position)
 
     @rpc
     def get_gripper_position(self, hardware_id: str) -> float | None:

dimos/control/hardware_interface.py

@@ -136,6 +136,19 @@ def read_state(self) -> dict[JointName, JointState]:
 
         return result
 
+    def set_gripper_position(self, position: float) -> bool:
+        """Write a raw gripper position and retain it for partial arm commands."""
+        if not self._gripper_joints:
+            return False
+        if not self._initialized:
+            self._initialize_last_commanded()
+        if not self._adapter.write_gripper_position(position):
+            return False
+        normalized = self._physical_to_normalized(position)
+        for joint_name in self._gripper_joints:
+            self._last_commanded[joint_name] = normalized
+        return True
+
     def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool:
         """Write commands - allows partial joint sets, holds last for missing.
 

dimos/control/test_control.py

@@ -69,6 +69,7 @@ def mock_adapter():
     adapter.read_joint_efforts.return_value = [0.0] * 6
     adapter.write_joint_positions.return_value = True
     adapter.write_joint_velocities.return_value = True
+    adapter.write_gripper_position.return_value = True
     adapter.set_control_mode.return_value = True
     return adapter
 
@@ -186,6 +187,24 @@ def test_get_position(self):
 
 
 class TestConnectedHardware:
+    def test_direct_gripper_command_updates_hold_last_value(self, mock_adapter):
+        mock_adapter.read_gripper_position.return_value = 0.0
+        component = HardwareComponent(
+            hardware_id="arm",
+            hardware_type=HardwareType.MANIPULATOR,
+            joints=make_joints("arm", 6),
+            gripper_joints=["arm/gripper"],
+        )
+        hardware = ConnectedHardware(mock_adapter, component)
+
+        assert hardware.set_gripper_position(0.85) is True
+        hardware.write_command({"arm/joint1": 0.1}, ControlMode.POSITION)
+
+        assert mock_adapter.write_gripper_position.call_args_list == [
+            ((0.85,), {}),
+            ((0.85,), {}),
+        ]
+
     def test_normalized_gripper_commands_are_mapped_at_hardware_boundary(self, mock_adapter):
         mock_adapter.read_gripper_position.return_value = 0.035
         component = HardwareComponent(
@@ -253,6 +272,24 @@ def make(**kwargs: Any) -> ControlCoordinator:
 
 
 class TestControlCoordinatorLifecycle:
+    def test_gripper_rpc_updates_hardware_hold_last_value(
+        self, make_coordinator, mock_adapter, mocker
+    ):
+        component = HardwareComponent(
+            hardware_id="arm",
+            hardware_type=HardwareType.MANIPULATOR,
+            joints=make_joints("arm", 6),
+            gripper_joints=["arm/gripper"],
+        )
+        hardware = ConnectedHardware(mock_adapter, component)
+        set_gripper_position = mocker.spy(hardware, "set_gripper_position")
+        coordinator = make_coordinator()
+        coordinator._hardware = {"arm": hardware}
+
+        assert coordinator.set_gripper_position("arm", 0.85) is True
+
+        set_gripper_position.assert_called_once_with(0.85)
+
     def test_dispatch_routes_ee_twist_only_to_matching_frame_id(self, make_coordinator):
         coordinator = make_coordinator()
         matching_task = RecordingTask("eef")

dimos/manipulation/manipulation_module.py

@@ -32,7 +32,7 @@
 import time
 from typing import Any, Literal, TypeAlias
 
-from pydantic import Field
+from pydantic import BaseModel, Field
 
 from dimos.agents.annotation import skill
 from dimos.agents.skill_result import SkillResult
@@ -146,6 +146,12 @@ class ConnectedPoseSequenceResult:
     paths: tuple[tuple[JointState, ...], ...]
 
 
+class StaticBoxObstacle(BaseModel):
+    name: str
+    center: tuple[float, float, float]
+    size: tuple[float, float, float]
+
+
 class ManipulationModuleConfig(ModuleConfig):
     """Configuration for ManipulationModule."""
 
@@ -163,6 +169,10 @@ class ManipulationModuleConfig(ModuleConfig):
     # to prevent the planner from routing trajectories below this height.
     # Set to None to disable.
     floor_z: float | None = None
+    # Static box obstacles added at startup, for scene furniture the planner
+    # must always respect (e.g. a table). Center and size are world-frame,
+    # axis-aligned, full extents.
+    static_box_obstacles: list[StaticBoxObstacle] = Field(default_factory=list)
 
 
 class ManipulationModule(Module):
@@ -286,6 +296,17 @@ def _initialize_planning(self) -> None:
             self._world_monitor.add_obstacle(floor_obs)
             logger.info(f"Floor obstacle added at z={fz:.3f}")
 
+        for box in self.config.static_box_obstacles:
+            self._world_monitor.add_obstacle(
+                Obstacle(
+                    name=box.name,
+                    pose=Pose(Vector3(*box.center), Quaternion(0.0, 0.0, 0.0, 1.0)),
+                    obstacle_type=ObstacleType.BOX,
+                    dimensions=tuple(box.size),
+                )
+            )
+            logger.info(f"Static obstacle '{box.name}' added at {box.center}")
+
         for _, (robot_id, _, _) in self._robots.items():
             self._world_monitor.start_state_monitor(robot_id)