feat(manip): GraspGenX provider on the grasping stack

dimos/manipulation/pick_and_place_module.py

@@ -46,6 +46,9 @@
 class PickAndPlaceModuleConfig(ModuleConfig):
     planning_frame: str = "base_link"
     pregrasp_offset: float = Field(default=0.10, gt=0.0)
+    # A learned provider returns a ranked spread whose best-scoring pose is not
+    # always kinematically reachable; a single-candidate provider is unaffected.
+    max_grasp_attempts: int = Field(default=5, gt=0)
     yaw_policy: Literal["generated", "preserve_current"] = "generated"
     grasp_verification: GraspVerificationConfig = Field(default_factory=GraspVerificationConfig)
 
@@ -141,37 +144,46 @@ def pick_object(
             return SkillResult.fail(
                 "ROBOT_NOT_FOUND", "Gripper-capable planning group is missing or ambiguous"
             )
-        candidate = candidates.candidates[0]
-        grasp = self._apply_yaw_policy(
-            PoseStamped(
-                ts=candidates.header.timestamp,
-                frame_id=candidates.header.frame_id,
-                position=candidate.pose.position,
-                orientation=candidate.pose.orientation,
-            ),
-            group,
-        )
-        pregrasp = self._offset_pose(grasp, self.config.pregrasp_offset)
         if failure := self._open_gripper(group, "pre-grasp open"):
             return failure
-        if failure := self._move(pregrasp, group):
-            return failure
-        if failure := self._move(grasp, group):
-            return failure
-        if failure := self._close_and_verify(group):
-            return failure
 
-        self._selected_object_id = object_id
-        self._selected_grasp = grasp
-        self._holding_object = True
-        if failure := self._move(pregrasp, group):
-            return failure
-        return SkillResult.ok(
-            "Pick complete",
-            object_id=object_id,
-            rank=0,
-            score=candidate.score,
-            candidates=len(candidates.candidates),
+        unreachable: SkillResult[ManipulationSkillError] | None = None
+        for rank, candidate in enumerate(candidates.candidates[: self.config.max_grasp_attempts]):
+            grasp = self._apply_yaw_policy(
+                PoseStamped(
+                    ts=candidates.header.timestamp,
+                    frame_id=candidates.header.frame_id,
+                    position=candidate.pose.position,
+                    orientation=candidate.pose.orientation,
+                ),
+                group,
+            )
+            pregrasp = self._offset_pose(grasp, self.config.pregrasp_offset)
+            failure = self._move(pregrasp, group) or self._move(grasp, group)
+            if failure is not None:
+                # Only an unreachable pose is worth demoting to the next candidate;
+                # a drive or execution fault would repeat for every one of them.
+                if failure.error_code != "PLANNING_FAILED":
+                    return failure
+                unreachable = failure
+                continue
+            if failure := self._close_and_verify(group):
+                return failure
+
+            self._selected_object_id = object_id
+            self._selected_grasp = grasp
+            self._holding_object = True
+            if failure := self._move(pregrasp, group):
+                return failure
+            return SkillResult.ok(
+                "Pick complete",
+                object_id=object_id,
+                rank=rank,
+                score=candidate.score,
+                candidates=len(candidates.candidates),
+            )
+        return unreachable or SkillResult.fail(
+            "PLANNING_FAILED", "No grasp candidate was reachable"
         )
 
     @rpc

dimos/manipulation/test_pick_and_place_unit.py

@@ -128,6 +128,61 @@ def test_pick_object_rejects_non_planning_frame(module: PickAndPlaceModule) -> N
     assert result.error_code == "GRASP_FRAME_MISMATCH"
 
 
+def test_pick_falls_through_to_the_next_reachable_candidate(
+    module: PickAndPlaceModule,
+) -> None:
+    """A learned provider's best-scoring pose is not always kinematically reachable."""
+    manipulation: Any = module._manipulation
+    module._grasp_generator.propose_grasps.return_value = GraspCandidateArray(
+        Header(1.0, "world"), [_candidate(0.1, score=0.9), _candidate(0.3, score=0.4)]
+    )
+    manipulation.plan_to_poses.side_effect = [
+        SimpleNamespace(succeeded=False, message="unreachable"),
+        SimpleNamespace(succeeded=True, message=""),
+        SimpleNamespace(succeeded=True, message=""),
+        SimpleNamespace(succeeded=True, message=""),
+    ]
+
+    result = module.pick_object("cup-1")
+
+    assert result.success
+    assert result.metadata["rank"] == 1
+    assert result.metadata["score"] == 0.4
+
+
+def test_pick_stops_walking_candidates_on_a_drive_fault(module: PickAndPlaceModule) -> None:
+    """An execution fault would repeat for every candidate, so it is not a demotion."""
+    manipulation: Any = module._manipulation
+    module._grasp_generator.propose_grasps.return_value = GraspCandidateArray(
+        Header(1.0, "world"), [_candidate(0.1), _candidate(0.3)]
+    )
+    manipulation.execute.return_value = SimpleNamespace(succeeded=False, message="drive fault")
+
+    result = module.pick_object("cup-1")
+
+    assert not result.success
+    assert result.error_code == "EXECUTION_FAILED"
+    assert manipulation.plan_to_poses.call_count == 1
+
+
+def test_pick_reports_no_reachable_candidate_when_every_attempt_fails(
+    module: PickAndPlaceModule,
+) -> None:
+    manipulation: Any = module._manipulation
+    module._grasp_generator.propose_grasps.return_value = GraspCandidateArray(
+        Header(1.0, "world"), [_candidate(0.1), _candidate(0.3)]
+    )
+    manipulation.plan_to_poses.return_value = SimpleNamespace(
+        succeeded=False, message="unreachable"
+    )
+
+    result = module.pick_object("cup-1")
+
+    assert not result.success
+    assert result.error_code == "PLANNING_FAILED"
+    assert not module._holding_object
+
+
 def test_pick_object_rejects_empty_candidates(module: PickAndPlaceModule) -> None:
     module._grasp_generator.propose_grasps.return_value = GraspCandidateArray(
         Header(1.0, "world"), []

dimos/robot/all_blueprints.py

@@ -175,6 +175,8 @@
     "unity-sim": "dimos.simulation.unity.blueprint:unity_sim",
     "xarm-grasp": "dimos.robot.manipulators.xarm.blueprints.grasp:xarm_grasp",
     "xarm-grasp-agent": "dimos.robot.manipulators.xarm.blueprints.agentic:xarm_grasp_agent",
+    "xarm-grasp-graspgenx": "dimos.robot.manipulators.xarm.blueprints.grasp:xarm_grasp_graspgenx",
+    "xarm-grasp-graspgenx-agent": "dimos.robot.manipulators.xarm.blueprints.agentic:xarm_grasp_graspgenx_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",

dimos/robot/manipulators/xarm/blueprints/agentic.py

@@ -25,7 +25,7 @@
     MANIPULATION_AGENT_SYSTEM_PROMPT,
 )
 from dimos.robot.manipulators.xarm.blueprints.basic import xarm7_planner_coordinator
-from dimos.robot.manipulators.xarm.blueprints.grasp import xarm_grasp
+from dimos.robot.manipulators.xarm.blueprints.grasp import xarm_grasp, xarm_grasp_graspgenx
 from dimos.robot.manipulators.xarm.blueprints.simulation import xarm_perception_sim
 
 xarm7_planner_coordinator_agent = autoconnect(
@@ -44,6 +44,12 @@
     McpClient.blueprint(system_prompt=MANIPULATION_AGENT_SYSTEM_PROMPT),
 ).global_config(n_workers=6)
 
+xarm_grasp_graspgenx_agent = autoconnect(
+    xarm_grasp_graspgenx,
+    McpServer.blueprint(),
+    McpClient.blueprint(system_prompt=MANIPULATION_AGENT_SYSTEM_PROMPT),
+).global_config(n_workers=6)
+
 xarm_perception_sim_agent = autoconnect(
     xarm_perception_sim,
     McpServer.blueprint(),

dimos/robot/manipulators/xarm/blueprints/grasp.py

@@ -14,14 +14,15 @@
 
 """The xArm grasping stack, on hardware by default.
 
-``dimos run xarm-grasp --xarm7-ip 192.168.1.x``   the real arm
+``dimos run xarm-grasp --xarm7-ip 192.168.1.x``   heuristic grasps
+``dimos run xarm-grasp-graspgenx --xarm7-ip ...`` learned grasps
 ``dimos run xarm-grasp --simulation mujoco``      the same stack in MuJoCo
 
-The arm-versus-sim split is decided here at import time, because composition runs
-before module config is applied. What it swaps is the hardware adapter, the base
-pose, the camera and the engine behind it, the detector backends and the home
-pose. Everything else -- the coordinator, pick-and-place, scene registration --
-is the same stack either way.
+Only the grasp provider separates the two blueprints. The arm-versus-sim split is
+decided here at import time, because composition runs before module config is
+applied. What it swaps is the hardware adapter, the base pose, the camera and the
+engine behind it, the detector backends and the home pose. Everything else -- the
+coordinator, pick-and-place, scene registration -- is the same stack either way.
 """
 
 from __future__ import annotations
@@ -30,6 +31,7 @@
 from dimos.core.coordination.blueprints import Blueprint, autoconnect
 from dimos.core.global_config import global_config
 from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera
+from dimos.manipulation.grasping.grasp_gen_x import GraspGenXModule
 from dimos.manipulation.grasping.heuristic_grasp import HeuristicGraspModule
 from dimos.manipulation.manipulation_module import ManipulationModule
 from dimos.manipulation.manipulation_skills import ManipulationSkills
@@ -70,6 +72,28 @@
     "green ring",
 ]
 
+# Measured off data/xarm_grasp_sim (mj_forward at the driver joint limits) and the
+# gripper URDF, expressed in GraspGenX's convention -- approach along +Z, jaws
+# closing along X -- with the origin on xarm_gripper_base_link, the frame
+# GraspGenX predicts into. The geometry is the real gripper's, so it holds on
+# hardware too.
+XARM_GRIPPER_SWEEP_VOLUME = {
+    "extents_open": (0.0889, 0.030, 0.0370),
+    "offset_open": (0.0, 0.0, 0.1421),
+    "extents_half_open": (0.0479, 0.030, 0.0370),
+    "offset_half_open": (0.0, 0.0, 0.1530),
+    "fingertip_depth": 0.1606,
+}
+# xarm_gripper_base_link -> link_tcp, the planning tip frame: +0.172 m along the
+# approach axis (xarm_gripper.urdf.xacro joint_tcp) plus the quarter turn that
+# takes GraspGenX's X closing axis onto the xArm gripper's Y.
+XARM_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),
+)
+
 # Hand-eye calibration for the eye-in-hand RealSense. RealSenseCamera publishes
 # only its own subtree, so without this edge camera_link has no parent, nothing
 # resolves into world, and every cloud the camera produces is silently unusable.
