feat(manipulation): unify xArm pick and place

CONTEXT.md

@@ -0,0 +1,131 @@
+# Grasp Planning
+
+This context defines the language at the boundary between upstream perception, grasp generation, and robot motion planning.
+
+## Language
+
+**Segmented Object Cloud**:
+A target-only 3D point cloud supplied by upstream perception in the manipulation planning frame.
+_Avoid_: Scene cloud, detection cloud, raw camera cloud
+
+**Grasp Provider**:
+The single configured source of ranked grasp candidates for a Pick/Place Transaction. Provider order is authoritative, provider failure ends proposal generation, and a transaction never switches providers automatically.
+_Avoid_: Grasp fallback, grasp strategy
+
+**Feasible Grasp Sequence**:
+A connected trajectory from the robot's current state through any required safety lift, pre-grasp, grasp, and retreat. Preparation and pre-grasp approach are collision-checked; intentional contact legs use Unchecked Contact Motion. Each segment begins at the preceding segment's endpoint, validation is a no-motion dry run, and execution replans from fresh measured state.
+_Avoid_: Reachable grasp, feasible pose, independent IK success
+
+**Unchecked Contact Motion**:
+A short, straight TCP motion that intentionally bypasses all planning-scene collision checks while retaining sequential kinematic feasibility, joint limits, tracking, timing, and execution-result validation.
+_Avoid_: Collision-free contact, target-only collision allowance, unvalidated motion
+
+**Safety Lift**:
+An optional shared preparation segment planned before evaluating grasp candidates. If required and unplannable, the pick aborts once in `PREPARE`; the failure is not attributed to every candidate.
+_Avoid_: Pre-grasp failure, candidate rejection
+
+**Retreat Feasibility (MVP)**:
+A connected grasp-to-retreat motion validated through sequential collision-disabled IK. Planning-scene obstacles, attached-object geometry, and held-object clearance are not checked during this contact leg.
+_Avoid_: Payload-safe retreat, attached-object validation
+
+**Live-Scene Validation (MVP)**:
+Each collision-checked segment of a Feasible Grasp Sequence uses the latest available planning scene. The MVP does not snapshot the scene or freeze obstacle updates across the sequence; Unchecked Contact Motion does not query the planning scene.
+_Avoid_: Atomic scene validation, frozen-scene guarantee
+
+**Gripper Geometry During Validation (MVP)**:
+Arm-path collision checks use the gripper configuration currently represented in the planning scene. Dry-run validation does not model the open-to-closed gripper transition or claim separate clearance guarantees for each finger configuration.
+_Avoid_: Coordinated arm-gripper plan, validated finger sweep
+
+**Candidate Rejection Reason (MVP)**:
+Candidate rejection is reported by failed sequence stage: `pre_grasp_infeasible`, `grasp_infeasible`, or `retreat_infeasible`. Detailed IK and planner outcomes remain diagnostic logs rather than public skill-result categories.
+_Avoid_: Backend-specific public failure codes
+
+**Pipeline Demo**:
+A no-hardware contributor command that runs a recorded Segmented Object Cloud through real grasp proposal and connected motion validation, then saves candidate outcomes and all planned segments. It stops before trajectory execution.
+_Avoid_: Grasp-only demo, hardware pick demo
+
+**Visualization Layer**:
+A display-only, named collection of visual elements owned by exactly one producer. Publishing replaces its contents, while clearing leaves the layer registered and preserves viewer-owned visibility; the layer cannot affect collision checking or other planning behavior.
+_Avoid_: Collision layer, shared scene state, visualization object
+
+**Visual Element**:
+A backend-neutral drawable contained in a Visualization Layer, initially a point cloud or line set. It carries no collision or planning authority.
+_Avoid_: Grasp visualization command, Viser handle, collision object
+
+**Visualization Layer Group**:
+A viewer-only grouping of independently replaceable and toggleable Visualization Layers that share a name prefix, such as `grasp/object-cloud` and `grasp/proposals`.
+_Avoid_: Compound layer, element-level visibility
+
+**Accepted Collision Projection**:
+A display-only representation published after the planning world accepts a collision-object change. Its presence, absence, or rendering failure never changes collision checking.
+_Avoid_: Collision authority, visualization obstacle
+
+## Manipulation Tasks
+
+**Detection Snapshot**:
+The immutable, numbered set of objects produced by one completed scene scan.
+_Avoid_: Live detections, object list
+
+**Object Obstacle Proxy**:
+Planning geometry derived for one detected object in a Detection Snapshot. It represents the best collision evidence available from its source and is not assumed to be exact physical geometry.
+_Avoid_: Ground-truth geometry, object mesh, detection
+
+**Object Number**:
+A human-facing selection handle scoped to one Detection Snapshot. It has no meaning outside that snapshot and is never a persistent object identity.
+_Avoid_: Object ID, stable ID
+
+**Selected Object**:
+The object record pinned from a Detection Snapshot before a Pick/Place Transaction begins. Later detection ordering cannot change which physical object the selection denotes.
+_Avoid_: Object number, current detection
+
+**Prepared Pick**:
+A Selected Object together with its pinned Segmented Object Cloud and ranked grasp candidates. It is inspection input for a later Pick/Place Transaction and carries no reservation or continuing feasibility guarantee; a later scan, preparation timeout, or planning-frame mismatch makes it stale.
+_Avoid_: Planned pick, reserved pick, feasible grasp
+
+**Verified Pick**:
+A completed pick whose configured gripper feedback indicates that an object prevented empty closure. Pick completion without that evidence is a failure, not an unverified success.
+_Avoid_: Assumed hold, commanded pick, unverified pick
+
+**Object Placement Target**:
+The desired world position of the held object's reference point, initially its detected center. It is not a robot TCP target; the Pick/Place Transaction derives the required TCP pose from the retained grasp relationship.
+_Avoid_: TCP target, gripper position, drop pose
+
+**Manipulation World Frame**:
+The canonical `world` frame in which Detection Snapshots, Segmented Object Clouds, grasp candidates, Object Obstacle Proxies, and Object Placement Targets are expressed.
+_Avoid_: Planning-frame option, robot base coordinates, camera coordinates
+
+**Pick/Place Transaction**:
+A single physical operation that picks one selected object and either places it at a target pose or into a selected container. One owner has exclusive control of the selected robot from validation through completion and owns the physical success or failure outcome, but not the policy for choosing a sequence of objects.
+_Avoid_: Box-filling workflow, manipulation task
+
+**Box-Filling Task**:
+A task policy that selects which objects belong in a container and invokes one Pick/Place Transaction for each selected object. Its container measurement and drop behavior are application-specific and are not part of the generic Pick/Place Transaction interface.
+_Avoid_: Pick/place transaction, grasp pipeline
+
+# Manipulation Planning
+
+This context describes requests for planning robot motion through joint and Cartesian spaces.
+
+## Language
+
+**Cartesian Waypoint**:
+One absolute TCP pose or relative rigid displacement within a Cartesian target.
+
+**Cartesian Target**:
+An ordered, homogeneous sequence of Cartesian waypoints for one planning group, including its starting waypoint. An absolute target contains only `PoseStamped` waypoints and starts at the current TCP pose. A relative target contains only `Transform` waypoints, starts with the identity transform, and measures every waypoint from the planning-start TCP pose.
+_Avoid_: Cartesian track
+
+**Cartesian Path Configuration**:
+Per-planning-call policy that selects how Cartesian waypoints are connected and constrains that operation. It is independent of the startup configuration that selects and constructs a planner backend.
+
+**Standard Cartesian Planning**:
+Cartesian waypoint planning through a backend's supported serializable options. For RoboPlan, this includes multi-waypoint and simultaneous multi-end-effector paths, bounded and time-optimal speed modes, tracking tolerances, and solver tuning.
+
+**Bounded Speed Mode**:
+A Cartesian timing policy that treats configured tool speeds and accelerations as maxima and slows the motion further when required by tracking or joint limits.
+
+**Time-Optimal Speed Mode**:
+A Cartesian timing policy that resolves the requested path into joint space and retimes it against joint limits, optionally blending intermediate corners.
+
+**Custom Planner Components**:
+Backend-native solver tasks, constraints, and barriers injected as live objects. These are outside standard Cartesian planning and require a separate constrained-IK interface.

