diff --git a/XARM_GRASP_SIM_README.md b/XARM_GRASP_SIM_README.md new file mode 100644 index 0000000000..c2b70ec3b8 --- /dev/null +++ b/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. diff --git a/data/.lfs/xarm_grasp_sim.tar.gz b/data/.lfs/xarm_grasp_sim.tar.gz new file mode 100644 index 0000000000..1ba71ecbc2 --- /dev/null +++ b/data/.lfs/xarm_grasp_sim.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7541ab8f252219be685e7c98cd79c08acc40cd783dccaa3d5f634617d596db6a +size 2295052 diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index eeed8dead0..059d5c0699 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -913,7 +913,7 @@ def set_gripper_position(self, hardware_id: str, position: float) -> bool: if isinstance(hw, ConnectedTwistBase): logger.warning(f"Hardware '{hardware_id}' is a twist base, no gripper support") return False - return hw.adapter.write_gripper_position(position) + return hw.set_gripper_position(position) @rpc def get_gripper_position(self, hardware_id: str) -> float | None: diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 3a7c74f430..aaa8392596 100644 --- a/dimos/control/hardware_interface.py +++ b/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. diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index cbd3dd9f48..569853e02d 100644 --- a/dimos/control/test_control.py +++ b/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") diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index f131c42ae3..dd845ae30e 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/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) diff --git a/dimos/manipulation/pick_and_place_module.py b/dimos/manipulation/pick_and_place_module.py index bd7b149df7..7c30012dc3 100644 --- a/dimos/manipulation/pick_and_place_module.py +++ b/dimos/manipulation/pick_and_place_module.py @@ -23,8 +23,8 @@ from __future__ import annotations from collections import Counter -from collections.abc import Sequence -from contextlib import suppress +from collections.abc import Iterator, Sequence +from contextlib import contextmanager, nullcontext, suppress from dataclasses import dataclass, field from enum import Enum import math @@ -118,6 +118,7 @@ class PickAndPlaceModuleConfig(ManipulationModuleConfig): grasp_pre_grasp_offset: FiniteFloat | None = Field(default=None, gt=0.0) grasp_retreat_offset: FiniteFloat | None = Field(default=None, gt=0.0) grasp_approach_vector: tuple[FiniteFloat, FiniteFloat, FiniteFloat] = (0.0, 0.0, -1.0) + pick_suppress_all_object_obstacles: bool = False grasp_verification: GraspVerificationConfig = Field(default_factory=GraspVerificationConfig) # Gripper geometry for grasp visualization only. Source from the same config # object as the grasp generator: grasp_gen_x bakes grasp_frame_to_tcp into @@ -125,6 +126,10 @@ class PickAndPlaceModuleConfig(ManipulationModuleConfig): # draw correct-looking grasps while the robot goes elsewhere. grasp_viz_gripper: SweepVolumeGripperConfig | None = None grasp_viz_frame_to_tcp: RigidTransform = IDENTITY_TRANSFORM + # Convex hulls of the detected clouds instead of bounding boxes. A box + # envelops the object at every height, so a gripper reaching in from the + # side collides with empty space. + use_mesh_obstacles: bool = False @model_validator(mode="after") def _validate_grasp_pipeline(self) -> PickAndPlaceModuleConfig: @@ -180,6 +185,7 @@ class _PickTransaction: selected: _FeasibleGrasp | None = None rejections: Counter[str] = field(default_factory=Counter) gripper_closed: bool = False + cleanup_error: str | None = None class _PickPipelineError(RuntimeError): @@ -228,7 +234,7 @@ def start(self) -> None: # Start obstacle monitor for perception integration if self._world_monitor is not None: - self._world_monitor.start_obstacle_monitor() + self._world_monitor.start_obstacle_monitor(self.config.use_mesh_obstacles) logger.info("PickAndPlaceModule started") @@ -687,6 +693,24 @@ def _provider_candidates( self._publish_candidate_layers(ranked[: self.config.max_grasp_candidates_to_check]) return ranked + @contextmanager + def _suppress_target(self, transaction: _PickTransaction) -> Iterator[None]: + """Hide only the pick target, so the rest of the scene stays planned against. + + A failed restore leaves the world without that obstacle, so it is + recorded on the transaction rather than dropped. + """ + if self._world_monitor is None or self.config.pick_suppress_all_object_obstacles: + yield + return + handle = None + try: + with self._world_monitor.suppress_object_obstacle(transaction.object_id) as handle: + yield + finally: + if handle is not None and handle.cleanup_error: + transaction.cleanup_error = handle.cleanup_error + def _grasp_viz(self) -> Any | None: """Visualization backend, or None when disabled or gripper geometry is unset.""" if self._world_monitor is None or self.config.grasp_viz_gripper is None: @@ -750,6 +774,14 @@ def _valid_candidate(candidate: GraspCandidate) -> bool: np.all(np.isfinite(values)) and np.isclose(np.linalg.norm(quaternion), 1.0, atol=1e-5) ) + @staticmethod + def _candidate_retraction_vector(candidate: GraspCandidate, vector: Vector3) -> np.ndarray: + local = np.asarray([vector.x, vector.y, vector.z], dtype=float) + norm = np.linalg.norm(local) + if norm == 0.0: + return np.zeros(3, dtype=float) + return candidate.pose.orientation.to_rotation_matrix() @ (local / norm) + def _select_feasible_grasp( self, candidates: list[GraspCandidate], @@ -766,22 +798,51 @@ def _select_feasible_grasp( for rank, candidate in enumerate(candidates[:limit], start=1): if not self._valid_candidate(candidate): transaction.rejections[_CandidateRejection.INVALID.value] += 1 + logger.info( + "Rejected grasp candidate rank=%d score=%.4f: invalid", + rank, + candidate.score, + ) continue pre_grasp = self._compute_pre_grasp_pose(candidate.pose, pre_offset, vector) retreat = self._compute_pre_grasp_pose(candidate.pose, retreat_offset, vector) self._publish_attempt_layer(candidate.pose, pre_grasp) + retraction = self._candidate_retraction_vector(candidate, vector) rejections = ( _CandidateRejection.PRE_GRASP_INFEASIBLE, _CandidateRejection.GRASP_INFEASIBLE, _CandidateRejection.RETREAT_INFEASIBLE, ) - failed_index, _ = self._check_connected_pose_sequence( - (pre_grasp, candidate.pose, retreat), - robot_name, - start=sequence_start, + # The pre-grasp is checked against the whole scene; only the leg + # that touches the target may ignore the target. + failed_index, mid = self._check_connected_pose_sequence( + (pre_grasp,), robot_name, start=sequence_start ) + if failed_index is None: + with self._suppress_target(transaction): + failed_index, _ = self._check_connected_pose_sequence( + (candidate.pose, retreat), robot_name, start=mid + ) + if failed_index is not None: + failed_index += 1 if failed_index is not None: transaction.rejections[rejections[failed_index].value] += 1 + logger.info( + "Rejected grasp candidate rank=%d score=%.4f retraction=(%.3f, %.3f, %.3f) " + "grasp=(%.3f, %.3f, %.3f) pre_grasp=(%.3f, %.3f, %.3f): %s", + rank, + candidate.score, + retraction[0], + retraction[1], + retraction[2], + candidate.pose.position.x, + candidate.pose.position.y, + candidate.pose.position.z, + pre_grasp.position.x, + pre_grasp.position.y, + pre_grasp.position.z, + rejections[failed_index].value, + ) continue return _FeasibleGrasp(candidate, rank, pre_grasp, retreat) @@ -885,33 +946,40 @@ def _execute_selected_pick( transaction, execution.error_code or "EXECUTION_FAILED", execution.message ) - transaction.phase = _PickPhase.GRASP - if not self.plan_to_pose(selected.candidate.pose, robot_name): - return self._phase_failure(transaction, "PLANNING_FAILED", "grasp planning failed") - execution = self._preview_execute_wait(robot_name) - if not execution.is_success(): - return self._phase_failure( - transaction, execution.error_code or "EXECUTION_FAILED", execution.message - ) + # From here the gripper must reach into, and then carry, the target, so + # the target alone is hidden. Every other obstacle stays. + with self._suppress_target(transaction): + transaction.phase = _PickPhase.GRASP + if not self.plan_to_pose(selected.candidate.pose, robot_name): + return self._phase_failure(transaction, "PLANNING_FAILED", "grasp planning failed") + execution = self._preview_execute_wait(robot_name) + if not execution.is_success(): + return self._phase_failure( + transaction, execution.error_code or "EXECUTION_FAILED", execution.message + ) - transaction.phase = _PickPhase.CLOSE - if not self._set_gripper_position(float(verification.closed_position), robot_name): - return self._phase_failure(transaction, "GRIPPER_FAILED", "close command failed") - transaction.gripper_closed = True + transaction.phase = _PickPhase.CLOSE + if not self._set_gripper_position(float(verification.closed_position), robot_name): + return self._phase_failure(transaction, "GRIPPER_FAILED", "close command failed") + transaction.gripper_closed = True - transaction.phase = _PickPhase.VERIFY - verified = self._verify_grasp(robot_name) - if not verified.held: - return self._phase_failure(transaction, "GRASP_VERIFICATION_FAILED", verified.detail) + transaction.phase = _PickPhase.VERIFY + verified = self._verify_grasp(robot_name) + if not verified.held: + return self._phase_failure( + transaction, "GRASP_VERIFICATION_FAILED", verified.detail + ) - transaction.phase = _PickPhase.RETREAT - if not self.plan_to_pose(selected.retreat_pose, robot_name): - return self._phase_failure(transaction, "PLANNING_FAILED", "retreat planning failed") - execution = self._preview_execute_wait(robot_name) - if not execution.is_success(): - return self._phase_failure( - transaction, execution.error_code or "EXECUTION_FAILED", execution.message - ) + transaction.phase = _PickPhase.RETREAT + if not self.plan_to_pose(selected.retreat_pose, robot_name): + return self._phase_failure( + transaction, "PLANNING_FAILED", "retreat planning failed" + ) + execution = self._preview_execute_wait(robot_name) + if not execution.is_success(): + return self._phase_failure( + transaction, execution.error_code or "EXECUTION_FAILED", execution.message + ) transaction.phase = _PickPhase.DONE self._last_pick_pose = selected.candidate.pose @@ -966,7 +1034,15 @@ def pick( "WORLD_MONITOR_UNAVAILABLE", "Planning world monitor is unavailable" ) - with self._world_monitor.suppress_object_obstacle(detection.object_id) as suppression: + # Staged by default: selection and the approach plan against the + # whole scene, and only the target-touching legs suppress the + # target (see _select_feasible_grasp / _execute_selected_pick). + suppression_context: Any = ( + self._world_monitor.suppress_all_object_obstacles() + if self.config.pick_suppress_all_object_obstacles + else nullcontext() + ) + with suppression_context as suppression: sequence_start = None lift_pose = self._safety_lift_pose(rname) if lift_pose is not None: @@ -988,12 +1064,15 @@ def pick( sequence_start, ) result = self._execute_selected_pick(transaction, rname) - if suppression.cleanup_error is not None: + cleanup_error = transaction.cleanup_error or ( + suppression.cleanup_error if suppression is not None else None + ) + if cleanup_error is not None: if result.is_success(): return self._phase_failure( - transaction, "WORLD_MONITOR_UNAVAILABLE", suppression.cleanup_error + transaction, "WORLD_MONITOR_UNAVAILABLE", cleanup_error ) - result.message = f"{result.message}; cleanup: {suppression.cleanup_error}" + result.message = f"{result.message}; cleanup: {cleanup_error}" return result except _PickPipelineError as exc: return self._phase_failure(transaction, exc.code, str(exc)) diff --git a/dimos/manipulation/planning/monitor/test_world_obstacle_suppression.py b/dimos/manipulation/planning/monitor/test_world_obstacle_suppression.py index 995a137931..ab306aaafe 100644 --- a/dimos/manipulation/planning/monitor/test_world_obstacle_suppression.py +++ b/dimos/manipulation/planning/monitor/test_world_obstacle_suppression.py @@ -128,6 +128,27 @@ def test_nested_suppression_removes_and_restores_once(mocker: MockerFixture) -> assert "target" in monitor._object_obstacles +def test_all_object_suppression_removes_and_restores_cached_objects( + mocker: MockerFixture, +) -> None: + monitor, parent = _monitor(mocker) + monitor.on_objects([_object("target"), _object("other")]) + monitor.refresh_obstacles() + parent.add_obstacle.reset_mock() + parent.remove_obstacle.reset_mock() + + with monitor.suppress_all_object_obstacles() as suppression: + refreshed = monitor.refresh_obstacles() + + assert suppression.removed is True + assert refreshed == [] + assert monitor._object_obstacles == {} + + assert set(monitor._object_obstacles) == {"target", "other"} + assert parent.remove_obstacle.call_count == 2 + assert parent.add_obstacle.call_count == 2 + + def test_suppression_restores_after_cancellation(mocker: MockerFixture) -> None: class Cancelled(BaseException): pass diff --git a/dimos/manipulation/planning/monitor/world_monitor.py b/dimos/manipulation/planning/monitor/world_monitor.py index dc1c616230..f09b78e6f7 100644 --- a/dimos/manipulation/planning/monitor/world_monitor.py +++ b/dimos/manipulation/planning/monitor/world_monitor.py @@ -234,7 +234,7 @@ def start_state_monitor( self._state_monitors[robot_id] = monitor logger.info(f"State monitor started for '{robot_id}'") - def start_obstacle_monitor(self) -> None: + def start_obstacle_monitor(self, use_mesh_obstacles: bool = False) -> None: """Start monitoring obstacle updates.""" with self._lock: if self._obstacle_monitor is not None: @@ -243,6 +243,7 @@ def start_obstacle_monitor(self) -> None: self._obstacle_monitor = WorldObstacleMonitor( parent=self, + use_mesh_obstacles=use_mesh_obstacles, ) self._obstacle_monitor.start() logger.info("Obstacle monitor started") @@ -325,6 +326,15 @@ def suppress_object_obstacle(self, object_id: str) -> Iterator[ObjectObstacleSup with self._obstacle_monitor.suppress_object_obstacle(object_id) as suppression: yield suppression + @contextmanager + def suppress_all_object_obstacles(self) -> Iterator[ObjectObstacleSuppression]: + """Temporarily exclude all perception objects from collision checking.""" + if self._obstacle_monitor is None: + yield ObjectObstacleSuppression(object_id="*") + return + with self._obstacle_monitor.suppress_all_object_obstacles() as suppression: + yield suppression + def clear_perception_obstacles(self) -> int: """Remove all perception obstacles. Returns count removed.""" if self._obstacle_monitor is not None: diff --git a/dimos/manipulation/planning/monitor/world_obstacle_monitor.py b/dimos/manipulation/planning/monitor/world_obstacle_monitor.py index 6f870b171c..bd405704b4 100644 --- a/dimos/manipulation/planning/monitor/world_obstacle_monitor.py +++ b/dimos/manipulation/planning/monitor/world_obstacle_monitor.py @@ -40,6 +40,7 @@ Obstacle, ) from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -591,11 +592,40 @@ def suppress_object_obstacle(self, object_id: str) -> Iterator[ObjectObstacleSup raise RuntimeError(f"failed to suppress obstacle for object '{object_id}'") del self._object_obstacles[object_id] handle.removed = True + logger.info(f"Suppressed obstacle for object '{object_id}'") try: yield handle finally: self._release_object_suppression(handle) + @contextmanager + def suppress_all_object_obstacles(self) -> Iterator[ObjectObstacleSuppression]: + """Exclude all cached object obstacles for the lifetime of the context.""" + handle = ObjectObstacleSuppression(object_id="*") + suppressed_ids: set[str] = set() + removed = 0 + with self._lock: + suppressed_ids = set(self._object_cache) | set(self._object_obstacles) + for object_id in suppressed_ids: + self._object_suppressions[object_id] += 1 + for object_id, obstacle_id in list(self._object_obstacles.items()): + if not self._parent.remove_obstacle(obstacle_id): + for suppressed_id in suppressed_ids: + depth = self._object_suppressions.get(suppressed_id, 0) + if depth <= 1: + self._object_suppressions.pop(suppressed_id, None) + else: + self._object_suppressions[suppressed_id] = depth - 1 + raise RuntimeError("failed to suppress all object obstacles") + del self._object_obstacles[object_id] + removed += 1 + handle.removed = removed > 0 + logger.info("Suppressed %d object obstacle(s)", removed) + try: + yield handle + finally: + self._release_all_object_suppressions(handle, suppressed_ids) + def _release_object_suppression(self, handle: ObjectObstacleSuppression) -> None: object_id = handle.object_id cached: Object | None = None @@ -620,10 +650,49 @@ def _release_object_suppression(self, handle: ObjectObstacleSuppression) -> None obstacle_id = self._parent.add_obstacle(obstacle) if obstacle_id: self._object_obstacles[object_id] = obstacle_id + logger.info(f"Restored obstacle for object '{object_id}'") return handle.cleanup_error = f"failed to restore obstacle for object '{object_id}'" logger.error(handle.cleanup_error) + def _release_all_object_suppressions( + self, handle: ObjectObstacleSuppression, object_ids: set[str] + ) -> None: + cached_objects: list[tuple[str, Object]] = [] + with self._lock: + for object_id in object_ids: + depth = self._object_suppressions.get(object_id, 0) + if depth > 1: + self._object_suppressions[object_id] = depth - 1 + continue + self._object_suppressions.pop(object_id, None) + entry = self._object_cache.get(object_id) + if entry is not None: + cached_objects.append((object_id, entry[0])) + + restored = 0 + errors: list[str] = [] + for object_id, cached in cached_objects: + obstacle = self._object_to_obstacle(cached) + with self._lock: + if self._object_suppressions.get(object_id, 0) > 0: + continue + if object_id in self._object_obstacles: + continue + obstacle_id = self._parent.add_obstacle(obstacle) + if obstacle_id: + self._object_obstacles[object_id] = obstacle_id + restored += 1 + else: + errors.append(object_id) + if restored: + logger.info("Restored %d object obstacle(s)", restored) + if errors: + handle.cleanup_error = "failed to restore obstacle(s) for object(s): " + ", ".join( + sorted(errors) + ) + logger.error(handle.cleanup_error) + def clear_perception_obstacles(self) -> int: """Remove all object obstacles from the planning world. @@ -711,17 +780,30 @@ def _object_to_obstacle(self, obj: object) -> Obstacle: if self._use_mesh_obstacles and obj.pointcloud is not None: try: from dimos.manipulation.planning.utils.mesh_utils import ( + _CACHE_DIR, pointcloud_to_convex_hull_obj, ) points, _ = obj.pointcloud.as_numpy() if points is not None and points.shape[0] >= 4: - mesh_path = pointcloud_to_convex_hull_obj(points) + # One stable file per object: a fresh name each refresh + # would grow the cache without bound. + mesh_path = pointcloud_to_convex_hull_obj( + points, _CACHE_DIR / "convex_hulls" / f"{name}.obj" + ) if mesh_path is not None: + # The hull is centered on the cloud mean, so it must be + # placed there: obj.pose carries the bbox center, which + # differs by ~1.5cm on tall asymmetric objects. + centroid = points.mean(axis=0) return Obstacle( name=name, obstacle_type=ObstacleType.MESH, - pose=obj.pose, + pose=PoseStamped( + position=Vector3( + float(centroid[0]), float(centroid[1]), float(centroid[2]) + ) + ), color=(0.2, 0.8, 0.2, 0.6), mesh_path=mesh_path, ) diff --git a/dimos/manipulation/planning/utils/mesh_utils.py b/dimos/manipulation/planning/utils/mesh_utils.py index 33fce6d6c8..1db03f40b4 100644 --- a/dimos/manipulation/planning/utils/mesh_utils.py +++ b/dimos/manipulation/planning/utils/mesh_utils.py @@ -36,6 +36,7 @@ import re import shutil from typing import TYPE_CHECKING +import uuid from dimos.constants import CACHE_DIR from dimos.utils.logging_config import setup_logger @@ -304,7 +305,10 @@ def pointcloud_to_convex_hull_obj( if output_path is None: hull_dir = _CACHE_DIR / "convex_hulls" hull_dir.mkdir(parents=True, exist_ok=True) - output_path = hull_dir / f"hull_{id(points):x}.obj" + # Not id(points): that is a memory address, and CPython reuses a freed + # address for the next same-sized array, so sequential callers silently + # overwrite each other's hulls. + output_path = hull_dir / f"hull_{uuid.uuid4().hex}.obj" output_path = Path(output_path) output_path.parent.mkdir(parents=True, exist_ok=True) diff --git a/dimos/manipulation/planning/world/roboplan_world.py b/dimos/manipulation/planning/world/roboplan_world.py index 4fe2cc3659..bf8df52847 100644 --- a/dimos/manipulation/planning/world/roboplan_world.py +++ b/dimos/manipulation/planning/world/roboplan_world.py @@ -83,6 +83,8 @@ _WORLD_FRAME = "dimos_world" _CARTESIAN_COLLISION_STEP_SIZE = 0.05 +_START_STATE_ATOL = 0.01 + @dataclass class _RoboPlanRobotData: @@ -613,7 +615,9 @@ def _validated_selection_start( start_by_name = dict(zip(normalized.name, normalized.position, strict=True)) current_by_name = self._current_global_positions() if any( - not np.isclose(start_by_name[name], current_by_name[name], atol=1e-6, rtol=0.0) + not np.isclose( + start_by_name[name], current_by_name[name], atol=_START_STATE_ATOL, rtol=0.0 + ) for name in selection.joint_names ): raise ValueError("Requested start state does not match current scene state") diff --git a/dimos/manipulation/test_pick_and_place_unit.py b/dimos/manipulation/test_pick_and_place_unit.py index ae61f22026..69aa15eb5e 100644 --- a/dimos/manipulation/test_pick_and_place_unit.py +++ b/dimos/manipulation/test_pick_and_place_unit.py @@ -408,14 +408,16 @@ def test_selection_skips_higher_scored_infeasible_candidate( self, module: PickAndPlaceModule, mocker: MockerFixture ) -> None: endpoint = JointState(name=["arm/joint1"], position=[0.1]) + # Each candidate is checked in two legs: pre-grasp against the whole + # scene, then grasp+retreat with the target suppressed. plan_sequence = mocker.patch.object( module, "_check_connected_pose_sequence", - side_effect=[(0, None), (None, endpoint)], + side_effect=[(0, None), (None, endpoint), (None, endpoint)], ) plan_motion = mocker.patch.object(module, "plan_to_pose") command_gripper = mocker.patch.object(module, "_set_gripper_position") - transaction = SimpleNamespace(rejections=Counter()) + transaction = SimpleNamespace(rejections=Counter(), object_id="abc12345") selected = module._select_feasible_grasp( [_candidate(0.4, 0.9), _candidate(0.5, 0.8)], @@ -426,7 +428,7 @@ def test_selection_skips_higher_scored_infeasible_candidate( assert selected.rank == 2 assert selected.candidate.score == 0.8 - assert plan_sequence.call_count == 2 + assert plan_sequence.call_count == 3 assert transaction.rejections == {"pre_grasp_infeasible": 1} plan_motion.assert_not_called() command_gripper.assert_not_called() @@ -525,6 +527,22 @@ def test_success_executes_ordered_pick_and_records_metadata( mocker.call(Pose(0.4, 0.0, 0.3), "arm"), ] + def test_pick_can_suppress_all_object_obstacles_for_diagnostics( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + self._arrange_success(module, mocker) + module.config.pick_suppress_all_object_obstacles = True + all_suppression = SimpleNamespace(cleanup_error=None) + module._world_monitor.suppress_all_object_obstacles.return_value = nullcontext( # type: ignore[union-attr] + all_suppression + ) + + result = module.pick("cup", object_id="abc12345") + + assert result.is_success() + module._world_monitor.suppress_all_object_obstacles.assert_called_once_with() # type: ignore[union-attr] + module._world_monitor.suppress_object_obstacle.assert_not_called() # type: ignore[union-attr] + def test_no_safety_lift_validates_candidates_from_current_state( self, module: PickAndPlaceModule, mocker: MockerFixture ) -> None: @@ -621,7 +639,8 @@ def test_cleanup_failure_does_not_hide_primary_failure( ) -> None: _, suppression = self._arrange_success(module, mocker) suppression.cleanup_error = "restore failed" - module.plan_to_pose.side_effect = [False] + # Approach plans; the grasp leg fails inside the target suppression. + module.plan_to_pose.side_effect = [True, False] result = module.pick("cup", object_id="abc12345") @@ -720,7 +739,8 @@ def test_full_pick_pipeline_uses_real_messages_and_fake_boundary_providers( plan_sequence = mocker.patch.object( module, "_check_connected_pose_sequence", - side_effect=[(0, None), (None, JointState())], + # Per candidate: the pre-grasp leg, then the suppressed grasp+retreat leg. + side_effect=[(0, None), (None, JointState()), (None, JointState())], ) mocker.patch.object(module, "_safety_lift_pose", return_value=None) mocker.patch.object(module, "_lift_if_low", return_value=SkillResult.ok()) @@ -742,9 +762,9 @@ def test_full_pick_pipeline_uses_real_messages_and_fake_boundary_providers( assert result.metadata["rejections"] == {"pre_grasp_infeasible": 1} scene.get_object_pointcloud_by_object_id.assert_called_once_with("abc12345") generator.propose_grasps.assert_called_once_with(scene.get_object_pointcloud_by_object_id()) - world.suppress_object_obstacle.assert_called_once_with("abc12345") - assert world.method_calls == [mocker.call.suppress_object_obstacle("abc12345")] - assert plan_sequence.call_count == 2 + # Suppressed twice: once to validate the grasp leg, once to execute it. + assert world.method_calls == [mocker.call.suppress_object_obstacle("abc12345")] * 2 + assert plan_sequence.call_count == 3 assert plan.call_count == 3 assert execute.call_count == 3 assert gripper.call_args_list == [mocker.call(0.85, "arm"), mocker.call(0.0, "arm")] diff --git a/dimos/manipulation/visualization/grasp_layers.py b/dimos/manipulation/visualization/grasp_layers.py index 4983387c3f..3a9f7a31fa 100644 --- a/dimos/manipulation/visualization/grasp_layers.py +++ b/dimos/manipulation/visualization/grasp_layers.py @@ -92,9 +92,7 @@ def _wireframe(pose: Pose, gripper: Any, grasp_frame_to_tcp: Any) -> tuple[np.nd world_to_grasp = world_to_tcp @ np.linalg.inv(grasp_to_tcp) rotation = world_to_grasp[:3, :3] translation = world_to_grasp[:3, 3] - strips = tuple( - (rotation @ strip.T).T + translation for strip in _fork_strips_local(gripper) - ) + strips = tuple((rotation @ strip.T).T + translation for strip in _fork_strips_local(gripper)) vertices = np.vstack(strips).astype(np.float32) edges = np.arange(len(vertices), dtype=np.int32).reshape((-1, 2)) return vertices, edges diff --git a/dimos/perception/sim_object_scene.py b/dimos/perception/sim_object_scene.py new file mode 100644 index 0000000000..c00585816a --- /dev/null +++ b/dimos/perception/sim_object_scene.py @@ -0,0 +1,195 @@ +# 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. + +"""Ground-truth object scene from simulator state, in place of camera perception. + +Implements the same spec and ports as ObjectSceneRegistrationModule, so the +manipulation stack cannot tell the difference, but the detections come from the +simulator's own body poses and the clouds from the objects' meshes. Sim only: +it exists to take perception out of the loop while grasping is under test. +""" + +from __future__ import annotations + +import threading +import time +from typing import TYPE_CHECKING + +import numpy as np +import open3d as o3d +from pydantic import Field + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import Out +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.perception.experimental.object import Object as DetObject +from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from numpy.typing import NDArray + +logger = setup_logger() + + +class SimObjectSceneConfig(ModuleConfig): + # MuJoCo body name -> mesh file sampled to produce that object's cloud. + objects: dict[str, str] = Field(default_factory=dict) + frame_id: str = "world" + publish_hz: float = Field(default=2.0, gt=0.0) + points_per_object: int = Field(default=2000, gt=0) + + +class SimObjectScene(Module): + """Publish simulator ground truth through the perception object-scene API.""" + + config: SimObjectSceneConfig + _sim: MujocoSimModule | None = None + + objects: Out[list[DetObject]] + + def __init__(self, **kwargs: object) -> None: + super().__init__(**kwargs) + self._samples: dict[str, NDArray[np.float64]] = {} + self._extents: dict[str, NDArray[np.float64]] = {} + self._clouds: dict[str, PointCloud2] = {} + self._lock = threading.Lock() + self._stop_event = threading.Event() + self._thread: threading.Thread | None = None + + @rpc + def start(self) -> None: + super().start() + for name, mesh_path in self.config.objects.items(): + mesh = o3d.io.read_triangle_mesh(str(mesh_path)) + if mesh.is_empty(): + logger.warning(f"SimObjectScene: empty mesh for '{name}' at {mesh_path}") + continue + o3d.utility.random.seed(42) + sampled = mesh.sample_points_uniformly(number_of_points=self.config.points_per_object) + self._samples[name] = np.asarray(sampled.points, dtype=np.float64) + bounds = mesh.get_axis_aligned_bounding_box() + self._extents[name] = np.asarray(bounds.get_extent(), dtype=np.float64) + self._stop_event.clear() + self._thread = threading.Thread(target=self._publish_loop, daemon=True) + self._thread.start() + logger.info(f"SimObjectScene started with {len(self._samples)} objects") + + @rpc + def stop(self) -> None: + self._stop_event.set() + if self._thread is not None: + self._thread.join(timeout=2.0) + self._thread = None + super().stop() + + def _publish_loop(self) -> None: + period = 1.0 / self.config.publish_hz + while not self._stop_event.is_set(): + try: + detections = self._build_detections() + if detections: + self.objects.publish(detections) + except Exception: + logger.warning("SimObjectScene publish failed", exc_info=True) + self._stop_event.wait(period) + + def _build_detections(self) -> list[DetObject]: + if self._sim is None or not self._samples: + return [] + poses = self._sim.get_body_poses(list(self._samples)) + now = time.time() + detections: list[DetObject] = [] + clouds: dict[str, PointCloud2] = {} + for name, sample in self._samples.items(): + pose = poses.get(name) + if pose is None: + continue + translation = Vector3(*pose[:3]) + rotation = Quaternion(*pose[3:]) + cloud = PointCloud2.from_numpy( + sample, frame_id=self.config.frame_id, timestamp=now + ).transform( + Transform( + translation=translation, + rotation=rotation, + frame_id=self.config.frame_id, + child_frame_id=name, + ts=now, + ) + ) + cloud.ts = now + clouds[name] = cloud + extent = self._extents[name] + detections.append( + DetObject( + name=name, + object_id=name, + center=Vector3(pose[0], pose[1], pose[2] + extent[2] / 2.0), + size=Vector3(*extent), + pose=PoseStamped(), + pointcloud=cloud, + frame_id=self.config.frame_id, + bbox=(0.0, 0.0, 1.0, 1.0), + track_id=0, + class_id=0, + confidence=1.0, + ts=now, + image=Image(), + ) + ) + with self._lock: + self._clouds = clouds + return detections + + def _stamped(self, cloud: PointCloud2 | None) -> PointCloud2 | None: + # Callers reject clouds older than a few seconds; the geometry is exact + # at any age, so re-stamp on read rather than force a faster loop. + if cloud is not None: + cloud.ts = time.time() + return cloud + + @rpc + def get_object_pointcloud_by_name(self, name: str) -> PointCloud2 | None: + with self._lock: + return self._stamped(self._clouds.get(name)) + + @rpc + def get_object_pointcloud_by_object_id(self, object_id: str) -> PointCloud2 | None: + with self._lock: + return self._stamped(self._clouds.get(object_id)) + + @rpc + def get_full_scene_pointcloud( + self, + exclude_object_id: str | None = None, + depth_trunc: float = 2.0, + voxel_size: float = 0.01, + ) -> PointCloud2 | None: + with self._lock: + clouds = [c for name, c in self._clouds.items() if name != exclude_object_id] + if not clouds: + return None + merged = clouds[0] + for cloud in clouds[1:]: + merged = merged + cloud + merged = merged.voxel_downsample(voxel_size) + merged.ts = time.time() + return merged diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 8b20bd3dcc..418479dabf 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -155,6 +155,8 @@ "xarm-graspgenx-agent": "dimos.robot.manipulators.xarm.blueprints.agentic:xarm_graspgenx_agent", "xarm-perception": "dimos.robot.manipulators.xarm.blueprints.perception:xarm_perception", "xarm-perception-agent": "dimos.robot.manipulators.xarm.blueprints.agentic:xarm_perception_agent", + "xarm-grasp-sim": "dimos.robot.manipulators.xarm.blueprints.simulation:xarm_grasp_sim", + "xarm-grasp-sim-agent": "dimos.robot.manipulators.xarm.blueprints.agentic:xarm_grasp_sim_agent", "xarm-perception-sim": "dimos.robot.manipulators.xarm.blueprints.simulation:xarm_perception_sim", "xarm-perception-sim-agent": "dimos.robot.manipulators.xarm.blueprints.agentic:xarm_perception_sim_agent", "xarm6-worldbelief": "dimos.experimental.world_belief.xarm6_blueprint:xarm6_worldbelief", diff --git a/dimos/robot/manipulators/xarm/blueprints/agentic.py b/dimos/robot/manipulators/xarm/blueprints/agentic.py index 465370a77b..701322777e 100644 --- a/dimos/robot/manipulators/xarm/blueprints/agentic.py +++ b/dimos/robot/manipulators/xarm/blueprints/agentic.py @@ -26,7 +26,10 @@ from dimos.robot.manipulators.xarm.blueprints.basic import xarm7_planner_coordinator from dimos.robot.manipulators.xarm.blueprints.graspgenx import xarm_graspgenx from dimos.robot.manipulators.xarm.blueprints.perception import xarm_perception -from dimos.robot.manipulators.xarm.blueprints.simulation import xarm_perception_sim +from dimos.robot.manipulators.xarm.blueprints.simulation import ( + xarm_grasp_sim, + xarm_perception_sim, +) xarm7_planner_coordinator_agent = autoconnect( xarm7_planner_coordinator, @@ -51,3 +54,9 @@ McpServer.blueprint(), McpClient.blueprint(system_prompt=MANIPULATION_AGENT_SYSTEM_PROMPT), ) + +xarm_grasp_sim_agent = autoconnect( + xarm_grasp_sim, + McpServer.blueprint(), + McpClient.blueprint(system_prompt=MANIPULATION_AGENT_SYSTEM_PROMPT), +) diff --git a/dimos/robot/manipulators/xarm/blueprints/simulation.py b/dimos/robot/manipulators/xarm/blueprints/simulation.py index 7e7d2fa1de..68b1057424 100644 --- a/dimos/robot/manipulators/xarm/blueprints/simulation.py +++ b/dimos/robot/manipulators/xarm/blueprints/simulation.py @@ -17,32 +17,76 @@ from __future__ import annotations from dimos.core.coordination.blueprints import autoconnect +from dimos.manipulation.grasping.grasp_gen_x import GraspGenXModule from dimos.manipulation.pick_and_place_module import PickAndPlaceModule from dimos.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule +from dimos.perception.sim_object_scene import SimObjectScene from dimos.robot.manipulators.common.blueprints import coordinator, trajectory_task from dimos.robot.manipulators.xarm.config import ( XARM7_SIM_PATH, + XARM_GRASP_SIM_PATH, make_xarm7_sim_hardware, make_xarm7_sim_module_kwargs, make_xarm7_sim_robot_config, ) +from dimos.robot.manipulators.xarm.grasp_config import make_xarm_graspgenx_config from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule +from dimos.utils.data import LfsPath from dimos.visualization.rerun.bridge import RerunBridgeModule -_xarm7_sim_hw = make_xarm7_sim_hardware(XARM7_SIM_PATH) -xarm_perception_sim = autoconnect( - PickAndPlaceModule.blueprint( - robots=[make_xarm7_sim_robot_config()], - planning_timeout=10.0, - visualization={"backend": "meshcat"}, - heuristic_grasp_fallback=True, +def _xarm7_perception_sim( + scene_path: object, + static_box_obstacles: tuple = (), + object_scene: object | None = None, + pick_and_place_kwargs: dict[str, object] | None = None, +) -> object: + hw = make_xarm7_sim_hardware(scene_path) + return autoconnect( + PickAndPlaceModule.blueprint( + robots=[make_xarm7_sim_robot_config()], + planning_timeout=10.0, + visualization={"backend": "viser"}, + heuristic_grasp_fallback=True, + static_box_obstacles=list(static_box_obstacles), + **(pick_and_place_kwargs or {}), + ), + MujocoSimModule.blueprint(**make_xarm7_sim_module_kwargs(scene_path)), + object_scene or ObjectSceneRegistrationModule.blueprint(target_frame="world"), + coordinator(hardware=[hw], tasks=[trajectory_task(hw)]), + RerunBridgeModule.blueprint(), + ) + + +xarm_perception_sim = _xarm7_perception_sim(XARM7_SIM_PATH) + +# The room-and-objects scene with learned grasps: GraspGenX proposals feed +# pick's provider path, and the table matches data/xarm_grasp_sim/scene.xml so +# the planner always respects it. +_XARM_GRASP_TABLE = {"name": "table", "center": (0.47, 0.0, 0.065), "size": (0.38, 0.60, 0.13)} + +# Ground-truth detections from sim state instead of the camera: perception is +# the weak link in this scene, and grasping is what we are testing. +_XARM_GRASPGENX = make_xarm_graspgenx_config() +_XARM_GRASP_MESH_DIR = LfsPath("xarm_grasp_sim") / "assets" / "manip" +_XARM_GRASP_OBJECTS = { + name: str(_XARM_GRASP_MESH_DIR / f"{name}.obj") + for name in ("bottle", "box", "can", "cup", "marker", "tape") +} + +xarm_grasp_sim = autoconnect( + _xarm7_perception_sim( + XARM_GRASP_SIM_PATH, + static_box_obstacles=(_XARM_GRASP_TABLE,), + object_scene=SimObjectScene.blueprint(objects=_XARM_GRASP_OBJECTS), + pick_and_place_kwargs={ + "max_grasp_candidates_to_check": 30, + "grasp_viz_gripper": _XARM_GRASPGENX.gripper, + "grasp_viz_frame_to_tcp": _XARM_GRASPGENX.grasp_frame_to_tcp, + "use_mesh_obstacles": True, + }, ), - MujocoSimModule.blueprint(**make_xarm7_sim_module_kwargs(XARM7_SIM_PATH)), - ObjectSceneRegistrationModule.blueprint(target_frame="world"), - coordinator( - hardware=[_xarm7_sim_hw], - tasks=[trajectory_task(_xarm7_sim_hw)], + GraspGenXModule.blueprint( + **_XARM_GRASPGENX.model_dump(exclude={"rpc_transport", "tf_transport", "g"}) ), - RerunBridgeModule.blueprint(), ) diff --git a/dimos/robot/manipulators/xarm/blueprints/test_graspgenx.py b/dimos/robot/manipulators/xarm/blueprints/test_graspgenx.py index 9cde5043e8..a6b9199df1 100644 --- a/dimos/robot/manipulators/xarm/blueprints/test_graspgenx.py +++ b/dimos/robot/manipulators/xarm/blueprints/test_graspgenx.py @@ -25,9 +25,14 @@ from dimos.perception.experimental.object_scene_registration import ( ObjectSceneRegistrationModule, ) -from dimos.robot.manipulators.xarm.blueprints.agentic import xarm_graspgenx_agent +from dimos.perception.sim_object_scene import SimObjectScene +from dimos.robot.manipulators.xarm.blueprints.agentic import ( + xarm_grasp_sim_agent, + xarm_graspgenx_agent, +) from dimos.robot.manipulators.xarm.blueprints.graspgenx import xarm_graspgenx from dimos.robot.manipulators.xarm.blueprints.perception import xarm_perception +from dimos.robot.manipulators.xarm.blueprints.simulation import xarm_grasp_sim from dimos.robot.manipulators.xarm.grasp_config import ( XARM_GRASP_FRAME_TO_TCP, XARM_GRIPPER_SWEEP, @@ -51,7 +56,12 @@ def test_xarm_graspgenx_geometry_is_explicit_and_import_safe() -> None: assert config.gripper.extents_open == (0.085, 0.032, 0.067) assert config.gripper.extents_half_open == (0.0425, 0.032, 0.067) assert config.gripper.fingertip_depth == 0.162 - assert config.grasp_frame_to_tcp[2][3] == 0.172 + assert config.grasp_frame_to_tcp == ( + (0.0, -1.0, 0.0, 0.0), + (1.0, 0.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.172), + (0.0, 0.0, 0.0, 1.0), + ) def test_existing_xarm_perception_keeps_explicit_heuristic_fallback() -> None: @@ -78,3 +88,26 @@ def test_xarm_graspgenx_agent_composes_one_mcp_pair() -> None: assert _module_count(xarm_graspgenx_agent, ObjectSceneRegistrationModule) == 1 assert _module_count(xarm_graspgenx_agent, GraspGenXModule) == 1 assert _module_count(xarm_graspgenx_agent, PickAndPlaceModule) == 1 + + +def test_xarm_grasp_sim_uses_gt_scene_and_pick_diagnostics() -> None: + config = PickAndPlaceModuleConfig(**_module_kwargs(xarm_grasp_sim, PickAndPlaceModule)) + + assert _module_count(xarm_grasp_sim, ObjectSceneRegistrationModule) == 0 + assert _module_count(xarm_grasp_sim, SimObjectScene) == 1 + assert _module_count(xarm_grasp_sim, GraspGenXModule) == 1 + assert _module_count(xarm_grasp_sim, PickAndPlaceModule) == 1 + assert config.max_grasp_candidates_to_check == 30 + assert config.pick_suppress_all_object_obstacles is False + assert config.grasp_viz_gripper == XARM_GRIPPER_SWEEP + assert config.grasp_viz_frame_to_tcp == XARM_GRASP_FRAME_TO_TCP + assert config.use_mesh_obstacles is True + + +def test_xarm_grasp_sim_agent_keeps_gt_scene_provider() -> None: + assert _module_count(xarm_grasp_sim_agent, McpServer) == 1 + assert _module_count(xarm_grasp_sim_agent, McpClient) == 1 + assert _module_count(xarm_grasp_sim_agent, ObjectSceneRegistrationModule) == 0 + assert _module_count(xarm_grasp_sim_agent, SimObjectScene) == 1 + assert _module_count(xarm_grasp_sim_agent, GraspGenXModule) == 1 + assert _module_count(xarm_grasp_sim_agent, PickAndPlaceModule) == 1 diff --git a/dimos/robot/manipulators/xarm/config.py b/dimos/robot/manipulators/xarm/config.py index 0906610545..d1fe6280b8 100644 --- a/dimos/robot/manipulators/xarm/config.py +++ b/dimos/robot/manipulators/xarm/config.py @@ -58,6 +58,10 @@ XARM_PACKAGE_PATHS: dict[str, Path] = {"xarm_description": LfsPath("xarm_description")} XARM6_SIM_PATH = LfsPath("xarm6/scene.xml") XARM7_SIM_PATH = LfsPath("xarm7/scene.xml") +# Self-contained fork of the xarm7 model with an enclosed room (no skybox) and +# six real tabletop manipulables, for the perception/grasp demo. Kept separate +# from XARM7_SIM_PATH so teleop/basic blueprints keep the bare stock table. +XARM_GRASP_SIM_PATH = LfsPath("xarm_grasp_sim/scene.xml") XARM_GRIPPER_PARAMS = { "gripper_joint": make_gripper_joints("arm")[0], "gripper_open_pos": 0.85, @@ -72,7 +76,7 @@ def make_xarm7_sim_robot_config() -> RobotModelConfig: add_gripper=True, tf_extra_links=["link7"], home_joints=XARM7_SIM_HOME, - pre_grasp_offset=0.05, + pre_grasp_offset=0.10, ) diff --git a/dimos/robot/manipulators/xarm/grasp_config.py b/dimos/robot/manipulators/xarm/grasp_config.py index 16b572641d..0612e5c210 100644 --- a/dimos/robot/manipulators/xarm/grasp_config.py +++ b/dimos/robot/manipulators/xarm/grasp_config.py @@ -21,14 +21,18 @@ SweepVolumeGripperConfig, ) -# Geometry was derived from UFACTORY's xarm_ros gripper URDF and collision -# meshes at commit 0b5118eb6bf664fc3891c14b203e6ecbd5095dca: -# - link_tcp is 0.172 m along +Z from xarm_gripper_base_link -# - the inner finger volume is approximately 0.085 x 0.032 x 0.067 m -# The model's grasp frame is the gripper base; DimOS plans for link_tcp. +# Frame and geometry were measured from data/xarm_grasp_sim/xarm7.xml: +# - link_tcp is 0.172 m along +Z from xarm_gripper_base_link; +# - the finger-pad tips end at approximately +0.162 m, which is the +# GraspGenX fingertip depth; +# - GraspGenX closes along local X, whereas the xArm closes along local Y. +# +# The transform maps the GraspGenX gripper-base frame to xArm link_tcp. Its +# +90 degree Z rotation maps GraspGenX +X onto xArm +Y while preserving the +# common +Z approach direction. XARM_GRASP_FRAME_TO_TCP = ( + (0.0, -1.0, 0.0, 0.0), (1.0, 0.0, 0.0, 0.0), - (0.0, 1.0, 0.0, 0.0), (0.0, 0.0, 1.0, 0.172), (0.0, 0.0, 0.0, 1.0), ) diff --git a/dimos/robot/manipulators/xarm/test_config.py b/dimos/robot/manipulators/xarm/test_config.py new file mode 100644 index 0000000000..066ccda99c --- /dev/null +++ b/dimos/robot/manipulators/xarm/test_config.py @@ -0,0 +1,21 @@ +# 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.robot.manipulators.xarm.config import make_xarm7_sim_robot_config + + +def test_xarm7_sim_pre_grasp_offset_keeps_gripper_clear_of_target() -> None: + config = make_xarm7_sim_robot_config() + + assert config.pre_grasp_offset == 0.10 diff --git a/dimos/simulation/engines/mujoco_engine.py b/dimos/simulation/engines/mujoco_engine.py index feb65fe79e..a7417ab5f1 100644 --- a/dimos/simulation/engines/mujoco_engine.py +++ b/dimos/simulation/engines/mujoco_engine.py @@ -862,6 +862,18 @@ def get_root_pose_unlocked(self) -> tuple[NDArray[np.float64], NDArray[np.float6 qw, qx, qy, qz = self._data.qpos[qpos_adr + 3 : qpos_adr + 7].copy() return position, np.array([qx, qy, qz, qw], dtype=np.float64) + def get_body_pose( + self, body_name: str + ) -> tuple[NDArray[np.float64], NDArray[np.float64]] | None: + """World pose of any body, as (position, xyzw quaternion).""" + with self._lock: + body_id = mujoco.mj_name2id(self._model, mujoco.mjtObj.mjOBJ_BODY, body_name) + if body_id < 0: + return None + position = self._data.xpos[body_id].copy() + qw, qx, qy, qz = self._data.xquat[body_id].copy() + return position, np.array([qx, qy, qz, qw], dtype=np.float64) + def get_actuator_ctrl_range(self, joint_index: int) -> tuple[float, float] | None: mapping = self._joint_mappings[joint_index] if mapping.actuator_id is None: diff --git a/dimos/simulation/engines/mujoco_sim_module.py b/dimos/simulation/engines/mujoco_sim_module.py index 41151f722d..b53aca9834 100644 --- a/dimos/simulation/engines/mujoco_sim_module.py +++ b/dimos/simulation/engines/mujoco_sim_module.py @@ -214,7 +214,9 @@ def post_step(self, engine: MujocoEngine) -> None: if self._gripper_idx is not None: positions = engine.joint_positions if self._gripper_idx < len(positions): - shm.write_gripper_state(positions[self._gripper_idx]) + shm.write_gripper_state( + self._gripper_joint_to_position(positions[self._gripper_idx]) + ) def clear_latched_commands(self) -> None: self._latest_pd_pos_target = None @@ -231,6 +233,12 @@ def _gripper_joint_to_ctrl(self, joint_position: float) -> float: t = (clamped - jlo) / (jhi - jlo) return chi - t * (chi - clo) + def _gripper_joint_to_position(self, joint_position: float) -> float: + """Convert the internal closing-joint angle to the public aperture.""" + jlo, jhi = self._gripper_joint_range + clamped = max(jlo, min(jhi, joint_position)) + return jlo + jhi - clamped + class MujocoSimModuleConfig(ModuleConfig, DepthCameraConfig): """Configuration for the unified MuJoCo simulation module. @@ -698,6 +706,20 @@ def reset(self) -> bool: logger.info("MujocoSimModule: reset requested", applied=applied) return applied + @rpc + def get_body_poses(self, names: list[str]) -> dict[str, list[float]]: + """World poses [x, y, z, qx, qy, qz, qw] for named bodies; unknown names omitted.""" + engine = self._engine + if engine is None: + return {} + poses: dict[str, list[float]] = {} + for name in names: + pose = engine.get_body_pose(name) + if pose is not None: + position, orientation = pose + poses[name] = [*position.tolist(), *orientation.tolist()] + return poses + @rpc def respawn_at( self, diff --git a/dimos/simulation/engines/test_mujoco_sim_module.py b/dimos/simulation/engines/test_mujoco_sim_module.py index f192b3b8dc..e40edb7123 100644 --- a/dimos/simulation/engines/test_mujoco_sim_module.py +++ b/dimos/simulation/engines/test_mujoco_sim_module.py @@ -25,7 +25,11 @@ import pytest from dimos.simulation.engines.mujoco_engine import CameraFrame, MujocoEngine -from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule, MujocoSimModuleConfig +from dimos.simulation.engines.mujoco_sim_module import ( + MujocoSimModule, + MujocoSimModuleConfig, + _WholeBodySimHooks, +) class _FakeData: @@ -81,6 +85,30 @@ def clear_latched_commands(self) -> None: self.cleared = True +@pytest.mark.parametrize( + ("driver_joint_position", "expected_aperture"), + [(0.0, 0.85), (0.425, 0.425), (0.85, 0.0)], +) +def test_gripper_feedback_reports_aperture_not_closing_joint_angle( + driver_joint_position: float, expected_aperture: float +) -> None: + shm = MagicMock() + hooks = _WholeBodySimHooks( + shm, + 7, + gripper_idx=7, + gripper_joint_range=(0.0, 0.85), + ) + engine = MagicMock() + engine.joint_positions = [0.0] * 7 + [driver_joint_position] + engine.joint_velocities = [0.0] * 8 + engine.joint_efforts = [0.0] * 8 + + hooks.post_step(engine) + + shm.write_gripper_state.assert_called_once_with(pytest.approx(expected_aperture)) + + def test_ready_signal_happens_after_joint_state_and_imu_write() -> None: events: list[str] = [] module = MujocoSimModule() @@ -168,6 +196,28 @@ def disconnect(self) -> None: module.stop() +def test_get_body_poses_omits_unknown_bodies() -> None: + module = MujocoSimModule() + + class _FakeEngine: + def get_body_pose(self, body_name: str) -> tuple[np.ndarray, np.ndarray] | None: + if body_name == "known": + return np.array([1.0, 2.0, 3.0]), np.array([0.0, 0.0, 0.0, 1.0]) + return None + + def disconnect(self) -> None: + pass + + try: + module._engine = _FakeEngine() + + assert module.get_body_poses(["known", "missing"]) == { + "known": [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0] + } + finally: + module.stop() + + def test_reset_requests_engine_reset_and_clears_latched_commands() -> None: engine = _FakeRespawnEngine() hooks = _FakeSimHooks()