@@ -180,3 +204,11 @@ def _scene_registration() -> Blueprint:
 )
 
 xarm_grasp = autoconnect(*_XARM_GRASP_MODULES, HeuristicGraspModule.blueprint())
+
+xarm_grasp_graspgenx = autoconnect(
+    *_XARM_GRASP_MODULES,
+    GraspGenXModule.blueprint(
+        gripper=XARM_GRIPPER_SWEEP_VOLUME,
+        grasp_frame_to_tcp=XARM_GRASP_FRAME_TO_TCP,
+    ),
+)

dimos/robot/test_all_blueprints.py

@@ -67,6 +67,8 @@
         "teleop-webxr-xarm7-video",
         "xarm-grasp",
         "xarm-grasp-agent",
+        "xarm-grasp-graspgenx",
+        "xarm-grasp-graspgenx-agent",
         "xarm-perception-sim",
         "xarm-perception-sim-agent",
         "xarm7-planner-coordinator",

docs/capabilities/manipulation/xarm-grasp.md

@@ -1,19 +1,31 @@
 # xArm Grasping
 
-`xarm-grasp` is the xArm7 grasping stack: the control coordinator, the wrist
-camera, scene registration, pick-and-place and a top-down heuristic grasp
-provider. It runs on the real arm by default and switches to the MuJoCo room
-scene with `--simulation`:
+Two blueprints, differing only in which grasp provider they compose. Both carry
+the control coordinator, the wrist camera, scene registration and
+pick-and-place; both run on the real arm by default and switch to the MuJoCo
+room scene with `--simulation`:
+
+| Blueprint | Grasps |
+|---|---|
+| `xarm-grasp` | one top-down heuristic grasp, score 1.0 |
+| `xarm-grasp-graspgenx` | up to 100 ranked learned grasps |
 
 ```bash
-dimos run xarm-grasp --xarm7-ip 192.168.1.x     # hardware
-dimos run xarm-grasp --simulation mujoco        # the room scene
+dimos run xarm-grasp-graspgenx --xarm7-ip 192.168.1.x     # hardware
+dimos run xarm-grasp-graspgenx --simulation mujoco        # the room scene
 ```
 
 Miss `--xarm7-ip` on hardware and the arm has no address to reach; leave
 `--simulation` set and everything reverts to MuJoCo regardless of the IP.
