feat: integrate GraspGenX grasp proposals

CONTEXT.md

@@ -1,3 +1,57 @@
+# 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
+
+**Feasible Grasp Sequence**:
+A connected, collision-free trajectory from the robot's current state through any required safety lift, pre-grasp, grasp, and retreat. Each segment begins at the preceding segment's endpoint. Validation is a no-motion dry run; execution replans each segment from fresh measured state.
+_Avoid_: Reachable grasp, feasible pose, independent IK success
+
+**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 plan that collision-checks the robot and gripper against the non-target scene. The selected target remains excluded; attached-object geometry and held-object clearance are not modeled.
+_Avoid_: Payload-safe retreat, attached-object validation
+
+**Live-Scene Validation (MVP)**:
+Each segment of a feasible grasp sequence is checked against the latest available planning scene. The MVP does not snapshot the scene or freeze non-target obstacle updates across the sequence. Execution replans against freshly measured state and scene data.
+_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 Planning
 
 This context describes requests for planning robot motion through joint and Cartesian spaces.

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_client.py

@@ -21,7 +21,7 @@
 
 from langchain.agents import create_agent
 from langchain.chat_models import init_chat_model
-from langchain_core.messages import HumanMessage
+from langchain_core.messages import HumanMessage, ToolMessage
 from langchain_core.messages.base import BaseMessage
 from langchain_core.tools import StructuredTool
 from langchain_openai import ChatOpenAI
@@ -69,6 +69,7 @@ class McpClient(Module):
     agent: Out[BaseMessage]
     human_input: In[str]
     agent_idle: Out[bool]
+    agent_cancel: In[bool]
 
     _lock: RLock
     _state_graph: CompiledStateGraph[Any, Any, Any, Any] | None
@@ -77,6 +78,7 @@ class McpClient(Module):
     _history: list[BaseMessage]
     _thread: Thread
     _stop_event: Event
+    _cancel_event: Event
     _http_client: requests.Session
     _seq_ids: SequentialIds
     _tool_stream_cleanup: Callable[[], None] | None
@@ -94,6 +96,7 @@ def __init__(self, **kwargs: Any) -> None:
             daemon=True,
         )
         self._stop_event = Event()
+        self._cancel_event = Event()
         self._http_client = requests.Session()
         self._seq_ids = SequentialIds()
         self._tool_stream_cleanup = None
@@ -217,6 +220,12 @@ def _on_human_input(string: str) -> None:
 
         self.register_disposable(Disposable(self.human_input.subscribe(_on_human_input)))
 
+        def _on_agent_cancel(_cancel: bool) -> None:
+            self._cancel_event.set()
+            self.agent_idle.publish(True)
+
+        self.register_disposable(Disposable(self.agent_cancel.subscribe(_on_agent_cancel)))
+
         # Subscribe directly over LCM rather than through the server's GET
         # /mcp SSE channel.  HTTP would add a startup race: the first few
         # updates of a short-lived stream can fire before the SSE connection
@@ -330,7 +339,13 @@ def _thread_loop(self) -> None:
             with self._lock:
                 if not self._state_graph:
                     raise ValueError("No state graph initialized")