bin/setup-graspgenx-env

@@ -0,0 +1,13 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+PYTHON=.venv/bin/python
+uv pip install --python "$PYTHON" --extra-index-url https://download.pytorch.org/whl/cu128 \
+  --index-strategy unsafe-best-match \
+  torch==2.7.1+cu128 torchvision==0.22.1+cu128
+uv pip install --python "$PYTHON" \
+  torch-geometric h5py hydra-core matplotlib numpy==1.26.4 webdataset scikit-learn scipy \
+  tensorboard transformers tensordict diffusers==0.11.1 timm==1.0.15 \
+  huggingface-hub==0.25.2 PyOpenGL==3.1.5 addict yapf==0.40.1 tensorboardx \
+  sharedarray yourdfpy==0.0.56 urdfpy imageio viser tqdm pyyaml edgetam-dimos "networkx>=3.3"
+uv pip install --python "$PYTHON" --no-deps "git+https://github.com/NVlabs/GraspGenX.git"

dimos/agents/capabilities.py

@@ -35,6 +35,7 @@
 from typing import NamedTuple
 
 CAP_MOVEMENT = "movement"
+CAP_PERCEPTION = "perception"
 
 
 class _Hold(NamedTuple):

dimos/agents/mcp/mcp_server.py

@@ -33,7 +33,7 @@
 from dimos.agents.capabilities import CapabilityRegistry
 from dimos.agents.mcp import tool_stream
 from dimos.core.core import rpc