-`xarm-grasp-agent` adds an MCP agent over the top; drive it with
-`dimos agent-send "..."`.
+`xarm-grasp-agent` and `xarm-grasp-graspgenx-agent` add an MCP agent over the
+top; drive those with `dimos agent-send "..."`.
+
+`xarm-grasp-graspgenx` needs the `graspgenx` extra and a CUDA GPU. Checkpoints
+download once from Hugging Face and cache under `~/.cache/huggingface`.
+
+```bash
+uv sync --extra graspgenx
+```
 
 What differs between the arm and the sim is decided at import time: the hardware
 adapter, the base pose, the camera (RealSense plus its mount edge, versus the
@@ -99,7 +111,18 @@ app.PickAndPlaceModule.place_at(0.45, -0.25, 0.25)
 
 `pick_object` generates the grasps itself, so there is no separate grasp call.
 It opens the gripper, plans to the pregrasp, moves in, closes, verifies, and
-retreats. The prompt set includes a `green ring` fallback because the tape loses
+retreats; with a learned provider it walks the ranked candidates until one is
+reachable, and the result metadata carries the winning rank, its score and the
+candidate count. To inspect grasps without moving the arm, call `propose_grasps`
+on the provider directly:
+
+```python skip
+cloud = scene.get_object_pointcloud_by_object_id("<object_id>")
+candidates = app.GraspGenXModule.propose_grasps(cloud)   # HeuristicGraspModule in the base blueprint
+print(len(candidates.candidates), [c.score for c in candidates.candidates[:5]])
+```
+
+The prompt set includes a `green ring` fallback because the tape loses
 its category silhouette in the wrist camera's top-down view.
 
 A failed grasp knocks free-body targets out of place, and `MujocoSimModule.reset()`