-                self._process_message(self._state_graph, message)
+                self._cancel_event.clear()
+                try:
+                    self._process_message(self._state_graph, message)
+                except Exception:
+                    self._close_cancelled_tool_calls()
+                    logger.exception("Agent turn failed")
+                    self.agent_idle.publish(True)
 
     def _process_message(
         self, state_graph: CompiledStateGraph[Any, Any, Any, Any], message: BaseMessage
@@ -341,15 +356,39 @@ def _process_message(
         self.agent.publish(message)
 
         for update in state_graph.stream({"messages": self._history}, stream_mode="updates"):
+            if self._cancel_event.is_set():
+                self._close_cancelled_tool_calls()
+                break
             for node_output in update.values():
                 for msg in node_output.get("messages", []):
                     self._history.append(msg)
                     pretty_print_langchain_message(msg)
                     self.agent.publish(msg)
 
-        if self._message_queue.empty():
+            if self._cancel_event.is_set():
+                self._close_cancelled_tool_calls()
+                break
+
+        if self._cancel_event.is_set() or self._message_queue.empty():
             self.agent_idle.publish(True)
 
+    def _close_cancelled_tool_calls(self) -> None:
+        """Give every retained tool call a result before sending history to the model again."""
+        pending: dict[str, str] = {}
+        for message in self._history:
+            for tool_call in getattr(message, "tool_calls", []):
+                if call_id := tool_call.get("id"):
+                    pending[call_id] = tool_call.get("name", "tool")
+            if isinstance(message, ToolMessage):
+                pending.pop(message.tool_call_id, None)
+        for call_id, name in pending.items():
+            self._history.append(
+                ToolMessage(
+                    content=f"{name} was cancelled before a result was available.",
+                    tool_call_id=call_id,
+                )
+            )
+
 
 def _append_image_to_history(
     mcp_client: McpClient, func_name: str, uuid_: str, result: Any

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_client_unit.py

@@ -18,7 +18,7 @@
 from threading import RLock
 from unittest.mock import MagicMock, create_autospec, patch
 
-from langchain_core.messages import HumanMessage
+from langchain_core.messages import AIMessage, HumanMessage, ToolMessage
 from langchain_core.messages.base import BaseMessage
 from langchain_openai import ChatOpenAI
 import pytest
@@ -163,6 +163,28 @@ def test_tool_stream_notification_becomes_human_message(mcp_client: McpClient) -
     assert "Person follow stopped: lost track." in str(msg.content)
 
 
+def test_cancelled_turn_closes_unresolved_tool_calls(mcp_client: McpClient) -> None:
+    mcp_client._history = [
+        AIMessage(
+            content="",
+            tool_calls=[
+                {"name": "move_to_pose", "args": {}, "id": "call-pending", "type": "tool_call"}
+            ],
+        ),
+        AIMessage(
+            content="",
+            tool_calls=[{"name": "scan", "args": {}, "id": "call-complete", "type": "tool_call"}],
+        ),
+        ToolMessage(content="Detected 2 object(s)", tool_call_id="call-complete"),
+    ]
+
+    mcp_client._close_cancelled_tool_calls()
+
+    assert isinstance(mcp_client._history[-1], ToolMessage)
+    assert mcp_client._history[-1].tool_call_id == "call-pending"
+    assert "cancelled" in str(mcp_client._history[-1].content)
+
+
 def test_tool_stream_ignores_unrelated_frames(mcp_client: McpClient) -> None:
     """Unknown methods and empty bodies are dropped on the floor."""
 

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/agents/test_utils.py

@@ -0,0 +1,25 @@
+# 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.agents.utils import _display_message_content
+
+
+def test_display_message_content_omits_encrypted_reasoning() -> None:
+    content = [
+        {"type": "reasoning", "encrypted_content": "secret"},
+        {"type": "function_call", "name": "go_home"},
+        {"type": "text", "text": "Reached home."},
+    ]
+
+    assert _display_message_content(content) == "Reached home."

dimos/agents/utils.py

@@ -51,7 +51,7 @@ def pretty_print_langchain_message(msg: BaseMessage) -> None:
     time_str = f"{GRAY}{timestamp}{RESET}  "
     type_str = f"{type_color}{msg_type:<{TYPE_WIDTH}}{RESET}"
 
-    content = _try_to_remove_url_data(d.get("content", ""))
+    content = _display_message_content(d.get("content", ""))
     tool_calls = d.get("tool_calls", [])
 
     # 12 chars for timestamp + 1 space + TYPE_WIDTH + 1 space
@@ -96,16 +96,14 @@ def _log_message(msg_type: str, content: object, tool_calls: list[dict[str, Any]
     logger.info("Agent message", **kw)
 
 
-def _try_to_remove_url_data(content: Any) -> Any:
+def _display_message_content(content: Any) -> Any:
+    """Keep only user-visible text from OpenAI Responses content blocks."""
     if not isinstance(content, list):
         return content
-
-    ret = []
-
+    text_parts = []
     for item in content:
-        if isinstance(item, dict) and item.get("type") == "image_url":
-            ret.append({**item, "image_url": "<removed>"})
-        else:
-            ret.append(item)
-
-    return ret
+        if isinstance(item, dict) and item.get("type") == "text":
+            text = item.get("text")
+            if isinstance(text, str):
+                text_parts.append(text)
+    return "\n".join(text_parts)