-from dimos.core.module import Module
+from dimos.core.module import Module, ModuleConfig
 from dimos.core.rpc_client import RpcCall, RPCClient
 from dimos.core.transport_factory import make_transport
 from dimos.utils.logging_config import setup_logger
@@ -52,6 +52,14 @@
 # `_can_wait` in `_handle_tools_call`).
 DEFAULT_CAP_ACQUIRE_TIMEOUT = 30.0  # seconds
 
+
+class McpServerConfig(ModuleConfig):
+    """Configuration for the MCP HTTP server."""
+
+    allowed_skills: list[str] | None = None
+    """Optional names of skills exposed through MCP; None exposes every deployed skill."""
+
+
 app = FastAPI()
 app.add_middleware(
     CORSMiddleware,
@@ -80,6 +88,27 @@ def _jsonrpc_error(req_id: Any, code: int, message: str) -> dict[str, Any]:
     return {"jsonrpc": "2.0", "id": req_id, "error": {"code": code, "message": message}}
 
 
+def _filter_skills(skills: list[SkillInfo], allowed_skills: list[str] | None) -> list[SkillInfo]:
+    """Keep only explicitly exposed skills when an MCP allowlist is configured."""
+    if allowed_skills is None:
+        return skills
+    allowed = set(allowed_skills)
+    return [skill_info for skill_info in skills if skill_info.func_name in allowed]
+
+
+def _select_module_skills(
+    modules: list[RPCClient], allowed_skills: list[str] | None
+) -> list[tuple[RPCClient, SkillInfo]]:
+    """Return exposed skills together with their deployed module RPC address."""
+    allowed = set(allowed_skills) if allowed_skills is not None else None
+    return [
+        (module, skill_info)
+        for module in modules
+        for skill_info in (module.get_skills() or [])
+        if allowed is None or skill_info.func_name in allowed
+    ]
+
+
 def _handle_initialize(req_id: Any) -> dict[str, Any]:
     return _jsonrpc_result(
         req_id,
@@ -345,6 +374,7 @@ async def event_generator() -> AsyncGenerator[str, None]:
 
 
 class McpServer(Module):
+    config: McpServerConfig
     _uvicorn_server: uvicorn.Server | None = None
     _serve_future: concurrent.futures.Future[None] | None = None
     _tool_stream_cleanup: Callable[[], None] | None = None
@@ -381,15 +411,14 @@ def stop(self) -> None:
     def on_system_modules(self, modules: list[RPCClient]) -> None:
         # TODO: this is a bit hacky, also not thread-safe
         assert self.rpc is not None
-        app.state.skills = [
-            skill_info for module in modules for skill_info in (module.get_skills() or [])
-        ]
+        module_skills = _select_module_skills(modules, self.config.allowed_skills)
+        app.state.skills = [skill_info for _, skill_info in module_skills]
         app.state.skills_by_name = {s.func_name: s for s in app.state.skills}
         app.state.rpc_calls = {
             skill_info.func_name: RpcCall(
-                None, self.rpc, skill_info.func_name, skill_info.class_name, []
+                None, self.rpc, skill_info.func_name, module.remote_name, []
             )
-            for skill_info in app.state.skills
+            for module, skill_info in module_skills
         }
 
     @skill

dimos/agents/mcp/test_mcp_server.py

@@ -20,7 +20,7 @@
 from unittest.mock import MagicMock
 
 from dimos.agents.capabilities import CapabilityRegistry
-from dimos.agents.mcp.mcp_server import app, handle_request
+from dimos.agents.mcp.mcp_server import _filter_skills, _select_module_skills, app, handle_request
 from dimos.core.module import SkillInfo
 
 
@@ -39,6 +39,26 @@ def _make_rpc_calls(
     return rpc_calls
 
 
+def test_filter_skills_respects_allowlist() -> None:
+    schema = json.dumps({"type": "object", "properties": {}})
+    skills = [
+        SkillInfo(class_name="TestSkills", func_name="safe", args_schema=schema),
+        SkillInfo(class_name="TestSkills", func_name="unsafe", args_schema=schema),
+    ]
+
+    assert [skill.func_name for skill in _filter_skills(skills, ["safe"])] == ["safe"]
+    assert _filter_skills(skills, None) == skills
+
+
+def test_select_module_skills_retains_deployed_remote_name() -> None:
+    schema = json.dumps({"type": "object", "properties": {}})
+    skill = SkillInfo(class_name="PickNPlaceModule", func_name="scan", args_schema=schema)
+    module = MagicMock(remote_name="pnp")
+    module.get_skills.return_value = [skill]
+
+    assert _select_module_skills([module], ["scan"]) == [(module, skill)]
+
+
 def test_mcp_module_request_flow() -> None:
     schema = json.dumps(
         {

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,17 @@ def read_state(self) -> dict[JointName, JointState]:
 
         return result
 
+    def set_gripper_position(self, position: float) -> bool:
+        """Command the gripper and preserve that command during arm trajectories."""
+        if not self._gripper_joints:
+            return False
+        if not self._initialized:
+            self._initialize_last_commanded()
+        normalized_position = self._physical_to_normalized(position)
+        for joint_name in self._gripper_joints:
+            self._last_commanded[joint_name] = normalized_position
+        return self._adapter.write_gripper_position(position)
+
     def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool:
         """Write commands - allows partial joint sets, holds last for missing.
 

dimos/hardware/manipulators/xarm/adapter.py

@@ -128,7 +128,6 @@ def set_control_mode(self, mode: ControlMode) -> bool:
         """
         if not self._arm:
             return False
-
         mode_map = {
             ControlMode.POSITION: _XARM_MODE_POSITION,
             ControlMode.SERVO_POSITION: _XARM_MODE_SERVO_CARTESIAN,  # Mode 1 for high-freq
@@ -231,7 +230,6 @@ def activate(self) -> bool:
         """Enable motion and move the arm to its initial joint pose."""
         if not self._arm:
             return False
-
         self._prepare_for_position_motion()
         if not self._move_to_initial_pose():
             return False
@@ -241,12 +239,14 @@ def deactivate(self) -> bool:
         """Move the arm to its initial joint pose and enter stopped state."""
         if not self._arm:
             return False
-
         self._prepare_for_position_motion()
         homed = self._move_to_initial_pose()
+        gripper_opened = True
+        if self._gripper_enabled:
+            gripper_opened = self._arm.set_gripper_position(0.85 * M_TO_MM, wait=True) == 0
         self._arm.motion_enable(enable=False)
         code: int = self._arm.set_state(4)
-        return homed and code == 0
+        return homed and gripper_opened and code == 0
 
     def _move_to_initial_pose(self) -> bool:
         if not self._arm:

dimos/hardware/manipulators/xarm/test_adapter.py

@@ -82,6 +82,14 @@ def set_servo_angle_j(self, angles: list[float], *, speed: float, mvacc: float)
         self.actions.append(("set_servo_angle_j", list(angles), speed, mvacc))
         return 0
 
+    def set_gripper_enable(self, enable: bool) -> int:
+        self.actions.append(("set_gripper_enable", enable))
+        return 0
+
+    def set_gripper_position(self, position: float, *, wait: bool) -> int:
+        self.actions.append(("set_gripper_position", position, wait))
+        return 0
+
 
 @pytest.fixture
 def xarm_adapter_module(monkeypatch: pytest.MonkeyPatch) -> Iterator[ModuleType]:
@@ -130,3 +138,18 @@ def test_joint_position_commands_use_degrees_for_xarm_sdk(
 
     arm = _FakeXArmSdk.instances[-1]
     assert arm.servo_joint_commands[-1] == pytest.approx([90.0, -45.0, 180.0])
+
+
+def test_deactivate_opens_an_enabled_gripper(xarm_adapter_module: ModuleType) -> None:
+    adapter = xarm_adapter_module.XArmAdapter(address="192.0.2.10", dof=6)
+    assert adapter.connect()
+    assert adapter.write_gripper_position(0.0)
+
+    assert adapter.deactivate()
+
+    arm = _FakeXArmSdk.instances[-1]
+    assert arm.actions[-3:] == [
+        ("set_gripper_position", 850.0, True),
+        ("motion_enable", False),
+        ("set_state", 4),
+    ]

dimos/manipulation/README.md

@@ -0,0 +1,26 @@
+# Pick and place
+
+`PickAndPlaceModule` is the single owner of generic pick/place transactions.
+Its public workflow is:
+
+1. `scan_objects(object_names)` creates a numbered, immutable scene snapshot.
+2. `select_object(number)` pins that object and GraspGenX proposals without moving.
+3. `pick_selected()` rechecks feasibility, approaches with collision checking,
+   executes straight contact/retreat legs without collision queries, closes the
+   gripper, and requires positive closure feedback.
+4. `place_at(x, y, z)` places the held object's reference point at the requested
+   world-frame position.
+
+`BoxFillingPickAndPlaceModule` derives from the generic module and adds only
+destination-container selection, fit checks, and box placement policy.
+
+The two public products are:
+
+```bash
+uv run dimos run xarm-box-filling --daemon
+uv run dimos run xarm-grasp-sim-agent --daemon
+```
+
+Both use GraspGenX as their only grasp provider. The real product uses an xArm6
+and wrist RealSense; the simulation product uses xArm7, MuJoCo, and ground-truth
+object geometry. Camera-based simulation remains test-only.