From 7fd8af98e5bc1d05e2331a6d44a00934de048a4c Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Fri, 7 Aug 2026 11:26:59 -0700 Subject: [PATCH 1/5] feat(manipulation): add xArm pick-and-place workflow --- CONTEXT.md | 27 - bin/setup-graspgenx-env | 13 + dimos/agents/capabilities.py | 1 + dimos/agents/mcp/mcp_server.py | 41 +- dimos/agents/mcp/test_mcp_server.py | 22 +- dimos/control/coordinator.py | 2 +- dimos/control/hardware_interface.py | 11 + dimos/hardware/manipulators/xarm/adapter.py | 8 +- .../manipulators/xarm/test_adapter.py | 23 + dimos/manipulation/README.md | 128 +++ dimos/manipulation/blueprints.py | 113 +++ dimos/manipulation/candidate_filter_spec.py | 32 + dimos/manipulation/grasping/grasp_gen_x.py | 11 +- .../grasping/grasp_gen_x_runtime.py | 76 +- .../manipulation/grasping/test_grasp_gen_x.py | 20 +- dimos/manipulation/manipulation_module.py | 248 ++++- dimos/manipulation/obstacle_world_spec.py | 43 + dimos/manipulation/pick_and_place_module.py | 552 +++++++++-- dimos/manipulation/pick_execution_spec.py | 42 + dimos/manipulation/picknplace.py | 936 ++++++++++++++++++ .../planning/monitor/test_world_monitor.py | 7 + .../test_world_obstacle_suppression.py | 178 ++++ .../planning/monitor/world_monitor.py | 16 +- .../monitor/world_obstacle_monitor.py | 74 +- dimos/manipulation/planning/spec/protocols.py | 9 + .../planning/world/drake_world.py | 9 + dimos/manipulation/pnpconsole.py | 314 ++++++ dimos/manipulation/skill_errors.py | 5 + dimos/manipulation/test_manipulation_unit.py | 374 +++++++ .../manipulation/test_pick_and_place_unit.py | 674 ++++++++++++- dimos/manipulation/test_picknplace.py | 456 +++++++++ dimos/manipulation/test_pnpconsole.py | 206 ++++ dimos/manipulation/test_table_collision.py | 34 + dimos/manipulation/visualization/layers.py | 226 +++++ .../visualization/pose_overlay.py | 68 ++ dimos/manipulation/visualization/rerun.py | 157 +++ .../visualization/test_factory.py | 18 + .../manipulation/visualization/test_layers.py | 186 ++++ .../manipulation/visualization/test_rerun.py | 48 + .../visualization/viser/layers.py | 277 ++++++ .../manipulation/visualization/viser/scene.py | 144 ++- .../visualization/viser/test_layers.py | 441 +++++++++ .../visualization/viser/visualizer.py | 50 + dimos/manipulation/visualization_spec.py | 24 + dimos/models/segmentation/edge_tam.py | 52 +- dimos/models/vl/moondream.py | 6 +- .../manipulation_msgs/GraspCandidateArray.py | 24 +- .../detection/detectors/moondream.py | 63 ++ .../detection/detectors/test_moondream.py | 78 ++ .../detection/type/detection2d/seg.py | 19 + .../detection/type/imageDetections.py | 2 +- .../experimental/object_scene_registration.py | 222 ++++- .../object_scene_registration_spec.py | 4 + ...test_object_scene_registration_temporal.py | 150 ++- dimos/robot/all_blueprints.py | 5 + .../manipulators/common/agent_prompts.py | 16 +- .../manipulators/xarm/blueprints/agentic.py | 7 + .../manipulators/xarm/blueprints/graspgenx.py | 61 ++ .../xarm/blueprints/perception.py | 1 + .../xarm/blueprints/simulation.py | 1 + dimos/robot/manipulators/xarm/grasp_config.py | 63 ++ docs/capabilities/manipulation/agentic.md | 50 + 62 files changed, 6895 insertions(+), 273 deletions(-) delete mode 100644 CONTEXT.md create mode 100644 bin/setup-graspgenx-env create mode 100644 dimos/manipulation/README.md create mode 100644 dimos/manipulation/candidate_filter_spec.py create mode 100644 dimos/manipulation/obstacle_world_spec.py create mode 100644 dimos/manipulation/pick_execution_spec.py create mode 100644 dimos/manipulation/picknplace.py create mode 100644 dimos/manipulation/planning/monitor/test_world_obstacle_suppression.py create mode 100644 dimos/manipulation/pnpconsole.py create mode 100644 dimos/manipulation/test_picknplace.py create mode 100644 dimos/manipulation/test_pnpconsole.py create mode 100644 dimos/manipulation/test_table_collision.py create mode 100644 dimos/manipulation/visualization/layers.py create mode 100644 dimos/manipulation/visualization/pose_overlay.py create mode 100644 dimos/manipulation/visualization/rerun.py create mode 100644 dimos/manipulation/visualization/test_layers.py create mode 100644 dimos/manipulation/visualization/test_rerun.py create mode 100644 dimos/manipulation/visualization/viser/layers.py create mode 100644 dimos/manipulation/visualization/viser/test_layers.py create mode 100644 dimos/manipulation/visualization_spec.py create mode 100644 dimos/perception/detection/detectors/moondream.py create mode 100644 dimos/perception/detection/detectors/test_moondream.py create mode 100644 dimos/robot/manipulators/xarm/blueprints/graspgenx.py create mode 100644 dimos/robot/manipulators/xarm/grasp_config.py diff --git a/CONTEXT.md b/CONTEXT.md deleted file mode 100644 index d97571b990..0000000000 --- a/CONTEXT.md +++ /dev/null @@ -1,27 +0,0 @@ -# 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. diff --git a/bin/setup-graspgenx-env b/bin/setup-graspgenx-env new file mode 100644 index 0000000000..712b341ec9 --- /dev/null +++ b/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" diff --git a/dimos/agents/capabilities.py b/dimos/agents/capabilities.py index 0f64d0af65..be505e6eb0 100644 --- a/dimos/agents/capabilities.py +++ b/dimos/agents/capabilities.py @@ -35,6 +35,7 @@ from typing import NamedTuple CAP_MOVEMENT = "movement" +CAP_PERCEPTION = "perception" class _Hold(NamedTuple): diff --git a/dimos/agents/mcp/mcp_server.py b/dimos/agents/mcp/mcp_server.py index 61d7572af8..113b22634f 100644 --- a/dimos/agents/mcp/mcp_server.py +++ b/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 diff --git a/dimos/agents/mcp/test_mcp_server.py b/dimos/agents/mcp/test_mcp_server.py index 0e2a74925b..e5a6851d84 100644 --- a/dimos/agents/mcp/test_mcp_server.py +++ b/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( { 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..bc3c5fb8ff 100644 --- a/dimos/control/hardware_interface.py +++ b/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. diff --git a/dimos/hardware/manipulators/xarm/adapter.py b/dimos/hardware/manipulators/xarm/adapter.py index c91292051c..73089c9271 100644 --- a/dimos/hardware/manipulators/xarm/adapter.py +++ b/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: diff --git a/dimos/hardware/manipulators/xarm/test_adapter.py b/dimos/hardware/manipulators/xarm/test_adapter.py index c3e21d58bc..67a268ad81 100644 --- a/dimos/hardware/manipulators/xarm/test_adapter.py +++ b/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), + ] diff --git a/dimos/manipulation/README.md b/dimos/manipulation/README.md new file mode 100644 index 0000000000..5717c1988a --- /dev/null +++ b/dimos/manipulation/README.md @@ -0,0 +1,128 @@ +# Pick And Place + +This directory contains the configurable xArm6 `picknplace` operator pipeline. +It uses the wrist-mounted RealSense and object-scene registration in `link_base`. + +## Setup + +GraspGenX runs in the main worktree `.venv` so it shares the live DimOS +pipeline. Its CUDA requirements differ from the repository lockfile; install +them once from the worktree root: + +```bash +bash bin/setup-graspgenx-env +``` + +The setup installs Torch 2.7.1 CUDA 12.8, which supports the RTX 5070's +`sm_120` architecture, along with GraspGenX and its inference dependencies. +Use `uv run --no-sync` afterwards. Plain `uv run` reconciles the environment to +the lockfile's Torch 2.6 and removes the GPU architecture support required by +GraspGenX. + +The first GraspGenX startup downloads the pinned model checkpoint to the +Hugging Face cache and loads it onto the GPU. Later starts reuse that cache. +The setup also installs `edgetam-dimos`, which provides the `sam2` runtime used +by the EdgeTAM blueprint. + +## Run + +Start the default YOLO-E and OBB-center-grasp pipeline: + +```bash +uv run --no-sync dimos run picknplace --daemon +``` + +Use text-prompted Moondream detection, EdgeTAM segmentation, and an OBB-center grasp: + +```bash +uv run --no-sync dimos run picknplace --daemon \ + -o osr.det=moondream -o osr.seg=edgetam -o pnp.grasp=obb_center +``` + +Use the same perception stack with GraspGenX: + +```bash +uv run --no-sync dimos run picknplace --daemon \ + -o osr.det=moondream -o osr.seg=edgetam -o pnp.grasp=graspgenx +``` + +`osr.det` accepts `yoloe` or `moondream`; `osr.seg` accepts `yolo` or `edgetam`. +Moondream requires EdgeTAM because it produces detection boxes rather than masks. +`pnp.grasp` accepts `obb_center` or `graspgenx`. GraspGenX loads only when selected. + +Then connect the console: + +```bash +uv run --no-sync python -m dimos.manipulation.pnpconsole +``` + +Stop a running pipeline with: + +```bash +uv run --no-sync dimos stop +``` + +## Operator Flow + +The console intentionally keeps planning and execution separate: + +1. Select `1` to scan the current scene. +2. Select `2` to inspect object number, name, and confidence. +3. Select `3` and choose an object. The GraspGenX blueprint prints its top + proposals and displays the selected grasp. Viser shows the selected object + cloud in amber, the grasp TCP axes in red, and the pre-grasp TCP axes in green. + The top ten proposals are filtered through collision-aware xArm IK; after + table calibration, candidates intersecting the table are omitted. +4. Select `4` to plan and preview the approach. Each Viser preview plays once + at a slow two-second duration. +5. Execute the approach only after inspecting the proposal and preview. +6. Select `6` to plan and preview descent, then select `7` to execute it. +7. Close the gripper with `8`, then select `9` to plan and preview ascent. +8. Select `10` to execute the ascent, `11` to open, and `13` to return home. +9. After a scene scan, select `14` to estimate and preview the tabletop. Once + the blue Viser outline matches the table, enter a collision clearance in + millimeters. The recommended clearance is 10 mm; enter `0` for no extra clearance. + The manual action installs the collision slab at the measured tabletop position for + all subsequent IK and trajectory plans. The pick-and-place blueprints do not install + a fixed floor slab. +10. After executing the approach, select `15` to collision-plan and execute the + descent, close the gripper, and execute the ascent without previews. It stops at + the first failed stage. + +Do not execute a learned grasp without checking its pose, the 100 mm pre-grasp +pose, the point-cloud/overlay visualization, and the collision-free preview. + +## Grasp Geometry + +`PickNPlaceModule.get_goal_pose()` stores the top ranked GraspGenX candidate as +the TCP goal in the candidate point cloud's frame. Its pre-grasp is computed as: + +```text +pre_grasp_position = grasp_position - grasp_orientation * (0, 0, 0.100 m) +``` + +GraspGenX local `+Z` is the final approach direction, so the pre-grasp retreats +along local `-Z`. It is not a world-Z lift: an angled or side grasp receives an +equally angled or sideward pre-grasp. Descent and ascent use Cartesian paths +between the current TCP pose and the selected grasp or pre-grasp target. + +The `picknplace-graspgenx` blueprint uses the xArm 85 mm gripper sweep-volume +and calibrated base-to-TCP transform. The TCP is rolled 90 degrees around the +GraspGenX approach axis so its closing jaws are perpendicular to a bottle's +length. Candidate score order comes from GraspGenX; no additional ranking is +applied by the operator pipeline. + +## Implementation Guide + +- `blueprints.py`: robot, camera, OBB, and GraspGenX blueprint composition. +- `picknplace.py`: scan request, target selection, OBB fallback, learned grasp + selection, and tool-axis pre-grasp calculation. +- `pnpconsole.py`: explicit operator stages and manual gripper/home controls. +- `grasping/grasp_gen_x.py`: import-safe proposal adapter and candidate contract. +- `grasping/grasp_gen_x_runtime.py`: in-process checkpoint load and GPU inference. +- `visualization/pose_overlay.py` and `visualization/rerun.py`: selected-object + cloud, image, and grasp overlays. + +The current scan is a single wrist-camera view. Automatic multi-view scanning, +EdgeTAM segmentation, and fused object clouds are planned follow-up work. Until +then, select targets with a complete enough visible point cloud for grasping. diff --git a/dimos/manipulation/blueprints.py b/dimos/manipulation/blueprints.py index 5c81178271..a5ad7ee5b4 100644 --- a/dimos/manipulation/blueprints.py +++ b/dimos/manipulation/blueprints.py @@ -17,6 +17,23 @@ Robot-owned manipulation blueprints now live under ``dimos.robot.manipulators``. """ +import math + +from dimos.agents.mcp.mcp_client import McpClient +from dimos.agents.mcp.mcp_server import McpServer +from dimos.core.coordination.blueprints import 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.manipulation_module import ManipulationModule +from dimos.manipulation.picknplace import PickNPlaceModule +from dimos.manipulation.visualization.rerun import picknplace_rerun_config +from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig +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.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule +from dimos.robot.manipulators.common.blueprints import coordinator, trajectory_task from dimos.robot.manipulators.xarm.blueprints.agentic import ( xarm7_planner_coordinator_agent as xarm7_planner_coordinator_agent, xarm_perception_agent as xarm_perception_agent, @@ -29,3 +46,99 @@ from dimos.robot.manipulators.xarm.blueprints.simulation import ( xarm_perception_sim as xarm_perception_sim, ) +from dimos.robot.manipulators.xarm.config import make_xarm6_model_config, xarm6_hardware +from dimos.robot.manipulators.xarm.grasp_config import make_xarm_graspgenx_config +from dimos.visualization.vis_module import vis_module + +PICKNPLACE_CAMERA_TRANSFORM = Transform( + translation=Vector3(0.06693724, -0.0309563, 0.00691482), + rotation=Quaternion(0.70513398, 0.00535696, 0.70897578, -0.01052180), +) + +BOX_FILLING_SYSTEM_PROMPT = """You are operating an xArm box-filling workspace with RGB-D perception. + +Your recurring task is to collect requested blocks from the table and drop them into the measured white box. The available tools are the live interface to the robot, planner, gripper, and scene. Use their results as authoritative, make multiple calls when needed, and only report physical actions after a tool confirms success. + +For a collection task: go home to observe, use ``scan_objects`` with separate simple noun phrases such as ``["colored wooden block", "white box"]``, estimate and install the table collision with no added margin, and measure the white box with ``install_open_box``. Use ``get_object_geometry`` to identify blocks whose centers are inside the measured box opening; those blocks are complete and must be ignored. Select only outside blocks, then call ``pick_selected``. If it succeeds, call ``place_selected`` to drop it into the remembered box. Repeat for other outside blocks. If pickup verification fails, rescan and select before another attempt. + +When the user says put, place, or drop an object in the box, use ``place_selected``. It is a depth-derived drop: it computes the box-rim and held-object clearance itself, releases above the rim, and does not lower the end effector into the box. Do not substitute manually chosen poses or individual gripper commands for pick or drop sequences. +""" + +_picknplace_xarm6_hardware = xarm6_hardware("arm", gripper=True) +_picknplace_xarm6_model = make_xarm6_model_config( + name="arm", + add_gripper=True, + tf_extra_links=["link_base", "link6"], + home_joints=[0.0, math.radians(-40.0), math.radians(-50.0), 0.0, math.radians(90.0), 0.0], +) +_picknplace_xarm6_model.max_velocity = 0.25 +_picknplace_xarm6_model.max_acceleration = 0.5 +_xarm_graspgenx = make_xarm_graspgenx_config() + + +picknplace = autoconnect( + coordinator( + hardware=[_picknplace_xarm6_hardware], + tasks=[trajectory_task(_picknplace_xarm6_hardware)], + ), + ManipulationModule.blueprint( + robots=[_picknplace_xarm6_model], + visualization=ViserVisualizationConfig(port=8095), + planning_timeout=10.0, + ), + RealSenseCamera.blueprint( + width=848, + height=480, + fps=15, + camera_name="camera", + base_frame_id="link6", + base_transform=PICKNPLACE_CAMERA_TRANSFORM, + enable_depth=True, + align_depth_to_color=True, + enable_pointcloud=False, + ), + ObjectSceneRegistrationModule.blueprint( + instance_name="osr", + target_frame="link_base", + register_objects=False, + detect_on_request=True, + detector_confidence=0.4, + object_voxel_downsample=0.001, + ), + PickNPlaceModule.blueprint(instance_name="pnp", align_grasp_yaw=True), + GraspGenXModule.blueprint( + instance_name="ggx", + load_on_start=False, + **_xarm_graspgenx.model_dump( + exclude={"rpc_transport", "tf_transport", "g", "instance_name", "load_on_start"} + ), + ), + vis_module( + global_config.viewer, + rerun_config=picknplace_rerun_config(), + ), +).global_config(rerun_open="web") + +picknplace_agent = autoconnect( + picknplace, + McpServer.blueprint( + allowed_skills=[ + "describe_scene", + "scan_objects", + "estimate_table", + "select_object", + "pick_selected", + "place_selected", + "get_object_geometry", + "install_open_box", + "set_table_collision", + "get_robot_state", + "reset", + "move_to_pose", + "close_gripper", + "open_gripper", + "go_home", + ] + ), + McpClient.blueprint(system_prompt=BOX_FILLING_SYSTEM_PROMPT), +) diff --git a/dimos/manipulation/candidate_filter_spec.py b/dimos/manipulation/candidate_filter_spec.py new file mode 100644 index 0000000000..da6ca47c99 --- /dev/null +++ b/dimos/manipulation/candidate_filter_spec.py @@ -0,0 +1,32 @@ +# 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. + +"""Planner contract used to reject unsafe learned grasp proposals.""" + +from typing import Protocol + +from dimos.manipulation.planning.spec.models import IKResult, RobotName +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.spec.utils import Spec + + +class GraspCandidateFilterSpec(Spec, Protocol): + def inverse_kinematics_single( + self, + pose: Pose, + robot_name: RobotName | None = None, + seed: JointState | None = None, + check_collision: bool = True, + ) -> IKResult: ... diff --git a/dimos/manipulation/grasping/grasp_gen_x.py b/dimos/manipulation/grasping/grasp_gen_x.py index 0107da1a32..a383e14d12 100644 --- a/dimos/manipulation/grasping/grasp_gen_x.py +++ b/dimos/manipulation/grasping/grasp_gen_x.py @@ -74,6 +74,7 @@ class GraspGenXConfig(ModuleConfig): gripper: SweepVolumeGripperConfig grasp_frame_to_tcp: RigidTransform = IDENTITY_TRANSFORM max_candidates: PositiveCount = 100 + load_on_start: bool = True # Relational matrix properties cannot be expressed through scalar Field constraints. @field_validator("grasp_frame_to_tcp") @@ -114,6 +115,10 @@ def __init__(self, **kwargs: Any) -> None: @rpc def start(self) -> None: super().start() + if self.config.load_on_start: + self._ensure_runtime() + + def _ensure_runtime(self) -> None: if self._runtime is not None: return try: @@ -123,13 +128,13 @@ def start(self) -> None: @rpc def stop(self) -> None: + if self._runtime is not None: + self._runtime.stop() self._runtime = None super().stop() @rpc def propose_grasps(self, object_pointcloud: PointCloud2) -> GraspCandidateArray: - if self._runtime is None: - raise GraspGenXError("GraspGenX module has not been started") if object_pointcloud.ts is None: raise ValueError("object pointcloud must have a timestamp") if not object_pointcloud.frame_id: @@ -141,6 +146,8 @@ def propose_grasps(self, object_pointcloud: PointCloud2) -> GraspCandidateArray: if not np.all(np.isfinite(points)): raise ValueError("object pointcloud XYZ values must be finite floats in metres") + self._ensure_runtime() + assert self._runtime is not None try: poses, scores = self._runtime.infer(points) except Exception as exc: diff --git a/dimos/manipulation/grasping/grasp_gen_x_runtime.py b/dimos/manipulation/grasping/grasp_gen_x_runtime.py index 34a76c7f2e..b7d89d6d52 100644 --- a/dimos/manipulation/grasping/grasp_gen_x_runtime.py +++ b/dimos/manipulation/grasping/grasp_gen_x_runtime.py @@ -12,14 +12,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""First-use GraspGenX runtime with top-level optional dependency imports.""" +"""First-use in-process GraspGenX runtime.""" from __future__ import annotations import os from pathlib import Path -from huggingface_hub import snapshot_download import numpy as np from dimos.manipulation.grasping.grasp_gen_x import ( @@ -29,37 +28,6 @@ GraspGenXConfig, ) -_snapshot_root = Path( - snapshot_download( - repo_id=GRASPGENX_MODEL_REPO, - revision=GRASPGENX_MODEL_REVISION, - allow_patterns=[ - f"{GRASPGENX_MODEL_VERSION}/gen/*", - f"{GRASPGENX_MODEL_VERSION}/dis/*", - ], - ) -).resolve() -_checkpoint_root = _snapshot_root / GRASPGENX_MODEL_VERSION -_gen_dir = _checkpoint_root / "gen" -_dis_dir = _checkpoint_root / "dis" -if not _gen_dir.is_dir() or not _dis_dir.is_dir(): - raise FileNotFoundError( - f"GraspGenX checkpoint must contain release/gen and release/dis: {_snapshot_root}" - ) - -# Upstream's package initializer otherwise performs Git clones while importing. The -# sweep-volume runtime does not consume named gripper assets, so the existing snapshot -# directory also suppresses that unused asset clone. -os.environ["GRASPGENX_CHECKPOINT_DIR"] = str(_snapshot_root) -os.environ["GRASPGENX_GRIPPER_CFG_DIR"] = str(_snapshot_root) - -from graspgenx.grasp_server import ( - SWEEP_VOLUME_ONLY_BACKBONES, - GraspGenXSampler, -) -from graspgenx.utils.checkpoint_io import load_model_cfg -from graspgenx.x_grippers import make_sweep_volume_gripper_info - _GRIPPER_TYPES = { "parallel_2f": 0, "revolute_2f": 1, @@ -71,7 +39,35 @@ class GraspGenXRuntime: """Loaded GraspGenX sampler and exact tensor conversion boundary.""" def __init__(self, config: GraspGenXConfig) -> None: - model_config = load_model_cfg(_gen_dir, _dis_dir, gen_pth=None, dis_pth=None) + from huggingface_hub import snapshot_download + + snapshot_root = Path( + snapshot_download( + repo_id=GRASPGENX_MODEL_REPO, + revision=GRASPGENX_MODEL_REVISION, + allow_patterns=[ + f"{GRASPGENX_MODEL_VERSION}/gen/*", + f"{GRASPGENX_MODEL_VERSION}/dis/*", + ], + ) + ).resolve() + checkpoint_root = snapshot_root / GRASPGENX_MODEL_VERSION + gen_dir = checkpoint_root / "gen" + dis_dir = checkpoint_root / "dis" + if not gen_dir.is_dir() or not dis_dir.is_dir(): + raise FileNotFoundError( + f"GraspGenX checkpoint must contain release/gen and release/dis: {snapshot_root}" + ) + + # Sweep-volume grippers do not use named gripper assets, so avoid the + # upstream package's on-import Git clone. + os.environ["GRASPGENX_CHECKPOINT_DIR"] = str(snapshot_root) + os.environ["GRASPGENX_GRIPPER_CFG_DIR"] = str(snapshot_root) + from graspgenx.grasp_server import SWEEP_VOLUME_ONLY_BACKBONES, GraspGenXSampler + from graspgenx.utils.checkpoint_io import load_model_cfg + from graspgenx.x_grippers import make_sweep_volume_gripper_info + + model_config = load_model_cfg(gen_dir, dis_dir, gen_pth=None, dis_pth=None) for component in ("diffusion", "discriminator"): backbone = getattr(model_config, component).gripper_backbone if backbone not in SWEEP_VOLUME_ONLY_BACKBONES: @@ -87,12 +83,14 @@ def __init__(self, config: GraspGenXConfig) -> None: gripper_type=_GRIPPER_TYPES[config.gripper.family], fingertip_depth=config.gripper.fingertip_depth, ) + self._sampler_type = GraspGenXSampler self._sampler = GraspGenXSampler(model_config, gripper_info=gripper_info) def infer(self, points: np.ndarray) -> tuple[np.ndarray, np.ndarray]: """Run inference and copy the known torch tensors to CPU NumPy arrays.""" - poses, scores = GraspGenXSampler.run_inference(points, self._sampler) - return ( - poses.detach().cpu().numpy(), - scores.detach().cpu().numpy(), - ) + poses, scores = self._sampler_type.run_inference(points, self._sampler) + return poses.detach().cpu().numpy(), scores.detach().cpu().numpy() + + def stop(self) -> None: + """Release the sampler when the module stops.""" + del self._sampler diff --git a/dimos/manipulation/grasping/test_grasp_gen_x.py b/dimos/manipulation/grasping/test_grasp_gen_x.py index 5621eeb711..dff678af88 100644 --- a/dimos/manipulation/grasping/test_grasp_gen_x.py +++ b/dimos/manipulation/grasping/test_grasp_gen_x.py @@ -107,6 +107,18 @@ def test_messages_round_trip_empty_and_score() -> None: ) +def test_candidate_array_lcm_round_trip() -> None: + value = GraspCandidateArray( + Header(3.0, "camera"), [GraspCandidate(Pose(1, 2, 3), 0.25)], selected_index=1 + ) + + decoded = GraspCandidateArray.lcm_decode(value.lcm_encode()) + + assert decoded.header.frame_id == "camera" + assert decoded.candidates[0].score == pytest.approx(0.25) + assert decoded.selected_index == 1 + + def test_ranked_spec_is_canonical_during_legacy_contract_transition() -> None: legacy_signature = inspect.signature(LegacyGraspGenSpec.generate_grasps) signature = inspect.signature(GraspGenSpec.propose_grasps) @@ -168,6 +180,7 @@ def test_start_is_synchronous_and_idempotent(runtime: Any) -> None: assert len(module.propose_grasps(cloud())) == 1 finally: module.stop() + runtime.return_value.stop.assert_called_once_with() def test_start_failure_is_explicit(runtime: Any) -> None: @@ -277,16 +290,15 @@ def test_inference_failure_is_wrapped(runtime: Any) -> None: module.stop() -def test_not_started_and_missing_metadata_are_rejected(runtime: Any) -> None: +def test_lazy_runtime_loading_and_missing_metadata_are_rejected(runtime: Any) -> None: module = GraspGenXModule(**module_args()) missing_frame = cloud() missing_frame.frame_id = "" missing_timestamp = cloud() missing_timestamp.ts = None try: - with pytest.raises(GraspGenXError, match="not been started"): - module.propose_grasps(cloud()) - module.start() + assert len(module.propose_grasps(cloud())) == 1 + runtime.assert_called_once_with(module.config) with pytest.raises(ValueError, match="frame_id"): module.propose_grasps(missing_frame) with pytest.raises(ValueError, match="timestamp"): diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 405956f145..f0431a74be 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -25,6 +25,7 @@ from __future__ import annotations from collections.abc import Mapping, Sequence +from dataclasses import dataclass from enum import Enum import math import threading @@ -90,6 +91,7 @@ NoManipulationVisualizationConfig, ) from dimos.manipulation.visualization.factory import create_manipulation_visualization +from dimos.manipulation.visualization.layers import VisualizationLayer from dimos.manipulation.visualization.operator import ManipulationOperator from dimos.manipulation.visualization.types import TargetEvaluation from dimos.msgs.geometry_msgs.Pose import Pose @@ -136,6 +138,15 @@ class ManipulationState(Enum): FAULT = 4 +@dataclass(frozen=True) +class ConnectedPoseSequenceResult: + """Motion-free result for an ordered sequence of pose plans.""" + + failed_index: int | None + endpoint: JointState | None + paths: tuple[tuple[JointState, ...], ...] + + class ManipulationModuleConfig(ModuleConfig): """Configuration for ManipulationModule.""" @@ -998,6 +1009,14 @@ def plan_to_pose(self, pose: Pose, robot_name: RobotName | None = None) -> bool: return False return self.plan_to_pose_targets({group_id: pose}) + @rpc + def set_visualization_layer(self, layer: VisualizationLayer) -> bool: + """Replace one display-only layer in the active manipulation visualizer.""" + if self._world_monitor is None or self._world_monitor.visualization is None: + return False + self._world_monitor.visualization.set_layer(layer) + return True + @rpc def plan_to_pose_targets( self, @@ -1122,6 +1141,152 @@ def generate_plan_to_pose_targets( logger.info(f"IK solved, error: {ik.position_error:.4f}m") return self._plan_selected_path(group_ids, start, ik.joint_state, planning_epoch) + def _check_connected_pose_sequence( + self, + poses: Sequence[Pose], + robot_name: RobotName, + start: JointState | None = None, + ) -> tuple[int | None, JointState | None]: + """Dry-run a connected pose sequence and return failure index and endpoint. + + The check does not store a plan or change module state. Each IK solve and + path plan starts at the preceding path's endpoint. When ``start`` is + omitted, the sequence begins at the robot's current authoritative state. + """ + result = self._plan_connected_pose_sequence(poses, robot_name, start) + return result.failed_index, result.endpoint + + def _plan_connected_pose_sequence( + self, + poses: Sequence[Pose], + robot_name: RobotName, + start: JointState | None = None, + ) -> ConnectedPoseSequenceResult: + """Dry-run connected pose plans and retain each successful segment path.""" + if not poses: + return ConnectedPoseSequenceResult(None, start, ()) + if self._world_monitor is None or self._kinematics is None or self._planner is None: + logger.warning("Connected pose planning is unavailable") + return ConnectedPoseSequenceResult(0, None, ()) + try: + group_id = self._require_unique_pose_group_id_for_robot(robot_name) + selection = self._world_monitor.planning_groups.select((group_id,)) + if start is None: + current = self._world_monitor.current_global_joint_state() + start = filter_joint_state_to_selected_joints(current, selection.joint_names) + else: + start = filter_joint_state_to_selected_joints(start, selection.joint_names) + except (KeyError, ValueError) as exc: + logger.warning("Failed to initialize connected pose planning: %s", exc) + return ConnectedPoseSequenceResult(0, None, ()) + + paths: list[tuple[JointState, ...]] = [] + for index, pose in enumerate(poses): + target = PoseStamped( + frame_id="world", + position=pose.position, + orientation=pose.orientation, + ) + ik = self.inverse_kinematics( + pose_targets={group_id: target}, + seed=start, + check_collision=True, + ) + if not ik.is_success() or ik.joint_state is None: + logger.info( + "Connected pose planning failed IK at index %d: %s%s", + index, + ik.status.name, + f": {ik.message}" if ik.message else "", + ) + return ConnectedPoseSequenceResult(index, None, tuple(paths)) + result = self._planner.plan_selected_joint_path( + world=self._world_monitor.world, + selection=selection, + start=start, + goal=ik.joint_state, + timeout=self.config.planning_timeout, + ) + if not result.is_success() or not result.path: + logger.info( + "Connected pose planning failed path at index %d: %s%s", + index, + result.status.name, + f": {result.message}" if result.message else "", + ) + return ConnectedPoseSequenceResult(index, None, tuple(paths)) + try: + start = filter_joint_state_to_selected_joints( + result.path[-1], selection.joint_names + ) + except ValueError as exc: + logger.info( + "Connected pose planning returned an invalid endpoint at index %d: %s", + index, + exc, + ) + return ConnectedPoseSequenceResult(index, None, tuple(paths)) + paths.append(tuple(result.path)) + return ConnectedPoseSequenceResult(None, start, tuple(paths)) + + @rpc + def plan_cartesian_targets( + self, + targets: Mapping[PlanningGroupID | PlanningGroup, CartesianTarget], + config: CartesianPathConfig, + auxiliary_groups: Sequence[PlanningGroupID | PlanningGroup] = (), + ) -> bool: + """Plan TCP motion through absolute or relative Cartesian waypoints.""" + return self.generate_cartesian_plan(targets, config, auxiliary_groups) is not None + + def generate_cartesian_plan( + self, + targets: Mapping[PlanningGroupID | PlanningGroup, CartesianTarget], + config: CartesianPathConfig, + auxiliary_groups: Sequence[PlanningGroupID | PlanningGroup] = (), + ) -> GeneratedPlan | None: + """Generate and store a timed Cartesian plan through PlannerSpec.""" + if self._world_monitor is None or self._planner is None: + return None + if not targets: + self._fail("At least one Cartesian target is required") + return None + normalized_targets = { + planning_group_id_from_selector(group): target for group, target in targets.items() + } + if len(normalized_targets) != len(targets): + self._fail("Cartesian target groups must be unique") + return None + auxiliary_ids = tuple(planning_group_id_from_selector(group) for group in auxiliary_groups) + group_ids = tuple((*normalized_targets.keys(), *auxiliary_ids)) + planning_epoch = self._begin_group_planning() + if planning_epoch is None: + return None + resolved = self._resolve_group_plan_start(group_ids, planning_epoch) + if resolved is None: + return None + selection, start = resolved + result = self._planner.plan_cartesian_path( + world=self._world_monitor.world, + selection=selection, + start=start, + targets=normalized_targets, + config=config, + auxiliary_groups=auxiliary_ids, + ) + if not result.is_success(): + detail = f": {result.message}" if result.message else "" + self._fail_planning_epoch( + planning_epoch, f"Cartesian planning failed: {result.status.name}{detail}" + ) + return None + return self._store_generated_plan( + group_ids, + result, + planning_epoch, + preserve_timing=True, + ) + @rpc def plan_cartesian_targets( self, @@ -1541,6 +1706,11 @@ def execute_plan(self, plan: GeneratedPlan | None = None) -> bool: self._error_message = result.message return bool(result and result.accepted) + @rpc + def execute_and_wait(self, timeout: float = 60.0) -> bool: + """Execute the stored plan and wait for its expected trajectory duration.""" + return self.execute_plan() and self._wait_for_trajectory_completion(timeout) + @property def world_monitor(self) -> WorldMonitor | None: """Access the world monitor for advanced obstacle/world operations.""" @@ -1578,6 +1748,41 @@ def add_obstacle( ) return self._world_monitor.add_obstacle(obstacle) + @skill(uses=["movement"]) + def set_table_collision( + self, + center_x: float, + center_y: float, + tabletop_z: float, + width: float, + depth: float, + safety_margin: float = 0.0, + thickness: float = 0.20, + ) -> bool: + """Install or update a horizontal table collision slab. + + All dimensions are meters. ``tabletop_z`` is the measured physical tabletop height. The default + uses no added clearance so a grasp can descend to an object's measured contact pose. + """ + if self._world_monitor is None: + return False + if width <= 0.0 or depth <= 0.0 or thickness <= 0.0 or safety_margin < 0.0: + raise ValueError("table dimensions must be positive and safety_margin non-negative") + protected_top = tabletop_z + safety_margin + table = Obstacle( + name="calibrated-table", + obstacle_type=ObstacleType.BOX, + pose=PoseStamped( + position=Vector3(center_x, center_y, protected_top - thickness / 2), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + ), + dimensions=(width, depth, thickness), + color=(0.2, 0.5, 0.9, 0.35), + ) + if self._world_monitor.update_obstacle(table): + return True + return bool(self._world_monitor.add_obstacle(table)) + @rpc def update_obstacle( self, @@ -1710,24 +1915,43 @@ def _wait_for_trajectory_completion(self, timeout: float = 60.0) -> bool: time.sleep(wait_time) return True - def _lift_if_low( + def _safety_lift_pose( self, robot_name: RobotName | None = None, min_z: float = 0.05 - ) -> SkillResult[ManipulationSkillError]: - """If the end-effector is below *min_z*, plan and execute a short lift.""" + ) -> Pose | None: + """Return the required safety-lift target, if the end effector is low.""" ee = self.get_ee_pose(robot_name) if ee is None or ee.position.z >= min_z: - return SkillResult.ok() + return None lift_z = min_z + 0.05 logger.info(f"EE z={ee.position.z:.3f} < {min_z}, lifting to z={lift_z:.3f}") - lift_pose = Pose(Vector3(ee.position.x, ee.position.y, lift_z), ee.orientation) + return Pose(Vector3(ee.position.x, ee.position.y, lift_z), ee.orientation) + + def _lift_if_low( + self, robot_name: RobotName | None = None, min_z: float = 0.05 + ) -> SkillResult[ManipulationSkillError]: + """If the end-effector is below *min_z*, plan and execute a short lift.""" + lift_pose = self._safety_lift_pose(robot_name, min_z) + if lift_pose is None: + return SkillResult.ok() if not self.plan_to_pose(lift_pose, robot_name): return SkillResult.fail( "PLANNING_FAILED", - f"Failed to plan lift from z={ee.position.z:.3f}", + f"Failed to plan safety lift to z={lift_pose.position.z:.3f}", ) return self._preview_execute_wait(robot_name) + def _planner_fault_result(self) -> SkillResult[ManipulationSkillError] | None: + """Return an actionable failure while the planner requires recovery.""" + with self._lock: + if self._state != ManipulationState.FAULT: + return None + detail = self._error_message or "unknown planner error" + return SkillResult.fail( + "INVALID_STATE", + f"Planner is FAULT ({detail}). Call reset before issuing another motion command.", + ) + def _preview_execute_wait( self, robot_name: RobotName | None = None, preview_duration: float = 0.5 ) -> SkillResult[ManipulationSkillError]: @@ -1806,6 +2030,9 @@ def move_to_pose( yaw: Target yaw in radians (omit to keep current orientation). robot_name: Robot to move (only needed for multi-arm setups). """ + if fault := self._planner_fault_result(): + return fault + logger.info(f"Planning motion to ({x:.3f}, {y:.3f}, {z:.3f})...") # If no orientation specified, preserve the current EE orientation. @@ -1863,6 +2090,9 @@ def move_to_joints( joints: Comma-separated joint positions in radians, e.g. "0.1, -0.5, 1.2, 0.0, 0.3, -0.1". robot_name: Robot to move (only needed for multi-arm setups). """ + if fault := self._planner_fault_result(): + return fault + try: joint_values = [float(j.strip()) for j in joints.split(",")] except ValueError: @@ -1899,6 +2129,9 @@ def go_home(self, robot_name: str | None = None) -> SkillResult[ManipulationSkil Args: robot_name: Robot to move (only needed for multi-arm setups). """ + if fault := self._planner_fault_result(): + return fault + robot = self._get_robot(robot_name) if robot is None: return SkillResult.fail("ROBOT_NOT_FOUND", "Robot not found") @@ -1935,6 +2168,9 @@ def go_init(self, robot_name: str | None = None) -> SkillResult[ManipulationSkil Args: robot_name: Robot to move (only needed for multi-arm setups). """ + if fault := self._planner_fault_result(): + return fault + robot = self._get_robot(robot_name) if robot is None: return SkillResult.fail("ROBOT_NOT_FOUND", "Robot not found") diff --git a/dimos/manipulation/obstacle_world_spec.py b/dimos/manipulation/obstacle_world_spec.py new file mode 100644 index 0000000000..f0870ee7fb --- /dev/null +++ b/dimos/manipulation/obstacle_world_spec.py @@ -0,0 +1,43 @@ +# 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. + +"""Planner obstacle mutation protocol for scene-derived geometry.""" + +from typing import Literal, Protocol + +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.spec.utils import Spec + + +class ObstacleWorldSpec(Spec, Protocol): + def add_obstacle( + self, + name: str, + pose: Pose, + shape: Literal["box", "sphere", "cylinder", "mesh"], + dimensions: list[float] | None = None, + mesh_path: str | None = None, + ) -> str: ... + + def update_obstacle( + self, + name: str, + pose: Pose, + shape: Literal["box", "sphere", "cylinder", "mesh"], + dimensions: list[float] | None = None, + mesh_path: str | None = None, + color: list[float] | None = None, + ) -> bool: ... + + def remove_obstacle(self, obstacle_id: str) -> bool: ... diff --git a/dimos/manipulation/pick_and_place_module.py b/dimos/manipulation/pick_and_place_module.py index c788277b94..6394a43217 100644 --- a/dimos/manipulation/pick_and_place_module.py +++ b/dimos/manipulation/pick_and_place_module.py @@ -22,14 +22,22 @@ from __future__ import annotations +from collections import Counter +from dataclasses import dataclass, field +from enum import Enum import math +import threading import time -from typing import TYPE_CHECKING, Any +from typing import TYPE_CHECKING, Any, Literal + +import numpy as np +from pydantic import Field, FiniteFloat, model_validator from dimos.agents.annotation import skill from dimos.agents.skill_result import SkillResult from dimos.core.core import rpc from dimos.core.stream import In +from dimos.manipulation.grasping.grasp_gen_spec import GraspGenSpec from dimos.manipulation.manipulation_module import ( ManipulationModule, ManipulationModuleConfig, @@ -38,13 +46,18 @@ from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.manipulation_msgs.GraspCandidate import GraspCandidate from dimos.perception.experimental.object import ( Object as DetObject, ) +from dimos.perception.experimental.object_scene_registration_spec import ObjectSceneRegistrationSpec +from dimos.protocol.service.spec import BaseConfig from dimos.utils.logging_config import setup_logger +from dimos.utils.transform_utils import offset_distance if TYPE_CHECKING: from dimos.msgs.geometry_msgs.PoseArray import PoseArray + from dimos.msgs.sensor_msgs.JointState import JointState from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 logger = setup_logger() @@ -62,9 +75,102 @@ _TALL_OBJECT_MIN_HEIGHT = 0.06 +class GraspVerificationConfig(BaseConfig): + """Robot-specific gripper closure verification settings.""" + + enabled: bool = False + open_position: FiniteFloat = 0.85 + closed_position: FiniteFloat = 0.0 + held_threshold: FiniteFloat = 0.02 + timeout: FiniteFloat = Field(default=2.0, gt=0.0) + poll_interval: FiniteFloat = Field(default=0.05, gt=0.0) + + @model_validator(mode="after") + def _validate_threshold(self) -> GraspVerificationConfig: + low = min(self.open_position, self.closed_position) + high = max(self.open_position, self.closed_position) + if self.open_position == self.closed_position: + raise ValueError("gripper open_position and closed_position must differ") + if not low < self.held_threshold < high: + raise ValueError("held_threshold must lie between open_position and closed_position") + if self.poll_interval > self.timeout: + raise ValueError("poll_interval must not exceed timeout") + return self + + class PickAndPlaceModuleConfig(ManipulationModuleConfig): """Configuration for PickAndPlaceModule.""" + heuristic_grasp_fallback: bool = False + planning_frame: str = "world" + max_object_pointcloud_age: FiniteFloat = Field(default=10.0, gt=0.0) + max_grasp_candidates_to_check: int = Field(default=5, gt=0) + 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) + grasp_verification: GraspVerificationConfig = Field(default_factory=GraspVerificationConfig) + + @model_validator(mode="after") + def _validate_grasp_pipeline(self) -> PickAndPlaceModuleConfig: + if not self.planning_frame.strip(): + raise ValueError("planning_frame must not be empty") + vector = np.asarray(self.grasp_approach_vector, dtype=float) + if not np.isclose(np.linalg.norm(vector), 1.0, atol=1e-6): + raise ValueError("grasp_approach_vector must be a unit vector") + return self + + +class _PickPhase(str, Enum): + RESOLVE = "RESOLVE" + PROPOSE = "PROPOSE" + SELECT = "SELECT" + PREPARE = "PREPARE" + APPROACH = "APPROACH" + GRASP = "GRASP" + CLOSE = "CLOSE" + VERIFY = "VERIFY" + RETREAT = "RETREAT" + DONE = "DONE" + + +class _CandidateRejection(str, Enum): + INVALID = "invalid" + PRE_GRASP_INFEASIBLE = "pre_grasp_infeasible" + GRASP_INFEASIBLE = "grasp_infeasible" + RETREAT_INFEASIBLE = "retreat_infeasible" + + +@dataclass(frozen=True) +class _FeasibleGrasp: + candidate: GraspCandidate + rank: int + pre_grasp_pose: Pose + retreat_pose: Pose + + +@dataclass(frozen=True) +class _GraspVerification: + held: bool + position: float | None + detail: str + + +@dataclass +class _PickTransaction: + object_id: str = "" + object_name: str = "" + proposal_source: Literal["grasp_provider", "heuristic"] = "grasp_provider" + phase: _PickPhase = _PickPhase.RESOLVE + selected: _FeasibleGrasp | None = None + rejections: Counter[str] = field(default_factory=Counter) + gripper_closed: bool = False + + +class _PickPipelineError(RuntimeError): + def __init__(self, code: ManipulationSkillError, message: str) -> None: + super().__init__(message) + self.code = code + class PickAndPlaceModule(ManipulationModule): """Manipulation module with perception integration and pick-and-place skills. @@ -76,6 +182,8 @@ class PickAndPlaceModule(ManipulationModule): """ config: PickAndPlaceModuleConfig + _object_scene: ObjectSceneRegistrationSpec | None = None + _grasp_generator: GraspGenSpec | None = None # Input: Objects from perception (for obstacle integration) objects: In[list[DetObject]] @@ -90,6 +198,7 @@ def __init__(self, **kwargs: Any) -> None: # The live detection cache is volatile (labels change every frame), # so pick/place use this stable snapshot instead. self._detection_snapshot: list[DetObject] = [] + self._pick_guard = threading.Lock() @rpc def start(self) -> None: @@ -177,7 +286,12 @@ def generate_grasps( "GraspGen Docker support removed; see issue #1266 for re-implementation as NativeModule subclass" ) - def _compute_pre_grasp_pose(self, grasp_pose: Pose, offset: float = 0.10) -> Pose: + def _compute_pre_grasp_pose( + self, + grasp_pose: Pose, + offset: float = 0.10, + approach_vector: Vector3 | None = None, + ) -> Pose: """Compute a pre-grasp pose offset along the approach direction (local -Z). Args: @@ -187,9 +301,11 @@ def _compute_pre_grasp_pose(self, grasp_pose: Pose, offset: float = 0.10) -> Pos Returns: Pre-grasp pose offset from the grasp pose """ - from dimos.utils.transform_utils import offset_distance - - return offset_distance(grasp_pose, offset) + return offset_distance( + grasp_pose, + offset, + approach_vector if approach_vector is not None else Vector3(0.0, 0.0, -1.0), + ) def _find_object_in_detections( self, object_name: str, object_id: str | None = None @@ -224,10 +340,19 @@ def _find_object_in_detections( logger.warning(f"Ambiguous object_id prefix '{object_id}' matches {ids}") return None - # Second pass: match by name - for det in self._detection_snapshot: - if object_name.lower() in det.name.lower() or det.name.lower() in object_name.lower(): - return det + # Second pass: require a unique name match. + normalized = object_name.casefold() + name_matches = [ + det + for det in self._detection_snapshot + if normalized in det.name.casefold() or det.name.casefold() in normalized + ] + if len(name_matches) == 1: + return name_matches[0] + if len(name_matches) > 1: + ids = [det.object_id for det in name_matches] + logger.warning("Ambiguous object name", object_name=object_name, object_ids=ids) + return None available = [det.name for det in self._detection_snapshot] logger.warning(f"Object '{object_name}' not found in snapshot. Available: {available}") @@ -454,7 +579,8 @@ def scan_objects( for det in detections: c = det.center lines.append( - f" - {det.name}: ({c.x:.3f}, {c.y:.3f}, {c.z:.3f}) [{det.detections_count} views]" + f" - {det.name} [id={det.object_id[:8]}]: " + f"({c.x:.3f}, {c.y:.3f}, {c.z:.3f}) [{det.detections_count} views]" ) if obstacles: @@ -462,6 +588,282 @@ def scan_objects( return SkillResult.ok("\n".join(lines)) + def _require_pick_object(self, object_name: str, object_id: str | None) -> DetObject: + detection = self._find_object_in_detections(object_name, object_id) + if detection is not None: + return detection + selector = f"id '{object_id}'" if object_id else f"name '{object_name}'" + raise _PickPipelineError( + "OBJECT_NOT_DETECTED", + f"No unique current detection matches {selector}; scan again and use an object ID", + ) + + def _provider_candidates( + self, detection: DetObject, transaction: _PickTransaction + ) -> list[GraspCandidate]: + if self._grasp_generator is None: + if not self.config.heuristic_grasp_fallback: + raise _PickPipelineError( + "GRASP_PROVIDER_UNAVAILABLE", + "No grasp proposal provider is connected and heuristic fallback is disabled", + ) + transaction.proposal_source = "heuristic" + poses = self._generate_grasps_for_pick(detection.name, detection.object_id) + if not poses: + raise _PickPipelineError( + "GRASP_GENERATION_FAILED", + f"Heuristic grasp generation failed for '{detection.name}'", + ) + return [GraspCandidate(pose=pose, score=0.0) for pose in poses] + + if self._object_scene is None: + raise _PickPipelineError( + "GRASP_PROVIDER_UNAVAILABLE", + "No object-scene provider is connected for learned grasp input", + ) + + pointcloud = self._object_scene.get_object_pointcloud_by_object_id(detection.object_id) + if pointcloud is None: + raise _PickPipelineError( + "GRASP_INPUT_INVALID", + f"No point cloud is available for object '{detection.object_id}'", + ) + points = pointcloud.points_f32() + if points.ndim != 2 or points.shape[1] != 3 or len(points) == 0: + raise _PickPipelineError( + "GRASP_INPUT_INVALID", + f"Object '{detection.object_id}' has an empty or invalid point cloud", + ) + if ( + pointcloud.ts is None + or time.time() - pointcloud.ts > self.config.max_object_pointcloud_age + ): + raise _PickPipelineError( + "GRASP_INPUT_INVALID", + f"Object '{detection.object_id}' point cloud is stale", + ) + if pointcloud.frame_id != self.config.planning_frame: + raise _PickPipelineError( + "GRASP_FRAME_MISMATCH", + f"Object cloud frame '{pointcloud.frame_id}' does not match " + f"planning frame '{self.config.planning_frame}'", + ) + + try: + proposals = self._grasp_generator.propose_grasps(pointcloud) + except Exception as exc: + raise _PickPipelineError( + "GRASP_GENERATION_FAILED", f"Grasp proposal failed: {exc}" + ) from exc + if proposals.header.frame_id != self.config.planning_frame: + raise _PickPipelineError( + "GRASP_FRAME_MISMATCH", + f"Proposal frame '{proposals.header.frame_id}' does not match " + f"planning frame '{self.config.planning_frame}'", + ) + if not proposals.candidates: + raise _PickPipelineError( + "GRASP_GENERATION_FAILED", + f"No grasp proposals were generated for '{detection.name}'", + ) + return sorted(proposals.candidates, key=lambda candidate: candidate.score, reverse=True) + + @staticmethod + def _valid_candidate(candidate: GraspCandidate) -> bool: + pose = candidate.pose + values = np.asarray( + [ + pose.position.x, + pose.position.y, + pose.position.z, + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + candidate.score, + ], + dtype=float, + ) + quaternion = values[3:7] + return bool( + np.all(np.isfinite(values)) and np.isclose(np.linalg.norm(quaternion), 1.0, atol=1e-5) + ) + + def _select_feasible_grasp( + self, + candidates: list[GraspCandidate], + robot_name: str, + robot_pre_grasp_offset: float, + transaction: _PickTransaction, + sequence_start: JointState | None = None, + ) -> _FeasibleGrasp: + vector = Vector3(self.config.grasp_approach_vector) + pre_offset = self.config.grasp_pre_grasp_offset or robot_pre_grasp_offset + retreat_offset = self.config.grasp_retreat_offset or pre_offset + limit = min(len(candidates), self.config.max_grasp_candidates_to_check) + + for rank, candidate in enumerate(candidates[:limit], start=1): + if not self._valid_candidate(candidate): + transaction.rejections[_CandidateRejection.INVALID.value] += 1 + continue + pre_grasp = self._compute_pre_grasp_pose(candidate.pose, pre_offset, vector) + retreat = self._compute_pre_grasp_pose(candidate.pose, retreat_offset, 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, + ) + if failed_index is not None: + transaction.rejections[rejections[failed_index].value] += 1 + continue + return _FeasibleGrasp(candidate, rank, pre_grasp, retreat) + + summary = ", ".join( + f"{reason}={count}" for reason, count in sorted(transaction.rejections.items()) + ) + raise _PickPipelineError( + "GRASP_ATTEMPTS_EXHAUSTED", + f"No feasible grasp among {limit} candidate(s)" + (f" ({summary})" if summary else ""), + ) + + def _verify_grasp(self, robot_name: str) -> _GraspVerification: + verification = self.config.grasp_verification + if not verification.enabled: + return _GraspVerification(True, None, "gripper feedback verification disabled") + + deadline = time.monotonic() + verification.timeout + last_position: float | None = None + while time.monotonic() < deadline: + last_position = self.get_gripper(robot_name) + if last_position is not None: + closes_upward = verification.closed_position > verification.open_position + empty = ( + last_position >= verification.held_threshold + if closes_upward + else last_position <= verification.held_threshold + ) + if empty: + return _GraspVerification( + False, last_position, "gripper reached the empty-closed region" + ) + time.sleep(verification.poll_interval) + + if last_position is None: + return _GraspVerification(False, None, "gripper feedback was unavailable") + movement = abs(last_position - verification.open_position) + if movement < 1e-3: + return _GraspVerification( + False, last_position, "gripper did not leave the open position" + ) + closes_upward = verification.closed_position > verification.open_position + held = ( + last_position < verification.held_threshold + if closes_upward + else last_position > verification.held_threshold + ) + detail = ( + "grasp verified by gripper closure feedback" + if held + else "gripper reached the empty-closed region" + ) + return _GraspVerification(held, last_position, detail) + + @staticmethod + def _phase_failure( + transaction: _PickTransaction, + code: ManipulationSkillError, + message: str, + ) -> SkillResult[ManipulationSkillError]: + may_hold = transaction.gripper_closed + suffix = "; object may be held" if may_hold else "" + result = SkillResult[ManipulationSkillError].fail( + code, f"{transaction.phase.value}: {message}{suffix}" + ) + result.metadata = { + "phase": transaction.phase.value, + "object_id": transaction.object_id, + "proposal_source": transaction.proposal_source, + "object_may_be_held": may_hold, + "rejections": dict(transaction.rejections), + } + if transaction.selected is not None: + result.metadata.update( + candidate_rank=transaction.selected.rank, + candidate_score=transaction.selected.candidate.score, + ) + return result + + def _execute_selected_pick( + self, transaction: _PickTransaction, robot_name: str + ) -> SkillResult[ManipulationSkillError]: + assert transaction.selected is not None + selected = transaction.selected + verification = self.config.grasp_verification + + transaction.phase = _PickPhase.PREPARE + lift = self._lift_if_low(robot_name) + if not lift.is_success(): + return self._phase_failure( + transaction, lift.error_code or "EXECUTION_FAILED", lift.message + ) + if not self._set_gripper_position(float(verification.open_position), robot_name): + return self._phase_failure(transaction, "GRIPPER_FAILED", "open command failed") + + transaction.phase = _PickPhase.APPROACH + if not self.plan_to_pose(selected.pre_grasp_pose, robot_name): + return self._phase_failure(transaction, "PLANNING_FAILED", "pre-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.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.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.DONE + self._last_pick_pose = selected.candidate.pose + return SkillResult.ok( + f"Pick complete — grasped '{transaction.object_name}' using candidate " + f"{selected.rank} (score={selected.candidate.score:.4f}); {verified.detail}", + object_id=transaction.object_id, + proposal_source=transaction.proposal_source, + candidate_rank=selected.rank, + candidate_score=selected.candidate.score, + verification=verified.detail, + rejections=dict(transaction.rejections), + ) + @skill def pick( self, @@ -479,80 +881,64 @@ def pick( object_id: Optional unique object ID from perception for precise identification. robot_name: Robot to use (only needed for multi-arm setups). """ - robot = self._get_robot(robot_name) - if robot is None: - return SkillResult.fail("ROBOT_NOT_FOUND", "Robot not found") - rname, _, config, _ = robot - pre_grasp_offset = config.pre_grasp_offset - - # 1. Generate grasps (uses already-cached detections — call scan_objects first) - logger.info(f"Generating grasp poses for '{object_name}'...") - grasp_poses = self._generate_grasps_for_pick(object_name, object_id) - if not grasp_poses: - return SkillResult.fail( - "GRASP_GENERATION_FAILED", - f"No grasp poses found for '{object_name}'. Object may not be detected.", - ) + if not self._pick_guard.acquire(blocking=False): + return SkillResult.fail("PICK_BUSY", "Another pick transaction is active") - # Lift if EE is low before approaching - lift = self._lift_if_low(rname) - if not lift.is_success(): - return lift - - # 2. Try each grasp candidate - max_attempts = min(len(grasp_poses), 5) - for i, grasp_pose in enumerate(grasp_poses[:max_attempts]): - # Reduce pre-grasp height for far objects (arm can't reach high + far) - gp = grasp_pose.position - xy_dist = (gp.x**2 + gp.y**2) ** 0.5 - offset = pre_grasp_offset if xy_dist < _FAR_REACH_XY_THRESHOLD else 0.05 - pre_grasp_pose = self._compute_pre_grasp_pose(grasp_pose, offset) - - logger.info(f"Planning approach to pre-grasp (attempt {i + 1}/{max_attempts})...") - if not self.plan_to_pose(pre_grasp_pose, rname): - logger.info(f"Grasp candidate {i + 1} approach planning failed, trying next") - continue # Try next candidate - - # 3. Open gripper before approach - logger.info("Opening gripper...") - self._set_gripper_position(0.85, rname) - time.sleep(0.5) - - # 4. Execute approach to pre-grasp - exec_result = self._preview_execute_wait(rname) - if not exec_result.is_success(): - return exec_result - - # 5. Move to grasp pose - logger.info("Moving to grasp position...") - if not self.plan_to_pose(grasp_pose, rname): - return SkillResult.fail("PLANNING_FAILED", "Grasp pose planning failed") - exec_result = self._preview_execute_wait(rname) - if not exec_result.is_success(): - return exec_result - - # 6. Close gripper - logger.info("Closing gripper...") - self._set_gripper_position(0.0, rname) - time.sleep(1.5) # Wait for gripper to close - - # 7. Retract to pre-grasp - logger.info("Retracting with object...") - if not self.plan_to_pose(pre_grasp_pose, rname): - return SkillResult.fail("PLANNING_FAILED", "Retract planning failed") - exec_result = self._preview_execute_wait(rname) - if not exec_result.is_success(): - return exec_result - - # Store pick pose so place_back() can return with same orientation - self._last_pick_pose = grasp_pose - - return SkillResult.ok(f"Pick complete — grasped '{object_name}' successfully") - - return SkillResult.fail( - "GRASP_ATTEMPTS_EXHAUSTED", - f"All {max_attempts} grasp attempts failed for '{object_name}'", - ) + transaction = _PickTransaction() + suppression = None + result: SkillResult[ManipulationSkillError] + try: + robot = self._get_robot(robot_name) + if robot is None: + return SkillResult.fail("ROBOT_NOT_FOUND", "Robot not found") + rname, _, robot_config, _ = robot + + detection = self._require_pick_object(object_name, object_id) + transaction.object_id = detection.object_id + transaction.object_name = detection.name + transaction.phase = _PickPhase.PROPOSE + candidates = self._provider_candidates(detection, transaction) + + if self._world_monitor is None: + raise _PickPipelineError( + "WORLD_MONITOR_UNAVAILABLE", "Planning world monitor is unavailable" + ) + + with self._world_monitor.suppress_object_obstacle(detection.object_id) as suppression: + sequence_start = None + lift_pose = self._safety_lift_pose(rname) + if lift_pose is not None: + transaction.phase = _PickPhase.PREPARE + failed_index, sequence_start = self._check_connected_pose_sequence( + (lift_pose,), rname + ) + if failed_index is not None: + raise _PickPipelineError( + "PLANNING_FAILED", + "Required safety-lift planning failed", + ) + transaction.phase = _PickPhase.SELECT + transaction.selected = self._select_feasible_grasp( + candidates, + rname, + robot_config.pre_grasp_offset, + transaction, + sequence_start, + ) + result = self._execute_selected_pick(transaction, rname) + if suppression.cleanup_error is not None: + if result.is_success(): + return self._phase_failure( + transaction, "WORLD_MONITOR_UNAVAILABLE", suppression.cleanup_error + ) + result.message = f"{result.message}; cleanup: {suppression.cleanup_error}" + return result + except _PickPipelineError as exc: + return self._phase_failure(transaction, exc.code, str(exc)) + except RuntimeError as exc: + return self._phase_failure(transaction, "WORLD_MONITOR_UNAVAILABLE", str(exc)) + finally: + self._pick_guard.release() @skill def place( diff --git a/dimos/manipulation/pick_execution_spec.py b/dimos/manipulation/pick_execution_spec.py new file mode 100644 index 0000000000..e09236f39d --- /dev/null +++ b/dimos/manipulation/pick_execution_spec.py @@ -0,0 +1,42 @@ +# 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. + +"""Manipulation operations used by the selected-object pick transaction.""" + +from typing import Protocol + +from dimos.agents.skill_result import SkillResult +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.spec.utils import Spec + + +class PickExecutionSpec(Spec, Protocol): + def open_gripper(self, robot_name: str | None = None) -> SkillResult: ... + + def close_gripper(self, robot_name: str | None = None) -> SkillResult: ... + + def get_gripper(self, robot_name: str | None = None) -> float | None: ... + + def get_ee_pose(self, robot_name: str | None = None) -> Pose | None: ... + + def move_to_pose( + self, + x: float, + y: float, + z: float, + roll: float | None = None, + pitch: float | None = None, + yaw: float | None = None, + robot_name: str | None = None, + ) -> SkillResult: ... diff --git a/dimos/manipulation/picknplace.py b/dimos/manipulation/picknplace.py new file mode 100644 index 0000000000..8fc5bb5054 --- /dev/null +++ b/dimos/manipulation/picknplace.py @@ -0,0 +1,936 @@ +# 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. + +"""Request-driven perception interface for the pick-and-place workflow.""" + +import math +import threading +import time +from typing import Literal + +import numpy as np +from pydantic import AliasChoices, Field + +from dimos.agents.annotation import skill +from dimos.agents.capabilities import CAP_MOVEMENT, CAP_PERCEPTION +from dimos.agents.skill_result import SkillResult +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +from dimos.manipulation.candidate_filter_spec import GraspCandidateFilterSpec +from dimos.manipulation.grasping.grasp_gen_spec import GraspGenSpec +from dimos.manipulation.obstacle_world_spec import ObstacleWorldSpec +from dimos.manipulation.pick_execution_spec import PickExecutionSpec +from dimos.manipulation.visualization.layers import ( + LineSetElement, + MeshElement, + PointCloudElement, + VisualizationLayer, +) +from dimos.manipulation.visualization.pose_overlay import draw_pose_axes +from dimos.manipulation.visualization_spec import ManipulationVisualizationSpec +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.perception.experimental.object import ( + Object as DetObject, + to_detection3d_array, +) +from dimos.perception.experimental.object_scene_registration_spec import ObjectSceneRegistrationSpec + + +def _estimate_table_surface(points: np.ndarray) -> dict[str, float] | None: + """Fit the dominant horizontal support plane and return a conservative footprint.""" + if points.ndim != 2 or points.shape[1] != 3 or len(points) < 30: + return None + import open3d as o3d # type: ignore[import-untyped] + + cloud = o3d.geometry.PointCloud() + cloud.points = o3d.utility.Vector3dVector(points) + plane, inliers = cloud.segment_plane(distance_threshold=0.01, ransac_n=3, num_iterations=1000) + normal = np.asarray(plane[:3], dtype=np.float64) + normal /= np.linalg.norm(normal) + if abs(normal[2]) < 0.98 or len(inliers) < 30: + return None + surface = points[np.asarray(inliers)] + x_low, y_low = np.quantile(surface[:, :2], 0.02, axis=0) + x_high, y_high = np.quantile(surface[:, :2], 0.98, axis=0) + # Extend the observed tabletop patch so collision protection includes its edges. + margin = 0.10 + return { + "center_x": float((x_low + x_high) / 2), + "center_y": float((y_low + y_high) / 2), + "tabletop_z": float(np.median(surface[:, 2])), + "width": float(max(x_high - x_low + 2 * margin, 0.20)), + "depth": float(max(y_high - y_low + 2 * margin, 0.20)), + "inlier_count": float(len(inliers)), + } + + +def _table_midpoint_grasp_z( + points: np.ndarray, tabletop_z: float | None, fallback_z: float +) -> float: + """Return the midpoint from the physical table plane to an object's observed top surface.""" + if tabletop_z is None or points.ndim != 2 or points.shape[1] != 3 or len(points) < 10: + return fallback_z + top_z = float(np.quantile(points[:, 2], 0.95)) + if top_z <= tabletop_z: + return fallback_z + return tabletop_z + (top_z - tabletop_z) / 2.0 + + +def _primitive_mesh( + shape: Literal["box", "sphere", "cylinder"], + center: Vector3, + dimensions: tuple[float, ...], + orientation: Quaternion, +) -> tuple[np.ndarray, np.ndarray]: + """Create a display mesh for one planner primitive.""" + if shape == "box": + x, y, z = (dimension / 2.0 for dimension in dimensions) + vertices = np.asarray( + [ + [-x, -y, -z], + [x, -y, -z], + [x, y, -z], + [-x, y, -z], + [-x, -y, z], + [x, -y, z], + [x, y, z], + [-x, y, z], + ] + ) + triangles = np.asarray( + [ + [0, 1, 2], + [0, 2, 3], + [4, 6, 5], + [4, 7, 6], + [0, 4, 5], + [0, 5, 1], + [1, 5, 6], + [1, 6, 2], + [2, 6, 7], + [2, 7, 3], + [3, 7, 4], + [3, 4, 0], + ] + ) + else: + segments = 16 + angles = np.linspace(0.0, 2.0 * math.pi, segments, endpoint=False) + radius = dimensions[0] + if shape == "cylinder": + half_height = dimensions[1] / 2.0 + vertices = np.vstack( + ( + np.column_stack( + (radius * np.cos(angles), radius * np.sin(angles), -half_height) + ), + np.column_stack( + (radius * np.cos(angles), radius * np.sin(angles), half_height) + ), + [[0.0, 0.0, -half_height], [0.0, 0.0, half_height]], + ) + ) + bottom_center, top_center = 2 * segments, 2 * segments + 1 + triangles = np.asarray( + [ + triangle + for index in range(segments) + for triangle in ( + [index, (index + 1) % segments, segments + index], + [ + (index + 1) % segments, + segments + (index + 1) % segments, + segments + index, + ], + [bottom_center, (index + 1) % segments, index], + [top_center, segments + index, segments + (index + 1) % segments], + ) + ] + ) + else: + rings = 8 + phi = np.linspace(0.0, math.pi, rings + 1) + vertices = np.asarray( + [ + [ + radius * math.sin(p) * math.cos(a), + radius * math.sin(p) * math.sin(a), + radius * math.cos(p), + ] + for p in phi + for a in angles + ] + ) + triangles = np.asarray( + [ + triangle + for ring in range(rings) + for index in range(segments) + for triangle in ( + [ + ring * segments + index, + ring * segments + (index + 1) % segments, + (ring + 1) * segments + index, + ], + [ + ring * segments + (index + 1) % segments, + (ring + 1) * segments + (index + 1) % segments, + (ring + 1) * segments + index, + ], + ) + ] + ) + transformed = vertices @ orientation.to_rotation_matrix().T + return transformed + np.asarray(center.as_tuple), triangles + + +class PickNPlaceConfig(ModuleConfig): + """Configuration for PickNPlaceModule.""" + + align_grasp_yaw: bool = False + grasp: Literal["obb_center", "graspgenx"] = Field( + default="obb_center", validation_alias=AliasChoices("grasp", "grasp_strategy") + ) + graspgenx_pregrasp_offset: float = 0.10 + graspgenx_ik_filter_limit: int = 10 + grasp_empty_closed_threshold: float = 0.01 + grasp_feedback_delay: float = 0.5 + + +class PickNPlaceModule(Module): + """Provide request-driven perception and target selection for pick and place.""" + + config: PickNPlaceConfig + _scene: ObjectSceneRegistrationSpec + _grasp_generator: GraspGenSpec | None + _grasp_filter: GraspCandidateFilterSpec + _pick_execution: PickExecutionSpec + _obstacle_world: ObstacleWorldSpec + _visualization: ManipulationVisualizationSpec + objects: In[list[DetObject]] + camera_info: In[CameraInfo] + basic_grasp_overlay: Out[Image] + graspgenx_candidates: Out[GraspCandidateArray] + + def __init__(self, **kwargs: object) -> None: + super().__init__(**kwargs) + self._objects_condition = threading.Condition() + self._latest_objects: tuple[DetObject, ...] = () + self._objects_version = 0 + self._camera_info: CameraInfo | None = None + self._goal_pose: PoseStamped | None = None + self._pre_grasp_pose: PoseStamped | None = None + self._grasp_candidates: GraspCandidateArray | None = None + self._selected_object: DetObject | None = None + self._held_object_size: Vector3 | None = None + self._tabletop_z: float | None = None + self._open_box: dict[str, float] | None = None + self._scene_geometry_ids: set[str] = set() + + @rpc + def start(self) -> None: + super().start() + self.objects.subscribe(self._on_objects) + self.camera_info.subscribe(self._on_camera_info) + + def _on_objects(self, objects: list[DetObject]) -> None: + with self._objects_condition: + self._latest_objects = tuple(objects) + self._objects_version += 1 + self._objects_condition.notify_all() + + def _on_camera_info(self, camera_info: CameraInfo) -> None: + with self._objects_condition: + self._camera_info = camera_info + + @rpc + def scan_scene( + self, prompt: str | None = None, prompts: list[str] | None = None + ) -> Detection3DArray: + """Run one RGB-D detection pass, optionally targeting one or more text prompts.""" + if prompt is not None and prompts is not None: + raise ValueError("Specify either prompt or prompts, not both") + with self._objects_condition: + objects_version = self._objects_version + if prompts is not None: + self._scene.set_prompts(prompts) + elif prompt: + self._scene.set_prompts([prompt]) + detections = self._scene.scan_scene() + with self._objects_condition: + received_result = self._objects_condition.wait_for( + lambda: self._objects_version > objects_version, + timeout=5.0, + ) + objects = self._latest_objects + if received_result: + # Stream delivery crosses process boundaries and can lag the OSR RPC response. + # Return the same snapshot used by the object/grasp APIs, not the prior response. + return to_detection3d_array( + list(objects), + frame_id=objects[0].frame_id if objects else detections.frame_id, + ts=objects[0].ts if objects else detections.ts, + ) + return detections + + @skill(uses=[CAP_PERCEPTION]) + def scan(self, prompt: str) -> SkillResult: + """Detect a prompted object from one RGB-D frame without moving the robot. + + Returns numbered objects. Use a returned number with ``select_object`` to create a grasp target + or ``get_object_geometry`` to inspect a container target. + """ + if not prompt.strip(): + return SkillResult.fail("INVALID_INPUT", "A nonempty object prompt is required") + try: + detections = self.scan_scene(prompt) + except RuntimeError as exc: + return SkillResult.fail("PERCEPTION_FAILED", str(exc)) + self._publish_scene_objects() + return SkillResult.ok( + f"Detected {detections.detections_length} object(s)", objects=self.get_scene_info() + ) + + @skill(uses=[CAP_PERCEPTION]) + def scan_objects(self, object_names: list[str]) -> SkillResult: + """Detect instances of simple object names from one RGB-D frame. + + Pass one short noun phrase per item, for example ``["wooden block", "white box"]``. Each name is + an independent Moondream query and can return multiple instances. Do not pass instructions, + exclusions, counting requests, or full sentences as object names. + """ + names = [name.strip() for name in object_names if name.strip()] + if not names: + return SkillResult.fail("INVALID_INPUT", "At least one simple object name is required") + try: + detections = self.scan_scene(prompts=names) + except RuntimeError as exc: + return SkillResult.fail("PERCEPTION_FAILED", str(exc)) + self._publish_scene_objects() + return SkillResult.ok( + f"Detected {detections.detections_length} object(s)", + queried_names=names, + objects=self.get_scene_info(), + ) + + @rpc + def get_scene_info(self) -> list[dict[str, object]]: + """Return the number, name, and confidence for current detections.""" + with self._objects_condition: + objects = self._latest_objects + return [ + { + "number": number, + "name": obj.name, + "confidence": obj.confidence, + } + for number, obj in enumerate(objects, 1) + ] + + @skill + def describe_scene(self, question: str = "What objects are visible on the table?") -> str: + """Answer an open-ended question about the latest camera image without moving the robot. + + Requires ``osr.det=moondream`` and is descriptive only; use ``scan`` for numbered 3D objects. + """ + return self._scene.describe_scene(question) + + @skill + def get_object_geometry(self, number: int) -> dict[str, object] | None: + """Return a scanned object's center and OBB size without moving the robot. + + ``number`` must come from the latest ``scan`` result. ``center`` and ``size`` are ``[x, y, z]`` + lists in meters in the returned planning frame; use this to derive a container placement target. + """ + with self._objects_condition: + if number < 1 or number > len(self._latest_objects): + return None + obj = self._latest_objects[number - 1] + return { + "number": number, + "name": obj.name, + "frame_id": obj.frame_id, + "center": [obj.center.x, obj.center.y, obj.center.z], + "size": [obj.size.x, obj.size.y, obj.size.z], + } + + @skill + def install_object_obstacle( + self, number: int, shape: Literal["box", "sphere", "cylinder"] = "box" + ) -> SkillResult: + """Install one measured object as a planner obstacle and render the same primitive in Viser. + + ``number`` must come from the latest ``scan`` result. Choose ``box`` for rectangular objects, + ``cylinder`` for upright round objects, and ``sphere`` only for near-spherical objects. + """ + obj = self._object_for_number(number) + if obj is None: + return SkillResult.fail("INVALID_INPUT", f"No detected object numbered {number}") + orientation = self._upright_orientation(obj) + if shape == "box": + dimensions = (obj.size.x, obj.size.y, obj.size.z) + elif shape == "sphere": + dimensions = (max(obj.size.x, obj.size.y, obj.size.z) / 2.0,) + else: + dimensions = (max(obj.size.x, obj.size.y) / 2.0, obj.size.z) + name = f"scene-object-{number}" + center = Vector3(obj.center) + if not self._install_geometry(name, center, orientation, shape, dimensions): + return SkillResult.fail("EXECUTION_FAILED", f"Failed to install obstacle '{name}'") + return SkillResult.ok( + "Obstacle installed", + name=name, + shape=shape, + center=[center.x, center.y, center.z], + dimensions=list(dimensions), + ) + + @skill + def install_open_box(self, number: int, wall_thickness: float = 0.01) -> SkillResult: + """Measure an open rectangular box and render it as a display-only solid box in Viser. + + Call ``estimate_table`` first. The result describes the free opening for top-down placement, but + does not add box walls to the planning world. + """ + if wall_thickness <= 0.0: + return SkillResult.fail("INVALID_INPUT", "wall_thickness must be positive") + if self._tabletop_z is None: + return SkillResult.fail( + "INVALID_STATE", "Estimate the table before modeling an open box" + ) + obj = self._object_for_number(number) + if obj is None: + return SkillResult.fail("INVALID_INPUT", f"No detected object numbered {number}") + width, depth = obj.size.x, obj.size.y + if width <= 2.0 * wall_thickness or depth <= 2.0 * wall_thickness: + return SkillResult.fail( + "INVALID_INPUT", "Box opening is smaller than twice wall_thickness" + ) + points = obj.pointcloud.points_f32() + rim_z = ( + float(np.quantile(points[:, 2], 0.95)) if len(points) else obj.center.z + obj.size.z / 2 + ) + height = rim_z - self._tabletop_z + if height <= 0.0: + return SkillResult.fail( + "PERCEPTION_FAILED", "Box rim is not above the estimated tabletop" + ) + center = Vector3(obj.center.x, obj.center.y, self._tabletop_z + height / 2.0) + orientation = self._upright_orientation(obj) + + vertices, triangles = _primitive_mesh("box", center, (width, depth, height), orientation) + self._visualization.set_visualization_layer( + VisualizationLayer( + "picknplace/open-box", + "world", + ( + MeshElement( + "box-envelope", + vertices, + triangles, + color=np.asarray([230, 230, 230]), + opacity=0.25, + ), + ), + ) + ) + self._open_box = { + "center_x": center.x, + "center_y": center.y, + "tabletop_z": self._tabletop_z, + "rim_z": rim_z, + "opening_width": width - 2.0 * wall_thickness, + "opening_depth": depth - 2.0 * wall_thickness, + } + return SkillResult.ok( + "Open box measured and displayed", + center=[center.x, center.y], + rim_z=rim_z, + opening_width=width - 2.0 * wall_thickness, + opening_depth=depth - 2.0 * wall_thickness, + ) + + @skill + def clear_scene_geometry(self) -> SkillResult: + """Remove temporary scene obstacles and the display-only open-box marker.""" + removed = [ + geometry_id + for geometry_id in tuple(self._scene_geometry_ids) + if self._obstacle_world.remove_obstacle(geometry_id) + ] + self._scene_geometry_ids.difference_update(removed) + self._visualization.set_visualization_layer( + VisualizationLayer("picknplace/open-box", "world", ()) + ) + self._open_box = None + return SkillResult.ok("Temporary scene geometry cleared", removed=removed) + + @rpc + def get_goal_pose(self, number: int) -> PoseStamped | None: + """Select an object and return its downward-facing, floor-clamped grasp goal.""" + selection = self._basic_grasp(number) + if selection is None: + return None + grasp, obj = selection + if self.config.grasp == "graspgenx": + if self._grasp_generator is None: + raise RuntimeError("GraspGenX is not configured for this pick-and-place blueprint") + candidates = self._grasp_generator.propose_grasps(obj.pointcloud) + self._selected_object = obj + self._grasp_candidates = self._filter_graspgenx_candidates(candidates) + if not self._grasp_candidates.candidates: + self.graspgenx_candidates.publish(self._grasp_candidates) + return None + return self._select_graspgenx_candidate(0) + yaw = self._grasp_yaw(obj) if self.config.align_grasp_yaw else 0.0 + pick_execution = getattr(self, "_pick_execution", None) + current_pose = pick_execution.get_ee_pose() if pick_execution is not None else None + if current_pose is not None: + yaw = self._closest_parallel_jaw_yaw(yaw, current_pose.orientation.to_euler().z) + self._grasp_candidates = None + self._selected_object = obj + self.graspgenx_candidates.publish(GraspCandidateArray()) + grasp_z = _table_midpoint_grasp_z( + obj.pointcloud.points_f32(), self._tabletop_z, grasp.position.z + ) + self._goal_pose = PoseStamped( + ts=grasp.ts, + frame_id=grasp.frame_id, + position=Vector3(grasp.position.x, grasp.position.y, max(grasp_z, 0.100)), + orientation=Quaternion.from_euler(Vector3(-math.pi, 0.0, yaw)), + ) + self._pre_grasp_pose = None + return self._goal_pose + + @skill + def select_object(self, number: int) -> SkillResult: + """Select a scanned object and return grasp and pre-grasp targets without moving the robot. + + ``number`` must come from the latest ``scan`` result. Returned target values are XYZ in meters and + roll/pitch/yaw in radians. Move to ``pre_grasp`` first, then move to ``goal`` for gripper contact; + ``pre_grasp`` is 100 mm above the object and is not a grasp pose. + """ + goal = self.get_goal_pose(number) + if goal is None: + return SkillResult.fail("INVALID_INPUT", f"No selectable object numbered {number}") + pre_grasp = self.get_pre_grasp_pose() + if pre_grasp is None: + return SkillResult.fail("INVALID_STATE", "Selected object has no pre-grasp target") + + def pose_target(pose: PoseStamped) -> dict[str, float]: + euler = pose.orientation.to_euler() + return { + "x": pose.position.x, + "y": pose.position.y, + "z": pose.position.z, + "roll": euler.x, + "pitch": euler.y, + "yaw": euler.z, + } + + return SkillResult.ok( + "Object selected. Move to pre_grasp, then goal for gripper contact before closing the gripper.", + goal=pose_target(goal), + pre_grasp=pose_target(pre_grasp), + ) + + @skill(uses=[CAP_MOVEMENT]) + def pick_selected(self, robot_name: str | None = None) -> SkillResult: + """Pick the object most recently selected with ``select_object``. + + Executes the full pre-grasp, contact-grasp, close, feedback verification, and retreat sequence. + A gripper position at or below the empty-closed threshold means no object was picked up and returns + ``GRASP_VERIFICATION_FAILED``. Call ``select_object`` before this tool; do not manually recreate + the grasp sequence with individual motion and gripper tools. + """ + goal = self._goal_pose + pre_grasp = self._pre_grasp_pose + if goal is None or pre_grasp is None: + return SkillResult.fail("INVALID_STATE", "Select an object before starting a pick") + + def move(pose: PoseStamped) -> SkillResult: + euler = pose.orientation.to_euler() + return self._pick_execution.move_to_pose( + pose.position.x, + pose.position.y, + pose.position.z, + euler.x, + euler.y, + euler.z, + robot_name, + ) + + opened = self._pick_execution.open_gripper(robot_name) + if not opened.is_success(): + return opened + approach = move(pre_grasp) + if not approach.is_success(): + return approach + contact = move(goal) + if not contact.is_success(): + return contact + closed = self._pick_execution.close_gripper(robot_name) + if not closed.is_success(): + return closed + + time.sleep(self.config.grasp_feedback_delay) + gripper_position = self._pick_execution.get_gripper(robot_name) + if gripper_position is None: + return SkillResult.fail( + "GRIPPER_FAILED", "Cannot verify pickup: gripper feedback unavailable" + ) + if gripper_position <= self.config.grasp_empty_closed_threshold: + self._pick_execution.open_gripper(robot_name) + recovery = move(pre_grasp) + result = SkillResult.fail( + "GRASP_VERIFICATION_FAILED", + "Pickup failed: gripper reached the empty-closed position; rescan and select before retrying", + ) + result.metadata = { + "gripper_position": gripper_position, + "rescan_required": True, + "recovered_to_pre_grasp": recovery.is_success(), + } + return result + + retreat = move(pre_grasp) + if not retreat.is_success(): + return retreat + selected_object = self._selected_object + if selected_object is None: + return SkillResult.fail( + "INVALID_STATE", "Selected object details are unavailable after grasp" + ) + self._held_object_size = Vector3(selected_object.size) + return SkillResult.ok( + "Pick complete: grasp verified and object retreated from the table", + gripper_position=gripper_position, + ) + + @skill(uses=[CAP_MOVEMENT]) + def place_selected(self, robot_name: str | None = None) -> SkillResult: + """Drop the verified held object into the most recently measured open box. + + Call ``install_open_box`` for the destination and complete ``pick_selected`` first. This tool moves + above the remembered opening and releases above the rim. It first lifts the held object for transit, + then lowers only at the box center. It never lowers the end effector into the box. The box remains + display-only and does not add planner collisions. + """ + box = self._open_box + held_size = self._held_object_size + if box is None: + return SkillResult.fail( + "INVALID_STATE", "Measure the destination with install_open_box first" + ) + if held_size is None: + return SkillResult.fail( + "INVALID_STATE", "No verified held object is available to place" + ) + if held_size.x > box["opening_width"] or held_size.y > box["opening_depth"]: + return SkillResult.fail( + "INVALID_INPUT", "Held object does not fit inside the measured box opening" + ) + + def move(x: float, y: float, z: float) -> SkillResult: + return self._pick_execution.move_to_pose(x, y, z, robot_name=robot_name) + + # The held object's bottom remains above the rim throughout lateral travel. + drop_z = box["rim_z"] + held_size.z / 2.0 + 0.02 + transit_z = drop_z + 0.10 + current_pose = self._pick_execution.get_ee_pose(robot_name) + if current_pose is not None and current_pose.position.z < transit_z: + lift = move(current_pose.position.x, current_pose.position.y, transit_z) + if not lift.is_success(): + return lift + approach = move(box["center_x"], box["center_y"], transit_z) + if not approach.is_success(): + return approach + lower = move(box["center_x"], box["center_y"], drop_z) + if not lower.is_success(): + return lower + opened = self._pick_execution.open_gripper(robot_name) + if not opened.is_success(): + return opened + self._held_object_size = None + return SkillResult.ok( + "Drop complete: object released above the measured box opening", + drop_z=drop_z, + object_bottom_clearance=0.02, + ) + + @rpc + def select_grasp_candidate(self, rank: int) -> PoseStamped | None: + """Select one ranked GraspGenX proposal as the goal and Rerun highlight.""" + return self._select_graspgenx_candidate(rank) + + def _select_graspgenx_candidate(self, rank: int) -> PoseStamped | None: + candidates = self._grasp_candidates + if candidates is None or rank < 0 or rank >= len(candidates.candidates): + return None + candidates.selected_index = rank + self.graspgenx_candidates.publish(candidates) + candidate = candidates.candidates[rank] + self._goal_pose = PoseStamped( + ts=candidates.header.timestamp, + frame_id=candidates.header.frame_id, + position=candidate.pose.position, + orientation=candidate.pose.orientation, + ) + self._pre_grasp_pose = None + return self._goal_pose + + def _filter_graspgenx_candidates(self, candidates: GraspCandidateArray) -> GraspCandidateArray: + """Keep only top-ranked proposals whose TCP IK is collision-free in the live world.""" + accepted = [] + for candidate in candidates.candidates[: self.config.graspgenx_ik_filter_limit]: + result = self._grasp_filter.inverse_kinematics_single( + candidate.pose, "arm", check_collision=True + ) + if result.is_success(): + accepted.append(candidate) + return GraspCandidateArray(candidates.header, accepted) + + @rpc + def get_pre_grasp_pose(self) -> PoseStamped | None: + """Return the selected goal offset 100 mm opposite its final approach direction.""" + if self._goal_pose is None: + return None + if self.config.grasp == "graspgenx": + offset = self._goal_pose.orientation.rotate_vector( + # GraspGenX local +Z points in the direction of the final + # approach. A pre-grasp retreats along the opposite axis. + Vector3(0.0, 0.0, -self.config.graspgenx_pregrasp_offset) + ) + else: + offset = Vector3(0.0, 0.0, 0.100) + self._pre_grasp_pose = PoseStamped( + ts=self._goal_pose.ts, + frame_id=self._goal_pose.frame_id, + position=Vector3( + self._goal_pose.position.x + offset.x, + self._goal_pose.position.y + offset.y, + self._goal_pose.position.z + offset.z, + ), + orientation=self._goal_pose.orientation, + ) + self._publish_viser_selection() + return self._pre_grasp_pose + + def _publish_viser_selection(self) -> None: + """Show the selected object and TCP targets without mutating the planning scene.""" + obj = self._selected_object + goal = self._goal_pose + pre_grasp = self._pre_grasp_pose + if obj is None or goal is None or pre_grasp is None: + return + points = obj.pointcloud.points_f32() + if len(points) == 0: + return + cloud_colors = np.repeat(np.array([[255, 190, 70]], dtype=np.uint8), len(points), axis=0) + vertices: list[np.ndarray] = [] + edges: list[list[int]] = [] + colors: list[list[int]] = [] + for pose, color in ((goal, [255, 70, 70]), (pre_grasp, [70, 255, 120])): + start = len(vertices) + origin = np.asarray(pose.position.as_tuple, dtype=np.float32) + axes = pose.orientation.to_rotation_matrix().astype(np.float32) * 0.06 + vertices.extend((origin, origin + axes[:, 0], origin + axes[:, 1], origin + axes[:, 2])) + edges.extend(((start, start + 1), (start, start + 2), (start, start + 3))) + colors.extend((color, color, color)) + self._visualization.set_visualization_layer( + VisualizationLayer( + "picknplace/selection", + "world", + ( + PointCloudElement("object", points, cloud_colors, point_size=0.003), + LineSetElement( + "tcp-targets", + np.asarray(vertices), + np.asarray(edges), + np.asarray(colors), + ), + ), + ) + ) + + def _publish_scene_objects(self) -> None: + """Display the latest measured object envelopes without affecting planning.""" + visualization = getattr(self, "_visualization", None) + if visualization is None: + return + with self._objects_condition: + objects = tuple(self._latest_objects) + colors = ([255, 180, 70], [80, 180, 255], [130, 230, 130], [230, 150, 230]) + elements: list[MeshElement] = [] + for number, obj in enumerate(objects, 1): + dimensions = (obj.size.x, obj.size.y, obj.size.z) + if any(dimension <= 0.0 for dimension in dimensions): + continue + vertices, triangles = _primitive_mesh( + "box", Vector3(obj.center), dimensions, self._upright_orientation(obj) + ) + elements.append( + MeshElement( + f"object-{number}", + vertices, + triangles, + color=np.asarray(colors[(number - 1) % len(colors)]), + opacity=0.30, + ) + ) + visualization.set_visualization_layer( + VisualizationLayer("picknplace/scene-objects", "world", tuple(elements)) + ) + + @rpc + def get_grasp_candidates(self) -> GraspCandidateArray: + """Return the GraspGenX proposals generated for the selected object.""" + return self._grasp_candidates or GraspCandidateArray() + + @rpc + def estimate_table_surface(self) -> dict[str, float] | None: + """Estimate a horizontal tabletop from the latest full RGB-D scene cloud.""" + scene = self._scene.get_full_scene_pointcloud(voxel_size=0.01) + if scene is None: + return None + estimate = _estimate_table_surface(scene.points_f32()) + if estimate is None: + return None + self._tabletop_z = estimate["tabletop_z"] + z = estimate["tabletop_z"] + half_width = estimate["width"] / 2 + half_depth = estimate["depth"] / 2 + x = estimate["center_x"] + y = estimate["center_y"] + vertices = np.asarray( + [ + [x - half_width, y - half_depth, z], + [x + half_width, y - half_depth, z], + [x + half_width, y + half_depth, z], + [x - half_width, y + half_depth, z], + ] + ) + self._visualization.set_visualization_layer( + VisualizationLayer( + "picknplace/table-estimate", + "world", + ( + MeshElement( + "tabletop-fill", + vertices, + np.asarray([[0, 1, 2], [0, 2, 3]]), + color=np.asarray([80, 180, 255]), + opacity=1.0, + ), + LineSetElement( + "tabletop", + vertices, + np.asarray([[0, 1], [1, 2], [2, 3], [3, 0]]), + colors=np.asarray([[80, 180, 255]] * 4), + line_width=2.0, + ), + ), + ) + ) + return estimate + + @skill(uses=[CAP_PERCEPTION]) + def estimate_table(self) -> SkillResult: + """Run a fresh RGB-D scan, then estimate the tabletop without moving the robot. + + Pass the returned ``center_x``, ``center_y``, ``tabletop_z``, ``width``, and ``depth`` directly to + ``set_table_collision`` before requesting motion near the table. Do not call this concurrently with + ``scan``; both tools exclusively use the perception pipeline. + """ + try: + self.scan_scene() + except RuntimeError as exc: + return SkillResult.fail("PERCEPTION_FAILED", str(exc)) + estimate = self.estimate_table_surface() + if estimate is None: + return SkillResult.fail( + "PERCEPTION_FAILED", "No horizontal tabletop estimate is available" + ) + return SkillResult.ok("Table estimated", **estimate) + + def _basic_grasp(self, number: int) -> tuple[PoseStamped, DetObject] | None: + """Return the selected cloud's OBB-center grasp frame and object geometry.""" + with self._objects_condition: + if number < 1 or number > len(self._latest_objects): + return None + obj = self._latest_objects[number - 1] + camera_info = self._camera_info + grasp = PoseStamped( + ts=obj.ts, + frame_id=obj.frame_id, + position=obj.center, + orientation=obj.pose.orientation, + ) + if camera_info is not None and obj.camera_transform is not None and obj.image is not None: + if overlay := draw_pose_axes( + obj.image, grasp, obj.camera_transform.inverse(), camera_info + ): + self.basic_grasp_overlay.publish(overlay) + return grasp, obj + + def _object_for_number(self, number: int) -> DetObject | None: + with self._objects_condition: + if number < 1 or number > len(self._latest_objects): + return None + return self._latest_objects[number - 1] + + @staticmethod + def _upright_orientation(obj: DetObject) -> Quaternion: + """Keep measured horizontal yaw while constraining scene primitives upright.""" + return Quaternion.from_euler(Vector3(0.0, 0.0, obj.pose.orientation.to_euler().z)) + + def _install_geometry( + self, + name: str, + center: Vector3, + orientation: Quaternion, + shape: Literal["box", "sphere", "cylinder"], + dimensions: tuple[float, ...], + ) -> bool: + pose = Pose(center, orientation) + if self._obstacle_world.update_obstacle(name, pose, shape, list(dimensions)): + self._scene_geometry_ids.add(name) + return True + obstacle_id = self._obstacle_world.add_obstacle(name, pose, shape, list(dimensions)) + if obstacle_id: + self._scene_geometry_ids.add(obstacle_id) + return True + return False + + @staticmethod + def _grasp_yaw(obj: DetObject) -> float: + """Align the gripper's local Y closing axis with the narrowest horizontal OBB axis.""" + rotation = obj.pose.orientation.to_rotation_matrix() + extents = (obj.size.x, obj.size.y, obj.size.z) + horizontal_axes = sorted(range(3), key=lambda axis: abs(rotation[2, axis]))[:2] + narrow_axis = min(horizontal_axes, key=lambda axis: extents[axis]) + return math.atan2(rotation[1, narrow_axis], rotation[0, narrow_axis]) - math.pi / 2 + + @staticmethod + def _closest_parallel_jaw_yaw(target_yaw: float, current_yaw: float) -> float: + """Choose the equivalent parallel-jaw yaw requiring the smallest wrist rotation.""" + return target_yaw + math.pi * round((current_yaw - target_yaw) / math.pi) diff --git a/dimos/manipulation/planning/monitor/test_world_monitor.py b/dimos/manipulation/planning/monitor/test_world_monitor.py index b14000496d..34a5a58523 100644 --- a/dimos/manipulation/planning/monitor/test_world_monitor.py +++ b/dimos/manipulation/planning/monitor/test_world_monitor.py @@ -35,6 +35,7 @@ VisualizationStateFrame, ) from dimos.manipulation.planning.spec.protocols import VisualizationSpec +from dimos.manipulation.visualization.layers import VisualizationLayer from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -220,6 +221,12 @@ def remove_vis_obstacle(self, obstacle_id: str) -> None: def clear_vis_obstacles(self) -> None: self.calls.append(("clear_vis_obstacles",)) + def set_layer(self, layer: VisualizationLayer) -> None: + self.calls.append(("set_layer", layer)) + + def clear_layer(self, layer_id: str) -> None: + self.calls.append(("clear_layer", layer_id)) + def _robot_config() -> RobotModelConfig: return RobotModelConfig( diff --git a/dimos/manipulation/planning/monitor/test_world_obstacle_suppression.py b/dimos/manipulation/planning/monitor/test_world_obstacle_suppression.py new file mode 100644 index 0000000000..995a137931 --- /dev/null +++ b/dimos/manipulation/planning/monitor/test_world_obstacle_suppression.py @@ -0,0 +1,178 @@ +# 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 __future__ import annotations + +import threading +import time +from types import SimpleNamespace + +import open3d as o3d +import pytest +from pytest_mock import MockerFixture + +from dimos.manipulation.planning.monitor.world_obstacle_monitor import WorldObstacleMonitor +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +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 + + +def _object(object_id: str) -> Object: + return Object( + name=f"object-{object_id}", + object_id=object_id, + center=Vector3(0.4, 0.0, 0.2), + size=Vector3(0.05, 0.05, 0.1), + pose=PoseStamped(), + pointcloud=PointCloud2(o3d.geometry.PointCloud()), + bbox=(0.0, 0.0, 1.0, 1.0), + track_id=0, + class_id=0, + confidence=1.0, + ts=time.time(), + image=Image(), + ) + + +def _monitor(mocker: MockerFixture) -> tuple[WorldObstacleMonitor, SimpleNamespace]: + parent = SimpleNamespace( + _lock=threading.RLock(), + add_obstacle=mocker.Mock( + side_effect=lambda obstacle: f"world-{obstacle.name}-{time.monotonic_ns()}" + ), + remove_obstacle=mocker.Mock(return_value=True), + ) + monitor = WorldObstacleMonitor(parent) # type: ignore[arg-type] + monitor.start() + return monitor, parent + + +def test_suppression_skips_target_but_refreshes_other_objects( + mocker: MockerFixture, +) -> None: + monitor, parent = _monitor(mocker) + target = _object("target") + other = _object("other") + monitor.on_objects([target, other]) + monitor.refresh_obstacles() + parent.add_obstacle.reset_mock() + + with monitor.suppress_object_obstacle("target") as suppression: + refreshed = monitor.refresh_obstacles() + + assert suppression.removed is True + assert [item["object_id"] for item in refreshed] == ["other"] + assert set(monitor._object_obstacles) == {"other"} + + assert set(monitor._object_obstacles) == {"target", "other"} + assert parent.remove_obstacle.call_count >= 1 + + +def test_suppression_wins_race_with_in_progress_refresh(mocker: MockerFixture) -> None: + monitor, _ = _monitor(mocker) + monitor.on_objects([_object("target"), _object("other")]) + conversion_started = threading.Event() + continue_conversion = threading.Event() + original_conversion = monitor._object_to_obstacle + + def delayed_conversion(obj: Object): + if obj.object_id == "target": + conversion_started.set() + assert continue_conversion.wait(timeout=1.0) + return original_conversion(obj) + + mocker.patch.object(monitor, "_object_to_obstacle", side_effect=delayed_conversion) + refreshed: list[list[dict[str, object]]] = [] + thread = threading.Thread(target=lambda: refreshed.append(monitor.refresh_obstacles())) + thread.start() + assert conversion_started.wait(timeout=1.0) + + with monitor.suppress_object_obstacle("target"): + continue_conversion.set() + thread.join(timeout=1.0) + + assert not thread.is_alive() + assert [item["object_id"] for item in refreshed[0]] == ["other"] + assert set(monitor._object_obstacles) == {"other"} + + assert set(monitor._object_obstacles) == {"target", "other"} + + +def test_nested_suppression_removes_and_restores_once(mocker: MockerFixture) -> None: + monitor, parent = _monitor(mocker) + monitor.on_objects([_object("target")]) + monitor.refresh_obstacles() + parent.add_obstacle.reset_mock() + parent.remove_obstacle.reset_mock() + + with monitor.suppress_object_obstacle("target"): + with monitor.suppress_object_obstacle("target"): + assert "target" not in monitor._object_obstacles + assert "target" not in monitor._object_obstacles + + assert parent.remove_obstacle.call_count == 1 + assert parent.add_obstacle.call_count == 1 + assert "target" in monitor._object_obstacles + + +def test_suppression_restores_after_cancellation(mocker: MockerFixture) -> None: + class Cancelled(BaseException): + pass + + monitor, _ = _monitor(mocker) + monitor.on_objects([_object("target")]) + monitor.refresh_obstacles() + + with pytest.raises(Cancelled): + with monitor.suppress_object_obstacle("target"): + raise Cancelled + + assert monitor._object_suppressions == {} + assert "target" in monitor._object_obstacles + + +def test_suppression_reports_restore_failure_without_masking_body( + mocker: MockerFixture, +) -> None: + monitor, parent = _monitor(mocker) + monitor.on_objects([_object("target")]) + monitor.refresh_obstacles() + parent.add_obstacle.side_effect = None + parent.add_obstacle.return_value = "" + + with monitor.suppress_object_obstacle("target") as suppression: + body_completed = True + + assert body_completed is True + assert suppression.cleanup_error == "failed to restore obstacle for object 'target'" + assert "target" not in monitor._object_obstacles + + +def test_failed_suppression_removal_restores_internal_tracking( + mocker: MockerFixture, +) -> None: + monitor, parent = _monitor(mocker) + monitor.on_objects([_object("target")]) + monitor.refresh_obstacles() + parent.remove_obstacle.return_value = False + + with pytest.raises(RuntimeError, match="failed to suppress") as exc_info: + with monitor.suppress_object_obstacle("target"): + raise AssertionError("suppression body must not run") + + assert str(exc_info.value) == "failed to suppress obstacle for object 'target'" + assert monitor._object_suppressions == {} + assert "target" in monitor._object_obstacles diff --git a/dimos/manipulation/planning/monitor/world_monitor.py b/dimos/manipulation/planning/monitor/world_monitor.py index bcbb2b7ed8..dc1c616230 100644 --- a/dimos/manipulation/planning/monitor/world_monitor.py +++ b/dimos/manipulation/planning/monitor/world_monitor.py @@ -16,7 +16,7 @@ from __future__ import annotations -from collections.abc import Sequence +from collections.abc import Iterator, Sequence from contextlib import contextmanager import threading from typing import TYPE_CHECKING, Any @@ -29,7 +29,10 @@ from dimos.manipulation.planning.groups.registry import PlanningGroupRegistry from dimos.manipulation.planning.groups.utils import filter_joint_state_to_selected_joints from dimos.manipulation.planning.monitor.robot_state_monitor import RobotStateMonitor -from dimos.manipulation.planning.monitor.world_obstacle_monitor import WorldObstacleMonitor +from dimos.manipulation.planning.monitor.world_obstacle_monitor import ( + ObjectObstacleSuppression, + WorldObstacleMonitor, +) from dimos.manipulation.planning.spec.models import ( PlanningSceneInfo, VisualizationSession, @@ -313,6 +316,15 @@ def remove_object_obstacle(self, object_id: str) -> bool: return self._obstacle_monitor.remove_object_obstacle(object_id) return False + @contextmanager + def suppress_object_obstacle(self, object_id: str) -> Iterator[ObjectObstacleSuppression]: + """Temporarily exclude one perception object from collision checking.""" + if self._obstacle_monitor is None: + yield ObjectObstacleSuppression(object_id=object_id) + return + with self._obstacle_monitor.suppress_object_obstacle(object_id) 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 481e6976ca..6f870b171c 100644 --- a/dimos/manipulation/planning/monitor/world_obstacle_monitor.py +++ b/dimos/manipulation/planning/monitor/world_obstacle_monitor.py @@ -26,7 +26,10 @@ from __future__ import annotations -from dataclasses import replace +from collections import Counter +from collections.abc import Iterator +from contextlib import contextmanager +from dataclasses import dataclass, replace import time from typing import TYPE_CHECKING, Any @@ -49,6 +52,15 @@ logger = setup_logger() +@dataclass +class ObjectObstacleSuppression: + """Result of a scoped object-obstacle suppression.""" + + object_id: str + removed: bool = False + cleanup_error: str | None = None + + class WorldObstacleMonitor: """Monitors world obstacles and updates its parent WorldMonitor. @@ -95,6 +107,7 @@ def __init__( self._object_cache: dict[str, tuple[Object, float, float]] = {} # object_id -> obstacle_id (objects currently added to Drake world) self._object_obstacles: dict[str, str] = {} + self._object_suppressions: Counter[str] = Counter() # Running state self._running = False @@ -123,6 +136,7 @@ def clear_tracking(self) -> None: self._perception_objects.clear() self._perception_timestamps.clear() self._object_obstacles.clear() + self._object_suppressions.clear() def on_collision_object(self, msg: CollisionObjectMessage) -> None: """Handle explicit collision object message. @@ -497,6 +511,8 @@ def refresh_obstacles(self, min_duration: float = 0.0) -> list[dict[str, Any]]: for oid, (obj, first_seen, last_seen) in self._object_cache.items(): if not isinstance(obj, Object): continue + if self._object_suppressions[oid] > 0: + continue if last_seen - first_seen < min_duration: continue eligible.append((oid, obj)) @@ -517,6 +533,10 @@ def refresh_obstacles(self, min_duration: float = 0.0) -> list[dict[str, Any]]: result: list[dict[str, Any]] = [] for oid, obj, obstacle in prepared: + # Suppression may have started while obstacle geometry was + # computed outside the lock. + if self._object_suppressions[oid] > 0: + continue assert isinstance(obj, Object) obs_id = self._parent.add_obstacle(obstacle) if not obs_id: @@ -552,6 +572,58 @@ def remove_object_obstacle(self, object_id: str) -> bool: logger.info(f"Removed obstacle for object '{object_id}'") return True + @contextmanager + def suppress_object_obstacle(self, object_id: str) -> Iterator[ObjectObstacleSuppression]: + """Exclude one cached object obstacle for the lifetime of the context. + + Nested callers share one removal. Live refreshes skip suppressed object + IDs, and the outermost exit restores the latest cached geometry. + """ + handle = ObjectObstacleSuppression(object_id=object_id) + with self._lock: + depth = self._object_suppressions[object_id] + self._object_suppressions[object_id] = depth + 1 + if depth == 0: + obstacle_id = self._object_obstacles.get(object_id) + if obstacle_id is not None: + if not self._parent.remove_obstacle(obstacle_id): + del self._object_suppressions[object_id] + raise RuntimeError(f"failed to suppress obstacle for object '{object_id}'") + del self._object_obstacles[object_id] + handle.removed = True + try: + yield handle + finally: + self._release_object_suppression(handle) + + def _release_object_suppression(self, handle: ObjectObstacleSuppression) -> None: + object_id = handle.object_id + cached: Object | None = None + with self._lock: + depth = self._object_suppressions.get(object_id, 0) + if depth > 1: + self._object_suppressions[object_id] = depth - 1 + return + self._object_suppressions.pop(object_id, None) + entry = self._object_cache.get(object_id) + if entry is not None: + cached = entry[0] + + if cached is None: + return + obstacle = self._object_to_obstacle(cached) + with self._lock: + if self._object_suppressions.get(object_id, 0) > 0: + return + if object_id in self._object_obstacles: + return + obstacle_id = self._parent.add_obstacle(obstacle) + if obstacle_id: + self._object_obstacles[object_id] = obstacle_id + return + handle.cleanup_error = f"failed to restore obstacle for object '{object_id}'" + logger.error(handle.cleanup_error) + def clear_perception_obstacles(self) -> int: """Remove all object obstacles from the planning world. diff --git a/dimos/manipulation/planning/spec/protocols.py b/dimos/manipulation/planning/spec/protocols.py index ff33953025..36e663fc7d 100644 --- a/dimos/manipulation/planning/spec/protocols.py +++ b/dimos/manipulation/planning/spec/protocols.py @@ -42,6 +42,7 @@ VisualizationStateFrame, WorldRobotID, ) + from dimos.manipulation.visualization.layers import VisualizationLayer from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory @@ -224,6 +225,14 @@ def clear_vis_obstacles(self) -> None: """Clear obstacle representations from the visualization.""" ... + def set_layer(self, layer: VisualizationLayer) -> None: + """Replace one complete display-only visualization layer.""" + ... + + def clear_layer(self, layer_id: str) -> None: + """Clear one display-only layer while retaining viewer-owned state.""" + ... + def get_visualization_url(self) -> str | None: """Get visualization URL if enabled.""" ... diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index b856379cc7..6c4c64b1b2 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -57,6 +57,7 @@ VisualizationSession, VisualizationStateFrame, ) + from dimos.manipulation.visualization.layers import VisualizationLayer try: from pydrake.geometry import ( @@ -1208,6 +1209,14 @@ def clear_vis_obstacles(self) -> None: """Embedded Meshcat observes native WorldSpec obstacle mutations.""" return None + def set_layer(self, layer: VisualizationLayer) -> None: + """Embedded Meshcat ignores generic display-only layers.""" + return None + + def clear_layer(self, layer_id: str) -> None: + """Embedded Meshcat ignores generic display-only layers.""" + return None + def get_visualization_url(self) -> str | None: """Get visualization URL if enabled.""" if self._meshcat is not None: diff --git a/dimos/manipulation/pnpconsole.py b/dimos/manipulation/pnpconsole.py new file mode 100644 index 0000000000..304cb3d361 --- /dev/null +++ b/dimos/manipulation/pnpconsole.py @@ -0,0 +1,314 @@ +# 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. + +"""Interactive RPC client for the stepwise ``picknplace`` pipeline. + +Start the blueprint first, then run: + + uv run --no-sync python -m dimos.manipulation.pnpconsole +""" + +from __future__ import annotations + +from pprint import pprint +import time +from typing import Any + +from dimos import Dimos +from dimos.manipulation.planning.planners.roboplan_config import RoboPlanCartesianPathConfig +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped + + +def _object_number() -> int | None: + value = input("Object number: ").strip() + try: + number = int(value) + except ValueError: + print("Enter a positive whole number.") + return None + if number < 1: + print("Enter a positive whole number.") + return None + return number + + +def _grasp_rank(candidate_count: int) -> int | None: + value = input(f"Grasp rank [0-{candidate_count - 1}, Enter=0]: ").strip() + if not value: + return 0 + try: + rank = int(value) + except ValueError: + print("Enter a whole-number grasp rank.") + return None + if rank < 0 or rank >= candidate_count: + print(f"Choose a rank from 0 to {candidate_count - 1}.") + return None + return rank + + +def _print_pose(pose: Any) -> None: + if pose is None: + print("No pose is available.") + return + if frame_id := getattr(pose, "frame_id", None): + print(f"frame: {frame_id}") + print(f"position: {pose.position.as_tuple}") + print( + "orientation: " + f"({pose.orientation.x}, {pose.orientation.y}, {pose.orientation.z}, {pose.orientation.w})" + ) + + +def _cartesian_waypoints(manipulation: Any, target: Any) -> list[PoseStamped] | None: + current = manipulation.get_ee_pose("arm") + if current is None: + return None + return [ + PoseStamped(frame_id="world", position=current.position, orientation=current.orientation), + PoseStamped(frame_id="world", position=target.position, orientation=target.orientation), + ] + + +def _preview(manipulation: Any) -> None: + print(f"Viser preview: {manipulation.get_visualization_url()}") + print(manipulation.preview_plan(duration=2.0)) + + +def _print_grasp_candidates(candidates: Any) -> None: + if not candidates.candidates: + return + print(f"GraspGenX proposals: {len(candidates.candidates)}") + for rank, candidate in enumerate(candidates.candidates[:10]): + pose = candidate.pose + print( + f"{rank}: score={candidate.score:.3f} position={pose.position.as_tuple} " + f"orientation={pose.orientation.to_tuple()}" + ) + + +def main() -> None: + """Connect to PickNPlaceModule and run one explicit pick-pipeline stage.""" + print("Connecting to PickNPlaceModule...") + app = Dimos.connect() + pnp = app.pnp + manipulation = app.ManipulationModule + goal = None + pre_grasp = None + approach_planned = False + approach_executed = False + descent_planned = False + descent_executed = False + gripper_closed = False + ascent_planned = False + ascent_executed = False + print("Connected. Every planned motion is previewed in Viser before execution.") + + while True: + print("\n1) Scan 2) Info 3) Select target 4) Plan/preview approach") + print("5) Execute approach 6) Plan/preview descent 7) Execute descent 8) Close") + print("9) Plan/preview ascent 10) Execute ascent 11) Open 12) Current EE 13) Go home") + print("14) Scan/estimate/install table collision 15) Grasp + lift now (no preview)") + print("16) Describe current camera scene q) Quit") + choice = input("Select: ").strip().lower() + try: + if choice == "q": + return + if choice == "1": + prompt = input("Object prompt (blank = current detector prompt): ").strip() + detections = pnp.scan_scene(prompt or None) + print(f"Detected {detections.detections_length} object(s).") + elif choice == "2": + pprint(pnp.get_scene_info()) + elif choice == "3": + if (number := _object_number()) is not None: + goal = pnp.get_goal_pose(number) + candidates = pnp.get_grasp_candidates() + _print_grasp_candidates(candidates) + if candidates.candidates: + rank = _grasp_rank(min(10, len(candidates.candidates))) + if rank is None: + continue + goal = pnp.select_grasp_candidate(rank) + pre_grasp = pnp.get_pre_grasp_pose() + approach_planned = False + approach_executed = False + descent_planned = False + descent_executed = False + gripper_closed = False + ascent_planned = False + ascent_executed = False + print("Goal:") + _print_pose(goal) + print("Pre-grasp:") + _print_pose(pre_grasp) + elif choice == "4": + if pre_grasp is None: + print("Select a target first.") + else: + approach_planned = manipulation.plan_to_pose( + Pose(pre_grasp.position, pre_grasp.orientation), "arm" + ) + print(approach_planned) + if approach_planned: + _preview(manipulation) + elif choice == "5": + if not approach_planned: + print("Plan the approach first.") + else: + approach_executed = manipulation.execute_and_wait() + print(approach_executed) + elif choice == "6": + if goal is None or not approach_executed: + print("Execute the approach first.") + else: + waypoints = _cartesian_waypoints(manipulation, goal) + descent_planned = False + descent_planned = manipulation.plan_cartesian_targets( + {"arm/manipulator": waypoints}, + RoboPlanCartesianPathConfig(max_linear_speed=0.03), + ) + print(descent_planned) + if descent_planned: + _preview(manipulation) + elif choice == "7": + if not descent_planned: + print("Plan the descent first.") + else: + descent_executed = manipulation.execute_and_wait() + print(descent_executed) + elif choice == "8": + if not descent_executed: + print("Execute the descent first.") + else: + gripper_closed = manipulation.close_gripper("arm").is_success() + print(gripper_closed) + elif choice == "9": + if pre_grasp is None or not gripper_closed: + print("Close the gripper before planning ascent.") + else: + waypoints = _cartesian_waypoints(manipulation, pre_grasp) + ascent_planned = False + ascent_planned = manipulation.plan_cartesian_targets( + {"arm/manipulator": waypoints}, + RoboPlanCartesianPathConfig(max_linear_speed=0.03), + ) + print(ascent_planned) + if ascent_planned: + _preview(manipulation) + elif choice == "10": + if not ascent_planned: + print("Plan the ascent first.") + else: + # Gripper commands are asynchronous on xArm. Reassert close before + # lift and let that command settle before dispatching the trajectory. + gripper_closed = manipulation.close_gripper("arm").is_success() + if not gripper_closed: + print("Failed to keep the gripper closed; ascent was not executed.") + else: + time.sleep(1.5) + ascent_executed = manipulation.execute_and_wait() + print(ascent_executed) + elif choice == "11": + if not ascent_executed: + print("Execute the ascent before opening the gripper.") + else: + print(manipulation.open_gripper("arm")) + elif choice == "12": + _print_pose(manipulation.get_ee_pose("arm")) + elif choice == "13": + print(manipulation.go_home("arm")) + elif choice == "14": + # A fresh RGB-D snapshot is required before fitting the table plane. + pnp.scan_scene() + estimate = pnp.estimate_table_surface() + if estimate is None: + print("No horizontal tabletop estimate. Scan the scene and try again.") + else: + print( + "Table estimate: " + f"z={estimate['tabletop_z']:.3f} m, center=({estimate['center_x']:.3f}, " + f"{estimate['center_y']:.3f}) m, size=({estimate['width']:.3f}, " + f"{estimate['depth']:.3f}) m" + ) + clearance_text = input( + "Table clearance in mm [10 recommended, 0 = no clearance]: " + ).strip() + try: + clearance_mm = 10.0 if not clearance_text else float(clearance_text) + except ValueError: + print("Enter a non-negative clearance in millimeters.") + continue + if clearance_mm < 0.0: + print("Enter a non-negative clearance in millimeters.") + continue + print( + manipulation.set_table_collision( + estimate["center_x"], + estimate["center_y"], + estimate["tabletop_z"], + estimate["width"], + estimate["depth"], + safety_margin=clearance_mm / 1000.0, + ) + ) + elif choice == "15": + if goal is None or pre_grasp is None or not approach_executed: + print("Execute the approach first.") + continue + print("Executing descent, gripper close, and ascent without previews.") + descent_planned = False + if (waypoints := _cartesian_waypoints(manipulation, goal)) is not None: + descent_planned = manipulation.plan_cartesian_targets( + {"arm/manipulator": waypoints}, + RoboPlanCartesianPathConfig(max_linear_speed=0.03), + ) + if not descent_planned: + print("Could not plan the descent; grasp sequence stopped.") + continue + descent_executed = manipulation.execute_and_wait() + if not descent_executed: + print("Descent failed; grasp sequence stopped.") + continue + gripper_closed = manipulation.close_gripper("arm").is_success() + if not gripper_closed: + print("Failed to close the gripper; grasp sequence stopped.") + continue + # Gripper commands are asynchronous; wait before lifting the object. + time.sleep(1.5) + ascent_planned = False + if (waypoints := _cartesian_waypoints(manipulation, pre_grasp)) is not None: + ascent_planned = manipulation.plan_cartesian_targets( + {"arm/manipulator": waypoints}, + RoboPlanCartesianPathConfig(max_linear_speed=0.03), + ) + if not ascent_planned: + print("Could not plan the ascent; grasp sequence stopped with gripper closed.") + continue + ascent_executed = manipulation.execute_and_wait() + print(ascent_executed) + elif choice == "16": + question = input( + "Scene question [What objects are visible on the table?]: " + ).strip() + print(pnp.describe_scene(question or "What objects are visible on the table?")) + else: + print("Choose 1-16 or q.") + except Exception as exc: + print(f"RPC failed: {exc}") + + +if __name__ == "__main__": + main() diff --git a/dimos/manipulation/skill_errors.py b/dimos/manipulation/skill_errors.py index 9a17085ec5..c980149c78 100644 --- a/dimos/manipulation/skill_errors.py +++ b/dimos/manipulation/skill_errors.py @@ -35,6 +35,11 @@ "COLLISION_AT_START", "GRASP_GENERATION_FAILED", "GRASP_ATTEMPTS_EXHAUSTED", + "GRASP_PROVIDER_UNAVAILABLE", + "GRASP_INPUT_INVALID", + "GRASP_FRAME_MISMATCH", + "GRASP_VERIFICATION_FAILED", + "PICK_BUSY", "GRIPPER_FAILED", "WORLD_MONITOR_UNAVAILABLE", ] diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 8c886aeafa..efae275b0f 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -17,6 +17,7 @@ from __future__ import annotations from pathlib import Path +from types import SimpleNamespace from unittest.mock import MagicMock import pytest @@ -185,6 +186,22 @@ def _generated_plan_trajectory(joint_names: list[str], *points: list[float]) -> ) +def _connected_sequence_module( + robot_config: RobotModelConfig, +) -> tuple[ManipulationModule, list[str], JointState]: + module = _make_module() + module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._world_monitor = MagicMock() + module._world_monitor.world = MagicMock() + module._world_monitor.planning_groups = PlanningGroupRegistry([robot_config]) + names = ["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"] + live = JointState(name=names, position=[0.0, 0.0, 0.0]) + module._world_monitor.current_global_joint_state.return_value = live + module._kinematics = MagicMock() + module._planner = MagicMock() + return module, names, live + + def _make_trajectory(*points: tuple[float, list[float]]) -> JointTrajectory: joint_names = [f"j{i}" for i in range(len(points[0][1]))] if points else [] return JointTrajectory( @@ -196,6 +213,63 @@ def _make_trajectory(*points: tuple[float, list[float]]) -> JointTrajectory: ) +def _make_module() -> ManipulationModule: + """Create a module for pure planning helpers without opening transports.""" + module = ManipulationModule() + module.stop() + return module + + +class TestSafetyLift: + @pytest.mark.parametrize( + ("current_z", "expected_z"), + [(0.01, 0.10), (0.05, None), (0.20, None)], + ) + def test_safety_lift_target_is_derived_without_motion( + self, + mocker: MockerFixture, + current_z: float, + expected_z: float | None, + ) -> None: + module = _make_module() + orientation = Quaternion(0.0, 0.0, 0.0, 1.0) + mocker.patch.object( + module, + "get_ee_pose", + return_value=Pose(Vector3(0.2, -0.1, current_z), orientation), + ) + plan = mocker.patch.object(module, "plan_to_pose") + + target = module._safety_lift_pose("test_arm") + + if expected_z is None: + assert target is None + else: + assert target is not None + assert target.position.as_tuple == pytest.approx((0.2, -0.1, expected_z)) + assert ( + target.orientation.x, + target.orientation.y, + target.orientation.z, + target.orientation.w, + ) == pytest.approx((0.0, 0.0, 0.0, 1.0)) + plan.assert_not_called() + + +class TestAgentMotionRecovery: + def test_move_to_pose_explains_how_to_recover_from_fault(self, module_factory) -> None: + module = module_factory() + module._state = ManipulationState.FAULT + module._error_message = "Trajectory execution timed out" + + result = module.move_to_pose(0.2, 0.0, 0.1) + + assert not result.is_success() + assert result.error_code == "INVALID_STATE" + assert "FAULT" in result.message + assert "reset" in result.message + + class TestObstacleUpdates: def test_complete_update_forwards_new_obstacle_value(self, module_factory) -> None: module = module_factory() @@ -761,6 +835,306 @@ def test_plan_to_pose_targets_uses_group_ik_and_selected_path( _, planner_kwargs = module._planner.plan_selected_joint_path.call_args assert planner_kwargs["goal"] is ik_goal + def test_connected_pose_check_chains_each_plan_endpoint_into_the_next_start( + self, robot_config, mocker: MockerFixture + ): + module, names, live = _connected_sequence_module(robot_config) + goals = [ + JointState(name=names, position=[0.1, 0.0, 0.0]), + JointState(name=names, position=[0.2, 0.1, 0.0]), + JointState(name=names, position=[0.1, 0.2, 0.1]), + ] + solve = mocker.patch.object( + module, + "inverse_kinematics", + side_effect=[IKResult(status=IKStatus.SUCCESS, joint_state=goal) for goal in goals], + ) + module._planner.plan_selected_joint_path.side_effect = lambda **kwargs: PlanningResult( + status=PlanningStatus.SUCCESS, + path=[kwargs["start"], kwargs["goal"]], + ) + poses = [ + Pose(position=Vector3(x=0.4 + index * 0.1), orientation=Quaternion()) + for index in range(3) + ] + + failed_index, endpoint = module._check_connected_pose_sequence(poses, "test_arm") + + assert failed_index is None + assert endpoint is not None + assert endpoint.position == goals[-1].position + ik_seeds = [call.kwargs["seed"] for call in solve.call_args_list] + planner_starts = [ + call.kwargs["start"] for call in module._planner.plan_selected_joint_path.call_args_list + ] + assert [seed.position for seed in ik_seeds] == [ + live.position, + goals[0].position, + goals[1].position, + ] + assert [start.position for start in planner_starts] == [ + live.position, + goals[0].position, + goals[1].position, + ] + assert module._state == ManipulationState.IDLE + assert module._last_plan is None + + def test_connected_pose_plan_exposes_paths_without_storing_them( + self, robot_config, mocker: MockerFixture + ) -> None: + module, names, live = _connected_sequence_module(robot_config) + goals = [ + JointState(name=names, position=[0.1, 0.0, 0.0]), + JointState(name=names, position=[0.2, 0.1, 0.0]), + ] + mocker.patch.object( + module, + "inverse_kinematics", + side_effect=[IKResult(status=IKStatus.SUCCESS, joint_state=goal) for goal in goals], + ) + module._planner.plan_selected_joint_path.side_effect = lambda **kwargs: PlanningResult( + status=PlanningStatus.SUCCESS, + path=[kwargs["start"], kwargs["goal"]], + ) + + result = module._plan_connected_pose_sequence( + [ + Pose(position=Vector3(x=0.4), orientation=Quaternion()), + Pose(position=Vector3(x=0.5), orientation=Quaternion()), + ], + "test_arm", + ) + + assert result.failed_index is None + assert result.endpoint is not None + assert result.endpoint.position == goals[-1].position + assert [[state.position for state in path] for path in result.paths] == [ + [live.position, goals[0].position], + [goals[0].position, goals[1].position], + ] + assert module._state == ManipulationState.IDLE + assert module._last_plan is None + + def test_connected_pose_check_accepts_explicit_start(self, robot_config, mocker: MockerFixture): + module, names, _ = _connected_sequence_module(robot_config) + explicit_start = JointState(name=names, position=[0.3, 0.2, 0.1]) + goal = JointState(name=names, position=[0.4, 0.2, 0.1]) + solve = mocker.patch.object( + module, + "inverse_kinematics", + return_value=IKResult(status=IKStatus.SUCCESS, joint_state=goal), + ) + module._planner.plan_selected_joint_path.return_value = PlanningResult( + status=PlanningStatus.SUCCESS, + path=[explicit_start, goal], + ) + + failed_index, endpoint = module._check_connected_pose_sequence( + [Pose(position=Vector3(x=0.5), orientation=Quaternion())], + "test_arm", + explicit_start, + ) + + assert failed_index is None + assert endpoint is not None + assert endpoint.position == goal.position + assert solve.call_args.kwargs["seed"].position == explicit_start.position + assert ( + module._planner.plan_selected_joint_path.call_args.kwargs["start"].position + == explicit_start.position + ) + module._world_monitor.current_global_joint_state.assert_not_called() + + def test_connected_pose_check_returns_first_ik_failure_with_diagnostics( + self, + robot_config, + mocker: MockerFixture, + ): + module, names, live = _connected_sequence_module(robot_config) + first_goal = JointState(name=names, position=[0.1, 0.0, 0.0]) + mocker.patch.object( + module, + "inverse_kinematics", + side_effect=[ + IKResult(status=IKStatus.SUCCESS, joint_state=first_goal), + IKResult(status=IKStatus.NO_SOLUTION, message="blocked IK"), + ], + ) + module._planner.plan_selected_joint_path.return_value = PlanningResult( + status=PlanningStatus.SUCCESS, + path=[live, first_goal], + ) + poses = [ + Pose(position=Vector3(x=0.4), orientation=Quaternion()), + Pose(position=Vector3(x=0.5), orientation=Quaternion()), + ] + log_info = mocker.patch("dimos.manipulation.manipulation_module.logger.info") + + failed_index, endpoint = module._check_connected_pose_sequence(poses, "test_arm") + + assert (failed_index, endpoint) == (1, None) + log_info.assert_called_with( + "Connected pose planning failed IK at index %d: %s%s", + 1, + "NO_SOLUTION", + ": blocked IK", + ) + assert module._planner.plan_selected_joint_path.call_count == 1 + + def test_connected_pose_check_returns_path_failure_with_diagnostics( + self, + robot_config, + mocker: MockerFixture, + ): + module, names, _ = _connected_sequence_module(robot_config) + goal = JointState(name=names, position=[0.1, 0.0, 0.0]) + mocker.patch.object( + module, + "inverse_kinematics", + return_value=IKResult(status=IKStatus.SUCCESS, joint_state=goal), + ) + module._planner.plan_selected_joint_path.return_value = PlanningResult( + status=PlanningStatus.NO_SOLUTION, + message="blocked path", + ) + log_info = mocker.patch("dimos.manipulation.manipulation_module.logger.info") + + failed_index, endpoint = module._check_connected_pose_sequence( + [Pose(position=Vector3(x=0.4), orientation=Quaternion())], + "test_arm", + ) + + assert (failed_index, endpoint) == (0, None) + log_info.assert_called_with( + "Connected pose planning failed path at index %d: %s%s", + 0, + "NO_SOLUTION", + ": blocked path", + ) + + @pytest.mark.parametrize( + "path", + [ + [], + [ + JointState( + name=["wrong/joint1", "wrong/joint2", "wrong/joint3"], + position=[0.1, 0.0, 0.0], + ) + ], + ], + ids=["empty", "malformed-endpoint"], + ) + def test_connected_pose_check_rejects_invalid_success_path( + self, + robot_config, + mocker: MockerFixture, + path: list[JointState], + ): + module, names, _ = _connected_sequence_module(robot_config) + goal = JointState(name=names, position=[0.1, 0.0, 0.0]) + mocker.patch.object( + module, + "inverse_kinematics", + return_value=IKResult(status=IKStatus.SUCCESS, joint_state=goal), + ) + module._planner.plan_selected_joint_path.return_value = PlanningResult( + status=PlanningStatus.SUCCESS, + path=path, + ) + + result = module._check_connected_pose_sequence( + [Pose(position=Vector3(x=0.4), orientation=Quaternion())], + "test_arm", + ) + + assert result == (0, None) + + @pytest.mark.parametrize( + "blocked_index", + [None, 0, 1, 2], + ids=["open-scene", "pre-grasp-blocked", "grasp-blocked", "retreat-blocked"], + ) + def test_connected_pose_check_uses_one_reusable_scene_with_optional_blocker( + self, + robot_config, + mocker: MockerFixture, + blocked_index: int | None, + ) -> None: + module, names, live = _connected_sequence_module(robot_config) + goals = [ + JointState(name=names, position=[0.1 + index * 0.1, 0.0, 0.0]) for index in range(3) + ] + mocker.patch.object( + module, + "inverse_kinematics", + side_effect=[IKResult(status=IKStatus.SUCCESS, joint_state=goal) for goal in goals], + ) + starts = [live, goals[0], goals[1]] + module._planner.plan_selected_joint_path.side_effect = [ + ( + PlanningResult(status=PlanningStatus.NO_SOLUTION, message="scene blocker") + if index == blocked_index + else PlanningResult( + status=PlanningStatus.SUCCESS, + path=[starts[index], goals[index]], + ) + ) + for index in range(3) + ] + poses = [ + Pose(position=Vector3(x=0.4 + index * 0.1), orientation=Quaternion()) + for index in range(3) + ] + + failed_index, endpoint = module._check_connected_pose_sequence(poses, "test_arm") + + assert failed_index == blocked_index + if blocked_index is None: + assert endpoint is not None + assert endpoint.position == goals[-1].position + else: + assert endpoint is None + assert module._planner.plan_selected_joint_path.call_count == blocked_index + 1 + + def test_connected_pose_check_observes_live_scene_on_each_segment( + self, robot_config, mocker: MockerFixture + ) -> None: + module, names, live = _connected_sequence_module(robot_config) + world = SimpleNamespace(revision=0) + module._world_monitor.world = world + goals = [ + JointState(name=names, position=[0.1 + index * 0.1, 0.0, 0.0]) for index in range(3) + ] + mocker.patch.object( + module, + "inverse_kinematics", + side_effect=[IKResult(status=IKStatus.SUCCESS, joint_state=goal) for goal in goals], + ) + starts = [live, goals[0], goals[1]] + observed_revisions: list[int] = [] + + def plan_with_live_scene(**kwargs) -> PlanningResult: + observed_revisions.append(kwargs["world"].revision) + index = len(observed_revisions) - 1 + world.revision += 1 + return PlanningResult( + status=PlanningStatus.SUCCESS, + path=[starts[index], goals[index]], + ) + + module._planner.plan_selected_joint_path.side_effect = plan_with_live_scene + poses = [ + Pose(position=Vector3(x=0.4 + index * 0.1), orientation=Quaternion()) + for index in range(3) + ] + + failed_index, _ = module._check_connected_pose_sequence(poses, "test_arm") + + assert failed_index is None + assert observed_revisions == [0, 1, 2] + def test_failed_plan_materialization_clears_generated_plan(self, robot_config, module_factory): module = module_factory() registry = PlanningGroupRegistry([robot_config]) diff --git a/dimos/manipulation/test_pick_and_place_unit.py b/dimos/manipulation/test_pick_and_place_unit.py index 6ee984f0e2..1dd84ae38c 100644 --- a/dimos/manipulation/test_pick_and_place_unit.py +++ b/dimos/manipulation/test_pick_and_place_unit.py @@ -16,18 +16,42 @@ from __future__ import annotations +from collections import Counter +from contextlib import nullcontext +import json +from types import SimpleNamespace from unittest.mock import patch +import numpy as np import open3d as o3d import pytest +from pytest_mock import MockerFixture +from dimos.agents.skill_result import SkillResult +from dimos.core.coordination.blueprints import BlueprintAtom, autoconnect +from dimos.core.coordination.module_coordinator import _resolve_single_ref from dimos.core.module import ModuleBase -from dimos.manipulation.pick_and_place_module import PickAndPlaceModule +from dimos.manipulation.grasping.grasp_gen_x import GraspGenXModule +from dimos.manipulation.pick_and_place_module import ( + GraspVerificationConfig, + PickAndPlaceModule, + PickAndPlaceModuleConfig, + _FeasibleGrasp, + _GraspVerification, +) +from dimos.manipulation.skill_errors import ManipulationSkillError +from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.manipulation_msgs.GraspCandidate import GraspCandidate +from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.sensor_msgs.JointState import JointState from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.std_msgs.Header import Header from dimos.perception.experimental.object import Object as DetObject +from dimos.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule def _make_det_object( @@ -57,7 +81,9 @@ def _make_det_object( def module() -> PickAndPlaceModule: """Create a PickAndPlaceModule with heavy base init (RPC, config) patched out.""" with patch.object(ModuleBase, "__init__", lambda self, config_args: None): - return PickAndPlaceModule() + result = PickAndPlaceModule() + result.config = PickAndPlaceModuleConfig() + return result class TestFindObjectInDetections: @@ -99,6 +125,14 @@ def test_find_missing_returns_none(self, module): result = module._find_object_in_detections("keyboard") assert result is None + def test_find_by_name_requires_unique_match(self, module): + module._detection_snapshot = [ + _make_det_object(name="cup", object_id="first"), + _make_det_object(name="red cup", object_id="second"), + ] + + assert module._find_object_in_detections("cup") is None + def test_empty_snapshot_returns_none(self, module): module._detection_snapshot = [] @@ -158,3 +192,639 @@ def test_place_back_no_pick_pose_errors(self, module): assert not result.is_success() assert result.error_code == "NO_PRIOR_POSE" assert "pick" in result.message.lower() + + +def test_grasp_pipeline_error_agent_encoding_is_structured() -> None: + result = SkillResult[ManipulationSkillError].fail("PICK_BUSY", "pick in progress") + + payload = json.loads(result.agent_encode()[0]["text"]) + + assert payload == { + "success": False, + "message": "pick in progress", + "error_code": "PICK_BUSY", + "duration_ms": 0.0, + } + + +@pytest.mark.parametrize( + ("kwargs", "message"), + [ + ({"planning_frame": " "}, "planning_frame"), + ({"grasp_approach_vector": (0.0, 0.0, 2.0)}, "unit vector"), + ( + { + "grasp_verification": { + "open_position": 0.85, + "closed_position": 0.0, + "held_threshold": 0.9, + } + }, + "held_threshold", + ), + ], +) +def test_grasp_pipeline_config_rejects_invalid_values( + kwargs: dict[str, object], message: str +) -> None: + with pytest.raises(ValueError, match=message): + PickAndPlaceModuleConfig(**kwargs) + + +def test_pick_module_declares_optional_perception_and_grasp_specs() -> None: + atom = BlueprintAtom.create(PickAndPlaceModule, kwargs={}) + + refs = {ref.name: ref for ref in atom.module_refs} + + assert refs["_object_scene"].optional is True + assert refs["_grasp_generator"].optional is True + + +@pytest.mark.parametrize( + ("ref_name", "provider"), + [ + ("_grasp_generator", GraspGenXModule), + ("_object_scene", ObjectSceneRegistrationModule), + ], +) +def test_optional_provider_resolves_when_absent_present_or_ambiguous( + ref_name: str, provider: type[ModuleBase] +) -> None: + consumer = BlueprintAtom.create(PickAndPlaceModule, kwargs={}) + module_ref = next(ref for ref in consumer.module_refs if ref.name == ref_name) + + absent = autoconnect(PickAndPlaceModule.blueprint()) + assert _resolve_single_ref(consumer, module_ref, module_ref.spec, absent, set()) is None + + present = autoconnect(PickAndPlaceModule.blueprint(), provider.blueprint()) + assert ( + _resolve_single_ref(consumer, module_ref, module_ref.spec, present, set()) == provider.name + ) + + ambiguous = autoconnect( + PickAndPlaceModule.blueprint(), + provider.blueprint(instance_name="provider-a"), + provider.blueprint(instance_name="provider-b"), + ) + with pytest.raises(Exception, match="Multiple modules met that spec"): + _resolve_single_ref(consumer, module_ref, module_ref.spec, ambiguous, set()) + + +def _pointcloud(frame_id: str = "world", timestamp: float | None = None) -> PointCloud2: + return PointCloud2.from_numpy( + np.asarray([[0.4, 0.0, 0.2], [0.41, 0.01, 0.2]], dtype=np.float32), + frame_id=frame_id, + timestamp=timestamp, + ) + + +def _candidate(x: float, score: float) -> GraspCandidate: + return GraspCandidate( + Pose(Vector3(x, 0.0, 0.2), Quaternion(0.0, 0.0, 0.0, 1.0)), + score, + ) + + +class TestProposalSelection: + def test_provider_receives_real_world_frame_cloud( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + now = 100.0 + cloud = _pointcloud(timestamp=now) + detection = _make_det_object() + scene = mocker.Mock() + scene.get_object_pointcloud_by_object_id.return_value = cloud + generator = mocker.Mock() + generator.propose_grasps.return_value = GraspCandidateArray( + Header(now, "world"), [_candidate(0.4, 0.8)] + ) + module._object_scene = scene + module._grasp_generator = generator + mocker.patch("dimos.manipulation.pick_and_place_module.time.time", return_value=now + 0.1) + + candidates = module._provider_candidates( + detection, SimpleNamespace(proposal_source="grasp_provider") + ) + + generator.propose_grasps.assert_called_once_with(cloud) + assert [(candidate.pose.position.x, candidate.score) for candidate in candidates] == [ + (0.4, 0.8) + ] + + @pytest.mark.parametrize("cloud_available", [False, True]) + def test_provider_rejects_missing_or_stale_cloud( + self, + module: PickAndPlaceModule, + mocker: MockerFixture, + cloud_available: bool, + ) -> None: + scene = mocker.Mock() + scene.get_object_pointcloud_by_object_id.return_value = ( + _pointcloud(timestamp=1.0) if cloud_available else None + ) + module._object_scene = scene + module._grasp_generator = mocker.Mock() + mocker.patch("dimos.manipulation.pick_and_place_module.time.time", return_value=100.0) + + with pytest.raises(RuntimeError, match="point cloud"): + module._provider_candidates( + _make_det_object(), SimpleNamespace(proposal_source="grasp_provider") + ) + + @pytest.mark.parametrize( + ("cloud_frame", "proposal_frame"), + [("camera", "world"), ("world", "camera")], + ) + def test_provider_rejects_frame_mismatch( + self, + module: PickAndPlaceModule, + mocker: MockerFixture, + cloud_frame: str, + proposal_frame: str, + ) -> None: + now = 100.0 + scene = mocker.Mock() + scene.get_object_pointcloud_by_object_id.return_value = _pointcloud( + cloud_frame, timestamp=now + ) + generator = mocker.Mock() + generator.propose_grasps.return_value = GraspCandidateArray( + Header(now, proposal_frame), [_candidate(0.4, 0.8)] + ) + module._object_scene = scene + module._grasp_generator = generator + mocker.patch("dimos.manipulation.pick_and_place_module.time.time", return_value=now) + + with pytest.raises(RuntimeError, match="frame"): + module._provider_candidates( + _make_det_object(), SimpleNamespace(proposal_source="grasp_provider") + ) + + def test_provider_preserves_stable_order_for_equal_scores( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + now = 100.0 + scene = mocker.Mock() + scene.get_object_pointcloud_by_object_id.return_value = _pointcloud(timestamp=now) + generator = mocker.Mock() + generator.propose_grasps.return_value = GraspCandidateArray( + Header(now, "world"), + [_candidate(0.1, 0.5), _candidate(0.2, 0.7), _candidate(0.3, 0.7)], + ) + module._object_scene = scene + module._grasp_generator = generator + mocker.patch("dimos.manipulation.pick_and_place_module.time.time", return_value=now) + + candidates = module._provider_candidates( + _make_det_object(), SimpleNamespace(proposal_source="grasp_provider") + ) + + assert [candidate.pose.position.x for candidate in candidates] == [0.2, 0.3, 0.1] + + def test_explicit_heuristic_fallback_identifies_source( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + module.config.heuristic_grasp_fallback = True + transaction = SimpleNamespace(proposal_source="grasp_provider") + pose = Pose(0.4, 0.0, 0.2) + mocker.patch.object(module, "_generate_grasps_for_pick", return_value=[pose]) + + candidates = module._provider_candidates(_make_det_object(), transaction) + + assert transaction.proposal_source == "heuristic" + assert [(candidate.pose, candidate.score) for candidate in candidates] == [(pose, 0.0)] + + def test_provider_is_required_when_fallback_is_disabled( + self, module: PickAndPlaceModule + ) -> None: + with pytest.raises(RuntimeError, match="fallback is disabled"): + module._provider_candidates( + _make_det_object(), SimpleNamespace(proposal_source="grasp_provider") + ) + + def test_selection_skips_higher_scored_infeasible_candidate( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + endpoint = JointState(name=["arm/joint1"], position=[0.1]) + plan_sequence = mocker.patch.object( + module, + "_check_connected_pose_sequence", + side_effect=[(0, None), (None, endpoint)], + ) + plan_motion = mocker.patch.object(module, "plan_to_pose") + command_gripper = mocker.patch.object(module, "_set_gripper_position") + transaction = SimpleNamespace(rejections=Counter()) + + selected = module._select_feasible_grasp( + [_candidate(0.4, 0.9), _candidate(0.5, 0.8)], + "arm", + 0.1, + transaction, + ) + + assert selected.rank == 2 + assert selected.candidate.score == 0.8 + assert plan_sequence.call_count == 2 + assert transaction.rejections == {"pre_grasp_infeasible": 1} + plan_motion.assert_not_called() + command_gripper.assert_not_called() + + @pytest.mark.parametrize( + ("failed_index", "expected_rejection"), + [ + (0, "pre_grasp_infeasible"), + (1, "grasp_infeasible"), + (2, "retreat_infeasible"), + ], + ) + def test_selection_reports_failed_connected_segment( + self, + module: PickAndPlaceModule, + mocker: MockerFixture, + failed_index: int, + expected_rejection: str, + ) -> None: + mocker.patch.object( + module, + "_check_connected_pose_sequence", + return_value=(failed_index, None), + ) + transaction = SimpleNamespace(rejections=Counter()) + + with pytest.raises(RuntimeError, match="No feasible grasp among 1"): + module._select_feasible_grasp([_candidate(0.4, 0.9)], "arm", 0.1, transaction) + + assert transaction.rejections == {expected_rejection: 1} + + def test_selection_rejects_malformed_candidate_and_honors_limit( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + module.config.max_grasp_candidates_to_check = 1 + invalid = _candidate(0.4, 0.9) + invalid.pose.orientation.w = 0.0 + plan_sequence = mocker.patch.object(module, "_check_connected_pose_sequence") + transaction = SimpleNamespace(rejections=Counter()) + + with pytest.raises(RuntimeError, match="No feasible grasp among 1"): + module._select_feasible_grasp([invalid, _candidate(0.5, 0.8)], "arm", 0.1, transaction) + + plan_sequence.assert_not_called() + assert transaction.rejections == {"invalid": 1} + + +class TestPickTransaction: + def _arrange_success( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> tuple[GraspCandidate, SimpleNamespace]: + detection = _make_det_object() + candidate = _candidate(0.4, 0.9) + selected = _FeasibleGrasp(candidate, 1, Pose(0.4, 0.0, 0.3), Pose(0.4, 0.0, 0.3)) + robot_config = SimpleNamespace(pre_grasp_offset=0.1) + mocker.patch.object( + module, "_get_robot", return_value=("arm", "robot-id", robot_config, None) + ) + mocker.patch.object(module, "_require_pick_object", return_value=detection) + mocker.patch.object(module, "_provider_candidates", return_value=[candidate]) + mocker.patch.object(module, "_select_feasible_grasp", return_value=selected) + mocker.patch.object(module, "_safety_lift_pose", return_value=None) + mocker.patch.object(module, "_lift_if_low", return_value=SkillResult.ok()) + mocker.patch.object(module, "plan_to_pose", return_value=True) + mocker.patch.object(module, "_preview_execute_wait", return_value=SkillResult.ok()) + mocker.patch.object(module, "_set_gripper_position", return_value=True) + mocker.patch.object( + module, + "_verify_grasp", + return_value=_GraspVerification(True, 0.1, "verified"), + ) + suppression = SimpleNamespace(cleanup_error=None) + world = mocker.Mock() + world.suppress_object_obstacle.return_value = nullcontext(suppression) + module._world_monitor = world + return candidate, suppression + + def test_success_executes_ordered_pick_and_records_metadata( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + candidate, _ = self._arrange_success(module, mocker) + + result = module.pick("cup", object_id="abc12345") + + assert result.is_success() + assert result.metadata["candidate_rank"] == 1 + assert result.metadata["candidate_score"] == 0.9 + assert module._last_pick_pose is candidate.pose + assert module._set_gripper_position.call_args_list == [ + mocker.call(0.85, "arm"), + mocker.call(0.0, "arm"), + ] + assert module.plan_to_pose.call_args_list == [ + mocker.call(Pose(0.4, 0.0, 0.3), "arm"), + mocker.call(candidate.pose, "arm"), + mocker.call(Pose(0.4, 0.0, 0.3), "arm"), + ] + + def test_no_safety_lift_validates_candidates_from_current_state( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + self._arrange_success(module, mocker) + check_sequence = mocker.patch.object(module, "_check_connected_pose_sequence") + + result = module.pick("cup", object_id="abc12345") + + assert result.is_success() + check_sequence.assert_not_called() + assert module._select_feasible_grasp.call_args.args[4] is None + + def test_safety_lift_endpoint_is_shared_with_candidate_validation( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + self._arrange_success(module, mocker) + lift_pose = Pose(0.2, 0.0, 0.1) + lift_endpoint = JointState(name=["arm/joint1"], position=[0.2]) + module._safety_lift_pose.return_value = lift_pose + check_sequence = mocker.patch.object( + module, + "_check_connected_pose_sequence", + return_value=(None, lift_endpoint), + ) + + result = module.pick("cup", object_id="abc12345") + + assert result.is_success() + check_sequence.assert_called_once_with((lift_pose,), "arm") + assert module._select_feasible_grasp.call_args.args[4] is lift_endpoint + + def test_safety_lift_planning_failure_aborts_prepare_without_candidate_rejection( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + self._arrange_success(module, mocker) + lift_pose = Pose(0.2, 0.0, 0.1) + module._safety_lift_pose.return_value = lift_pose + check_sequence = mocker.patch.object( + module, + "_check_connected_pose_sequence", + return_value=(0, None), + ) + + result = module.pick("cup", object_id="abc12345") + + assert result.error_code == "PLANNING_FAILED" + assert result.metadata["phase"] == "PREPARE" + assert result.metadata["rejections"] == {} + check_sequence.assert_called_once_with((lift_pose,), "arm") + module._select_feasible_grasp.assert_not_called() + module._lift_if_low.assert_not_called() + module.plan_to_pose.assert_not_called() + module._set_gripper_position.assert_not_called() + + def test_retreat_failure_keeps_gripper_closed( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + self._arrange_success(module, mocker) + module.plan_to_pose.side_effect = [True, True, False] + + result = module.pick("cup", object_id="abc12345") + + assert result.error_code == "PLANNING_FAILED" + assert result.metadata["object_may_be_held"] is True + assert module._set_gripper_position.call_args_list == [ + mocker.call(0.85, "arm"), + mocker.call(0.0, "arm"), + ] + + def test_concurrent_pick_is_rejected_without_robot_access( + self, + module: PickAndPlaceModule, + mocker: MockerFixture, + ) -> None: + get_robot = mocker.patch.object(module, "_get_robot") + log = mocker.patch("dimos.agents.annotation.logger.info") + module._pick_guard.acquire() + try: + result = module.pick("cup") + finally: + module._pick_guard.release() + + assert result.error_code == "PICK_BUSY" + get_robot.assert_not_called() + log.assert_called_once() + assert log.call_args.args[:3] == ( + "SKILL %s result=%s duration_ms=%.1f", + "pick", + "PICK_BUSY", + ) + + def test_cleanup_failure_does_not_hide_primary_failure( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + _, suppression = self._arrange_success(module, mocker) + suppression.cleanup_error = "restore failed" + module.plan_to_pose.side_effect = [False] + + result = module.pick("cup", object_id="abc12345") + + assert result.error_code == "PLANNING_FAILED" + assert "cleanup: restore failed" in result.message + + def test_cleanup_failure_turns_success_into_scene_failure( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + _, suppression = self._arrange_success(module, mocker) + suppression.cleanup_error = "restore failed" + + result = module.pick("cup", object_id="abc12345") + + assert result.error_code == "WORLD_MONITOR_UNAVAILABLE" + assert "restore failed" in result.message + + @pytest.mark.parametrize( + ("setup", "expected_code", "expected_phase"), + [ + ("prepare", "EXECUTION_FAILED", "PREPARE"), + ("open", "GRIPPER_FAILED", "PREPARE"), + ("approach_planning", "PLANNING_FAILED", "APPROACH"), + ("approach_execution", "EXECUTION_FAILED", "APPROACH"), + ("grasp_planning", "PLANNING_FAILED", "GRASP"), + ("grasp_execution", "EXECUTION_FAILED", "GRASP"), + ("close", "GRIPPER_FAILED", "CLOSE"), + ("verification", "GRASP_VERIFICATION_FAILED", "VERIFY"), + ("retreat_planning", "PLANNING_FAILED", "RETREAT"), + ("retreat_execution", "EXECUTION_FAILED", "RETREAT"), + ], + ) + def test_phase_failures_stop_the_pipeline( + self, + module: PickAndPlaceModule, + mocker: MockerFixture, + setup: str, + expected_code: str, + expected_phase: str, + ) -> None: + self._arrange_success(module, mocker) + if setup == "prepare": + module._lift_if_low.return_value = SkillResult.fail("EXECUTION_FAILED", "lift failed") + elif setup == "open": + module._set_gripper_position.return_value = False + elif setup == "approach_planning": + module.plan_to_pose.side_effect = [False] + elif setup == "approach_execution": + module._preview_execute_wait.side_effect = [ + SkillResult.fail("EXECUTION_FAILED", "rejected") + ] + elif setup == "grasp_planning": + module.plan_to_pose.side_effect = [True, False] + elif setup == "grasp_execution": + module._preview_execute_wait.side_effect = [ + SkillResult.ok(), + SkillResult.fail("EXECUTION_FAILED", "rejected"), + ] + elif setup == "close": + module._set_gripper_position.side_effect = [True, False] + elif setup == "verification": + module._verify_grasp.return_value = _GraspVerification(False, 0.0, "empty close") + elif setup == "retreat_planning": + module.plan_to_pose.side_effect = [True, True, False] + else: + module._preview_execute_wait.side_effect = [ + SkillResult.ok(), + SkillResult.ok(), + SkillResult.fail("EXECUTION_FAILED", "rejected"), + ] + + result = module.pick("cup", object_id="abc12345") + + assert result.error_code == expected_code + assert result.metadata["phase"] == expected_phase + module._select_feasible_grasp.assert_called_once() + + +def test_full_pick_pipeline_uses_real_messages_and_fake_boundary_providers( + module: PickAndPlaceModule, mocker: MockerFixture +) -> None: + now = 100.0 + detection = _make_det_object() + module._detection_snapshot = [detection] + scene = mocker.Mock() + scene.get_object_pointcloud_by_object_id.return_value = _pointcloud(timestamp=now) + generator = mocker.Mock() + generator.propose_grasps.return_value = GraspCandidateArray( + Header(now, "world"), + [_candidate(0.4, 0.9), _candidate(0.5, 0.8)], + ) + module._object_scene = scene + module._grasp_generator = generator + robot_config = SimpleNamespace(pre_grasp_offset=0.1) + mocker.patch.object(module, "_get_robot", return_value=("arm", "robot-id", robot_config, None)) + plan_sequence = mocker.patch.object( + module, + "_check_connected_pose_sequence", + side_effect=[(0, None), (None, JointState())], + ) + mocker.patch.object(module, "_safety_lift_pose", return_value=None) + mocker.patch.object(module, "_lift_if_low", return_value=SkillResult.ok()) + plan = mocker.patch.object(module, "plan_to_pose", return_value=True) + execute = mocker.patch.object(module, "_preview_execute_wait", return_value=SkillResult.ok()) + gripper = mocker.patch.object(module, "_set_gripper_position", return_value=True) + suppression = SimpleNamespace(cleanup_error=None) + world = mocker.Mock() + world.suppress_object_obstacle.return_value = nullcontext(suppression) + module._world_monitor = world + mocker.patch("dimos.manipulation.pick_and_place_module.time.time", return_value=now) + + result = module.pick("cup", object_id="abc12345") + + assert result.is_success() + assert result.metadata["proposal_source"] == "grasp_provider" + assert result.metadata["candidate_rank"] == 2 + assert result.metadata["candidate_score"] == 0.8 + 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 + 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")] + + +class TestGraspVerification: + def test_empty_close_fails_immediately( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + module.config.grasp_verification = GraspVerificationConfig( + enabled=True, + timeout=1.0, + poll_interval=0.1, + held_threshold=0.02, + ) + mocker.patch.object(module, "get_gripper", return_value=0.0) + mocker.patch( + "dimos.manipulation.pick_and_place_module.time.monotonic", + side_effect=[0.0, 0.1], + ) + + result = module._verify_grasp("arm") + + assert result == _GraspVerification(False, 0.0, "gripper reached the empty-closed region") + + def test_held_position_succeeds_after_timeout( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + module.config.grasp_verification = GraspVerificationConfig( + enabled=True, + timeout=1.0, + poll_interval=0.1, + held_threshold=0.02, + ) + mocker.patch.object(module, "get_gripper", return_value=0.1) + mocker.patch( + "dimos.manipulation.pick_and_place_module.time.monotonic", + side_effect=[0.0, 0.1, 1.1], + ) + sleep = mocker.patch("dimos.manipulation.pick_and_place_module.time.sleep") + + result = module._verify_grasp("arm") + + assert result == _GraspVerification(True, 0.1, "grasp verified by gripper closure feedback") + sleep.assert_called_once_with(0.1) + + def test_no_gripper_motion_is_not_misclassified_as_a_grasp( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + module.config.grasp_verification = GraspVerificationConfig( + enabled=True, + timeout=1.0, + poll_interval=0.1, + held_threshold=0.02, + ) + mocker.patch.object(module, "get_gripper", return_value=0.85) + mocker.patch( + "dimos.manipulation.pick_and_place_module.time.monotonic", + side_effect=[0.0, 0.1, 1.1], + ) + mocker.patch("dimos.manipulation.pick_and_place_module.time.sleep") + + result = module._verify_grasp("arm") + + assert result == _GraspVerification(False, 0.85, "gripper did not leave the open position") + + def test_feedback_timeout_is_reported( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + module.config.grasp_verification = GraspVerificationConfig( + enabled=True, + timeout=1.0, + poll_interval=0.1, + held_threshold=0.02, + ) + mocker.patch.object(module, "get_gripper", return_value=None) + mocker.patch( + "dimos.manipulation.pick_and_place_module.time.monotonic", + side_effect=[0.0, 0.1, 1.1], + ) + mocker.patch("dimos.manipulation.pick_and_place_module.time.sleep") + + result = module._verify_grasp("arm") + + assert result == _GraspVerification(False, None, "gripper feedback was unavailable") diff --git a/dimos/manipulation/test_picknplace.py b/dimos/manipulation/test_picknplace.py new file mode 100644 index 0000000000..f1d693800f --- /dev/null +++ b/dimos/manipulation/test_picknplace.py @@ -0,0 +1,456 @@ +# 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. + +import math +from unittest.mock import MagicMock, patch + +import numpy as np +import pytest + +from dimos.core.coordination.blueprint_config.parser import BlueprintConfigParser +from dimos.core.module import ModuleBase +from dimos.manipulation.blueprints import _picknplace_xarm6_model, _xarm_graspgenx, picknplace +from dimos.manipulation.picknplace import ( + PickNPlaceConfig, + PickNPlaceModule, + _estimate_table_surface, + _table_midpoint_grasp_z, +) +from dimos.manipulation.planning.spec.models import IKResult, IKStatus +from dimos.manipulation.visualization.layers import MeshElement +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.manipulation_msgs.GraspCandidate import GraspCandidate +from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray +from dimos.msgs.std_msgs.Header import Header +from dimos.robot.manipulators.xarm.grasp_config import XARM_TCP_TO_GRASP_FRAME + + +def test_picknplace_scans_and_selects_target() -> None: + with patch.object(ModuleBase, "__init__", lambda self, config_args: None): + module = PickNPlaceModule() + module.config = PickNPlaceConfig() + module._visualization = MagicMock() + scene = MagicMock() + detections = MagicMock() + module._scene = scene + obj = MagicMock( + ts=1.0, + frame_id="link_base", + center=Vector3(0.1, 0.2, 0.04), + confidence=0.9, + ) + obj.name = "cup" + obj.size = Vector3(0.4, 0.1, 0.2) + obj.camera_transform = None + obj.image = None + obj.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) + scene.scan_scene.side_effect = lambda: (module._on_objects([obj]), detections)[1] + + with patch("dimos.manipulation.picknplace.to_detection3d_array") as to_detection3d_array: + result = MagicMock() + to_detection3d_array.return_value = result + assert module.scan_scene() is result + to_detection3d_array.assert_called_once_with([obj], frame_id="link_base", ts=1.0) + + assert module.get_scene_info() == [{"number": 1, "name": "cup", "confidence": 0.9}] + goal = module.get_goal_pose(1) + assert goal is not None + assert goal.position == Vector3(0.1, 0.2, 0.100) + assert goal.orientation == Quaternion.from_euler(Vector3(-3.141592653589793, 0.0, 0.0)) + pre_grasp = module.get_pre_grasp_pose() + assert pre_grasp is not None + assert pre_grasp.position == Vector3(0.1, 0.2, 0.200) + + selected = module.select_object(1) + assert selected.is_success() + assert selected.metadata["goal"] == { + "x": 0.1, + "y": 0.2, + "z": 0.1, + "roll": -math.pi, + "pitch": 0.0, + "yaw": 0.0, + } + assert selected.metadata["pre_grasp"] == { + "x": 0.1, + "y": 0.2, + "z": 0.2, + "roll": -math.pi, + "pitch": 0.0, + "yaw": 0.0, + } + assert module._selected_object is obj + + module.scan_scene("water bottle") + scene.set_prompts.assert_called_once_with(["water bottle"]) + + module.config = PickNPlaceConfig(align_grasp_yaw=True) + yaw_aligned_goal = module.get_goal_pose(1) + assert yaw_aligned_goal is not None + expected = Quaternion.from_euler(Vector3(-math.pi, 0.0, 0.0)) + assert yaw_aligned_goal.orientation.angle_to(expected) == pytest.approx(0.0) + + +def test_scan_objects_uses_independent_simple_queries() -> None: + with patch.object(ModuleBase, "__init__", lambda self, config_args: None): + module = PickNPlaceModule() + module._visualization = MagicMock() + module.scan_scene = MagicMock(detections_length=3) + module.get_scene_info = MagicMock(return_value=[]) + module._publish_scene_objects = MagicMock() + + result = module.scan_objects([" wooden block ", "white box", " "]) + + assert result.is_success() + assert result.metadata["queried_names"] == ["wooden block", "white box"] + module.scan_scene.assert_called_once_with(prompts=["wooden block", "white box"]) + + +def test_pick_selected_verifies_gripper_did_not_fully_close() -> None: + with patch.object(ModuleBase, "__init__", lambda self, config_args: None): + module = PickNPlaceModule() + module.config = PickNPlaceConfig(grasp_feedback_delay=0.0) + module._goal_pose = PoseStamped( + position=Vector3(0.3, 0.1, 0.12), + orientation=Quaternion.from_euler(Vector3(-math.pi, 0.0, 0.0)), + ) + module._pre_grasp_pose = PoseStamped( + position=Vector3(0.3, 0.1, 0.22), + orientation=Quaternion.from_euler(Vector3(-math.pi, 0.0, 0.0)), + ) + module._pick_execution = MagicMock() + module._pick_execution.open_gripper.return_value = MagicMock(is_success=lambda: True) + module._pick_execution.move_to_pose.return_value = MagicMock(is_success=lambda: True) + module._pick_execution.close_gripper.return_value = MagicMock(is_success=lambda: True) + module._pick_execution.get_gripper.return_value = 0.0 + + result = module.pick_selected() + + assert not result.is_success() + assert result.error_code == "GRASP_VERIFICATION_FAILED" + assert "empty-closed" in result.message + assert result.metadata["gripper_position"] == 0.0 + assert result.metadata["rescan_required"] is True + assert result.metadata["recovered_to_pre_grasp"] is True + assert module._pick_execution.open_gripper.call_count == 2 + assert module._pick_execution.move_to_pose.call_count == 3 + + +def test_place_selected_uses_remembered_box_and_held_object() -> None: + with patch.object(ModuleBase, "__init__", lambda self, config_args: None): + module = PickNPlaceModule() + module.config = PickNPlaceConfig() + module._open_box = { + "center_x": 0.4, + "center_y": -0.1, + "tabletop_z": 0.1, + "rim_z": 0.18, + "opening_width": 0.18, + "opening_depth": 0.14, + } + module._held_object_size = Vector3(0.04, 0.03, 0.02) + module._pick_execution = MagicMock() + module._pick_execution.move_to_pose.return_value = MagicMock(is_success=lambda: True) + module._pick_execution.open_gripper.return_value = MagicMock(is_success=lambda: True) + module._pick_execution.get_ee_pose.return_value = Pose( + Vector3(0.3, 0.1, 0.22), Quaternion(0.0, 0.0, 0.0, 1.0) + ) + + result = module.place_selected() + + assert result.is_success() + assert module._pick_execution.move_to_pose.call_args_list[0].args[:3] == pytest.approx( + (0.3, 0.1, 0.31) + ) + assert module._pick_execution.move_to_pose.call_args_list[1].args[:3] == pytest.approx( + (0.4, -0.1, 0.31) + ) + assert module._pick_execution.move_to_pose.call_args_list[2].args[:3] == pytest.approx( + (0.4, -0.1, 0.21) + ) + assert result.metadata["object_bottom_clearance"] == pytest.approx(0.02) + assert module._pick_execution.move_to_pose.call_count == 3 + assert module._held_object_size is None + + +def test_picknplace_home_matches_xarm_lifecycle_home() -> None: + assert _picknplace_xarm6_model.home_joints == [ + 0.0, + math.radians(-40.0), + math.radians(-50.0), + 0.0, + math.radians(90.0), + 0.0, + ] + + +def test_picknplace_graspgenx_uses_xarm_tcp_calibration() -> None: + assert _xarm_graspgenx.grasp_frame_to_tcp[2][3] == pytest.approx(0.172) + assert _xarm_graspgenx.grasp_frame_to_tcp[:2] == ((0.0, -1.0, 0.0, 0.0), (1.0, 0.0, 0.0, 0.0)) + assert np.allclose( + np.asarray(_xarm_graspgenx.grasp_frame_to_tcp) @ np.asarray(XARM_TCP_TO_GRASP_FRAME), + np.eye(4), + ) + + +def test_picknplace_yaw_alignment_defaults_to_disabled() -> None: + assert not PickNPlaceConfig().align_grasp_yaw + + +def test_parallel_jaw_yaw_uses_the_nearest_equivalent_orientation() -> None: + assert PickNPlaceModule._closest_parallel_jaw_yaw(-math.pi + 0.02, 0.0) == pytest.approx(0.02) + + +def test_picknplace_blueprint_accepts_short_backend_and_grasp_options() -> None: + config = BlueprintConfigParser(picknplace) + + options = config.parse( + overrides={"osr": {"det": "moondream", "seg": "edgetam"}, "pnp": {"grasp": "graspgenx"}} + ) + + assert options.module_configs["osr"]["det"] == "moondream" + assert options.module_configs["osr"]["seg"] == "edgetam" + assert options.module_configs["pnp"]["grasp"] == "graspgenx" + + +def test_table_surface_estimate_ignores_objects_above_the_table() -> None: + x, y = np.meshgrid(np.linspace(0.2, 0.8, 20), np.linspace(-0.4, 0.4, 20)) + table = np.column_stack((x.ravel(), y.ravel(), np.full(x.size, 0.35))) + object_points = np.array([[0.5, 0.0, 0.55], [0.51, 0.0, 0.57], [0.5, 0.01, 0.56]]) + + estimate = _estimate_table_surface(np.vstack((table, object_points))) + + assert estimate is not None + assert estimate["tabletop_z"] == pytest.approx(0.35, abs=0.01) + assert estimate["width"] >= 0.8 + assert estimate["depth"] >= 1.0 + + +def test_table_midpoint_grasp_uses_observed_object_height() -> None: + points = np.array( + [ + [0.2, 0.1, 0.117], + [0.2, 0.1, 0.119], + [0.2, 0.1, 0.120], + [0.2, 0.1, 0.121], + [0.2, 0.1, 0.120], + [0.2, 0.1, 0.119], + [0.2, 0.1, 0.120], + [0.2, 0.1, 0.119], + [0.2, 0.1, 0.120], + [0.2, 0.1, 0.120], + ] + ) + + assert _table_midpoint_grasp_z(points, 0.100, 0.120) == pytest.approx(0.110275) + assert _table_midpoint_grasp_z(points[:9], 0.100, 0.120) == pytest.approx(0.120) + + +def test_table_surface_estimate_displays_filled_tabletop() -> None: + with patch.object(ModuleBase, "__init__", lambda self, config_args: None): + module = PickNPlaceModule() + x, y = np.meshgrid(np.linspace(0.2, 0.8, 20), np.linspace(-0.4, 0.4, 20)) + scene_cloud = MagicMock() + scene_cloud.points_f32.return_value = np.column_stack( + (x.ravel(), y.ravel(), np.full(x.size, 0.35)) + ) + module._scene = MagicMock(get_full_scene_pointcloud=MagicMock(return_value=scene_cloud)) + module._visualization = MagicMock() + + assert module.estimate_table_surface() is not None + + layer = module._visualization.set_visualization_layer.call_args.args[0] + assert isinstance(layer.elements[0], MeshElement) + assert layer.elements[0].opacity == pytest.approx(1.0) + np.testing.assert_array_equal(layer.elements[0].triangles, [[0, 1, 2], [0, 2, 3]]) + + +def test_estimate_table_runs_a_fresh_scan_before_fitting() -> None: + with patch.object(ModuleBase, "__init__", lambda self, config_args: None): + module = PickNPlaceModule() + module.scan_scene = MagicMock() + module.estimate_table_surface = MagicMock( + return_value={ + "center_x": 0.5, + "center_y": 0.0, + "tabletop_z": 0.35, + "width": 0.8, + "depth": 1.0, + } + ) + + result = module.estimate_table() + + assert result.is_success() + module.scan_scene.assert_called_once_with() + + +def test_install_open_box_is_display_only() -> None: + with patch.object(ModuleBase, "__init__", lambda self, config_args: None): + module = PickNPlaceModule() + obj = MagicMock( + center=Vector3(0.4, 0.1, 0.14), + size=Vector3(0.20, 0.16, 0.08), + ) + obj.pose.orientation = Quaternion.from_euler(Vector3(0.0, 0.0, 0.0)) + obj.pointcloud.points_f32.return_value = np.asarray([[0.3, 0.1, 0.18]] * 10, dtype=np.float32) + module._latest_objects = (obj,) + module._tabletop_z = 0.10 + module._obstacle_world = MagicMock() + module._obstacle_world.update_obstacle.return_value = False + module._obstacle_world.add_obstacle.side_effect = lambda name, *_: name + module._visualization = MagicMock() + + result = module.install_open_box(1, wall_thickness=0.01) + + assert result.is_success() + assert result.metadata["opening_width"] == pytest.approx(0.18) + assert result.metadata["opening_depth"] == pytest.approx(0.14) + module._obstacle_world.add_obstacle.assert_not_called() + module._obstacle_world.update_obstacle.assert_not_called() + layer = module._visualization.set_visualization_layer.call_args.args[0] + assert layer.id == "picknplace/open-box" + assert layer.elements[0].id == "box-envelope" + + +def test_picknplace_uses_top_graspgenx_candidate() -> None: + with patch.object(ModuleBase, "__init__", lambda self, config_args: None): + module = PickNPlaceModule() + module.config = PickNPlaceConfig(grasp_strategy="graspgenx") + obj = MagicMock( + ts=1.0, + frame_id="link_base", + center=Vector3(0.1, 0.2, 0.3), + pointcloud=MagicMock(), + ) + obj.camera_transform = None + obj.image = None + obj.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) + obj.pointcloud.points_f32.return_value = np.asarray([[0.4, 0.5, 0.6]], dtype=np.float32) + module._latest_objects = (obj,) + candidate = GraspCandidate( + Pose( + Vector3(0.4, 0.5, 0.6), + Quaternion.from_euler(Vector3(0.0, math.pi / 2.0, 0.0)), + ), + score=0.9, + ) + second_candidate = GraspCandidate( + Pose(Vector3(0.2, 0.3, 0.4), Quaternion()), + score=0.8, + ) + module._grasp_generator = MagicMock( + propose_grasps=MagicMock( + return_value=GraspCandidateArray( + Header(2.0, "link_base"), [candidate, second_candidate] + ) + ) + ) + module._grasp_filter = MagicMock( + inverse_kinematics_single=MagicMock(return_value=IKResult(IKStatus.SUCCESS)) + ) + module.graspgenx_candidates = MagicMock() + module._visualization = MagicMock() + + goal = module.get_goal_pose(1) + + assert goal is not None + assert goal.ts == 2.0 + assert goal.frame_id == "link_base" + assert goal.position == candidate.pose.position + assert goal.orientation == candidate.pose.orientation + assert module.get_grasp_candidates().candidates == [candidate, second_candidate] + module.graspgenx_candidates.publish.assert_called_once_with(module.get_grasp_candidates()) + pre_grasp = module.get_pre_grasp_pose() + assert pre_grasp is not None + assert pre_grasp.position.x == pytest.approx(goal.position.x - 0.1) + assert pre_grasp.position.z == pytest.approx(goal.position.z) + layer = module._visualization.set_visualization_layer.call_args.args[0] + assert layer.id == "picknplace/selection" + assert layer.elements[0].points.shape[1] == 3 + assert layer.elements[1].line_width is None + selected_goal = module.select_grasp_candidate(1) + assert selected_goal is not None + assert selected_goal.position == second_candidate.pose.position + assert module.get_grasp_candidates().selected_index == 1 + pre_grasp = module.get_pre_grasp_pose() + assert pre_grasp is not None + assert pre_grasp.position.x == pytest.approx(selected_goal.position.x) + assert pre_grasp.position.z == pytest.approx(selected_goal.position.z - 0.1) + + +def test_picknplace_excludes_collision_or_ik_infeasible_graspgenx_candidates() -> None: + with patch.object(ModuleBase, "__init__", lambda self, config_args: None): + module = PickNPlaceModule() + module.config = PickNPlaceConfig(grasp_strategy="graspgenx") + safe = GraspCandidate(Pose(Vector3(0.4, 0.5, 0.6), Quaternion()), score=0.9) + unsafe = GraspCandidate(Pose(Vector3(0.2, 0.3, 0.4), Quaternion()), score=0.8) + module._grasp_filter = MagicMock( + inverse_kinematics_single=MagicMock( + side_effect=[IKResult(IKStatus.SUCCESS), IKResult(IKStatus.NO_SOLUTION)] + ) + ) + + filtered = module._filter_graspgenx_candidates( + GraspCandidateArray(Header(2.0, "link_base"), [safe, unsafe]) + ) + + assert filtered.candidates == [safe] + module._grasp_filter.inverse_kinematics_single.assert_any_call( + safe.pose, "arm", check_collision=True + ) + module._grasp_filter.inverse_kinematics_single.assert_any_call( + unsafe.pose, "arm", check_collision=True + ) + + +def test_picknplace_clears_candidates_when_no_graspgenx_proposal_is_safe() -> None: + with patch.object(ModuleBase, "__init__", lambda self, config_args: None): + module = PickNPlaceModule() + module.config = PickNPlaceConfig(grasp_strategy="graspgenx") + obj = MagicMock( + ts=1.0, + frame_id="link_base", + center=Vector3(0.1, 0.2, 0.3), + pointcloud=MagicMock(), + ) + obj.camera_transform = None + obj.image = None + obj.pose.orientation = Quaternion() + module._latest_objects = (obj,) + unsafe = GraspCandidate(Pose(Vector3(0.2, 0.3, 0.4), Quaternion()), score=0.8) + module._grasp_generator = MagicMock( + propose_grasps=MagicMock( + return_value=GraspCandidateArray(Header(2.0, "link_base"), [unsafe]) + ) + ) + module._grasp_filter = MagicMock( + inverse_kinematics_single=MagicMock(return_value=IKResult(IKStatus.NO_SOLUTION)) + ) + module.graspgenx_candidates = MagicMock() + + assert module.get_goal_pose(1) is None + module.graspgenx_candidates.publish.assert_called_once() + assert module.graspgenx_candidates.publish.call_args.args[0].candidates == [] + + +def test_picknplace_returns_empty_candidates_for_obb_grasps() -> None: + with patch.object(ModuleBase, "__init__", lambda self, config_args: None): + module = PickNPlaceModule() + module.config = PickNPlaceConfig() + + assert module.get_grasp_candidates().candidates == [] diff --git a/dimos/manipulation/test_pnpconsole.py b/dimos/manipulation/test_pnpconsole.py new file mode 100644 index 0000000000..b1d302b73e --- /dev/null +++ b/dimos/manipulation/test_pnpconsole.py @@ -0,0 +1,206 @@ +# 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 unittest.mock import MagicMock + +from dimos.manipulation import pnpconsole +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray + + +def test_client_scans_scene_and_quits(monkeypatch) -> None: # type: ignore[no-untyped-def] + pnp = MagicMock() + pnp.scan_scene.return_value = MagicMock(detections_length=3) + app = MagicMock(pnp=pnp) + monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) + choices = iter(["1", "", "q"]) + monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) + + pnpconsole.main() + + pnp.scan_scene.assert_called_once_with(None) + + +def test_client_scans_scene_with_text_prompt(monkeypatch) -> None: # type: ignore[no-untyped-def] + pnp = MagicMock() + pnp.scan_scene.return_value = MagicMock(detections_length=1) + app = MagicMock(pnp=pnp) + monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) + choices = iter(["1", "water bottle", "q"]) + monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) + + pnpconsole.main() + + pnp.scan_scene.assert_called_once_with("water bottle") + + +def test_client_describes_current_scene(monkeypatch) -> None: # type: ignore[no-untyped-def] + pnp = MagicMock() + pnp.describe_scene.return_value = "A blue block is on the table." + app = MagicMock(pnp=pnp) + monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) + choices = iter(["16", "", "q"]) + monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) + + pnpconsole.main() + + pnp.describe_scene.assert_called_once_with("What objects are visible on the table?") + + +def test_client_does_not_execute_without_a_plan(monkeypatch) -> None: # type: ignore[no-untyped-def] + app = MagicMock() + manipulation = app.ManipulationModule + monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) + choices = iter(["5", "q"]) + monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) + + pnpconsole.main() + + manipulation.execute_and_wait.assert_not_called() + + +def test_client_goes_home(monkeypatch) -> None: # type: ignore[no-untyped-def] + app = MagicMock() + manipulation = app.ManipulationModule + monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) + choices = iter(["13", "q"]) + monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) + + pnpconsole.main() + + manipulation.go_home.assert_called_once_with("arm") + + +def test_client_does_not_execute_descent_without_a_plan(monkeypatch) -> None: # type: ignore[no-untyped-def] + app = MagicMock() + manipulation = app.ManipulationModule + monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) + choices = iter(["7", "q"]) + monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) + + pnpconsole.main() + + manipulation.execute_and_wait.assert_not_called() + + +def test_client_previews_descent_before_explicit_execution(monkeypatch) -> None: # type: ignore[no-untyped-def] + goal = PoseStamped(position=Vector3(0.1, 0.2, 0.3)) + pre_grasp = PoseStamped(position=Vector3(0.1, 0.2, 0.2)) + pnp = MagicMock() + pnp.get_goal_pose.return_value = goal + pnp.get_grasp_candidates.return_value = GraspCandidateArray() + pnp.get_pre_grasp_pose.return_value = pre_grasp + app = MagicMock(pnp=pnp) + manipulation = app.ManipulationModule + manipulation.plan_to_pose.return_value = True + manipulation.execute_and_wait.return_value = True + manipulation.get_ee_pose.return_value = pre_grasp + manipulation.plan_cartesian_targets.return_value = True + monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) + choices = iter(["3", "1", "4", "5", "6", "q"]) + monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) + + pnpconsole.main() + + assert manipulation.execute_and_wait.call_count == 1 + manipulation.preview_plan.assert_called_with(duration=2.0) + + +def test_client_runs_grasp_and_lift_without_preview(monkeypatch) -> None: # type: ignore[no-untyped-def] + goal = PoseStamped(position=Vector3(0.1, 0.2, 0.3)) + pre_grasp = PoseStamped(position=Vector3(0.1, 0.2, 0.2)) + pnp = MagicMock() + pnp.get_goal_pose.return_value = goal + pnp.get_grasp_candidates.return_value = GraspCandidateArray() + pnp.get_pre_grasp_pose.return_value = pre_grasp + app = MagicMock(pnp=pnp) + manipulation = app.ManipulationModule + manipulation.plan_to_pose.return_value = True + manipulation.plan_cartesian_targets.return_value = True + manipulation.execute_and_wait.return_value = True + manipulation.get_ee_pose.return_value = pre_grasp + manipulation.close_gripper.return_value.is_success.return_value = True + monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) + monkeypatch.setattr(pnpconsole.time, "sleep", lambda _: None) + choices = iter(["3", "1", "4", "5", "15", "q"]) + monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) + + pnpconsole.main() + + assert manipulation.execute_and_wait.call_count == 3 + assert manipulation.plan_cartesian_targets.call_count == 2 + manipulation.close_gripper.assert_called_once_with("arm") + manipulation.preview_plan.assert_called_once_with(duration=2.0) + + +def test_client_installs_table_collision_with_recommended_clearance(monkeypatch) -> None: # type: ignore[no-untyped-def] + pnp = MagicMock() + pnp.estimate_table_surface.return_value = { + "center_x": 0.5, + "center_y": 0.0, + "tabletop_z": 0.35, + "width": 0.8, + "depth": 1.0, + } + app = MagicMock(pnp=pnp) + manipulation = app.ManipulationModule + monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) + choices = iter(["14", "", "q"]) + monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) + + pnpconsole.main() + + pnp.scan_scene.assert_called_once_with() + manipulation.set_table_collision.assert_called_once_with( + 0.5, 0.0, 0.35, 0.8, 1.0, safety_margin=0.01 + ) + + +def test_client_accepts_zero_table_clearance(monkeypatch) -> None: # type: ignore[no-untyped-def] + pnp = MagicMock() + pnp.estimate_table_surface.return_value = { + "center_x": 0.5, + "center_y": 0.0, + "tabletop_z": 0.35, + "width": 0.8, + "depth": 1.0, + } + app = MagicMock(pnp=pnp) + manipulation = app.ManipulationModule + monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) + choices = iter(["14", "0", "q"]) + monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) + + pnpconsole.main() + + manipulation.set_table_collision.assert_called_once_with( + 0.5, 0.0, 0.35, 0.8, 1.0, safety_margin=0.0 + ) + + +def test_preview_plays_once_slowly() -> None: + manipulation = MagicMock() + + pnpconsole._preview(manipulation) + + manipulation.preview_plan.assert_called_once_with(duration=2.0) + + +def test_grasp_rank_accepts_default_and_valid_selection(monkeypatch) -> None: # type: ignore[no-untyped-def] + choices = iter(["", "7"]) + monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) + + assert pnpconsole._grasp_rank(10) == 0 + assert pnpconsole._grasp_rank(10) == 7 diff --git a/dimos/manipulation/test_table_collision.py b/dimos/manipulation/test_table_collision.py new file mode 100644 index 0000000000..e09ab40622 --- /dev/null +++ b/dimos/manipulation/test_table_collision.py @@ -0,0 +1,34 @@ +# 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 unittest.mock import MagicMock + +import pytest + +from dimos.manipulation.manipulation_module import ManipulationModule + + +def test_table_collision_is_a_conservative_slab() -> None: + module = object.__new__(ManipulationModule) + monitor = MagicMock() + monitor.update_obstacle.return_value = False + monitor.add_obstacle.return_value = "calibrated-table" + module._world_monitor = monitor + + assert module.set_table_collision(0.5, 0.0, 0.35, 0.8, 1.0) + + obstacle = monitor.add_obstacle.call_args.args[0] + assert obstacle.name == "calibrated-table" + assert obstacle.dimensions == (0.8, 1.0, 0.2) + assert obstacle.pose.position.z == pytest.approx(0.25) diff --git a/dimos/manipulation/visualization/layers.py b/dimos/manipulation/visualization/layers.py new file mode 100644 index 0000000000..e85b1a19f2 --- /dev/null +++ b/dimos/manipulation/visualization/layers.py @@ -0,0 +1,226 @@ +# 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. + +"""Backend-neutral, display-only manipulation visualization layers.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +import re +from typing import TypeAlias + +import numpy as np +from numpy.typing import NDArray + +_ID_SEGMENT = re.compile(r"^[A-Za-z0-9][A-Za-z0-9_.-]*$") + + +def _validate_id(value: str, *, hierarchical: bool) -> str: + if not isinstance(value, str) or not value: + raise ValueError("visualization ID must be a nonempty string") + segments = value.split("/") if hierarchical else [value] + if any(not _ID_SEGMENT.fullmatch(segment) for segment in segments): + kind = "layer" if hierarchical else "element" + raise ValueError(f"{kind} ID contains an invalid segment: {value!r}") + return value + + +def _snapshot_positions(value: NDArray[np.generic], *, name: str) -> NDArray[np.float32]: + result = np.array(value, dtype=np.float32, copy=True) + if result.ndim != 2 or result.shape[1:] != (3,): + raise ValueError(f"{name} must have shape (N, 3)") + if not np.all(np.isfinite(result)): + raise ValueError(f"{name} must contain only finite values") + result.setflags(write=False) + return result + + +def _snapshot_colors( + value: NDArray[np.generic] | None, + *, + count: int, + allow_uniform: bool, +) -> NDArray[np.uint8] | None: + if value is None: + return None + source = np.asarray(value) + valid_shapes: set[tuple[int, ...]] = {(count, 3)} + if allow_uniform: + valid_shapes.add((3,)) + if source.shape not in valid_shapes: + expected = "(3,) or (N, 3)" if allow_uniform else "(N, 3)" + raise ValueError(f"colors must have shape {expected}") + if not np.issubdtype(source.dtype, np.number): + raise ValueError("colors must be numeric RGB values") + numeric = np.asarray(source, dtype=np.float64) + if not np.all(np.isfinite(numeric)): + raise ValueError("colors must contain only finite values") + if np.issubdtype(source.dtype, np.floating) and np.all((numeric >= 0.0) & (numeric <= 1.0)): + numeric = np.rint(numeric * 255.0) + if np.any(numeric < 0.0) or np.any(numeric > 255.0): + raise ValueError("colors must be in [0, 1] or [0, 255]") + if not np.all(numeric == np.rint(numeric)): + raise ValueError("colors above 1 must be integer RGB values") + result = np.array(numeric, dtype=np.uint8, copy=True) + result.setflags(write=False) + return result + + +def _validate_size(value: float | None, *, name: str) -> float | None: + if value is None: + return None + result = float(value) + if not math.isfinite(result) or result <= 0.0: + raise ValueError(f"{name} must be finite and positive") + return result + + +def _snapshot_triangles(value: NDArray[np.generic], *, vertex_count: int) -> NDArray[np.int32]: + source = np.asarray(value) + if source.ndim != 2 or source.shape[1:] != (3,): + raise ValueError("triangles must have shape (M, 3)") + if not np.issubdtype(source.dtype, np.number): + raise ValueError("triangles must contain integer indices") + numeric = np.asarray(source, dtype=np.float64) + if not np.all(np.isfinite(numeric)) or not np.all(numeric == np.rint(numeric)): + raise ValueError("triangles must contain finite integer indices") + if np.any(numeric < 0) or (numeric.size and np.any(numeric >= vertex_count)): + raise ValueError("triangles contain an out-of-range vertex index") + result = np.array(numeric, dtype=np.int32, copy=True) + result.setflags(write=False) + return result + + +def _validate_opacity(value: float) -> float: + result = float(value) + if not math.isfinite(result) or not 0.0 < result <= 1.0: + raise ValueError("opacity must be finite and in (0, 1]") + return result + + +@dataclass(frozen=True) +class PointCloudElement: + """A generic colored point cloud with no planning authority.""" + + id: str + points: NDArray[np.generic] + colors: NDArray[np.generic] | None = None + point_size: float | None = None + + def __post_init__(self) -> None: + object.__setattr__(self, "id", _validate_id(self.id, hierarchical=False)) + points = _snapshot_positions(self.points, name="points") + object.__setattr__(self, "points", points) + object.__setattr__( + self, + "colors", + _snapshot_colors(self.colors, count=len(points), allow_uniform=False), + ) + object.__setattr__(self, "point_size", _validate_size(self.point_size, name="point_size")) + + +@dataclass(frozen=True) +class LineSetElement: + """Indexed line geometry with optional uniform or per-line RGB.""" + + id: str + vertices: NDArray[np.generic] + edges: NDArray[np.generic] + colors: NDArray[np.generic] | None = None + line_width: float | None = None + + def __post_init__(self) -> None: + object.__setattr__(self, "id", _validate_id(self.id, hierarchical=False)) + vertices = _snapshot_positions(self.vertices, name="vertices") + object.__setattr__(self, "vertices", vertices) + + source_edges = np.asarray(self.edges) + if source_edges.ndim != 2 or source_edges.shape[1:] != (2,): + raise ValueError("edges must have shape (M, 2)") + if not np.issubdtype(source_edges.dtype, np.number): + raise ValueError("edges must contain integer indices") + numeric_edges = np.asarray(source_edges, dtype=np.float64) + if not np.all(np.isfinite(numeric_edges)) or not np.all( + numeric_edges == np.rint(numeric_edges) + ): + raise ValueError("edges must contain finite integer indices") + if np.any(numeric_edges < 0) or ( + numeric_edges.size and np.any(numeric_edges >= len(vertices)) + ): + raise ValueError("edges contain an out-of-range vertex index") + edges = np.array(numeric_edges, dtype=np.int32, copy=True) + edges.setflags(write=False) + object.__setattr__(self, "edges", edges) + object.__setattr__( + self, + "colors", + _snapshot_colors(self.colors, count=len(edges), allow_uniform=True), + ) + object.__setattr__(self, "line_width", _validate_size(self.line_width, name="line_width")) + + +@dataclass(frozen=True) +class MeshElement: + """Indexed triangle mesh with a uniform RGB color and opacity.""" + + id: str + vertices: NDArray[np.generic] + triangles: NDArray[np.generic] + color: NDArray[np.generic] + opacity: float = 1.0 + + def __post_init__(self) -> None: + object.__setattr__(self, "id", _validate_id(self.id, hierarchical=False)) + vertices = _snapshot_positions(self.vertices, name="vertices") + object.__setattr__(self, "vertices", vertices) + object.__setattr__( + self, + "triangles", + _snapshot_triangles(self.triangles, vertex_count=len(vertices)), + ) + color = _snapshot_colors(self.color, count=1, allow_uniform=True) + if color is None or color.ndim != 1: + raise ValueError("color must have shape (3,)") + object.__setattr__(self, "color", color) + object.__setattr__(self, "opacity", _validate_opacity(self.opacity)) + + +VisualizationElement: TypeAlias = PointCloudElement | LineSetElement | MeshElement + + +@dataclass(frozen=True) +class VisualizationLayer: + """A complete, owner-scoped set of display-only visual elements.""" + + id: str + frame_id: str + elements: tuple[VisualizationElement, ...] + default_visible: bool = True + + def __post_init__(self) -> None: + object.__setattr__(self, "id", _validate_id(self.id, hierarchical=True)) + if not isinstance(self.frame_id, str) or not self.frame_id.strip(): + raise ValueError("frame_id must be a nonempty string") + object.__setattr__(self, "frame_id", self.frame_id.strip()) + elements = tuple(self.elements) + if any( + not isinstance(item, (PointCloudElement, LineSetElement, MeshElement)) + for item in elements + ): + raise TypeError("elements must be point-cloud, line-set, or mesh elements") + ids = [item.id for item in elements] + if len(ids) != len(set(ids)): + raise ValueError("element IDs must be unique within a layer") + object.__setattr__(self, "elements", elements) diff --git a/dimos/manipulation/visualization/pose_overlay.py b/dimos/manipulation/visualization/pose_overlay.py new file mode 100644 index 0000000000..3b0bb97f34 --- /dev/null +++ b/dimos/manipulation/visualization/pose_overlay.py @@ -0,0 +1,68 @@ +# 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. + +"""Project 3D poses onto camera images.""" + +import cv2 + +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.msgs.sensor_msgs.Image import Image + + +def draw_pose_axes( + image: Image, + pose: PoseStamped, + camera_from_pose_frame: Transform, + camera_info: CameraInfo, + axis_length_m: float = 0.05, +) -> Image | None: + """Draw a pose midpoint and projected RGB coordinate axes onto an image.""" + center_px = _project_point( + _transform_point(camera_from_pose_frame, pose.position), + camera_info, + ) + if center_px is None: + return None + + overlay = image.to_opencv().copy() + axes = ( + (Vector3(1.0, 0.0, 0.0), (0, 0, 255), "X"), + (Vector3(0.0, 1.0, 0.0), (0, 255, 0), "Y"), + (Vector3(0.0, 0.0, 1.0), (255, 0, 0), "Z"), + ) + for axis, color, label in axes: + endpoint = pose.position + pose.orientation.rotate_vector(axis * axis_length_m) + endpoint_px = _project_point( + _transform_point(camera_from_pose_frame, endpoint), camera_info + ) + if endpoint_px is None: + continue + cv2.arrowedLine(overlay, center_px, endpoint_px, color, 2, tipLength=0.2) + cv2.putText(overlay, label, endpoint_px, cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2) + cv2.circle(overlay, center_px, 4, (255, 255, 255), -1) + return Image.from_opencv(overlay, frame_id=image.frame_id, ts=image.ts) + + +def _transform_point(transform: Transform, point: Vector3) -> Vector3: + return transform.rotation.rotate_vector(point) + transform.translation + + +def _project_point(point: Vector3, camera_info: CameraInfo) -> tuple[int, int] | None: + if point.z <= 0: + return None + fx, fy, cx, cy = camera_info.K[0], camera_info.K[4], camera_info.K[2], camera_info.K[5] + return (round(fx * point.x / point.z + cx), round(fy * point.y / point.z + cy)) diff --git a/dimos/manipulation/visualization/rerun.py b/dimos/manipulation/visualization/rerun.py new file mode 100644 index 0000000000..61c4559ec2 --- /dev/null +++ b/dimos/manipulation/visualization/rerun.py @@ -0,0 +1,157 @@ +# 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. + +"""Rerun configuration for the pick-and-place workflow.""" + +from functools import partial +from typing import Any, cast + +import rerun.blueprint as rrb + +from dimos.robot.manipulators.xarm.grasp_config import XARM_TCP_TO_GRASP_FRAME + + +def picknplace_rerun_config() -> dict[str, Any]: + """Return the Rerun layout and message conversions for pick and place.""" + return { + "blueprint": _blueprint, + "topic_to_entity": _topic_to_entity, + "visual_override": { + "world/color_camera": partial( + _camera_info_to_rerun, + image_topic="world/color_camera/color_image", + ), + "world/pointcloud": _pointcloud_to_rerun, + "world/graspgenx_candidates": _graspgenx_candidates_to_rerun, + "world/detections_3d": None, + "world/depth_camera": None, + "world/depth_camera/depth_image": None, + }, + } + + +def _blueprint() -> rrb.Blueprint: + return rrb.Blueprint( + rrb.Horizontal( + rrb.Vertical( + rrb.Spatial2DView(origin="world/annotated_image", name="Object Segmentation"), + rrb.Spatial2DView(origin="world/basic_grasp_overlay", name="Grasp Pose"), + rrb.Spatial2DView(origin="world/color_camera/color_image", name="RGB"), + ), + rrb.Spatial3DView(origin="world", name="Filtered Objects"), + ) + ) + + +def _topic_to_entity(topic: Any) -> str: + topic_name = str(getattr(topic, "name", topic)).split("#", 1)[0] + entities = { + "/color_image": "world/color_camera/color_image", + "/camera_info": "world/color_camera", + "/depth_image": "world/depth_camera/depth_image", + "/depth_camera_info": "world/depth_camera", + "/basic_grasp_overlay": "world/basic_grasp_overlay", + "/graspgenx_candidates": "world/graspgenx_candidates", + "/detections_3d": "world/detections_3d", + "/pointcloud": "world/pointcloud", + } + for suffix, entity in entities.items(): + if topic_name == suffix or topic_name.endswith(suffix): + return entity + return f"world/{topic_name.lstrip('/')}" + + +def _camera_info_to_rerun(msg: Any, image_topic: str) -> list[tuple[str, Any]]: + return cast( + "list[tuple[str, Any]]", + msg.to_rerun(image_topic=image_topic, optical_frame=getattr(msg, "frame_id", None)), + ) + + +def _pointcloud_to_rerun(msg: Any) -> Any: + return msg.to_rerun(voxel_size=0.001, mode="points") + + +def _graspgenx_candidates_to_rerun(msg: Any) -> list[tuple[str, Any]]: + """Render calibrated xArm TCP grasp candidates and their gripper geometry.""" + import rerun as rr + + root = "world/graspgenx_candidates" + data: list[tuple[str, Any]] = [(root, rr.Clear(recursive=True))] + frame_id = msg.header.frame_id + if frame_id: + data.append((root, rr.Transform3D(parent_frame=f"tf#/{frame_id}"))) + for rank, candidate in enumerate(msg.candidates[:10]): + pose = candidate.pose + path = f"{root}/{rank:02d}" + selected = rank == msg.selected_index + gripper_color = [255, 255, 0] if selected else [100, 190, 255] + data.extend( + [ + ( + path, + rr.Transform3D( + translation=pose.position.as_tuple, + rotation=rr.Quaternion(xyzw=pose.orientation.to_tuple()), + ), + ), + ( + f"{path}/tcp_axes", + rr.Arrows3D( + origins=[[0.0, 0.0, 0.0]] * 3, + vectors=[ + [0.04, 0.0, 0.0], + [0.0, 0.04, 0.0], + [0.0, 0.0, 0.04], + ], + colors=[[255, 0, 0], [0, 255, 0], [0, 128, 255]], + radii=[0.0015] * 3, + ), + ), + ( + f"{path}/gripper_base", + rr.Transform3D( + translation=[0.0, 0.0, XARM_TCP_TO_GRASP_FRAME[2][3]], + rotation=rr.Quaternion(xyzw=[0.0, 0.0, -0.70710678, 0.70710678]), + ), + ), + ( + f"{path}/gripper_base/jaws", + # The model sweep geometry is in the gripper-base frame: + # local X closes the jaws and local +Z approaches the object. + rr.LineStrips3D( + strips=[ + [[-0.0425, 0.0, 0.095], [-0.0425, 0.0, 0.162]], + [[0.0425, 0.0, 0.095], [0.0425, 0.0, 0.162]], + [[-0.0425, 0.0, 0.095], [0.0425, 0.0, 0.095]], + ], + colors=[gripper_color] * 3, + radii=[0.0015] * 3, + ), + ), + ] + ) + if selected: + data.append( + ( + f"{path}/selected", + rr.Points3D( + positions=[[0.0, 0.0, 0.0]], + labels=[f"SELECTED #{rank} score={candidate.score:.3f}"], + colors=[[255, 255, 0]], + radii=[0.008], + ), + ) + ) + return data diff --git a/dimos/manipulation/visualization/test_factory.py b/dimos/manipulation/visualization/test_factory.py index 9f952f0deb..a53913ce74 100644 --- a/dimos/manipulation/visualization/test_factory.py +++ b/dimos/manipulation/visualization/test_factory.py @@ -41,6 +41,7 @@ NoManipulationVisualizationConfig, ) from dimos.manipulation.visualization.factory import create_manipulation_visualization +from dimos.manipulation.visualization.layers import VisualizationLayer from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState @@ -83,6 +84,12 @@ def remove_vis_obstacle(self, obstacle_id: str) -> None: def clear_vis_obstacles(self) -> None: return None + def set_layer(self, layer: VisualizationLayer) -> None: + return None + + def clear_layer(self, layer_id: str) -> None: + return None + class FakeWorld: def add_robot(self, config: RobotModelConfig) -> WorldRobotID: @@ -237,6 +244,12 @@ def remove_vis_obstacle(self, obstacle_id: str) -> None: def clear_vis_obstacles(self) -> None: self.visualization_calls.append(("clear_vis_obstacles",)) + def set_layer(self, layer: VisualizationLayer) -> None: + self.visualization_calls.append(("set_layer", layer)) + + def clear_layer(self, layer_id: str) -> None: + self.visualization_calls.append(("clear_layer", layer_id)) + def test_config_defaults_to_no_visualization() -> None: config = ManipulationModuleConfig() @@ -307,6 +320,7 @@ def test_create_visualization_meshcat_accepts_structural_world() -> None: pose=PoseStamped(), dimensions=(1.0, 1.0, 1.0), ) + layer = VisualizationLayer("debug/cloud", "world", ()) visualization.initialize(session) assert visualization.get_visualization_url() == "meshcat://test" visualization.update_state(frame) @@ -316,6 +330,8 @@ def test_create_visualization_meshcat_accepts_structural_world() -> None: visualization.add_vis_obstacle("box", obstacle) visualization.remove_vis_obstacle("box") visualization.clear_vis_obstacles() + visualization.set_layer(layer) + visualization.clear_layer(layer.id) assert fake_world.visualization_calls == [ ("initialize", session), ("get_visualization_url",), @@ -326,6 +342,8 @@ def test_create_visualization_meshcat_accepts_structural_world() -> None: ("add_vis_obstacle", "box", obstacle), ("remove_vis_obstacle", "box"), ("clear_vis_obstacles",), + ("set_layer", layer), + ("clear_layer", layer.id), ] assert fake_world.native_calls == [] diff --git a/dimos/manipulation/visualization/test_layers.py b/dimos/manipulation/visualization/test_layers.py new file mode 100644 index 0000000000..4103b15f1e --- /dev/null +++ b/dimos/manipulation/visualization/test_layers.py @@ -0,0 +1,186 @@ +# 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. + +"""Tests for backend-neutral visualization layers.""" + +from dataclasses import FrozenInstanceError + +import numpy as np +import pytest + +from dimos.manipulation.visualization.layers import ( + LineSetElement, + MeshElement, + PointCloudElement, + VisualizationLayer, +) + + +def test_point_cloud_snapshots_positions_and_normalizes_colors() -> None: + points = np.asarray([[1.0, 2.0, 3.0]], dtype=np.float64) + colors = np.asarray([[0.0, 0.5, 1.0]], dtype=np.float32) + + element = PointCloudElement("object", points, colors, point_size=0.005) + points[:] = 9.0 + colors[:] = 0.0 + + np.testing.assert_array_equal(element.points, [[1.0, 2.0, 3.0]]) + np.testing.assert_array_equal(element.colors, [[0, 128, 255]]) + assert element.points.dtype == np.float32 + assert element.points.flags.writeable is False + assert element.colors is not None and element.colors.flags.writeable is False + with pytest.raises(ValueError, match="read-only"): + element.points[0, 0] = 4.0 + with pytest.raises(FrozenInstanceError): + element.point_size = 1.0 # type: ignore[misc] + + +def test_line_set_accepts_uniform_and_per_line_colors() -> None: + vertices = np.asarray([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [1.0, 1.0, 0.0]]) + edges = np.asarray([[0, 1], [1, 2]]) + + uniform = LineSetElement("uniform", vertices, edges, colors=np.asarray([255, 0, 0])) + per_line = LineSetElement( + "rank-1", + vertices, + edges, + colors=np.asarray([[0, 255, 0], [255, 128, 0]]), + line_width=2.0, + ) + + np.testing.assert_array_equal(uniform.colors, [255, 0, 0]) + np.testing.assert_array_equal(per_line.colors, [[0, 255, 0], [255, 128, 0]]) + assert per_line.edges.dtype == np.int32 + assert per_line.edges.flags.writeable is False + + +def test_mesh_snapshots_triangles_color_and_opacity() -> None: + vertices = np.asarray([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]]) + triangles = np.asarray([[0, 1, 2]]) + + element = MeshElement("surface", vertices, triangles, np.asarray([0.0, 0.5, 1.0]), 0.65) + vertices[:] = 9.0 + triangles[:] = 0 + + np.testing.assert_array_equal(element.vertices[1], [1.0, 0.0, 0.0]) + np.testing.assert_array_equal(element.triangles, [[0, 1, 2]]) + np.testing.assert_array_equal(element.color, [0, 128, 255]) + assert element.opacity == pytest.approx(0.65) + + +@pytest.mark.parametrize( + ("triangles", "color", "opacity", "message"), + [ + (np.asarray([[0, 1]]), np.asarray([0, 0, 0]), 1.0, "shape"), + (np.asarray([[0, 1, 3]]), np.asarray([0, 0, 0]), 1.0, "out-of-range"), + (np.asarray([[0, 1, 2]]), np.asarray([[0, 0, 0]]), 1.0, "color"), + (np.asarray([[0, 1, 2]]), np.asarray([0, 0, 0]), 0.0, "opacity"), + ], +) +def test_mesh_rejects_invalid_geometry( + triangles: np.ndarray, color: np.ndarray, opacity: float, message: str +) -> None: + with pytest.raises(ValueError, match=message): + MeshElement("surface", np.zeros((3, 3)), triangles, color, opacity) + + +@pytest.mark.parametrize("value", ["", "/grasp", "grasp/", "grasp//cloud", "grasp cloud"]) +def test_layer_rejects_invalid_id(value: str) -> None: + with pytest.raises(ValueError, match="layer ID|visualization ID"): + VisualizationLayer(value, "world", ()) + + +@pytest.mark.parametrize("value", ["", "rank/1", "rank 1"]) +def test_element_rejects_invalid_id(value: str) -> None: + with pytest.raises(ValueError, match="element ID|visualization ID"): + PointCloudElement(value, np.empty((0, 3))) + + +@pytest.mark.parametrize( + ("kwargs", "message"), + [ + ({"points": np.zeros((3,))}, "shape"), + ({"points": np.asarray([[np.nan, 0.0, 0.0]])}, "finite"), + ( + { + "points": np.zeros((2, 3)), + "colors": np.zeros((1, 3)), + }, + "colors", + ), + ( + { + "points": np.zeros((1, 3)), + "colors": np.asarray([[256, 0, 0]]), + }, + "colors", + ), + ({"points": np.zeros((1, 3)), "point_size": 0.0}, "positive"), + ], +) +def test_point_cloud_rejects_invalid_geometry(kwargs: dict[str, object], message: str) -> None: + with pytest.raises(ValueError, match=message): + PointCloudElement("cloud", **kwargs) # type: ignore[arg-type] + + +@pytest.mark.parametrize( + ("edges", "message"), + [ + (np.zeros((2, 3)), "shape"), + (np.asarray([[0.5, 1.0]]), "integer"), + (np.asarray([[-1, 0]]), "out-of-range"), + (np.asarray([[0, 2]]), "out-of-range"), + ], +) +def test_line_set_rejects_invalid_edges(edges: np.ndarray, message: str) -> None: + with pytest.raises(ValueError, match=message): + LineSetElement( + "lines", + np.asarray([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]]), + edges, + ) + + +def test_line_set_rejects_invalid_appearance() -> None: + with pytest.raises(ValueError, match="colors"): + LineSetElement( + "lines", + np.asarray([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]]), + np.asarray([[0, 1]]), + colors=np.asarray([[0, 0, 0], [255, 255, 255]]), + ) + with pytest.raises(ValueError, match="positive"): + LineSetElement( + "lines", + np.asarray([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]]), + np.asarray([[0, 1]]), + line_width=float("nan"), + ) + + +def test_layer_rejects_empty_frame_and_duplicate_elements() -> None: + element = PointCloudElement("object", np.empty((0, 3))) + with pytest.raises(ValueError, match="frame_id"): + VisualizationLayer("grasp/object-cloud", " ", (element,)) + with pytest.raises(ValueError, match="unique"): + VisualizationLayer("grasp/object-cloud", "world", (element, element)) + + +def test_layer_owns_element_tuple() -> None: + source = [PointCloudElement("object", np.empty((0, 3)))] + + layer = VisualizationLayer("grasp/object-cloud", "world", source) # type: ignore[arg-type] + source.clear() + + assert [item.id for item in layer.elements] == ["object"] diff --git a/dimos/manipulation/visualization/test_rerun.py b/dimos/manipulation/visualization/test_rerun.py new file mode 100644 index 0000000000..dc37761bdb --- /dev/null +++ b/dimos/manipulation/visualization/test_rerun.py @@ -0,0 +1,48 @@ +# 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.manipulation.visualization.rerun import ( + _graspgenx_candidates_to_rerun, + _topic_to_entity, +) +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.manipulation_msgs.GraspCandidate import GraspCandidate +from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray +from dimos.msgs.std_msgs.Header import Header + + +def test_qualified_grasp_candidate_topic_uses_candidate_entity() -> None: + assert ( + _topic_to_entity("dimos/PickNPlaceModule/graspgenx_candidates") + == "world/graspgenx_candidates" + ) + + +def test_top_grasp_candidate_has_selected_rerun_marker() -> None: + candidates = GraspCandidateArray( + Header(0.0, "link_base"), + [ + GraspCandidate(Pose(Vector3(0.1, 0.2, 0.3)), 0.9), + GraspCandidate(Pose(Vector3(0.2, 0.3, 0.4)), 0.8), + ], + selected_index=1, + ) + + paths = [path for path, _ in _graspgenx_candidates_to_rerun(candidates)] + + assert "world/graspgenx_candidates/00/selected" not in paths + assert "world/graspgenx_candidates/01/selected" in paths + assert "world/graspgenx_candidates/01/gripper_base" in paths + assert "world/graspgenx_candidates/01/gripper_base/jaws" in paths diff --git a/dimos/manipulation/visualization/viser/layers.py b/dimos/manipulation/visualization/viser/layers.py new file mode 100644 index 0000000000..5152cc2c8a --- /dev/null +++ b/dimos/manipulation/visualization/viser/layers.py @@ -0,0 +1,277 @@ +# 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. + +"""Latest-wins Viser registry for generic visualization layers.""" + +from __future__ import annotations + +from dataclasses import dataclass +from threading import Condition, Thread +import time +from typing import Any + +from dimos.manipulation.visualization.layers import VisualizationLayer +from dimos.manipulation.visualization.viser.scene import ViserManipulationScene +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +def _display_name(segment: str) -> str: + return segment.replace("-", " ").replace("_", " ").title() + + +@dataclass +class _LayerState: + visible: bool + generation: int = 0 + warning: str | None = None + + +@dataclass(frozen=True) +class _LayerOperation: + sequence: int + layer: VisualizationLayer | None + + +class ViserLayerManager: + """Own Viser layer state, controls, and asynchronous scene reconciliation.""" + + def __init__(self, server: Any, scene: ViserManipulationScene) -> None: + self._server = server + self._scene = scene + self._condition = Condition() + self._states: dict[str, _LayerState] = {} + self._pending: dict[str, _LayerOperation] = {} + self._sequence = 0 + self._processing = False + self._closed = False + self._leaf_handles: dict[str, Any] = {} + self._group_folders: dict[str, Any] = {} + self._group_handles: dict[str, Any] = {} + self._control_sync_depth = 0 + self._root_folder = server.gui.add_folder("Visualization Layers", expand_by_default=True) + self._worker = Thread( + target=self._run, + name="viser-visualization-layers", + daemon=True, + ) + self._worker.start() + + @property + def layer_ids(self) -> tuple[str, ...]: + with self._condition: + return tuple(sorted(self._states)) + + def visibility(self, layer_id: str) -> bool | None: + with self._condition: + state = self._states.get(layer_id) + return None if state is None else state.visible + + def warning(self, layer_id: str) -> str | None: + with self._condition: + state = self._states.get(layer_id) + return None if state is None else state.warning + + def set_layer(self, layer: VisualizationLayer) -> None: + """Queue a complete replacement without waiting for scene rendering.""" + with self._condition: + if self._closed: + return + if layer.id not in self._states: + self._states[layer.id] = _LayerState(visible=layer.default_visible) + self._rebuild_controls() + self._sequence += 1 + self._pending[layer.id] = _LayerOperation(self._sequence, layer) + self._condition.notify() + + def clear_layer(self, layer_id: str) -> None: + """Queue a clear for a known layer while retaining its viewer state.""" + with self._condition: + if self._closed or layer_id not in self._states: + return + self._sequence += 1 + self._pending[layer_id] = _LayerOperation(self._sequence, None) + self._condition.notify() + + def set_visible(self, layer_id: str, visible: bool) -> None: + with self._condition: + state = self._states.get(layer_id) + if self._closed or state is None: + return + state.visible = bool(visible) + handle = self._leaf_handles.get(layer_id) + if handle is not None: + self._set_control_value(handle, state.visible) + self._sync_parent_controls(layer_id) + self._scene.set_visualization_layer_visible(layer_id, bool(visible)) + + def set_group_visible(self, group_id: str, visible: bool) -> None: + descendants = [ + layer_id for layer_id in self.layer_ids if layer_id.startswith(f"{group_id}/") + ] + for layer_id in descendants: + self.set_visible(layer_id, visible) + + def wait_idle(self, timeout: float = 2.0) -> bool: + deadline = time.monotonic() + timeout + with self._condition: + while self._pending or self._processing: + remaining = deadline - time.monotonic() + if remaining <= 0.0: + return False + self._condition.wait(remaining) + return True + + def close(self) -> None: + with self._condition: + if self._closed: + return + self._closed = True + self._pending.clear() + self._condition.notify_all() + self._worker.join(timeout=2.0) + self._scene.clear_visualization_layers() + for handle in ( + *self._leaf_handles.values(), + *self._group_handles.values(), + *reversed(self._group_folders.values()), + self._root_folder, + ): + remove = getattr(handle, "remove", None) + if callable(remove): + remove() + self._leaf_handles.clear() + self._group_handles.clear() + self._group_folders.clear() + + def _run(self) -> None: + while True: + with self._condition: + while not self._pending and not self._closed: + self._condition.wait() + if self._closed: + self._processing = False + self._condition.notify_all() + return + layer_id = next(iter(self._pending)) + operation = self._pending.pop(layer_id) + state = self._states[layer_id] + state.generation += 1 + generation = state.generation + visible = state.visible + self._processing = True + try: + if operation.layer is None: + self._scene.clear_visualization_layer(layer_id) + else: + self._scene.replace_visualization_layer( + operation.layer, + generation=generation, + visible=visible, + ) + warning = None + except Exception as error: + warning = str(error) + logger.warning( + "Visualization layer update failed for '%s': %s", + layer_id, + error, + exc_info=True, + ) + with self._condition: + state.warning = warning + self._processing = False + self._condition.notify_all() + + def _add_layer_controls(self, layer_id: str) -> None: + segments = layer_id.split("/") + parent = self._root_folder + for index, segment in enumerate(segments[:-1], start=1): + group_id = "/".join(segments[:index]) + folder = self._group_folders.get(group_id) + if folder is None: + with parent: + folder = self._server.gui.add_folder( + _display_name(segment), expand_by_default=True + ) + self._group_folders[group_id] = folder + with folder: + group_handle = self._server.gui.add_checkbox("All", initial_value=True) + group_handle.on_update( + lambda event, selected_group=group_id: self._on_group_update( + selected_group, + bool(event.target.value), + ) + ) + self._group_handles[group_id] = group_handle + parent = folder + state = self._states[layer_id] + with parent: + leaf = self._server.gui.add_checkbox( + _display_name(segments[-1]), initial_value=state.visible + ) + leaf.on_update( + lambda event, selected_layer=layer_id: self._on_leaf_update( + selected_layer, + bool(event.target.value), + ) + ) + self._leaf_handles[layer_id] = leaf + self._sync_parent_controls(layer_id) + + def _rebuild_controls(self) -> None: + for handle in ( + *self._leaf_handles.values(), + *self._group_handles.values(), + *reversed(self._group_folders.values()), + ): + remove = getattr(handle, "remove", None) + if callable(remove): + remove() + self._leaf_handles.clear() + self._group_handles.clear() + self._group_folders.clear() + for layer_id in sorted(self._states): + self._add_layer_controls(layer_id) + + def _sync_parent_controls(self, layer_id: str) -> None: + segments = layer_id.split("/") + for index in range(1, len(segments)): + group_id = "/".join(segments[:index]) + handle = self._group_handles.get(group_id) + if handle is None: + continue + descendants = [ + state.visible + for candidate, state in self._states.items() + if candidate.startswith(f"{group_id}/") + ] + self._set_control_value(handle, bool(descendants) and all(descendants)) + + def _set_control_value(self, handle: Any, value: bool) -> None: + """Update Viser state without treating its callback as a user action.""" + self._control_sync_depth += 1 + try: + handle.value = value + finally: + self._control_sync_depth -= 1 + + def _on_leaf_update(self, layer_id: str, visible: bool) -> None: + if self._control_sync_depth == 0: + self.set_visible(layer_id, visible) + + def _on_group_update(self, group_id: str, visible: bool) -> None: + if self._control_sync_depth == 0: + self.set_group_visible(group_id, visible) diff --git a/dimos/manipulation/visualization/viser/scene.py b/dimos/manipulation/visualization/viser/scene.py index 092ba61a92..9ad274ba79 100644 --- a/dimos/manipulation/visualization/viser/scene.py +++ b/dimos/manipulation/visualization/viser/scene.py @@ -29,6 +29,7 @@ import xml.etree.ElementTree as ET import numpy as np +from numpy.typing import NDArray import trimesh from yourdfpy import URDF # type: ignore[import-untyped] @@ -37,6 +38,12 @@ from dimos.manipulation.planning.spec.enums import ObstacleType from dimos.manipulation.planning.spec.models import DEFAULT_OBSTACLE_RGBA, Obstacle from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake +from dimos.manipulation.visualization.layers import ( + LineSetElement, + MeshElement, + PointCloudElement, + VisualizationLayer, +) from dimos.manipulation.visualization.viser.animation import ( GroupPreviewAnimation, PreviewFrame, @@ -102,6 +109,12 @@ OBSTACLE_FALLBACK_COLOR = (55, 190, 210) OBSTACLE_FALLBACK_OPACITY = 0.55 OBSTACLE_PROXY_COLOR = (255, 45, 25) +VISUALIZATION_LAYER_NAMESPACE = "/manipulation/layers" +VISUALIZATION_POINT_CAP = 20_000 +VISUALIZATION_DEFAULT_POINT_SIZE = 0.005 +VISUALIZATION_DEFAULT_POINT_COLOR = (0, 204, 204) +VISUALIZATION_DEFAULT_LINE_COLOR = (255, 255, 255) +VISUALIZATION_DEFAULT_LINE_WIDTH = 1.0 class RobotDisplayMode(StrEnum): @@ -147,6 +160,8 @@ def __init__( self._obstacles_visible = True self._obstacle_gui_handles: list[object] = [] self._obstacle_warning_handle: Any | None = None + self._visualization_layer_handles: dict[str, list[Any]] = {} + self._visualization_layer_visibility: dict[str, bool] = {} self._closed = False self._ensure_obstacle_control() self._ensure_reference_grid() @@ -161,6 +176,130 @@ def set_obstacles_visible(self, visible: bool) -> None: for handle in handles: self._set_handle_visibility(handle, self._obstacles_visible) + @staticmethod + def _visualization_path_segment(value: str) -> str: + return f"id-{value.encode('utf-8').hex()}" + + def replace_visualization_layer( + self, + layer: VisualizationLayer, + *, + generation: int, + visible: bool, + ) -> None: + """Atomically replace a display-only layer with one complete generation.""" + if layer.frame_id != "world": + raise ValueError( + f"Viser visualization layer '{layer.id}' requires frame 'world', " + f"got '{layer.frame_id}'" + ) + base_path = ( + f"{VISUALIZATION_LAYER_NAMESPACE}/" + f"{self._visualization_path_segment(layer.id)}/generation-{generation}" + ) + pending: list[Any] = [] + with self._scene_lock: + if self._closed: + raise RuntimeError("Viser scene is closed") + try: + for element in layer.elements: + path = f"{base_path}/{self._visualization_path_segment(element.id)}" + if isinstance(element, PointCloudElement): + handle = self._render_point_cloud_element(path, element) + elif isinstance(element, LineSetElement): + handle = self._render_line_set_element(path, element) + elif isinstance(element, MeshElement): + handle = self._render_mesh_element(path, element) + else: + raise TypeError(f"unsupported visualization element: {type(element)!r}") + if handle is not None: + self._set_handle_visibility(handle, False) + pending.append(handle) + except Exception: + for handle in pending: + self._remove_scene_handle(handle) + raise + + previous = self._visualization_layer_handles.get(layer.id, []) + for handle in pending: + self._set_handle_visibility(handle, visible) + self._visualization_layer_handles[layer.id] = pending + self._visualization_layer_visibility[layer.id] = visible + for handle in previous: + self._remove_scene_handle(handle) + + def clear_visualization_layer(self, layer_id: str) -> None: + """Remove one layer's handles while retaining its visibility.""" + with self._scene_lock: + for handle in self._visualization_layer_handles.pop(layer_id, []): + self._remove_scene_handle(handle) + + def clear_visualization_layers(self) -> None: + """Remove every generic layer handle.""" + with self._scene_lock: + for layer_id in list(self._visualization_layer_handles): + self.clear_visualization_layer(layer_id) + + def set_visualization_layer_visible(self, layer_id: str, visible: bool) -> None: + """Apply viewer-owned visibility to a layer's current generation.""" + with self._scene_lock: + self._visualization_layer_visibility[layer_id] = bool(visible) + for handle in self._visualization_layer_handles.get(layer_id, []): + self._set_handle_visibility(handle, bool(visible)) + + def _render_point_cloud_element(self, path: str, element: PointCloudElement) -> Any | None: + if len(element.points) == 0: + return None + stride = max(1, math.ceil(len(element.points) / VISUALIZATION_POINT_CAP)) + points = element.points[::stride] + colors: NDArray[np.uint8] | tuple[int, int, int] + if element.colors is None: + colors = VISUALIZATION_DEFAULT_POINT_COLOR + else: + colors = np.asarray(element.colors[::stride], dtype=np.uint8) + return self.server.scene.add_point_cloud( + path, + points=points, + colors=colors, + point_size=element.point_size or VISUALIZATION_DEFAULT_POINT_SIZE, + point_shape="circle", + visible=False, + ) + + def _render_line_set_element(self, path: str, element: LineSetElement) -> Any | None: + if len(element.edges) == 0: + return None + points = element.vertices[element.edges] + colors: NDArray[np.uint8] | tuple[int, int, int] + if element.colors is None: + colors = VISUALIZATION_DEFAULT_LINE_COLOR + elif element.colors.ndim == 1: + colors = np.asarray(element.colors, dtype=np.uint8) + else: + colors = np.asarray( + np.repeat(element.colors[:, np.newaxis, :], 2, axis=1), + dtype=np.uint8, + ) + return self.server.scene.add_line_segments( + path, + points=points, + colors=colors, + line_width=element.line_width or VISUALIZATION_DEFAULT_LINE_WIDTH, + visible=False, + ) + + def _render_mesh_element(self, path: str, element: MeshElement) -> Any | None: + if len(element.triangles) == 0: + return None + return self.server.scene.add_mesh_simple( + path, + vertices=element.vertices, + faces=element.triangles, + color=tuple(int(value) for value in element.color), + opacity=element.opacity, + visible=False, + ) + def add_vis_obstacle(self, obstacle_id: str, obstacle: Obstacle) -> None: """Render one accepted planner obstacle under the local obstacle namespace.""" with self._scene_lock: @@ -665,6 +804,8 @@ def close(self) -> None: self._obstacle_handles.clear() self._obstacles.clear() self._obstacle_render_failures.clear() + self.clear_visualization_layers() + self._visualization_layer_visibility.clear() for key in list(self._handles): self._remove_handle(key) if self._grid_handle is not None: @@ -676,7 +817,8 @@ def close(self) -> None: self._remove_scene_handle(frame) for urdf in self._collision_fallback_urdfs.values(): self._remove_scene_handle(urdf) - for handle in self._obstacle_gui_handles: + # Viser folders own their children, so remove children before folders. + for handle in reversed(self._obstacle_gui_handles): self._remove_scene_handle(handle) self._obstacle_gui_handles.clear() self._urdfs.clear() diff --git a/dimos/manipulation/visualization/viser/test_layers.py b/dimos/manipulation/visualization/viser/test_layers.py new file mode 100644 index 0000000000..102137fe0f --- /dev/null +++ b/dimos/manipulation/visualization/viser/test_layers.py @@ -0,0 +1,441 @@ +# 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. + +"""Hermetic Viser tests for generic visualization layers.""" + +from __future__ import annotations + +from collections.abc import Callable, Iterator +from dataclasses import dataclass, field +from threading import Event +from types import SimpleNamespace + +import numpy as np +import pytest +from pytest_mock import MockerFixture + +pytest.importorskip("viser", reason="Viser optional dependency is not installed") + +from dimos.manipulation.visualization.layers import ( + LineSetElement, + MeshElement, + PointCloudElement, + VisualizationLayer, +) +from dimos.manipulation.visualization.viser.layers import ViserLayerManager +from dimos.manipulation.visualization.viser.scene import ViserManipulationScene + + +@dataclass +class Handle: + name: str + visible: bool = True + value: bool = True + callback: Callable[[object], None] | None = None + removed: bool = False + callback_on_assignment: bool = False + _initialized: bool = field(default=False, init=False) + + def __post_init__(self) -> None: + self._initialized = True + + def __setattr__(self, name: str, value: object) -> None: + previous = getattr(self, name, None) + object.__setattr__(self, name, value) + if ( + name == "value" + and getattr(self, "_initialized", False) + and getattr(self, "callback_on_assignment", False) + and previous != value + and self.callback is not None + ): + self.callback(SimpleNamespace(target=self)) + + def on_update(self, callback: Callable[[object], None]) -> None: + self.callback = callback + + def remove(self) -> None: + self.removed = True + + def trigger(self, value: bool) -> None: + self.value = value + if not self.callback_on_assignment: + assert self.callback is not None + self.callback(SimpleNamespace(target=self)) + + +class Folder(Handle): + def __enter__(self) -> Folder: + return self + + def __exit__(self, *_args: object) -> bool: + return False + + +class Gui: + def __init__(self, *, callback_on_assignment: bool = False) -> None: + self.folders: list[Folder] = [] + self.checkboxes: list[Handle] = [] + self.callback_on_assignment = callback_on_assignment + + def add_folder(self, label: str, **_kwargs: object) -> Folder: + handle = Folder(label) + self.folders.append(handle) + return handle + + def add_checkbox(self, label: str, *, initial_value: bool) -> Handle: + handle = Handle( + label, + value=initial_value, + callback_on_assignment=self.callback_on_assignment, + ) + self.checkboxes.append(handle) + return handle + + +class SceneApi: + def __init__(self) -> None: + self.handles: list[Handle] = [] + self.calls: list[tuple[str, str, dict[str, object]]] = [] + self.fail_on_name: str | None = None + + def add_grid(self, name: str, **_kwargs: object) -> Handle: + return self._add("grid", name, {}) + + def add_point_cloud(self, name: str, **kwargs: object) -> Handle: + return self._add("point_cloud", name, kwargs) + + def add_line_segments(self, name: str, **kwargs: object) -> Handle: + return self._add("line_segments", name, kwargs) + + def add_mesh_simple( + self, name: str, vertices: np.ndarray, faces: np.ndarray, **kwargs: object + ) -> Handle: + return self._add("mesh", name, {"vertices": vertices, "faces": faces, **kwargs}) + + def _add(self, kind: str, name: str, kwargs: dict[str, object]) -> Handle: + if self.fail_on_name is not None and self.fail_on_name in name: + raise RuntimeError("injected render failure") + handle = Handle(name, visible=bool(kwargs.get("visible", True))) + for key, value in kwargs.items(): + setattr(handle, key, value) + self.handles.append(handle) + self.calls.append((kind, name, kwargs)) + return handle + + +class Server: + def __init__(self, *, callback_on_assignment: bool = False) -> None: + self.gui = Gui(callback_on_assignment=callback_on_assignment) + self.scene = SceneApi() + + +class Urdf: + pass + + +@pytest.fixture +def scene() -> Iterator[ViserManipulationScene]: + value = ViserManipulationScene(Server(), Urdf) # type: ignore[arg-type] + try: + yield value + finally: + value.close() + + +@pytest.fixture +def manager( + scene: ViserManipulationScene, +) -> Iterator[ViserLayerManager]: + value = ViserLayerManager(scene.server, scene) + try: + yield value + finally: + value.close() + + +def point_layer( + value: float = 0.0, + *, + visible: bool = True, + count: int = 1, + colors: np.ndarray | None = None, +) -> VisualizationLayer: + points = np.full((count, 3), value, dtype=np.float32) + return VisualizationLayer( + "grasp/object-cloud", + "world", + (PointCloudElement("object", points, colors),), + default_visible=visible, + ) + + +def test_scene_renders_cloud_with_paired_cap_and_fallback_color( + scene: ViserManipulationScene, monkeypatch: pytest.MonkeyPatch +) -> None: + monkeypatch.setattr("dimos.manipulation.visualization.viser.scene.VISUALIZATION_POINT_CAP", 2) + colors = np.asarray( + [[0, 1, 2], [3, 4, 5], [6, 7, 8], [9, 10, 11], [12, 13, 14]], + dtype=np.uint8, + ) + layer = point_layer(count=5, colors=colors) + + scene.replace_visualization_layer(layer, generation=1, visible=True) + cloud_call = next(call for call in scene.server.scene.calls if call[0] == "point_cloud") + + np.testing.assert_array_equal( + cloud_call[2]["points"], + np.asarray([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], dtype=np.float32), + ) + np.testing.assert_array_equal(cloud_call[2]["colors"], colors[::3]) + assert cloud_call[2]["point_size"] == pytest.approx(0.005) + assert scene.server.scene.handles[-1].visible is True + + scene.replace_visualization_layer(point_layer(value=1.0), generation=2, visible=True) + fallback = scene.server.scene.calls[-1][2] + assert fallback["colors"] == (0, 204, 204) + + +def test_scene_renders_indexed_line_set_and_encodes_logical_ids( + scene: ViserManipulationScene, +) -> None: + element = LineSetElement( + "rank-1", + np.asarray([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [1.0, 1.0, 0.0]]), + np.asarray([[0, 1], [1, 2]]), + colors=np.asarray([[0, 255, 0], [255, 128, 0]]), + line_width=2.5, + ) + layer = VisualizationLayer("grasp/proposals", "world", (element,)) + + scene.replace_visualization_layer(layer, generation=7, visible=True) + kind, name, kwargs = scene.server.scene.calls[-1] + + assert kind == "line_segments" + assert "grasp/proposals" not in name + assert "rank-1" not in name + assert "generation-7" in name + np.testing.assert_array_equal( + kwargs["points"], + np.asarray( + [ + [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]], + [[1.0, 0.0, 0.0], [1.0, 1.0, 0.0]], + ] + ), + ) + np.testing.assert_array_equal( + kwargs["colors"], + np.asarray( + [ + [[0, 255, 0], [0, 255, 0]], + [[255, 128, 0], [255, 128, 0]], + ] + ), + ) + assert kwargs["line_width"] == pytest.approx(2.5) + + +def test_scene_renders_filled_mesh(scene: ViserManipulationScene) -> None: + element = MeshElement( + "tabletop-fill", + np.asarray([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [1.0, 1.0, 0.0], [0.0, 1.0, 0.0]]), + np.asarray([[0, 1, 2], [0, 2, 3]]), + np.asarray([80, 180, 255]), + opacity=0.65, + ) + + scene.replace_visualization_layer( + VisualizationLayer("pick/table", "world", (element,)), generation=1, visible=True + ) + + kind, _name, kwargs = scene.server.scene.calls[-1] + assert kind == "mesh" + np.testing.assert_array_equal(kwargs["faces"], [[0, 1, 2], [0, 2, 3]]) + assert kwargs["color"] == (80, 180, 255) + assert kwargs["opacity"] == pytest.approx(0.65) + + +def test_scene_failed_replacement_retains_previous_generation( + scene: ViserManipulationScene, +) -> None: + scene.replace_visualization_layer(point_layer(), generation=1, visible=True) + previous = scene.server.scene.handles[-1] + first = PointCloudElement("first", np.asarray([[1.0, 0.0, 0.0]])) + failing = PointCloudElement("fail", np.asarray([[2.0, 0.0, 0.0]])) + replacement = VisualizationLayer("grasp/object-cloud", "world", (first, failing)) + scene.server.scene.fail_on_name = "6661696c" # "fail" in hexadecimal + + with pytest.raises(RuntimeError, match="injected"): + scene.replace_visualization_layer(replacement, generation=2, visible=True) + + assert previous.removed is False + partial = next(handle for handle in scene.server.scene.handles if "generation-2" in handle.name) + assert partial.removed is True + + +def test_manager_registers_hierarchy_and_preserves_hidden_state( + manager: ViserLayerManager, + scene: ViserManipulationScene, +) -> None: + manager.set_layer(point_layer(visible=False)) + manager.set_layer( + VisualizationLayer( + "grasp/proposals", + "world", + ( + LineSetElement( + "rank-1", + np.asarray([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]]), + np.asarray([[0, 1]]), + ), + ), + ) + ) + assert manager.wait_idle() + + assert manager.layer_ids == ("grasp/object-cloud", "grasp/proposals") + assert manager.visibility("grasp/object-cloud") is False + assert [folder.name for folder in scene.server.gui.folders if not folder.removed].count( + "Grasp" + ) == 1 + object_handle = next( + handle + for handle in scene.server.gui.checkboxes + if handle.name == "Object Cloud" and not handle.removed + ) + object_handle.trigger(True) + assert manager.visibility("grasp/object-cloud") is True + + manager.set_visible("grasp/object-cloud", False) + manager.set_layer(point_layer(value=2.0, visible=True)) + assert manager.wait_idle() + assert manager.visibility("grasp/object-cloud") is False + assert scene.server.scene.handles[-1].visible is False + + manager.clear_layer("grasp/object-cloud") + assert manager.wait_idle() + assert "grasp/object-cloud" in manager.layer_ids + assert manager.visibility("grasp/object-cloud") is False + + +def test_manager_parent_toggle_updates_all_descendants( + manager: ViserLayerManager, + scene: ViserManipulationScene, +) -> None: + manager.set_layer(point_layer()) + manager.set_layer(VisualizationLayer("grasp/proposals", "world", ())) + assert manager.wait_idle() + parent = next( + handle + for handle in scene.server.gui.checkboxes + if handle.name == "All" and not handle.removed + ) + + parent.trigger(False) + + assert manager.visibility("grasp/object-cloud") is False + assert manager.visibility("grasp/proposals") is False + + +def test_manager_leaf_toggle_does_not_cascade_through_reactive_parent() -> None: + scene = ViserManipulationScene(Server(callback_on_assignment=True), Urdf) # type: ignore[arg-type] + manager = ViserLayerManager(scene.server, scene) + try: + manager.set_layer(point_layer()) + manager.set_layer(VisualizationLayer("grasp/proposals", "world", ())) + assert manager.wait_idle() + object_handle = next( + handle + for handle in scene.server.gui.checkboxes + if handle.name == "Object Cloud" and not handle.removed + ) + + object_handle.trigger(False) + + assert manager.visibility("grasp/object-cloud") is False + assert manager.visibility("grasp/proposals") is True + finally: + manager.close() + scene.close() + + +def test_manager_latest_pending_operation_wins( + manager: ViserLayerManager, + scene: ViserManipulationScene, + mocker: MockerFixture, +) -> None: + entered = Event() + release = Event() + original = scene.replace_visualization_layer + + def block_first(layer: VisualizationLayer, *, generation: int, visible: bool) -> None: + if not entered.is_set(): + entered.set() + assert release.wait(2.0) + original(layer, generation=generation, visible=visible) + + mocker.patch.object(scene, "replace_visualization_layer", side_effect=block_first) + manager.set_layer(point_layer(value=1.0)) + assert entered.wait(2.0) + manager.set_layer(point_layer(value=2.0)) + manager.clear_layer("grasp/object-cloud") + release.set() + + assert manager.wait_idle() + assert not any( + not handle.removed and "generation-" in handle.name for handle in scene.server.scene.handles + ) + + +def test_manager_failure_is_contained_and_cross_layer_remains_independent( + manager: ViserLayerManager, + scene: ViserManipulationScene, +) -> None: + scene.server.scene.fail_on_name = "6661696c" + manager.set_layer( + VisualizationLayer( + "debug/failing", + "world", + (PointCloudElement("fail", np.asarray([[0.0, 0.0, 0.0]])),), + ) + ) + manager.set_layer(point_layer()) + + assert manager.wait_idle() + assert "injected render failure" in (manager.warning("debug/failing") or "") + assert manager.warning("grasp/object-cloud") is None + assert any( + not handle.removed and "generation-" in handle.name for handle in scene.server.scene.handles + ) + + +def test_scene_rejects_unsupported_frame_without_replacing_current( + scene: ViserManipulationScene, +) -> None: + scene.replace_visualization_layer(point_layer(), generation=1, visible=True) + previous = scene.server.scene.handles[-1] + + with pytest.raises(ValueError, match="requires frame 'world'"): + scene.replace_visualization_layer( + VisualizationLayer( + "grasp/object-cloud", + "camera", + (PointCloudElement("object", np.asarray([[0.0, 0.0, 0.0]])),), + ), + generation=2, + visible=True, + ) + + assert previous.removed is False diff --git a/dimos/manipulation/visualization/viser/visualizer.py b/dimos/manipulation/visualization/viser/visualizer.py index 1b64601128..69201fe343 100644 --- a/dimos/manipulation/visualization/viser/visualizer.py +++ b/dimos/manipulation/visualization/viser/visualizer.py @@ -18,6 +18,7 @@ from contextlib import suppress from typing import TYPE_CHECKING +from dimos.manipulation.visualization.layers import VisualizationLayer from dimos.manipulation.visualization.viser.animation import ( GroupPreviewAnimation, PreviewFrame, @@ -25,6 +26,7 @@ ) from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig from dimos.manipulation.visualization.viser.gui import ViserPanelGui +from dimos.manipulation.visualization.viser.layers import ViserLayerManager from dimos.manipulation.visualization.viser.runtime import ( VISER_URDF_INSTALL_HINT, ViserRuntime, @@ -73,6 +75,7 @@ def __init__( self._server: ViserServer | None = None self._scene: ViserManipulationScene | None = None self._gui: ViserPanelGui | None = None + self._layer_manager: ViserLayerManager | None = None self._session_scene: PlanningSceneInfo | None = None self._operator: object | None = None self._current_states: dict[str, JointState] = {} @@ -120,6 +123,7 @@ def _ensure_started(self) -> None: self._server = None self._scene = None self._gui = None + self._layer_manager = None self._closed = True raise self._runtime = runtime @@ -129,6 +133,15 @@ def _ensure_started(self) -> None: self._closed = False logger.info(f"Viser manipulation visualization: {self.get_visualization_url()}") + def _ensure_layer_manager(self) -> ViserLayerManager | None: + """Create generic layer resources only when a layer is first published.""" + if self._layer_manager is not None: + return self._layer_manager + if self._server is None or self._scene is None or self._closed: + return None + self._layer_manager = ViserLayerManager(self._server, self._scene) + return self._layer_manager + def initialize(self, session: VisualizationSession) -> None: """Initialize Viser robot visuals from a one-shot visualization session.""" self._operator = session.operator @@ -227,6 +240,37 @@ def clear_vis_obstacles(self) -> None: if self._scene is not None: self._scene.clear_vis_obstacles() + def set_layer(self, layer: VisualizationLayer) -> None: + """Queue one complete display-only layer replacement.""" + if self._closed: + return + try: + self._ensure_started() + manager = self._ensure_layer_manager() + if manager is not None: + manager.set_layer(layer) + except Exception: + logger.warning( + "Visualization layer submission failed for '%s'", + layer.id, + exc_info=True, + ) + + def clear_layer(self, layer_id: str) -> None: + """Queue a display-only layer clear while retaining viewer state.""" + if self._closed: + return + try: + self._ensure_started() + if self._layer_manager is not None: + self._layer_manager.clear_layer(layer_id) + except Exception: + logger.warning( + "Visualization layer clear failed for '%s'", + layer_id, + exc_info=True, + ) + def update_state(self, frame: VisualizationStateFrame) -> None: """Update current robot render state from a pushed state frame.""" if self._closed: @@ -322,6 +366,11 @@ def close(self) -> None: self._closed = True errors: list[BaseException] = [] try: + if self._layer_manager is not None: + try: + self._layer_manager.close() + except Exception as e: + errors.append(e) if self._gui is not None: try: self._gui.close() @@ -342,5 +391,6 @@ def close(self) -> None: self._server = None self._scene = None self._gui = None + self._layer_manager = None if errors: raise errors[0] diff --git a/dimos/manipulation/visualization_spec.py b/dimos/manipulation/visualization_spec.py new file mode 100644 index 0000000000..d9a144856f --- /dev/null +++ b/dimos/manipulation/visualization_spec.py @@ -0,0 +1,24 @@ +# 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. + +"""RPC contract for display-only manipulation visualization layers.""" + +from typing import Protocol + +from dimos.manipulation.visualization.layers import VisualizationLayer +from dimos.spec.utils import Spec + + +class ManipulationVisualizationSpec(Spec, Protocol): + def set_visualization_layer(self, layer: VisualizationLayer) -> bool: ... diff --git a/dimos/models/segmentation/edge_tam.py b/dimos/models/segmentation/edge_tam.py index 72f1484af8..8243139d5d 100644 --- a/dimos/models/segmentation/edge_tam.py +++ b/dimos/models/segmentation/edge_tam.py @@ -48,7 +48,7 @@ class SAM2InferenceState(TypedDict): def _build_model() -> "SAM2VideoPredictor": - """Build the EdgeTAM SAM2 model from the local config + checkpoint.""" + """Build the EdgeTAM SAM2 model from the local config and checkpoint.""" local_config_path = Path(__file__).parent / "configs" / "edgetam.yaml" if not local_config_path.exists(): @@ -76,59 +76,59 @@ def _build_model() -> "SAM2VideoPredictor": predictor: SAM2VideoPredictor = instantiate(cfg.model, _recursive_=True) - # Suppress the per-frame "propagate in video" tqdm bar from sam2 + # Suppress the per-frame "propagate in video" tqdm bar from sam2. import sam2.sam2_video_predictor as _svp - _svp.tqdm = lambda iterable, *a, **kw: iterable + _svp.tqdm = lambda iterable, *args, **kwargs: iterable - ckpt_path = str(get_data("models_edgetam") / "edgetam.pt") - - sd = torch.load(ckpt_path, map_location="cpu", weights_only=True)["model"] - missing_keys, unexpected_keys = predictor.load_state_dict(sd) + checkpoint = get_data("models_edgetam") / "edgetam.pt" + state_dict = torch.load(checkpoint, map_location="cpu", weights_only=True)["model"] + missing_keys, unexpected_keys = predictor.load_state_dict(state_dict) if missing_keys: - raise RuntimeError("Missing keys in checkpoint") + raise RuntimeError("Missing keys in EdgeTAM checkpoint") if unexpected_keys: - raise RuntimeError("Unexpected keys in checkpoint") + raise RuntimeError("Unexpected keys in EdgeTAM checkpoint") - predictor = predictor.to("cuda") - predictor.eval() - return predictor + return predictor.to("cuda").eval() class EdgeTAMImageSegmenter: - """Box-prompted single-image segmentation using the EdgeTAM checkpoint.""" + """Refine detector boxes into single-image EdgeTAM masks.""" def __init__(self) -> None: from sam2.sam2_image_predictor import SAM2ImagePredictor self._predictor = SAM2ImagePredictor(_build_model()) - def segment(self, detections: ImageDetections2D) -> ImageDetections2D: - """Refine box detections into mask detections (Detection2DSeg).""" + def segment( + self, detections: ImageDetections2D[Detection2DBBox] + ) -> ImageDetections2D[Detection2DSeg]: + """Return masks that preserve each input detection's metadata.""" import cv2 - if not len(detections): - return detections + if not detections.detections: + return ImageDetections2D(detections.image, []) image = detections.image rgb = cv2.cvtColor(image.to_opencv(), cv2.COLOR_BGR2RGB) - + boxes = np.asarray( + [detection.bbox for detection in detections.detections], dtype=np.float32 + ) with torch.no_grad(), torch.autocast("cuda", dtype=torch.bfloat16): self._predictor.set_image(rgb) - boxes = np.array([det.bbox for det in detections], dtype=np.float32) masks, _, _ = self._predictor.predict(box=boxes, multimask_output=False) - masks = masks.reshape(-1, *masks.shape[-2:]) # (N, H, W) regardless of batch dim - segmented: list[Detection2DBBox] = [ + masks = masks.reshape(-1, *masks.shape[-2:]) + segmented = [ Detection2DSeg.from_sam2_result( mask, - det.track_id, + detection.track_id, image, - class_id=det.class_id, - name=det.name, - confidence=det.confidence, + class_id=detection.class_id, + name=detection.name, + confidence=detection.confidence, ) - for det, mask in zip(detections, masks, strict=False) + for detection, mask in zip(detections.detections, masks, strict=True) ] return ImageDetections2D(image, segmented) diff --git a/dimos/models/vl/moondream.py b/dimos/models/vl/moondream.py index e3cfe744ce..ef5b85b408 100644 --- a/dimos/models/vl/moondream.py +++ b/dimos/models/vl/moondream.py @@ -38,6 +38,7 @@ class MoondreamConfig(HuggingFaceModelConfig, VlModelConfig): model_name: str = "vikhyatk/moondream2" dtype: torch.dtype = torch.bfloat16 auto_resize: tuple[int, int] | None = MOONDREAM_DEFAULT_AUTO_RESIZE + compile_model: bool = False class MoondreamVlModel(HuggingFaceModel, VlModel): @@ -46,13 +47,14 @@ class MoondreamVlModel(HuggingFaceModel, VlModel): @cached_property def _model(self) -> AutoModelForCausalLM: - """Load model with compile() for optimization.""" + """Load the model, optionally enabling its experimental compile path.""" model = AutoModelForCausalLM.from_pretrained( self.config.model_name, trust_remote_code=self.config.trust_remote_code, torch_dtype=self.config.dtype, ).to(self.config.device) - model.compile() + if self.config.compile_model: + model.compile() return model def _to_pil(self, image: Image | np.ndarray[Any, Any]) -> PILImage.Image: diff --git a/dimos/msgs/manipulation_msgs/GraspCandidateArray.py b/dimos/msgs/manipulation_msgs/GraspCandidateArray.py index 5144d0b5b9..06011017f0 100644 --- a/dimos/msgs/manipulation_msgs/GraspCandidateArray.py +++ b/dimos/msgs/manipulation_msgs/GraspCandidateArray.py @@ -27,10 +27,14 @@ class GraspCandidateArray: msg_name = "manipulation_msgs.GraspCandidateArray" def __init__( - self, header: Header | None = None, candidates: list[GraspCandidate] | None = None + self, + header: Header | None = None, + candidates: list[GraspCandidate] | None = None, + selected_index: int = 0, ) -> None: self.header = header if header is not None else Header(0.0) self.candidates = candidates if candidates is not None else [] + self.selected_index = selected_index def __len__(self) -> int: return len(self.candidates) @@ -40,9 +44,23 @@ def __iter__(self) -> Iterator[GraspCandidate]: def encode(self) -> bytes: """Encode using the repository's pickle transport convention.""" - return pickle.dumps({"header": self.header, "candidates": self.candidates}) + return pickle.dumps( + { + "header": self.header, + "candidates": self.candidates, + "selected_index": self.selected_index, + } + ) @classmethod def decode(cls, data: bytes) -> GraspCandidateArray: value = pickle.loads(data) - return cls(value["header"], value["candidates"]) + return cls(value["header"], value["candidates"], value.get("selected_index", 0)) + + # Typed LCM transport lets the Rerun bridge subscribe to proposal updates. + def lcm_encode(self) -> bytes: + return self.encode() + + @classmethod + def lcm_decode(cls, data: bytes, **kwargs: object) -> GraspCandidateArray: + return cls.decode(data) diff --git a/dimos/perception/detection/detectors/moondream.py b/dimos/perception/detection/detectors/moondream.py new file mode 100644 index 0000000000..2b005abed9 --- /dev/null +++ b/dimos/perception/detection/detectors/moondream.py @@ -0,0 +1,63 @@ +# 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. + +"""Text-prompted Moondream object detector adapter.""" + +from dimos.models.vl.moondream import MoondreamVlModel +from dimos.msgs.sensor_msgs.Image import Image +from dimos.perception.detection.detectors.base import Detector +from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D + + +class Moondream2DDetector(Detector): + """Identify prompted objects with Moondream's native box detector.""" + + def __init__(self, model: MoondreamVlModel | None = None, *, max_objects: int = 5) -> None: + if max_objects < 1: + raise ValueError("max_objects must be positive") + self._model = model or MoondreamVlModel() + self._model.start() + self._max_objects = max_objects + self._text_prompts: tuple[str, ...] = () + + def set_prompts( + self, + text: list[str] | None = None, + bboxes: object | None = None, + ) -> None: + """Set one or more text queries; visual prompts are unsupported.""" + if bboxes is not None: + raise ValueError("Moondream detector supports text prompts only") + if text is None: + raise ValueError("Moondream detector requires at least one text prompt") + prompts = tuple(prompt.strip() for prompt in text if prompt.strip()) + if not prompts: + raise ValueError("Moondream detector requires at least one nonempty text prompt") + self._text_prompts = prompts + + def process_image(self, image: Image) -> ImageDetections2D: + """Run prompted object identification on one image.""" + detections = ImageDetections2D(image) + for prompt in self._text_prompts: + result = self._model.query_detections(image, prompt, max_objects=self._max_objects) + detections.detections.extend(result.detections) + return detections + + def describe_image(self, image: Image, question: str) -> str: + """Answer an open-ended question about an image with the loaded VLM.""" + return str(self._model.query(image, question)) + + def stop(self) -> None: + """Release the Moondream model and GPU memory.""" + self._model.stop() diff --git a/dimos/perception/detection/detectors/test_moondream.py b/dimos/perception/detection/detectors/test_moondream.py new file mode 100644 index 0000000000..8c596b03ce --- /dev/null +++ b/dimos/perception/detection/detectors/test_moondream.py @@ -0,0 +1,78 @@ +# 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 unittest.mock import MagicMock + +import numpy as np +import pytest + +from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.perception.detection.detectors.moondream import Moondream2DDetector +from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox +from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D + + +def test_moondream_detector_queries_each_text_prompt() -> None: + image = Image(data=np.zeros((4, 4, 3), dtype=np.uint8), format=ImageFormat.BGR) + model = MagicMock() + model.query_detections.side_effect = [ + ImageDetections2D( + image, + [ + Detection2DBBox( + (0.0, 0.0, 2.0, 2.0), + track_id=0, + class_id=-1, + confidence=1.0, + name="cup", + ts=image.ts, + image=image, + ) + ], + ), + ImageDetections2D( + image, + [ + Detection2DBBox( + (2.0, 2.0, 3.0, 3.0), + track_id=1, + class_id=-1, + confidence=1.0, + name="bottle", + ts=image.ts, + image=image, + ) + ], + ), + ] + detector = Moondream2DDetector(model=model) + + detector.set_prompts(text=["cup", "bottle"]) + detections = detector.process_image(image) + + assert [detection.name for detection in detections] == ["cup", "bottle"] + assert model.query_detections.call_args_list[0].args[1] == "cup" + assert model.query_detections.call_args_list[1].args[1] == "bottle" + detector.stop() + model.start.assert_called_once_with() + model.stop.assert_called_once_with() + + +def test_moondream_detector_rejects_empty_and_visual_prompts() -> None: + detector = Moondream2DDetector(model=MagicMock()) + + with pytest.raises(ValueError, match="nonempty"): + detector.set_prompts(text=[" "]) + with pytest.raises(ValueError, match="text prompts"): + detector.set_prompts(text=["cup"], bboxes=object()) diff --git a/dimos/perception/detection/type/detection2d/seg.py b/dimos/perception/detection/type/detection2d/seg.py index eb6ec734fa..4a510598bb 100644 --- a/dimos/perception/detection/type/detection2d/seg.py +++ b/dimos/perception/detection/type/detection2d/seg.py @@ -34,6 +34,25 @@ class Detection2DSeg(Detection2DBBox): mask: np.ndarray[Any, np.dtype[np.uint8]] # Binary mask [H, W], uint8 0 or 255 + def draw_on(self, img: Any, scale: float = 1.0) -> None: + """Blend the segmentation mask onto a BGR image, then draw its box and label.""" + mask = self.mask + if mask.shape[:2] != img.shape[:2]: + mask = cv2.resize( + mask, + (img.shape[1], img.shape[0]), + interpolation=cv2.INTER_NEAREST, + ) + + selected = mask > 0 + if np.any(selected): + mask_color = np.array([0, 180, 255], dtype=np.float32) + img[selected] = (img[selected].astype(np.float32) * 0.55 + mask_color * 0.45).astype( + np.uint8 + ) + + super().draw_on(img, scale=scale) + @classmethod def from_sam2_result( cls, diff --git a/dimos/perception/detection/type/imageDetections.py b/dimos/perception/detection/type/imageDetections.py index 98fd0e5388..4aa609cc3e 100644 --- a/dimos/perception/detection/type/imageDetections.py +++ b/dimos/perception/detection/type/imageDetections.py @@ -94,4 +94,4 @@ def annotated_image(self, scale: float = 1.0) -> Image: from dimos.msgs.sensor_msgs.Image import Image as ImageMsg - return ImageMsg.from_opencv(img, ts=self.image.ts) + return ImageMsg.from_opencv(img, frame_id=self.image.frame_id, ts=self.image.ts) diff --git a/dimos/perception/experimental/object_scene_registration.py b/dimos/perception/experimental/object_scene_registration.py index 914301afab..8d4cacaff1 100644 --- a/dimos/perception/experimental/object_scene_registration.py +++ b/dimos/perception/experimental/object_scene_registration.py @@ -13,14 +13,15 @@ # limitations under the License. import time -from typing import Any +from typing import Any, Literal import numpy as np from numpy.typing import NDArray +from pydantic import AliasChoices, Field, model_validator from dimos.agents.annotation import skill from dimos.core.core import rpc -from dimos.core.module import Module +from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo @@ -30,6 +31,7 @@ from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.perception.detection.detectors.base import Detector from dimos.perception.detection.detectors.yoloe import Yoloe2DDetector, YoloePromptMode from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.experimental.object import ( @@ -46,8 +48,36 @@ logger = setup_logger() +class ObjectSceneRegistrationConfig(ModuleConfig): + """Configurable detector, segmenter, and RGB-D object reconstruction settings.""" + + target_frame: str = "map" + prompt_mode: YoloePromptMode = YoloePromptMode.LRPC + distance_threshold: float = 0.2 + min_detections_for_permanent: int = 6 + register_objects: bool = True + detect_on_request: bool = False + detector_confidence: float = 0.6 + det: Literal["yoloe", "moondream"] = Field( + default="yoloe", validation_alias=AliasChoices("det", "detector_backend") + ) + seg: Literal["yolo", "edgetam"] = Field( + default="yolo", validation_alias=AliasChoices("seg", "segmentation_backend") + ) + object_voxel_downsample: float = 0.005 + max_distance: float = 0.0 + use_aabb: bool = False + max_obstacle_width: float = 0.0 + + @model_validator(mode="after") + def _require_edgetam_for_moondream(self) -> "ObjectSceneRegistrationConfig": + if self.det == "moondream" and self.seg != "edgetam": + raise ValueError("osr.det=moondream requires osr.seg=edgetam") + return self + + class ObjectSceneRegistrationModule(Module): - """Module for detecting objects in camera images using YOLO-E with 2D and 3D detection.""" + """Module for prompted 2D detection, segmentation, and RGB-D object reconstruction.""" color_image: In[Image] depth_image: In[Image] @@ -56,53 +86,64 @@ class ObjectSceneRegistrationModule(Module): detections_2d: Out[Detection2DArray] detections_3d: Out[Detection3DArray] + annotated_image: Out[Image] objects: Out[list[DetObject]] pointcloud: Out[PointCloud2] - _detector: Yoloe2DDetector | None = None + _detector: Detector | None = None + _segmenter: Any | None = None _camera_info: CameraInfo | None = None _object_db: ObjectDB + _latest_objects: list[Object] + _latest_output_objects: tuple[Object, ...] + _latest_aligned_frames: tuple[Image, Image] | None = None # A tuple assignment/read is atomic, so depth and its transform cannot be # observed from different frames by get_full_scene_pointcloud(). _latest_scene_snapshot: tuple[Image, Transform | None] | None = None + config: ObjectSceneRegistrationConfig - def __init__( - self, - target_frame: str = "map", - prompt_mode: YoloePromptMode = YoloePromptMode.LRPC, - # ObjectDB tuning - distance_threshold: float = 0.2, - min_detections_for_permanent: int = 6, - # Object 3D reconstruction tuning - max_distance: float = 0.0, - use_aabb: bool = False, - max_obstacle_width: float = 0.0, - **kwargs: Any, - ) -> None: + def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) - self._target_frame = target_frame - self._prompt_mode = prompt_mode + self._target_frame = self.config.target_frame + self._prompt_mode = self.config.prompt_mode + self._register_objects = self.config.register_objects + self._detect_on_request = self.config.detect_on_request + self._detector_confidence = self.config.detector_confidence + self._detector_backend = self.config.det + self._segmentation_backend = self.config.seg self._object_db = ObjectDB( - distance_threshold=distance_threshold, - min_detections_for_permanent=min_detections_for_permanent, + distance_threshold=self.config.distance_threshold, + min_detections_for_permanent=self.config.min_detections_for_permanent, ) - self._max_distance = max_distance - self._use_aabb = use_aabb - self._max_obstacle_width = max_obstacle_width + self._latest_objects = [] + self._latest_output_objects = () + self._object_voxel_downsample = self.config.object_voxel_downsample + self._max_distance = self.config.max_distance + self._use_aabb = self.config.use_aabb + self._max_obstacle_width = self.config.max_obstacle_width @rpc def start(self) -> None: super().start() - if self._prompt_mode == YoloePromptMode.LRPC: - model_name = "yoloe-11l-seg-pf.pt" + if self._detector_backend == "moondream": + from dimos.perception.detection.detectors.moondream import Moondream2DDetector + + self._detector = Moondream2DDetector() else: - model_name = "yoloe-11l-seg.pt" + if self._prompt_mode == YoloePromptMode.LRPC: + model_name = "yoloe-11l-seg-pf.pt" + else: + model_name = "yoloe-11l-seg.pt" + self._detector = Yoloe2DDetector( + model_name=model_name, + prompt_mode=self._prompt_mode, + conf=self._detector_confidence, + ) + if self._segmentation_backend == "edgetam": + from dimos.models.segmentation.edge_tam import EdgeTAMImageSegmenter - self._detector = Yoloe2DDetector( - model_name=model_name, - prompt_mode=self._prompt_mode, - ) + self._segmenter = EdgeTAMImageSegmenter() self.camera_info.subscribe(lambda msg: setattr(self, "_camera_info", msg)) @@ -121,8 +162,12 @@ def stop(self) -> None: if self._detector: self._detector.stop() self._detector = None + self._segmenter = None self._object_db.clear() + self._latest_objects = [] + self._latest_output_objects = () + self._latest_aligned_frames = None logger.info("ObjectSceneRegistrationModule stopped") super().stop() @@ -135,37 +180,69 @@ def set_prompts( ) -> None: """Set prompts for detection. Provide either text or bboxes, not both.""" if self._detector is not None: - self._detector.set_prompts(text=text, bboxes=bboxes) + set_prompts = getattr(self._detector, "set_prompts", None) + if not callable(set_prompts): + raise RuntimeError("configured detector does not support prompts") + set_prompts(text=text, bboxes=bboxes) @rpc def select_object(self, track_id: int) -> dict[str, Any] | None: """Get object data by track_id and promote to permanent.""" - for obj in self._object_db.get_all_objects(): + for obj in self._known_objects(): if obj.track_id == track_id: - self._object_db.promote(obj.object_id) + if self._register_objects: + self._object_db.promote(obj.object_id) return obj.to_dict() return None @rpc def get_object_track_ids(self) -> list[int]: """Get track_ids of all permanent objects.""" - return [obj.track_id for obj in self._object_db.get_all_objects()] + return [obj.track_id for obj in self._known_objects()] @rpc def get_detected_objects(self) -> list[dict[str, Any]]: """Get all detected objects with object_id (UUID) and name.""" - return [obj.agent_encode() for obj in self._object_db.get_all_objects()] + return [obj.agent_encode() for obj in self._known_objects()] + + @rpc + def scan_scene(self) -> Detection3DArray: + """Run detection on the latest aligned RGB-D frame and return its 3D detections.""" + frames = self._latest_aligned_frames + if frames is None: + return to_detection3d_array([], frame_id=self._target_frame) + + if not self._register_objects: + self._latest_objects = [] + self._latest_output_objects = () + self._process_images(*frames) + return to_detection3d_array( + list(self._latest_output_objects), + frame_id=self._target_frame, + ts=frames[0].ts, + ) + + @rpc + def describe_scene(self, question: str) -> str: + """Answer an open-ended scene question using the configured Moondream detector.""" + frames = self._latest_aligned_frames + if frames is None: + raise RuntimeError("No aligned RGB-D frame is available") + describe_image = getattr(self._detector, "describe_image", None) + if not callable(describe_image): + raise RuntimeError("Scene description requires osr.det=moondream") + return str(describe_image(frames[0], question)) @rpc def get_object_pointcloud_by_name(self, name: str) -> PointCloud2 | None: """Get pointcloud for an object by class name.""" - objects = self._object_db.find_by_name(name) + objects = [obj for obj in self._known_objects() if obj.name == name] return objects[0].pointcloud if objects else None @rpc def get_object_pointcloud_by_object_id(self, object_id: str) -> PointCloud2 | None: """Get pointcloud for an object by its stable object_id (searches all objects).""" - obj = self._object_db.find_by_object_id(object_id) + obj = next((obj for obj in self._known_objects() if obj.object_id == object_id), None) if obj is None: logger.warning(f"No object found with object_id='{object_id}'") return None @@ -178,7 +255,7 @@ def _get_object_mask(self, object_id: str) -> NDArray[np.uint8] | None: """Get dilated mask for an object by ID.""" import cv2 - for obj in self._object_db.get_all_objects(): + for obj in self._known_objects(): if obj.object_id != object_id: continue if obj.mask is None: @@ -193,6 +270,11 @@ def _get_object_mask(self, object_id: str) -> NDArray[np.uint8] | None: return None + def _known_objects(self) -> list[Object]: + if self._register_objects: + return self._object_db.get_all_objects() + return self._latest_objects + @rpc def get_full_scene_pointcloud( self, @@ -290,6 +372,9 @@ def select(self, track_id: int) -> str: def _on_aligned_frames(self, frames) -> None: # type: ignore[no-untyped-def] color_msg, depth_msg = frames + if self._detect_on_request: + self._latest_aligned_frames = (color_msg, depth_msg) + return self._process_images(color_msg, depth_msg) def _process_images(self, color_msg: Image, depth_msg: Image) -> None: @@ -308,8 +393,26 @@ def _process_images(self, color_msg: Image, depth_msg: Image) -> None: data=depth_cv, format=ImageFormat.DEPTH, frame_id=depth_msg.frame_id, ts=depth_msg.ts ) - # Run 2D detection + # Log each expensive stage separately so a stalled on-demand scan can be localized. + t0 = time.monotonic() + logger.info("Object detection started", detector=self._detector_backend) detections_2d: ImageDetections2D[Any] = self._detector.process_image(color_image) + logger.info( + "Object detection completed", + detector=self._detector_backend, + duration_s=round(time.monotonic() - t0, 3), + detections=len(detections_2d.detections), + ) + if self._segmenter is not None: + t0 = time.monotonic() + logger.info("Object segmentation started", segmenter=self._segmentation_backend) + detections_2d = self._segmenter.segment(detections_2d) + logger.info( + "Object segmentation completed", + segmenter=self._segmentation_backend, + duration_s=round(time.monotonic() - t0, 3), + detections=len(detections_2d.detections), + ) detections_2d_msg = Detection2DArray( detections_length=len(detections_2d.detections), @@ -317,6 +420,7 @@ def _process_images(self, color_msg: Image, depth_msg: Image) -> None: detections=[det.to_ros_detection2d() for det in detections_2d.detections], ) self.detections_2d.publish(detections_2d_msg) + self.annotated_image.publish(detections_2d.annotated_image()) # Process 3D detections self._process_3d_detections(detections_2d, color_image, depth_image) @@ -338,7 +442,9 @@ def _process_3d_detections( self._target_frame, color_image.frame_id, color_image.ts, - 0.1, + # Request-driven scans can use a cached camera frame while + # inference starts; retain temporal alignment within that cache. + 3.0, forward_tolerance=0.2, ) if camera_transform is None: @@ -354,25 +460,33 @@ def _process_3d_detections( depth_image=depth_image, camera_info=self._camera_info, camera_transform=camera_transform, + voxel_downsample=self._object_voxel_downsample, max_distance=self._max_distance, use_aabb=self._use_aabb, max_obstacle_width=self._max_obstacle_width, ) - if not objects: - return - - # Add objects to spatial memory database - self._object_db.add_objects(objects) + if self._register_objects: + if not objects: + return + self._object_db.add_objects(objects) + # Registered mode publishes the complete confirmed scene, not just this frame. + output_objects = self._object_db.get_objects() + else: + self._latest_objects = objects + output_objects = objects - # Publish ALL permanent objects so downstream consumers get the full set, - # not just this frame's batch (which may be a subset of what's on the table). - all_permanent = self._object_db.get_objects() + self._latest_output_objects = tuple(output_objects) - detections_3d = to_detection3d_array(all_permanent) + detections_3d = to_detection3d_array( + output_objects, + frame_id=self._target_frame, + ts=color_image.ts, + ) self.detections_3d.publish(detections_3d) - self.objects.publish(all_permanent) + self.objects.publish(output_objects) - objects_for_pc = all_permanent - aggregated_pc = aggregate_pointclouds(objects_for_pc) + aggregated_pc = aggregate_pointclouds(output_objects) + if not output_objects: + aggregated_pc.frame_id = self._target_frame + aggregated_pc.ts = color_image.ts self.pointcloud.publish(aggregated_pc) - return diff --git a/dimos/perception/experimental/object_scene_registration_spec.py b/dimos/perception/experimental/object_scene_registration_spec.py index 59aae79cab..c48f5548b4 100644 --- a/dimos/perception/experimental/object_scene_registration_spec.py +++ b/dimos/perception/experimental/object_scene_registration_spec.py @@ -15,10 +15,14 @@ from typing import Protocol from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray from dimos.spec.utils import Spec class ObjectSceneRegistrationSpec(Spec, Protocol): + def set_prompts(self, text: list[str] | None = None) -> None: ... + def scan_scene(self) -> Detection3DArray: ... + def describe_scene(self, question: str) -> str: ... def get_object_pointcloud_by_name(self, name: str) -> PointCloud2 | None: ... def get_object_pointcloud_by_object_id(self, object_id: str) -> PointCloud2 | None: ... def get_full_scene_pointcloud( diff --git a/dimos/perception/experimental/test_object_scene_registration_temporal.py b/dimos/perception/experimental/test_object_scene_registration_temporal.py index 99e79eb876..f36f4ce08e 100644 --- a/dimos/perception/experimental/test_object_scene_registration_temporal.py +++ b/dimos/perception/experimental/test_object_scene_registration_temporal.py @@ -17,12 +17,13 @@ from collections.abc import Iterator import sys from typing import Any -from unittest.mock import MagicMock +from unittest.mock import ANY, MagicMock import numpy as np import pytest from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule @@ -75,7 +76,56 @@ def test_temporal_tf_lookup_uses_bounded_image_timestamp( _image(12.5), ) - assert tf.calls == [(("map", "camera", 12.5, 0.1), {"forward_tolerance": 0.2})] + assert tf.calls == [(("map", "camera", 12.5, 3.0), {"forward_tolerance": 0.2})] + + +def test_detector_confidence_is_configurable() -> None: + module = ObjectSceneRegistrationModule(detector_confidence=0.4) + try: + assert module._detector_confidence == 0.4 + finally: + module.stop() + + +def test_segmentation_backend_defaults_to_yolo() -> None: + module = ObjectSceneRegistrationModule() + try: + assert module._segmentation_backend == "yolo" + finally: + module.stop() + + with pytest.raises(ValueError, match="seg"): + ObjectSceneRegistrationModule(segmentation_backend="invalid") # type: ignore[arg-type] + with pytest.raises(ValueError, match="det"): + ObjectSceneRegistrationModule(detector_backend="invalid") # type: ignore[arg-type] + with pytest.raises(ValueError, match="requires"): + ObjectSceneRegistrationModule(det="moondream", seg="yolo") + + +def test_edgetam_backend_refines_yolo_detections( + monkeypatch: Any, module: ObjectSceneRegistrationModule +) -> None: + color = Image( + data=np.zeros((2, 2, 3), dtype=np.uint8), + format=ImageFormat.BGR, + frame_id="camera", + ts=4.0, + ) + raw_detections = ImageDetections2D(color, []) + segmented_detections = ImageDetections2D(color, []) + module._detector = MagicMock() + module._detector.process_image.return_value = raw_detections + module._segmenter = MagicMock() + module._segmenter.segment.return_value = segmented_detections + module.detections_2d = MagicMock() + module.annotated_image = MagicMock() + process_3d = MagicMock() + monkeypatch.setattr(module, "_process_3d_detections", process_3d) + + module._process_images(color, _image(4.0)) + + module._segmenter.segment.assert_called_once_with(raw_detections) + process_3d.assert_called_once_with(segmented_detections, color, ANY) def test_failed_lookup_does_not_retry_without_time_or_replace_coherent_cache( @@ -106,7 +156,7 @@ def test_failed_lookup_does_not_retry_without_time_or_replace_coherent_cache( ) assert len(tf.calls) == 2 - assert tf.calls[1] == (("map", "camera", 2.0, 0.1), {"forward_tolerance": 0.2}) + assert tf.calls[1] == (("map", "camera", 2.0, 3.0), {"forward_tolerance": 0.2}) assert module._latest_scene_snapshot == (old_depth, old_transform) @@ -143,3 +193,97 @@ def voxel_down_sample(self, voxel_size: float) -> _PointCloud: module.get_full_scene_pointcloud() result.transform.assert_called_once_with(transform) + + +def test_process_images_publishes_annotated_detection_image( + monkeypatch: Any, module: ObjectSceneRegistrationModule +) -> None: + annotated = MagicMock(spec=Image) + detections = MagicMock(spec=ImageDetections2D) + detections.detections = [] + detections.annotated_image.return_value = annotated + module._detector = MagicMock() + module._detector.process_image.return_value = detections + module.detections_2d = MagicMock() + module.annotated_image = MagicMock() + process_3d = MagicMock() + monkeypatch.setattr(module, "_process_3d_detections", process_3d) + + color = Image( + data=np.zeros((2, 2, 3), dtype=np.uint8), + format=ImageFormat.BGR, + frame_id="camera", + ts=4.0, + ) + module._process_images(color, _image(4.0)) + + module.annotated_image.publish.assert_called_once_with(annotated) + process_3d.assert_called_once() + + +def test_live_mode_publishes_current_objects_without_registration( + monkeypatch: Any, +) -> None: + module = ObjectSceneRegistrationModule(target_frame="camera", register_objects=False) + module._camera_info = MagicMock() + module._object_db.add_objects = MagicMock() + module.detections_3d = MagicMock() + module.objects = MagicMock() + module.pointcloud = MagicMock() + detected_object = MagicMock() + pointcloud = MagicMock() + monkeypatch.setattr( + "dimos.perception.experimental.object_scene_registration.Object.from_2d_to_list", + lambda **_: [detected_object], + ) + monkeypatch.setattr( + "dimos.perception.experimental.object_scene_registration.to_detection3d_array", + lambda *_args, **_kwargs: MagicMock(), + ) + monkeypatch.setattr( + "dimos.perception.experimental.object_scene_registration.aggregate_pointclouds", + lambda _objects: pointcloud, + ) + + ObjectSceneRegistrationModule._process_3d_detections( + module, + MagicMock(spec=ImageDetections2D), + _image(4.0), + _image(4.0), + ) + + module._object_db.add_objects.assert_not_called() + module.objects.publish.assert_called_once_with([detected_object]) + module.pointcloud.publish.assert_called_once_with(pointcloud) + assert module._latest_objects == [detected_object] + module.stop() + + +def test_request_driven_scan_processes_latest_cached_frame(monkeypatch: Any) -> None: + module = ObjectSceneRegistrationModule(target_frame="camera", detect_on_request=True) + color = Image( + data=np.zeros((2, 2, 3), dtype=np.uint8), + format=ImageFormat.BGR, + frame_id="camera", + ts=4.0, + ) + depth = _image(4.0) + module._latest_aligned_frames = (color, depth) + output = MagicMock(frame_id="camera", ts=4.0) + + def process_images(got_color: Image, got_depth: Image) -> None: + assert (got_color, got_depth) == (color, depth) + module._latest_output_objects = (output,) + + detections = MagicMock(spec=Detection3DArray) + monkeypatch.setattr(module, "_process_images", process_images) + monkeypatch.setattr( + "dimos.perception.experimental.object_scene_registration.to_detection3d_array", + lambda *_args, **_kwargs: detections, + ) + + result = module.scan_scene() + + assert result is detections + assert module._latest_output_objects == (output,) + module.stop() diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 10de6be2f8..b50328fd4e 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -91,6 +91,8 @@ "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_planner_coordinator", "openyam-planner-coordinator": "dimos.robot.manipulators.openyam.blueprints.basic:openyam_planner_coordinator", "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", + "picknplace": "dimos.manipulation.blueprints:picknplace", + "picknplace-agent": "dimos.manipulation.blueprints:picknplace_agent", "teleop-hosted-go2-multicam": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_go2_multicam", "teleop-hosted-go2-transport": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_go2_transport", "teleop-hosted-xarm6": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_xarm6", @@ -151,6 +153,8 @@ "unitree-go2-webrtc-keyboard-teleop": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_webrtc_keyboard_teleop:unitree_go2_webrtc_keyboard_teleop", "unitree-go2-webrtc-rage-keyboard-teleop": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_webrtc_rage_keyboard_teleop:unitree_go2_webrtc_rage_keyboard_teleop", "unity-sim": "dimos.simulation.unity.blueprint:unity_sim", + "xarm-graspgenx": "dimos.robot.manipulators.xarm.blueprints.graspgenx:xarm_graspgenx", + "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-perception-sim": "dimos.robot.manipulators.xarm.blueprints.simulation:xarm_perception_sim", @@ -257,6 +261,7 @@ "pgo": "dimos.navigation.cmu_nav.modules.pgo.pgo.PGO", "phone-teleop-module": "dimos.teleop.phone.phone_teleop_module.PhoneTeleopModule", "pick-and-place-module": "dimos.manipulation.pick_and_place_module.PickAndPlaceModule", + "pick-n-place-module": "dimos.manipulation.picknplace.PickNPlaceModule", "point-lio": "dimos.hardware.sensors.lidar.pointlio.module.PointLio", "pointlio-recorder": "dimos.hardware.sensors.lidar.pointlio.recorder.PointlioRecorder", "quest-teleop-module": "dimos.teleop.quest.quest_teleop_module.QuestTeleopModule", diff --git a/dimos/robot/manipulators/common/agent_prompts.py b/dimos/robot/manipulators/common/agent_prompts.py index a010bf8018..31e02c1507 100644 --- a/dimos/robot/manipulators/common/agent_prompts.py +++ b/dimos/robot/manipulators/common/agent_prompts.py @@ -59,14 +59,15 @@ ## Pick & Place - **pick **: Pick up a detected object by name. Use the EXACT name from \ look/scan_objects output. When duplicates exist, pass the object_id shown in brackets \ -(e.g. [id=abc12345]). Example: "pick the cup", "grab the spray can" +(e.g. [id=abc12345]). On GraspGenX-enabled stacks, pick ranks learned grasp proposals, \ +checks motion feasibility before moving, and can verify calibrated closure feedback. Example: \ +"pick the cup", "grab the spray can" - **place **: Place a held object at explicit world-frame coordinates. \ Example: "place it at 0.4, 0.3, 0.1" - **drop_on **: Drop a held object onto another detected object. \ Automatically compensates for camera occlusion. Example: "drop it in the bowl", \ "put it on the box" - **place_back**: Return a held object to its original pick position. -- **pick_and_place **: Pick then place in one command. ## Motion - **move_to_pose [roll pitch yaw]**: Move end-effector to an absolute \ @@ -99,13 +100,18 @@ - NEVER open the gripper while holding an object unless the user asks or you are \ executing place/drop_on. The gripper stays closed during movement. - After pick or place, return to init with **go_init** unless another action follows. +- If pick reports that the object may be held, do not open the gripper automatically. \ +Report the failure phase and ask the user before releasing or recovering. +- If pick fails before closure, call **reset** if the robot entered FAULT, then \ +**scan_objects** before retrying. Do not clear all perception obstacles merely to force \ +a plan through a changed scene. # Coordinate System World frame (meters): X = forward, Y = left, Z = up. Z = 0 is robot base. Typical working area: X 0.3-0.7, Y -0.5 to 0.5, Z 0.05-0.5. # Error Recovery -If planning fails with COLLISION_AT_START: call **clear_perception_obstacles**, then \ -**reset**, then retry. -After any planning failure, call **reset** before more planning or motion. +If planning fails with COLLISION_AT_START, inspect or rescan the scene. Clear perception \ +obstacles only when they are known to be stale. After any robot motion fault, call \ +**reset** before more planning or motion. """ diff --git a/dimos/robot/manipulators/xarm/blueprints/agentic.py b/dimos/robot/manipulators/xarm/blueprints/agentic.py index 073ede3607..465370a77b 100644 --- a/dimos/robot/manipulators/xarm/blueprints/agentic.py +++ b/dimos/robot/manipulators/xarm/blueprints/agentic.py @@ -24,6 +24,7 @@ MANIPULATION_AGENT_SYSTEM_PROMPT, ) 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 @@ -39,6 +40,12 @@ McpClient.blueprint(system_prompt=MANIPULATION_AGENT_SYSTEM_PROMPT), ) +xarm_graspgenx_agent = autoconnect( + xarm_graspgenx, + McpServer.blueprint(), + McpClient.blueprint(system_prompt=MANIPULATION_AGENT_SYSTEM_PROMPT), +) + xarm_perception_sim_agent = autoconnect( xarm_perception_sim, McpServer.blueprint(), diff --git a/dimos/robot/manipulators/xarm/blueprints/graspgenx.py b/dimos/robot/manipulators/xarm/blueprints/graspgenx.py new file mode 100644 index 0000000000..c753194c97 --- /dev/null +++ b/dimos/robot/manipulators/xarm/blueprints/graspgenx.py @@ -0,0 +1,61 @@ +# 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. + +"""GraspGenX-enabled real-hardware xArm perception blueprint.""" + +from __future__ import annotations + +import math + +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.robot.manipulators.xarm.blueprints.perception import xarm_perception +from dimos.robot.manipulators.xarm.config import make_xarm7_model_config +from dimos.robot.manipulators.xarm.grasp_config import make_xarm_graspgenx_config + +_graspgenx_config = make_xarm_graspgenx_config() + +xarm_graspgenx = autoconnect( + xarm_perception, + PickAndPlaceModule.blueprint( + robots=[ + make_xarm7_model_config( + name="arm", + add_gripper=True, + pitch=math.radians(45), + tf_extra_links=["link7"], + ) + ], + planning_timeout=10.0, + visualization={"backend": "meshcat"}, + floor_z=-0.02, + heuristic_grasp_fallback=False, + planning_frame="world", + grasp_approach_vector=(0.0, 0.0, -1.0), + grasp_verification={ + # Enable only after completing the hardware calibration recorded + # in the grasp-pipeline OpenSpec change. + "enabled": False, + "open_position": 0.85, + "closed_position": 0.0, + "held_threshold": 0.02, + "timeout": 2.0, + "poll_interval": 0.05, + }, + ), + GraspGenXModule.blueprint( + **_graspgenx_config.model_dump(exclude={"rpc_transport", "tf_transport", "g"}) + ), +).global_config(n_workers=5) diff --git a/dimos/robot/manipulators/xarm/blueprints/perception.py b/dimos/robot/manipulators/xarm/blueprints/perception.py index f187e7ab50..ead451ca37 100644 --- a/dimos/robot/manipulators/xarm/blueprints/perception.py +++ b/dimos/robot/manipulators/xarm/blueprints/perception.py @@ -45,6 +45,7 @@ planning_timeout=10.0, visualization={"backend": "meshcat"}, floor_z=-0.02, + heuristic_grasp_fallback=True, ), RealSenseCamera.blueprint( base_frame_id="link7", diff --git a/dimos/robot/manipulators/xarm/blueprints/simulation.py b/dimos/robot/manipulators/xarm/blueprints/simulation.py index fb6e21f09a..7e7d2fa1de 100644 --- a/dimos/robot/manipulators/xarm/blueprints/simulation.py +++ b/dimos/robot/manipulators/xarm/blueprints/simulation.py @@ -36,6 +36,7 @@ robots=[make_xarm7_sim_robot_config()], planning_timeout=10.0, visualization={"backend": "meshcat"}, + heuristic_grasp_fallback=True, ), MujocoSimModule.blueprint(**make_xarm7_sim_module_kwargs(XARM7_SIM_PATH)), ObjectSceneRegistrationModule.blueprint(target_frame="world"), diff --git a/dimos/robot/manipulators/xarm/grasp_config.py b/dimos/robot/manipulators/xarm/grasp_config.py new file mode 100644 index 0000000000..b70ced75a6 --- /dev/null +++ b/dimos/robot/manipulators/xarm/grasp_config.py @@ -0,0 +1,63 @@ +# 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. + +"""GraspGenX geometry for the UFACTORY xArm gripper.""" + +from __future__ import annotations + +from dimos.manipulation.grasping.grasp_gen_x import ( + GraspGenXConfig, + 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 +# - link_tcp's closing axis is 90 degrees counter-clockwise around local +Z +# from GraspGenX's local +X closing axis +# - 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. +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), +) + +# Inverse of ``XARM_GRASP_FRAME_TO_TCP``. Rerun receives TCP poses, while the +# sweep geometry below is expressed in the GraspGenX gripper-base frame. +XARM_TCP_TO_GRASP_FRAME = ( + (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), +) + +XARM_GRIPPER_SWEEP = SweepVolumeGripperConfig( + extents_open=(0.085, 0.032, 0.067), + offset_open=(0.0, 0.0, 0.1285), + extents_half_open=(0.0425, 0.032, 0.067), + offset_half_open=(0.0, 0.0, 0.1285), + fingertip_depth=0.162, + family="revolute_2f", +) + + +def make_xarm_graspgenx_config() -> GraspGenXConfig: + """Return the import-safe learned-grasp deployment configuration.""" + return GraspGenXConfig( + gripper=XARM_GRIPPER_SWEEP, + grasp_frame_to_tcp=XARM_GRASP_FRAME_TO_TCP, + max_candidates=100, + ) diff --git a/docs/capabilities/manipulation/agentic.md b/docs/capabilities/manipulation/agentic.md index 3b3b03f9ef..6d57f2a1b5 100644 --- a/docs/capabilities/manipulation/agentic.md +++ b/docs/capabilities/manipulation/agentic.md @@ -43,6 +43,56 @@ uv run dimos stop Use `dimos log -f` to follow the log while the run is active. +## Learned grasp-to-pick pipeline + +The real-hardware `xarm-graspgenx-agent` blueprint adds GraspGenX proposals to +the xArm perception stack. Install the optional runtime and start it with: + +```bash +uv sync --extra graspgenx --extra manipulation --inexact +uv run dimos run xarm-graspgenx-agent +``` + +`pick` remains the only high-level picking tool. It resolves one current +object, obtains that object's planning-frame point cloud, requests ranked +GraspGenX candidates, and rejects candidates that fail pre-grasp, grasp, or +retreat inverse kinematics. During planning, the selected target is +temporarily removed from the collision scene while all other obstacles remain +active. The selected candidate then runs through prepare, approach, grasp, +close, verify, and retreat phases. + +Use the stable object ID returned by `scan_objects` whenever names are +ambiguous. A name is accepted only when it identifies exactly one current +detection; an object-ID prefix must also be unique. Existing +`xarm-perception` and `xarm-perception-sim` blueprints retain their explicit +heuristic grasp fallback and do not load the optional GraspGenX runtime. + +The learned pipeline configuration lives in +`dimos/robot/manipulators/xarm/grasp_config.py`. It records the xArm gripper +sweep volume and the transform from GraspGenX's gripper frame to the planned +TCP. `PickAndPlaceModuleConfig` controls the planning frame, maximum point +cloud age, candidate-check limit, TCP approach direction, approach/retreat +offsets, heuristic fallback, and closure-feedback verification thresholds. +Changing the frame transform, approach direction, or closure threshold +requires robot-specific calibration. + +Failures are phase-specific and stop motion immediately. Before closure, a +failed transaction leaves the gripper in its current safe state. After a +successful close command, failures never automatically reopen the gripper; +the result includes `object_may_be_held=true`, and an operator or agent should +inspect state before issuing another motion. Target collision geometry is +restored on every exit path, and restoration errors are reported without +hiding the primary failure. + +The current verification is a closure-position proxy: an xArm gripper that +stops above the calibrated empty-close threshold is treated as holding +something. It does not measure grasp force, detect slip, or prove that the +intended object was acquired. Force/torque or tactile feedback is required for +those stronger guarantees. The shipped learned-grasp blueprint keeps this +proxy disabled until the open, empty-close, and representative held-object +positions have been measured on the target xArm; enable +`grasp_verification.enabled` only after recording that calibration. + ## Daily interaction For normal interactive use, start the human-friendly terminal client: From cb4f548bf2cd1a36733518e3e481551b9d2cc517 Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Fri, 7 Aug 2026 11:59:50 -0700 Subject: [PATCH 2/5] refactor(manipulation): add heuristic grasp provider --- dimos/manipulation/README.md | 27 +++-- dimos/manipulation/blueprints.py | 4 +- dimos/manipulation/grasping/grasp_gen_spec.py | 4 + .../manipulation/grasping/heuristic_grasp.py | 73 +++++++++++ .../grasping/test_heuristic_grasp.py | 51 ++++++++ dimos/manipulation/picknplace.py | 114 +++--------------- dimos/manipulation/test_picknplace.py | 54 +++------ dimos/manipulation/visualization/rerun.py | 2 - dimos/robot/all_blueprints.py | 1 + 9 files changed, 183 insertions(+), 147 deletions(-) create mode 100644 dimos/manipulation/grasping/heuristic_grasp.py create mode 100644 dimos/manipulation/grasping/test_heuristic_grasp.py diff --git a/dimos/manipulation/README.md b/dimos/manipulation/README.md index 5717c1988a..e84624cdad 100644 --- a/dimos/manipulation/README.md +++ b/dimos/manipulation/README.md @@ -26,17 +26,17 @@ by the EdgeTAM blueprint. ## Run -Start the default YOLO-E and OBB-center-grasp pipeline: +Start the default YOLO-E and deterministic heuristic-grasp pipeline: ```bash uv run --no-sync dimos run picknplace --daemon ``` -Use text-prompted Moondream detection, EdgeTAM segmentation, and an OBB-center grasp: +Use text-prompted Moondream detection, EdgeTAM segmentation, and the deterministic heuristic grasp: ```bash uv run --no-sync dimos run picknplace --daemon \ - -o osr.det=moondream -o osr.seg=edgetam -o pnp.grasp=obb_center + -o osr.det=moondream -o osr.seg=edgetam -o pnp.grasp=heuristic ``` Use the same perception stack with GraspGenX: @@ -48,7 +48,8 @@ uv run --no-sync dimos run picknplace --daemon \ `osr.det` accepts `yoloe` or `moondream`; `osr.seg` accepts `yolo` or `edgetam`. Moondream requires EdgeTAM because it produces detection boxes rather than masks. -`pnp.grasp` accepts `obb_center` or `graspgenx`. GraspGenX loads only when selected. +`pnp.grasp` accepts `heuristic` or `graspgenx`. The heuristic provider derives one top-down +proposal from the segmented object cloud; GraspGenX loads only when selected. Then connect the console: @@ -68,11 +69,11 @@ The console intentionally keeps planning and execution separate: 1. Select `1` to scan the current scene. 2. Select `2` to inspect object number, name, and confidence. -3. Select `3` and choose an object. The GraspGenX blueprint prints its top - proposals and displays the selected grasp. Viser shows the selected object +3. Select `3` and choose an object. The selected provider returns proposals and displays the selected grasp. + Viser shows the selected object cloud in amber, the grasp TCP axes in red, and the pre-grasp TCP axes in green. - The top ten proposals are filtered through collision-aware xArm IK; after - table calibration, candidates intersecting the table are omitted. + GraspGenX's top ten proposals are filtered through collision-aware xArm IK; after table calibration, + candidates intersecting the table are omitted. The heuristic provider returns one deterministic proposal. 4. Select `4` to plan and preview the approach. Each Viser preview plays once at a slow two-second duration. 5. Execute the approach only after inspecting the proposal and preview. @@ -94,14 +95,14 @@ pose, the point-cloud/overlay visualization, and the collision-free preview. ## Grasp Geometry -`PickNPlaceModule.get_goal_pose()` stores the top ranked GraspGenX candidate as +`PickNPlaceModule.get_goal_pose()` stores the selected provider candidate as the TCP goal in the candidate point cloud's frame. Its pre-grasp is computed as: ```text pre_grasp_position = grasp_position - grasp_orientation * (0, 0, 0.100 m) ``` -GraspGenX local `+Z` is the final approach direction, so the pre-grasp retreats +Each provider uses local `+Z` as the final approach direction, so the pre-grasp retreats along local `-Z`. It is not a world-Z lift: an angled or side grasp receives an equally angled or sideward pre-grasp. Descent and ascent use Cartesian paths between the current TCP pose and the selected grasp or pre-grasp target. @@ -114,12 +115,12 @@ applied by the operator pipeline. ## Implementation Guide -- `blueprints.py`: robot, camera, OBB, and GraspGenX blueprint composition. -- `picknplace.py`: scan request, target selection, OBB fallback, learned grasp - selection, and tool-axis pre-grasp calculation. +- `blueprints.py`: robot, camera, heuristic, and GraspGenX blueprint composition. +- `picknplace.py`: scan request, target selection, provider selection, and tool-axis pre-grasp calculation. - `pnpconsole.py`: explicit operator stages and manual gripper/home controls. - `grasping/grasp_gen_x.py`: import-safe proposal adapter and candidate contract. - `grasping/grasp_gen_x_runtime.py`: in-process checkpoint load and GPU inference. +- `grasping/heuristic_grasp.py`: deterministic top-down proposal provider. - `visualization/pose_overlay.py` and `visualization/rerun.py`: selected-object cloud, image, and grasp overlays. diff --git a/dimos/manipulation/blueprints.py b/dimos/manipulation/blueprints.py index a5ad7ee5b4..a64d5c6430 100644 --- a/dimos/manipulation/blueprints.py +++ b/dimos/manipulation/blueprints.py @@ -25,6 +25,7 @@ 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.picknplace import PickNPlaceModule from dimos.manipulation.visualization.rerun import picknplace_rerun_config @@ -105,7 +106,8 @@ detector_confidence=0.4, object_voxel_downsample=0.001, ), - PickNPlaceModule.blueprint(instance_name="pnp", align_grasp_yaw=True), + PickNPlaceModule.blueprint(instance_name="pnp"), + HeuristicGraspModule.blueprint(instance_name="heuristic_grasp"), GraspGenXModule.blueprint( instance_name="ggx", load_on_start=False, diff --git a/dimos/manipulation/grasping/grasp_gen_spec.py b/dimos/manipulation/grasping/grasp_gen_spec.py index d3071d76d8..cfadb50d0a 100644 --- a/dimos/manipulation/grasping/grasp_gen_spec.py +++ b/dimos/manipulation/grasping/grasp_gen_spec.py @@ -30,3 +30,7 @@ def generate_grasps( class GraspGenSpec(Spec, Protocol): def propose_grasps(self, object_pointcloud: PointCloud2) -> GraspCandidateArray: ... + + +class HeuristicGraspSpec(GraspGenSpec, Protocol): + """Contract for deterministic grasp proposals from an object point cloud.""" diff --git a/dimos/manipulation/grasping/heuristic_grasp.py b/dimos/manipulation/grasping/heuristic_grasp.py new file mode 100644 index 0000000000..4986b802a5 --- /dev/null +++ b/dimos/manipulation/grasping/heuristic_grasp.py @@ -0,0 +1,73 @@ +# 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. + +"""Deterministic top-down grasp proposals for segmented object point clouds.""" + +from __future__ import annotations + +import math + +import numpy as np + +from dimos.core.core import rpc +from dimos.core.module import Module +from dimos.manipulation.grasping.grasp_gen_spec import HeuristicGraspSpec +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.manipulation_msgs.GraspCandidate import GraspCandidate +from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.std_msgs.Header import Header + + +class HeuristicGraspModule(Module, HeuristicGraspSpec): + """Propose one top-down parallel-jaw grasp from object-cloud geometry.""" + + @rpc + def propose_grasps(self, object_pointcloud: PointCloud2) -> GraspCandidateArray: + """Return a center grasp with jaws aligned to the narrow horizontal cloud axis.""" + if object_pointcloud.ts is None: + raise ValueError("object pointcloud must have a timestamp") + if not object_pointcloud.frame_id: + raise ValueError("object pointcloud frame_id must not be empty") + points = object_pointcloud.points_f32() + if points.ndim != 2 or points.shape[1] != 3 or len(points) < 3: + raise ValueError("object pointcloud must contain at least three XYZ points") + if not np.all(np.isfinite(points)): + raise ValueError("object pointcloud XYZ values must be finite floats in metres") + + xy = points[:, :2] + center_xy = np.median(xy, axis=0) + low_z, high_z = np.quantile(points[:, 2], [0.05, 0.95]) + yaw = self._narrow_axis_yaw(xy) + pose = Pose( + Vector3(float(center_xy[0]), float(center_xy[1]), float((low_z + high_z) / 2.0)), + Quaternion.from_euler(Vector3(-math.pi, 0.0, yaw)), + ) + return GraspCandidateArray( + Header(float(object_pointcloud.ts), object_pointcloud.frame_id), + [GraspCandidate(pose, score=1.0)], + ) + + @staticmethod + def _narrow_axis_yaw(xy: np.ndarray) -> float: + """Align the gripper closing axis with the cloud's narrow horizontal axis.""" + centered = xy - np.mean(xy, axis=0) + covariance = centered.T @ centered + values, vectors = np.linalg.eigh(covariance) + if values[1] <= 0.0 or np.isclose(values[0], values[1], rtol=0.05): + return 0.0 + narrow_axis = vectors[:, 0] + return math.atan2(float(narrow_axis[1]), float(narrow_axis[0])) - math.pi / 2.0 diff --git a/dimos/manipulation/grasping/test_heuristic_grasp.py b/dimos/manipulation/grasping/test_heuristic_grasp.py new file mode 100644 index 0000000000..61742b6410 --- /dev/null +++ b/dimos/manipulation/grasping/test_heuristic_grasp.py @@ -0,0 +1,51 @@ +# 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 unittest.mock import patch + +import numpy as np +import pytest + +from dimos.core.module import ModuleBase +from dimos.manipulation.grasping.heuristic_grasp import HeuristicGraspModule +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + + +def test_heuristic_grasp_proposes_centered_top_down_pose() -> None: + with patch.object(ModuleBase, "__init__", lambda self, config_args: None): + module = HeuristicGraspModule() + cloud = PointCloud2.from_numpy( + np.asarray( + [ + [-0.10, -0.02, 0.10], + [-0.10, 0.02, 0.10], + [0.10, -0.02, 0.20], + [0.10, 0.02, 0.20], + ], + dtype=np.float32, + ), + frame_id="link_base", + timestamp=1.0, + ) + + proposals = module.propose_grasps(cloud) + + assert len(proposals.candidates) == 1 + pose = proposals.candidates[0].pose + assert pose.position.x == pytest.approx(0.0) + assert pose.position.y == pytest.approx(0.0) + assert pose.position.z == pytest.approx(0.15) + approach = pose.orientation.rotate_vector(Vector3(0.0, 0.0, 1.0)) + assert approach.z == pytest.approx(-1.0) diff --git a/dimos/manipulation/picknplace.py b/dimos/manipulation/picknplace.py index 8fc5bb5054..e1bd23fe81 100644 --- a/dimos/manipulation/picknplace.py +++ b/dimos/manipulation/picknplace.py @@ -29,7 +29,7 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.manipulation.candidate_filter_spec import GraspCandidateFilterSpec -from dimos.manipulation.grasping.grasp_gen_spec import GraspGenSpec +from dimos.manipulation.grasping.grasp_gen_spec import GraspGenSpec, HeuristicGraspSpec from dimos.manipulation.obstacle_world_spec import ObstacleWorldSpec from dimos.manipulation.pick_execution_spec import PickExecutionSpec from dimos.manipulation.visualization.layers import ( @@ -38,15 +38,12 @@ PointCloudElement, VisualizationLayer, ) -from dimos.manipulation.visualization.pose_overlay import draw_pose_axes from dimos.manipulation.visualization_spec import ManipulationVisualizationSpec from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray from dimos.perception.experimental.object import ( Object as DetObject, @@ -83,18 +80,6 @@ def _estimate_table_surface(points: np.ndarray) -> dict[str, float] | None: } -def _table_midpoint_grasp_z( - points: np.ndarray, tabletop_z: float | None, fallback_z: float -) -> float: - """Return the midpoint from the physical table plane to an object's observed top surface.""" - if tabletop_z is None or points.ndim != 2 or points.shape[1] != 3 or len(points) < 10: - return fallback_z - top_z = float(np.quantile(points[:, 2], 0.95)) - if top_z <= tabletop_z: - return fallback_z - return tabletop_z + (top_z - tabletop_z) / 2.0 - - def _primitive_mesh( shape: Literal["box", "sphere", "cylinder"], center: Vector3, @@ -206,9 +191,8 @@ def _primitive_mesh( class PickNPlaceConfig(ModuleConfig): """Configuration for PickNPlaceModule.""" - align_grasp_yaw: bool = False - grasp: Literal["obb_center", "graspgenx"] = Field( - default="obb_center", validation_alias=AliasChoices("grasp", "grasp_strategy") + grasp: Literal["heuristic", "graspgenx"] = Field( + default="heuristic", validation_alias=AliasChoices("grasp", "grasp_strategy") ) graspgenx_pregrasp_offset: float = 0.10 graspgenx_ik_filter_limit: int = 10 @@ -222,13 +206,12 @@ class PickNPlaceModule(Module): config: PickNPlaceConfig _scene: ObjectSceneRegistrationSpec _grasp_generator: GraspGenSpec | None + _heuristic_grasp_generator: HeuristicGraspSpec _grasp_filter: GraspCandidateFilterSpec _pick_execution: PickExecutionSpec _obstacle_world: ObstacleWorldSpec _visualization: ManipulationVisualizationSpec objects: In[list[DetObject]] - camera_info: In[CameraInfo] - basic_grasp_overlay: Out[Image] graspgenx_candidates: Out[GraspCandidateArray] def __init__(self, **kwargs: object) -> None: @@ -236,7 +219,6 @@ def __init__(self, **kwargs: object) -> None: self._objects_condition = threading.Condition() self._latest_objects: tuple[DetObject, ...] = () self._objects_version = 0 - self._camera_info: CameraInfo | None = None self._goal_pose: PoseStamped | None = None self._pre_grasp_pose: PoseStamped | None = None self._grasp_candidates: GraspCandidateArray | None = None @@ -250,7 +232,6 @@ def __init__(self, **kwargs: object) -> None: def start(self) -> None: super().start() self.objects.subscribe(self._on_objects) - self.camera_info.subscribe(self._on_camera_info) def _on_objects(self, objects: list[DetObject]) -> None: with self._objects_condition: @@ -258,10 +239,6 @@ def _on_objects(self, objects: list[DetObject]) -> None: self._objects_version += 1 self._objects_condition.notify_all() - def _on_camera_info(self, camera_info: CameraInfo) -> None: - with self._objects_condition: - self._camera_info = camera_info - @rpc def scan_scene( self, prompt: str | None = None, prompts: list[str] | None = None @@ -487,39 +464,22 @@ def clear_scene_geometry(self) -> SkillResult: @rpc def get_goal_pose(self, number: int) -> PoseStamped | None: """Select an object and return its downward-facing, floor-clamped grasp goal.""" - selection = self._basic_grasp(number) - if selection is None: + obj = self._object_for_number(number) + if obj is None: return None - grasp, obj = selection if self.config.grasp == "graspgenx": if self._grasp_generator is None: raise RuntimeError("GraspGenX is not configured for this pick-and-place blueprint") candidates = self._grasp_generator.propose_grasps(obj.pointcloud) - self._selected_object = obj - self._grasp_candidates = self._filter_graspgenx_candidates(candidates) - if not self._grasp_candidates.candidates: - self.graspgenx_candidates.publish(self._grasp_candidates) - return None - return self._select_graspgenx_candidate(0) - yaw = self._grasp_yaw(obj) if self.config.align_grasp_yaw else 0.0 - pick_execution = getattr(self, "_pick_execution", None) - current_pose = pick_execution.get_ee_pose() if pick_execution is not None else None - if current_pose is not None: - yaw = self._closest_parallel_jaw_yaw(yaw, current_pose.orientation.to_euler().z) - self._grasp_candidates = None + candidates = self._filter_graspgenx_candidates(candidates) + else: + candidates = self._heuristic_grasp_generator.propose_grasps(obj.pointcloud) self._selected_object = obj - self.graspgenx_candidates.publish(GraspCandidateArray()) - grasp_z = _table_midpoint_grasp_z( - obj.pointcloud.points_f32(), self._tabletop_z, grasp.position.z - ) - self._goal_pose = PoseStamped( - ts=grasp.ts, - frame_id=grasp.frame_id, - position=Vector3(grasp.position.x, grasp.position.y, max(grasp_z, 0.100)), - orientation=Quaternion.from_euler(Vector3(-math.pi, 0.0, yaw)), - ) - self._pre_grasp_pose = None - return self._goal_pose + self._grasp_candidates = candidates + if not candidates.candidates: + self.graspgenx_candidates.publish(candidates) + return None + return self._select_graspgenx_candidate(0) @skill def select_object(self, number: int) -> SkillResult: @@ -714,14 +674,10 @@ def get_pre_grasp_pose(self) -> PoseStamped | None: """Return the selected goal offset 100 mm opposite its final approach direction.""" if self._goal_pose is None: return None - if self.config.grasp == "graspgenx": - offset = self._goal_pose.orientation.rotate_vector( - # GraspGenX local +Z points in the direction of the final - # approach. A pre-grasp retreats along the opposite axis. - Vector3(0.0, 0.0, -self.config.graspgenx_pregrasp_offset) - ) - else: - offset = Vector3(0.0, 0.0, 0.100) + offset = self._goal_pose.orientation.rotate_vector( + # Both providers express local +Z as the final approach direction. + Vector3(0.0, 0.0, -self.config.graspgenx_pregrasp_offset) + ) self._pre_grasp_pose = PoseStamped( ts=self._goal_pose.ts, frame_id=self._goal_pose.frame_id, @@ -872,26 +828,6 @@ def estimate_table(self) -> SkillResult: ) return SkillResult.ok("Table estimated", **estimate) - def _basic_grasp(self, number: int) -> tuple[PoseStamped, DetObject] | None: - """Return the selected cloud's OBB-center grasp frame and object geometry.""" - with self._objects_condition: - if number < 1 or number > len(self._latest_objects): - return None - obj = self._latest_objects[number - 1] - camera_info = self._camera_info - grasp = PoseStamped( - ts=obj.ts, - frame_id=obj.frame_id, - position=obj.center, - orientation=obj.pose.orientation, - ) - if camera_info is not None and obj.camera_transform is not None and obj.image is not None: - if overlay := draw_pose_axes( - obj.image, grasp, obj.camera_transform.inverse(), camera_info - ): - self.basic_grasp_overlay.publish(overlay) - return grasp, obj - def _object_for_number(self, number: int) -> DetObject | None: with self._objects_condition: if number < 1 or number > len(self._latest_objects): @@ -920,17 +856,3 @@ def _install_geometry( self._scene_geometry_ids.add(obstacle_id) return True return False - - @staticmethod - def _grasp_yaw(obj: DetObject) -> float: - """Align the gripper's local Y closing axis with the narrowest horizontal OBB axis.""" - rotation = obj.pose.orientation.to_rotation_matrix() - extents = (obj.size.x, obj.size.y, obj.size.z) - horizontal_axes = sorted(range(3), key=lambda axis: abs(rotation[2, axis]))[:2] - narrow_axis = min(horizontal_axes, key=lambda axis: extents[axis]) - return math.atan2(rotation[1, narrow_axis], rotation[0, narrow_axis]) - math.pi / 2 - - @staticmethod - def _closest_parallel_jaw_yaw(target_yaw: float, current_yaw: float) -> float: - """Choose the equivalent parallel-jaw yaw requiring the smallest wrist rotation.""" - return target_yaw + math.pi * round((current_yaw - target_yaw) / math.pi) diff --git a/dimos/manipulation/test_picknplace.py b/dimos/manipulation/test_picknplace.py index f1d693800f..dc40c80941 100644 --- a/dimos/manipulation/test_picknplace.py +++ b/dimos/manipulation/test_picknplace.py @@ -25,7 +25,6 @@ PickNPlaceConfig, PickNPlaceModule, _estimate_table_surface, - _table_midpoint_grasp_z, ) from dimos.manipulation.planning.spec.models import IKResult, IKStatus from dimos.manipulation.visualization.layers import MeshElement @@ -58,6 +57,23 @@ def test_picknplace_scans_and_selects_target() -> None: obj.camera_transform = None obj.image = None obj.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) + module._heuristic_grasp_generator = MagicMock( + propose_grasps=MagicMock( + return_value=GraspCandidateArray( + Header(1.0, "link_base"), + [ + GraspCandidate( + Pose( + Vector3(0.1, 0.2, 0.1), + Quaternion.from_euler(Vector3(-math.pi, 0.0, 0.0)), + ), + score=1.0, + ) + ], + ) + ) + ) + module.graspgenx_candidates = MagicMock() scene.scan_scene.side_effect = lambda: (module._on_objects([obj]), detections)[1] with patch("dimos.manipulation.picknplace.to_detection3d_array") as to_detection3d_array: @@ -98,11 +114,7 @@ def test_picknplace_scans_and_selects_target() -> None: module.scan_scene("water bottle") scene.set_prompts.assert_called_once_with(["water bottle"]) - module.config = PickNPlaceConfig(align_grasp_yaw=True) - yaw_aligned_goal = module.get_goal_pose(1) - assert yaw_aligned_goal is not None - expected = Quaternion.from_euler(Vector3(-math.pi, 0.0, 0.0)) - assert yaw_aligned_goal.orientation.angle_to(expected) == pytest.approx(0.0) + assert module._heuristic_grasp_generator.propose_grasps.call_count == 2 def test_scan_objects_uses_independent_simple_queries() -> None: @@ -207,14 +219,6 @@ def test_picknplace_graspgenx_uses_xarm_tcp_calibration() -> None: ) -def test_picknplace_yaw_alignment_defaults_to_disabled() -> None: - assert not PickNPlaceConfig().align_grasp_yaw - - -def test_parallel_jaw_yaw_uses_the_nearest_equivalent_orientation() -> None: - assert PickNPlaceModule._closest_parallel_jaw_yaw(-math.pi + 0.02, 0.0) == pytest.approx(0.02) - - def test_picknplace_blueprint_accepts_short_backend_and_grasp_options() -> None: config = BlueprintConfigParser(picknplace) @@ -240,26 +244,6 @@ def test_table_surface_estimate_ignores_objects_above_the_table() -> None: assert estimate["depth"] >= 1.0 -def test_table_midpoint_grasp_uses_observed_object_height() -> None: - points = np.array( - [ - [0.2, 0.1, 0.117], - [0.2, 0.1, 0.119], - [0.2, 0.1, 0.120], - [0.2, 0.1, 0.121], - [0.2, 0.1, 0.120], - [0.2, 0.1, 0.119], - [0.2, 0.1, 0.120], - [0.2, 0.1, 0.119], - [0.2, 0.1, 0.120], - [0.2, 0.1, 0.120], - ] - ) - - assert _table_midpoint_grasp_z(points, 0.100, 0.120) == pytest.approx(0.110275) - assert _table_midpoint_grasp_z(points[:9], 0.100, 0.120) == pytest.approx(0.120) - - def test_table_surface_estimate_displays_filled_tabletop() -> None: with patch.object(ModuleBase, "__init__", lambda self, config_args: None): module = PickNPlaceModule() @@ -448,7 +432,7 @@ def test_picknplace_clears_candidates_when_no_graspgenx_proposal_is_safe() -> No assert module.graspgenx_candidates.publish.call_args.args[0].candidates == [] -def test_picknplace_returns_empty_candidates_for_obb_grasps() -> None: +def test_picknplace_returns_empty_candidates_before_provider_selection() -> None: with patch.object(ModuleBase, "__init__", lambda self, config_args: None): module = PickNPlaceModule() module.config = PickNPlaceConfig() diff --git a/dimos/manipulation/visualization/rerun.py b/dimos/manipulation/visualization/rerun.py index 61c4559ec2..c8f1a39284 100644 --- a/dimos/manipulation/visualization/rerun.py +++ b/dimos/manipulation/visualization/rerun.py @@ -46,7 +46,6 @@ def _blueprint() -> rrb.Blueprint: rrb.Horizontal( rrb.Vertical( rrb.Spatial2DView(origin="world/annotated_image", name="Object Segmentation"), - rrb.Spatial2DView(origin="world/basic_grasp_overlay", name="Grasp Pose"), rrb.Spatial2DView(origin="world/color_camera/color_image", name="RGB"), ), rrb.Spatial3DView(origin="world", name="Filtered Objects"), @@ -61,7 +60,6 @@ def _topic_to_entity(topic: Any) -> str: "/camera_info": "world/color_camera", "/depth_image": "world/depth_camera/depth_image", "/depth_camera_info": "world/depth_camera", - "/basic_grasp_overlay": "world/basic_grasp_overlay", "/graspgenx_candidates": "world/graspgenx_candidates", "/detections_3d": "world/detections_3d", "/pointcloud": "world/pointcloud", diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index b50328fd4e..da27cdf942 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -221,6 +221,7 @@ "grasp-gen-x-module": "dimos.manipulation.grasping.grasp_gen_x.GraspGenXModule", "grasping-module": "dimos.manipulation.grasping.grasping.GraspingModule", "gstreamer-camera-module": "dimos.hardware.sensors.camera.gstreamer.gstreamer_camera.GstreamerCameraModule", + "heuristic-grasp-module": "dimos.manipulation.grasping.heuristic_grasp.HeuristicGraspModule", "hosted-stats-module": "dimos.teleop.hosted.hosted_stats.HostedStatsModule", "joint-trajectory-controller": "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller.JointTrajectoryController", "joystick-module": "dimos.robot.unitree.b1.joystick_module.JoystickModule", From 0ed4e653d7f16060381a442f21b407ec727f08bd Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Fri, 7 Aug 2026 17:35:06 -0700 Subject: [PATCH 3/5] refactor(manipulation): generalize grasp candidates --- dimos/manipulation/README.md | 9 +++ dimos/manipulation/picknplace.py | 59 +++++++++++++------ dimos/manipulation/pnpconsole.py | 2 +- dimos/manipulation/test_picknplace.py | 56 +++++++++++++++--- dimos/manipulation/visualization/rerun.py | 8 +-- .../manipulation/visualization/test_rerun.py | 17 +++--- 6 files changed, 108 insertions(+), 43 deletions(-) diff --git a/dimos/manipulation/README.md b/dimos/manipulation/README.md index e84624cdad..3fc64c70a3 100644 --- a/dimos/manipulation/README.md +++ b/dimos/manipulation/README.md @@ -50,6 +50,9 @@ uv run --no-sync dimos run picknplace --daemon \ Moondream requires EdgeTAM because it produces detection boxes rather than masks. `pnp.grasp` accepts `heuristic` or `graspgenx`. The heuristic provider derives one top-down proposal from the segmented object cloud; GraspGenX loads only when selected. +`pnp.candidate_filter` accepts `ik_collision` (default) or `off`; `pnp.candidate_ranking` accepts +`confidence` (default) or `ik_feasibility`. Additional policies can be added in +`PickNPlaceModule._prepare_candidates` without changing either provider. Then connect the console: @@ -57,6 +60,12 @@ Then connect the console: uv run --no-sync python -m dimos.manipulation.pnpconsole ``` +Run the box-filling agent with: + +```bash +uv run --no-sync dimos run picknplace-agent --daemon +``` + Stop a running pipeline with: ```bash diff --git a/dimos/manipulation/picknplace.py b/dimos/manipulation/picknplace.py index e1bd23fe81..47be85d316 100644 --- a/dimos/manipulation/picknplace.py +++ b/dimos/manipulation/picknplace.py @@ -194,8 +194,10 @@ class PickNPlaceConfig(ModuleConfig): grasp: Literal["heuristic", "graspgenx"] = Field( default="heuristic", validation_alias=AliasChoices("grasp", "grasp_strategy") ) - graspgenx_pregrasp_offset: float = 0.10 - graspgenx_ik_filter_limit: int = 10 + pregrasp_offset: float = 0.10 + candidate_filter: Literal["off", "ik_collision"] = "ik_collision" + candidate_ranking: Literal["confidence", "ik_feasibility"] = "confidence" + candidate_ik_limit: int = Field(default=10, gt=0) grasp_empty_closed_threshold: float = 0.01 grasp_feedback_delay: float = 0.5 @@ -212,7 +214,7 @@ class PickNPlaceModule(Module): _obstacle_world: ObstacleWorldSpec _visualization: ManipulationVisualizationSpec objects: In[list[DetObject]] - graspgenx_candidates: Out[GraspCandidateArray] + grasp_candidates: Out[GraspCandidateArray] def __init__(self, **kwargs: object) -> None: super().__init__(**kwargs) @@ -471,15 +473,15 @@ def get_goal_pose(self, number: int) -> PoseStamped | None: if self._grasp_generator is None: raise RuntimeError("GraspGenX is not configured for this pick-and-place blueprint") candidates = self._grasp_generator.propose_grasps(obj.pointcloud) - candidates = self._filter_graspgenx_candidates(candidates) else: candidates = self._heuristic_grasp_generator.propose_grasps(obj.pointcloud) + candidates = self._prepare_candidates(candidates) self._selected_object = obj self._grasp_candidates = candidates if not candidates.candidates: - self.graspgenx_candidates.publish(candidates) + self.grasp_candidates.publish(candidates) return None - return self._select_graspgenx_candidate(0) + return self._select_grasp_candidate(0) @skill def select_object(self, number: int) -> SkillResult: @@ -639,15 +641,15 @@ def move(x: float, y: float, z: float) -> SkillResult: @rpc def select_grasp_candidate(self, rank: int) -> PoseStamped | None: - """Select one ranked GraspGenX proposal as the goal and Rerun highlight.""" - return self._select_graspgenx_candidate(rank) + """Select one ranked grasp proposal as the goal and Rerun highlight.""" + return self._select_grasp_candidate(rank) - def _select_graspgenx_candidate(self, rank: int) -> PoseStamped | None: + def _select_grasp_candidate(self, rank: int) -> PoseStamped | None: candidates = self._grasp_candidates if candidates is None or rank < 0 or rank >= len(candidates.candidates): return None candidates.selected_index = rank - self.graspgenx_candidates.publish(candidates) + self.grasp_candidates.publish(candidates) candidate = candidates.candidates[rank] self._goal_pose = PoseStamped( ts=candidates.header.timestamp, @@ -658,16 +660,35 @@ def _select_graspgenx_candidate(self, rank: int) -> PoseStamped | None: self._pre_grasp_pose = None return self._goal_pose - def _filter_graspgenx_candidates(self, candidates: GraspCandidateArray) -> GraspCandidateArray: - """Keep only top-ranked proposals whose TCP IK is collision-free in the live world.""" - accepted = [] - for candidate in candidates.candidates[: self.config.graspgenx_ik_filter_limit]: + def _prepare_candidates(self, candidates: GraspCandidateArray) -> GraspCandidateArray: + """Filter and rank provider proposals using independently selectable policies.""" + if self.config.candidate_filter == "off" and self.config.candidate_ranking == "confidence": + return GraspCandidateArray( + candidates.header, + sorted(candidates.candidates, key=lambda candidate: -candidate.score), + ) + + evaluated = [] + candidates_to_evaluate = candidates.candidates[: self.config.candidate_ik_limit] + for candidate in candidates_to_evaluate: result = self._grasp_filter.inverse_kinematics_single( candidate.pose, "arm", check_collision=True ) - if result.is_success(): - accepted.append(candidate) - return GraspCandidateArray(candidates.header, accepted) + evaluated.append((candidate, result.is_success())) + + if self.config.candidate_filter == "ik_collision": + evaluated = [(candidate, feasible) for candidate, feasible in evaluated if feasible] + else: + evaluated.extend( + (candidate, False) + for candidate in candidates.candidates[len(candidates_to_evaluate) :] + ) + + if self.config.candidate_ranking == "ik_feasibility": + evaluated.sort(key=lambda item: (not item[1], -item[0].score)) + else: + evaluated.sort(key=lambda item: -item[0].score) + return GraspCandidateArray(candidates.header, [candidate for candidate, _ in evaluated]) @rpc def get_pre_grasp_pose(self) -> PoseStamped | None: @@ -676,7 +697,7 @@ def get_pre_grasp_pose(self) -> PoseStamped | None: return None offset = self._goal_pose.orientation.rotate_vector( # Both providers express local +Z as the final approach direction. - Vector3(0.0, 0.0, -self.config.graspgenx_pregrasp_offset) + Vector3(0.0, 0.0, -self.config.pregrasp_offset) ) self._pre_grasp_pose = PoseStamped( ts=self._goal_pose.ts, @@ -759,7 +780,7 @@ def _publish_scene_objects(self) -> None: @rpc def get_grasp_candidates(self) -> GraspCandidateArray: - """Return the GraspGenX proposals generated for the selected object.""" + """Return proposals generated by the selected grasp provider.""" return self._grasp_candidates or GraspCandidateArray() @rpc diff --git a/dimos/manipulation/pnpconsole.py b/dimos/manipulation/pnpconsole.py index 304cb3d361..9a13492016 100644 --- a/dimos/manipulation/pnpconsole.py +++ b/dimos/manipulation/pnpconsole.py @@ -90,7 +90,7 @@ def _preview(manipulation: Any) -> None: def _print_grasp_candidates(candidates: Any) -> None: if not candidates.candidates: return - print(f"GraspGenX proposals: {len(candidates.candidates)}") + print(f"Grasp proposals: {len(candidates.candidates)}") for rank, candidate in enumerate(candidates.candidates[:10]): pose = candidate.pose print( diff --git a/dimos/manipulation/test_picknplace.py b/dimos/manipulation/test_picknplace.py index dc40c80941..ff72961ad9 100644 --- a/dimos/manipulation/test_picknplace.py +++ b/dimos/manipulation/test_picknplace.py @@ -73,7 +73,10 @@ def test_picknplace_scans_and_selects_target() -> None: ) ) ) - module.graspgenx_candidates = MagicMock() + module._grasp_filter = MagicMock( + inverse_kinematics_single=MagicMock(return_value=IKResult(IKStatus.SUCCESS)) + ) + module.grasp_candidates = MagicMock() scene.scan_scene.side_effect = lambda: (module._on_objects([obj]), detections)[1] with patch("dimos.manipulation.picknplace.to_detection3d_array") as to_detection3d_array: @@ -347,7 +350,7 @@ def test_picknplace_uses_top_graspgenx_candidate() -> None: module._grasp_filter = MagicMock( inverse_kinematics_single=MagicMock(return_value=IKResult(IKStatus.SUCCESS)) ) - module.graspgenx_candidates = MagicMock() + module.grasp_candidates = MagicMock() module._visualization = MagicMock() goal = module.get_goal_pose(1) @@ -358,7 +361,7 @@ def test_picknplace_uses_top_graspgenx_candidate() -> None: assert goal.position == candidate.pose.position assert goal.orientation == candidate.pose.orientation assert module.get_grasp_candidates().candidates == [candidate, second_candidate] - module.graspgenx_candidates.publish.assert_called_once_with(module.get_grasp_candidates()) + module.grasp_candidates.publish.assert_called_once_with(module.get_grasp_candidates()) pre_grasp = module.get_pre_grasp_pose() assert pre_grasp is not None assert pre_grasp.position.x == pytest.approx(goal.position.x - 0.1) @@ -377,7 +380,7 @@ def test_picknplace_uses_top_graspgenx_candidate() -> None: assert pre_grasp.position.z == pytest.approx(selected_goal.position.z - 0.1) -def test_picknplace_excludes_collision_or_ik_infeasible_graspgenx_candidates() -> None: +def test_picknplace_excludes_collision_or_ik_infeasible_candidates() -> None: with patch.object(ModuleBase, "__init__", lambda self, config_args: None): module = PickNPlaceModule() module.config = PickNPlaceConfig(grasp_strategy="graspgenx") @@ -389,7 +392,7 @@ def test_picknplace_excludes_collision_or_ik_infeasible_graspgenx_candidates() - ) ) - filtered = module._filter_graspgenx_candidates( + filtered = module._prepare_candidates( GraspCandidateArray(Header(2.0, "link_base"), [safe, unsafe]) ) @@ -402,7 +405,42 @@ def test_picknplace_excludes_collision_or_ik_infeasible_graspgenx_candidates() - ) -def test_picknplace_clears_candidates_when_no_graspgenx_proposal_is_safe() -> None: +def test_picknplace_can_skip_ik_filter_and_keep_confidence_order() -> None: + with patch.object(ModuleBase, "__init__", lambda self, config_args: None): + module = PickNPlaceModule() + module.config = PickNPlaceConfig(candidate_filter="off") + low_score = GraspCandidate(Pose(Vector3(0.4, 0.5, 0.6), Quaternion()), score=0.2) + high_score = GraspCandidate(Pose(Vector3(0.2, 0.3, 0.4), Quaternion()), score=0.9) + module._grasp_filter = MagicMock() + + prepared = module._prepare_candidates( + GraspCandidateArray(Header(2.0, "link_base"), [low_score, high_score]) + ) + + assert prepared.candidates == [high_score, low_score] + module._grasp_filter.inverse_kinematics_single.assert_not_called() + + +def test_picknplace_can_rank_candidates_by_ik_feasibility() -> None: + with patch.object(ModuleBase, "__init__", lambda self, config_args: None): + module = PickNPlaceModule() + module.config = PickNPlaceConfig(candidate_filter="off", candidate_ranking="ik_feasibility") + unsafe = GraspCandidate(Pose(Vector3(0.4, 0.5, 0.6), Quaternion()), score=0.9) + safe = GraspCandidate(Pose(Vector3(0.2, 0.3, 0.4), Quaternion()), score=0.2) + module._grasp_filter = MagicMock( + inverse_kinematics_single=MagicMock( + side_effect=[IKResult(IKStatus.NO_SOLUTION), IKResult(IKStatus.SUCCESS)] + ) + ) + + prepared = module._prepare_candidates( + GraspCandidateArray(Header(2.0, "link_base"), [unsafe, safe]) + ) + + assert prepared.candidates == [safe, unsafe] + + +def test_picknplace_clears_candidates_when_no_proposal_is_safe() -> None: with patch.object(ModuleBase, "__init__", lambda self, config_args: None): module = PickNPlaceModule() module.config = PickNPlaceConfig(grasp_strategy="graspgenx") @@ -425,11 +463,11 @@ def test_picknplace_clears_candidates_when_no_graspgenx_proposal_is_safe() -> No module._grasp_filter = MagicMock( inverse_kinematics_single=MagicMock(return_value=IKResult(IKStatus.NO_SOLUTION)) ) - module.graspgenx_candidates = MagicMock() + module.grasp_candidates = MagicMock() assert module.get_goal_pose(1) is None - module.graspgenx_candidates.publish.assert_called_once() - assert module.graspgenx_candidates.publish.call_args.args[0].candidates == [] + module.grasp_candidates.publish.assert_called_once() + assert module.grasp_candidates.publish.call_args.args[0].candidates == [] def test_picknplace_returns_empty_candidates_before_provider_selection() -> None: diff --git a/dimos/manipulation/visualization/rerun.py b/dimos/manipulation/visualization/rerun.py index c8f1a39284..e557b1a0ba 100644 --- a/dimos/manipulation/visualization/rerun.py +++ b/dimos/manipulation/visualization/rerun.py @@ -33,7 +33,7 @@ def picknplace_rerun_config() -> dict[str, Any]: image_topic="world/color_camera/color_image", ), "world/pointcloud": _pointcloud_to_rerun, - "world/graspgenx_candidates": _graspgenx_candidates_to_rerun, + "world/grasp_candidates": _grasp_candidates_to_rerun, "world/detections_3d": None, "world/depth_camera": None, "world/depth_camera/depth_image": None, @@ -60,7 +60,7 @@ def _topic_to_entity(topic: Any) -> str: "/camera_info": "world/color_camera", "/depth_image": "world/depth_camera/depth_image", "/depth_camera_info": "world/depth_camera", - "/graspgenx_candidates": "world/graspgenx_candidates", + "/grasp_candidates": "world/grasp_candidates", "/detections_3d": "world/detections_3d", "/pointcloud": "world/pointcloud", } @@ -81,11 +81,11 @@ def _pointcloud_to_rerun(msg: Any) -> Any: return msg.to_rerun(voxel_size=0.001, mode="points") -def _graspgenx_candidates_to_rerun(msg: Any) -> list[tuple[str, Any]]: +def _grasp_candidates_to_rerun(msg: Any) -> list[tuple[str, Any]]: """Render calibrated xArm TCP grasp candidates and their gripper geometry.""" import rerun as rr - root = "world/graspgenx_candidates" + root = "world/grasp_candidates" data: list[tuple[str, Any]] = [(root, rr.Clear(recursive=True))] frame_id = msg.header.frame_id if frame_id: diff --git a/dimos/manipulation/visualization/test_rerun.py b/dimos/manipulation/visualization/test_rerun.py index dc37761bdb..67ad24a607 100644 --- a/dimos/manipulation/visualization/test_rerun.py +++ b/dimos/manipulation/visualization/test_rerun.py @@ -13,7 +13,7 @@ # limitations under the License. from dimos.manipulation.visualization.rerun import ( - _graspgenx_candidates_to_rerun, + _grasp_candidates_to_rerun, _topic_to_entity, ) from dimos.msgs.geometry_msgs.Pose import Pose @@ -24,10 +24,7 @@ def test_qualified_grasp_candidate_topic_uses_candidate_entity() -> None: - assert ( - _topic_to_entity("dimos/PickNPlaceModule/graspgenx_candidates") - == "world/graspgenx_candidates" - ) + assert _topic_to_entity("dimos/PickNPlaceModule/grasp_candidates") == "world/grasp_candidates" def test_top_grasp_candidate_has_selected_rerun_marker() -> None: @@ -40,9 +37,9 @@ def test_top_grasp_candidate_has_selected_rerun_marker() -> None: selected_index=1, ) - paths = [path for path, _ in _graspgenx_candidates_to_rerun(candidates)] + paths = [path for path, _ in _grasp_candidates_to_rerun(candidates)] - assert "world/graspgenx_candidates/00/selected" not in paths - assert "world/graspgenx_candidates/01/selected" in paths - assert "world/graspgenx_candidates/01/gripper_base" in paths - assert "world/graspgenx_candidates/01/gripper_base/jaws" in paths + assert "world/grasp_candidates/00/selected" not in paths + assert "world/grasp_candidates/01/selected" in paths + assert "world/grasp_candidates/01/gripper_base" in paths + assert "world/grasp_candidates/01/gripper_base/jaws" in paths From 1c7b486c387dff7c4535f5ff9188d368a28c99b0 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 8 Aug 2026 09:32:42 -0700 Subject: [PATCH 4/5] feat(manipulation): unify xarm pick and place --- CONTEXT.md | 131 +++ dimos/manipulation/README.md | 148 +-- dimos/manipulation/blueprints.py | 146 --- .../box_filling_pick_and_place_module.py | 132 +++ dimos/manipulation/candidate_filter_spec.py | 32 - dimos/manipulation/manipulation_module.py | 288 +++++- dimos/manipulation/obstacle_world_spec.py | 43 - dimos/manipulation/pick_and_place_module.py | 421 ++++++--- dimos/manipulation/pick_execution_spec.py | 42 - dimos/manipulation/picknplace.py | 879 ------------------ dimos/manipulation/planning/README.md | 2 +- .../planning/monitor/world_monitor.py | 8 +- .../monitor/world_obstacle_monitor.py | 23 +- .../planning/planners/roboplan_config.py | 1 + dimos/manipulation/pnpconsole.py | 314 ------- dimos/manipulation/skill_errors.py | 1 + .../manipulation/test_pick_and_place_unit.py | 358 ++++--- dimos/manipulation/test_picknplace.py | 478 ---------- dimos/manipulation/test_pnpconsole.py | 206 ---- dimos/manipulation/visualization_spec.py | 24 - dimos/perception/sim_object_scene.py | 228 +++++ dimos/robot/all_blueprints.py | 13 +- .../manipulators/xarm/blueprints/agentic.py | 20 +- .../xarm/blueprints/box_filling.py | 106 +++ .../manipulators/xarm/blueprints/graspgenx.py | 61 -- .../xarm/blueprints/perception.py | 62 -- .../xarm/blueprints/simulation.py | 49 +- dimos/robot/manipulators/xarm/config.py | 1 + dimos/robot/test_all_blueprints.py | 6 +- dimos/simulation/engines/mujoco_sim_module.py | 14 + ...ick-place-with-manipulation-state-owner.md | 3 + ...0002-compose-exactly-one-grasp-provider.md | 3 + ...3-use-unchecked-motion-for-contact-legs.md | 3 + .../0004-pin-numbered-object-selections.md | 3 + ...ing-out-of-generic-pick-place-interface.md | 3 + .../0006-place-held-objects-not-the-tcp.md | 3 + ...-revalidate-prepared-picks-at-execution.md | 3 + ...quire-verified-holding-for-pick-success.md | 3 + ...eplace-snapshot-obstacles-automatically.md | 3 + ...-use-world-for-the-pick-place-interface.md | 3 + ...t-the-first-feasible-provider-candidate.md | 3 + ...-publish-two-xarm-pick-place-blueprints.md | 3 + docs/capabilities/manipulation/agentic.md | 161 +--- docs/capabilities/manipulation/index.md | 8 +- 44 files changed, 1607 insertions(+), 2835 deletions(-) create mode 100644 CONTEXT.md delete mode 100644 dimos/manipulation/blueprints.py create mode 100644 dimos/manipulation/box_filling_pick_and_place_module.py delete mode 100644 dimos/manipulation/candidate_filter_spec.py delete mode 100644 dimos/manipulation/obstacle_world_spec.py delete mode 100644 dimos/manipulation/pick_execution_spec.py delete mode 100644 dimos/manipulation/picknplace.py delete mode 100644 dimos/manipulation/pnpconsole.py delete mode 100644 dimos/manipulation/test_picknplace.py delete mode 100644 dimos/manipulation/test_pnpconsole.py delete mode 100644 dimos/manipulation/visualization_spec.py create mode 100644 dimos/perception/sim_object_scene.py create mode 100644 dimos/robot/manipulators/xarm/blueprints/box_filling.py delete mode 100644 dimos/robot/manipulators/xarm/blueprints/graspgenx.py delete mode 100644 dimos/robot/manipulators/xarm/blueprints/perception.py create mode 100644 docs/adr/0001-keep-pick-place-with-manipulation-state-owner.md create mode 100644 docs/adr/0002-compose-exactly-one-grasp-provider.md create mode 100644 docs/adr/0003-use-unchecked-motion-for-contact-legs.md create mode 100644 docs/adr/0004-pin-numbered-object-selections.md create mode 100644 docs/adr/0005-keep-box-filling-out-of-generic-pick-place-interface.md create mode 100644 docs/adr/0006-place-held-objects-not-the-tcp.md create mode 100644 docs/adr/0007-revalidate-prepared-picks-at-execution.md create mode 100644 docs/adr/0008-require-verified-holding-for-pick-success.md create mode 100644 docs/adr/0009-replace-snapshot-obstacles-automatically.md create mode 100644 docs/adr/0010-use-world-for-the-pick-place-interface.md create mode 100644 docs/adr/0011-select-the-first-feasible-provider-candidate.md create mode 100644 docs/adr/0012-publish-two-xarm-pick-place-blueprints.md diff --git a/CONTEXT.md b/CONTEXT.md new file mode 100644 index 0000000000..ae018e1a99 --- /dev/null +++ b/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. diff --git a/dimos/manipulation/README.md b/dimos/manipulation/README.md index 3fc64c70a3..c6e4074942 100644 --- a/dimos/manipulation/README.md +++ b/dimos/manipulation/README.md @@ -1,138 +1,26 @@ -# Pick And Place +# Pick and place -This directory contains the configurable xArm6 `picknplace` operator pipeline. -It uses the wrist-mounted RealSense and object-scene registration in `link_base`. +`PickAndPlaceModule` is the single owner of generic pick/place transactions. +Its public workflow is: -## Setup +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. -GraspGenX runs in the main worktree `.venv` so it shares the live DimOS -pipeline. Its CUDA requirements differ from the repository lockfile; install -them once from the worktree root: +`BoxFillingPickAndPlaceModule` derives from the generic module and adds only +destination-container selection, fit checks, and box placement policy. -```bash -bash bin/setup-graspgenx-env -``` - -The setup installs Torch 2.7.1 CUDA 12.8, which supports the RTX 5070's -`sm_120` architecture, along with GraspGenX and its inference dependencies. -Use `uv run --no-sync` afterwards. Plain `uv run` reconciles the environment to -the lockfile's Torch 2.6 and removes the GPU architecture support required by -GraspGenX. - -The first GraspGenX startup downloads the pinned model checkpoint to the -Hugging Face cache and loads it onto the GPU. Later starts reuse that cache. -The setup also installs `edgetam-dimos`, which provides the `sam2` runtime used -by the EdgeTAM blueprint. - -## Run - -Start the default YOLO-E and deterministic heuristic-grasp pipeline: - -```bash -uv run --no-sync dimos run picknplace --daemon -``` - -Use text-prompted Moondream detection, EdgeTAM segmentation, and the deterministic heuristic grasp: +The two public products are: ```bash -uv run --no-sync dimos run picknplace --daemon \ - -o osr.det=moondream -o osr.seg=edgetam -o pnp.grasp=heuristic +uv run dimos run xarm-box-filling --daemon +uv run dimos run xarm-grasp-sim-agent --daemon ``` -Use the same perception stack with GraspGenX: - -```bash -uv run --no-sync dimos run picknplace --daemon \ - -o osr.det=moondream -o osr.seg=edgetam -o pnp.grasp=graspgenx -``` - -`osr.det` accepts `yoloe` or `moondream`; `osr.seg` accepts `yolo` or `edgetam`. -Moondream requires EdgeTAM because it produces detection boxes rather than masks. -`pnp.grasp` accepts `heuristic` or `graspgenx`. The heuristic provider derives one top-down -proposal from the segmented object cloud; GraspGenX loads only when selected. -`pnp.candidate_filter` accepts `ik_collision` (default) or `off`; `pnp.candidate_ranking` accepts -`confidence` (default) or `ik_feasibility`. Additional policies can be added in -`PickNPlaceModule._prepare_candidates` without changing either provider. - -Then connect the console: - -```bash -uv run --no-sync python -m dimos.manipulation.pnpconsole -``` - -Run the box-filling agent with: - -```bash -uv run --no-sync dimos run picknplace-agent --daemon -``` - -Stop a running pipeline with: - -```bash -uv run --no-sync dimos stop -``` - -## Operator Flow - -The console intentionally keeps planning and execution separate: - -1. Select `1` to scan the current scene. -2. Select `2` to inspect object number, name, and confidence. -3. Select `3` and choose an object. The selected provider returns proposals and displays the selected grasp. - Viser shows the selected object - cloud in amber, the grasp TCP axes in red, and the pre-grasp TCP axes in green. - GraspGenX's top ten proposals are filtered through collision-aware xArm IK; after table calibration, - candidates intersecting the table are omitted. The heuristic provider returns one deterministic proposal. -4. Select `4` to plan and preview the approach. Each Viser preview plays once - at a slow two-second duration. -5. Execute the approach only after inspecting the proposal and preview. -6. Select `6` to plan and preview descent, then select `7` to execute it. -7. Close the gripper with `8`, then select `9` to plan and preview ascent. -8. Select `10` to execute the ascent, `11` to open, and `13` to return home. -9. After a scene scan, select `14` to estimate and preview the tabletop. Once - the blue Viser outline matches the table, enter a collision clearance in - millimeters. The recommended clearance is 10 mm; enter `0` for no extra clearance. - The manual action installs the collision slab at the measured tabletop position for - all subsequent IK and trajectory plans. The pick-and-place blueprints do not install - a fixed floor slab. -10. After executing the approach, select `15` to collision-plan and execute the - descent, close the gripper, and execute the ascent without previews. It stops at - the first failed stage. - -Do not execute a learned grasp without checking its pose, the 100 mm pre-grasp -pose, the point-cloud/overlay visualization, and the collision-free preview. - -## Grasp Geometry - -`PickNPlaceModule.get_goal_pose()` stores the selected provider candidate as -the TCP goal in the candidate point cloud's frame. Its pre-grasp is computed as: - -```text -pre_grasp_position = grasp_position - grasp_orientation * (0, 0, 0.100 m) -``` - -Each provider uses local `+Z` as the final approach direction, so the pre-grasp retreats -along local `-Z`. It is not a world-Z lift: an angled or side grasp receives an -equally angled or sideward pre-grasp. Descent and ascent use Cartesian paths -between the current TCP pose and the selected grasp or pre-grasp target. - -The `picknplace-graspgenx` blueprint uses the xArm 85 mm gripper sweep-volume -and calibrated base-to-TCP transform. The TCP is rolled 90 degrees around the -GraspGenX approach axis so its closing jaws are perpendicular to a bottle's -length. Candidate score order comes from GraspGenX; no additional ranking is -applied by the operator pipeline. - -## Implementation Guide - -- `blueprints.py`: robot, camera, heuristic, and GraspGenX blueprint composition. -- `picknplace.py`: scan request, target selection, provider selection, and tool-axis pre-grasp calculation. -- `pnpconsole.py`: explicit operator stages and manual gripper/home controls. -- `grasping/grasp_gen_x.py`: import-safe proposal adapter and candidate contract. -- `grasping/grasp_gen_x_runtime.py`: in-process checkpoint load and GPU inference. -- `grasping/heuristic_grasp.py`: deterministic top-down proposal provider. -- `visualization/pose_overlay.py` and `visualization/rerun.py`: selected-object - cloud, image, and grasp overlays. - -The current scan is a single wrist-camera view. Automatic multi-view scanning, -EdgeTAM segmentation, and fused object clouds are planned follow-up work. Until -then, select targets with a complete enough visible point cloud for grasping. +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. diff --git a/dimos/manipulation/blueprints.py b/dimos/manipulation/blueprints.py deleted file mode 100644 index a64d5c6430..0000000000 --- a/dimos/manipulation/blueprints.py +++ /dev/null @@ -1,146 +0,0 @@ -# Copyright 2025-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. - -"""Compatibility exports for manipulation blueprints. - -Robot-owned manipulation blueprints now live under ``dimos.robot.manipulators``. -""" - -import math - -from dimos.agents.mcp.mcp_client import McpClient -from dimos.agents.mcp.mcp_server import McpServer -from dimos.core.coordination.blueprints import 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.picknplace import PickNPlaceModule -from dimos.manipulation.visualization.rerun import picknplace_rerun_config -from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig -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.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule -from dimos.robot.manipulators.common.blueprints import coordinator, trajectory_task -from dimos.robot.manipulators.xarm.blueprints.agentic import ( - xarm7_planner_coordinator_agent as xarm7_planner_coordinator_agent, - xarm_perception_agent as xarm_perception_agent, - xarm_perception_sim_agent as xarm_perception_sim_agent, -) -from dimos.robot.manipulators.xarm.blueprints.basic import ( - xarm7_planner_coordinator as xarm7_planner_coordinator, -) -from dimos.robot.manipulators.xarm.blueprints.perception import xarm_perception as xarm_perception -from dimos.robot.manipulators.xarm.blueprints.simulation import ( - xarm_perception_sim as xarm_perception_sim, -) -from dimos.robot.manipulators.xarm.config import make_xarm6_model_config, xarm6_hardware -from dimos.robot.manipulators.xarm.grasp_config import make_xarm_graspgenx_config -from dimos.visualization.vis_module import vis_module - -PICKNPLACE_CAMERA_TRANSFORM = Transform( - translation=Vector3(0.06693724, -0.0309563, 0.00691482), - rotation=Quaternion(0.70513398, 0.00535696, 0.70897578, -0.01052180), -) - -BOX_FILLING_SYSTEM_PROMPT = """You are operating an xArm box-filling workspace with RGB-D perception. - -Your recurring task is to collect requested blocks from the table and drop them into the measured white box. The available tools are the live interface to the robot, planner, gripper, and scene. Use their results as authoritative, make multiple calls when needed, and only report physical actions after a tool confirms success. - -For a collection task: go home to observe, use ``scan_objects`` with separate simple noun phrases such as ``["colored wooden block", "white box"]``, estimate and install the table collision with no added margin, and measure the white box with ``install_open_box``. Use ``get_object_geometry`` to identify blocks whose centers are inside the measured box opening; those blocks are complete and must be ignored. Select only outside blocks, then call ``pick_selected``. If it succeeds, call ``place_selected`` to drop it into the remembered box. Repeat for other outside blocks. If pickup verification fails, rescan and select before another attempt. - -When the user says put, place, or drop an object in the box, use ``place_selected``. It is a depth-derived drop: it computes the box-rim and held-object clearance itself, releases above the rim, and does not lower the end effector into the box. Do not substitute manually chosen poses or individual gripper commands for pick or drop sequences. -""" - -_picknplace_xarm6_hardware = xarm6_hardware("arm", gripper=True) -_picknplace_xarm6_model = make_xarm6_model_config( - name="arm", - add_gripper=True, - tf_extra_links=["link_base", "link6"], - home_joints=[0.0, math.radians(-40.0), math.radians(-50.0), 0.0, math.radians(90.0), 0.0], -) -_picknplace_xarm6_model.max_velocity = 0.25 -_picknplace_xarm6_model.max_acceleration = 0.5 -_xarm_graspgenx = make_xarm_graspgenx_config() - - -picknplace = autoconnect( - coordinator( - hardware=[_picknplace_xarm6_hardware], - tasks=[trajectory_task(_picknplace_xarm6_hardware)], - ), - ManipulationModule.blueprint( - robots=[_picknplace_xarm6_model], - visualization=ViserVisualizationConfig(port=8095), - planning_timeout=10.0, - ), - RealSenseCamera.blueprint( - width=848, - height=480, - fps=15, - camera_name="camera", - base_frame_id="link6", - base_transform=PICKNPLACE_CAMERA_TRANSFORM, - enable_depth=True, - align_depth_to_color=True, - enable_pointcloud=False, - ), - ObjectSceneRegistrationModule.blueprint( - instance_name="osr", - target_frame="link_base", - register_objects=False, - detect_on_request=True, - detector_confidence=0.4, - object_voxel_downsample=0.001, - ), - PickNPlaceModule.blueprint(instance_name="pnp"), - HeuristicGraspModule.blueprint(instance_name="heuristic_grasp"), - GraspGenXModule.blueprint( - instance_name="ggx", - load_on_start=False, - **_xarm_graspgenx.model_dump( - exclude={"rpc_transport", "tf_transport", "g", "instance_name", "load_on_start"} - ), - ), - vis_module( - global_config.viewer, - rerun_config=picknplace_rerun_config(), - ), -).global_config(rerun_open="web") - -picknplace_agent = autoconnect( - picknplace, - McpServer.blueprint( - allowed_skills=[ - "describe_scene", - "scan_objects", - "estimate_table", - "select_object", - "pick_selected", - "place_selected", - "get_object_geometry", - "install_open_box", - "set_table_collision", - "get_robot_state", - "reset", - "move_to_pose", - "close_gripper", - "open_gripper", - "go_home", - ] - ), - McpClient.blueprint(system_prompt=BOX_FILLING_SYSTEM_PROMPT), -) diff --git a/dimos/manipulation/box_filling_pick_and_place_module.py b/dimos/manipulation/box_filling_pick_and_place_module.py new file mode 100644 index 0000000000..4d846a584f --- /dev/null +++ b/dimos/manipulation/box_filling_pick_and_place_module.py @@ -0,0 +1,132 @@ +# 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. + +"""Box-filling application policy layered on generic pick and place.""" + +from __future__ import annotations + +from dataclasses import dataclass + +from pydantic import Field, FiniteFloat + +from dimos.agents.annotation import skill +from dimos.agents.skill_result import SkillResult +from dimos.manipulation.pick_and_place_module import ( + PickAndPlaceModule, + PickAndPlaceModuleConfig, +) +from dimos.manipulation.skill_errors import ManipulationSkillError + + +class BoxFillingPickAndPlaceModuleConfig(PickAndPlaceModuleConfig): + """Configuration for destination-container placement policy.""" + + box_wall_thickness: FiniteFloat = Field(default=0.01, gt=0.0) + drop_clearance: FiniteFloat = Field(default=0.02, ge=0.0) + + +@dataclass(frozen=True) +class _DestinationBox: + snapshot_version: int + center_x: float + center_y: float + rim_z: float + opening_width: float + opening_depth: float + + +class BoxFillingPickAndPlaceModule(PickAndPlaceModule): + """Add destination-box selection and fit-checked placement.""" + + config: BoxFillingPickAndPlaceModuleConfig + + def __init__(self, **kwargs: object) -> None: + super().__init__(**kwargs) + self._destination_box: _DestinationBox | None = None + + @skill + def select_destination_container( + self, number: int + ) -> SkillResult[ManipulationSkillError]: + """Use one object from the latest scan as the destination open box. + + Args: + number: One-based object number returned by the latest scan_objects call. + """ + if number < 1 or number > len(self._detection_snapshot): + return SkillResult.fail("INVALID_INPUT", f"No detected object numbered {number}") + box = self._detection_snapshot[number - 1] + wall = float(self.config.box_wall_thickness) + opening_width = box.size.x - 2.0 * wall + opening_depth = box.size.y - 2.0 * wall + if opening_width <= 0.0 or opening_depth <= 0.0: + return SkillResult.fail("INVALID_INPUT", "Container opening is too small") + self._destination_box = _DestinationBox( + snapshot_version=self._snapshot_version, + center_x=box.center.x, + center_y=box.center.y, + rim_z=box.center.z + box.size.z / 2.0, + opening_width=opening_width, + opening_depth=opening_depth, + ) + return SkillResult.ok( + f"Selected {number}. {box.name} as destination container", + number=number, + opening_width=opening_width, + opening_depth=opening_depth, + rim_z=self._destination_box.rim_z, + ) + + @skill + def place_in_destination( + self, robot_name: str | None = None + ) -> SkillResult[ManipulationSkillError]: + """Place the verified held object above the selected box opening. + + Args: + robot_name: Robot to use (only needed for multi-arm setups). + """ + destination = self._destination_box + if destination is None: + return SkillResult.fail( + "INVALID_STATE", "Select a destination container before placing" + ) + if destination.snapshot_version != self._snapshot_version: + self._destination_box = None + return SkillResult.fail( + "INVALID_STATE", "Destination is stale; scan and select it again" + ) + held_size = self._held_object_size + if held_size is None: + return SkillResult.fail( + "INVALID_STATE", "No verified held object is available to place" + ) + if ( + held_size.x > destination.opening_width + or held_size.y > destination.opening_depth + ): + return SkillResult.fail( + "INVALID_INPUT", "Held object does not fit inside the destination opening" + ) + object_z = ( + destination.rim_z + + held_size.z / 2.0 + + float(self.config.drop_clearance) + ) + return self.place_at( + destination.center_x, + destination.center_y, + object_z, + robot_name, + ) diff --git a/dimos/manipulation/candidate_filter_spec.py b/dimos/manipulation/candidate_filter_spec.py deleted file mode 100644 index da6ca47c99..0000000000 --- a/dimos/manipulation/candidate_filter_spec.py +++ /dev/null @@ -1,32 +0,0 @@ -# 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. - -"""Planner contract used to reject unsafe learned grasp proposals.""" - -from typing import Protocol - -from dimos.manipulation.planning.spec.models import IKResult, RobotName -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.spec.utils import Spec - - -class GraspCandidateFilterSpec(Spec, Protocol): - def inverse_kinematics_single( - self, - pose: Pose, - robot_name: RobotName | None = None, - seed: JointState | None = None, - check_collision: bool = True, - ) -> IKResult: ... diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index f0431a74be..82f647d209 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 @@ -67,9 +67,12 @@ CartesianPathConfig, ManipulationPlannerConfig, ) -from dimos.manipulation.planning.planners.roboplan_config import RoboPlanPlannerConfig +from dimos.manipulation.planning.planners.roboplan_config import ( + RoboPlanCartesianPathConfig, + RoboPlanPlannerConfig, +) from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.manipulation.planning.spec.enums import IKStatus, ObstacleType +from dimos.manipulation.planning.spec.enums import IKStatus, ObstacleType, PlanningStatus from dimos.manipulation.planning.spec.models import ( DEFAULT_OBSTACLE_RGBA, CartesianTarget, @@ -106,6 +109,10 @@ logger = setup_logger() +_UNCHECKED_LINEAR_IK_STEP_SIZE = 0.005 +_UNCHECKED_ANGULAR_IK_STEP_SIZE = 0.02 +_UNCHECKED_MAX_ANGULAR_SPEED = 0.5 + # Composite type aliases for readability (using semantic IDs from planning.spec) RobotEntry: TypeAlias = tuple[WorldRobotID, RobotModelConfig, JointTrajectoryGenerator] """(world_robot_id, config, trajectory_generator)""" @@ -147,6 +154,14 @@ class ConnectedPoseSequenceResult: paths: tuple[tuple[JointState, ...], ...] +class StaticBoxObstacle(BaseModel): + """Axis-aligned world-frame box installed when planning starts.""" + + name: str + center: tuple[float, float, float] + size: tuple[float, float, float] + + class ManipulationModuleConfig(ModuleConfig): """Configuration for ManipulationModule.""" @@ -164,6 +179,7 @@ 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: list[StaticBoxObstacle] = Field(default_factory=list) class ManipulationModule(Module): @@ -287,6 +303,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()), + obstacle_type=ObstacleType.BOX, + dimensions=tuple(box.size), + ) + ) + logger.info("Static obstacle '%s' added at %s", box.name, box.center) + for _, (robot_id, _, _) in self._robots.items(): self._world_monitor.start_state_monitor(robot_id) @@ -1156,6 +1183,51 @@ def _check_connected_pose_sequence( result = self._plan_connected_pose_sequence(poses, robot_name, start) return result.failed_index, result.endpoint + def _check_pose_ik_sequence( + self, + poses: Sequence[Pose], + robot_name: RobotName, + start: JointState | None = None, + *, + check_collision: bool = True, + ) -> tuple[int | None, JointState | None]: + """Dry-run sequential pose IK and return the failure index and endpoint.""" + if not poses: + return None, start + if self._world_monitor is None or self._kinematics is None: + logger.warning("Pose IK sequence checking is unavailable") + return 0, None + try: + group_id = self._require_unique_pose_group_id_for_robot(robot_name) + selection = self._world_monitor.planning_groups.select((group_id,)) + if start is None: + current = self._world_monitor.current_global_joint_state() + start = filter_joint_state_to_selected_joints(current, selection.joint_names) + else: + start = filter_joint_state_to_selected_joints(start, selection.joint_names) + except (KeyError, ValueError) as exc: + logger.warning("Failed to initialize pose IK sequence checking: %s", exc) + return 0, None + + for index, pose in enumerate(poses): + target = PoseStamped( + frame_id="world", + position=pose.position, + orientation=pose.orientation, + ) + ik = self.inverse_kinematics( + pose_targets={group_id: target}, + seed=start, + check_collision=check_collision, + ) + if not ik.is_success() or ik.joint_state is None: + return index, None + try: + start = filter_joint_state_to_selected_joints(ik.joint_state, selection.joint_names) + except ValueError: + return index, None + return None, start + def _plan_connected_pose_sequence( self, poses: Sequence[Pose], @@ -1239,6 +1311,216 @@ def plan_cartesian_targets( """Plan TCP motion through absolute or relative Cartesian waypoints.""" return self.generate_cartesian_plan(targets, config, auxiliary_groups) is not None + @rpc + def plan_linear( + self, + target_pose: Pose, + robot_name: RobotName | None = None, + max_linear_speed: float = 0.03, + check_collision: bool = True, + ) -> bool: + """Plan a straight TCP path to an absolute world-frame pose.""" + if not math.isfinite(max_linear_speed) or max_linear_speed <= 0.0: + return self._record_error("Linear speed must be finite and positive") + robot = self._get_robot(robot_name) + if robot is None: + return self._record_error("Robot not found or robot_name is required") + selected_robot_name, _, _, _ = robot + try: + group_id = self._require_unique_pose_group_id_for_robot(selected_robot_name) + except ValueError as exc: + return self._record_error(str(exc)) + current_pose = self.get_ee_pose(selected_robot_name) + if current_pose is None: + return self._record_error("Current TCP pose is unavailable") + if not check_collision: + logger.warning("Planning linear TCP motion with collision checking disabled") + return self._plan_unchecked_linear( + current_pose, + target_pose, + group_id, + max_linear_speed, + ) + waypoints = ( + PoseStamped( + frame_id="world", + position=current_pose.position, + orientation=current_pose.orientation, + ), + PoseStamped( + frame_id="world", + position=target_pose.position, + orientation=target_pose.orientation, + ), + ) + config = RoboPlanCartesianPathConfig( + max_linear_speed=max_linear_speed, + check_collision=True, + ) + return self.plan_cartesian_targets({group_id: waypoints}, config) + + def _plan_unchecked_linear( + self, + start_pose: Pose, + target_pose: Pose, + group_id: PlanningGroupID, + max_linear_speed: float, + ) -> bool: + """Build a straight TCP path from sequential IK without collision queries.""" + if self._world_monitor is None or self._kinematics is None: + return self._record_error("Planning not initialized") + planning_epoch = self._begin_group_planning() + if planning_epoch is None: + return False + resolved = self._resolve_group_plan_start((group_id,), planning_epoch) + if resolved is None: + return False + selection, seed = resolved + + dx = target_pose.position.x - start_pose.position.x + dy = target_pose.position.y - start_pose.position.y + dz = target_pose.position.z - start_pose.position.z + distance = math.sqrt(dx * dx + dy * dy + dz * dz) + start_orientation = start_pose.orientation + target_orientation = target_pose.orientation + dot = sum( + start * target + for start, target in zip( + ( + start_orientation.x, + start_orientation.y, + start_orientation.z, + start_orientation.w, + ), + ( + target_orientation.x, + target_orientation.y, + target_orientation.z, + target_orientation.w, + ), + strict=True, + ) + ) + angular_distance = 2.0 * math.acos(min(1.0, abs(dot))) + sample_count = max( + 1, + math.ceil(distance / _UNCHECKED_LINEAR_IK_STEP_SIZE - 1e-9), + math.ceil(angular_distance / _UNCHECKED_ANGULAR_IK_STEP_SIZE - 1e-9), + ) + + path = [seed] + for sample_index in range(1, sample_count + 1): + fraction = sample_index / sample_count + orientation = self._interpolate_quaternion( + start_orientation, + target_orientation, + fraction, + ) + pose = Pose( + Vector3( + start_pose.position.x + fraction * dx, + start_pose.position.y + fraction * dy, + start_pose.position.z + fraction * dz, + ), + orientation, + ) + ik = self.inverse_kinematics( + { + group_id: PoseStamped( + frame_id="world", + position=pose.position, + orientation=pose.orientation, + ) + }, + seed=seed, + check_collision=False, + ) + if not ik.is_success() or ik.joint_state is None: + detail = f": {ik.message}" if ik.message else "" + return self._fail_planning_epoch( + planning_epoch, + "Unchecked linear IK failed at sample " + f"{sample_index}/{sample_count}: {ik.status.name}{detail}", + ) + try: + seed = filter_joint_state_to_selected_joints( + ik.joint_state, + selection.joint_names, + ) + except ValueError as exc: + return self._fail_planning_epoch( + planning_epoch, + f"Unchecked linear IK returned an invalid joint state: {exc}", + ) + path.append(seed) + + result = PlanningResult( + status=PlanningStatus.SUCCESS, + path=path, + planning_time=0.0, + message="Unchecked sequential-IK Cartesian path found", + ) + plan = self._store_generated_plan((group_id,), result, planning_epoch) + if plan is None: + return False + minimum_duration = max( + distance / max_linear_speed, + angular_distance / _UNCHECKED_MAX_ANGULAR_SPEED, + ) + current_duration = plan.trajectory.duration + if current_duration > 0.0 and current_duration < minimum_duration: + scale = minimum_duration / current_duration + for point in plan.trajectory.points: + point.time_from_start *= scale + point.velocities = [velocity / scale for velocity in point.velocities] + return True + + @staticmethod + def _interpolate_quaternion( + start: Quaternion, + target: Quaternion, + fraction: float, + ) -> Quaternion: + """Return a normalized shortest-arc quaternion interpolation.""" + start_values = (start.x, start.y, start.z, start.w) + target_values = (target.x, target.y, target.z, target.w) + if sum(a * b for a, b in zip(start_values, target_values, strict=True)) < 0.0: + target_values = tuple(-value for value in target_values) + values = tuple( + a + fraction * (b - a) + for a, b in zip(start_values, target_values, strict=True) + ) + norm = math.sqrt(sum(value * value for value in values)) + if norm <= 1e-12: + return Quaternion(target.x, target.y, target.z, target.w) + return Quaternion(*(value / norm for value in values)) + + def _execute_linear_motion( + self, + target_pose: Pose, + robot_name: RobotName | None = None, + max_linear_speed: float = 0.03, + *, + check_collision: bool, + ) -> SkillResult[ManipulationSkillError]: + """Plan and execute a linear motion with an explicit collision policy.""" + robot = self._get_robot(robot_name) + if robot is None: + return SkillResult.fail("ROBOT_NOT_FOUND", "Robot not found") + selected_robot_name, _, _, _ = robot + if not self.plan_linear( + target_pose, + selected_robot_name, + max_linear_speed, + check_collision, + ): + detail = f": {self._error_message}" if self._error_message else "" + return SkillResult.fail( + "PLANNING_FAILED", + f"Linear Cartesian planning failed{detail}", + ) + return self._preview_execute_wait(selected_robot_name) + def generate_cartesian_plan( self, targets: Mapping[PlanningGroupID | PlanningGroup, CartesianTarget], diff --git a/dimos/manipulation/obstacle_world_spec.py b/dimos/manipulation/obstacle_world_spec.py deleted file mode 100644 index f0870ee7fb..0000000000 --- a/dimos/manipulation/obstacle_world_spec.py +++ /dev/null @@ -1,43 +0,0 @@ -# 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. - -"""Planner obstacle mutation protocol for scene-derived geometry.""" - -from typing import Literal, Protocol - -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.spec.utils import Spec - - -class ObstacleWorldSpec(Spec, Protocol): - def add_obstacle( - self, - name: str, - pose: Pose, - shape: Literal["box", "sphere", "cylinder", "mesh"], - dimensions: list[float] | None = None, - mesh_path: str | None = None, - ) -> str: ... - - def update_obstacle( - self, - name: str, - pose: Pose, - shape: Literal["box", "sphere", "cylinder", "mesh"], - dimensions: list[float] | None = None, - mesh_path: str | None = None, - color: list[float] | None = None, - ) -> bool: ... - - def remove_obstacle(self, obstacle_id: str) -> bool: ... diff --git a/dimos/manipulation/pick_and_place_module.py b/dimos/manipulation/pick_and_place_module.py index 6394a43217..1aa3152b47 100644 --- a/dimos/manipulation/pick_and_place_module.py +++ b/dimos/manipulation/pick_and_place_module.py @@ -78,7 +78,6 @@ class GraspVerificationConfig(BaseConfig): """Robot-specific gripper closure verification settings.""" - enabled: bool = False open_position: FiniteFloat = 0.85 closed_position: FiniteFloat = 0.0 held_threshold: FiniteFloat = 0.02 @@ -101,13 +100,17 @@ def _validate_threshold(self) -> GraspVerificationConfig: class PickAndPlaceModuleConfig(ManipulationModuleConfig): """Configuration for PickAndPlaceModule.""" - heuristic_grasp_fallback: bool = False planning_frame: str = "world" max_object_pointcloud_age: FiniteFloat = Field(default=10.0, gt=0.0) max_grasp_candidates_to_check: int = Field(default=5, gt=0) - grasp_pre_grasp_offset: FiniteFloat | None = Field(default=None, gt=0.0) - grasp_retreat_offset: FiniteFloat | None = Field(default=None, gt=0.0) + grasp_pre_grasp_offset: FiniteFloat = Field(default=0.25, gt=0.0) + grasp_retreat_offset: FiniteFloat = Field(default=0.10, gt=0.0) + grasp_retreat_lift_offset: FiniteFloat = Field(default=0.01, ge=0.0) grasp_approach_vector: tuple[FiniteFloat, FiniteFloat, FiniteFloat] = (0.0, 0.0, -1.0) + grasp_linear_speed: FiniteFloat = Field(default=0.03, gt=0.0) + preparation_timeout: FiniteFloat = Field(default=30.0, gt=0.0) + use_mesh_obstacles: bool = False + perception_obstacle_padding: FiniteFloat = Field(default=0.01, ge=0.0) grasp_verification: GraspVerificationConfig = Field(default_factory=GraspVerificationConfig) @model_validator(mode="after") @@ -159,13 +162,23 @@ class _GraspVerification: class _PickTransaction: object_id: str = "" object_name: str = "" - proposal_source: Literal["grasp_provider", "heuristic"] = "grasp_provider" + proposal_source: Literal["grasp_provider"] = "grasp_provider" phase: _PickPhase = _PickPhase.RESOLVE selected: _FeasibleGrasp | None = None rejections: Counter[str] = field(default_factory=Counter) gripper_closed: bool = False +@dataclass(frozen=True) +class _PreparedPick: + """Immutable object and proposal selection tied to one scan snapshot.""" + + snapshot_version: int + detection: DetObject + candidates: tuple[GraspCandidate, ...] + prepared_at: float + + class _PickPipelineError(RuntimeError): def __init__(self, code: ManipulationSkillError, message: str) -> None: super().__init__(message) @@ -198,6 +211,14 @@ def __init__(self, **kwargs: Any) -> None: # The live detection cache is volatile (labels change every frame), # so pick/place use this stable snapshot instead. self._detection_snapshot: list[DetObject] = [] + self._snapshot_version = 0 + self._prepared_pick: _PreparedPick | None = None + self._objects_condition = threading.Condition() + self._objects_version = 0 + self._latest_objects: tuple[DetObject, ...] = () + self._held_object_to_tcp: Pose | None = None + self._held_object_orientation: Quaternion | None = None + self._held_object_size: Vector3 | None = None self._pick_guard = threading.Lock() @rpc @@ -212,7 +233,10 @@ 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( + use_mesh_obstacles=self.config.use_mesh_obstacles, + obstacle_padding=float(self.config.perception_obstacle_padding), + ) logger.info("PickAndPlaceModule started") @@ -221,6 +245,10 @@ def _on_objects(self, objects: list[DetObject]) -> None: try: if self._world_monitor is not None: self._world_monitor.on_objects(objects) + with self._objects_condition: + self._latest_objects = tuple(objects) + self._objects_version += 1 + self._objects_condition.notify_all() except Exception as e: logger.error(f"Exception in _on_objects: {e}") @@ -234,11 +262,16 @@ def refresh_obstacles(self, min_duration: float = 0.0) -> list[dict[str, Any]]: return [] result = self._world_monitor.refresh_obstacles(min_duration) # Snapshot detections at refresh time — the live cache is volatile - self._detection_snapshot = self._world_monitor.get_cached_objects() + self._replace_detection_snapshot(self._world_monitor.get_cached_objects()) logger.info(f"Detection snapshot: {[d.name for d in self._detection_snapshot]}") return result - @skill + def _replace_detection_snapshot(self, objects: list[DetObject]) -> None: + """Atomically replace numbered detections and invalidate prior selection.""" + self._detection_snapshot = list(objects) + self._snapshot_version += 1 + self._prepared_pick = None + def clear_perception_obstacles(self) -> SkillResult[ManipulationSkillError]: """Clear all perception obstacles from the planning world. @@ -462,7 +495,6 @@ def _resolve_object_position(self, object_name: str) -> tuple[float, float, floa return None return det.center.x, det.center.y, det.center.z - @skill def get_scene_info(self, robot_name: str | None = None) -> SkillResult[ManipulationSkillError]: """Get current robot state, detected objects, and scene information. @@ -521,7 +553,6 @@ def get_scene_info(self, robot_name: str | None = None) -> SkillResult[Manipulat return SkillResult.ok("\n".join(lines)) - @skill def look(self, robot_name: str | None = None) -> SkillResult[ManipulationSkillError]: """Quick check of what objects are visible from the current camera position. @@ -551,42 +582,61 @@ def look(self, robot_name: str | None = None) -> SkillResult[ManipulationSkillEr @skill def scan_objects( self, - min_duration: float = 0.0, - robot_name: str | None = None, + object_names: list[str], ) -> SkillResult[ManipulationSkillError]: - """Scan for objects — moves to init position first for a clear camera view, \ -then refreshes perception obstacles. - - Use this before pick/place operations or after a failed attempt. + """Scan one RGB-D frame for named objects and create a numbered snapshot. Args: - min_duration: Minimum time an object must be seen to be included. - robot_name: Robot context (only needed for multi-arm setups). + object_names: Simple object names to detect, one noun phrase per item. """ - # Go to init for a clear camera view - init_result = self.go_init(robot_name) - if not init_result.is_success(): - return init_result - - obstacles = self.refresh_obstacles(min_duration) - + names = [name.strip() for name in object_names if name.strip()] + if not names: + return SkillResult.fail("INVALID_INPUT", "At least one object name is required") + if self._object_scene is None: + return SkillResult.fail("PERCEPTION_FAILED", "No object-scene provider is connected") + with self._objects_condition: + objects_version = self._objects_version + try: + self._object_scene.set_prompts(names) + self._object_scene.scan_scene() + except RuntimeError as exc: + return SkillResult.fail("PERCEPTION_FAILED", str(exc)) + with self._objects_condition: + received = self._objects_condition.wait_for( + lambda: self._objects_version > objects_version, + timeout=5.0, + ) + objects = list(self._latest_objects) + if not received: + return SkillResult.fail( + "PERCEPTION_FAILED", + "Timed out waiting for the detected-object snapshot", + ) + self._replace_detection_snapshot(objects) + obstacles = self._world_monitor.refresh_obstacles(0.0) if self._world_monitor else [] detections = self._detection_snapshot if not detections: - # See look(): an empty scan is a valid observation, not a failure. return SkillResult.ok("No objects detected in scene") lines = [f"Detected {len(detections)} object(s):"] - for det in detections: + numbered: list[dict[str, object]] = [] + for number, det in enumerate(detections, start=1): c = det.center lines.append( - f" - {det.name} [id={det.object_id[:8]}]: " - f"({c.x:.3f}, {c.y:.3f}, {c.z:.3f}) [{det.detections_count} views]" + f" {number}. {det.name}: ({c.x:.3f}, {c.y:.3f}, {c.z:.3f})" + ) + numbered.append( + { + "number": number, + "name": det.name, + "confidence": det.confidence, + } ) if obstacles: lines.append(f"\n{len(obstacles)} obstacle(s) added to planning world") - return SkillResult.ok("\n".join(lines)) + return SkillResult.ok("\n".join(lines), objects=numbered, queried_names=names) def _require_pick_object(self, object_name: str, object_id: str | None) -> DetObject: detection = self._find_object_in_detections(object_name, object_id) @@ -598,23 +648,12 @@ def _require_pick_object(self, object_name: str, object_id: str | None) -> DetOb f"No unique current detection matches {selector}; scan again and use an object ID", ) - def _provider_candidates( - self, detection: DetObject, transaction: _PickTransaction - ) -> list[GraspCandidate]: + def _provider_candidates(self, detection: DetObject) -> list[GraspCandidate]: if self._grasp_generator is None: - if not self.config.heuristic_grasp_fallback: - raise _PickPipelineError( - "GRASP_PROVIDER_UNAVAILABLE", - "No grasp proposal provider is connected and heuristic fallback is disabled", - ) - transaction.proposal_source = "heuristic" - poses = self._generate_grasps_for_pick(detection.name, detection.object_id) - if not poses: - raise _PickPipelineError( - "GRASP_GENERATION_FAILED", - f"Heuristic grasp generation failed for '{detection.name}'", - ) - return [GraspCandidate(pose=pose, score=0.0) for pose in poses] + raise _PickPipelineError( + "GRASP_PROVIDER_UNAVAILABLE", + "No grasp proposal provider is connected", + ) if self._object_scene is None: raise _PickPipelineError( @@ -668,6 +707,114 @@ def _provider_candidates( ) return sorted(proposals.candidates, key=lambda candidate: candidate.score, reverse=True) + @skill + def select_object(self, number: int) -> SkillResult[ManipulationSkillError]: + """Prepare grasp proposals for one object in the latest numbered snapshot. + + Args: + number: One-based object number returned by the latest scan_objects call. + """ + if number < 1 or number > len(self._detection_snapshot): + return SkillResult.fail("INVALID_INPUT", f"No detected object numbered {number}") + detection = self._detection_snapshot[number - 1] + if detection.frame_id != "world": + return SkillResult.fail( + "GRASP_FRAME_MISMATCH", + f"Object frame '{detection.frame_id}' does not match required frame 'world'", + ) + try: + candidates = self._provider_candidates(detection) + except _PickPipelineError as exc: + return SkillResult.fail(exc.code, str(exc)) + self._prepared_pick = _PreparedPick( + snapshot_version=self._snapshot_version, + detection=detection, + candidates=tuple(candidates), + prepared_at=time.monotonic(), + ) + return SkillResult.ok( + f"Selected {number}. {detection.name}; prepared {len(candidates)} grasp candidate(s)", + number=number, + object_id=detection.object_id, + candidate_count=len(candidates), + ) + + @skill + def pick_selected( + self, robot_name: str | None = None + ) -> SkillResult[ManipulationSkillError]: + """Pick the object prepared by select_object using fresh feasibility checks. + + Args: + robot_name: Robot to use (only needed for multi-arm setups). + """ + prepared = self._prepared_pick + if prepared is None: + return SkillResult.fail("INVALID_STATE", "Select an object before starting a pick") + if prepared.snapshot_version != self._snapshot_version: + self._prepared_pick = None + return SkillResult.fail("INVALID_STATE", "Selection is stale; scan and select again") + if time.monotonic() - prepared.prepared_at > self.config.preparation_timeout: + self._prepared_pick = None + return SkillResult.fail("INVALID_STATE", "Selection timed out; select the object again") + if prepared.detection.frame_id != "world": + self._prepared_pick = None + return SkillResult.fail("GRASP_FRAME_MISMATCH", "Selected object is not in world frame") + if not self._pick_guard.acquire(blocking=False): + return SkillResult.fail("PICK_BUSY", "Another pick transaction is active") + + transaction = _PickTransaction( + object_id=prepared.detection.object_id, + object_name=prepared.detection.name, + phase=_PickPhase.SELECT, + ) + try: + robot = self._get_robot(robot_name) + if robot is None: + return SkillResult.fail("ROBOT_NOT_FOUND", "Robot not found") + rname, _, _, _ = robot + sequence_start = None + lift_pose = self._safety_lift_pose(rname) + if lift_pose is not None: + transaction.phase = _PickPhase.PREPARE + failed_index, sequence_start = self._check_connected_pose_sequence( + (lift_pose,), rname + ) + if failed_index is not None: + raise _PickPipelineError( + "PLANNING_FAILED", "Required safety-lift planning failed" + ) + transaction.phase = _PickPhase.SELECT + transaction.selected = self._select_feasible_grasp( + list(prepared.candidates), + rname, + transaction, + sequence_start, + ) + result = self._execute_selected_pick(transaction, rname) + if result.is_success(): + self._held_object_to_tcp = self._relative_pose( + prepared.detection.pose, + transaction.selected.candidate.pose, + ) + self._held_object_orientation = Quaternion(prepared.detection.pose.orientation) + self._held_object_size = Vector3(prepared.detection.size) + self._prepared_pick = None + return result + except _PickPipelineError as exc: + return self._phase_failure(transaction, exc.code, str(exc)) + finally: + self._pick_guard.release() + + @staticmethod + def _relative_pose(parent: Pose, child: Pose) -> Pose: + """Return child expressed in parent coordinates.""" + inverse_orientation = parent.orientation.inverse() + return Pose( + inverse_orientation.rotate_vector(child.position - parent.position), + inverse_orientation * child.orientation, + ) + @staticmethod def _valid_candidate(candidate: GraspCandidate) -> bool: pose = candidate.pose @@ -693,33 +840,43 @@ def _select_feasible_grasp( self, candidates: list[GraspCandidate], robot_name: str, - robot_pre_grasp_offset: float, transaction: _PickTransaction, sequence_start: JointState | None = None, ) -> _FeasibleGrasp: vector = Vector3(self.config.grasp_approach_vector) - pre_offset = self.config.grasp_pre_grasp_offset or robot_pre_grasp_offset - retreat_offset = self.config.grasp_retreat_offset or pre_offset limit = min(len(candidates), self.config.max_grasp_candidates_to_check) for rank, candidate in enumerate(candidates[:limit], start=1): if not self._valid_candidate(candidate): transaction.rejections[_CandidateRejection.INVALID.value] += 1 continue - pre_grasp = self._compute_pre_grasp_pose(candidate.pose, pre_offset, vector) - retreat = self._compute_pre_grasp_pose(candidate.pose, retreat_offset, vector) - rejections = ( - _CandidateRejection.PRE_GRASP_INFEASIBLE, - _CandidateRejection.GRASP_INFEASIBLE, - _CandidateRejection.RETREAT_INFEASIBLE, + pre_grasp = self._compute_pre_grasp_pose( + candidate.pose, + float(self.config.grasp_pre_grasp_offset), + vector, ) - failed_index, _ = self._check_connected_pose_sequence( - (pre_grasp, candidate.pose, retreat), + retreat = self._compute_retreat_pose(candidate.pose, vector) + failed_index, endpoint = self._check_connected_pose_sequence( + (pre_grasp,), robot_name, start=sequence_start, ) if failed_index is not None: - transaction.rejections[rejections[failed_index].value] += 1 + transaction.rejections[_CandidateRejection.PRE_GRASP_INFEASIBLE.value] += 1 + continue + failed_index, _ = self._check_pose_ik_sequence( + (candidate.pose, retreat), + robot_name, + start=endpoint, + check_collision=False, + ) + if failed_index is not None: + rejection = ( + _CandidateRejection.GRASP_INFEASIBLE + if failed_index == 0 + else _CandidateRejection.RETREAT_INFEASIBLE + ) + transaction.rejections[rejection.value] += 1 continue return _FeasibleGrasp(candidate, rank, pre_grasp, retreat) @@ -731,11 +888,24 @@ def _select_feasible_grasp( f"No feasible grasp among {limit} candidate(s)" + (f" ({summary})" if summary else ""), ) + def _compute_retreat_pose(self, grasp_pose: Pose, approach_vector: Vector3) -> Pose: + """Retract opposite the grasp approach with a small world-up bias.""" + retracted = self._compute_pre_grasp_pose( + grasp_pose, + float(self.config.grasp_retreat_offset), + approach_vector, + ) + return Pose( + Vector3( + retracted.position.x, + retracted.position.y, + retracted.position.z + float(self.config.grasp_retreat_lift_offset), + ), + retracted.orientation, + ) + def _verify_grasp(self, robot_name: str) -> _GraspVerification: verification = self.config.grasp_verification - if not verification.enabled: - return _GraspVerification(True, None, "gripper feedback verification disabled") - deadline = time.monotonic() + verification.timeout last_position: float | None = None while time.monotonic() < deadline: @@ -824,9 +994,12 @@ def _execute_selected_pick( ) 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) + execution = self._execute_linear_motion( + selected.candidate.pose, + robot_name, + float(self.config.grasp_linear_speed), + check_collision=False, + ) if not execution.is_success(): return self._phase_failure( transaction, execution.error_code or "EXECUTION_FAILED", execution.message @@ -843,9 +1016,12 @@ def _execute_selected_pick( 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) + execution = self._execute_linear_motion( + selected.retreat_pose, + robot_name, + float(self.config.grasp_linear_speed), + check_collision=False, + ) if not execution.is_success(): return self._phase_failure( transaction, execution.error_code or "EXECUTION_FAILED", execution.message @@ -864,7 +1040,6 @@ def _execute_selected_pick( rejections=dict(transaction.rejections), ) - @skill def pick( self, object_name: str, @@ -885,54 +1060,43 @@ def pick( return SkillResult.fail("PICK_BUSY", "Another pick transaction is active") transaction = _PickTransaction() - suppression = None - result: SkillResult[ManipulationSkillError] try: robot = self._get_robot(robot_name) if robot is None: return SkillResult.fail("ROBOT_NOT_FOUND", "Robot not found") - rname, _, robot_config, _ = robot + rname, _, _, _ = robot detection = self._require_pick_object(object_name, object_id) transaction.object_id = detection.object_id transaction.object_name = detection.name transaction.phase = _PickPhase.PROPOSE - candidates = self._provider_candidates(detection, transaction) + candidates = self._provider_candidates(detection) if self._world_monitor is None: raise _PickPipelineError( "WORLD_MONITOR_UNAVAILABLE", "Planning world monitor is unavailable" ) - with self._world_monitor.suppress_object_obstacle(detection.object_id) as suppression: - sequence_start = None - lift_pose = self._safety_lift_pose(rname) - if lift_pose is not None: - transaction.phase = _PickPhase.PREPARE - failed_index, sequence_start = self._check_connected_pose_sequence( - (lift_pose,), rname - ) - if failed_index is not None: - raise _PickPipelineError( - "PLANNING_FAILED", - "Required safety-lift planning failed", - ) - transaction.phase = _PickPhase.SELECT - transaction.selected = self._select_feasible_grasp( - candidates, - rname, - robot_config.pre_grasp_offset, - transaction, - sequence_start, + sequence_start = None + lift_pose = self._safety_lift_pose(rname) + if lift_pose is not None: + transaction.phase = _PickPhase.PREPARE + failed_index, sequence_start = self._check_connected_pose_sequence( + (lift_pose,), rname ) - result = self._execute_selected_pick(transaction, rname) - if suppression.cleanup_error is not None: - if result.is_success(): - return self._phase_failure( - transaction, "WORLD_MONITOR_UNAVAILABLE", suppression.cleanup_error + if failed_index is not None: + raise _PickPipelineError( + "PLANNING_FAILED", + "Required safety-lift planning failed", ) - result.message = f"{result.message}; cleanup: {suppression.cleanup_error}" - return result + transaction.phase = _PickPhase.SELECT + transaction.selected = self._select_feasible_grasp( + candidates, + rname, + transaction, + sequence_start, + ) + return self._execute_selected_pick(transaction, rname) except _PickPipelineError as exc: return self._phase_failure(transaction, exc.code, str(exc)) except RuntimeError as exc: @@ -941,6 +1105,40 @@ def pick( self._pick_guard.release() @skill + def place_at( + self, + x: float, + y: float, + z: float, + robot_name: str | None = None, + ) -> SkillResult[ManipulationSkillError]: + """Place the held object's reference point at a world-frame position. + + Args: + x: Held-object reference X position in world, in meters. + y: Held-object reference Y position in world, in meters. + z: Held-object reference Z position in world, in meters. + robot_name: Robot to use (only needed for multi-arm setups). + """ + object_to_tcp = self._held_object_to_tcp + object_orientation = self._held_object_orientation + if object_to_tcp is None or object_orientation is None: + return SkillResult.fail("INVALID_STATE", "No verified held object is available to place") + desired_object_pose = Pose(Vector3(x, y, z), object_orientation) + target_tcp = desired_object_pose + object_to_tcp + result = self._place_with_orientation( + target_tcp.position.x, + target_tcp.position.y, + target_tcp.position.z, + target_tcp.orientation, + robot_name, + ) + if result.is_success(): + self._held_object_to_tcp = None + self._held_object_orientation = None + self._held_object_size = None + return result + def place( self, x: float, @@ -1001,10 +1199,13 @@ def _place_with_orientation( return exec_result # 2. Lower to place position - logger.info("Lowering to place position...") - if not self.plan_to_pose(place_pose, rname): - return SkillResult.fail("PLANNING_FAILED", "Place pose planning failed") - exec_result = self._preview_execute_wait(rname) + logger.info("Lowering to place position with collision checking disabled...") + exec_result = self._execute_linear_motion( + place_pose, + rname, + float(self.config.grasp_linear_speed), + check_collision=False, + ) if not exec_result.is_success(): return exec_result @@ -1014,16 +1215,18 @@ def _place_with_orientation( time.sleep(1.0) # 4. Retract - logger.info("Retracting...") - if not self.plan_to_pose(pre_place_pose, rname): - return SkillResult.fail("PLANNING_FAILED", "Retract planning failed") - exec_result = self._preview_execute_wait(rname) + logger.info("Retracting with collision checking disabled...") + exec_result = self._execute_linear_motion( + pre_place_pose, + rname, + float(self.config.grasp_linear_speed), + check_collision=False, + ) if not exec_result.is_success(): return exec_result return SkillResult.ok(f"Place complete — object released at ({x:.3f}, {y:.3f}, {z:.3f})") - @skill def place_back(self, robot_name: str | None = None) -> SkillResult[ManipulationSkillError]: """Place the held object back at its original pick position. @@ -1043,7 +1246,6 @@ def place_back(self, robot_name: str | None = None) -> SkillResult[ManipulationS logger.info(f"Placing back at original position ({p.x:.3f}, {p.y:.3f}, {p.z:.3f})...") return self._place_with_orientation(p.x, p.y, p.z, o, robot_name) - @skill def drop_on( self, target_object_name: str, @@ -1073,7 +1275,6 @@ def drop_on( ) return self.place(x, y, z, robot_name) - @skill def pick_and_place( self, object_name: str, diff --git a/dimos/manipulation/pick_execution_spec.py b/dimos/manipulation/pick_execution_spec.py deleted file mode 100644 index e09236f39d..0000000000 --- a/dimos/manipulation/pick_execution_spec.py +++ /dev/null @@ -1,42 +0,0 @@ -# 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. - -"""Manipulation operations used by the selected-object pick transaction.""" - -from typing import Protocol - -from dimos.agents.skill_result import SkillResult -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.spec.utils import Spec - - -class PickExecutionSpec(Spec, Protocol): - def open_gripper(self, robot_name: str | None = None) -> SkillResult: ... - - def close_gripper(self, robot_name: str | None = None) -> SkillResult: ... - - def get_gripper(self, robot_name: str | None = None) -> float | None: ... - - def get_ee_pose(self, robot_name: str | None = None) -> Pose | None: ... - - def move_to_pose( - self, - x: float, - y: float, - z: float, - roll: float | None = None, - pitch: float | None = None, - yaw: float | None = None, - robot_name: str | None = None, - ) -> SkillResult: ... diff --git a/dimos/manipulation/picknplace.py b/dimos/manipulation/picknplace.py deleted file mode 100644 index 47be85d316..0000000000 --- a/dimos/manipulation/picknplace.py +++ /dev/null @@ -1,879 +0,0 @@ -# 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. - -"""Request-driven perception interface for the pick-and-place workflow.""" - -import math -import threading -import time -from typing import Literal - -import numpy as np -from pydantic import AliasChoices, Field - -from dimos.agents.annotation import skill -from dimos.agents.capabilities import CAP_MOVEMENT, CAP_PERCEPTION -from dimos.agents.skill_result import SkillResult -from dimos.core.core import rpc -from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import In, Out -from dimos.manipulation.candidate_filter_spec import GraspCandidateFilterSpec -from dimos.manipulation.grasping.grasp_gen_spec import GraspGenSpec, HeuristicGraspSpec -from dimos.manipulation.obstacle_world_spec import ObstacleWorldSpec -from dimos.manipulation.pick_execution_spec import PickExecutionSpec -from dimos.manipulation.visualization.layers import ( - LineSetElement, - MeshElement, - PointCloudElement, - VisualizationLayer, -) -from dimos.manipulation.visualization_spec import ManipulationVisualizationSpec -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray -from dimos.perception.experimental.object import ( - Object as DetObject, - to_detection3d_array, -) -from dimos.perception.experimental.object_scene_registration_spec import ObjectSceneRegistrationSpec - - -def _estimate_table_surface(points: np.ndarray) -> dict[str, float] | None: - """Fit the dominant horizontal support plane and return a conservative footprint.""" - if points.ndim != 2 or points.shape[1] != 3 or len(points) < 30: - return None - import open3d as o3d # type: ignore[import-untyped] - - cloud = o3d.geometry.PointCloud() - cloud.points = o3d.utility.Vector3dVector(points) - plane, inliers = cloud.segment_plane(distance_threshold=0.01, ransac_n=3, num_iterations=1000) - normal = np.asarray(plane[:3], dtype=np.float64) - normal /= np.linalg.norm(normal) - if abs(normal[2]) < 0.98 or len(inliers) < 30: - return None - surface = points[np.asarray(inliers)] - x_low, y_low = np.quantile(surface[:, :2], 0.02, axis=0) - x_high, y_high = np.quantile(surface[:, :2], 0.98, axis=0) - # Extend the observed tabletop patch so collision protection includes its edges. - margin = 0.10 - return { - "center_x": float((x_low + x_high) / 2), - "center_y": float((y_low + y_high) / 2), - "tabletop_z": float(np.median(surface[:, 2])), - "width": float(max(x_high - x_low + 2 * margin, 0.20)), - "depth": float(max(y_high - y_low + 2 * margin, 0.20)), - "inlier_count": float(len(inliers)), - } - - -def _primitive_mesh( - shape: Literal["box", "sphere", "cylinder"], - center: Vector3, - dimensions: tuple[float, ...], - orientation: Quaternion, -) -> tuple[np.ndarray, np.ndarray]: - """Create a display mesh for one planner primitive.""" - if shape == "box": - x, y, z = (dimension / 2.0 for dimension in dimensions) - vertices = np.asarray( - [ - [-x, -y, -z], - [x, -y, -z], - [x, y, -z], - [-x, y, -z], - [-x, -y, z], - [x, -y, z], - [x, y, z], - [-x, y, z], - ] - ) - triangles = np.asarray( - [ - [0, 1, 2], - [0, 2, 3], - [4, 6, 5], - [4, 7, 6], - [0, 4, 5], - [0, 5, 1], - [1, 5, 6], - [1, 6, 2], - [2, 6, 7], - [2, 7, 3], - [3, 7, 4], - [3, 4, 0], - ] - ) - else: - segments = 16 - angles = np.linspace(0.0, 2.0 * math.pi, segments, endpoint=False) - radius = dimensions[0] - if shape == "cylinder": - half_height = dimensions[1] / 2.0 - vertices = np.vstack( - ( - np.column_stack( - (radius * np.cos(angles), radius * np.sin(angles), -half_height) - ), - np.column_stack( - (radius * np.cos(angles), radius * np.sin(angles), half_height) - ), - [[0.0, 0.0, -half_height], [0.0, 0.0, half_height]], - ) - ) - bottom_center, top_center = 2 * segments, 2 * segments + 1 - triangles = np.asarray( - [ - triangle - for index in range(segments) - for triangle in ( - [index, (index + 1) % segments, segments + index], - [ - (index + 1) % segments, - segments + (index + 1) % segments, - segments + index, - ], - [bottom_center, (index + 1) % segments, index], - [top_center, segments + index, segments + (index + 1) % segments], - ) - ] - ) - else: - rings = 8 - phi = np.linspace(0.0, math.pi, rings + 1) - vertices = np.asarray( - [ - [ - radius * math.sin(p) * math.cos(a), - radius * math.sin(p) * math.sin(a), - radius * math.cos(p), - ] - for p in phi - for a in angles - ] - ) - triangles = np.asarray( - [ - triangle - for ring in range(rings) - for index in range(segments) - for triangle in ( - [ - ring * segments + index, - ring * segments + (index + 1) % segments, - (ring + 1) * segments + index, - ], - [ - ring * segments + (index + 1) % segments, - (ring + 1) * segments + (index + 1) % segments, - (ring + 1) * segments + index, - ], - ) - ] - ) - transformed = vertices @ orientation.to_rotation_matrix().T - return transformed + np.asarray(center.as_tuple), triangles - - -class PickNPlaceConfig(ModuleConfig): - """Configuration for PickNPlaceModule.""" - - grasp: Literal["heuristic", "graspgenx"] = Field( - default="heuristic", validation_alias=AliasChoices("grasp", "grasp_strategy") - ) - pregrasp_offset: float = 0.10 - candidate_filter: Literal["off", "ik_collision"] = "ik_collision" - candidate_ranking: Literal["confidence", "ik_feasibility"] = "confidence" - candidate_ik_limit: int = Field(default=10, gt=0) - grasp_empty_closed_threshold: float = 0.01 - grasp_feedback_delay: float = 0.5 - - -class PickNPlaceModule(Module): - """Provide request-driven perception and target selection for pick and place.""" - - config: PickNPlaceConfig - _scene: ObjectSceneRegistrationSpec - _grasp_generator: GraspGenSpec | None - _heuristic_grasp_generator: HeuristicGraspSpec - _grasp_filter: GraspCandidateFilterSpec - _pick_execution: PickExecutionSpec - _obstacle_world: ObstacleWorldSpec - _visualization: ManipulationVisualizationSpec - objects: In[list[DetObject]] - grasp_candidates: Out[GraspCandidateArray] - - def __init__(self, **kwargs: object) -> None: - super().__init__(**kwargs) - self._objects_condition = threading.Condition() - self._latest_objects: tuple[DetObject, ...] = () - self._objects_version = 0 - self._goal_pose: PoseStamped | None = None - self._pre_grasp_pose: PoseStamped | None = None - self._grasp_candidates: GraspCandidateArray | None = None - self._selected_object: DetObject | None = None - self._held_object_size: Vector3 | None = None - self._tabletop_z: float | None = None - self._open_box: dict[str, float] | None = None - self._scene_geometry_ids: set[str] = set() - - @rpc - def start(self) -> None: - super().start() - self.objects.subscribe(self._on_objects) - - def _on_objects(self, objects: list[DetObject]) -> None: - with self._objects_condition: - self._latest_objects = tuple(objects) - self._objects_version += 1 - self._objects_condition.notify_all() - - @rpc - def scan_scene( - self, prompt: str | None = None, prompts: list[str] | None = None - ) -> Detection3DArray: - """Run one RGB-D detection pass, optionally targeting one or more text prompts.""" - if prompt is not None and prompts is not None: - raise ValueError("Specify either prompt or prompts, not both") - with self._objects_condition: - objects_version = self._objects_version - if prompts is not None: - self._scene.set_prompts(prompts) - elif prompt: - self._scene.set_prompts([prompt]) - detections = self._scene.scan_scene() - with self._objects_condition: - received_result = self._objects_condition.wait_for( - lambda: self._objects_version > objects_version, - timeout=5.0, - ) - objects = self._latest_objects - if received_result: - # Stream delivery crosses process boundaries and can lag the OSR RPC response. - # Return the same snapshot used by the object/grasp APIs, not the prior response. - return to_detection3d_array( - list(objects), - frame_id=objects[0].frame_id if objects else detections.frame_id, - ts=objects[0].ts if objects else detections.ts, - ) - return detections - - @skill(uses=[CAP_PERCEPTION]) - def scan(self, prompt: str) -> SkillResult: - """Detect a prompted object from one RGB-D frame without moving the robot. - - Returns numbered objects. Use a returned number with ``select_object`` to create a grasp target - or ``get_object_geometry`` to inspect a container target. - """ - if not prompt.strip(): - return SkillResult.fail("INVALID_INPUT", "A nonempty object prompt is required") - try: - detections = self.scan_scene(prompt) - except RuntimeError as exc: - return SkillResult.fail("PERCEPTION_FAILED", str(exc)) - self._publish_scene_objects() - return SkillResult.ok( - f"Detected {detections.detections_length} object(s)", objects=self.get_scene_info() - ) - - @skill(uses=[CAP_PERCEPTION]) - def scan_objects(self, object_names: list[str]) -> SkillResult: - """Detect instances of simple object names from one RGB-D frame. - - Pass one short noun phrase per item, for example ``["wooden block", "white box"]``. Each name is - an independent Moondream query and can return multiple instances. Do not pass instructions, - exclusions, counting requests, or full sentences as object names. - """ - names = [name.strip() for name in object_names if name.strip()] - if not names: - return SkillResult.fail("INVALID_INPUT", "At least one simple object name is required") - try: - detections = self.scan_scene(prompts=names) - except RuntimeError as exc: - return SkillResult.fail("PERCEPTION_FAILED", str(exc)) - self._publish_scene_objects() - return SkillResult.ok( - f"Detected {detections.detections_length} object(s)", - queried_names=names, - objects=self.get_scene_info(), - ) - - @rpc - def get_scene_info(self) -> list[dict[str, object]]: - """Return the number, name, and confidence for current detections.""" - with self._objects_condition: - objects = self._latest_objects - return [ - { - "number": number, - "name": obj.name, - "confidence": obj.confidence, - } - for number, obj in enumerate(objects, 1) - ] - - @skill - def describe_scene(self, question: str = "What objects are visible on the table?") -> str: - """Answer an open-ended question about the latest camera image without moving the robot. - - Requires ``osr.det=moondream`` and is descriptive only; use ``scan`` for numbered 3D objects. - """ - return self._scene.describe_scene(question) - - @skill - def get_object_geometry(self, number: int) -> dict[str, object] | None: - """Return a scanned object's center and OBB size without moving the robot. - - ``number`` must come from the latest ``scan`` result. ``center`` and ``size`` are ``[x, y, z]`` - lists in meters in the returned planning frame; use this to derive a container placement target. - """ - with self._objects_condition: - if number < 1 or number > len(self._latest_objects): - return None - obj = self._latest_objects[number - 1] - return { - "number": number, - "name": obj.name, - "frame_id": obj.frame_id, - "center": [obj.center.x, obj.center.y, obj.center.z], - "size": [obj.size.x, obj.size.y, obj.size.z], - } - - @skill - def install_object_obstacle( - self, number: int, shape: Literal["box", "sphere", "cylinder"] = "box" - ) -> SkillResult: - """Install one measured object as a planner obstacle and render the same primitive in Viser. - - ``number`` must come from the latest ``scan`` result. Choose ``box`` for rectangular objects, - ``cylinder`` for upright round objects, and ``sphere`` only for near-spherical objects. - """ - obj = self._object_for_number(number) - if obj is None: - return SkillResult.fail("INVALID_INPUT", f"No detected object numbered {number}") - orientation = self._upright_orientation(obj) - if shape == "box": - dimensions = (obj.size.x, obj.size.y, obj.size.z) - elif shape == "sphere": - dimensions = (max(obj.size.x, obj.size.y, obj.size.z) / 2.0,) - else: - dimensions = (max(obj.size.x, obj.size.y) / 2.0, obj.size.z) - name = f"scene-object-{number}" - center = Vector3(obj.center) - if not self._install_geometry(name, center, orientation, shape, dimensions): - return SkillResult.fail("EXECUTION_FAILED", f"Failed to install obstacle '{name}'") - return SkillResult.ok( - "Obstacle installed", - name=name, - shape=shape, - center=[center.x, center.y, center.z], - dimensions=list(dimensions), - ) - - @skill - def install_open_box(self, number: int, wall_thickness: float = 0.01) -> SkillResult: - """Measure an open rectangular box and render it as a display-only solid box in Viser. - - Call ``estimate_table`` first. The result describes the free opening for top-down placement, but - does not add box walls to the planning world. - """ - if wall_thickness <= 0.0: - return SkillResult.fail("INVALID_INPUT", "wall_thickness must be positive") - if self._tabletop_z is None: - return SkillResult.fail( - "INVALID_STATE", "Estimate the table before modeling an open box" - ) - obj = self._object_for_number(number) - if obj is None: - return SkillResult.fail("INVALID_INPUT", f"No detected object numbered {number}") - width, depth = obj.size.x, obj.size.y - if width <= 2.0 * wall_thickness or depth <= 2.0 * wall_thickness: - return SkillResult.fail( - "INVALID_INPUT", "Box opening is smaller than twice wall_thickness" - ) - points = obj.pointcloud.points_f32() - rim_z = ( - float(np.quantile(points[:, 2], 0.95)) if len(points) else obj.center.z + obj.size.z / 2 - ) - height = rim_z - self._tabletop_z - if height <= 0.0: - return SkillResult.fail( - "PERCEPTION_FAILED", "Box rim is not above the estimated tabletop" - ) - center = Vector3(obj.center.x, obj.center.y, self._tabletop_z + height / 2.0) - orientation = self._upright_orientation(obj) - - vertices, triangles = _primitive_mesh("box", center, (width, depth, height), orientation) - self._visualization.set_visualization_layer( - VisualizationLayer( - "picknplace/open-box", - "world", - ( - MeshElement( - "box-envelope", - vertices, - triangles, - color=np.asarray([230, 230, 230]), - opacity=0.25, - ), - ), - ) - ) - self._open_box = { - "center_x": center.x, - "center_y": center.y, - "tabletop_z": self._tabletop_z, - "rim_z": rim_z, - "opening_width": width - 2.0 * wall_thickness, - "opening_depth": depth - 2.0 * wall_thickness, - } - return SkillResult.ok( - "Open box measured and displayed", - center=[center.x, center.y], - rim_z=rim_z, - opening_width=width - 2.0 * wall_thickness, - opening_depth=depth - 2.0 * wall_thickness, - ) - - @skill - def clear_scene_geometry(self) -> SkillResult: - """Remove temporary scene obstacles and the display-only open-box marker.""" - removed = [ - geometry_id - for geometry_id in tuple(self._scene_geometry_ids) - if self._obstacle_world.remove_obstacle(geometry_id) - ] - self._scene_geometry_ids.difference_update(removed) - self._visualization.set_visualization_layer( - VisualizationLayer("picknplace/open-box", "world", ()) - ) - self._open_box = None - return SkillResult.ok("Temporary scene geometry cleared", removed=removed) - - @rpc - def get_goal_pose(self, number: int) -> PoseStamped | None: - """Select an object and return its downward-facing, floor-clamped grasp goal.""" - obj = self._object_for_number(number) - if obj is None: - return None - if self.config.grasp == "graspgenx": - if self._grasp_generator is None: - raise RuntimeError("GraspGenX is not configured for this pick-and-place blueprint") - candidates = self._grasp_generator.propose_grasps(obj.pointcloud) - else: - candidates = self._heuristic_grasp_generator.propose_grasps(obj.pointcloud) - candidates = self._prepare_candidates(candidates) - self._selected_object = obj - self._grasp_candidates = candidates - if not candidates.candidates: - self.grasp_candidates.publish(candidates) - return None - return self._select_grasp_candidate(0) - - @skill - def select_object(self, number: int) -> SkillResult: - """Select a scanned object and return grasp and pre-grasp targets without moving the robot. - - ``number`` must come from the latest ``scan`` result. Returned target values are XYZ in meters and - roll/pitch/yaw in radians. Move to ``pre_grasp`` first, then move to ``goal`` for gripper contact; - ``pre_grasp`` is 100 mm above the object and is not a grasp pose. - """ - goal = self.get_goal_pose(number) - if goal is None: - return SkillResult.fail("INVALID_INPUT", f"No selectable object numbered {number}") - pre_grasp = self.get_pre_grasp_pose() - if pre_grasp is None: - return SkillResult.fail("INVALID_STATE", "Selected object has no pre-grasp target") - - def pose_target(pose: PoseStamped) -> dict[str, float]: - euler = pose.orientation.to_euler() - return { - "x": pose.position.x, - "y": pose.position.y, - "z": pose.position.z, - "roll": euler.x, - "pitch": euler.y, - "yaw": euler.z, - } - - return SkillResult.ok( - "Object selected. Move to pre_grasp, then goal for gripper contact before closing the gripper.", - goal=pose_target(goal), - pre_grasp=pose_target(pre_grasp), - ) - - @skill(uses=[CAP_MOVEMENT]) - def pick_selected(self, robot_name: str | None = None) -> SkillResult: - """Pick the object most recently selected with ``select_object``. - - Executes the full pre-grasp, contact-grasp, close, feedback verification, and retreat sequence. - A gripper position at or below the empty-closed threshold means no object was picked up and returns - ``GRASP_VERIFICATION_FAILED``. Call ``select_object`` before this tool; do not manually recreate - the grasp sequence with individual motion and gripper tools. - """ - goal = self._goal_pose - pre_grasp = self._pre_grasp_pose - if goal is None or pre_grasp is None: - return SkillResult.fail("INVALID_STATE", "Select an object before starting a pick") - - def move(pose: PoseStamped) -> SkillResult: - euler = pose.orientation.to_euler() - return self._pick_execution.move_to_pose( - pose.position.x, - pose.position.y, - pose.position.z, - euler.x, - euler.y, - euler.z, - robot_name, - ) - - opened = self._pick_execution.open_gripper(robot_name) - if not opened.is_success(): - return opened - approach = move(pre_grasp) - if not approach.is_success(): - return approach - contact = move(goal) - if not contact.is_success(): - return contact - closed = self._pick_execution.close_gripper(robot_name) - if not closed.is_success(): - return closed - - time.sleep(self.config.grasp_feedback_delay) - gripper_position = self._pick_execution.get_gripper(robot_name) - if gripper_position is None: - return SkillResult.fail( - "GRIPPER_FAILED", "Cannot verify pickup: gripper feedback unavailable" - ) - if gripper_position <= self.config.grasp_empty_closed_threshold: - self._pick_execution.open_gripper(robot_name) - recovery = move(pre_grasp) - result = SkillResult.fail( - "GRASP_VERIFICATION_FAILED", - "Pickup failed: gripper reached the empty-closed position; rescan and select before retrying", - ) - result.metadata = { - "gripper_position": gripper_position, - "rescan_required": True, - "recovered_to_pre_grasp": recovery.is_success(), - } - return result - - retreat = move(pre_grasp) - if not retreat.is_success(): - return retreat - selected_object = self._selected_object - if selected_object is None: - return SkillResult.fail( - "INVALID_STATE", "Selected object details are unavailable after grasp" - ) - self._held_object_size = Vector3(selected_object.size) - return SkillResult.ok( - "Pick complete: grasp verified and object retreated from the table", - gripper_position=gripper_position, - ) - - @skill(uses=[CAP_MOVEMENT]) - def place_selected(self, robot_name: str | None = None) -> SkillResult: - """Drop the verified held object into the most recently measured open box. - - Call ``install_open_box`` for the destination and complete ``pick_selected`` first. This tool moves - above the remembered opening and releases above the rim. It first lifts the held object for transit, - then lowers only at the box center. It never lowers the end effector into the box. The box remains - display-only and does not add planner collisions. - """ - box = self._open_box - held_size = self._held_object_size - if box is None: - return SkillResult.fail( - "INVALID_STATE", "Measure the destination with install_open_box first" - ) - if held_size is None: - return SkillResult.fail( - "INVALID_STATE", "No verified held object is available to place" - ) - if held_size.x > box["opening_width"] or held_size.y > box["opening_depth"]: - return SkillResult.fail( - "INVALID_INPUT", "Held object does not fit inside the measured box opening" - ) - - def move(x: float, y: float, z: float) -> SkillResult: - return self._pick_execution.move_to_pose(x, y, z, robot_name=robot_name) - - # The held object's bottom remains above the rim throughout lateral travel. - drop_z = box["rim_z"] + held_size.z / 2.0 + 0.02 - transit_z = drop_z + 0.10 - current_pose = self._pick_execution.get_ee_pose(robot_name) - if current_pose is not None and current_pose.position.z < transit_z: - lift = move(current_pose.position.x, current_pose.position.y, transit_z) - if not lift.is_success(): - return lift - approach = move(box["center_x"], box["center_y"], transit_z) - if not approach.is_success(): - return approach - lower = move(box["center_x"], box["center_y"], drop_z) - if not lower.is_success(): - return lower - opened = self._pick_execution.open_gripper(robot_name) - if not opened.is_success(): - return opened - self._held_object_size = None - return SkillResult.ok( - "Drop complete: object released above the measured box opening", - drop_z=drop_z, - object_bottom_clearance=0.02, - ) - - @rpc - def select_grasp_candidate(self, rank: int) -> PoseStamped | None: - """Select one ranked grasp proposal as the goal and Rerun highlight.""" - return self._select_grasp_candidate(rank) - - def _select_grasp_candidate(self, rank: int) -> PoseStamped | None: - candidates = self._grasp_candidates - if candidates is None or rank < 0 or rank >= len(candidates.candidates): - return None - candidates.selected_index = rank - self.grasp_candidates.publish(candidates) - candidate = candidates.candidates[rank] - self._goal_pose = PoseStamped( - ts=candidates.header.timestamp, - frame_id=candidates.header.frame_id, - position=candidate.pose.position, - orientation=candidate.pose.orientation, - ) - self._pre_grasp_pose = None - return self._goal_pose - - def _prepare_candidates(self, candidates: GraspCandidateArray) -> GraspCandidateArray: - """Filter and rank provider proposals using independently selectable policies.""" - if self.config.candidate_filter == "off" and self.config.candidate_ranking == "confidence": - return GraspCandidateArray( - candidates.header, - sorted(candidates.candidates, key=lambda candidate: -candidate.score), - ) - - evaluated = [] - candidates_to_evaluate = candidates.candidates[: self.config.candidate_ik_limit] - for candidate in candidates_to_evaluate: - result = self._grasp_filter.inverse_kinematics_single( - candidate.pose, "arm", check_collision=True - ) - evaluated.append((candidate, result.is_success())) - - if self.config.candidate_filter == "ik_collision": - evaluated = [(candidate, feasible) for candidate, feasible in evaluated if feasible] - else: - evaluated.extend( - (candidate, False) - for candidate in candidates.candidates[len(candidates_to_evaluate) :] - ) - - if self.config.candidate_ranking == "ik_feasibility": - evaluated.sort(key=lambda item: (not item[1], -item[0].score)) - else: - evaluated.sort(key=lambda item: -item[0].score) - return GraspCandidateArray(candidates.header, [candidate for candidate, _ in evaluated]) - - @rpc - def get_pre_grasp_pose(self) -> PoseStamped | None: - """Return the selected goal offset 100 mm opposite its final approach direction.""" - if self._goal_pose is None: - return None - offset = self._goal_pose.orientation.rotate_vector( - # Both providers express local +Z as the final approach direction. - Vector3(0.0, 0.0, -self.config.pregrasp_offset) - ) - self._pre_grasp_pose = PoseStamped( - ts=self._goal_pose.ts, - frame_id=self._goal_pose.frame_id, - position=Vector3( - self._goal_pose.position.x + offset.x, - self._goal_pose.position.y + offset.y, - self._goal_pose.position.z + offset.z, - ), - orientation=self._goal_pose.orientation, - ) - self._publish_viser_selection() - return self._pre_grasp_pose - - def _publish_viser_selection(self) -> None: - """Show the selected object and TCP targets without mutating the planning scene.""" - obj = self._selected_object - goal = self._goal_pose - pre_grasp = self._pre_grasp_pose - if obj is None or goal is None or pre_grasp is None: - return - points = obj.pointcloud.points_f32() - if len(points) == 0: - return - cloud_colors = np.repeat(np.array([[255, 190, 70]], dtype=np.uint8), len(points), axis=0) - vertices: list[np.ndarray] = [] - edges: list[list[int]] = [] - colors: list[list[int]] = [] - for pose, color in ((goal, [255, 70, 70]), (pre_grasp, [70, 255, 120])): - start = len(vertices) - origin = np.asarray(pose.position.as_tuple, dtype=np.float32) - axes = pose.orientation.to_rotation_matrix().astype(np.float32) * 0.06 - vertices.extend((origin, origin + axes[:, 0], origin + axes[:, 1], origin + axes[:, 2])) - edges.extend(((start, start + 1), (start, start + 2), (start, start + 3))) - colors.extend((color, color, color)) - self._visualization.set_visualization_layer( - VisualizationLayer( - "picknplace/selection", - "world", - ( - PointCloudElement("object", points, cloud_colors, point_size=0.003), - LineSetElement( - "tcp-targets", - np.asarray(vertices), - np.asarray(edges), - np.asarray(colors), - ), - ), - ) - ) - - def _publish_scene_objects(self) -> None: - """Display the latest measured object envelopes without affecting planning.""" - visualization = getattr(self, "_visualization", None) - if visualization is None: - return - with self._objects_condition: - objects = tuple(self._latest_objects) - colors = ([255, 180, 70], [80, 180, 255], [130, 230, 130], [230, 150, 230]) - elements: list[MeshElement] = [] - for number, obj in enumerate(objects, 1): - dimensions = (obj.size.x, obj.size.y, obj.size.z) - if any(dimension <= 0.0 for dimension in dimensions): - continue - vertices, triangles = _primitive_mesh( - "box", Vector3(obj.center), dimensions, self._upright_orientation(obj) - ) - elements.append( - MeshElement( - f"object-{number}", - vertices, - triangles, - color=np.asarray(colors[(number - 1) % len(colors)]), - opacity=0.30, - ) - ) - visualization.set_visualization_layer( - VisualizationLayer("picknplace/scene-objects", "world", tuple(elements)) - ) - - @rpc - def get_grasp_candidates(self) -> GraspCandidateArray: - """Return proposals generated by the selected grasp provider.""" - return self._grasp_candidates or GraspCandidateArray() - - @rpc - def estimate_table_surface(self) -> dict[str, float] | None: - """Estimate a horizontal tabletop from the latest full RGB-D scene cloud.""" - scene = self._scene.get_full_scene_pointcloud(voxel_size=0.01) - if scene is None: - return None - estimate = _estimate_table_surface(scene.points_f32()) - if estimate is None: - return None - self._tabletop_z = estimate["tabletop_z"] - z = estimate["tabletop_z"] - half_width = estimate["width"] / 2 - half_depth = estimate["depth"] / 2 - x = estimate["center_x"] - y = estimate["center_y"] - vertices = np.asarray( - [ - [x - half_width, y - half_depth, z], - [x + half_width, y - half_depth, z], - [x + half_width, y + half_depth, z], - [x - half_width, y + half_depth, z], - ] - ) - self._visualization.set_visualization_layer( - VisualizationLayer( - "picknplace/table-estimate", - "world", - ( - MeshElement( - "tabletop-fill", - vertices, - np.asarray([[0, 1, 2], [0, 2, 3]]), - color=np.asarray([80, 180, 255]), - opacity=1.0, - ), - LineSetElement( - "tabletop", - vertices, - np.asarray([[0, 1], [1, 2], [2, 3], [3, 0]]), - colors=np.asarray([[80, 180, 255]] * 4), - line_width=2.0, - ), - ), - ) - ) - return estimate - - @skill(uses=[CAP_PERCEPTION]) - def estimate_table(self) -> SkillResult: - """Run a fresh RGB-D scan, then estimate the tabletop without moving the robot. - - Pass the returned ``center_x``, ``center_y``, ``tabletop_z``, ``width``, and ``depth`` directly to - ``set_table_collision`` before requesting motion near the table. Do not call this concurrently with - ``scan``; both tools exclusively use the perception pipeline. - """ - try: - self.scan_scene() - except RuntimeError as exc: - return SkillResult.fail("PERCEPTION_FAILED", str(exc)) - estimate = self.estimate_table_surface() - if estimate is None: - return SkillResult.fail( - "PERCEPTION_FAILED", "No horizontal tabletop estimate is available" - ) - return SkillResult.ok("Table estimated", **estimate) - - def _object_for_number(self, number: int) -> DetObject | None: - with self._objects_condition: - if number < 1 or number > len(self._latest_objects): - return None - return self._latest_objects[number - 1] - - @staticmethod - def _upright_orientation(obj: DetObject) -> Quaternion: - """Keep measured horizontal yaw while constraining scene primitives upright.""" - return Quaternion.from_euler(Vector3(0.0, 0.0, obj.pose.orientation.to_euler().z)) - - def _install_geometry( - self, - name: str, - center: Vector3, - orientation: Quaternion, - shape: Literal["box", "sphere", "cylinder"], - dimensions: tuple[float, ...], - ) -> bool: - pose = Pose(center, orientation) - if self._obstacle_world.update_obstacle(name, pose, shape, list(dimensions)): - self._scene_geometry_ids.add(name) - return True - obstacle_id = self._obstacle_world.add_obstacle(name, pose, shape, list(dimensions)) - if obstacle_id: - self._scene_geometry_ids.add(obstacle_id) - return True - return False diff --git a/dimos/manipulation/planning/README.md b/dimos/manipulation/planning/README.md index e8a1d84fbb..72611bf55c 100644 --- a/dimos/manipulation/planning/README.md +++ b/dimos/manipulation/planning/README.md @@ -140,7 +140,7 @@ accepted. |-----------|-------------| | `xarm7-planner-coordinator` | XArm 7-DOF with coordinator | | `dual-xarm6-planner-coordinator` | Dual XArm 6-DOF with mock coordinator hardware | -| `xarm-perception-sim` | XArm 7-DOF simulation perception stack | +| `xarm-grasp-sim-agent` | XArm7 MuJoCo pick/place with ground-truth object geometry | ## Directory Structure diff --git a/dimos/manipulation/planning/monitor/world_monitor.py b/dimos/manipulation/planning/monitor/world_monitor.py index dc1c616230..f98470ddb2 100644 --- a/dimos/manipulation/planning/monitor/world_monitor.py +++ b/dimos/manipulation/planning/monitor/world_monitor.py @@ -234,7 +234,11 @@ 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, + obstacle_padding: float = 0.0, + ) -> None: """Start monitoring obstacle updates.""" with self._lock: if self._obstacle_monitor is not None: @@ -243,6 +247,8 @@ def start_obstacle_monitor(self) -> None: self._obstacle_monitor = WorldObstacleMonitor( parent=self, + use_mesh_obstacles=use_mesh_obstacles, + obstacle_padding=obstacle_padding, ) self._obstacle_monitor.start() logger.info("Obstacle monitor started") diff --git a/dimos/manipulation/planning/monitor/world_obstacle_monitor.py b/dimos/manipulation/planning/monitor/world_obstacle_monitor.py index 6f870b171c..69dab6361a 100644 --- a/dimos/manipulation/planning/monitor/world_obstacle_monitor.py +++ b/dimos/manipulation/planning/monitor/world_obstacle_monitor.py @@ -40,6 +40,8 @@ Obstacle, ) from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -84,6 +86,7 @@ def __init__( parent: WorldMonitor, detection_timeout: float = 2.0, use_mesh_obstacles: bool = False, + obstacle_padding: float = 0.0, ) -> None: """Create a world obstacle monitor. @@ -96,6 +99,7 @@ def __init__( self._lock = parent._lock self._detection_timeout = detection_timeout self._use_mesh_obstacles = use_mesh_obstacles + self._obstacle_padding = obstacle_padding # Track obstacles from different sources self._collision_objects: dict[str, str] = {} # msg_id -> obstacle_id @@ -728,11 +732,24 @@ def _object_to_obstacle(self, obj: object) -> Obstacle: except Exception as e: logger.debug(f"Convex hull failed for {name}, falling back to box: {e}") - # Default: bounding box + # Real perception uses a conservative upright box. Preserve measured + # horizontal yaw while discarding noisy roll/pitch from partial views. + yaw = obj.pose.orientation.to_euler().z + pose = PoseStamped( + ts=obj.ts, + frame_id=obj.frame_id, + position=obj.center, + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)), + ) + padding = self._obstacle_padding * 2.0 return Obstacle( name=name, obstacle_type=ObstacleType.BOX, - pose=obj.pose or PoseStamped(position=obj.center), - dimensions=(float(obj.size.x), float(obj.size.y), float(obj.size.z)), + pose=pose, + dimensions=( + float(obj.size.x) + padding, + float(obj.size.y) + padding, + float(obj.size.z) + padding, + ), color=(0.2, 0.8, 0.2, 0.6), ) diff --git a/dimos/manipulation/planning/planners/roboplan_config.py b/dimos/manipulation/planning/planners/roboplan_config.py index fd145d09a2..d87cf0d223 100644 --- a/dimos/manipulation/planning/planners/roboplan_config.py +++ b/dimos/manipulation/planning/planners/roboplan_config.py @@ -47,6 +47,7 @@ class RoboPlanCartesianPathConfig(BaseConfig): toppra_blend_deviation: float = 0.05 position_limit_gain: float = Field(default=1.0, gt=0.0, le=1.0) max_attempts_per_step: int = Field(default=16, ge=1) + check_collision: bool = True class RoboPlanPathShortcuttingConfig(BaseConfig): diff --git a/dimos/manipulation/pnpconsole.py b/dimos/manipulation/pnpconsole.py deleted file mode 100644 index 9a13492016..0000000000 --- a/dimos/manipulation/pnpconsole.py +++ /dev/null @@ -1,314 +0,0 @@ -# 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. - -"""Interactive RPC client for the stepwise ``picknplace`` pipeline. - -Start the blueprint first, then run: - - uv run --no-sync python -m dimos.manipulation.pnpconsole -""" - -from __future__ import annotations - -from pprint import pprint -import time -from typing import Any - -from dimos import Dimos -from dimos.manipulation.planning.planners.roboplan_config import RoboPlanCartesianPathConfig -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - - -def _object_number() -> int | None: - value = input("Object number: ").strip() - try: - number = int(value) - except ValueError: - print("Enter a positive whole number.") - return None - if number < 1: - print("Enter a positive whole number.") - return None - return number - - -def _grasp_rank(candidate_count: int) -> int | None: - value = input(f"Grasp rank [0-{candidate_count - 1}, Enter=0]: ").strip() - if not value: - return 0 - try: - rank = int(value) - except ValueError: - print("Enter a whole-number grasp rank.") - return None - if rank < 0 or rank >= candidate_count: - print(f"Choose a rank from 0 to {candidate_count - 1}.") - return None - return rank - - -def _print_pose(pose: Any) -> None: - if pose is None: - print("No pose is available.") - return - if frame_id := getattr(pose, "frame_id", None): - print(f"frame: {frame_id}") - print(f"position: {pose.position.as_tuple}") - print( - "orientation: " - f"({pose.orientation.x}, {pose.orientation.y}, {pose.orientation.z}, {pose.orientation.w})" - ) - - -def _cartesian_waypoints(manipulation: Any, target: Any) -> list[PoseStamped] | None: - current = manipulation.get_ee_pose("arm") - if current is None: - return None - return [ - PoseStamped(frame_id="world", position=current.position, orientation=current.orientation), - PoseStamped(frame_id="world", position=target.position, orientation=target.orientation), - ] - - -def _preview(manipulation: Any) -> None: - print(f"Viser preview: {manipulation.get_visualization_url()}") - print(manipulation.preview_plan(duration=2.0)) - - -def _print_grasp_candidates(candidates: Any) -> None: - if not candidates.candidates: - return - print(f"Grasp proposals: {len(candidates.candidates)}") - for rank, candidate in enumerate(candidates.candidates[:10]): - pose = candidate.pose - print( - f"{rank}: score={candidate.score:.3f} position={pose.position.as_tuple} " - f"orientation={pose.orientation.to_tuple()}" - ) - - -def main() -> None: - """Connect to PickNPlaceModule and run one explicit pick-pipeline stage.""" - print("Connecting to PickNPlaceModule...") - app = Dimos.connect() - pnp = app.pnp - manipulation = app.ManipulationModule - goal = None - pre_grasp = None - approach_planned = False - approach_executed = False - descent_planned = False - descent_executed = False - gripper_closed = False - ascent_planned = False - ascent_executed = False - print("Connected. Every planned motion is previewed in Viser before execution.") - - while True: - print("\n1) Scan 2) Info 3) Select target 4) Plan/preview approach") - print("5) Execute approach 6) Plan/preview descent 7) Execute descent 8) Close") - print("9) Plan/preview ascent 10) Execute ascent 11) Open 12) Current EE 13) Go home") - print("14) Scan/estimate/install table collision 15) Grasp + lift now (no preview)") - print("16) Describe current camera scene q) Quit") - choice = input("Select: ").strip().lower() - try: - if choice == "q": - return - if choice == "1": - prompt = input("Object prompt (blank = current detector prompt): ").strip() - detections = pnp.scan_scene(prompt or None) - print(f"Detected {detections.detections_length} object(s).") - elif choice == "2": - pprint(pnp.get_scene_info()) - elif choice == "3": - if (number := _object_number()) is not None: - goal = pnp.get_goal_pose(number) - candidates = pnp.get_grasp_candidates() - _print_grasp_candidates(candidates) - if candidates.candidates: - rank = _grasp_rank(min(10, len(candidates.candidates))) - if rank is None: - continue - goal = pnp.select_grasp_candidate(rank) - pre_grasp = pnp.get_pre_grasp_pose() - approach_planned = False - approach_executed = False - descent_planned = False - descent_executed = False - gripper_closed = False - ascent_planned = False - ascent_executed = False - print("Goal:") - _print_pose(goal) - print("Pre-grasp:") - _print_pose(pre_grasp) - elif choice == "4": - if pre_grasp is None: - print("Select a target first.") - else: - approach_planned = manipulation.plan_to_pose( - Pose(pre_grasp.position, pre_grasp.orientation), "arm" - ) - print(approach_planned) - if approach_planned: - _preview(manipulation) - elif choice == "5": - if not approach_planned: - print("Plan the approach first.") - else: - approach_executed = manipulation.execute_and_wait() - print(approach_executed) - elif choice == "6": - if goal is None or not approach_executed: - print("Execute the approach first.") - else: - waypoints = _cartesian_waypoints(manipulation, goal) - descent_planned = False - descent_planned = manipulation.plan_cartesian_targets( - {"arm/manipulator": waypoints}, - RoboPlanCartesianPathConfig(max_linear_speed=0.03), - ) - print(descent_planned) - if descent_planned: - _preview(manipulation) - elif choice == "7": - if not descent_planned: - print("Plan the descent first.") - else: - descent_executed = manipulation.execute_and_wait() - print(descent_executed) - elif choice == "8": - if not descent_executed: - print("Execute the descent first.") - else: - gripper_closed = manipulation.close_gripper("arm").is_success() - print(gripper_closed) - elif choice == "9": - if pre_grasp is None or not gripper_closed: - print("Close the gripper before planning ascent.") - else: - waypoints = _cartesian_waypoints(manipulation, pre_grasp) - ascent_planned = False - ascent_planned = manipulation.plan_cartesian_targets( - {"arm/manipulator": waypoints}, - RoboPlanCartesianPathConfig(max_linear_speed=0.03), - ) - print(ascent_planned) - if ascent_planned: - _preview(manipulation) - elif choice == "10": - if not ascent_planned: - print("Plan the ascent first.") - else: - # Gripper commands are asynchronous on xArm. Reassert close before - # lift and let that command settle before dispatching the trajectory. - gripper_closed = manipulation.close_gripper("arm").is_success() - if not gripper_closed: - print("Failed to keep the gripper closed; ascent was not executed.") - else: - time.sleep(1.5) - ascent_executed = manipulation.execute_and_wait() - print(ascent_executed) - elif choice == "11": - if not ascent_executed: - print("Execute the ascent before opening the gripper.") - else: - print(manipulation.open_gripper("arm")) - elif choice == "12": - _print_pose(manipulation.get_ee_pose("arm")) - elif choice == "13": - print(manipulation.go_home("arm")) - elif choice == "14": - # A fresh RGB-D snapshot is required before fitting the table plane. - pnp.scan_scene() - estimate = pnp.estimate_table_surface() - if estimate is None: - print("No horizontal tabletop estimate. Scan the scene and try again.") - else: - print( - "Table estimate: " - f"z={estimate['tabletop_z']:.3f} m, center=({estimate['center_x']:.3f}, " - f"{estimate['center_y']:.3f}) m, size=({estimate['width']:.3f}, " - f"{estimate['depth']:.3f}) m" - ) - clearance_text = input( - "Table clearance in mm [10 recommended, 0 = no clearance]: " - ).strip() - try: - clearance_mm = 10.0 if not clearance_text else float(clearance_text) - except ValueError: - print("Enter a non-negative clearance in millimeters.") - continue - if clearance_mm < 0.0: - print("Enter a non-negative clearance in millimeters.") - continue - print( - manipulation.set_table_collision( - estimate["center_x"], - estimate["center_y"], - estimate["tabletop_z"], - estimate["width"], - estimate["depth"], - safety_margin=clearance_mm / 1000.0, - ) - ) - elif choice == "15": - if goal is None or pre_grasp is None or not approach_executed: - print("Execute the approach first.") - continue - print("Executing descent, gripper close, and ascent without previews.") - descent_planned = False - if (waypoints := _cartesian_waypoints(manipulation, goal)) is not None: - descent_planned = manipulation.plan_cartesian_targets( - {"arm/manipulator": waypoints}, - RoboPlanCartesianPathConfig(max_linear_speed=0.03), - ) - if not descent_planned: - print("Could not plan the descent; grasp sequence stopped.") - continue - descent_executed = manipulation.execute_and_wait() - if not descent_executed: - print("Descent failed; grasp sequence stopped.") - continue - gripper_closed = manipulation.close_gripper("arm").is_success() - if not gripper_closed: - print("Failed to close the gripper; grasp sequence stopped.") - continue - # Gripper commands are asynchronous; wait before lifting the object. - time.sleep(1.5) - ascent_planned = False - if (waypoints := _cartesian_waypoints(manipulation, pre_grasp)) is not None: - ascent_planned = manipulation.plan_cartesian_targets( - {"arm/manipulator": waypoints}, - RoboPlanCartesianPathConfig(max_linear_speed=0.03), - ) - if not ascent_planned: - print("Could not plan the ascent; grasp sequence stopped with gripper closed.") - continue - ascent_executed = manipulation.execute_and_wait() - print(ascent_executed) - elif choice == "16": - question = input( - "Scene question [What objects are visible on the table?]: " - ).strip() - print(pnp.describe_scene(question or "What objects are visible on the table?")) - else: - print("Choose 1-16 or q.") - except Exception as exc: - print(f"RPC failed: {exc}") - - -if __name__ == "__main__": - main() diff --git a/dimos/manipulation/skill_errors.py b/dimos/manipulation/skill_errors.py index c980149c78..2c4b2acb89 100644 --- a/dimos/manipulation/skill_errors.py +++ b/dimos/manipulation/skill_errors.py @@ -30,6 +30,7 @@ ManipulationError = Literal[ "NO_PRIOR_POSE", "OBJECT_NOT_DETECTED", + "PERCEPTION_FAILED", "IK_FAILED", "PLANNING_FAILED", "COLLISION_AT_START", diff --git a/dimos/manipulation/test_pick_and_place_unit.py b/dimos/manipulation/test_pick_and_place_unit.py index 1dd84ae38c..703f6fc2b4 100644 --- a/dimos/manipulation/test_pick_and_place_unit.py +++ b/dimos/manipulation/test_pick_and_place_unit.py @@ -17,7 +17,6 @@ from __future__ import annotations from collections import Counter -from contextlib import nullcontext import json from types import SimpleNamespace from unittest.mock import patch @@ -31,6 +30,10 @@ from dimos.core.coordination.blueprints import BlueprintAtom, autoconnect from dimos.core.coordination.module_coordinator import _resolve_single_ref from dimos.core.module import ModuleBase +from dimos.manipulation.box_filling_pick_and_place_module import ( + BoxFillingPickAndPlaceModule, + BoxFillingPickAndPlaceModuleConfig, +) from dimos.manipulation.grasping.grasp_gen_x import GraspGenXModule from dimos.manipulation.pick_and_place_module import ( GraspVerificationConfig, @@ -73,6 +76,7 @@ def _make_det_object( class_id=0, confidence=1.0, ts=0.0, + frame_id="world", image=Image(), ) @@ -193,6 +197,42 @@ def test_place_back_no_pick_pose_errors(self, module): assert result.error_code == "NO_PRIOR_POSE" assert "pick" in result.message.lower() + def test_place_contact_legs_use_unchecked_linear_motion( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + robot_config = SimpleNamespace(pre_grasp_offset=0.1) + mocker.patch.object( + module, + "_get_robot", + return_value=("arm", "robot-id", robot_config, None), + ) + mocker.patch.object(module, "_lift_if_low", return_value=SkillResult.ok()) + approach = mocker.patch.object(module, "plan_to_pose", return_value=True) + mocker.patch.object(module, "_preview_execute_wait", return_value=SkillResult.ok()) + linear = mocker.patch.object( + module, + "_execute_linear_motion", + return_value=SkillResult.ok(), + ) + mocker.patch.object(module, "_set_gripper_position", return_value=True) + mocker.patch("dimos.manipulation.pick_and_place_module.time.sleep") + place_pose = Pose(Vector3(0.5, 0.0, 0.2), Quaternion()) + + result = module._place_with_orientation( + place_pose.position.x, + place_pose.position.y, + place_pose.position.z, + place_pose.orientation, + ) + + assert result.is_success() + approach.assert_called_once() + pre_place_pose = approach.call_args.args[0] + assert linear.call_args_list == [ + mocker.call(place_pose, "arm", 0.03, check_collision=False), + mocker.call(pre_place_pose, "arm", 0.03, check_collision=False), + ] + def test_grasp_pipeline_error_agent_encoding_is_structured() -> None: result = SkillResult[ManipulationSkillError].fail("PICK_BUSY", "pick in progress") @@ -302,9 +342,7 @@ def test_provider_receives_real_world_frame_cloud( module._grasp_generator = generator mocker.patch("dimos.manipulation.pick_and_place_module.time.time", return_value=now + 0.1) - candidates = module._provider_candidates( - detection, SimpleNamespace(proposal_source="grasp_provider") - ) + candidates = module._provider_candidates(detection) generator.propose_grasps.assert_called_once_with(cloud) assert [(candidate.pose.position.x, candidate.score) for candidate in candidates] == [ @@ -327,9 +365,7 @@ def test_provider_rejects_missing_or_stale_cloud( mocker.patch("dimos.manipulation.pick_and_place_module.time.time", return_value=100.0) with pytest.raises(RuntimeError, match="point cloud"): - module._provider_candidates( - _make_det_object(), SimpleNamespace(proposal_source="grasp_provider") - ) + module._provider_candidates(_make_det_object()) @pytest.mark.parametrize( ("cloud_frame", "proposal_frame"), @@ -356,9 +392,7 @@ def test_provider_rejects_frame_mismatch( mocker.patch("dimos.manipulation.pick_and_place_module.time.time", return_value=now) with pytest.raises(RuntimeError, match="frame"): - module._provider_candidates( - _make_det_object(), SimpleNamespace(proposal_source="grasp_provider") - ) + module._provider_candidates(_make_det_object()) def test_provider_preserves_stable_order_for_equal_scores( self, module: PickAndPlaceModule, mocker: MockerFixture @@ -375,32 +409,13 @@ def test_provider_preserves_stable_order_for_equal_scores( module._grasp_generator = generator mocker.patch("dimos.manipulation.pick_and_place_module.time.time", return_value=now) - candidates = module._provider_candidates( - _make_det_object(), SimpleNamespace(proposal_source="grasp_provider") - ) + candidates = module._provider_candidates(_make_det_object()) assert [candidate.pose.position.x for candidate in candidates] == [0.2, 0.3, 0.1] - def test_explicit_heuristic_fallback_identifies_source( - self, module: PickAndPlaceModule, mocker: MockerFixture - ) -> None: - module.config.heuristic_grasp_fallback = True - transaction = SimpleNamespace(proposal_source="grasp_provider") - pose = Pose(0.4, 0.0, 0.2) - mocker.patch.object(module, "_generate_grasps_for_pick", return_value=[pose]) - - candidates = module._provider_candidates(_make_det_object(), transaction) - - assert transaction.proposal_source == "heuristic" - assert [(candidate.pose, candidate.score) for candidate in candidates] == [(pose, 0.0)] - - def test_provider_is_required_when_fallback_is_disabled( - self, module: PickAndPlaceModule - ) -> None: - with pytest.raises(RuntimeError, match="fallback is disabled"): - module._provider_candidates( - _make_det_object(), SimpleNamespace(proposal_source="grasp_provider") - ) + def test_provider_is_required(self, module: PickAndPlaceModule) -> None: + with pytest.raises(RuntimeError, match="No grasp proposal provider"): + module._provider_candidates(_make_det_object()) def test_selection_skips_higher_scored_infeasible_candidate( self, module: PickAndPlaceModule, mocker: MockerFixture @@ -411,20 +426,30 @@ def test_selection_skips_higher_scored_infeasible_candidate( "_check_connected_pose_sequence", side_effect=[(0, None), (None, endpoint)], ) + ik_sequence = mocker.patch.object( + module, + "_check_pose_ik_sequence", + return_value=(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)], "arm", - 0.1, transaction, ) assert selected.rank == 2 assert selected.candidate.score == 0.8 assert plan_sequence.call_count == 2 + ik_sequence.assert_called_once_with( + (selected.candidate.pose, selected.retreat_pose), + "arm", + start=endpoint, + check_collision=False, + ) assert transaction.rejections == {"pre_grasp_infeasible": 1} plan_motion.assert_not_called() command_gripper.assert_not_called() @@ -444,17 +469,22 @@ def test_selection_reports_failed_connected_segment( failed_index: int, expected_rejection: str, ) -> None: - mocker.patch.object( - module, - "_check_connected_pose_sequence", - return_value=(failed_index, None), - ) + pre_grasp = mocker.patch.object(module, "_check_connected_pose_sequence") + contact = mocker.patch.object(module, "_check_pose_ik_sequence") + if failed_index == 0: + pre_grasp.return_value = (0, None) + else: + endpoint = JointState(name=["arm/joint1"], position=[0.1]) + pre_grasp.return_value = (None, endpoint) + contact.return_value = (failed_index - 1, None) transaction = SimpleNamespace(rejections=Counter()) with pytest.raises(RuntimeError, match="No feasible grasp among 1"): - module._select_feasible_grasp([_candidate(0.4, 0.9)], "arm", 0.1, transaction) + module._select_feasible_grasp([_candidate(0.4, 0.9)], "arm", transaction) assert transaction.rejections == {expected_rejection: 1} + if failed_index == 0: + contact.assert_not_called() def test_selection_rejects_malformed_candidate_and_honors_limit( self, module: PickAndPlaceModule, mocker: MockerFixture @@ -463,19 +493,50 @@ def test_selection_rejects_malformed_candidate_and_honors_limit( invalid = _candidate(0.4, 0.9) invalid.pose.orientation.w = 0.0 plan_sequence = mocker.patch.object(module, "_check_connected_pose_sequence") + ik_sequence = mocker.patch.object(module, "_check_pose_ik_sequence") transaction = SimpleNamespace(rejections=Counter()) with pytest.raises(RuntimeError, match="No feasible grasp among 1"): - module._select_feasible_grasp([invalid, _candidate(0.5, 0.8)], "arm", 0.1, transaction) + module._select_feasible_grasp([invalid, _candidate(0.5, 0.8)], "arm", transaction) plan_sequence.assert_not_called() + ik_sequence.assert_not_called() assert transaction.rejections == {"invalid": 1} + def test_pre_grasp_uses_fixed_configured_offset(self, module: PickAndPlaceModule) -> None: + module.config.grasp_pre_grasp_offset = 0.25 + grasp = Pose(Vector3(0.4, 0.0, 0.3), Quaternion()) + + result = module._compute_pre_grasp_pose( + grasp, + float(module.config.grasp_pre_grasp_offset), + Vector3(0.0, 0.0, -1.0), + ) + + assert result == Pose(Vector3(0.4, 0.0, 0.05), grasp.orientation) + + def test_retreat_moves_back_with_world_up_bias(self, module: PickAndPlaceModule) -> None: + module.config.grasp_retreat_offset = 0.10 + module.config.grasp_retreat_lift_offset = 0.01 + half_sqrt = 2**-0.5 + grasp = Pose( + Vector3(0.4, 0.0, 0.2), + Quaternion(0.0, half_sqrt, 0.0, half_sqrt), + ) + + retreat = module._compute_retreat_pose( + grasp, + Vector3(0.0, 0.0, -1.0), + ) + + assert retreat.position.x == pytest.approx(0.3) + assert retreat.position.y == pytest.approx(0.0) + assert retreat.position.z == pytest.approx(0.21) + assert retreat.orientation == grasp.orientation + class TestPickTransaction: - def _arrange_success( - self, module: PickAndPlaceModule, mocker: MockerFixture - ) -> tuple[GraspCandidate, SimpleNamespace]: + def _arrange_success(self, module: PickAndPlaceModule, mocker: MockerFixture) -> GraspCandidate: detection = _make_det_object() candidate = _candidate(0.4, 0.9) selected = _FeasibleGrasp(candidate, 1, Pose(0.4, 0.0, 0.3), Pose(0.4, 0.0, 0.3)) @@ -490,22 +551,20 @@ def _arrange_success( mocker.patch.object(module, "_lift_if_low", return_value=SkillResult.ok()) mocker.patch.object(module, "plan_to_pose", return_value=True) mocker.patch.object(module, "_preview_execute_wait", return_value=SkillResult.ok()) + mocker.patch.object(module, "_execute_linear_motion", return_value=SkillResult.ok()) mocker.patch.object(module, "_set_gripper_position", return_value=True) mocker.patch.object( module, "_verify_grasp", return_value=_GraspVerification(True, 0.1, "verified"), ) - suppression = SimpleNamespace(cleanup_error=None) - world = mocker.Mock() - world.suppress_object_obstacle.return_value = nullcontext(suppression) - module._world_monitor = world - return candidate, suppression + module._world_monitor = mocker.Mock() + return candidate def test_success_executes_ordered_pick_and_records_metadata( self, module: PickAndPlaceModule, mocker: MockerFixture ) -> None: - candidate, _ = self._arrange_success(module, mocker) + candidate = self._arrange_success(module, mocker) result = module.pick("cup", object_id="abc12345") @@ -519,9 +578,12 @@ def test_success_executes_ordered_pick_and_records_metadata( ] assert module.plan_to_pose.call_args_list == [ mocker.call(Pose(0.4, 0.0, 0.3), "arm"), - mocker.call(candidate.pose, "arm"), - mocker.call(Pose(0.4, 0.0, 0.3), "arm"), ] + assert module._execute_linear_motion.call_args_list == [ + mocker.call(candidate.pose, "arm", 0.03, check_collision=False), + mocker.call(Pose(0.4, 0.0, 0.3), "arm", 0.03, check_collision=False), + ] + assert module._world_monitor.method_calls == [] def test_no_safety_lift_validates_candidates_from_current_state( self, module: PickAndPlaceModule, mocker: MockerFixture @@ -533,7 +595,7 @@ def test_no_safety_lift_validates_candidates_from_current_state( assert result.is_success() check_sequence.assert_not_called() - assert module._select_feasible_grasp.call_args.args[4] is None + assert module._select_feasible_grasp.call_args.args[3] is None def test_safety_lift_endpoint_is_shared_with_candidate_validation( self, module: PickAndPlaceModule, mocker: MockerFixture @@ -552,7 +614,7 @@ def test_safety_lift_endpoint_is_shared_with_candidate_validation( assert result.is_success() check_sequence.assert_called_once_with((lift_pose,), "arm") - assert module._select_feasible_grasp.call_args.args[4] is lift_endpoint + assert module._select_feasible_grasp.call_args.args[3] is lift_endpoint def test_safety_lift_planning_failure_aborts_prepare_without_candidate_rejection( self, module: PickAndPlaceModule, mocker: MockerFixture @@ -581,7 +643,10 @@ def test_retreat_failure_keeps_gripper_closed( self, module: PickAndPlaceModule, mocker: MockerFixture ) -> None: self._arrange_success(module, mocker) - module.plan_to_pose.side_effect = [True, True, False] + module._execute_linear_motion.side_effect = [ + SkillResult.ok(), + SkillResult.fail("PLANNING_FAILED", "linear planning failed"), + ] result = module.pick("cup", object_id="abc12345") @@ -598,7 +663,6 @@ def test_concurrent_pick_is_rejected_without_robot_access( mocker: MockerFixture, ) -> None: get_robot = mocker.patch.object(module, "_get_robot") - log = mocker.patch("dimos.agents.annotation.logger.info") module._pick_guard.acquire() try: result = module.pick("cup") @@ -607,35 +671,6 @@ def test_concurrent_pick_is_rejected_without_robot_access( assert result.error_code == "PICK_BUSY" get_robot.assert_not_called() - log.assert_called_once() - assert log.call_args.args[:3] == ( - "SKILL %s result=%s duration_ms=%.1f", - "pick", - "PICK_BUSY", - ) - - def test_cleanup_failure_does_not_hide_primary_failure( - self, module: PickAndPlaceModule, mocker: MockerFixture - ) -> None: - _, suppression = self._arrange_success(module, mocker) - suppression.cleanup_error = "restore failed" - module.plan_to_pose.side_effect = [False] - - result = module.pick("cup", object_id="abc12345") - - assert result.error_code == "PLANNING_FAILED" - assert "cleanup: restore failed" in result.message - - def test_cleanup_failure_turns_success_into_scene_failure( - self, module: PickAndPlaceModule, mocker: MockerFixture - ) -> None: - _, suppression = self._arrange_success(module, mocker) - suppression.cleanup_error = "restore failed" - - result = module.pick("cup", object_id="abc12345") - - assert result.error_code == "WORLD_MONITOR_UNAVAILABLE" - assert "restore failed" in result.message @pytest.mark.parametrize( ("setup", "expected_code", "expected_phase"), @@ -644,12 +679,10 @@ def test_cleanup_failure_turns_success_into_scene_failure( ("open", "GRIPPER_FAILED", "PREPARE"), ("approach_planning", "PLANNING_FAILED", "APPROACH"), ("approach_execution", "EXECUTION_FAILED", "APPROACH"), - ("grasp_planning", "PLANNING_FAILED", "GRASP"), - ("grasp_execution", "EXECUTION_FAILED", "GRASP"), + ("grasp_motion", "PLANNING_FAILED", "GRASP"), ("close", "GRIPPER_FAILED", "CLOSE"), ("verification", "GRASP_VERIFICATION_FAILED", "VERIFY"), - ("retreat_planning", "PLANNING_FAILED", "RETREAT"), - ("retreat_execution", "EXECUTION_FAILED", "RETREAT"), + ("retreat_motion", "EXECUTION_FAILED", "RETREAT"), ], ) def test_phase_failures_stop_the_pipeline( @@ -671,22 +704,16 @@ def test_phase_failures_stop_the_pipeline( module._preview_execute_wait.side_effect = [ SkillResult.fail("EXECUTION_FAILED", "rejected") ] - elif setup == "grasp_planning": - module.plan_to_pose.side_effect = [True, False] - elif setup == "grasp_execution": - module._preview_execute_wait.side_effect = [ - SkillResult.ok(), - SkillResult.fail("EXECUTION_FAILED", "rejected"), + elif setup == "grasp_motion": + module._execute_linear_motion.side_effect = [ + SkillResult.fail("PLANNING_FAILED", "linear planning failed") ] elif setup == "close": module._set_gripper_position.side_effect = [True, False] elif setup == "verification": module._verify_grasp.return_value = _GraspVerification(False, 0.0, "empty close") - elif setup == "retreat_planning": - module.plan_to_pose.side_effect = [True, True, False] else: - module._preview_execute_wait.side_effect = [ - SkillResult.ok(), + module._execute_linear_motion.side_effect = [ SkillResult.ok(), SkillResult.fail("EXECUTION_FAILED", "rejected"), ] @@ -720,14 +747,23 @@ def test_full_pick_pipeline_uses_real_messages_and_fake_boundary_providers( "_check_connected_pose_sequence", side_effect=[(0, None), (None, JointState())], ) + ik_sequence = mocker.patch.object( + module, + "_check_pose_ik_sequence", + return_value=(None, JointState()), + ) mocker.patch.object(module, "_safety_lift_pose", return_value=None) mocker.patch.object(module, "_lift_if_low", return_value=SkillResult.ok()) + mocker.patch.object( + module, + "_verify_grasp", + return_value=_GraspVerification(True, 0.1, "verified"), + ) plan = mocker.patch.object(module, "plan_to_pose", return_value=True) execute = mocker.patch.object(module, "_preview_execute_wait", return_value=SkillResult.ok()) + linear = mocker.patch.object(module, "_execute_linear_motion", return_value=SkillResult.ok()) gripper = mocker.patch.object(module, "_set_gripper_position", return_value=True) - suppression = SimpleNamespace(cleanup_error=None) world = mocker.Mock() - world.suppress_object_obstacle.return_value = nullcontext(suppression) module._world_monitor = world mocker.patch("dimos.manipulation.pick_and_place_module.time.time", return_value=now) @@ -740,11 +776,12 @@ 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 world.method_calls == [] assert plan_sequence.call_count == 2 - assert plan.call_count == 3 - assert execute.call_count == 3 + assert ik_sequence.call_count == 1 + assert plan.call_count == 1 + assert execute.call_count == 1 + assert linear.call_count == 2 assert gripper.call_args_list == [mocker.call(0.85, "arm"), mocker.call(0.0, "arm")] @@ -753,7 +790,6 @@ def test_empty_close_fails_immediately( self, module: PickAndPlaceModule, mocker: MockerFixture ) -> None: module.config.grasp_verification = GraspVerificationConfig( - enabled=True, timeout=1.0, poll_interval=0.1, held_threshold=0.02, @@ -772,7 +808,6 @@ def test_held_position_succeeds_after_timeout( self, module: PickAndPlaceModule, mocker: MockerFixture ) -> None: module.config.grasp_verification = GraspVerificationConfig( - enabled=True, timeout=1.0, poll_interval=0.1, held_threshold=0.02, @@ -793,7 +828,6 @@ def test_no_gripper_motion_is_not_misclassified_as_a_grasp( self, module: PickAndPlaceModule, mocker: MockerFixture ) -> None: module.config.grasp_verification = GraspVerificationConfig( - enabled=True, timeout=1.0, poll_interval=0.1, held_threshold=0.02, @@ -813,7 +847,6 @@ def test_feedback_timeout_is_reported( self, module: PickAndPlaceModule, mocker: MockerFixture ) -> None: module.config.grasp_verification = GraspVerificationConfig( - enabled=True, timeout=1.0, poll_interval=0.1, held_threshold=0.02, @@ -828,3 +861,108 @@ def test_feedback_timeout_is_reported( result = module._verify_grasp("arm") assert result == _GraspVerification(False, None, "gripper feedback was unavailable") + + +class TestNumberedSelectionApi: + def test_select_object_pins_snapshot_and_provider_order( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + detection = _make_det_object() + candidates = [_candidate(0.4, 0.9), _candidate(0.5, 0.8)] + module._replace_detection_snapshot([detection]) + provider = mocker.patch.object(module, "_provider_candidates", return_value=candidates) + + result = module.select_object(1) + + assert result.is_success() + provider.assert_called_once_with(detection) + assert module._prepared_pick is not None + assert module._prepared_pick.snapshot_version == module._snapshot_version + assert module._prepared_pick.detection is detection + assert module._prepared_pick.candidates == tuple(candidates) + + def test_new_scan_invalidates_prepared_selection( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + module._replace_detection_snapshot([_make_det_object()]) + mocker.patch.object(module, "_provider_candidates", return_value=[_candidate(0.4, 0.9)]) + assert module.select_object(1).is_success() + + module._replace_detection_snapshot([_make_det_object(name="bottle")]) + + assert module._prepared_pick is None + + def test_pick_selected_rejects_expired_preparation_without_robot_access( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + module._replace_detection_snapshot([_make_det_object()]) + mocker.patch.object(module, "_provider_candidates", return_value=[_candidate(0.4, 0.9)]) + mocker.patch("dimos.manipulation.pick_and_place_module.time.monotonic", return_value=0.0) + assert module.select_object(1).is_success() + module.config.preparation_timeout = 1.0 + get_robot = mocker.patch.object(module, "_get_robot") + + with patch( + "dimos.manipulation.pick_and_place_module.time.monotonic", return_value=2.0 + ): + result = module.pick_selected() + + assert result.error_code == "INVALID_STATE" + get_robot.assert_not_called() + + def test_place_at_converts_object_reference_to_tcp_target( + self, module: PickAndPlaceModule, mocker: MockerFixture + ) -> None: + quarter_turn = Quaternion.from_euler(Vector3(0.0, 0.0, np.pi / 2.0)) + module._held_object_orientation = quarter_turn + module._held_object_to_tcp = Pose(Vector3(0.1, 0.0, 0.0), Quaternion()) + module._held_object_size = Vector3(0.05, 0.05, 0.1) + place = mocker.patch.object(module, "_place_with_orientation", return_value=SkillResult.ok()) + + result = module.place_at(0.5, 0.2, 0.3) + + assert result.is_success() + target = place.call_args.args + assert target[0] == pytest.approx(0.5) + assert target[1] == pytest.approx(0.3) + assert target[2] == pytest.approx(0.3) + assert target[3] == quarter_turn + assert module._held_object_to_tcp is None + + +@pytest.fixture +def box_module() -> BoxFillingPickAndPlaceModule: + with patch.object(ModuleBase, "__init__", lambda self, config_args: None): + result = BoxFillingPickAndPlaceModule() + result.config = BoxFillingPickAndPlaceModuleConfig() + return result + + +class TestBoxFillingPolicy: + def test_destination_policy_computes_fit_checked_object_target( + self, box_module: BoxFillingPickAndPlaceModule, mocker: MockerFixture + ) -> None: + box_module._replace_detection_snapshot( + [_make_det_object(name="box", center=(0.6, 0.1, 0.1), size=(0.3, 0.2, 0.2))] + ) + assert box_module.select_destination_container(1).is_success() + box_module._held_object_size = Vector3(0.05, 0.04, 0.10) + place = mocker.patch.object(box_module, "place_at", return_value=SkillResult.ok()) + + result = box_module.place_in_destination("arm") + + assert result.is_success() + place.assert_called_once_with(0.6, 0.1, pytest.approx(0.27), "arm") + + def test_destination_policy_rejects_object_that_does_not_fit( + self, box_module: BoxFillingPickAndPlaceModule + ) -> None: + box_module._replace_detection_snapshot( + [_make_det_object(name="box", size=(0.1, 0.1, 0.2))] + ) + assert box_module.select_destination_container(1).is_success() + box_module._held_object_size = Vector3(0.2, 0.04, 0.05) + + result = box_module.place_in_destination() + + assert result.error_code == "INVALID_INPUT" diff --git a/dimos/manipulation/test_picknplace.py b/dimos/manipulation/test_picknplace.py deleted file mode 100644 index ff72961ad9..0000000000 --- a/dimos/manipulation/test_picknplace.py +++ /dev/null @@ -1,478 +0,0 @@ -# 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. - -import math -from unittest.mock import MagicMock, patch - -import numpy as np -import pytest - -from dimos.core.coordination.blueprint_config.parser import BlueprintConfigParser -from dimos.core.module import ModuleBase -from dimos.manipulation.blueprints import _picknplace_xarm6_model, _xarm_graspgenx, picknplace -from dimos.manipulation.picknplace import ( - PickNPlaceConfig, - PickNPlaceModule, - _estimate_table_surface, -) -from dimos.manipulation.planning.spec.models import IKResult, IKStatus -from dimos.manipulation.visualization.layers import MeshElement -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.manipulation_msgs.GraspCandidate import GraspCandidate -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.std_msgs.Header import Header -from dimos.robot.manipulators.xarm.grasp_config import XARM_TCP_TO_GRASP_FRAME - - -def test_picknplace_scans_and_selects_target() -> None: - with patch.object(ModuleBase, "__init__", lambda self, config_args: None): - module = PickNPlaceModule() - module.config = PickNPlaceConfig() - module._visualization = MagicMock() - scene = MagicMock() - detections = MagicMock() - module._scene = scene - obj = MagicMock( - ts=1.0, - frame_id="link_base", - center=Vector3(0.1, 0.2, 0.04), - confidence=0.9, - ) - obj.name = "cup" - obj.size = Vector3(0.4, 0.1, 0.2) - obj.camera_transform = None - obj.image = None - obj.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) - module._heuristic_grasp_generator = MagicMock( - propose_grasps=MagicMock( - return_value=GraspCandidateArray( - Header(1.0, "link_base"), - [ - GraspCandidate( - Pose( - Vector3(0.1, 0.2, 0.1), - Quaternion.from_euler(Vector3(-math.pi, 0.0, 0.0)), - ), - score=1.0, - ) - ], - ) - ) - ) - module._grasp_filter = MagicMock( - inverse_kinematics_single=MagicMock(return_value=IKResult(IKStatus.SUCCESS)) - ) - module.grasp_candidates = MagicMock() - scene.scan_scene.side_effect = lambda: (module._on_objects([obj]), detections)[1] - - with patch("dimos.manipulation.picknplace.to_detection3d_array") as to_detection3d_array: - result = MagicMock() - to_detection3d_array.return_value = result - assert module.scan_scene() is result - to_detection3d_array.assert_called_once_with([obj], frame_id="link_base", ts=1.0) - - assert module.get_scene_info() == [{"number": 1, "name": "cup", "confidence": 0.9}] - goal = module.get_goal_pose(1) - assert goal is not None - assert goal.position == Vector3(0.1, 0.2, 0.100) - assert goal.orientation == Quaternion.from_euler(Vector3(-3.141592653589793, 0.0, 0.0)) - pre_grasp = module.get_pre_grasp_pose() - assert pre_grasp is not None - assert pre_grasp.position == Vector3(0.1, 0.2, 0.200) - - selected = module.select_object(1) - assert selected.is_success() - assert selected.metadata["goal"] == { - "x": 0.1, - "y": 0.2, - "z": 0.1, - "roll": -math.pi, - "pitch": 0.0, - "yaw": 0.0, - } - assert selected.metadata["pre_grasp"] == { - "x": 0.1, - "y": 0.2, - "z": 0.2, - "roll": -math.pi, - "pitch": 0.0, - "yaw": 0.0, - } - assert module._selected_object is obj - - module.scan_scene("water bottle") - scene.set_prompts.assert_called_once_with(["water bottle"]) - - assert module._heuristic_grasp_generator.propose_grasps.call_count == 2 - - -def test_scan_objects_uses_independent_simple_queries() -> None: - with patch.object(ModuleBase, "__init__", lambda self, config_args: None): - module = PickNPlaceModule() - module._visualization = MagicMock() - module.scan_scene = MagicMock(detections_length=3) - module.get_scene_info = MagicMock(return_value=[]) - module._publish_scene_objects = MagicMock() - - result = module.scan_objects([" wooden block ", "white box", " "]) - - assert result.is_success() - assert result.metadata["queried_names"] == ["wooden block", "white box"] - module.scan_scene.assert_called_once_with(prompts=["wooden block", "white box"]) - - -def test_pick_selected_verifies_gripper_did_not_fully_close() -> None: - with patch.object(ModuleBase, "__init__", lambda self, config_args: None): - module = PickNPlaceModule() - module.config = PickNPlaceConfig(grasp_feedback_delay=0.0) - module._goal_pose = PoseStamped( - position=Vector3(0.3, 0.1, 0.12), - orientation=Quaternion.from_euler(Vector3(-math.pi, 0.0, 0.0)), - ) - module._pre_grasp_pose = PoseStamped( - position=Vector3(0.3, 0.1, 0.22), - orientation=Quaternion.from_euler(Vector3(-math.pi, 0.0, 0.0)), - ) - module._pick_execution = MagicMock() - module._pick_execution.open_gripper.return_value = MagicMock(is_success=lambda: True) - module._pick_execution.move_to_pose.return_value = MagicMock(is_success=lambda: True) - module._pick_execution.close_gripper.return_value = MagicMock(is_success=lambda: True) - module._pick_execution.get_gripper.return_value = 0.0 - - result = module.pick_selected() - - assert not result.is_success() - assert result.error_code == "GRASP_VERIFICATION_FAILED" - assert "empty-closed" in result.message - assert result.metadata["gripper_position"] == 0.0 - assert result.metadata["rescan_required"] is True - assert result.metadata["recovered_to_pre_grasp"] is True - assert module._pick_execution.open_gripper.call_count == 2 - assert module._pick_execution.move_to_pose.call_count == 3 - - -def test_place_selected_uses_remembered_box_and_held_object() -> None: - with patch.object(ModuleBase, "__init__", lambda self, config_args: None): - module = PickNPlaceModule() - module.config = PickNPlaceConfig() - module._open_box = { - "center_x": 0.4, - "center_y": -0.1, - "tabletop_z": 0.1, - "rim_z": 0.18, - "opening_width": 0.18, - "opening_depth": 0.14, - } - module._held_object_size = Vector3(0.04, 0.03, 0.02) - module._pick_execution = MagicMock() - module._pick_execution.move_to_pose.return_value = MagicMock(is_success=lambda: True) - module._pick_execution.open_gripper.return_value = MagicMock(is_success=lambda: True) - module._pick_execution.get_ee_pose.return_value = Pose( - Vector3(0.3, 0.1, 0.22), Quaternion(0.0, 0.0, 0.0, 1.0) - ) - - result = module.place_selected() - - assert result.is_success() - assert module._pick_execution.move_to_pose.call_args_list[0].args[:3] == pytest.approx( - (0.3, 0.1, 0.31) - ) - assert module._pick_execution.move_to_pose.call_args_list[1].args[:3] == pytest.approx( - (0.4, -0.1, 0.31) - ) - assert module._pick_execution.move_to_pose.call_args_list[2].args[:3] == pytest.approx( - (0.4, -0.1, 0.21) - ) - assert result.metadata["object_bottom_clearance"] == pytest.approx(0.02) - assert module._pick_execution.move_to_pose.call_count == 3 - assert module._held_object_size is None - - -def test_picknplace_home_matches_xarm_lifecycle_home() -> None: - assert _picknplace_xarm6_model.home_joints == [ - 0.0, - math.radians(-40.0), - math.radians(-50.0), - 0.0, - math.radians(90.0), - 0.0, - ] - - -def test_picknplace_graspgenx_uses_xarm_tcp_calibration() -> None: - assert _xarm_graspgenx.grasp_frame_to_tcp[2][3] == pytest.approx(0.172) - assert _xarm_graspgenx.grasp_frame_to_tcp[:2] == ((0.0, -1.0, 0.0, 0.0), (1.0, 0.0, 0.0, 0.0)) - assert np.allclose( - np.asarray(_xarm_graspgenx.grasp_frame_to_tcp) @ np.asarray(XARM_TCP_TO_GRASP_FRAME), - np.eye(4), - ) - - -def test_picknplace_blueprint_accepts_short_backend_and_grasp_options() -> None: - config = BlueprintConfigParser(picknplace) - - options = config.parse( - overrides={"osr": {"det": "moondream", "seg": "edgetam"}, "pnp": {"grasp": "graspgenx"}} - ) - - assert options.module_configs["osr"]["det"] == "moondream" - assert options.module_configs["osr"]["seg"] == "edgetam" - assert options.module_configs["pnp"]["grasp"] == "graspgenx" - - -def test_table_surface_estimate_ignores_objects_above_the_table() -> None: - x, y = np.meshgrid(np.linspace(0.2, 0.8, 20), np.linspace(-0.4, 0.4, 20)) - table = np.column_stack((x.ravel(), y.ravel(), np.full(x.size, 0.35))) - object_points = np.array([[0.5, 0.0, 0.55], [0.51, 0.0, 0.57], [0.5, 0.01, 0.56]]) - - estimate = _estimate_table_surface(np.vstack((table, object_points))) - - assert estimate is not None - assert estimate["tabletop_z"] == pytest.approx(0.35, abs=0.01) - assert estimate["width"] >= 0.8 - assert estimate["depth"] >= 1.0 - - -def test_table_surface_estimate_displays_filled_tabletop() -> None: - with patch.object(ModuleBase, "__init__", lambda self, config_args: None): - module = PickNPlaceModule() - x, y = np.meshgrid(np.linspace(0.2, 0.8, 20), np.linspace(-0.4, 0.4, 20)) - scene_cloud = MagicMock() - scene_cloud.points_f32.return_value = np.column_stack( - (x.ravel(), y.ravel(), np.full(x.size, 0.35)) - ) - module._scene = MagicMock(get_full_scene_pointcloud=MagicMock(return_value=scene_cloud)) - module._visualization = MagicMock() - - assert module.estimate_table_surface() is not None - - layer = module._visualization.set_visualization_layer.call_args.args[0] - assert isinstance(layer.elements[0], MeshElement) - assert layer.elements[0].opacity == pytest.approx(1.0) - np.testing.assert_array_equal(layer.elements[0].triangles, [[0, 1, 2], [0, 2, 3]]) - - -def test_estimate_table_runs_a_fresh_scan_before_fitting() -> None: - with patch.object(ModuleBase, "__init__", lambda self, config_args: None): - module = PickNPlaceModule() - module.scan_scene = MagicMock() - module.estimate_table_surface = MagicMock( - return_value={ - "center_x": 0.5, - "center_y": 0.0, - "tabletop_z": 0.35, - "width": 0.8, - "depth": 1.0, - } - ) - - result = module.estimate_table() - - assert result.is_success() - module.scan_scene.assert_called_once_with() - - -def test_install_open_box_is_display_only() -> None: - with patch.object(ModuleBase, "__init__", lambda self, config_args: None): - module = PickNPlaceModule() - obj = MagicMock( - center=Vector3(0.4, 0.1, 0.14), - size=Vector3(0.20, 0.16, 0.08), - ) - obj.pose.orientation = Quaternion.from_euler(Vector3(0.0, 0.0, 0.0)) - obj.pointcloud.points_f32.return_value = np.asarray([[0.3, 0.1, 0.18]] * 10, dtype=np.float32) - module._latest_objects = (obj,) - module._tabletop_z = 0.10 - module._obstacle_world = MagicMock() - module._obstacle_world.update_obstacle.return_value = False - module._obstacle_world.add_obstacle.side_effect = lambda name, *_: name - module._visualization = MagicMock() - - result = module.install_open_box(1, wall_thickness=0.01) - - assert result.is_success() - assert result.metadata["opening_width"] == pytest.approx(0.18) - assert result.metadata["opening_depth"] == pytest.approx(0.14) - module._obstacle_world.add_obstacle.assert_not_called() - module._obstacle_world.update_obstacle.assert_not_called() - layer = module._visualization.set_visualization_layer.call_args.args[0] - assert layer.id == "picknplace/open-box" - assert layer.elements[0].id == "box-envelope" - - -def test_picknplace_uses_top_graspgenx_candidate() -> None: - with patch.object(ModuleBase, "__init__", lambda self, config_args: None): - module = PickNPlaceModule() - module.config = PickNPlaceConfig(grasp_strategy="graspgenx") - obj = MagicMock( - ts=1.0, - frame_id="link_base", - center=Vector3(0.1, 0.2, 0.3), - pointcloud=MagicMock(), - ) - obj.camera_transform = None - obj.image = None - obj.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) - obj.pointcloud.points_f32.return_value = np.asarray([[0.4, 0.5, 0.6]], dtype=np.float32) - module._latest_objects = (obj,) - candidate = GraspCandidate( - Pose( - Vector3(0.4, 0.5, 0.6), - Quaternion.from_euler(Vector3(0.0, math.pi / 2.0, 0.0)), - ), - score=0.9, - ) - second_candidate = GraspCandidate( - Pose(Vector3(0.2, 0.3, 0.4), Quaternion()), - score=0.8, - ) - module._grasp_generator = MagicMock( - propose_grasps=MagicMock( - return_value=GraspCandidateArray( - Header(2.0, "link_base"), [candidate, second_candidate] - ) - ) - ) - module._grasp_filter = MagicMock( - inverse_kinematics_single=MagicMock(return_value=IKResult(IKStatus.SUCCESS)) - ) - module.grasp_candidates = MagicMock() - module._visualization = MagicMock() - - goal = module.get_goal_pose(1) - - assert goal is not None - assert goal.ts == 2.0 - assert goal.frame_id == "link_base" - assert goal.position == candidate.pose.position - assert goal.orientation == candidate.pose.orientation - assert module.get_grasp_candidates().candidates == [candidate, second_candidate] - module.grasp_candidates.publish.assert_called_once_with(module.get_grasp_candidates()) - pre_grasp = module.get_pre_grasp_pose() - assert pre_grasp is not None - assert pre_grasp.position.x == pytest.approx(goal.position.x - 0.1) - assert pre_grasp.position.z == pytest.approx(goal.position.z) - layer = module._visualization.set_visualization_layer.call_args.args[0] - assert layer.id == "picknplace/selection" - assert layer.elements[0].points.shape[1] == 3 - assert layer.elements[1].line_width is None - selected_goal = module.select_grasp_candidate(1) - assert selected_goal is not None - assert selected_goal.position == second_candidate.pose.position - assert module.get_grasp_candidates().selected_index == 1 - pre_grasp = module.get_pre_grasp_pose() - assert pre_grasp is not None - assert pre_grasp.position.x == pytest.approx(selected_goal.position.x) - assert pre_grasp.position.z == pytest.approx(selected_goal.position.z - 0.1) - - -def test_picknplace_excludes_collision_or_ik_infeasible_candidates() -> None: - with patch.object(ModuleBase, "__init__", lambda self, config_args: None): - module = PickNPlaceModule() - module.config = PickNPlaceConfig(grasp_strategy="graspgenx") - safe = GraspCandidate(Pose(Vector3(0.4, 0.5, 0.6), Quaternion()), score=0.9) - unsafe = GraspCandidate(Pose(Vector3(0.2, 0.3, 0.4), Quaternion()), score=0.8) - module._grasp_filter = MagicMock( - inverse_kinematics_single=MagicMock( - side_effect=[IKResult(IKStatus.SUCCESS), IKResult(IKStatus.NO_SOLUTION)] - ) - ) - - filtered = module._prepare_candidates( - GraspCandidateArray(Header(2.0, "link_base"), [safe, unsafe]) - ) - - assert filtered.candidates == [safe] - module._grasp_filter.inverse_kinematics_single.assert_any_call( - safe.pose, "arm", check_collision=True - ) - module._grasp_filter.inverse_kinematics_single.assert_any_call( - unsafe.pose, "arm", check_collision=True - ) - - -def test_picknplace_can_skip_ik_filter_and_keep_confidence_order() -> None: - with patch.object(ModuleBase, "__init__", lambda self, config_args: None): - module = PickNPlaceModule() - module.config = PickNPlaceConfig(candidate_filter="off") - low_score = GraspCandidate(Pose(Vector3(0.4, 0.5, 0.6), Quaternion()), score=0.2) - high_score = GraspCandidate(Pose(Vector3(0.2, 0.3, 0.4), Quaternion()), score=0.9) - module._grasp_filter = MagicMock() - - prepared = module._prepare_candidates( - GraspCandidateArray(Header(2.0, "link_base"), [low_score, high_score]) - ) - - assert prepared.candidates == [high_score, low_score] - module._grasp_filter.inverse_kinematics_single.assert_not_called() - - -def test_picknplace_can_rank_candidates_by_ik_feasibility() -> None: - with patch.object(ModuleBase, "__init__", lambda self, config_args: None): - module = PickNPlaceModule() - module.config = PickNPlaceConfig(candidate_filter="off", candidate_ranking="ik_feasibility") - unsafe = GraspCandidate(Pose(Vector3(0.4, 0.5, 0.6), Quaternion()), score=0.9) - safe = GraspCandidate(Pose(Vector3(0.2, 0.3, 0.4), Quaternion()), score=0.2) - module._grasp_filter = MagicMock( - inverse_kinematics_single=MagicMock( - side_effect=[IKResult(IKStatus.NO_SOLUTION), IKResult(IKStatus.SUCCESS)] - ) - ) - - prepared = module._prepare_candidates( - GraspCandidateArray(Header(2.0, "link_base"), [unsafe, safe]) - ) - - assert prepared.candidates == [safe, unsafe] - - -def test_picknplace_clears_candidates_when_no_proposal_is_safe() -> None: - with patch.object(ModuleBase, "__init__", lambda self, config_args: None): - module = PickNPlaceModule() - module.config = PickNPlaceConfig(grasp_strategy="graspgenx") - obj = MagicMock( - ts=1.0, - frame_id="link_base", - center=Vector3(0.1, 0.2, 0.3), - pointcloud=MagicMock(), - ) - obj.camera_transform = None - obj.image = None - obj.pose.orientation = Quaternion() - module._latest_objects = (obj,) - unsafe = GraspCandidate(Pose(Vector3(0.2, 0.3, 0.4), Quaternion()), score=0.8) - module._grasp_generator = MagicMock( - propose_grasps=MagicMock( - return_value=GraspCandidateArray(Header(2.0, "link_base"), [unsafe]) - ) - ) - module._grasp_filter = MagicMock( - inverse_kinematics_single=MagicMock(return_value=IKResult(IKStatus.NO_SOLUTION)) - ) - module.grasp_candidates = MagicMock() - - assert module.get_goal_pose(1) is None - module.grasp_candidates.publish.assert_called_once() - assert module.grasp_candidates.publish.call_args.args[0].candidates == [] - - -def test_picknplace_returns_empty_candidates_before_provider_selection() -> None: - with patch.object(ModuleBase, "__init__", lambda self, config_args: None): - module = PickNPlaceModule() - module.config = PickNPlaceConfig() - - assert module.get_grasp_candidates().candidates == [] diff --git a/dimos/manipulation/test_pnpconsole.py b/dimos/manipulation/test_pnpconsole.py deleted file mode 100644 index b1d302b73e..0000000000 --- a/dimos/manipulation/test_pnpconsole.py +++ /dev/null @@ -1,206 +0,0 @@ -# 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 unittest.mock import MagicMock - -from dimos.manipulation import pnpconsole -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray - - -def test_client_scans_scene_and_quits(monkeypatch) -> None: # type: ignore[no-untyped-def] - pnp = MagicMock() - pnp.scan_scene.return_value = MagicMock(detections_length=3) - app = MagicMock(pnp=pnp) - monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) - choices = iter(["1", "", "q"]) - monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) - - pnpconsole.main() - - pnp.scan_scene.assert_called_once_with(None) - - -def test_client_scans_scene_with_text_prompt(monkeypatch) -> None: # type: ignore[no-untyped-def] - pnp = MagicMock() - pnp.scan_scene.return_value = MagicMock(detections_length=1) - app = MagicMock(pnp=pnp) - monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) - choices = iter(["1", "water bottle", "q"]) - monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) - - pnpconsole.main() - - pnp.scan_scene.assert_called_once_with("water bottle") - - -def test_client_describes_current_scene(monkeypatch) -> None: # type: ignore[no-untyped-def] - pnp = MagicMock() - pnp.describe_scene.return_value = "A blue block is on the table." - app = MagicMock(pnp=pnp) - monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) - choices = iter(["16", "", "q"]) - monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) - - pnpconsole.main() - - pnp.describe_scene.assert_called_once_with("What objects are visible on the table?") - - -def test_client_does_not_execute_without_a_plan(monkeypatch) -> None: # type: ignore[no-untyped-def] - app = MagicMock() - manipulation = app.ManipulationModule - monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) - choices = iter(["5", "q"]) - monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) - - pnpconsole.main() - - manipulation.execute_and_wait.assert_not_called() - - -def test_client_goes_home(monkeypatch) -> None: # type: ignore[no-untyped-def] - app = MagicMock() - manipulation = app.ManipulationModule - monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) - choices = iter(["13", "q"]) - monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) - - pnpconsole.main() - - manipulation.go_home.assert_called_once_with("arm") - - -def test_client_does_not_execute_descent_without_a_plan(monkeypatch) -> None: # type: ignore[no-untyped-def] - app = MagicMock() - manipulation = app.ManipulationModule - monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) - choices = iter(["7", "q"]) - monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) - - pnpconsole.main() - - manipulation.execute_and_wait.assert_not_called() - - -def test_client_previews_descent_before_explicit_execution(monkeypatch) -> None: # type: ignore[no-untyped-def] - goal = PoseStamped(position=Vector3(0.1, 0.2, 0.3)) - pre_grasp = PoseStamped(position=Vector3(0.1, 0.2, 0.2)) - pnp = MagicMock() - pnp.get_goal_pose.return_value = goal - pnp.get_grasp_candidates.return_value = GraspCandidateArray() - pnp.get_pre_grasp_pose.return_value = pre_grasp - app = MagicMock(pnp=pnp) - manipulation = app.ManipulationModule - manipulation.plan_to_pose.return_value = True - manipulation.execute_and_wait.return_value = True - manipulation.get_ee_pose.return_value = pre_grasp - manipulation.plan_cartesian_targets.return_value = True - monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) - choices = iter(["3", "1", "4", "5", "6", "q"]) - monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) - - pnpconsole.main() - - assert manipulation.execute_and_wait.call_count == 1 - manipulation.preview_plan.assert_called_with(duration=2.0) - - -def test_client_runs_grasp_and_lift_without_preview(monkeypatch) -> None: # type: ignore[no-untyped-def] - goal = PoseStamped(position=Vector3(0.1, 0.2, 0.3)) - pre_grasp = PoseStamped(position=Vector3(0.1, 0.2, 0.2)) - pnp = MagicMock() - pnp.get_goal_pose.return_value = goal - pnp.get_grasp_candidates.return_value = GraspCandidateArray() - pnp.get_pre_grasp_pose.return_value = pre_grasp - app = MagicMock(pnp=pnp) - manipulation = app.ManipulationModule - manipulation.plan_to_pose.return_value = True - manipulation.plan_cartesian_targets.return_value = True - manipulation.execute_and_wait.return_value = True - manipulation.get_ee_pose.return_value = pre_grasp - manipulation.close_gripper.return_value.is_success.return_value = True - monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) - monkeypatch.setattr(pnpconsole.time, "sleep", lambda _: None) - choices = iter(["3", "1", "4", "5", "15", "q"]) - monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) - - pnpconsole.main() - - assert manipulation.execute_and_wait.call_count == 3 - assert manipulation.plan_cartesian_targets.call_count == 2 - manipulation.close_gripper.assert_called_once_with("arm") - manipulation.preview_plan.assert_called_once_with(duration=2.0) - - -def test_client_installs_table_collision_with_recommended_clearance(monkeypatch) -> None: # type: ignore[no-untyped-def] - pnp = MagicMock() - pnp.estimate_table_surface.return_value = { - "center_x": 0.5, - "center_y": 0.0, - "tabletop_z": 0.35, - "width": 0.8, - "depth": 1.0, - } - app = MagicMock(pnp=pnp) - manipulation = app.ManipulationModule - monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) - choices = iter(["14", "", "q"]) - monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) - - pnpconsole.main() - - pnp.scan_scene.assert_called_once_with() - manipulation.set_table_collision.assert_called_once_with( - 0.5, 0.0, 0.35, 0.8, 1.0, safety_margin=0.01 - ) - - -def test_client_accepts_zero_table_clearance(monkeypatch) -> None: # type: ignore[no-untyped-def] - pnp = MagicMock() - pnp.estimate_table_surface.return_value = { - "center_x": 0.5, - "center_y": 0.0, - "tabletop_z": 0.35, - "width": 0.8, - "depth": 1.0, - } - app = MagicMock(pnp=pnp) - manipulation = app.ManipulationModule - monkeypatch.setattr(pnpconsole.Dimos, "connect", lambda: app) - choices = iter(["14", "0", "q"]) - monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) - - pnpconsole.main() - - manipulation.set_table_collision.assert_called_once_with( - 0.5, 0.0, 0.35, 0.8, 1.0, safety_margin=0.0 - ) - - -def test_preview_plays_once_slowly() -> None: - manipulation = MagicMock() - - pnpconsole._preview(manipulation) - - manipulation.preview_plan.assert_called_once_with(duration=2.0) - - -def test_grasp_rank_accepts_default_and_valid_selection(monkeypatch) -> None: # type: ignore[no-untyped-def] - choices = iter(["", "7"]) - monkeypatch.setattr("builtins.input", lambda _prompt: next(choices)) - - assert pnpconsole._grasp_rank(10) == 0 - assert pnpconsole._grasp_rank(10) == 7 diff --git a/dimos/manipulation/visualization_spec.py b/dimos/manipulation/visualization_spec.py deleted file mode 100644 index d9a144856f..0000000000 --- a/dimos/manipulation/visualization_spec.py +++ /dev/null @@ -1,24 +0,0 @@ -# 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. - -"""RPC contract for display-only manipulation visualization layers.""" - -from typing import Protocol - -from dimos.manipulation.visualization.layers import VisualizationLayer -from dimos.spec.utils import Spec - - -class ManipulationVisualizationSpec(Spec, Protocol): - def set_visualization_layer(self, layer: VisualizationLayer) -> bool: ... diff --git a/dimos/perception/sim_object_scene.py b/dimos/perception/sim_object_scene.py new file mode 100644 index 0000000000..f29f812d8d --- /dev/null +++ b/dimos/perception/sim_object_scene.py @@ -0,0 +1,228 @@ +# 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.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.perception.experimental.object import ( + Object as DetObject, + to_detection3d_array, +) +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._prompts: tuple[str, ...] = () + 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 + if not self._matches(name): + continue + extent = self._extents[name] + detections.append( + DetObject( # type: ignore[abstract] + 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 _matches(self, name: str) -> bool: + lowered = name.lower() + return not self._prompts or any(p in lowered or lowered in p for p in self._prompts) + + @rpc + def set_prompts(self, text: list[str] | None = None) -> None: + """Restrict published detections to objects whose name matches a prompt. + + Ground truth has no detector to prompt, so the prompts filter by name + substring instead. Scene and per-object clouds stay complete, matching a + real detector whose prompts never affect the raw depth cloud. + """ + self._prompts = tuple(prompt.lower() for prompt in text or ()) + + @rpc + def scan_scene(self) -> Detection3DArray: + """Publish and return one ground-truth detection pass.""" + detections = self._build_detections() + if detections: + self.objects.publish(detections) + return to_detection3d_array( + detections, + frame_id=self.config.frame_id, + ts=detections[0].ts if detections else time.time(), + ) + + 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 da27cdf942..37e0eafc58 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -91,8 +91,6 @@ "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_planner_coordinator", "openyam-planner-coordinator": "dimos.robot.manipulators.openyam.blueprints.basic:openyam_planner_coordinator", "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", - "picknplace": "dimos.manipulation.blueprints:picknplace", - "picknplace-agent": "dimos.manipulation.blueprints:picknplace_agent", "teleop-hosted-go2-multicam": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_go2_multicam", "teleop-hosted-go2-transport": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_go2_transport", "teleop-hosted-xarm6": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_xarm6", @@ -153,12 +151,8 @@ "unitree-go2-webrtc-keyboard-teleop": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_webrtc_keyboard_teleop:unitree_go2_webrtc_keyboard_teleop", "unitree-go2-webrtc-rage-keyboard-teleop": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_webrtc_rage_keyboard_teleop:unitree_go2_webrtc_rage_keyboard_teleop", "unity-sim": "dimos.simulation.unity.blueprint:unity_sim", - "xarm-graspgenx": "dimos.robot.manipulators.xarm.blueprints.graspgenx:xarm_graspgenx", - "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-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", + "xarm-box-filling": "dimos.robot.manipulators.xarm.blueprints.box_filling:xarm_box_filling", + "xarm-grasp-sim-agent": "dimos.robot.manipulators.xarm.blueprints.agentic:xarm_grasp_sim_agent", "xarm6-worldbelief": "dimos.experimental.world_belief.xarm6_blueprint:xarm6_worldbelief", "xarm7-planner-coordinator": "dimos.robot.manipulators.xarm.blueprints.basic:xarm7_planner_coordinator", "xarm7-planner-coordinator-agent": "dimos.robot.manipulators.xarm.blueprints.agentic:xarm7_planner_coordinator_agent", @@ -173,6 +167,7 @@ "b1-connection-module": "dimos.robot.unitree.b1.connection.B1ConnectionModule", "basic-path-follower": "dimos.navigation.basic_path_follower.module.BasicPathFollower", "benchmarker": "dimos.control.benchmarking.benchmark.Benchmarker", + "box-filling-pick-and-place-module": "dimos.manipulation.box_filling_pick_and_place_module.BoxFillingPickAndPlaceModule", "camera-module": "dimos.hardware.sensors.camera.module.CameraModule", "camera-mux-module": "dimos.teleop.hosted.camera_mux.CameraMuxModule", "cartesian-motion-controller": "dimos.manipulation.control.servo_control.cartesian_motion_controller.CartesianMotionController", @@ -262,7 +257,6 @@ "pgo": "dimos.navigation.cmu_nav.modules.pgo.pgo.PGO", "phone-teleop-module": "dimos.teleop.phone.phone_teleop_module.PhoneTeleopModule", "pick-and-place-module": "dimos.manipulation.pick_and_place_module.PickAndPlaceModule", - "pick-n-place-module": "dimos.manipulation.picknplace.PickNPlaceModule", "point-lio": "dimos.hardware.sensors.lidar.pointlio.module.PointLio", "pointlio-recorder": "dimos.hardware.sensors.lidar.pointlio.recorder.PointlioRecorder", "quest-teleop-module": "dimos.teleop.quest.quest_teleop_module.QuestTeleopModule", @@ -278,6 +272,7 @@ "rerun-web-socket-server": "dimos.visualization.rerun.websocket_server.RerunWebSocketServer", "security-module": "dimos.experimental.security_demo.security_module.SecurityModule", "semantic-search": "dimos.memory2.module.SemanticSearch", + "sim-object-scene": "dimos.perception.sim_object_scene.SimObjectScene", "simple-phone-teleop": "dimos.teleop.phone.phone_extensions.SimplePhoneTeleop", "simple-planner": "dimos.navigation.cmu_nav.modules.simple_planner.simple_planner.SimplePlanner", "spatial-memory": "dimos.perception.experimental.spatial_perception.SpatialMemory", diff --git a/dimos/robot/manipulators/xarm/blueprints/agentic.py b/dimos/robot/manipulators/xarm/blueprints/agentic.py index 465370a77b..eb2fc5b60f 100644 --- a/dimos/robot/manipulators/xarm/blueprints/agentic.py +++ b/dimos/robot/manipulators/xarm/blueprints/agentic.py @@ -24,9 +24,7 @@ MANIPULATION_AGENT_SYSTEM_PROMPT, ) 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 xarm7_planner_coordinator_agent = autoconnect( xarm7_planner_coordinator, @@ -34,20 +32,8 @@ McpClient.blueprint(system_prompt=BASE_MANIPULATION_AGENT_SYSTEM_PROMPT), ) -xarm_perception_agent = autoconnect( - xarm_perception, - McpServer.blueprint(), - McpClient.blueprint(system_prompt=MANIPULATION_AGENT_SYSTEM_PROMPT), -) - -xarm_graspgenx_agent = autoconnect( - xarm_graspgenx, - McpServer.blueprint(), - McpClient.blueprint(system_prompt=MANIPULATION_AGENT_SYSTEM_PROMPT), -) - -xarm_perception_sim_agent = autoconnect( - xarm_perception_sim, +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/box_filling.py b/dimos/robot/manipulators/xarm/blueprints/box_filling.py new file mode 100644 index 0000000000..721d8b6cc2 --- /dev/null +++ b/dimos/robot/manipulators/xarm/blueprints/box_filling.py @@ -0,0 +1,106 @@ +# 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. + +"""Real xArm6 box-filling product blueprint.""" + +import math + +from dimos.agents.mcp.mcp_client import McpClient +from dimos.agents.mcp.mcp_server import McpServer +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config +from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera +from dimos.manipulation.box_filling_pick_and_place_module import ( + BoxFillingPickAndPlaceModule, +) +from dimos.manipulation.grasping.grasp_gen_x import GraspGenXModule +from dimos.manipulation.visualization.rerun import picknplace_rerun_config +from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig +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.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule +from dimos.robot.manipulators.common.blueprints import coordinator, trajectory_task +from dimos.robot.manipulators.xarm.config import make_xarm6_model_config, xarm6_hardware +from dimos.robot.manipulators.xarm.grasp_config import make_xarm_graspgenx_config +from dimos.visualization.vis_module import vis_module + +_CAMERA_TRANSFORM = Transform( + translation=Vector3(0.06693724, -0.0309563, 0.00691482), + rotation=Quaternion(0.70513398, 0.00535696, 0.70897578, -0.01052180), +) + +BOX_FILLING_SYSTEM_PROMPT = """You operate an xArm that fills an open box. + +Use scan_objects with simple object names. Select the destination using +select_destination_container, then select and pick each source object with +select_object and pick_selected. After a verified pick, use +place_in_destination. Treat tool results as authoritative and rescan after a +stale-selection or perception failure. +""" + +_hardware = xarm6_hardware("arm", gripper=True) +_model = make_xarm6_model_config( + name="arm", + add_gripper=True, + tf_extra_links=["link_base", "link6"], + home_joints=[0.0, math.radians(-40.0), math.radians(-50.0), 0.0, math.radians(90.0), 0.0], +) +_model.max_velocity = 0.25 +_model.max_acceleration = 0.5 +_graspgenx = make_xarm_graspgenx_config() + +xarm_box_filling = autoconnect( + coordinator(hardware=[_hardware], tasks=[trajectory_task(_hardware)]), + BoxFillingPickAndPlaceModule.blueprint( + robots=[_model], + visualization=ViserVisualizationConfig(port=8095), + planning_timeout=10.0, + planning_frame="world", + grasp_verification={ + "open_position": 0.85, + "closed_position": 0.0, + "held_threshold": 0.02, + }, + ), + RealSenseCamera.blueprint( + width=848, + height=480, + fps=15, + camera_name="camera", + base_frame_id="link6", + base_transform=_CAMERA_TRANSFORM, + enable_depth=True, + align_depth_to_color=True, + enable_pointcloud=False, + ), + ObjectSceneRegistrationModule.blueprint( + instance_name="osr", + target_frame="world", + register_objects=False, + detect_on_request=True, + detector_confidence=0.4, + object_voxel_downsample=0.001, + ), + GraspGenXModule.blueprint( + instance_name="ggx", + load_on_start=False, + **_graspgenx.model_dump( + exclude={"rpc_transport", "tf_transport", "g", "instance_name", "load_on_start"} + ), + ), + vis_module(global_config.viewer, rerun_config=picknplace_rerun_config()), + McpServer.blueprint(), + McpClient.blueprint(system_prompt=BOX_FILLING_SYSTEM_PROMPT), +).global_config(rerun_open="web") diff --git a/dimos/robot/manipulators/xarm/blueprints/graspgenx.py b/dimos/robot/manipulators/xarm/blueprints/graspgenx.py deleted file mode 100644 index c753194c97..0000000000 --- a/dimos/robot/manipulators/xarm/blueprints/graspgenx.py +++ /dev/null @@ -1,61 +0,0 @@ -# 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. - -"""GraspGenX-enabled real-hardware xArm perception blueprint.""" - -from __future__ import annotations - -import math - -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.robot.manipulators.xarm.blueprints.perception import xarm_perception -from dimos.robot.manipulators.xarm.config import make_xarm7_model_config -from dimos.robot.manipulators.xarm.grasp_config import make_xarm_graspgenx_config - -_graspgenx_config = make_xarm_graspgenx_config() - -xarm_graspgenx = autoconnect( - xarm_perception, - PickAndPlaceModule.blueprint( - robots=[ - make_xarm7_model_config( - name="arm", - add_gripper=True, - pitch=math.radians(45), - tf_extra_links=["link7"], - ) - ], - planning_timeout=10.0, - visualization={"backend": "meshcat"}, - floor_z=-0.02, - heuristic_grasp_fallback=False, - planning_frame="world", - grasp_approach_vector=(0.0, 0.0, -1.0), - grasp_verification={ - # Enable only after completing the hardware calibration recorded - # in the grasp-pipeline OpenSpec change. - "enabled": False, - "open_position": 0.85, - "closed_position": 0.0, - "held_threshold": 0.02, - "timeout": 2.0, - "poll_interval": 0.05, - }, - ), - GraspGenXModule.blueprint( - **_graspgenx_config.model_dump(exclude={"rpc_transport", "tf_transport", "g"}) - ), -).global_config(n_workers=5) diff --git a/dimos/robot/manipulators/xarm/blueprints/perception.py b/dimos/robot/manipulators/xarm/blueprints/perception.py deleted file mode 100644 index ead451ca37..0000000000 --- a/dimos/robot/manipulators/xarm/blueprints/perception.py +++ /dev/null @@ -1,62 +0,0 @@ -# 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. - -"""Real-hardware xArm perception manipulation blueprints.""" - -from __future__ import annotations - -import math - -from dimos.core.coordination.blueprints import autoconnect -from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera -from dimos.manipulation.pick_and_place_module import PickAndPlaceModule -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.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule -from dimos.robot.manipulators.xarm.config import make_xarm7_model_config - -XARM_PERCEPTION_CAMERA_TRANSFORM = Transform( - translation=Vector3(x=0.06693724, y=-0.0309563, z=0.00691482), - rotation=Quaternion(0.70513398, 0.00535696, 0.70897578, -0.01052180), # xyzw -) - -xarm_perception = autoconnect( - PickAndPlaceModule.blueprint( - robots=[ - make_xarm7_model_config( - name="arm", - add_gripper=True, - pitch=math.radians(45), - tf_extra_links=["link7"], - ) - ], - planning_timeout=10.0, - visualization={"backend": "meshcat"}, - floor_z=-0.02, - heuristic_grasp_fallback=True, - ), - RealSenseCamera.blueprint( - base_frame_id="link7", - base_transform=XARM_PERCEPTION_CAMERA_TRANSFORM, - ), - ObjectSceneRegistrationModule.blueprint( - target_frame="world", - distance_threshold=0.08, - min_detections_for_permanent=3, - max_distance=1.0, - use_aabb=True, - max_obstacle_width=0.06, - ), -).global_config(n_workers=4) diff --git a/dimos/robot/manipulators/xarm/blueprints/simulation.py b/dimos/robot/manipulators/xarm/blueprints/simulation.py index 7e7d2fa1de..9a8bfab6a2 100644 --- a/dimos/robot/manipulators/xarm/blueprints/simulation.py +++ b/dimos/robot/manipulators/xarm/blueprints/simulation.py @@ -12,37 +12,54 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Simulation xArm perception manipulation blueprints.""" - -from __future__ import annotations +"""Private xArm grasp-simulation stack used by its public agent blueprint.""" from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config +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.manipulation.visualization.rerun import picknplace_rerun_config +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.visualization.rerun.bridge import RerunBridgeModule +from dimos.utils.data import LfsPath +from dimos.visualization.vis_module import vis_module -_xarm7_sim_hw = make_xarm7_sim_hardware(XARM7_SIM_PATH) +_TABLE = {"name": "table", "center": (0.47, 0.0, 0.065), "size": (0.38, 0.60, 0.13)} +_GRASPGENX = make_xarm_graspgenx_config() +_MESH_DIR = LfsPath("xarm_grasp_sim") / "assets" / "manip" +_OBJECTS = { + name: str(_MESH_DIR / f"{name}.obj") + for name in ("bottle", "box", "can", "cup", "marker", "tape") +} +_hardware = make_xarm7_sim_hardware(XARM_GRASP_SIM_PATH) -xarm_perception_sim = autoconnect( +_xarm_grasp_sim = autoconnect( PickAndPlaceModule.blueprint( robots=[make_xarm7_sim_robot_config()], planning_timeout=10.0, - visualization={"backend": "meshcat"}, - heuristic_grasp_fallback=True, + visualization={"backend": "viser"}, + static_box_obstacles=[_TABLE], + max_grasp_candidates_to_check=30, + use_mesh_obstacles=True, + grasp_verification={ + "open_position": 0.85, + "closed_position": 0.0, + "held_threshold": 0.02, + }, ), - 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)], + MujocoSimModule.blueprint(**make_xarm7_sim_module_kwargs(XARM_GRASP_SIM_PATH)), + SimObjectScene.blueprint(objects=_OBJECTS), + GraspGenXModule.blueprint( + **_GRASPGENX.model_dump(exclude={"rpc_transport", "tf_transport", "g"}) ), - RerunBridgeModule.blueprint(), + coordinator(hardware=[_hardware], tasks=[trajectory_task(_hardware)]), + vis_module(global_config.viewer, rerun_config=picknplace_rerun_config()), ) diff --git a/dimos/robot/manipulators/xarm/config.py b/dimos/robot/manipulators/xarm/config.py index 0906610545..2aa008c463 100644 --- a/dimos/robot/manipulators/xarm/config.py +++ b/dimos/robot/manipulators/xarm/config.py @@ -58,6 +58,7 @@ 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") +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, diff --git a/dimos/robot/test_all_blueprints.py b/dimos/robot/test_all_blueprints.py index 4a6692a663..23dc59c404 100644 --- a/dimos/robot/test_all_blueprints.py +++ b/dimos/robot/test_all_blueprints.py @@ -63,10 +63,8 @@ "teleop-quest-xarm7", "teleop-quest-xarm7-video", "unitree-g1-nav-sim", - "xarm-perception", - "xarm-perception-agent", - "xarm-perception-sim", - "xarm-perception-sim-agent", + "xarm-box-filling", + "xarm-grasp-sim-agent", "xarm7-planner-coordinator", "xarm7-planner-coordinator-agent", } diff --git a/dimos/simulation/engines/mujoco_sim_module.py b/dimos/simulation/engines/mujoco_sim_module.py index 41151f722d..dc4e9682ea 100644 --- a/dimos/simulation/engines/mujoco_sim_module.py +++ b/dimos/simulation/engines/mujoco_sim_module.py @@ -698,6 +698,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]]: + """Return world poses [x, y, z, qx, qy, qz, qw] for named bodies.""" + 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/docs/adr/0001-keep-pick-place-with-manipulation-state-owner.md b/docs/adr/0001-keep-pick-place-with-manipulation-state-owner.md new file mode 100644 index 0000000000..908f6115b5 --- /dev/null +++ b/docs/adr/0001-keep-pick-place-with-manipulation-state-owner.md @@ -0,0 +1,3 @@ +# Keep the Pick/Place Transaction with its manipulation state owner + +`PickAndPlaceModule` is the sole Pick/Place Transaction owner because it also owns planning, execution, gripper state, and exclusive robot use; perception and grasp generation remain injected modules. The separate `PickNPlaceModule` workflow is removed after its useful behavior moves to the canonical module or its box-filling derivative. A workflow over RPC was rejected because validation, contact motion, recovery, and post-closure handling form one safety-critical transaction: splitting them would require a reservation protocol and a broad interface that exposes manipulation internals. Refactoring the inherited central `ManipulationModule` skill surface is outside this integration. diff --git a/docs/adr/0002-compose-exactly-one-grasp-provider.md b/docs/adr/0002-compose-exactly-one-grasp-provider.md new file mode 100644 index 0000000000..87f4b29aef --- /dev/null +++ b/docs/adr/0002-compose-exactly-one-grasp-provider.md @@ -0,0 +1,3 @@ +# Compose exactly one Grasp Provider + +Each pick/place blueprint composes exactly one module implementing the grasp-provider interface, initially either heuristic grasp generation or GraspGenX. The transaction contains no embedded heuristic and never falls back automatically, so provider dependencies, candidate semantics, failures, and tests remain explicit and deterministic. diff --git a/docs/adr/0003-use-unchecked-motion-for-contact-legs.md b/docs/adr/0003-use-unchecked-motion-for-contact-legs.md new file mode 100644 index 0000000000..9f0816d805 --- /dev/null +++ b/docs/adr/0003-use-unchecked-motion-for-contact-legs.md @@ -0,0 +1,3 @@ +# Use Unchecked Contact Motion for contact legs + +Pre-grasp approach remains collision-checked, while grasp, retreat, place lowering, and post-release retract use short straight TCP paths with planning-scene collision checks disabled. This keeps the scene unchanged and matches the working implementation, but it provides no non-target collision guarantee on those legs; selected-target-only collision allowance is a future planner capability rather than part of the integration. diff --git a/docs/adr/0004-pin-numbered-object-selections.md b/docs/adr/0004-pin-numbered-object-selections.md new file mode 100644 index 0000000000..6216f75959 --- /dev/null +++ b/docs/adr/0004-pin-numbered-object-selections.md @@ -0,0 +1,3 @@ +# Pin numbered object selections before picking + +The operator and agent select objects by their number in one immutable Detection Snapshot, then invoke `pick_selected` on the pinned selection. The module resolves the number immediately and retains the object's stable identity and geometry, so later detection ordering cannot redirect the transaction; state-changing execution never reinterprets a number against a newer snapshot. diff --git a/docs/adr/0005-keep-box-filling-out-of-generic-pick-place-interface.md b/docs/adr/0005-keep-box-filling-out-of-generic-pick-place-interface.md new file mode 100644 index 0000000000..f59a649c62 --- /dev/null +++ b/docs/adr/0005-keep-box-filling-out-of-generic-pick-place-interface.md @@ -0,0 +1,3 @@ +# Keep box-filling behavior out of the generic pick/place interface + +The generic pick/place interface contains object selection and single-object physical transactions, not open-box measurement, fit policy, rim-clearance calculations, or repeated collection behavior. A derived box-filling module adds those application-specific capabilities; its blueprint only composes and configures that module and does not define or gate the module's interface. Generic manipulation callers therefore never need to learn the box-filling surface. diff --git a/docs/adr/0006-place-held-objects-not-the-tcp.md b/docs/adr/0006-place-held-objects-not-the-tcp.md new file mode 100644 index 0000000000..2d168eb9da --- /dev/null +++ b/docs/adr/0006-place-held-objects-not-the-tcp.md @@ -0,0 +1,3 @@ +# Define placement in object coordinates + +The generic `place_at(x, y, z)` operation positions the held object's reference point rather than the robot TCP. The pick transaction retains the grasp relationship and derives the required TCP placement pose while preserving the grasped object orientation, so callers describe the physical object outcome without compensating for gripper geometry. diff --git a/docs/adr/0007-revalidate-prepared-picks-at-execution.md b/docs/adr/0007-revalidate-prepared-picks-at-execution.md new file mode 100644 index 0000000000..94a0c63998 --- /dev/null +++ b/docs/adr/0007-revalidate-prepared-picks-at-execution.md @@ -0,0 +1,3 @@ +# Revalidate Prepared Picks at execution + +`select_object` pins the selected object, its cloud, and one ranked proposal set for inspection without moving or reserving the robot. A later completed scan, a configurable timeout measured from completed preparation, or a planning-frame mismatch makes that Prepared Pick stale. `pick_selected` acquires exclusive transaction ownership, rejects stale input, and performs the authoritative feasibility check from fresh robot state and the current planning scene before choosing the first feasible pinned candidate; it never silently rescans or substitutes an uninspected proposal set. diff --git a/docs/adr/0008-require-verified-holding-for-pick-success.md b/docs/adr/0008-require-verified-holding-for-pick-success.md new file mode 100644 index 0000000000..07421851e6 --- /dev/null +++ b/docs/adr/0008-require-verified-holding-for-pick-success.md @@ -0,0 +1,3 @@ +# Require verified holding for pick success + +`pick_selected` succeeds only when configured gripper feedback verifies that an object prevented empty closure. Real hardware, simulation, and test adapters must provide equivalent verification behavior; missing feedback, empty closure, or any post-close transaction error returns failure, and the module never reports an unverified success or automatically opens after a post-close failure. diff --git a/docs/adr/0009-replace-snapshot-obstacles-automatically.md b/docs/adr/0009-replace-snapshot-obstacles-automatically.md new file mode 100644 index 0000000000..ee8bbb95e4 --- /dev/null +++ b/docs/adr/0009-replace-snapshot-obstacles-automatically.md @@ -0,0 +1,3 @@ +# Replace perception obstacles with each scan + +Every completed scan atomically replaces the perception-derived obstacle set while leaving static environment obstacles untouched. Simulation may derive Object Obstacle Proxies from complete mesh clouds, while real RGB-D perception uses conservatively padded upright boxes from partial segmented clouds; the real path does not use the sim convex-hull assumption or silently clamp obstacle width. The selected target remains registered, and the agreed Unchecked Contact Motion handles intentional contact legs. diff --git a/docs/adr/0010-use-world-for-the-pick-place-interface.md b/docs/adr/0010-use-world-for-the-pick-place-interface.md new file mode 100644 index 0000000000..c3eb3677d5 --- /dev/null +++ b/docs/adr/0010-use-world-for-the-pick-place-interface.md @@ -0,0 +1,3 @@ +# Use `world` for the pick/place interface + +All generic pick/place inputs and outputs use the canonical `world` frame, including detections, object clouds, grasp candidates, obstacle proxies, and placement targets. Robot base and camera frames are transformed below this interface; missing transforms or frame mismatches fail before motion, and blueprint configuration cannot change the meaning of public coordinates. diff --git a/docs/adr/0011-select-the-first-feasible-provider-candidate.md b/docs/adr/0011-select-the-first-feasible-provider-candidate.md new file mode 100644 index 0000000000..b17d9c369b --- /dev/null +++ b/docs/adr/0011-select-the-first-feasible-provider-candidate.md @@ -0,0 +1,3 @@ +# Select the first feasible provider candidate + +The configured Grasp Provider owns candidate ranking, and the transaction checks candidates in that order up to one maximum count. Safety and complete-sequence feasibility validation are mandatory and cannot be configured off; the first candidate with feasible preparation, approach, grasp, and retreat is selected, while rejected candidates are summarized by failed stage. Separate filter and ranking modes are removed. diff --git a/docs/adr/0012-publish-two-xarm-pick-place-blueprints.md b/docs/adr/0012-publish-two-xarm-pick-place-blueprints.md new file mode 100644 index 0000000000..c67d5e648f --- /dev/null +++ b/docs/adr/0012-publish-two-xarm-pick-place-blueprints.md @@ -0,0 +1,3 @@ +# Publish two xArm pick/place blueprints + +The unified work renames the real `picknplace-agent` product to `xarm-box-filling` and retains `xarm-grasp-sim-agent` for generic agentic simulation. `xarm-box-filling` includes the box-filling agent despite omitting an `-agent` suffix because the application name identifies the complete runnable product. The real product retains the remote demo's xArm6 model, home pose, wrist RealSense, and calibration; simulation retains the existing xArm7 MuJoCo scene. The public simulation uses ground-truth object perception so manipulation and agent behavior remain deterministic; simulated-camera perception stays private integration-test coverage. Provider, perception, base-versus-agent, and box-filling-simulation combinations are not separate public blueprints; any reusable compositions remain private helpers or test fixtures, and obsolete overlapping runnable names are removed. diff --git a/docs/capabilities/manipulation/agentic.md b/docs/capabilities/manipulation/agentic.md index 6d57f2a1b5..9cdfcbf0df 100644 --- a/docs/capabilities/manipulation/agentic.md +++ b/docs/capabilities/manipulation/agentic.md @@ -1,153 +1,36 @@ --- -title: "Agentic xArm Simulation" +title: "Agentic xArm Pick and Place" --- -`xarm-perception-sim-agent` runs the xArm perception, planning, MuJoCo -simulation, MCP server, and built-in agent together. It is **simulation-only**; This guide uses this blueprint to provide a walk-through of dimos's agentic manipulation stack. - -See the [manipulation capability overview](/docs/capabilities/manipulation/) for -the underlying planning and perception stack. - -## Prerequisites - -Install the manipulation dependencies: - -```bash -uv sync --extra manipulation --inexact -``` - -The built-in agent requires an `OPENAI_API_KEY`. - - -## Start and stop - -Run in the foreground: - -```bash -uv run dimos run xarm-perception-sim-agent -``` - -Or run it as a daemon: - -```bash -uv run dimos run xarm-perception-sim-agent --daemon -``` - -Inspect and control the run from another terminal: - -```bash -uv run dimos status -uv run dimos log -uv run dimos stop -``` - -Use `dimos log -f` to follow the log while the run is active. - -## Learned grasp-to-pick pipeline - -The real-hardware `xarm-graspgenx-agent` blueprint adds GraspGenX proposals to -the xArm perception stack. Install the optional runtime and start it with: - -```bash -uv sync --extra graspgenx --extra manipulation --inexact -uv run dimos run xarm-graspgenx-agent -``` - -`pick` remains the only high-level picking tool. It resolves one current -object, obtains that object's planning-frame point cloud, requests ranked -GraspGenX candidates, and rejects candidates that fail pre-grasp, grasp, or -retreat inverse kinematics. During planning, the selected target is -temporarily removed from the collision scene while all other obstacles remain -active. The selected candidate then runs through prepare, approach, grasp, -close, verify, and retreat phases. - -Use the stable object ID returned by `scan_objects` whenever names are -ambiguous. A name is accepted only when it identifies exactly one current -detection; an object-ID prefix must also be unique. Existing -`xarm-perception` and `xarm-perception-sim` blueprints retain their explicit -heuristic grasp fallback and do not load the optional GraspGenX runtime. - -The learned pipeline configuration lives in -`dimos/robot/manipulators/xarm/grasp_config.py`. It records the xArm gripper -sweep volume and the transform from GraspGenX's gripper frame to the planned -TCP. `PickAndPlaceModuleConfig` controls the planning frame, maximum point -cloud age, candidate-check limit, TCP approach direction, approach/retreat -offsets, heuristic fallback, and closure-feedback verification thresholds. -Changing the frame transform, approach direction, or closure threshold -requires robot-specific calibration. - -Failures are phase-specific and stop motion immediately. Before closure, a -failed transaction leaves the gripper in its current safe state. After a -successful close command, failures never automatically reopen the gripper; -the result includes `object_may_be_held=true`, and an operator or agent should -inspect state before issuing another motion. Target collision geometry is -restored on every exit path, and restoration errors are reported without -hiding the primary failure. - -The current verification is a closure-position proxy: an xArm gripper that -stops above the calibrated empty-close threshold is treated as holding -something. It does not measure grasp force, detect slip, or prove that the -intended object was acquired. Force/torque or tactile feedback is required for -those stronger guarantees. The shipped learned-grasp blueprint keeps this -proxy disabled until the open, empty-close, and representative held-object -positions have been measured on the target xArm; enable -`grasp_verification.enabled` only after recording that calibration. - -## Daily interaction - -For normal interactive use, start the human-friendly terminal client: +`xarm-grasp-sim-agent` runs xArm7 MuJoCo simulation, ground-truth object +geometry, GraspGenX, planning, MCP, and the built-in agent together. ```bash -uv run dimos humancli -``` - -It connects to the running agent so you can send prompts and read responses in -one session. - -### Try these prompts - -Start with a non-motion state check: - -```text -Report the current robot state without moving. +uv sync --extra manipulation --extra graspgenx --inexact +uv run dimos run xarm-grasp-sim-agent --daemon ``` -Scan the scene for objects. This moves the arm to its observation pose: - -```text -Scan for objects. -``` - -Try basic motion commands: - -```text -Move 10 cm to the left. -``` - -```text -Move 10 cm above the detected object's pose. -``` - -## Debugging and testing interfaces - -Use `agent-send` for one-shot LCM input when testing or diagnosing the agent: +The real box-filling product is: ```bash -uv run dimos agent-send "Report the current robot state and visible objects; do not move the arm or gripper." +uv run dimos run xarm-box-filling --daemon ``` -The blueprint also includes an MCP server. Use these commands for direct -server inspection and tool-level testing: +Both products expose the same generic workflow: `scan_objects`, +`select_object`, `pick_selected`, and `place_at`. A scan creates a numbered +snapshot; selection pins its exact object and provider proposals; picking +performs fresh feasibility checks and requires gripper feedback before +reporting success. All public positions and geometry are in `world`. -```bash -uv run dimos mcp status -uv run dimos mcp list-tools -``` +The box-filling module derives from the generic module and adds +`select_destination_container` and `place_in_destination`. Those application +skills compute opening fit and release clearance without leaking box policy +into the generic API. -For example: +The planner checks collision-free preparation and pre-grasp motion. Straight +TCP contact, retreat, place-lower, and place-retract legs intentionally bypass +collision queries while retaining sequential IK, joint limits, timing, and +execution checks. A target-aware collision policy is a later safety extension. -```bash -uv run dimos mcp call get_robot_state -uv run dimos mcp call look -uv run dimos mcp call scan_objects -``` +After the gripper closes, failures never reopen it automatically because an +object may be held. Missing feedback and an empty-close reading are failures. diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index e1a1481c97..23de2c8b0a 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -62,7 +62,7 @@ The same nested shorthand applies to blueprints that instantiate `PickAndPlaceModule`: ```bash -dimos run xarm-perception-sim \ +dimos run xarm-grasp-sim-agent \ --kinematics.backend=pink ``` @@ -362,10 +362,8 @@ planner is locked for its whole native call. | `keyboard-teleop-xarm7` | XArm7 7-DOF keyboard teleop with Drake viz | | `xarm7-planner-coordinator` | XArm7 planner with coordinator integration | | `dual-xarm6-planner-coordinator` | Dual XArm6 planning with mock coordinator hardware | -| `xarm-perception` | XArm7 + RealSense camera for perception | -| `xarm-perception-agent` | XArm7 perception + LLM agent | -| `xarm-perception-sim` | XArm7 simulation perception stack | -| [`xarm-perception-sim-agent`](/docs/capabilities/manipulation/agentic.md) | XArm7 simulation perception stack + LLM agent | +| `xarm-box-filling` | Real XArm6 + wrist RealSense + GraspGenX box-filling agent | +| [`xarm-grasp-sim-agent`](/docs/capabilities/manipulation/agentic.md) | XArm7 MuJoCo + ground-truth objects + GraspGenX agent | ## Supported Robots From 3ff27c651bbe1003bdb76210918c1d08ab9de10d Mon Sep 17 00:00:00 2001 From: "autofix-ci[bot]" <114827586+autofix-ci[bot]@users.noreply.github.com> Date: Sat, 8 Aug 2026 17:26:30 +0000 Subject: [PATCH 5/5] [autofix.ci] apply automated fixes --- .../box_filling_pick_and_place_module.py | 15 +++------------ dimos/manipulation/manipulation_module.py | 3 +-- dimos/manipulation/pick_and_place_module.py | 12 +++++------- dimos/manipulation/test_pick_and_place_unit.py | 16 ++++++---------- 4 files changed, 15 insertions(+), 31 deletions(-) diff --git a/dimos/manipulation/box_filling_pick_and_place_module.py b/dimos/manipulation/box_filling_pick_and_place_module.py index 4d846a584f..713485a3bc 100644 --- a/dimos/manipulation/box_filling_pick_and_place_module.py +++ b/dimos/manipulation/box_filling_pick_and_place_module.py @@ -56,9 +56,7 @@ def __init__(self, **kwargs: object) -> None: self._destination_box: _DestinationBox | None = None @skill - def select_destination_container( - self, number: int - ) -> SkillResult[ManipulationSkillError]: + def select_destination_container(self, number: int) -> SkillResult[ManipulationSkillError]: """Use one object from the latest scan as the destination open box. Args: @@ -112,18 +110,11 @@ def place_in_destination( return SkillResult.fail( "INVALID_STATE", "No verified held object is available to place" ) - if ( - held_size.x > destination.opening_width - or held_size.y > destination.opening_depth - ): + if held_size.x > destination.opening_width or held_size.y > destination.opening_depth: return SkillResult.fail( "INVALID_INPUT", "Held object does not fit inside the destination opening" ) - object_z = ( - destination.rim_z - + held_size.z / 2.0 - + float(self.config.drop_clearance) - ) + object_z = destination.rim_z + held_size.z / 2.0 + float(self.config.drop_clearance) return self.place_at( destination.center_x, destination.center_y, diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 3ff3be9c82..7df415560b 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -1366,8 +1366,7 @@ def _interpolate_quaternion( if sum(a * b for a, b in zip(start_values, target_values, strict=True)) < 0.0: target_values = tuple(-value for value in target_values) values = tuple( - a + fraction * (b - a) - for a, b in zip(start_values, target_values, strict=True) + a + fraction * (b - a) for a, b in zip(start_values, target_values, strict=True) ) norm = math.sqrt(sum(value * value for value in values)) if norm <= 1e-12: diff --git a/dimos/manipulation/pick_and_place_module.py b/dimos/manipulation/pick_and_place_module.py index 657004750a..cec83f8909 100644 --- a/dimos/manipulation/pick_and_place_module.py +++ b/dimos/manipulation/pick_and_place_module.py @@ -622,9 +622,7 @@ def scan_objects( numbered: list[dict[str, object]] = [] for number, det in enumerate(detections, start=1): c = det.center - lines.append( - f" {number}. {det.name}: ({c.x:.3f}, {c.y:.3f}, {c.z:.3f})" - ) + lines.append(f" {number}. {det.name}: ({c.x:.3f}, {c.y:.3f}, {c.z:.3f})") numbered.append( { "number": number, @@ -740,9 +738,7 @@ def select_object(self, number: int) -> SkillResult[ManipulationSkillError]: ) @skill - def pick_selected( - self, robot_name: str | None = None - ) -> SkillResult[ManipulationSkillError]: + def pick_selected(self, robot_name: str | None = None) -> SkillResult[ManipulationSkillError]: """Pick the object prepared by select_object using fresh feasibility checks. Args: @@ -1123,7 +1119,9 @@ def place_at( object_to_tcp = self._held_object_to_tcp object_orientation = self._held_object_orientation if object_to_tcp is None or object_orientation is None: - return SkillResult.fail("INVALID_STATE", "No verified held object is available to place") + return SkillResult.fail( + "INVALID_STATE", "No verified held object is available to place" + ) desired_object_pose = Pose(Vector3(x, y, z), object_orientation) target_tcp = desired_object_pose + object_to_tcp result = self._place_with_orientation( diff --git a/dimos/manipulation/test_pick_and_place_unit.py b/dimos/manipulation/test_pick_and_place_unit.py index 50b9e52055..09bcf109c3 100644 --- a/dimos/manipulation/test_pick_and_place_unit.py +++ b/dimos/manipulation/test_pick_and_place_unit.py @@ -541,9 +541,7 @@ def _arrange_success(self, module: PickAndPlaceModule, mocker: MockerFixture) -> candidate = _candidate(0.4, 0.9) selected = _FeasibleGrasp(candidate, 1, Pose(0.4, 0.0, 0.3), Pose(0.4, 0.0, 0.3)) robot_config = SimpleNamespace(pre_grasp_offset=0.1) - mocker.patch.object( - module, "_get_robot", return_value=("arm", "robot-id", robot_config) - ) + mocker.patch.object(module, "_get_robot", return_value=("arm", "robot-id", robot_config)) mocker.patch.object(module, "_require_pick_object", return_value=detection) mocker.patch.object(module, "_provider_candidates", return_value=[candidate]) mocker.patch.object(module, "_select_feasible_grasp", return_value=selected) @@ -902,9 +900,7 @@ def test_pick_selected_rejects_expired_preparation_without_robot_access( module.config.preparation_timeout = 1.0 get_robot = mocker.patch.object(module, "_get_robot") - with patch( - "dimos.manipulation.pick_and_place_module.time.monotonic", return_value=2.0 - ): + with patch("dimos.manipulation.pick_and_place_module.time.monotonic", return_value=2.0): result = module.pick_selected() assert result.error_code == "INVALID_STATE" @@ -917,7 +913,9 @@ def test_place_at_converts_object_reference_to_tcp_target( module._held_object_orientation = quarter_turn module._held_object_to_tcp = Pose(Vector3(0.1, 0.0, 0.0), Quaternion()) module._held_object_size = Vector3(0.05, 0.05, 0.1) - place = mocker.patch.object(module, "_place_with_orientation", return_value=SkillResult.ok()) + place = mocker.patch.object( + module, "_place_with_orientation", return_value=SkillResult.ok() + ) result = module.place_at(0.5, 0.2, 0.3) @@ -957,9 +955,7 @@ def test_destination_policy_computes_fit_checked_object_target( def test_destination_policy_rejects_object_that_does_not_fit( self, box_module: BoxFillingPickAndPlaceModule ) -> None: - box_module._replace_detection_snapshot( - [_make_det_object(name="box", size=(0.1, 0.1, 0.2))] - ) + box_module._replace_detection_snapshot([_make_det_object(name="box", size=(0.1, 0.1, 0.2))]) assert box_module.select_destination_container(1).is_success() box_module._held_object_size = Vector3(0.2, 0.04, 0.05)