diff --git a/dimos/control/blueprints/_hardware.py b/dimos/control/blueprints/_hardware.py deleted file mode 100644 index f652265b35..0000000000 --- a/dimos/control/blueprints/_hardware.py +++ /dev/null @@ -1,209 +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. - -"""Hardware component helpers for coordinator blueprints.""" - -from __future__ import annotations - -from dimos.control.components import ( - HardwareComponent, - HardwareType, - make_joints, - make_twist_base_joints, -) -from dimos.core.global_config import global_config -from dimos.utils.data import LfsPath - -PIPER_FK_MODEL = LfsPath("piper_description/mujoco_model/piper_no_gripper_description.xml") -XARM6_FK_MODEL = LfsPath("xarm_description/urdf/xarm6/xarm6.urdf") -XARM7_FK_MODEL = LfsPath("xarm_description/urdf/xarm7/xarm7.urdf") -A750_FK_MODEL = LfsPath("a750_description/urdf/a750_rev1_no_gripper.urdf") - -XARM7_SIM_PATH = LfsPath("xarm7/scene.xml") -XARM6_SIM_PATH = LfsPath("xarm6/scene.xml") -PIPER_SIM_PATH = LfsPath("piper/scene.xml") - - -def _adapter_kwargs(home_joints: list[float] | None = None) -> dict[str, object]: - if home_joints is None: - return {} - return {"initial_positions": home_joints} - - -def manipulator( - hw_id: str, - dof: int, - *, - adapter_type: str = "mock", - address: str | None = None, - gripper: bool = False, - gripper_suffix: str = "gripper", - auto_enable: bool = True, - adapter_kwargs: dict[str, object] | None = None, - home_joints: list[float] | None = None, -) -> HardwareComponent: - """Create a manipulator hardware component with DimOS joint names.""" - kwargs = _adapter_kwargs(home_joints) - if adapter_kwargs: - kwargs.update(adapter_kwargs) - return HardwareComponent( - hardware_id=hw_id, - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints(hw_id, dof), - adapter_type=adapter_type, - address=address, - auto_enable=auto_enable, - gripper_joints=[f"{hw_id}/{gripper_suffix}"] if gripper else [], - adapter_kwargs=kwargs, - ) - - -def mock_arm( - hw_id: str = "arm", - dof: int = 7, - *, - gripper: bool = False, - gripper_suffix: str = "gripper", - home_joints: list[float] | None = None, -) -> HardwareComponent: - """Mock manipulator with no real hardware.""" - return manipulator( - hw_id, - dof, - gripper=gripper, - gripper_suffix=gripper_suffix, - home_joints=home_joints, - ) - - -def xarm7( - hw_id: str = "arm", - *, - gripper: bool = False, - mock_without_address: bool = False, - home_joints: list[float] | None = None, -) -> HardwareComponent: - """xArm7 hardware, MuJoCo when --simulation is set, or mock if requested.""" - if global_config.simulation: - return manipulator( - hw_id, - 7, - adapter_type="sim_mujoco", - address=str(XARM7_SIM_PATH), - gripper=gripper, - home_joints=home_joints, - ) - address = global_config.xarm7_ip - if mock_without_address and not address: - return mock_arm(hw_id, 7, gripper=gripper, home_joints=home_joints) - return manipulator( - hw_id, - 7, - adapter_type="xarm", - address=address, - gripper=gripper, - home_joints=home_joints, - ) - - -def xarm6( - hw_id: str = "arm", - *, - gripper: bool = False, - mock_without_address: bool = False, - home_joints: list[float] | None = None, -) -> HardwareComponent: - """xArm6 hardware, MuJoCo when --simulation is set, or mock if requested.""" - if global_config.simulation: - return manipulator( - hw_id, - 6, - adapter_type="sim_mujoco", - address=str(XARM6_SIM_PATH), - gripper=gripper, - home_joints=home_joints, - ) - address = global_config.xarm6_ip - if mock_without_address and not address: - return mock_arm(hw_id, 6, gripper=gripper, home_joints=home_joints) - return manipulator( - hw_id, - 6, - adapter_type="xarm", - address=address, - gripper=gripper, - home_joints=home_joints, - ) - - -def piper( - hw_id: str = "arm", - *, - gripper: bool = True, - mock_without_address: bool = False, - home_joints: list[float] | None = None, -) -> HardwareComponent: - """Piper hardware, MuJoCo when --simulation is set, or mock if requested.""" - if global_config.simulation: - return manipulator( - hw_id, - 6, - adapter_type="sim_mujoco", - address=str(PIPER_SIM_PATH), - gripper=gripper, - home_joints=home_joints, - ) - address = global_config.can_port or "can0" - if mock_without_address and not global_config.can_port: - return mock_arm(hw_id, 6, gripper=gripper, home_joints=home_joints) - return manipulator( - hw_id, - 6, - adapter_type="piper", - address=address, - gripper=gripper, - home_joints=home_joints, - ) - - -def a750(hw_id: str = "arm", *, mock_without_address: bool = False) -> HardwareComponent: - """A-750 hardware or mock when no device path is configured.""" - home_joints = [0.0, 0.0, -1.5707963267948966, 0.0, 0.0, 0.0] - if mock_without_address and not global_config.device_path: - return mock_arm( - hw_id, - 6, - gripper=True, - gripper_suffix="finger", - home_joints=home_joints, - ) - return manipulator( - hw_id, - 6, - adapter_type="a750", - address=global_config.device_path or "/dev/ttyACM0", - gripper=True, - gripper_suffix="finger", - home_joints=home_joints, - ) - - -def mock_twist_base(hw_id: str = "base") -> HardwareComponent: - """Mock holonomic twist base (3-DOF: vx, vy, wz).""" - return HardwareComponent( - hardware_id=hw_id, - hardware_type=HardwareType.BASE, - joints=make_twist_base_joints(hw_id), - adapter_type="mock_twist_base", - ) diff --git a/dimos/control/blueprints/basic.py b/dimos/control/blueprints/basic.py index 214d01275e..40194bae95 100644 --- a/dimos/control/blueprints/basic.py +++ b/dimos/control/blueprints/basic.py @@ -12,117 +12,18 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Single-arm coordinator blueprints with trajectory control. - -Each arm blueprint switches between real hardware and MuJoCo via `--simulation`. +"""Basic control coordinator blueprint. Usage: - dimos run coordinator-mock # Mock 7-DOF arm - dimos run coordinator-xarm7 # XArm7 real - dimos --simulation run coordinator-xarm7 # XArm7 in MuJoCo - dimos run coordinator-xarm6 # XArm6 real - dimos --simulation run coordinator-xarm6 # XArm6 in MuJoCo - dimos run coordinator-piper # Piper real (CAN) - dimos --simulation run coordinator-piper # Piper in MuJoCo + dimos run coordinator-basic """ from __future__ import annotations -from dimos.control.blueprints._hardware import ( - PIPER_SIM_PATH, - XARM6_SIM_PATH, - XARM7_SIM_PATH, - mock_arm, - piper, - xarm6, - xarm7, -) -from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.core.coordination.blueprints import Blueprint, autoconnect -from dimos.core.global_config import global_config -from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule - -_is_sim = global_config.simulation - - -def _mujoco_if_sim(sim_path: str, dof: int) -> tuple[Blueprint, ...]: - if not _is_sim: - return () - return (MujocoSimModule.blueprint(address=sim_path, headless=False, dof=dof),) +from dimos.control.coordinator import ControlCoordinator - -# Minimal blueprint (no hardware, no tasks) coordinator_basic = ControlCoordinator.blueprint( tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", ) - -# Mock 7-DOF arm (for testing) -_mock_hw = mock_arm("arm", 7) - -coordinator_mock = ControlCoordinator.blueprint( - hardware=[_mock_hw], - tasks=[ - TaskConfig( - name="traj_arm", - type="trajectory", - joint_names=_mock_hw.joints, - priority=10, - ) - ], -) - -# XArm7 (real, or MuJoCo with --simulation) -_xarm7_hw = xarm7("arm") - -coordinator_xarm7 = autoconnect( - ControlCoordinator.blueprint( - hardware=[_xarm7_hw], - tasks=[ - TaskConfig( - name="traj_arm", - type="trajectory", - joint_names=_xarm7_hw.joints, - priority=10, - ) - ], - ), - *_mujoco_if_sim(str(XARM7_SIM_PATH), len(_xarm7_hw.joints)), -) - -# XArm6 (real, or MuJoCo with --simulation) -_xarm6_hw = xarm6("arm", gripper=True) - -coordinator_xarm6 = autoconnect( - ControlCoordinator.blueprint( - hardware=[_xarm6_hw], - tasks=[ - TaskConfig( - name="traj_xarm", - type="trajectory", - joint_names=_xarm6_hw.joints, - priority=10, - ) - ], - ), - *_mujoco_if_sim(str(XARM6_SIM_PATH), len(_xarm6_hw.joints)), -) - -# Piper 6-DOF (CAN bus, or MuJoCo with --simulation) -_piper_hw = piper("arm") - -coordinator_piper = autoconnect( - ControlCoordinator.blueprint( - hardware=[_piper_hw], - tasks=[ - TaskConfig( - name="traj_piper", - type="trajectory", - joint_names=_piper_hw.joints, - priority=10, - ) - ], - ), - *_mujoco_if_sim(str(PIPER_SIM_PATH), len(_piper_hw.joints)), -) diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index 64a9090899..ab0230ee64 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -26,10 +26,10 @@ import os -from dimos.control.blueprints._hardware import mock_arm from dimos.control.components import ( HardwareComponent, HardwareType, + make_joints, make_twist_base_joints, ) from dimos.control.coordinator import ControlCoordinator, TaskConfig @@ -195,7 +195,12 @@ def _flowbase_twist_base( # Mock arm (7-DOF) + mock holonomic base (3-DOF) -_mock_arm_hw = mock_arm("arm", 7) +_mock_arm_hw = HardwareComponent( + hardware_id="arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("arm", 7), + adapter_type="mock", +) coordinator_mobile_manip_mock = ControlCoordinator.blueprint( hardware=[_mock_arm_hw, _mock_twist_base()], diff --git a/dimos/control/blueprints/teleop.py b/dimos/control/blueprints/teleop.py deleted file mode 100644 index 993cc15267..0000000000 --- a/dimos/control/blueprints/teleop.py +++ /dev/null @@ -1,264 +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. - -"""Advanced control coordinator blueprints: servo, velocity, cartesian IK, and teleop IK. - -Teleop blueprints switch between MuJoCo and real hardware via `--simulation`. - -Usage: - dimos run coordinator-teleop-xarm6 # Real XArm6 (TeleopIK) - dimos --simulation run coordinator-teleop-xarm6 # XArm6 in MuJoCo sim - dimos run coordinator-teleop-xarm7 # Real XArm7 - dimos --simulation run coordinator-teleop-xarm7 # XArm7 in MuJoCo sim - dimos run coordinator-teleop-piper # Real Piper - dimos --simulation run coordinator-teleop-piper # Piper in MuJoCo sim - dimos run coordinator-servo-xarm6 # Servo streaming (real-only) - dimos run coordinator-velocity-xarm6 # Velocity streaming (real-only) - dimos run coordinator-combined-xarm6 # Servo + velocity (real-only) - dimos run coordinator-cartesian-ik-mock # Cartesian IK (mock) - dimos run coordinator-cartesian-ik-piper # Cartesian IK (Piper, real-only) - dimos run coordinator-teleop-dual # TeleopIK dual arm (real-only) -""" - -from __future__ import annotations - -from dimos.control.blueprints._hardware import ( - PIPER_FK_MODEL, - PIPER_SIM_PATH, - XARM6_FK_MODEL, - XARM6_SIM_PATH, - XARM7_FK_MODEL, - XARM7_SIM_PATH, - manipulator, - mock_arm, - piper, - xarm6, - xarm7, -) -from dimos.control.components import make_gripper_joints -from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.core.coordination.blueprints import Blueprint, autoconnect -from dimos.core.global_config import global_config -from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule - -_is_sim = global_config.simulation - - -def _mujoco_if_sim(sim_path: str, dof: int) -> tuple[Blueprint, ...]: - if not _is_sim: - return () - return (MujocoSimModule.blueprint(address=sim_path, headless=False, dof=dof),) - - -_xarm6_hw = manipulator( - "arm", - 6, - adapter_type="xarm", - address=global_config.xarm6_ip, - gripper=True, -) -_piper_hw = manipulator( - "arm", - 6, - adapter_type="piper", - address=global_config.can_port or "can0", - gripper=True, -) - -_xarm7_teleop_hw = xarm7("arm", gripper=True) -_xarm6_teleop_hw = xarm6("arm", gripper=True) -_piper_teleop_hw = piper("arm") - -# XArm6 servo - streaming position control -coordinator_servo_xarm6 = ControlCoordinator.blueprint( - hardware=[_xarm6_hw], - tasks=[ - TaskConfig( - name="servo_arm", - type="servo", - joint_names=_xarm6_hw.joints, - priority=10, - ), - ], -) - -# XArm6 velocity control - streaming velocity for joystick -coordinator_velocity_xarm6 = ControlCoordinator.blueprint( - hardware=[_xarm6_hw], - tasks=[ - TaskConfig( - name="velocity_arm", - type="velocity", - joint_names=_xarm6_hw.joints, - priority=10, - ), - ], -) - -# XArm6 combined (servo + velocity tasks) -coordinator_combined_xarm6 = ControlCoordinator.blueprint( - hardware=[_xarm6_hw], - tasks=[ - TaskConfig( - name="servo_arm", - type="servo", - joint_names=_xarm6_hw.joints, - priority=10, - ), - TaskConfig( - name="velocity_arm", - type="velocity", - joint_names=_xarm6_hw.joints, - priority=10, - ), - ], -) - -# Mock 6-DOF arm with CartesianIK -_mock_6dof_hw = mock_arm("arm", 6) - -coordinator_cartesian_ik_mock = ControlCoordinator.blueprint( - hardware=[_mock_6dof_hw], - tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_mock_6dof_hw.joints, - priority=10, - params={"model_path": PIPER_FK_MODEL, "ee_joint_id": 6}, - ), - ], -) - -# Piper arm with CartesianIK -coordinator_cartesian_ik_piper = ControlCoordinator.blueprint( - hardware=[_piper_hw], - tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_piper_hw.joints, - priority=10, - params={"model_path": PIPER_FK_MODEL, "ee_joint_id": 6}, - ), - ], -) - -# XArm7 with TeleopIK (real, or MuJoCo with --simulation) -coordinator_teleop_xarm7 = autoconnect( - ControlCoordinator.blueprint( - hardware=[_xarm7_teleop_hw], - tasks=[ - TaskConfig( - name="teleop_xarm", - type="teleop_ik", - joint_names=_xarm7_teleop_hw.joints, - priority=10, - params={ - "model_path": XARM7_FK_MODEL, - "ee_joint_id": 7, - "hand": "right", - "gripper_joint": make_gripper_joints("arm")[0], - "gripper_open_pos": 0.85, - "gripper_closed_pos": 0.0, - }, - ), - ], - ), - *_mujoco_if_sim(str(XARM7_SIM_PATH), len(_xarm7_teleop_hw.joints)), -) - -# XArm6 with TeleopIK (real, or MuJoCo with --simulation) -coordinator_teleop_xarm6 = autoconnect( - ControlCoordinator.blueprint( - hardware=[_xarm6_teleop_hw], - tasks=[ - TaskConfig( - name="teleop_xarm", - type="teleop_ik", - joint_names=_xarm6_teleop_hw.joints, - priority=10, - params={ - "model_path": XARM6_FK_MODEL, - "ee_joint_id": 6, - "hand": "right", - "gripper_joint": make_gripper_joints("arm")[0], - "gripper_open_pos": 0.85, - "gripper_closed_pos": 0.0, - }, - ), - ], - ), - *_mujoco_if_sim(str(XARM6_SIM_PATH), len(_xarm6_teleop_hw.joints)), -) - -# Piper with TeleopIK (real, or MuJoCo with --simulation) -coordinator_teleop_piper = autoconnect( - ControlCoordinator.blueprint( - hardware=[_piper_teleop_hw], - tasks=[ - TaskConfig( - name="teleop_piper", - type="teleop_ik", - joint_names=_piper_teleop_hw.joints, - priority=10, - params={ - "model_path": PIPER_FK_MODEL, - "ee_joint_id": 6, - "hand": "left", - "gripper_joint": make_gripper_joints("arm")[0], - "gripper_open_pos": 0.0, - "gripper_closed_pos": 0.035, - }, - ), - ], - ), - *_mujoco_if_sim(str(PIPER_SIM_PATH), len(_piper_teleop_hw.joints)), -) - -# Dual arm teleop: XArm6 + Piper with TeleopIK (real-only) -_xarm6_dual_hw = manipulator( - "xarm_arm", - 6, - adapter_type="xarm", - address=global_config.xarm6_ip, - gripper=True, -) -_piper_dual_hw = manipulator( - "piper_arm", - 6, - adapter_type="piper", - address=global_config.can_port, - gripper=True, -) - -coordinator_teleop_dual = ControlCoordinator.blueprint( - hardware=[_xarm6_dual_hw, _piper_dual_hw], - tasks=[ - TaskConfig( - name="teleop_xarm", - type="teleop_ik", - joint_names=_xarm6_dual_hw.joints, - priority=10, - params={"model_path": XARM6_FK_MODEL, "ee_joint_id": 6, "hand": "left"}, - ), - TaskConfig( - name="teleop_piper", - type="teleop_ik", - joint_names=_piper_dual_hw.joints, - priority=10, - params={"model_path": PIPER_FK_MODEL, "ee_joint_id": 6, "hand": "right"}, - ), - ], -) diff --git a/dimos/control/blueprints/test_dual.py b/dimos/control/blueprints/test_dual.py index 082b6ec8a0..f83eb54ea7 100644 --- a/dimos/control/blueprints/test_dual.py +++ b/dimos/control/blueprints/test_dual.py @@ -14,23 +14,16 @@ """Tests for dual-arm control blueprints.""" -import pytest - -from dimos.control.blueprints.dual import coordinator_dual_xarm from dimos.control.coordinator import ControlCoordinator from dimos.core.coordination.blueprints import Blueprint from dimos.manipulation.manipulation_module import ManipulationModule -from dimos.manipulation.planning.groups.identifiers import make_global_joint_names +from dimos.robot.manipulators.xarm.blueprints.basic import coordinator_dual_xarm + +def _dual_xarm6_planner() -> Blueprint: + from dimos.robot.manipulators.xarm.blueprints.basic import dual_xarm6_planner -def _dual_xarm6_planner_coordinator() -> Blueprint: - try: - from dimos.manipulation.blueprints import dual_xarm6_planner_coordinator - except RuntimeError as exc: - if "Failed to pull LFS file" in str(exc): - pytest.skip(f"xArm LFS data unavailable: {exc}") - raise - return dual_xarm6_planner_coordinator + return dual_xarm6_planner def _coordinator_task_names(blueprint) -> list[str]: @@ -53,32 +46,29 @@ def _manipulation_visualization(blueprint): return atom.kwargs["visualization"] -def test_dual_xarm6_integrated_blueprint_has_planner_and_coordinator() -> None: - dual_xarm6_planner_coordinator = _dual_xarm6_planner_coordinator() - modules = [atom.module for atom in dual_xarm6_planner_coordinator.blueprints] +def test_dual_xarm6_planner_blueprint_has_planner() -> None: + dual_xarm6_planner = _dual_xarm6_planner() + modules = [atom.module for atom in dual_xarm6_planner.blueprints] assert ManipulationModule in modules - assert ControlCoordinator in modules -def test_dual_xarm6_integrated_blueprint_uses_viser_for_execution_ui() -> None: - dual_xarm6_planner_coordinator = _dual_xarm6_planner_coordinator() - visualization = _manipulation_visualization(dual_xarm6_planner_coordinator) +def test_dual_xarm6_planner_uses_meshcat_visualization() -> None: + dual_xarm6_planner = _dual_xarm6_planner() + visualization = _manipulation_visualization(dual_xarm6_planner) - assert visualization == {"backend": "viser", "allow_plan_execute": True} + assert visualization == {"backend": "meshcat"} -def test_dual_xarm6_integrated_tasks_match_planner_robots() -> None: - dual_xarm6_planner_coordinator = _dual_xarm6_planner_coordinator() - tasks_by_name = {task.name: task for task in _coordinator_tasks(dual_xarm6_planner_coordinator)} +def test_dual_xarm6_planner_robots_use_global_joint_mapping() -> None: + dual_xarm6_planner = _dual_xarm6_planner() - for robot in _manipulation_robots(dual_xarm6_planner_coordinator): - task = tasks_by_name[robot.coordinator_task_name] - assert task.joint_names == make_global_joint_names(robot.name, robot.joint_names) + for robot in _manipulation_robots(dual_xarm6_planner): + assert robot.coordinator_task_name == f"traj_{robot.name}" def test_dual_coordinator_xarm_task_names_match_manipulation_robot_defaults() -> None: assert _coordinator_task_names(coordinator_dual_xarm) == [ - "traj_left_arm", - "traj_right_arm", + "traj_left", + "traj_right", ] diff --git a/dimos/e2e_tests/test_control_coordinator.py b/dimos/e2e_tests/test_control_coordinator.py index 8d635805b8..40e4800b47 100644 --- a/dimos/e2e_tests/test_control_coordinator.py +++ b/dimos/e2e_tests/test_control_coordinator.py @@ -201,8 +201,8 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: """Test dual-arm coordinator with independent trajectories.""" lcm_spy.save_topic("/coordinator_joint_state#sensor_msgs.JointState") - # Start integrated dual-arm mock planner/coordinator - start_blueprint("dual-xarm6-planner-coordinator") + # Start dual-arm mock coordinator + start_blueprint("coordinator-dual-mock") lcm_spy.wait_for_saved_topic("/coordinator_joint_state#sensor_msgs.JointState") client = RPCClient(None, ControlCoordinator) @@ -213,15 +213,15 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: assert "right_arm/joint1" in joints tasks = client.list_tasks() - assert "traj_left_arm" in tasks - assert "traj_right_arm" in tasks + assert "traj_left" in tasks + assert "traj_right" in tasks # Create trajectories for both arms left_trajectory = JointTrajectory( - joint_names=[f"left_arm/joint{i + 1}" for i in range(6)], + joint_names=[f"left_arm/joint{i + 1}" for i in range(7)], points=[ - TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 6), - TrajectoryPoint(time_from_start=0.5, positions=[0.2] * 6), + TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 7), + TrajectoryPoint(time_from_start=0.5, positions=[0.2] * 7), ], ) @@ -235,11 +235,10 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: # Execute both via task_invoke assert ( - client.task_invoke("traj_left_arm", "execute", {"trajectory": left_trajectory}) - is True + client.task_invoke("traj_left", "execute", {"trajectory": left_trajectory}) is True ) assert ( - client.task_invoke("traj_right_arm", "execute", {"trajectory": right_trajectory}) + client.task_invoke("traj_right", "execute", {"trajectory": right_trajectory}) is True ) @@ -247,8 +246,8 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: time.sleep(1.0) # Both should complete - left_state = client.task_invoke("traj_left_arm", "get_state") - right_state = client.task_invoke("traj_right_arm", "get_state") + left_state = client.task_invoke("traj_left", "get_state") + right_state = client.task_invoke("traj_right", "get_state") assert left_state == TrajectoryState.COMPLETED assert right_state == TrajectoryState.COMPLETED diff --git a/dimos/manipulation/blueprints.py b/dimos/manipulation/blueprints.py index 5d6d215025..0dd9edbd9e 100644 --- a/dimos/manipulation/blueprints.py +++ b/dimos/manipulation/blueprints.py @@ -12,467 +12,22 @@ # See the License for the specific language governing permissions and # limitations under the License. -""" -Manipulation blueprints. - -Quick start: - # 1. Verify manipulation deps load correctly (standalone, no hardware): - dimos run xarm6-planner-only - - # 2. Keyboard teleop with mock arm: - dimos run keyboard-teleop-xarm7 +"""Compatibility exports for manipulation blueprints. - # 3. Interactive RPC client (plan, preview, execute from Python): - dimos run xarm7-planner-coordinator - python -i -m dimos.manipulation.planning.examples.manipulation_client - - # 4. Dual-arm xArm7 mock planner + coordinator demo: - dimos run dual-xarm7-planner-coordinator - python -i -m dimos.manipulation.planning.examples.manipulation_client +Robot-owned manipulation blueprints now live under ``dimos.robot.manipulators``. """ -import math -from pathlib import Path -from typing import Any - -from dimos.agents.mcp.mcp_client import McpClient -from dimos.agents.mcp.mcp_server import McpServer -from dimos.control.blueprints._hardware import XARM7_SIM_PATH, manipulator -from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.core.coordination.blueprints import autoconnect -from dimos.core.global_config import global_config -from dimos.core.transport import LCMTransport -from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera -from dimos.manipulation.manipulation_module import ManipulationModule -from dimos.manipulation.pick_and_place_module import PickAndPlaceModule -from dimos.manipulation.planning.spec.config import RobotModelConfig -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.JointState import JointState -from dimos.perception.object_scene_registration import ObjectSceneRegistrationModule -from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule -from dimos.utils.data import LfsPath -from dimos.visualization.rerun.bridge import RerunBridgeModule - -XARM_GRIPPER_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ - ("right_inner_knuckle", "right_outer_knuckle"), - ("left_inner_knuckle", "left_outer_knuckle"), - ("right_inner_knuckle", "right_finger"), - ("left_inner_knuckle", "left_finger"), - ("left_finger", "right_finger"), - ("left_outer_knuckle", "right_outer_knuckle"), - ("left_inner_knuckle", "right_inner_knuckle"), - ("left_outer_knuckle", "right_finger"), - ("right_outer_knuckle", "left_finger"), - ("xarm_gripper_base_link", "left_inner_knuckle"), - ("xarm_gripper_base_link", "right_inner_knuckle"), - ("xarm_gripper_base_link", "left_finger"), - ("xarm_gripper_base_link", "right_finger"), - ("link6", "xarm_gripper_base_link"), - ("link6", "left_outer_knuckle"), - ("link6", "right_outer_knuckle"), -] - -_XARM_MODEL_PATH = LfsPath("xarm_description") / "urdf/xarm_device.urdf.xacro" -_XARM_PACKAGE_PATHS: dict[str, Path] = {"xarm_description": LfsPath("xarm_description")} - - -def _base_pose( - x: float = 0.0, - y: float = 0.0, - z: float = 0.0, - pitch: float = 0.0, -) -> PoseStamped: - half_pitch = pitch / 2.0 - return PoseStamped( - position=Vector3(x=x, y=y, z=z), - orientation=Quaternion(0.0, math.sin(half_pitch), 0.0, math.cos(half_pitch)), - ) - - -def _make_xarm_model_config( - name: str, - dof: int, - *, - add_gripper: bool = True, - x_offset: float = 0.0, - y_offset: float = 0.0, - z_offset: float = 0.0, - pitch: float = 0.0, - tf_extra_links: list[str] | None = None, - home_joints: list[float] | None = None, - pre_grasp_offset: float = 0.10, - **overrides: Any, -) -> RobotModelConfig: - xacro_args = { - "dof": str(dof), - "limited": "true", - "attach_xyz": "0 0 0", - "attach_rpy": "0 0 0", - } - if add_gripper: - xacro_args["add_gripper"] = "true" - - defaults: dict[str, Any] = { - "name": name, - "model_path": _XARM_MODEL_PATH, - "base_pose": _base_pose(x_offset, y_offset, z_offset, pitch), - "strip_model_world_joint": True, - "joint_names": [f"joint{i}" for i in range(1, dof + 1)], - "end_effector_link": "link_tcp" if add_gripper else f"link{dof}", - "base_link": "link_base", - "package_paths": _XARM_PACKAGE_PATHS, - "xacro_args": xacro_args, - "auto_convert_meshes": True, - "collision_exclusion_pairs": XARM_GRIPPER_COLLISION_EXCLUSIONS if add_gripper else [], - "coordinator_task_name": f"traj_{name}", - "gripper_hardware_id": name if add_gripper else None, - "tf_extra_links": tf_extra_links or [], - "home_joints": home_joints or [0.0] * dof, - "pre_grasp_offset": pre_grasp_offset, - } - defaults.update(overrides) - return RobotModelConfig(**defaults) - - -def _make_xarm6_model_config(name: str = "arm", **kwargs: Any) -> RobotModelConfig: - return _make_xarm_model_config(name, 6, **kwargs) - - -def _make_xarm7_model_config(name: str = "arm", **kwargs: Any) -> RobotModelConfig: - return _make_xarm_model_config(name, 7, **kwargs) - - -def _trajectory_task(name: str, joint_names: list[str]) -> TaskConfig: - return TaskConfig( - name=f"traj_{name}", - type="trajectory", - joint_names=joint_names, - priority=10, - ) - - -# Single XArm6 planner (standalone, no coordinator) -xarm6_planner_only = ManipulationModule.blueprint( - robots=[_make_xarm6_model_config(name="arm")], - planning_timeout=10.0, - visualization={"backend": "meshcat"}, -) - - -# Dual XArm6 planner + coordinator with Viser execution UI. -_left_arm_hw = manipulator( - "left_arm", - 6, - adapter_type="xarm" if global_config.xarm6_ip else "mock", - address=global_config.xarm6_ip, -) -_right_arm_hw = manipulator( - "right_arm", - 6, - adapter_type="xarm" if global_config.xarm6_ip else "mock", - address=global_config.xarm6_ip, -) - -dual_xarm6_planner_coordinator = autoconnect( - ManipulationModule.blueprint( - robots=[ - _make_xarm6_model_config(name="left_arm", y_offset=0.5), - _make_xarm6_model_config(name="right_arm", y_offset=-0.5), - ], - planning_timeout=10.0, - visualization={"backend": "viser", "allow_plan_execute": True}, - ), - ControlCoordinator.blueprint( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[ - _left_arm_hw, - _right_arm_hw, - ], - tasks=[ - _trajectory_task("left_arm", _left_arm_hw.joints), - _trajectory_task("right_arm", _right_arm_hw.joints), - ], - ), -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - } -) - - -# Single XArm7 planner + coordinator (uses real hardware when XARM7_IP is set) -# Usage: XARM7_IP= dimos run xarm7-planner-coordinator -_xarm7_hw = manipulator( - "arm", - 7, - adapter_type="xarm" if global_config.xarm7_ip else "mock", - address=global_config.xarm7_ip, - gripper=True, +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, ) - -xarm7_planner_coordinator = autoconnect( - ManipulationModule.blueprint( - robots=[_make_xarm7_model_config(name="arm", add_gripper=True)], - planning_timeout=10.0, - visualization={"backend": "meshcat"}, - ), - ControlCoordinator.blueprint( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[_xarm7_hw], - tasks=[_trajectory_task("arm", _xarm7_hw.joints)], - ), -) - - -# Dual XArm7 mock planner + coordinator for bimanual planning demos. -# Usage: dimos run dual-xarm7-planner-coordinator -_left_xarm7_hw = manipulator( - "left_arm", - 7, - adapter_type="mock", -) -_right_xarm7_hw = manipulator( - "right_arm", - 7, - adapter_type="mock", -) - -dual_xarm7_planner_coordinator = autoconnect( - ManipulationModule.blueprint( - robots=[ - _make_xarm7_model_config(name="left_arm", add_gripper=False, y_offset=0.5), - _make_xarm7_model_config(name="right_arm", add_gripper=False, y_offset=-0.5), - ], - planning_timeout=10.0, - visualization={"backend": "viser", "allow_plan_execute": True}, - ), - ControlCoordinator.blueprint( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[ - _left_xarm7_hw, - _right_xarm7_hw, - ], - tasks=[ - _trajectory_task("left_arm", _left_xarm7_hw.joints), - _trajectory_task("right_arm", _right_xarm7_hw.joints), - ], - ), -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - } +from dimos.robot.manipulators.xarm.blueprints.basic import ( + dual_xarm6_planner as dual_xarm6_planner, + xarm6_planner_only as xarm6_planner_only, + xarm7_planner_coordinator as xarm7_planner_coordinator, ) - - -# XArm7 planner + LLM agent for testing base ManipulationModule skills -# No perception — uses the base module's planning + gripper skills only. -# Usage: dimos run coordinator-mock, then dimos run xarm7-planner-coordinator-agent -_BASE_MANIPULATION_AGENT_SYSTEM_PROMPT = """\ -You are a robotic manipulation assistant controlling an xArm7 robot arm. - -Available skills: -- get_robot_state: Get current joint positions, end-effector pose, and gripper state. -- move_to_pose: Move end-effector to ABSOLUTE x, y, z (meters) with optional roll, pitch, yaw (radians). -- move_to_joints: Move to a joint configuration (comma-separated radians). -- open_gripper / close_gripper / set_gripper: Control the gripper. -- go_home: Move to the home/observe position. -- go_init: Return to the startup position. -- reset: Clear a FAULT state and return to IDLE. Use this when a motion fails. - -COORDINATE SYSTEM (world frame, meters): -- X axis = forward (away from the robot base) -- Y axis = left -- Z axis = up -- Z=0 is the robot base level; typical working height is Z = 0.2-0.5 - -CRITICAL WORKFLOW for relative movement requests (e.g. "move 20cm forward"): -1. Call get_robot_state to get the current EE pose. -2. Add the requested offset to the CURRENT position. Example: if EE is at \ -(0.3, 0.0, 0.4) and user says "move 20cm forward", target is (0.5, 0.0, 0.4). -3. Call move_to_pose with the computed ABSOLUTE target. -NEVER pass only the offset as coordinates — that would send the robot to near-origin. - -ERROR RECOVERY: If a motion fails or the state becomes FAULT, call reset before retrying. -""" - -xarm7_planner_coordinator_agent = autoconnect( - xarm7_planner_coordinator, - McpServer.blueprint(), - McpClient.blueprint(system_prompt=_BASE_MANIPULATION_AGENT_SYSTEM_PROMPT), -) - - -# XArm7 with eye-in-hand RealSense camera for perception-based manipulation -# TF chain: world → link7 (ManipulationModule) → camera_link (RealSense) -# Usage: dimos run coordinator-mock, then dimos run xarm-perception -_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, - ), - 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) - - -# XArm7 perception + LLM agent for agentic manipulation. -# Skills (pick, place, move_to_pose, etc.) auto-register with the agent's SkillCoordinator. -# Usage: XARM7_IP= dimos run coordinator-xarm7 xarm-perception-agent -_MANIPULATION_AGENT_SYSTEM_PROMPT = """\ -You are a robotic manipulation assistant controlling an xArm7 robot arm with an \ -eye-in-hand RealSense camera and a gripper. - -# Skills - -## Perception -- **look**: Quick snapshot of objects visible from the current camera pose. Does NOT \ -move the arm. Example: "what do you see?", "what's on the table?" -- **scan_objects**: Full scan — moves the arm to the init position for a clear view, \ -then refreshes detections. Use before pick/place, after a failed grasp, or when the \ -user explicitly asks to scan. Example: "scan the table", "what objects are there?" - -## 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" -- **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 \ -world-frame pose (meters / radians). -- **move_to_joints **: Move to a joint configuration (radians). -- **go_home**: Move to the home/observe position. -- **go_init**: Return to the startup position. Use after pick/place as a safe resting pose. - -## Gripper -- **open_gripper / close_gripper / set_gripper**: Direct gripper control. - -## Status & Recovery -- **get_robot_state**: Current joint positions, end-effector pose, and gripper state. -- **get_scene_info**: Full robot state, detected objects, and scene overview. -- **reset**: Clear a FAULT state and return to IDLE. Available as both a skill and RPC. -- **clear_perception_obstacles**: Remove detected obstacles from the planning world. \ -Use when planning fails with COLLISION_AT_START. - -# Choosing look vs scan_objects -- "what can you see?" / "what's there?" → **look** (instant, no movement) -- "scan the scene" / before pick-and-place → **scan_objects** (thorough, moves arm) -- If objects were ALREADY detected by a previous look, do NOT scan again — just proceed. - -# Rules -- Use the EXACT object name from detection output. Do NOT substitute similar names \ -(e.g. if detection says "spray can", do not use "grinder"). -- "drop it in/on [object]" → use **drop_on**. "place it at [coords]" → use **place**. -- "bring it back" → pick, then **go_init**. Do NOT place randomly. -- "bring it to me" / "hand it over" → pick, then move toward user (≈ X=0, Y=0.5). -- 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. - -# 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. -""" - -xarm_perception_agent = autoconnect( - xarm_perception, - McpServer.blueprint(), - McpClient.blueprint(system_prompt=_MANIPULATION_AGENT_SYSTEM_PROMPT), -) - - -# Sim perception: MujocoSimModule owns the MujocoEngine and publishes both -# camera streams and joint state via shared memory. -# ShmMujocoAdapter attaches to the same SHM buffers by MJCF path. -_xarm7_sim_home = [0.0, 0.0, 0.0, 0.0, 0.0, -0.7, 0.0] -_xarm7_sim_hw = manipulator( - "arm", - 7, - adapter_type="sim_mujoco", - address=str(XARM7_SIM_PATH), - gripper=True, - home_joints=_xarm7_sim_home, -) - -xarm_perception_sim = autoconnect( - PickAndPlaceModule.blueprint( - robots=[ - _make_xarm7_model_config( - name="arm", - add_gripper=True, - pitch=math.radians(45), - tf_extra_links=["link7"], - home_joints=_xarm7_sim_home, - pre_grasp_offset=0.05, - ) - ], - planning_timeout=10.0, - visualization={"backend": "meshcat"}, - ), - MujocoSimModule.blueprint( - address=str(XARM7_SIM_PATH), - headless=False, - dof=7, - camera_name="wrist_camera", - base_frame_id="link7", - ), - ObjectSceneRegistrationModule.blueprint(target_frame="world"), - ControlCoordinator.blueprint( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[_xarm7_sim_hw], - tasks=[_trajectory_task("arm", _xarm7_sim_hw.joints)], - ), - RerunBridgeModule.blueprint(), -) - - -xarm_perception_sim_agent = autoconnect( - xarm_perception_sim, - McpServer.blueprint(), - McpClient.blueprint(system_prompt=_MANIPULATION_AGENT_SYSTEM_PROMPT), +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, ) diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 6e67bd95e4..0bcd4c0db9 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -45,12 +45,12 @@ assert_local_joint_names, make_global_joint_names, ) -from dimos.manipulation.planning.groups.models import PlanningGroup -from dimos.manipulation.planning.groups.utils import ( +from dimos.manipulation.planning.groups.joints import ( filter_joint_state_to_selected_joints, joint_target_to_global_names, planning_group_id_from_selector, ) +from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.manipulation.planning.kinematics.config import ( ManipulationKinematicsConfig, PinkKinematicsConfig, @@ -849,10 +849,11 @@ def plan_to_pose_targets( auxiliary_groups: Sequence[PlanningGroupID | PlanningGroup] = (), ) -> bool: """Plan to one or more group pose targets with optional auxiliary groups.""" - if self._world_monitor is None or self._kinematics is None: + if self._world_monitor is None or self._kinematics is None or self._planner is None: return False if not pose_targets: - return self._fail("At least one pose target is required") + logger.error("At least one pose target is required") + return False stamped_targets = { planning_group_id_from_selector(group): PoseStamped( @@ -909,7 +910,8 @@ def plan_to_joint_targets( if self._world_monitor is None or self._planner is None: return False if not joint_targets: - return self._fail("At least one joint target is required") + logger.error("At least one joint target is required") + return False group_ids = tuple( dict.fromkeys(planning_group_id_from_selector(group) for group in joint_targets) @@ -1026,6 +1028,7 @@ def get_robot_info(self, robot_name: RobotName | None = None) -> RobotInfo | Non "base_link": config.base_link, "max_velocity": config.max_velocity, "max_acceleration": config.max_acceleration, + "has_joint_name_mapping": bool(config.joint_name_mapping), "coordinator_task_name": config.coordinator_task_name, "home_joints": config.home_joints, "pre_grasp_offset": config.pre_grasp_offset, diff --git a/dimos/manipulation/planning/groups/identifiers.py b/dimos/manipulation/planning/groups/identifiers.py index db9d990081..7ce0fc0774 100644 --- a/dimos/manipulation/planning/groups/identifiers.py +++ b/dimos/manipulation/planning/groups/identifiers.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Planning-group and global-joint identifier helpers.""" +"""String grammar helpers for public manipulation identifiers.""" from __future__ import annotations @@ -74,7 +74,7 @@ def make_global_joint_name( def make_global_joint_names( robot_name: RobotName, - local_joint_names: list[LocalModelJointName] | tuple[LocalModelJointName, ...], + local_joint_names: Sequence[LocalModelJointName], ) -> list[GlobalJointName]: """Convert local model joint names to public global joint names.""" return [make_global_joint_name(robot_name, name) for name in local_joint_names] @@ -86,6 +86,28 @@ def is_global_joint_name(name: str) -> bool: return len(parts) == 2 and bool(parts[0]) and bool(parts[1]) +def parse_global_joint_name( + global_joint_name: GlobalJointName, +) -> tuple[RobotName, LocalModelJointName]: + """Split and validate a global joint name.""" + parts = global_joint_name.split("/", maxsplit=1) + if len(parts) != 2: + raise ValueError( + f"Invalid global joint name {global_joint_name!r}; " + "expected '{robot_name}/{local_joint_name}'" + ) + robot_name, local_name = parts + try: + assert_valid_robot_name(robot_name) + assert_valid_local_joint_name(local_name) + except ValueError as exc: + raise ValueError( + f"Invalid global joint name {global_joint_name!r}; " + "expected '{robot_name}/{local_joint_name}'" + ) from exc + return robot_name, local_name + + def assert_global_joint_names(names: Sequence[GlobalJointName]) -> None: """Validate that names are global joint names.""" invalid = [name for name in names if not is_global_joint_name(name)] @@ -99,14 +121,9 @@ def local_joint_name_from_global( ) -> LocalModelJointName: """Validate and strip a global joint name for backend internals.""" assert_valid_robot_name(robot_name) - prefix = f"{robot_name}/" - if not global_joint_name.startswith(prefix): + parsed_robot_name, local_name = parse_global_joint_name(global_joint_name) + if parsed_robot_name != robot_name: raise ValueError( f"Global joint name {global_joint_name!r} does not belong to robot {robot_name!r}" ) - local_name = global_joint_name[len(prefix) :] - try: - assert_valid_local_joint_name(local_name) - except ValueError as exc: - raise ValueError(f"Invalid global joint name: {global_joint_name!r}") from exc return local_name diff --git a/dimos/manipulation/planning/groups/utils.py b/dimos/manipulation/planning/groups/joints.py similarity index 98% rename from dimos/manipulation/planning/groups/utils.py rename to dimos/manipulation/planning/groups/joints.py index e55f997a33..c66b4410d6 100644 --- a/dimos/manipulation/planning/groups/utils.py +++ b/dimos/manipulation/planning/groups/joints.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Shared helpers for planning-group selectors and joint-state projection.""" +"""Planning-group joint target and joint-state helpers.""" from collections.abc import Mapping, Sequence diff --git a/dimos/manipulation/planning/kinematics/jacobian_ik.py b/dimos/manipulation/planning/kinematics/jacobian_ik.py index 9988b2009c..e250fd26d1 100644 --- a/dimos/manipulation/planning/kinematics/jacobian_ik.py +++ b/dimos/manipulation/planning/kinematics/jacobian_ik.py @@ -30,8 +30,8 @@ import numpy as np +from dimos.manipulation.planning.groups.joints import filter_joint_state_to_selected_joints from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection -from dimos.manipulation.planning.groups.utils import filter_joint_state_to_selected_joints from dimos.manipulation.planning.spec.enums import IKStatus from dimos.manipulation.planning.spec.models import ( IKResult, diff --git a/dimos/manipulation/planning/kinematics/pink_ik.py b/dimos/manipulation/planning/kinematics/pink_ik.py index ef46cad981..f5169d140b 100644 --- a/dimos/manipulation/planning/kinematics/pink_ik.py +++ b/dimos/manipulation/planning/kinematics/pink_ik.py @@ -24,9 +24,9 @@ import numpy as np from dimos.manipulation.planning.groups.identifiers import make_global_joint_name +from dimos.manipulation.planning.groups.joints import matching_global_joint_name from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection from dimos.manipulation.planning.groups.registry import PlanningGroupRegistry -from dimos.manipulation.planning.groups.utils import matching_global_joint_name from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus diff --git a/dimos/manipulation/planning/planning_identifiers.py b/dimos/manipulation/planning/planning_identifiers.py deleted file mode 100644 index 1acd15e824..0000000000 --- a/dimos/manipulation/planning/planning_identifiers.py +++ /dev/null @@ -1,31 +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 shim for planning-group identifier helpers. - -New code should import from ``dimos.manipulation.planning.groups.identifiers``. -""" - -from dimos.manipulation.planning.groups.identifiers import ( # noqa: F401 - assert_global_joint_names, - assert_local_joint_names, - assert_valid_local_joint_name, - assert_valid_robot_name, - is_global_joint_name, - local_joint_name_from_global, - make_global_joint_name, - make_global_joint_names, - make_planning_group_id, - parse_planning_group_id, -) diff --git a/dimos/manipulation/planning/spec/config.py b/dimos/manipulation/planning/spec/config.py index 686fb652ac..7ecca4d191 100644 --- a/dimos/manipulation/planning/spec/config.py +++ b/dimos/manipulation/planning/spec/config.py @@ -43,6 +43,9 @@ class RobotModelConfig(ModuleConfig): stripped when strip_model_world_joint is true. joint_names: Ordered list of controllable joints in the local model namespace. This is not a planning group. + joint_name_mapping: Optional mapping from external/coordinator joint + names to local model joint names for hardware adapters that publish + scoped joint names. end_effector_link: Compatibility robot-scoped end-effector link used by legacy helpers. New pose-targeted planning should use planning group target frames instead. @@ -70,6 +73,7 @@ class RobotModelConfig(ModuleConfig): base_pose: PoseStamped = Field(default_factory=PoseStamped) strip_model_world_joint: bool = False joint_names: list[str] + joint_name_mapping: dict[str, str] = Field(default_factory=dict) end_effector_link: str | None = None base_link: str = "base_link" planning_groups: list[PlanningGroupDefinition] = Field(default_factory=list) diff --git a/dimos/manipulation/planning/test_planning_group_identifiers.py b/dimos/manipulation/planning/test_planning_group_identifiers.py new file mode 100644 index 0000000000..d728123e8f --- /dev/null +++ b/dimos/manipulation/planning/test_planning_group_identifiers.py @@ -0,0 +1,39 @@ +# 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. + +"""Unit tests for planning group identifier grammar.""" + +from __future__ import annotations + +import pytest + +from dimos.manipulation.planning.groups.identifiers import ( + local_joint_name_from_global, + parse_global_joint_name, +) + + +def test_parse_global_joint_name_returns_robot_and_local_joint() -> None: + assert parse_global_joint_name("left_arm/joint1") == ("left_arm", "joint1") + + +@pytest.mark.parametrize("name", ["joint1", "/joint1", "left_arm/", "left_arm/foo/bar"]) +def test_parse_global_joint_name_rejects_invalid_names(name: str) -> None: + with pytest.raises(ValueError, match="Invalid global joint name"): + parse_global_joint_name(name) + + +def test_local_joint_name_from_global_requires_matching_robot() -> None: + with pytest.raises(ValueError, match="does not belong to robot"): + local_joint_name_from_global("right_arm", "left_arm/joint1") diff --git a/dimos/manipulation/planning/test_planning_group_utils.py b/dimos/manipulation/planning/test_planning_group_joints.py similarity index 96% rename from dimos/manipulation/planning/test_planning_group_utils.py rename to dimos/manipulation/planning/test_planning_group_joints.py index d479f34a4e..0391b1b136 100644 --- a/dimos/manipulation/planning/test_planning_group_utils.py +++ b/dimos/manipulation/planning/test_planning_group_joints.py @@ -18,8 +18,8 @@ import pytest +from dimos.manipulation.planning.groups.joints import joint_target_to_global_names from dimos.manipulation.planning.groups.models import PlanningGroup -from dimos.manipulation.planning.groups.utils import joint_target_to_global_names from dimos.msgs.sensor_msgs.JointState import JointState diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index daa0a25158..b639674c4c 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -893,11 +893,6 @@ def get_joint_state(self, ctx: Context, robot_id: WorldRobotID) -> JointState: raise KeyError(f"Robot '{robot_id}' not found") robot_data = self._robots[robot_id] - if robot_data.ee_frame is None: - raise ValueError( - f"Robot '{robot_id}' has no robot-scoped end-effector link; " - "use get_group_ee_pose() with an explicit planning group ID" - ) plant_ctx = self._diagram.GetSubsystemContext(self._plant, ctx) full_positions = self._plant.GetPositions(plant_ctx) @@ -1018,7 +1013,7 @@ def get_ee_pose(self, ctx: Context, robot_id: WorldRobotID) -> PoseStamped: if robot_data.ee_frame is None: raise ValueError( f"Robot '{robot_id}' has no robot-scoped end-effector link; " - "use get_group_jacobian() with an explicit planning group ID" + "use get_group_ee_pose() with an explicit planning group ID" ) plant_ctx = self._diagram.GetSubsystemContext(self._plant, ctx) diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 833feb029b..33ebb4d267 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -178,6 +178,34 @@ def test_begin_planning_state_checks(self, robot_config): module._state = ManipulationState.EXECUTING assert module._begin_planning() is False + def test_empty_plan_targets_do_not_fault_before_planning(self): + """Pre-flight validation errors do not corrupt the planning state machine.""" + module = _make_module() + module._world_monitor = MagicMock() + module._kinematics = MagicMock() + module._planner = MagicMock() + + assert module.plan_to_pose_targets({}) is False + assert module._state == ManipulationState.IDLE + assert module._error_message == "" + + assert module.plan_to_joint_targets({}) is False + assert module._state == ManipulationState.IDLE + assert module._error_message == "" + + def test_plan_to_pose_targets_requires_planner_before_planning(self): + """Pose-target planning requires both IK and path planner backends.""" + module = _make_module() + module._world_monitor = MagicMock() + module._kinematics = MagicMock() + module._planner = None + + pose = Pose(position=Vector3(), orientation=Quaternion()) + + assert module.plan_to_pose_targets({"test_arm/manipulator": pose}) is False + assert module._state == ManipulationState.IDLE + assert module._error_message == "" + class TestRobotSelection: """Test robot selection logic.""" diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index cc5797553d..5ba30bdd8c 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -18,31 +18,31 @@ all_blueprints = { "alfred-nav": "dimos.robot.diy.alfred.blueprints.alfred_nav:alfred_nav", "coordinator-basic": "dimos.control.blueprints.basic:coordinator_basic", - "coordinator-cartesian-ik-mock": "dimos.control.blueprints.teleop:coordinator_cartesian_ik_mock", - "coordinator-cartesian-ik-piper": "dimos.control.blueprints.teleop:coordinator_cartesian_ik_piper", - "coordinator-combined-xarm6": "dimos.control.blueprints.teleop:coordinator_combined_xarm6", - "coordinator-dual-mock": "dimos.control.blueprints.dual:coordinator_dual_mock", - "coordinator-dual-xarm": "dimos.control.blueprints.dual:coordinator_dual_xarm", + "coordinator-cartesian-ik-mock": "dimos.robot.manipulators.piper.blueprints.teleop:coordinator_cartesian_ik_mock", + "coordinator-cartesian-ik-piper": "dimos.robot.manipulators.piper.blueprints.teleop:coordinator_cartesian_ik_piper", + "coordinator-combined-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:coordinator_combined_xarm6", + "coordinator-dual-mock": "dimos.robot.manipulators.common.mock:coordinator_dual_mock", + "coordinator-dual-xarm": "dimos.robot.manipulators.xarm.blueprints.basic:coordinator_dual_xarm", "coordinator-flowbase": "dimos.control.blueprints.mobile:coordinator_flowbase", "coordinator-flowbase-keyboard-teleop": "dimos.control.blueprints.mobile:coordinator_flowbase_keyboard_teleop", "coordinator-flowbase-nav": "dimos.control.blueprints.mobile:coordinator_flowbase_nav", "coordinator-mobile-manip-mock": "dimos.control.blueprints.mobile:coordinator_mobile_manip_mock", - "coordinator-mock": "dimos.control.blueprints.basic:coordinator_mock", + "coordinator-mock": "dimos.robot.manipulators.common.mock:coordinator_mock", "coordinator-mock-twist-base": "dimos.control.blueprints.mobile:coordinator_mock_twist_base", - "coordinator-openarm-bimanual": "dimos.robot.manipulators.openarm.blueprints:coordinator_openarm_bimanual", - "coordinator-openarm-left": "dimos.robot.manipulators.openarm.blueprints:coordinator_openarm_left", - "coordinator-openarm-mock": "dimos.robot.manipulators.openarm.blueprints:coordinator_openarm_mock", - "coordinator-openarm-right": "dimos.robot.manipulators.openarm.blueprints:coordinator_openarm_right", - "coordinator-piper": "dimos.control.blueprints.basic:coordinator_piper", - "coordinator-piper-xarm": "dimos.control.blueprints.dual:coordinator_piper_xarm", - "coordinator-servo-xarm6": "dimos.control.blueprints.teleop:coordinator_servo_xarm6", - "coordinator-teleop-dual": "dimos.control.blueprints.teleop:coordinator_teleop_dual", - "coordinator-teleop-piper": "dimos.control.blueprints.teleop:coordinator_teleop_piper", - "coordinator-teleop-xarm6": "dimos.control.blueprints.teleop:coordinator_teleop_xarm6", - "coordinator-teleop-xarm7": "dimos.control.blueprints.teleop:coordinator_teleop_xarm7", - "coordinator-velocity-xarm6": "dimos.control.blueprints.teleop:coordinator_velocity_xarm6", - "coordinator-xarm6": "dimos.control.blueprints.basic:coordinator_xarm6", - "coordinator-xarm7": "dimos.control.blueprints.basic:coordinator_xarm7", + "coordinator-openarm-bimanual": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm_bimanual", + "coordinator-openarm-left": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm_left", + "coordinator-openarm-mock": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm_mock", + "coordinator-openarm-right": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm_right", + "coordinator-piper": "dimos.robot.manipulators.piper.blueprints.basic:coordinator_piper", + "coordinator-piper-xarm": "dimos.robot.manipulators.common.mixed:coordinator_piper_xarm", + "coordinator-servo-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:coordinator_servo_xarm6", + "coordinator-teleop-dual": "dimos.robot.manipulators.common.mixed:coordinator_teleop_dual", + "coordinator-teleop-piper": "dimos.robot.manipulators.piper.blueprints.teleop:coordinator_teleop_piper", + "coordinator-teleop-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:coordinator_teleop_xarm6", + "coordinator-teleop-xarm7": "dimos.robot.manipulators.xarm.blueprints.teleop:coordinator_teleop_xarm7", + "coordinator-velocity-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:coordinator_velocity_xarm6", + "coordinator-xarm6": "dimos.robot.manipulators.xarm.blueprints.basic:coordinator_xarm6", + "coordinator-xarm7": "dimos.robot.manipulators.xarm.blueprints.basic:coordinator_xarm7", "demo-agent": "dimos.agents.demo_agent:demo_agent", "demo-agent-camera": "dimos.agents.demo_agent:demo_agent_camera", "demo-camera": "dimos.hardware.sensors.camera.module:demo_camera", @@ -58,14 +58,13 @@ "desk-marker-tf": "dimos.perception.fiducial.blueprints.desk_marker_tf:desk_marker_tf", "drone-agentic": "dimos.robot.drone.blueprints.agentic.drone_agentic:drone_agentic", "drone-basic": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic", - "dual-xarm6-planner-coordinator": "dimos.manipulation.blueprints:dual_xarm6_planner_coordinator", - "dual-xarm7-planner-coordinator": "dimos.manipulation.blueprints:dual_xarm7_planner_coordinator", - "keyboard-teleop-a750": "dimos.robot.manipulators.a750.blueprints:keyboard_teleop_a750", - "keyboard-teleop-openarm": "dimos.robot.manipulators.openarm.blueprints:keyboard_teleop_openarm", - "keyboard-teleop-openarm-mock": "dimos.robot.manipulators.openarm.blueprints:keyboard_teleop_openarm_mock", - "keyboard-teleop-piper": "dimos.robot.manipulators.piper.blueprints:keyboard_teleop_piper", - "keyboard-teleop-xarm6": "dimos.robot.manipulators.xarm.blueprints:keyboard_teleop_xarm6", - "keyboard-teleop-xarm7": "dimos.robot.manipulators.xarm.blueprints:keyboard_teleop_xarm7", + "dual-xarm6-planner": "dimos.robot.manipulators.xarm.blueprints.basic:dual_xarm6_planner", + "keyboard-teleop-a750": "dimos.robot.manipulators.a750.blueprints.teleop:keyboard_teleop_a750", + "keyboard-teleop-openarm": "dimos.robot.manipulators.openarm.blueprints.teleop:keyboard_teleop_openarm", + "keyboard-teleop-openarm-mock": "dimos.robot.manipulators.openarm.blueprints.teleop:keyboard_teleop_openarm_mock", + "keyboard-teleop-piper": "dimos.robot.manipulators.piper.blueprints.teleop:keyboard_teleop_piper", + "keyboard-teleop-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:keyboard_teleop_xarm6", + "keyboard-teleop-xarm7": "dimos.robot.manipulators.xarm.blueprints.teleop:keyboard_teleop_xarm7", "mid360": "dimos.hardware.sensors.lidar.livox.livox_blueprints:mid360", "mid360-fastlio": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio", "mid360-fastlio-ray-trace": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_ray_trace", @@ -73,8 +72,8 @@ "mid360-fastlio-voxels-native": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels_native", "mid360-pointlio": "dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints:mid360_pointlio", "mid360-pointlio-voxels": "dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints:mid360_pointlio_voxels", - "openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_mock_planner_coordinator", - "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_planner_coordinator", + "openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_mock_planner_coordinator", + "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_planner_coordinator", "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", "teleop-hosted-go2": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_go2", "teleop-hosted-xarm7": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_xarm7", @@ -124,13 +123,13 @@ "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-perception": "dimos.manipulation.blueprints:xarm_perception", - "xarm-perception-agent": "dimos.manipulation.blueprints:xarm_perception_agent", - "xarm-perception-sim": "dimos.manipulation.blueprints:xarm_perception_sim", - "xarm-perception-sim-agent": "dimos.manipulation.blueprints:xarm_perception_sim_agent", - "xarm6-planner-only": "dimos.manipulation.blueprints:xarm6_planner_only", - "xarm7-planner-coordinator": "dimos.manipulation.blueprints:xarm7_planner_coordinator", - "xarm7-planner-coordinator-agent": "dimos.manipulation.blueprints:xarm7_planner_coordinator_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", + "xarm6-planner-only": "dimos.robot.manipulators.xarm.blueprints.basic:xarm6_planner_only", + "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", } diff --git a/dimos/robot/manipulators/_modeling.py b/dimos/robot/manipulators/_modeling.py new file mode 100644 index 0000000000..852a484b63 --- /dev/null +++ b/dimos/robot/manipulators/_modeling.py @@ -0,0 +1,59 @@ +# 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. + +"""Small helpers for manipulator planning model configs.""" + +from __future__ import annotations + +from typing import TypeAlias + +from dimos.manipulation.planning.spec.models import RobotName +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 + +DegreesOfFreedom: TypeAlias = int +JointPrefix: TypeAlias = str +UrdfJointPrefix: TypeAlias = str +UrdfJointName: TypeAlias = str +CoordinatorJointName: TypeAlias = str +JointNameMapping: TypeAlias = dict[CoordinatorJointName, UrdfJointName] + + +def base_pose(x: float = 0.0, y: float = 0.0, z: float = 0.0) -> PoseStamped: + return PoseStamped( + position=Vector3(x=x, y=y, z=z), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + ) + + +def joint_names( + dof: DegreesOfFreedom, + *, + prefix: JointPrefix = "joint", +) -> list[UrdfJointName]: + return [f"{prefix}{i}" for i in range(1, dof + 1)] + + +def coordinator_joint_mapping( + name: RobotName, + dof: DegreesOfFreedom, + *, + joint_prefix: JointPrefix | None = None, + urdf_joint_prefix: UrdfJointPrefix = "", +) -> JointNameMapping: + prefix = f"{name}/" if joint_prefix is None else joint_prefix + if not prefix: + return {} + return {f"{prefix}joint{i}": f"{urdf_joint_prefix}joint{i}" for i in range(1, dof + 1)} diff --git a/dimos/robot/manipulators/a750/blueprints.py b/dimos/robot/manipulators/a750/blueprints.py deleted file mode 100644 index ccc50ac43c..0000000000 --- a/dimos/robot/manipulators/a750/blueprints.py +++ /dev/null @@ -1,118 +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. - -"""Keyboard teleop blueprint for the A-750 arm. - -Launches the ControlCoordinator (mock adapter + CartesianIK), the -ManipulationModule (Drake/Meshcat visualization), and a pygame keyboard -teleop UI — all wired together via autoconnect. - -Usage: - dimos run keyboard-teleop-a750 -""" - -import math -from pathlib import Path - -from dimos.control.blueprints._hardware import A750_FK_MODEL, a750 -from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.core.coordination.blueprints import autoconnect -from dimos.manipulation.manipulation_module import ManipulationModule -from dimos.manipulation.planning.spec.config import RobotModelConfig -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.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -from dimos.utils.data import LfsPath - -A750_GRIPPER_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ - ("base_link", "link1"), - ("base_link", "link2"), - ("left_finger_link", "link3"), - ("left_finger_link", "link4"), - ("left_finger_link", "link5"), - ("left_finger_link", "link6"), - ("left_finger_link", "right_finger_link"), - ("link1", "link2"), - ("link2", "link3"), - ("link2", "link4"), - ("link3", "link4"), - ("link3", "link5"), - ("link3", "right_finger_link"), - ("link4", "link5"), - ("link4", "link6"), - ("link4", "right_finger_link"), - ("link5", "link6"), - ("link5", "right_finger_link"), - ("link6", "right_finger_link"), -] - -_A750_MODEL_PATH = LfsPath("a750_description") / "urdf/a750_rev1.urdf" -_A750_HOME_JOINTS = [0.0, 0.0, -math.radians(90), 0.0, 0.0, 0.0] -_A750_PACKAGE_PATHS: dict[str, Path] = { - "a750_description": LfsPath("a750_description"), - "a750_gazebo": LfsPath("a750_description"), -} - - -def _a750_model_config() -> RobotModelConfig: - return RobotModelConfig( - name="arm", - model_path=_A750_MODEL_PATH, - base_pose=PoseStamped( - position=Vector3(x=0.0, y=0.0, z=0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - ), - joint_names=[f"joint{i}" for i in range(1, 7)], - end_effector_link="gripper_base", - base_link="base_link", - package_paths=_A750_PACKAGE_PATHS, - auto_convert_meshes=True, - collision_exclusion_pairs=A750_GRIPPER_COLLISION_EXCLUSIONS, - coordinator_task_name="traj_arm", - gripper_hardware_id="arm", - home_joints=_A750_HOME_JOINTS, - ) - - -_a750_hw = a750("arm", mock_without_address=True) - -# A-750 6-DOF mock sim + keyboard teleop + Drake visualization -keyboard_teleop_a750 = autoconnect( - KeyboardTeleopModule.blueprint( - model_path=A750_FK_MODEL, - ee_joint_id=6, - home_joints=_A750_HOME_JOINTS, - joint_names=_a750_hw.joints, - ), - ControlCoordinator.blueprint( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[_a750_hw], - tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_a750_hw.joints, - priority=10, - params={"model_path": A750_FK_MODEL, "ee_joint_id": 6}, - ), - ], - ), - ManipulationModule.blueprint( - robots=[_a750_model_config()], - visualization={"backend": "meshcat"}, - ), -) diff --git a/dimos/robot/manipulators/a750/blueprints/teleop.py b/dimos/robot/manipulators/a750/blueprints/teleop.py new file mode 100644 index 0000000000..873a01817f --- /dev/null +++ b/dimos/robot/manipulators/a750/blueprints/teleop.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. + +"""A-750 teleop blueprints.""" + +from __future__ import annotations + +from dimos.control.coordinator import ControlCoordinator +from dimos.core.coordination.blueprints import autoconnect +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.robot.manipulators.a750.config import ( + A750_FK_MODEL, + A750_HOME_JOINTS, + a750_hardware, + make_a750_model_config, +) +from dimos.robot.manipulators.common.blueprints import cartesian_ik_task +from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule + +_a750_hw = a750_hardware("arm", mock_without_address=True) + +keyboard_teleop_a750 = autoconnect( + KeyboardTeleopModule.blueprint( + model_path=A750_FK_MODEL, + ee_joint_id=6, + home_joints=A750_HOME_JOINTS, + joint_names=_a750_hw.joints, + ), + ControlCoordinator.blueprint( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[_a750_hw], + tasks=[cartesian_ik_task(_a750_hw, model_path=A750_FK_MODEL, ee_joint_id=6)], + ), + ManipulationModule.blueprint( + robots=[make_a750_model_config()], + visualization={"backend": "meshcat"}, + ), +) diff --git a/dimos/robot/manipulators/a750/config.py b/dimos/robot/manipulators/a750/config.py new file mode 100644 index 0000000000..32bc57409a --- /dev/null +++ b/dimos/robot/manipulators/a750/config.py @@ -0,0 +1,123 @@ +# 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. + +"""A-750 planning model configuration helpers.""" + +from __future__ import annotations + +import math +from pathlib import Path + +from dimos.control.components import HardwareComponent, HardwareType, make_joints +from dimos.core.global_config import global_config +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.robot.manipulators._modeling import ( + base_pose, + coordinator_joint_mapping, + joint_names, +) +from dimos.utils.data import LfsPath + +A750_GRIPPER_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ + ("base_link", "link1"), + ("base_link", "link2"), + ("left_finger_link", "link3"), + ("left_finger_link", "link4"), + ("left_finger_link", "link5"), + ("left_finger_link", "link6"), + ("left_finger_link", "right_finger_link"), + ("link1", "link2"), + ("link2", "link3"), + ("link2", "link4"), + ("link3", "link4"), + ("link3", "link5"), + ("link3", "right_finger_link"), + ("link4", "link5"), + ("link4", "link6"), + ("link4", "right_finger_link"), + ("link5", "link6"), + ("link5", "right_finger_link"), + ("link6", "right_finger_link"), +] + +A750_HOME_JOINTS = [0.0, 0.0, -math.radians(90), 0.0, 0.0, 0.0] +A750_MODEL_PATH = LfsPath("a750_description") / "urdf/a750_rev1.urdf" +A750_FK_MODEL = LfsPath("a750_description/urdf/a750_rev1_no_gripper.urdf") +A750_PACKAGE_PATHS: dict[str, Path] = { + "a750_description": LfsPath("a750_description"), + "a750_gazebo": LfsPath("a750_description"), +} + + +def make_a750_hardware( + hw_id: str = "arm", + *, + adapter_type: str = "a750", + address: str | None = None, + gripper: bool = True, + auto_enable: bool = True, + home_joints: list[float] | None = None, +) -> HardwareComponent: + joints = make_joints(hw_id, 6) + return HardwareComponent( + hardware_id=hw_id, + hardware_type=HardwareType.MANIPULATOR, + joints=joints, + adapter_type=adapter_type, + address=address, + auto_enable=auto_enable, + gripper_joints=[f"{hw_id}/finger"] if gripper else [], + adapter_kwargs={"initial_positions": home_joints or A750_HOME_JOINTS}, + ) + + +def a750_hardware(hw_id: str = "arm", *, mock_without_address: bool = False) -> HardwareComponent: + if mock_without_address and not global_config.device_path: + return make_a750_hardware( + hw_id, + adapter_type="mock", + address=None, + ) + return make_a750_hardware( + hw_id, + address=global_config.device_path or "/dev/ttyACM0", + ) + + +def make_a750_model_config( + name: str = "arm", + *, + joint_prefix: str | None = None, + coordinator_task_name: str | None = None, +) -> RobotModelConfig: + dof = 6 + return RobotModelConfig( + name=name, + model_path=A750_MODEL_PATH, + base_pose=base_pose(), + joint_names=joint_names(dof), + end_effector_link="gripper_base", + base_link="base_link", + package_paths=A750_PACKAGE_PATHS, + auto_convert_meshes=True, + collision_exclusion_pairs=A750_GRIPPER_COLLISION_EXCLUSIONS, + joint_name_mapping=coordinator_joint_mapping( + name, + dof, + joint_prefix=joint_prefix, + ), + coordinator_task_name=coordinator_task_name or f"traj_{name}", + gripper_hardware_id=name, + home_joints=A750_HOME_JOINTS, + ) diff --git a/dimos/robot/manipulators/common/agent_prompts.py b/dimos/robot/manipulators/common/agent_prompts.py new file mode 100644 index 0000000000..a799455861 --- /dev/null +++ b/dimos/robot/manipulators/common/agent_prompts.py @@ -0,0 +1,109 @@ +# 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. + +"""System prompts used by manipulation agent blueprints.""" + +BASE_MANIPULATION_AGENT_SYSTEM_PROMPT = """\ +You are a robotic manipulation assistant controlling an xArm7 robot arm. + +Available skills: +- get_robot_state: Get current joint positions, end-effector pose, and gripper state. +- move_to_pose: Move end-effector to ABSOLUTE x, y, z (meters) with optional roll, pitch, yaw (radians). +- move_to_joints: Move to a joint configuration (comma-separated radians). +- open_gripper / close_gripper / set_gripper: Control the gripper. +- go_home: Move to the home/observe position. +- go_init: Return to the startup position. +- reset: Clear a FAULT state and return to IDLE. Use this when a motion fails. + +COORDINATE SYSTEM (world frame, meters): +- X axis = forward (away from the robot base) +- Y axis = left +- Z axis = up +- Z=0 is the robot base level; typical working height is Z = 0.2-0.5 + +CRITICAL WORKFLOW for relative movement requests (e.g. "move 20cm forward"): +1. Call get_robot_state to get the current EE pose. +2. Add the requested offset to the CURRENT position. Example: if EE is at \ +(0.3, 0.0, 0.4) and user says "move 20cm forward", target is (0.5, 0.0, 0.4). +3. Call move_to_pose with the computed ABSOLUTE target. +NEVER pass only the offset as coordinates — that would send the robot to near-origin. + +ERROR RECOVERY: If a motion fails or the state becomes FAULT, call reset before retrying. +""" + +MANIPULATION_AGENT_SYSTEM_PROMPT = """\ +You are a robotic manipulation assistant controlling an xArm7 robot arm with an \ +eye-in-hand RealSense camera and a gripper. + +# Skills + +## Perception +- **look**: Quick snapshot of objects visible from the current camera pose. Does NOT \ +move the arm. Example: "what do you see?", "what's on the table?" +- **scan_objects**: Full scan — moves the arm to the init position for a clear view, \ +then refreshes detections. Use before pick/place, after a failed grasp, or when the \ +user explicitly asks to scan. Example: "scan the table", "what objects are there?" + +## 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" +- **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 \ +world-frame pose (meters / radians). +- **move_to_joints **: Move to a joint configuration (radians). +- **go_home**: Move to the home/observe position. +- **go_init**: Return to the startup position. Use after pick/place as a safe resting pose. + +## Gripper +- **open_gripper / close_gripper / set_gripper**: Direct gripper control. + +## Status & Recovery +- **get_robot_state**: Current joint positions, end-effector pose, and gripper state. +- **get_scene_info**: Full robot state, detected objects, and scene overview. +- **reset**: Clear a FAULT state and return to IDLE. Available as both a skill and RPC. +- **clear_perception_obstacles**: Remove detected obstacles from the planning world. \ +Use when planning fails with COLLISION_AT_START. + +# Choosing look vs scan_objects +- "what can you see?" / "what's there?" → **look** (instant, no movement) +- "scan the scene" / before pick-and-place → **scan_objects** (thorough, moves arm) +- If objects were ALREADY detected by a previous look, do NOT scan again — just proceed. + +# Rules +- Use the EXACT object name from detection output. Do NOT substitute similar names \ +(e.g. if detection says "spray can", do not use "grinder"). +- "drop it in/on [object]" → use **drop_on**. "place it at [coords]" → use **place**. +- "bring it back" → pick, then **go_init**. Do NOT place randomly. +- "bring it to me" / "hand it over" → pick, then move toward user (≈ X=0, Y=0.5). +- 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. + +# 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. +""" diff --git a/dimos/robot/manipulators/common/blueprints.py b/dimos/robot/manipulators/common/blueprints.py new file mode 100644 index 0000000000..1f85998a67 --- /dev/null +++ b/dimos/robot/manipulators/common/blueprints.py @@ -0,0 +1,128 @@ +# 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. + +"""Small blueprint helpers shared by manipulator stacks.""" + +from __future__ import annotations + +from collections.abc import Sequence +from pathlib import Path +from typing import Any + +from dimos.control.components import HardwareComponent +from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.core.coordination.blueprints import Blueprint +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.robot.manipulators.common.topics import ( + CARTESIAN_IK_TASK_NAME, + COORDINATOR_FRAME_ID, + DEFAULT_TRAJECTORY_TASK_NAME, + trajectory_task_name, +) + + +def trajectory_task( + hardware: HardwareComponent, + *, + name: str | None = None, + priority: int = 10, +) -> TaskConfig: + return TaskConfig( + name=name or trajectory_task_name(hardware.hardware_id), + type="trajectory", + joint_names=hardware.joints, + priority=priority, + ) + + +def cartesian_ik_task( + hardware: HardwareComponent, + *, + model_path: Path, + ee_joint_id: int, + name: str = CARTESIAN_IK_TASK_NAME, + priority: int = 10, +) -> TaskConfig: + return TaskConfig( + name=name, + type="cartesian_ik", + joint_names=hardware.joints, + priority=priority, + params={"model_path": model_path, "ee_joint_id": ee_joint_id}, + ) + + +def teleop_ik_task( + hardware: HardwareComponent, + *, + model_path: Path, + ee_joint_id: int, + hand: str, + name: str, + priority: int = 10, + params: dict[str, Any] | None = None, +) -> TaskConfig: + task_params: dict[str, Any] = { + "model_path": model_path, + "ee_joint_id": ee_joint_id, + "hand": hand, + } + if params: + task_params.update(params) + return TaskConfig( + name=name, + type="teleop_ik", + joint_names=hardware.joints, + priority=priority, + params=task_params, + ) + + +def coordinator( + *, + hardware: Sequence[HardwareComponent] = (), + tasks: Sequence[TaskConfig] = (), + tick_rate: float = 100.0, + publish_joint_state: bool = True, + joint_state_frame_id: str = COORDINATOR_FRAME_ID, +) -> Blueprint: + return ControlCoordinator.blueprint( + tick_rate=tick_rate, + publish_joint_state=publish_joint_state, + joint_state_frame_id=joint_state_frame_id, + hardware=list(hardware), + tasks=list(tasks), + ) + + +def planner( + *, + robots: Sequence[RobotModelConfig], + planning_timeout: float = 10.0, + visualization: dict[str, Any] | None = None, + **kwargs: Any, +) -> Blueprint: + return ManipulationModule.blueprint( + robots=list(robots), + planning_timeout=planning_timeout, + visualization=visualization or {"backend": "meshcat"}, + **kwargs, + ) + + +def default_trajectory_task_name(hardware_id: str) -> str: + if hardware_id == "arm": + return DEFAULT_TRAJECTORY_TASK_NAME + return trajectory_task_name(hardware_id) diff --git a/dimos/control/blueprints/dual.py b/dimos/robot/manipulators/common/mixed.py similarity index 50% rename from dimos/control/blueprints/dual.py rename to dimos/robot/manipulators/common/mixed.py index 29ee797c0d..05cecaa268 100644 --- a/dimos/control/blueprints/dual.py +++ b/dimos/robot/manipulators/common/mixed.py @@ -1,4 +1,4 @@ -# Copyright 2025-2026 Dimensional Inc. +# 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. @@ -12,89 +12,70 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Dual-arm coordinator blueprints with trajectory control. - -Usage: - dimos run coordinator-dual-mock # Mock 7+6 DOF arms - dimos run coordinator-dual-xarm # XArm7 left + XArm6 right - dimos run coordinator-piper-xarm # XArm6 + Piper -""" +"""Mixed-manipulator coordinator blueprints.""" from __future__ import annotations -from dimos.control.blueprints._hardware import manipulator, mock_arm from dimos.control.coordinator import ControlCoordinator, TaskConfig from dimos.core.global_config import global_config +from dimos.robot.manipulators.piper.config import PIPER_FK_MODEL, make_piper_hardware +from dimos.robot.manipulators.xarm.config import XARM6_FK_MODEL, make_xarm_hardware -# Dual mock arms (7-DOF left, 6-DOF right) -_mock_left = mock_arm("left_arm", 7) -_mock_right = mock_arm("right_arm", 6) - -coordinator_dual_mock = ControlCoordinator.blueprint( - hardware=[_mock_left, _mock_right], - tasks=[ - TaskConfig(name="traj_left", type="trajectory", joint_names=_mock_left.joints, priority=10), - TaskConfig( - name="traj_right", type="trajectory", joint_names=_mock_right.joints, priority=10 - ), - ], -) - -# Dual XArm (XArm7 left, XArm6 right) -_xarm7_left = manipulator( - "left_arm", - 7, - adapter_type="xarm", - address=global_config.xarm7_ip, -) -_xarm6_right = manipulator( - "right_arm", +_xarm6_dual = make_xarm_hardware( + "xarm_arm", 6, adapter_type="xarm", address=global_config.xarm6_ip, ) +_piper_dual = make_piper_hardware( + "piper_arm", + adapter_type="piper", + address=global_config.can_port or "can0", + gripper=True, +) -coordinator_dual_xarm = ControlCoordinator.blueprint( - hardware=[_xarm7_left, _xarm6_right], +coordinator_piper_xarm = ControlCoordinator.blueprint( + hardware=[_xarm6_dual, _piper_dual], tasks=[ TaskConfig( - name="traj_left_arm", - type="trajectory", - joint_names=_xarm7_left.joints, - priority=10, + name="traj_xarm", type="trajectory", joint_names=_xarm6_dual.joints, priority=10 ), TaskConfig( - name="traj_right_arm", - type="trajectory", - joint_names=_xarm6_right.joints, - priority=10, + name="traj_piper", type="trajectory", joint_names=_piper_dual.joints, priority=10 ), ], ) -# Dual arm (XArm6 + Piper) -_xarm6_dual = manipulator( +_xarm6_teleop_hw = make_xarm_hardware( "xarm_arm", 6, adapter_type="xarm", address=global_config.xarm6_ip, + gripper=True, ) -_piper_dual = manipulator( +_piper_teleop_hw = make_piper_hardware( "piper_arm", - 6, adapter_type="piper", address=global_config.can_port or "can0", gripper=True, ) -coordinator_piper_xarm = ControlCoordinator.blueprint( - hardware=[_xarm6_dual, _piper_dual], +coordinator_teleop_dual = ControlCoordinator.blueprint( + hardware=[_xarm6_teleop_hw, _piper_teleop_hw], tasks=[ TaskConfig( - name="traj_xarm", type="trajectory", joint_names=_xarm6_dual.joints, priority=10 + name="teleop_xarm", + type="teleop_ik", + joint_names=_xarm6_teleop_hw.joints, + priority=10, + params={"model_path": XARM6_FK_MODEL, "ee_joint_id": 6, "hand": "left"}, ), TaskConfig( - name="traj_piper", type="trajectory", joint_names=_piper_dual.joints, priority=10 + name="teleop_piper", + type="teleop_ik", + joint_names=_piper_teleop_hw.joints, + priority=10, + params={"model_path": PIPER_FK_MODEL, "ee_joint_id": 6, "hand": "right"}, ), ], ) diff --git a/dimos/robot/manipulators/common/mock.py b/dimos/robot/manipulators/common/mock.py new file mode 100644 index 0000000000..c48efc035b --- /dev/null +++ b/dimos/robot/manipulators/common/mock.py @@ -0,0 +1,62 @@ +# 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. + +"""Mock manipulator coordinator blueprints.""" + +from __future__ import annotations + +from dimos.control.components import HardwareComponent, HardwareType, make_joints +from dimos.control.coordinator import ControlCoordinator, TaskConfig + +_mock_hw = HardwareComponent( + hardware_id="arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("arm", 7), + adapter_type="mock", +) + +coordinator_mock = ControlCoordinator.blueprint( + hardware=[_mock_hw], + tasks=[ + TaskConfig( + name="traj_arm", + type="trajectory", + joint_names=_mock_hw.joints, + priority=10, + ) + ], +) + +_mock_left = HardwareComponent( + hardware_id="left_arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("left_arm", 7), + adapter_type="mock", +) +_mock_right = HardwareComponent( + hardware_id="right_arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("right_arm", 6), + adapter_type="mock", +) + +coordinator_dual_mock = ControlCoordinator.blueprint( + hardware=[_mock_left, _mock_right], + tasks=[ + TaskConfig(name="traj_left", type="trajectory", joint_names=_mock_left.joints, priority=10), + TaskConfig( + name="traj_right", type="trajectory", joint_names=_mock_right.joints, priority=10 + ), + ], +) diff --git a/dimos/robot/manipulators/common/sim.py b/dimos/robot/manipulators/common/sim.py new file mode 100644 index 0000000000..d613cd9821 --- /dev/null +++ b/dimos/robot/manipulators/common/sim.py @@ -0,0 +1,31 @@ +# 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. + +"""Simulation helpers for manipulator blueprints.""" + +from __future__ import annotations + +from pathlib import Path + +from dimos.core.coordination.blueprints import Blueprint +from dimos.core.global_config import global_config + + +def mujoco_if_sim(sim_path: str | Path, dof: int) -> tuple[Blueprint, ...]: + if not global_config.simulation: + return () + + from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule + + return (MujocoSimModule.blueprint(address=str(sim_path), headless=False, dof=dof),) diff --git a/dimos/robot/manipulators/common/topics.py b/dimos/robot/manipulators/common/topics.py new file mode 100644 index 0000000000..aa0ed70206 --- /dev/null +++ b/dimos/robot/manipulators/common/topics.py @@ -0,0 +1,30 @@ +# 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. + +"""Shared control naming conventions for manipulator blueprints.""" + +from __future__ import annotations + +from typing import TypeAlias + +FrameId: TypeAlias = str +TaskName: TypeAlias = str + +COORDINATOR_FRAME_ID: FrameId = "coordinator" +CARTESIAN_IK_TASK_NAME: TaskName = "cartesian_ik_arm" +DEFAULT_TRAJECTORY_TASK_NAME: TaskName = "traj_arm" + + +def trajectory_task_name(hardware_id: str) -> TaskName: + return f"traj_{hardware_id}" diff --git a/dimos/robot/manipulators/openarm/blueprints.py b/dimos/robot/manipulators/openarm/blueprints.py deleted file mode 100644 index ebdd589084..0000000000 --- a/dimos/robot/manipulators/openarm/blueprints.py +++ /dev/null @@ -1,326 +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. - -"""OpenArm blueprints. Flip LEFT_CAN / RIGHT_CAN below if arms come up swapped.""" - -from __future__ import annotations - -from pathlib import Path -from typing import Any - -from dimos.control.components import HardwareComponent, HardwareType -from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.core.coordination.blueprints import autoconnect -from dimos.core.transport import LCMTransport -from dimos.manipulation.manipulation_module import ManipulationModule -from dimos.manipulation.planning.spec.config import RobotModelConfig -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.sensor_msgs.JointState import JointState -from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -from dimos.utils.data import LfsPath - -OPENARM_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ - ("openarm_left_link5", "openarm_left_link7"), - ("openarm_right_link5", "openarm_right_link7"), -] - -_OPENARM_PKG = LfsPath("openarm_description") -_OPENARM_LEFT_MODEL = _OPENARM_PKG / "urdf/robot/openarm_v10_left.urdf" -_OPENARM_RIGHT_MODEL = _OPENARM_PKG / "urdf/robot/openarm_v10_right.urdf" -OPENARM_V10_FK_MODEL = _OPENARM_PKG / "urdf/robot/openarm_v10_single.urdf" -_OPENARM_PACKAGE_PATHS: dict[str, Path] = {"openarm_description": _OPENARM_PKG} - - -def _base_pose() -> PoseStamped: - return PoseStamped( - position=Vector3(x=0.0, y=0.0, z=0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - ) - - -def _validate_side(side: str) -> None: - if side not in ("left", "right"): - raise ValueError(f"side must be 'left' or 'right', got {side!r}") - - -def _openarm_joints(side: str) -> list[str]: - _validate_side(side) - return [f"openarm_{side}_joint{i}" for i in range(1, 8)] - - -def _openarm_hardware( - side: str, - name: str | None = None, - *, - adapter_type: str = "mock", - address: str | None = None, - adapter_kwargs: dict[str, Any] | None = None, - scoped_joints: bool = False, -) -> HardwareComponent: - _validate_side(side) - resolved_name = name or f"{side}_arm" - local_joints = _openarm_joints(side) - kwargs = {"side": side} - if adapter_kwargs: - kwargs.update(adapter_kwargs) - return HardwareComponent( - hardware_id=resolved_name, - hardware_type=HardwareType.MANIPULATOR, - joints=( - [f"{resolved_name}/{joint}" for joint in local_joints] - if scoped_joints - else local_joints - ), - adapter_type=adapter_type, - address=address, - adapter_kwargs=kwargs, - ) - - -def _openarm_model_config(side: str, name: str | None = None) -> RobotModelConfig: - _validate_side(side) - resolved_name = name or f"{side}_arm" - return RobotModelConfig( - name=resolved_name, - model_path=_OPENARM_LEFT_MODEL if side == "left" else _OPENARM_RIGHT_MODEL, - base_pose=_base_pose(), - joint_names=_openarm_joints(side), - end_effector_link=f"openarm_{side}_link7", - base_link="openarm_body_link0", - package_paths=_OPENARM_PACKAGE_PATHS, - collision_exclusion_pairs=OPENARM_COLLISION_EXCLUSIONS, - auto_convert_meshes=True, - max_velocity=0.5, - max_acceleration=1.0, - coordinator_task_name=f"traj_{resolved_name}", - home_joints=[0.0] * 7, - ) - - -def _openarm_task(hw: HardwareComponent, name: str | None = None) -> TaskConfig: - return TaskConfig( - name=name or f"traj_{hw.hardware_id}", - type="trajectory", - joint_names=hw.joints, - priority=10, - ) - - -def _openarm_single_hardware( - *, - adapter_type: str = "mock", - address: str | None = None, -) -> HardwareComponent: - return _openarm_hardware( - "left", - name="arm", - adapter_type=adapter_type, - address=address, - ) - - -def _openarm_single_model_config() -> RobotModelConfig: - return RobotModelConfig( - name="arm", - model_path=OPENARM_V10_FK_MODEL, - base_pose=_base_pose(), - joint_names=_openarm_joints("left"), - end_effector_link="openarm_left_link7", - base_link="openarm_body_link0", - package_paths=_OPENARM_PACKAGE_PATHS, - auto_convert_meshes=True, - max_velocity=0.5, - max_acceleration=1.0, - coordinator_task_name="traj_arm", - home_joints=[0.0] * 7, - ) - - -# Mock bimanual: no hardware, great for verifying wiring. -_mock_left = _openarm_hardware(side="left") -_mock_right = _openarm_hardware(side="right") -_mock_planner_left = _openarm_hardware(side="left", scoped_joints=True) -_mock_planner_right = _openarm_hardware(side="right", scoped_joints=True) - -coordinator_openarm_mock = ControlCoordinator.blueprint( - hardware=[_mock_left, _mock_right], - tasks=[ - _openarm_task(_mock_left), - _openarm_task(_mock_right), - ], -) - -# Single-arm hardware blueprints (first real bring-up targets). -# CAN interface each physical arm is on. Linux assigns can0/can1 in USB -# enumeration order which is not guaranteed stable; if swapped, flip these. -LEFT_CAN = "can1" -RIGHT_CAN = "can0" - -# Flip to False to skip the CTRL_MODE=MIT write at connect-time. Leave True for -# normal operation; it is idempotent and ensures motors are in the expected mode. -AUTO_SET_MIT_MODE = True - -_ADAPTER_KWARGS = {"auto_set_mit_mode": AUTO_SET_MIT_MODE} -_left_hw = _openarm_hardware( - side="left", - address=LEFT_CAN, - adapter_type="openarm", - adapter_kwargs=_ADAPTER_KWARGS, -) -_right_hw = _openarm_hardware( - side="right", - address=RIGHT_CAN, - adapter_type="openarm", - adapter_kwargs=_ADAPTER_KWARGS, -) -_planner_left_hw = _openarm_hardware( - side="left", - address=LEFT_CAN, - adapter_type="openarm", - adapter_kwargs=_ADAPTER_KWARGS, - scoped_joints=True, -) -_planner_right_hw = _openarm_hardware( - side="right", - address=RIGHT_CAN, - adapter_type="openarm", - adapter_kwargs=_ADAPTER_KWARGS, - scoped_joints=True, -) - -coordinator_openarm_left = ControlCoordinator.blueprint( - hardware=[_left_hw], - tasks=[_openarm_task(_left_hw)], -) - -coordinator_openarm_right = ControlCoordinator.blueprint( - hardware=[_right_hw], - tasks=[_openarm_task(_right_hw)], -) - -coordinator_openarm_bimanual = ControlCoordinator.blueprint( - hardware=[_left_hw, _right_hw], - tasks=[ - _openarm_task(_left_hw), - _openarm_task(_right_hw), - ], -) - - -# Planner + coordinator (mock): Drake plans, mock adapters execute. -openarm_mock_planner_coordinator = autoconnect( - ManipulationModule.blueprint( - robots=[ - _openarm_model_config("left"), - _openarm_model_config("right"), - ], - planning_timeout=10.0, - visualization={"backend": "meshcat"}, - ), - ControlCoordinator.blueprint( - hardware=[_mock_planner_left, _mock_planner_right], - tasks=[ - _openarm_task(_mock_planner_left), - _openarm_task(_mock_planner_right), - ], - ), -).transports( - { - ("coordinator_joint_state", JointState): LCMTransport( - "/coordinator/joint_state", JointState - ), - } -) - -# Planner + coordinator (real hw): plan and execute on both arms. -openarm_planner_coordinator = autoconnect( - ManipulationModule.blueprint( - robots=[ - _openarm_model_config("left"), - _openarm_model_config("right"), - ], - planning_timeout=10.0, - visualization={"backend": "meshcat"}, - ), - ControlCoordinator.blueprint( - hardware=[_planner_left_hw, _planner_right_hw], - tasks=[ - _openarm_task(_planner_left_hw), - _openarm_task(_planner_right_hw), - ], - ), -).transports( - { - ("coordinator_joint_state", JointState): LCMTransport( - "/coordinator/joint_state", JointState - ), - } -) - - -# Keyboard teleop (single arm, mock). -_teleop_hw = _openarm_single_hardware() - -keyboard_teleop_openarm_mock = autoconnect( - KeyboardTeleopModule.blueprint( - model_path=OPENARM_V10_FK_MODEL, - ee_joint_id=7, - joint_names=_teleop_hw.joints, - ), - ControlCoordinator.blueprint( - hardware=[_teleop_hw], - tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_teleop_hw.joints, - priority=10, - params={"model_path": OPENARM_V10_FK_MODEL, "ee_joint_id": 7}, - ), - ], - ), - ManipulationModule.blueprint( - robots=[_openarm_single_model_config()], - visualization={"backend": "meshcat"}, - ), -) - -# Keyboard teleop (single arm, real hw on can0). -_teleop_real_hw = _openarm_single_hardware(adapter_type="openarm", address=LEFT_CAN) - -keyboard_teleop_openarm = autoconnect( - KeyboardTeleopModule.blueprint( - model_path=OPENARM_V10_FK_MODEL, - ee_joint_id=7, - joint_names=_teleop_real_hw.joints, - ), - ControlCoordinator.blueprint( - hardware=[_teleop_real_hw], - tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_teleop_real_hw.joints, - priority=10, - params={"model_path": OPENARM_V10_FK_MODEL, "ee_joint_id": 7}, - ), - ], - ), - ManipulationModule.blueprint( - robots=[_openarm_single_model_config()], - visualization={"backend": "meshcat"}, - ), -) diff --git a/dimos/robot/manipulators/openarm/blueprints/basic.py b/dimos/robot/manipulators/openarm/blueprints/basic.py new file mode 100644 index 0000000000..3d0ef80f8e --- /dev/null +++ b/dimos/robot/manipulators/openarm/blueprints/basic.py @@ -0,0 +1,74 @@ +# 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. + +"""Basic OpenArm coordinator blueprints.""" + +from __future__ import annotations + +from dimos.control.components import HardwareComponent +from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.robot.manipulators.common.blueprints import trajectory_task +from dimos.robot.manipulators.openarm.config import ( + LEFT_CAN, + OPENARM_ADAPTER_KWARGS, + RIGHT_CAN, + openarm_hardware, +) + + +def openarm_task(hw: HardwareComponent, name: str | None = None) -> TaskConfig: + return trajectory_task(hw, name=name) + + +mock_left = openarm_hardware(side="left") +mock_right = openarm_hardware(side="right") + +coordinator_openarm_mock = ControlCoordinator.blueprint( + hardware=[mock_left, mock_right], + tasks=[ + openarm_task(mock_left), + openarm_task(mock_right), + ], +) + +left_hw = openarm_hardware( + side="left", + address=LEFT_CAN, + adapter_type="openarm", + adapter_kwargs=OPENARM_ADAPTER_KWARGS, +) +right_hw = openarm_hardware( + side="right", + address=RIGHT_CAN, + adapter_type="openarm", + adapter_kwargs=OPENARM_ADAPTER_KWARGS, +) + +coordinator_openarm_left = ControlCoordinator.blueprint( + hardware=[left_hw], + tasks=[openarm_task(left_hw)], +) + +coordinator_openarm_right = ControlCoordinator.blueprint( + hardware=[right_hw], + tasks=[openarm_task(right_hw)], +) + +coordinator_openarm_bimanual = ControlCoordinator.blueprint( + hardware=[left_hw, right_hw], + tasks=[ + openarm_task(left_hw), + openarm_task(right_hw), + ], +) diff --git a/dimos/robot/manipulators/openarm/blueprints/planner.py b/dimos/robot/manipulators/openarm/blueprints/planner.py new file mode 100644 index 0000000000..9a341ca59b --- /dev/null +++ b/dimos/robot/manipulators/openarm/blueprints/planner.py @@ -0,0 +1,92 @@ +# 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. + +"""OpenArm planner + coordinator blueprints.""" + +from __future__ import annotations + +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.transport import LCMTransport +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.robot.manipulators.common.blueprints import coordinator, planner +from dimos.robot.manipulators.openarm.blueprints.basic import openarm_task +from dimos.robot.manipulators.openarm.config import ( + LEFT_CAN, + OPENARM_ADAPTER_KWARGS, + RIGHT_CAN, + openarm_hardware, + openarm_model_config, +) + +_mock_planner_left = openarm_hardware(side="left", scoped_joints=True) +_mock_planner_right = openarm_hardware(side="right", scoped_joints=True) + +_planner_left_hw = openarm_hardware( + side="left", + address=LEFT_CAN, + adapter_type="openarm", + adapter_kwargs=OPENARM_ADAPTER_KWARGS, + scoped_joints=True, +) +_planner_right_hw = openarm_hardware( + side="right", + address=RIGHT_CAN, + adapter_type="openarm", + adapter_kwargs=OPENARM_ADAPTER_KWARGS, + scoped_joints=True, +) + +openarm_mock_planner_coordinator = autoconnect( + planner( + robots=[ + openarm_model_config("left"), + openarm_model_config("right"), + ], + ), + coordinator( + hardware=[_mock_planner_left, _mock_planner_right], + tasks=[ + openarm_task(_mock_planner_left), + openarm_task(_mock_planner_right), + ], + ), +).transports( + { + ("coordinator_joint_state", JointState): LCMTransport( + "/coordinator/joint_state", JointState + ), + } +) + +openarm_planner_coordinator = autoconnect( + planner( + robots=[ + openarm_model_config("left"), + openarm_model_config("right"), + ], + ), + coordinator( + hardware=[_planner_left_hw, _planner_right_hw], + tasks=[ + openarm_task(_planner_left_hw), + openarm_task(_planner_right_hw), + ], + ), +).transports( + { + ("coordinator_joint_state", JointState): LCMTransport( + "/coordinator/joint_state", JointState + ), + } +) diff --git a/dimos/robot/manipulators/openarm/blueprints/teleop.py b/dimos/robot/manipulators/openarm/blueprints/teleop.py new file mode 100644 index 0000000000..55a477fb73 --- /dev/null +++ b/dimos/robot/manipulators/openarm/blueprints/teleop.py @@ -0,0 +1,71 @@ +# 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. + +"""OpenArm keyboard teleop blueprints.""" + +from __future__ import annotations + +from dimos.control.coordinator import ControlCoordinator +from dimos.core.coordination.blueprints import autoconnect +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.robot.manipulators.common.blueprints import cartesian_ik_task +from dimos.robot.manipulators.openarm.config import ( + LEFT_CAN, + OPENARM_V10_FK_MODEL, + openarm_single_hardware, + openarm_single_model_config, +) +from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule + +_teleop_hw = openarm_single_hardware() + +keyboard_teleop_openarm_mock = autoconnect( + KeyboardTeleopModule.blueprint( + model_path=OPENARM_V10_FK_MODEL, + ee_joint_id=7, + joint_names=_teleop_hw.joints, + ), + ControlCoordinator.blueprint( + hardware=[_teleop_hw], + tasks=[cartesian_ik_task(_teleop_hw, model_path=OPENARM_V10_FK_MODEL, ee_joint_id=7)], + ), + ManipulationModule.blueprint( + robots=[openarm_single_model_config()], + visualization={"backend": "meshcat"}, + ), +) + +_teleop_real_hw = openarm_single_hardware(adapter_type="openarm", address=LEFT_CAN) + +keyboard_teleop_openarm = autoconnect( + KeyboardTeleopModule.blueprint( + model_path=OPENARM_V10_FK_MODEL, + ee_joint_id=7, + joint_names=_teleop_real_hw.joints, + ), + ControlCoordinator.blueprint( + hardware=[_teleop_real_hw], + tasks=[ + cartesian_ik_task( + _teleop_real_hw, + model_path=OPENARM_V10_FK_MODEL, + ee_joint_id=7, + ) + ], + ), + ManipulationModule.blueprint( + robots=[openarm_single_model_config()], + visualization={"backend": "meshcat"}, + ), +) diff --git a/dimos/robot/manipulators/openarm/config.py b/dimos/robot/manipulators/openarm/config.py new file mode 100644 index 0000000000..e4faee3b90 --- /dev/null +++ b/dimos/robot/manipulators/openarm/config.py @@ -0,0 +1,134 @@ +# 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. + +"""OpenArm hardware and planning model configuration helpers.""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any + +from dimos.control.components import HardwareComponent, HardwareType +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.robot.manipulators._modeling import base_pose +from dimos.utils.data import LfsPath + +OPENARM_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ + ("openarm_left_link5", "openarm_left_link7"), + ("openarm_right_link5", "openarm_right_link7"), +] + +OPENARM_PKG = LfsPath("openarm_description") +OPENARM_LEFT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_left.urdf" +OPENARM_RIGHT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_right.urdf" +OPENARM_V10_FK_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_single.urdf" +OPENARM_PACKAGE_PATHS: dict[str, Path] = {"openarm_description": OPENARM_PKG} + +# Linux assigns can0/can1 in USB enumeration order, which is not guaranteed stable. +# Flip these if physical arms come up swapped. +LEFT_CAN = "can1" +RIGHT_CAN = "can0" + +# Leave true for normal operation; it is idempotent and ensures motors are in +# the expected CTRL_MODE=MIT mode at connect time. +AUTO_SET_MIT_MODE = True +OPENARM_ADAPTER_KWARGS = {"auto_set_mit_mode": AUTO_SET_MIT_MODE} + + +def validate_side(side: str) -> None: + if side not in ("left", "right"): + raise ValueError(f"side must be 'left' or 'right', got {side!r}") + + +def openarm_joints(side: str) -> list[str]: + validate_side(side) + return [f"openarm_{side}_joint{i}" for i in range(1, 8)] + + +def openarm_hardware( + side: str, + name: str | None = None, + *, + adapter_type: str = "mock", + address: str | None = None, + adapter_kwargs: dict[str, Any] | None = None, + scoped_joints: bool = False, +) -> HardwareComponent: + validate_side(side) + resolved_name = name or f"{side}_arm" + local_joints = openarm_joints(side) + joints = ( + [f"{resolved_name}/{joint}" for joint in local_joints] if scoped_joints else local_joints + ) + kwargs = {"side": side} + if adapter_kwargs: + kwargs.update(adapter_kwargs) + return HardwareComponent( + hardware_id=resolved_name, + hardware_type=HardwareType.MANIPULATOR, + joints=joints, + adapter_type=adapter_type, + address=address, + adapter_kwargs=kwargs, + ) + + +def openarm_model_config(side: str, name: str | None = None) -> RobotModelConfig: + validate_side(side) + resolved_name = name or f"{side}_arm" + return RobotModelConfig( + name=resolved_name, + model_path=OPENARM_LEFT_MODEL if side == "left" else OPENARM_RIGHT_MODEL, + base_pose=base_pose(), + joint_names=openarm_joints(side), + end_effector_link=f"openarm_{side}_link7", + base_link="openarm_body_link0", + package_paths=OPENARM_PACKAGE_PATHS, + collision_exclusion_pairs=OPENARM_COLLISION_EXCLUSIONS, + auto_convert_meshes=True, + max_velocity=0.5, + max_acceleration=1.0, + coordinator_task_name=f"traj_{resolved_name}", + home_joints=[0.0] * 7, + ) + + +def openarm_single_hardware( + *, + adapter_type: str = "mock", + address: str | None = None, +) -> HardwareComponent: + return openarm_hardware( + "left", + name="arm", + adapter_type=adapter_type, + address=address, + ) + + +def openarm_single_model_config() -> RobotModelConfig: + return RobotModelConfig( + name="arm", + model_path=OPENARM_V10_FK_MODEL, + base_pose=base_pose(), + joint_names=openarm_joints("left"), + end_effector_link="openarm_left_link7", + base_link="openarm_body_link0", + package_paths=OPENARM_PACKAGE_PATHS, + auto_convert_meshes=True, + max_velocity=0.5, + max_acceleration=1.0, + coordinator_task_name="traj_arm", + home_joints=[0.0] * 7, + ) diff --git a/dimos/robot/manipulators/piper/blueprints.py b/dimos/robot/manipulators/piper/blueprints.py deleted file mode 100644 index b9f8cac4dc..0000000000 --- a/dimos/robot/manipulators/piper/blueprints.py +++ /dev/null @@ -1,107 +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. - -"""Keyboard teleop blueprint for the Piper arm. - -Launches the ControlCoordinator (mock adapter + CartesianIK), the -ManipulationModule (Drake/Meshcat visualization), and a pygame keyboard -teleop UI — all wired together via autoconnect. - -Usage: - dimos run keyboard-teleop-piper -""" - -from pathlib import Path - -from dimos.control.blueprints._hardware import PIPER_FK_MODEL, manipulator -from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.core.coordination.blueprints import autoconnect -from dimos.core.global_config import global_config -from dimos.manipulation.manipulation_module import ManipulationModule -from dimos.manipulation.planning.spec.config import RobotModelConfig -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.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -from dimos.utils.data import LfsPath - -PIPER_GRIPPER_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ - ("gripper_base", "link7"), - ("gripper_base", "link8"), - ("link7", "link8"), - ("link6", "gripper_base"), -] - -_PIPER_MODEL_PATH = LfsPath("piper_description") / "urdf/piper_description.xacro" -_PIPER_PACKAGE_PATHS: dict[str, Path] = { - "piper_description": LfsPath("piper_description"), - "piper_gazebo": LfsPath("piper_description"), -} - - -def _piper_model_config() -> RobotModelConfig: - return RobotModelConfig( - name="arm", - model_path=_PIPER_MODEL_PATH, - base_pose=PoseStamped( - position=Vector3(x=0.0, y=0.0, z=0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - ), - joint_names=[f"joint{i}" for i in range(1, 7)], - end_effector_link="gripper_base", - base_link="base_link", - package_paths=_PIPER_PACKAGE_PATHS, - auto_convert_meshes=True, - collision_exclusion_pairs=PIPER_GRIPPER_COLLISION_EXCLUSIONS, - coordinator_task_name="traj_arm", - gripper_hardware_id="arm", - home_joints=[0.0] * 6, - ) - - -_piper_hw = manipulator( - "arm", - 6, - adapter_type="piper" if global_config.can_port else "mock", - address=global_config.can_port or "can0", - gripper=True, -) - -# Piper 6-DOF mock sim + keyboard teleop + Drake visualization -keyboard_teleop_piper = autoconnect( - KeyboardTeleopModule.blueprint( - model_path=PIPER_FK_MODEL, - ee_joint_id=6, - joint_names=_piper_hw.joints, - ), - ControlCoordinator.blueprint( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[_piper_hw], - tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_piper_hw.joints, - priority=10, - params={"model_path": PIPER_FK_MODEL, "ee_joint_id": 6}, - ), - ], - ), - ManipulationModule.blueprint( - robots=[_piper_model_config()], - visualization={"backend": "meshcat"}, - ), -) diff --git a/dimos/robot/manipulators/piper/blueprints/basic.py b/dimos/robot/manipulators/piper/blueprints/basic.py new file mode 100644 index 0000000000..415c08914d --- /dev/null +++ b/dimos/robot/manipulators/piper/blueprints/basic.py @@ -0,0 +1,39 @@ +# 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. + +"""Basic Piper coordinator blueprints.""" + +from __future__ import annotations + +from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.core.coordination.blueprints import autoconnect +from dimos.robot.manipulators.common.sim import mujoco_if_sim +from dimos.robot.manipulators.piper.config import PIPER_SIM_PATH, piper_hardware + +_piper_hw = piper_hardware("arm") + +coordinator_piper = autoconnect( + ControlCoordinator.blueprint( + hardware=[_piper_hw], + tasks=[ + TaskConfig( + name="traj_piper", + type="trajectory", + joint_names=_piper_hw.joints, + priority=10, + ) + ], + ), + *mujoco_if_sim(PIPER_SIM_PATH, len(_piper_hw.joints)), +) diff --git a/dimos/robot/manipulators/piper/blueprints/teleop.py b/dimos/robot/manipulators/piper/blueprints/teleop.py new file mode 100644 index 0000000000..df3d78d256 --- /dev/null +++ b/dimos/robot/manipulators/piper/blueprints/teleop.py @@ -0,0 +1,104 @@ +# 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. + +"""Piper teleop blueprints.""" + +from __future__ import annotations + +from dimos.control.components import make_gripper_joints +from dimos.control.coordinator import ControlCoordinator +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.robot.manipulators.common.blueprints import cartesian_ik_task, teleop_ik_task +from dimos.robot.manipulators.common.sim import mujoco_if_sim +from dimos.robot.manipulators.piper.config import ( + PIPER_FK_MODEL, + PIPER_SIM_PATH, + make_piper_hardware, + make_piper_model_config, + piper_hardware, +) +from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule + +_piper_keyboard_hw = make_piper_hardware( + "arm", + adapter_type="piper" if global_config.can_port else "mock", + address=global_config.can_port or "can0", + gripper=True, +) + +keyboard_teleop_piper = autoconnect( + KeyboardTeleopModule.blueprint( + model_path=PIPER_FK_MODEL, + ee_joint_id=6, + joint_names=_piper_keyboard_hw.joints, + ), + ControlCoordinator.blueprint( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[_piper_keyboard_hw], + tasks=[cartesian_ik_task(_piper_keyboard_hw, model_path=PIPER_FK_MODEL, ee_joint_id=6)], + ), + ManipulationModule.blueprint( + robots=[make_piper_model_config()], + visualization={"backend": "meshcat"}, + ), +) + +_piper_mock_cartesian_hw = make_piper_hardware( + "arm", + gripper=False, +) + +coordinator_cartesian_ik_mock = ControlCoordinator.blueprint( + hardware=[_piper_mock_cartesian_hw], + tasks=[cartesian_ik_task(_piper_mock_cartesian_hw, model_path=PIPER_FK_MODEL, ee_joint_id=6)], +) + +_piper_teleop_hw = piper_hardware("arm") + +coordinator_teleop_piper = autoconnect( + ControlCoordinator.blueprint( + hardware=[_piper_teleop_hw], + tasks=[ + teleop_ik_task( + _piper_teleop_hw, + model_path=PIPER_FK_MODEL, + ee_joint_id=6, + hand="left", + name="teleop_piper", + params={ + "gripper_joint": make_gripper_joints("arm")[0], + "gripper_open_pos": 0.0, + "gripper_closed_pos": 0.035, + }, + ), + ], + ), + *mujoco_if_sim(PIPER_SIM_PATH, len(_piper_teleop_hw.joints)), +) + +_piper_cartesian_hw = make_piper_hardware( + "arm", + adapter_type="piper", + address=global_config.can_port or "can0", + gripper=True, +) + +coordinator_cartesian_ik_piper = ControlCoordinator.blueprint( + hardware=[_piper_cartesian_hw], + tasks=[cartesian_ik_task(_piper_cartesian_hw, model_path=PIPER_FK_MODEL, ee_joint_id=6)], +) diff --git a/dimos/robot/manipulators/piper/config.py b/dimos/robot/manipulators/piper/config.py new file mode 100644 index 0000000000..f99e9589a5 --- /dev/null +++ b/dimos/robot/manipulators/piper/config.py @@ -0,0 +1,131 @@ +# 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. + +"""Piper planning model configuration helpers.""" + +from __future__ import annotations + +from pathlib import Path + +from dimos.control.components import HardwareComponent, HardwareType, make_joints +from dimos.core.global_config import global_config +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.robot.manipulators._modeling import ( + base_pose, + coordinator_joint_mapping, + joint_names, +) +from dimos.utils.data import LfsPath + +PIPER_GRIPPER_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ + ("gripper_base", "link7"), + ("gripper_base", "link8"), + ("link7", "link8"), + ("link6", "gripper_base"), +] + +PIPER_MODEL_PATH = LfsPath("piper_description") / "urdf/piper_description.xacro" +PIPER_PACKAGE_PATHS: dict[str, Path] = { + "piper_description": LfsPath("piper_description"), + "piper_gazebo": LfsPath("piper_description"), +} +PIPER_FK_MODEL = LfsPath("piper_description/mujoco_model/piper_no_gripper_description.xml") +PIPER_SIM_PATH = LfsPath("piper/scene.xml") + + +def _adapter_kwargs(home_joints: list[float] | None = None) -> dict[str, object]: + if home_joints is None: + return {} + return {"initial_positions": home_joints} + + +def make_piper_hardware( + hw_id: str = "arm", + *, + adapter_type: str = "mock", + address: str | None = None, + gripper: bool = True, + auto_enable: bool = True, + adapter_kwargs: dict[str, object] | None = None, + home_joints: list[float] | None = None, +) -> HardwareComponent: + kwargs = _adapter_kwargs(home_joints) + if adapter_kwargs: + kwargs.update(adapter_kwargs) + return HardwareComponent( + hardware_id=hw_id, + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints(hw_id, 6), + adapter_type=adapter_type, + address=address, + auto_enable=auto_enable, + gripper_joints=[f"{hw_id}/gripper"] if gripper else [], + adapter_kwargs=kwargs, + ) + + +def piper_hardware( + hw_id: str = "arm", + *, + gripper: bool = True, + mock_without_address: bool = False, + home_joints: list[float] | None = None, +) -> HardwareComponent: + if global_config.simulation: + return make_piper_hardware( + hw_id, + adapter_type="sim_mujoco", + address=str(PIPER_SIM_PATH), + gripper=gripper, + home_joints=home_joints, + ) + address = global_config.can_port or "can0" + if mock_without_address and not global_config.can_port: + return make_piper_hardware(hw_id, gripper=gripper, home_joints=home_joints) + return make_piper_hardware( + hw_id, + adapter_type="piper", + address=address, + gripper=gripper, + home_joints=home_joints, + ) + + +def make_piper_model_config( + name: str = "arm", + *, + joint_prefix: str | None = None, + coordinator_task_name: str | None = None, + home_joints: list[float] | None = None, +) -> RobotModelConfig: + dof = 6 + return RobotModelConfig( + name=name, + model_path=PIPER_MODEL_PATH, + base_pose=base_pose(), + joint_names=joint_names(dof), + end_effector_link="gripper_base", + base_link="base_link", + package_paths=PIPER_PACKAGE_PATHS, + auto_convert_meshes=True, + collision_exclusion_pairs=PIPER_GRIPPER_COLLISION_EXCLUSIONS, + joint_name_mapping=coordinator_joint_mapping( + name, + dof, + joint_prefix=joint_prefix, + ), + coordinator_task_name=coordinator_task_name or f"traj_{name}", + gripper_hardware_id=name, + home_joints=home_joints or [0.0] * dof, + ) diff --git a/dimos/robot/manipulators/xarm/blueprints.py b/dimos/robot/manipulators/xarm/blueprints.py deleted file mode 100644 index af4b6d92d0..0000000000 --- a/dimos/robot/manipulators/xarm/blueprints.py +++ /dev/null @@ -1,136 +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. - -"""Keyboard teleop blueprints for XArm6 and XArm7. - -Launches the ControlCoordinator (mock adapter + CartesianIK), the -ManipulationModule (Drake/Meshcat visualization), and a pygame keyboard -teleop UI — all wired together via autoconnect. - -Usage: - dimos run keyboard-teleop-xarm6 - dimos run keyboard-teleop-xarm7 -""" - -from pathlib import Path - -from dimos.control.blueprints._hardware import XARM6_FK_MODEL, XARM7_FK_MODEL, manipulator -from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.core.coordination.blueprints import autoconnect -from dimos.core.global_config import global_config -from dimos.manipulation.manipulation_module import ManipulationModule -from dimos.manipulation.planning.spec.config import RobotModelConfig -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.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -from dimos.utils.data import LfsPath - -_XARM_MODEL_PATH = LfsPath("xarm_description") / "urdf/xarm_device.urdf.xacro" -_XARM_PACKAGE_PATHS: dict[str, Path] = {"xarm_description": LfsPath("xarm_description")} - - -def _xarm_model_config(dof: int) -> RobotModelConfig: - return RobotModelConfig( - name="arm", - model_path=_XARM_MODEL_PATH, - base_pose=PoseStamped( - position=Vector3(x=0.0, y=0.0, z=0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - ), - strip_model_world_joint=True, - joint_names=[f"joint{i}" for i in range(1, dof + 1)], - end_effector_link=f"link{dof}", - base_link="link_base", - package_paths=_XARM_PACKAGE_PATHS, - xacro_args={ - "dof": str(dof), - "limited": "true", - "attach_xyz": "0 0 0", - "attach_rpy": "0 0 0", - }, - auto_convert_meshes=True, - coordinator_task_name="traj_arm", - home_joints=[0.0] * dof, - ) - - -_xarm6_hw = manipulator( - "arm", - 6, - adapter_type="xarm" if global_config.xarm6_ip else "mock", - address=global_config.xarm6_ip or None, -) -_xarm7_hw = manipulator( - "arm", - 7, - adapter_type="xarm" if global_config.xarm7_ip else "mock", - address=global_config.xarm7_ip or None, -) - -# XArm6 mock sim + keyboard teleop + Drake visualization -keyboard_teleop_xarm6 = autoconnect( - KeyboardTeleopModule.blueprint( - model_path=XARM6_FK_MODEL, - ee_joint_id=6, - joint_names=_xarm6_hw.joints, - ), - ControlCoordinator.blueprint( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[_xarm6_hw], - tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_xarm6_hw.joints, - priority=10, - params={"model_path": XARM6_FK_MODEL, "ee_joint_id": 6}, - ), - ], - ), - ManipulationModule.blueprint( - robots=[_xarm_model_config(6)], - visualization={"backend": "meshcat"}, - ), -) - -# XArm7 mock sim + keyboard teleop + Drake visualization -keyboard_teleop_xarm7 = autoconnect( - KeyboardTeleopModule.blueprint( - model_path=XARM7_FK_MODEL, - ee_joint_id=7, - joint_names=_xarm7_hw.joints, - ), - ControlCoordinator.blueprint( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[_xarm7_hw], - tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_xarm7_hw.joints, - priority=10, - params={"model_path": XARM7_FK_MODEL, "ee_joint_id": 7}, - ), - ], - ), - ManipulationModule.blueprint( - robots=[_xarm_model_config(7)], - visualization={"backend": "meshcat"}, - ), -) diff --git a/dimos/robot/manipulators/xarm/blueprints/agentic.py b/dimos/robot/manipulators/xarm/blueprints/agentic.py new file mode 100644 index 0000000000..073ede3607 --- /dev/null +++ b/dimos/robot/manipulators/xarm/blueprints/agentic.py @@ -0,0 +1,46 @@ +# 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. + +"""Agentic xArm manipulation blueprints.""" + +from __future__ import annotations + +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.robot.manipulators.common.agent_prompts import ( + BASE_MANIPULATION_AGENT_SYSTEM_PROMPT, + MANIPULATION_AGENT_SYSTEM_PROMPT, +) +from dimos.robot.manipulators.xarm.blueprints.basic import xarm7_planner_coordinator +from dimos.robot.manipulators.xarm.blueprints.perception import xarm_perception +from dimos.robot.manipulators.xarm.blueprints.simulation import xarm_perception_sim + +xarm7_planner_coordinator_agent = autoconnect( + xarm7_planner_coordinator, + McpServer.blueprint(), + 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_perception_sim_agent = autoconnect( + xarm_perception_sim, + McpServer.blueprint(), + McpClient.blueprint(system_prompt=MANIPULATION_AGENT_SYSTEM_PROMPT), +) diff --git a/dimos/robot/manipulators/xarm/blueprints/basic.py b/dimos/robot/manipulators/xarm/blueprints/basic.py new file mode 100644 index 0000000000..4f1660342e --- /dev/null +++ b/dimos/robot/manipulators/xarm/blueprints/basic.py @@ -0,0 +1,91 @@ +# 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. + +"""Basic xArm coordinator and planner blueprints.""" + +from __future__ import annotations + +from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.core.coordination.blueprints import autoconnect +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.robot.manipulators.common.blueprints import coordinator, planner, trajectory_task +from dimos.robot.manipulators.common.sim import mujoco_if_sim +from dimos.robot.manipulators.xarm.config import ( + XARM6_SIM_PATH, + XARM7_SIM_PATH, + make_xarm6_model_config, + make_xarm7_model_config, + xarm6_hardware, + xarm7_hardware, +) + +xarm6_planner_only = ManipulationModule.blueprint( + robots=[make_xarm6_model_config(name="arm")], + planning_timeout=10.0, + visualization={"backend": "meshcat"}, +) + +dual_xarm6_planner = ManipulationModule.blueprint( + robots=[ + make_xarm6_model_config(name="left_arm", y_offset=0.5), + make_xarm6_model_config(name="right_arm", y_offset=-0.5), + ], + planning_timeout=10.0, + visualization={"backend": "meshcat"}, +) + +_xarm7_hw = xarm7_hardware("arm", gripper=True, mock_without_address=True) + +xarm7_planner_coordinator = autoconnect( + planner(robots=[make_xarm7_model_config(name="arm", add_gripper=True)]), + coordinator( + hardware=[_xarm7_hw], + tasks=[trajectory_task(_xarm7_hw)], + ), +) + +_coordinator_xarm7_hw = xarm7_hardware("arm") + +coordinator_xarm7 = autoconnect( + coordinator( + hardware=[_coordinator_xarm7_hw], + tasks=[trajectory_task(_coordinator_xarm7_hw)], + ), + *mujoco_if_sim(XARM7_SIM_PATH, len(_coordinator_xarm7_hw.joints)), +) + +_coordinator_xarm6_hw = xarm6_hardware("arm", gripper=True) + +coordinator_xarm6 = autoconnect( + coordinator( + hardware=[_coordinator_xarm6_hw], + tasks=[trajectory_task(_coordinator_xarm6_hw)], + ), + *mujoco_if_sim(XARM6_SIM_PATH, len(_coordinator_xarm6_hw.joints)), +) + +_xarm7_left = xarm7_hardware("left_arm") +_xarm6_right = xarm6_hardware("right_arm") + +coordinator_dual_xarm = ControlCoordinator.blueprint( + hardware=[_xarm7_left, _xarm6_right], + tasks=[ + TaskConfig( + name="traj_left", type="trajectory", joint_names=_xarm7_left.joints, priority=10 + ), + TaskConfig( + name="traj_right", type="trajectory", joint_names=_xarm6_right.joints, priority=10 + ), + ], +) diff --git a/dimos/robot/manipulators/xarm/blueprints/perception.py b/dimos/robot/manipulators/xarm/blueprints/perception.py new file mode 100644 index 0000000000..d0ff469251 --- /dev/null +++ b/dimos/robot/manipulators/xarm/blueprints/perception.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. + +"""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.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, + ), + 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 new file mode 100644 index 0000000000..d4c397f48b --- /dev/null +++ b/dimos/robot/manipulators/xarm/blueprints/simulation.py @@ -0,0 +1,72 @@ +# 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. + +"""Simulation xArm perception manipulation blueprints.""" + +from __future__ import annotations + +import math + +from dimos.core.coordination.blueprints import autoconnect +from dimos.manipulation.pick_and_place_module import PickAndPlaceModule +from dimos.perception.object_scene_registration import ObjectSceneRegistrationModule +from dimos.robot.manipulators.common.blueprints import coordinator, trajectory_task +from dimos.robot.manipulators.xarm.config import ( + XARM7_SIM_PATH, + make_xarm7_model_config, + make_xarm_hardware, +) +from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule +from dimos.visualization.rerun.bridge import RerunBridgeModule + +XARM7_SIM_HOME = [0.0, 0.0, 0.0, 0.0, 0.0, -0.7, 0.0] + +_xarm7_sim_hw = make_xarm_hardware( + "arm", + 7, + adapter_type="sim_mujoco", + address=str(XARM7_SIM_PATH), + gripper=True, + home_joints=XARM7_SIM_HOME, +) + +xarm_perception_sim = autoconnect( + PickAndPlaceModule.blueprint( + robots=[ + make_xarm7_model_config( + name="arm", + add_gripper=True, + pitch=math.radians(45), + tf_extra_links=["link7"], + home_joints=XARM7_SIM_HOME, + pre_grasp_offset=0.05, + ) + ], + planning_timeout=10.0, + visualization={"backend": "meshcat"}, + ), + MujocoSimModule.blueprint( + address=str(XARM7_SIM_PATH), + headless=False, + dof=7, + camera_name="wrist_camera", + base_frame_id="link7", + ), + ObjectSceneRegistrationModule.blueprint(target_frame="world"), + coordinator( + hardware=[_xarm7_sim_hw], + tasks=[trajectory_task(_xarm7_sim_hw)], + ), + RerunBridgeModule.blueprint(), +) diff --git a/dimos/robot/manipulators/xarm/blueprints/teleop.py b/dimos/robot/manipulators/xarm/blueprints/teleop.py new file mode 100644 index 0000000000..e74c64001e --- /dev/null +++ b/dimos/robot/manipulators/xarm/blueprints/teleop.py @@ -0,0 +1,183 @@ +# 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. + +"""Keyboard teleop blueprints for xArm6 and xArm7.""" + +from __future__ import annotations + +from dimos.control.components import make_gripper_joints +from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.robot.manipulators.common.blueprints import cartesian_ik_task, teleop_ik_task +from dimos.robot.manipulators.common.sim import mujoco_if_sim +from dimos.robot.manipulators.xarm.config import ( + XARM6_FK_MODEL, + XARM6_SIM_PATH, + XARM7_FK_MODEL, + XARM7_SIM_PATH, + make_xarm6_model_config, + make_xarm7_model_config, + make_xarm_hardware, + xarm6_hardware, + xarm7_hardware, +) +from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule + +_xarm6_hw = make_xarm_hardware( + "arm", + 6, + adapter_type="xarm" if global_config.xarm6_ip else "mock", + address=global_config.xarm6_ip, +) +_xarm7_hw = make_xarm_hardware( + "arm", + 7, + adapter_type="xarm" if global_config.xarm7_ip else "mock", + address=global_config.xarm7_ip, +) + +keyboard_teleop_xarm6 = autoconnect( + KeyboardTeleopModule.blueprint( + model_path=XARM6_FK_MODEL, + ee_joint_id=6, + joint_names=_xarm6_hw.joints, + ), + ControlCoordinator.blueprint( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[_xarm6_hw], + tasks=[cartesian_ik_task(_xarm6_hw, model_path=XARM6_FK_MODEL, ee_joint_id=6)], + ), + ManipulationModule.blueprint( + robots=[make_xarm6_model_config(add_gripper=False)], + visualization={"backend": "meshcat"}, + ), +) + +keyboard_teleop_xarm7 = autoconnect( + KeyboardTeleopModule.blueprint( + model_path=XARM7_FK_MODEL, + ee_joint_id=7, + joint_names=_xarm7_hw.joints, + ), + ControlCoordinator.blueprint( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[_xarm7_hw], + tasks=[cartesian_ik_task(_xarm7_hw, model_path=XARM7_FK_MODEL, ee_joint_id=7)], + ), + ManipulationModule.blueprint( + robots=[make_xarm7_model_config(add_gripper=False)], + visualization={"backend": "meshcat"}, + ), +) + +_xarm6_control_hw = make_xarm_hardware( + "arm", + 6, + adapter_type="xarm", + address=global_config.xarm6_ip, + gripper=True, +) + +coordinator_servo_xarm6 = ControlCoordinator.blueprint( + hardware=[_xarm6_control_hw], + tasks=[ + TaskConfig( + name="servo_arm", + type="servo", + joint_names=_xarm6_control_hw.joints, + priority=10, + ), + ], +) + +coordinator_velocity_xarm6 = ControlCoordinator.blueprint( + hardware=[_xarm6_control_hw], + tasks=[ + TaskConfig( + name="velocity_arm", + type="velocity", + joint_names=_xarm6_control_hw.joints, + priority=10, + ), + ], +) + +coordinator_combined_xarm6 = ControlCoordinator.blueprint( + hardware=[_xarm6_control_hw], + tasks=[ + TaskConfig( + name="servo_arm", + type="servo", + joint_names=_xarm6_control_hw.joints, + priority=10, + ), + TaskConfig( + name="velocity_arm", + type="velocity", + joint_names=_xarm6_control_hw.joints, + priority=10, + ), + ], +) + +_xarm7_teleop_hw = xarm7_hardware("arm", gripper=True) +_xarm6_teleop_hw = xarm6_hardware("arm", gripper=True) + +coordinator_teleop_xarm7 = autoconnect( + ControlCoordinator.blueprint( + hardware=[_xarm7_teleop_hw], + tasks=[ + teleop_ik_task( + _xarm7_teleop_hw, + model_path=XARM7_FK_MODEL, + ee_joint_id=7, + hand="right", + name="teleop_xarm", + params={ + "gripper_joint": make_gripper_joints("arm")[0], + "gripper_open_pos": 0.85, + "gripper_closed_pos": 0.0, + }, + ), + ], + ), + *mujoco_if_sim(XARM7_SIM_PATH, len(_xarm7_teleop_hw.joints)), +) + +coordinator_teleop_xarm6 = autoconnect( + ControlCoordinator.blueprint( + hardware=[_xarm6_teleop_hw], + tasks=[ + teleop_ik_task( + _xarm6_teleop_hw, + model_path=XARM6_FK_MODEL, + ee_joint_id=6, + hand="right", + name="teleop_xarm", + params={ + "gripper_joint": make_gripper_joints("arm")[0], + "gripper_open_pos": 0.85, + "gripper_closed_pos": 0.0, + }, + ), + ], + ), + *mujoco_if_sim(XARM6_SIM_PATH, len(_xarm6_teleop_hw.joints)), +) diff --git a/dimos/robot/manipulators/xarm/config.py b/dimos/robot/manipulators/xarm/config.py new file mode 100644 index 0000000000..d918ee94a8 --- /dev/null +++ b/dimos/robot/manipulators/xarm/config.py @@ -0,0 +1,208 @@ +# 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. + +"""xArm planning model configuration helpers.""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any + +from dimos.control.components import HardwareComponent, HardwareType, make_joints +from dimos.core.global_config import global_config +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.robot.manipulators._modeling import ( + base_pose, + coordinator_joint_mapping, + joint_names, +) +from dimos.utils.data import LfsPath + +XARM_GRIPPER_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ + ("right_inner_knuckle", "right_outer_knuckle"), + ("left_inner_knuckle", "left_outer_knuckle"), + ("right_inner_knuckle", "right_finger"), + ("left_inner_knuckle", "left_finger"), + ("left_finger", "right_finger"), + ("left_outer_knuckle", "right_outer_knuckle"), + ("left_inner_knuckle", "right_inner_knuckle"), + ("left_outer_knuckle", "right_finger"), + ("right_outer_knuckle", "left_finger"), + ("xarm_gripper_base_link", "left_inner_knuckle"), + ("xarm_gripper_base_link", "right_inner_knuckle"), + ("xarm_gripper_base_link", "left_finger"), + ("xarm_gripper_base_link", "right_finger"), + ("link6", "xarm_gripper_base_link"), + ("link6", "left_outer_knuckle"), + ("link6", "right_outer_knuckle"), +] + +XARM_MODEL_PATH = LfsPath("xarm_description") / "urdf/xarm_device.urdf.xacro" +XARM_PACKAGE_PATHS: dict[str, Path] = {"xarm_description": LfsPath("xarm_description")} +XARM6_FK_MODEL = LfsPath("xarm_description/urdf/xarm6/xarm6.urdf") +XARM7_FK_MODEL = LfsPath("xarm_description/urdf/xarm7/xarm7.urdf") +XARM6_SIM_PATH = LfsPath("xarm6/scene.xml") +XARM7_SIM_PATH = LfsPath("xarm7/scene.xml") + + +def _adapter_kwargs(home_joints: list[float] | None = None) -> dict[str, object]: + if home_joints is None: + return {} + return {"initial_positions": home_joints} + + +def make_xarm_hardware( + hw_id: str, + dof: int, + *, + adapter_type: str = "mock", + address: str | None = None, + gripper: bool = False, + auto_enable: bool = True, + adapter_kwargs: dict[str, object] | None = None, + home_joints: list[float] | None = None, +) -> HardwareComponent: + kwargs = _adapter_kwargs(home_joints) + if adapter_kwargs: + kwargs.update(adapter_kwargs) + return HardwareComponent( + hardware_id=hw_id, + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints(hw_id, dof), + adapter_type=adapter_type, + address=address, + auto_enable=auto_enable, + gripper_joints=[f"{hw_id}/gripper"] if gripper else [], + adapter_kwargs=kwargs, + ) + + +def xarm7_hardware( + hw_id: str = "arm", + *, + gripper: bool = False, + mock_without_address: bool = False, + home_joints: list[float] | None = None, +) -> HardwareComponent: + if global_config.simulation: + return make_xarm_hardware( + hw_id, + 7, + adapter_type="sim_mujoco", + address=str(XARM7_SIM_PATH), + gripper=gripper, + home_joints=home_joints, + ) + address = global_config.xarm7_ip + if mock_without_address and not address: + return make_xarm_hardware(hw_id, 7, gripper=gripper, home_joints=home_joints) + return make_xarm_hardware( + hw_id, + 7, + adapter_type="xarm", + address=address, + gripper=gripper, + home_joints=home_joints, + ) + + +def xarm6_hardware( + hw_id: str = "arm", + *, + gripper: bool = False, + mock_without_address: bool = False, + home_joints: list[float] | None = None, +) -> HardwareComponent: + if global_config.simulation: + return make_xarm_hardware( + hw_id, + 6, + adapter_type="sim_mujoco", + address=str(XARM6_SIM_PATH), + gripper=gripper, + home_joints=home_joints, + ) + address = global_config.xarm6_ip + if mock_without_address and not address: + return make_xarm_hardware(hw_id, 6, gripper=gripper, home_joints=home_joints) + return make_xarm_hardware( + hw_id, + 6, + adapter_type="xarm", + address=address, + gripper=gripper, + home_joints=home_joints, + ) + + +def make_xarm_model_config( + name: str, + dof: int, + *, + add_gripper: bool = True, + x_offset: float = 0.0, + y_offset: float = 0.0, + z_offset: float = 0.0, + pitch: float = 0.0, + joint_prefix: str | None = None, + coordinator_task_name: str | None = None, + tf_extra_links: list[str] | None = None, + home_joints: list[float] | None = None, + pre_grasp_offset: float = 0.10, +) -> RobotModelConfig: + xacro_args = { + "dof": str(dof), + "limited": "true", + "attach_xyz": f"{x_offset} {y_offset} {z_offset}", + "attach_rpy": f"0 {pitch} 0", + } + if add_gripper: + xacro_args["add_gripper"] = "true" + + return RobotModelConfig( + name=name, + model_path=XARM_MODEL_PATH, + base_pose=base_pose(x_offset, y_offset, z_offset), + joint_names=joint_names(dof), + end_effector_link="link_tcp" if add_gripper else f"link{dof}", + base_link="link_base", + package_paths=XARM_PACKAGE_PATHS, + xacro_args=xacro_args, + auto_convert_meshes=True, + collision_exclusion_pairs=(XARM_GRIPPER_COLLISION_EXCLUSIONS if add_gripper else []), + joint_name_mapping=coordinator_joint_mapping( + name, + dof, + joint_prefix=joint_prefix, + ), + coordinator_task_name=coordinator_task_name or f"traj_{name}", + gripper_hardware_id=name if add_gripper else None, + tf_extra_links=tf_extra_links or [], + home_joints=home_joints or [0.0] * dof, + pre_grasp_offset=pre_grasp_offset, + ) + + +def make_xarm6_model_config( + name: str = "arm", + **kwargs: Any, +) -> RobotModelConfig: + return make_xarm_model_config(name, 6, **kwargs) + + +def make_xarm7_model_config( + name: str = "arm", + **kwargs: Any, +) -> RobotModelConfig: + return make_xarm_model_config(name, 7, **kwargs) diff --git a/dimos/robot/test_all_blueprints.py b/dimos/robot/test_all_blueprints.py index b0929ff1b6..cdf72e9b6b 100644 --- a/dimos/robot/test_all_blueprints.py +++ b/dimos/robot/test_all_blueprints.py @@ -49,8 +49,7 @@ "coordinator-velocity-xarm6", "coordinator-xarm6", "coordinator-xarm7", - "dual-xarm6-planner-coordinator", - "dual-xarm7-planner-coordinator", + "dual-xarm6-planner", "teleop-hosted-go2", "teleop-hosted-xarm7", "teleop-quest-dual", diff --git a/dimos/teleop/quest/blueprints.py b/dimos/teleop/quest/blueprints.py index 24c4bfc2e1..0b6644960a 100644 --- a/dimos/teleop/quest/blueprints.py +++ b/dimos/teleop/quest/blueprints.py @@ -20,17 +20,17 @@ """ from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE -from dimos.control.blueprints.teleop import ( - coordinator_teleop_dual, - coordinator_teleop_piper, - coordinator_teleop_xarm6, - coordinator_teleop_xarm7, -) from dimos.core.coordination.blueprints import autoconnect from dimos.core.transport import LCMTransport, pSHMTransport from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.sensor_msgs.Image import Image +from dimos.robot.manipulators.common.mixed import coordinator_teleop_dual +from dimos.robot.manipulators.piper.blueprints.teleop import coordinator_teleop_piper +from dimos.robot.manipulators.xarm.blueprints.teleop import ( + coordinator_teleop_xarm6, + coordinator_teleop_xarm7, +) from dimos.robot.unitree.go2.connection import GO2Connection from dimos.teleop.quest.quest_extensions import ( ArmTeleopModule, diff --git a/dimos/teleop/quest_hosted/blueprints.py b/dimos/teleop/quest_hosted/blueprints.py index 199ae0c3ba..5a182f9118 100644 --- a/dimos/teleop/quest_hosted/blueprints.py +++ b/dimos/teleop/quest_hosted/blueprints.py @@ -17,8 +17,8 @@ from pathlib import Path from dimos.constants import STATE_DIR -from dimos.control.blueprints.teleop import coordinator_teleop_xarm7 from dimos.core.coordination.blueprints import autoconnect +from dimos.robot.manipulators.xarm.blueprints.teleop import coordinator_teleop_xarm7 from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic from dimos.teleop.quest_hosted.hosted_extensions import ( HostedArmTeleopModule, diff --git a/docs/capabilities/manipulation/a750.md b/docs/capabilities/manipulation/a750.md index 8f4402ead0..ea1117eb6e 100644 --- a/docs/capabilities/manipulation/a750.md +++ b/docs/capabilities/manipulation/a750.md @@ -49,8 +49,10 @@ DEVICE_PATH=/dev/ttyACM0 dimos run keyboard-teleop-a750 ## Robot Model -The robot model, hardware, and task config used by the runnable stack are -defined in [`dimos/robot/manipulators/a750/blueprints.py`](/dimos/robot/manipulators/a750/blueprints.py). +The robot model and hardware config are defined in +[`dimos/robot/manipulators/a750/config.py`](/dimos/robot/manipulators/a750/config.py). +The runnable keyboard teleop stack is composed in +[`dimos/robot/manipulators/a750/blueprints/teleop.py`](/dimos/robot/manipulators/a750/blueprints/teleop.py). | Field | Value | |-------|-------| @@ -108,7 +110,8 @@ If `a750_control` is not installed, `connect()` records an error and returns `Fa ## Control Path -The `keyboard-teleop-a750` blueprint is defined in [`dimos/robot/manipulators/a750/blueprints.py`](/dimos/robot/manipulators/a750/blueprints.py#L22). +The `keyboard-teleop-a750` blueprint is defined in +[`dimos/robot/manipulators/a750/blueprints/teleop.py`](/dimos/robot/manipulators/a750/blueprints/teleop.py). ```text KeyboardTeleopModule diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index 0ed3bbf860..682a4f689f 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -643,7 +643,7 @@ def test_write_positions(mock_adapter): ### Integration test with coordinator ```python skip -from dimos.control.blueprints.basic import coordinator_mock +from dimos.robot.manipulators.common.mock import coordinator_mock from dimos.core.coordination.module_coordinator import ModuleCoordinator # Build and start coordinator with mock hardware diff --git a/docs/capabilities/manipulation/openarm_integration.md b/docs/capabilities/manipulation/openarm_integration.md index b70d9e9f55..45670e5445 100644 --- a/docs/capabilities/manipulation/openarm_integration.md +++ b/docs/capabilities/manipulation/openarm_integration.md @@ -218,7 +218,7 @@ If you don't know which Cartesian targets are reachable, check first with the wo ### Which CAN bus is which arm -Linux assigns `can0`/`can1` in USB-enumeration order, which isn't guaranteed stable across reboots or cable swaps. If the arms come up "swapped" (commanding `left_arm` moves the physical right arm), flip these two constants at the top of [blueprints.py](/dimos/robot/manipulators/openarm/blueprints.py): +Linux assigns `can0`/`can1` in USB-enumeration order, which isn't guaranteed stable across reboots or cable swaps. If the arms come up "swapped" (commanding `left_arm` moves the physical right arm), flip these two constants in [config.py](/dimos/robot/manipulators/openarm/config.py): ```python LEFT_CAN = "can0" @@ -247,7 +247,7 @@ The URDFs use the xacro-generated limits (which include per-side offsets for mir ### Disabling auto MIT-mode write -The adapter writes `CTRL_MODE=MIT` to every motor at `connect()`. It's idempotent (writing the same value is a no-op), so this is safe to leave on. To verify that a previous write persisted across a power cycle, flip `AUTO_SET_MIT_MODE = False` in [blueprints.py](/dimos/robot/manipulators/openarm/blueprints.py) and restart — the arms should still respond. +The adapter writes `CTRL_MODE=MIT` to every motor at `connect()`. It's idempotent (writing the same value is a no-op), so this is safe to leave on. To verify that a previous write persisted across a power cycle, flip `AUTO_SET_MIT_MODE = False` in [config.py](/dimos/robot/manipulators/openarm/config.py) and restart — the arms should still respond. --- @@ -335,10 +335,10 @@ Persistent across power cycles. - **`ip link ... fd on` → `Operation not supported`.** gs_usb firmware doesn't support CAN-FD. Use classical CAN @ 1 Mbit (our bringup script's default). - **Motors reply to probes but commands do nothing.** CTRL_MODE is not MIT. The adapter now writes MIT on connect, but if you disabled that and motors got reset, run `openarm_set_mit_mode.py`. -- **`COLLISION_AT_START` during planning.** `link5` and `link7` collision meshes overlap by 3 mm at every configuration. Handled by `OPENARM_COLLISION_EXCLUSIONS` in the OpenArm blueprint module. If you see it anyway, the exclusion pairs may not be getting applied — check that the collision filter log line appears during world build. +- **`COLLISION_AT_START` during planning.** `link5` and `link7` collision meshes overlap by 3 mm at every configuration. Handled by `OPENARM_COLLISION_EXCLUSIONS` in the OpenArm config module. If you see it anyway, the exclusion pairs may not be getting applied — check that the collision filter log line appears during world build. - **`INVALID_START` during planning.** Hardware encoder noise pushed a joint 1 mrad past a URDF limit. Joint4 used to be exactly `lower=0.0` which tripped this — it's now `-0.01` to give breathing room. If you see it on a different joint, widen that limit by ~10 mrad. - **"Transmit buffer full" (ENOBUFS) at 100 Hz.** Kernel TX queue too small. The bringup script sets `txqueuelen 1000`; the driver also retries on ENOBUFS. If you still see the error, check `ip -details link show canX | grep qlen`. -- **Arms swap sides.** USB enumeration order flipped. Swap `LEFT_CAN` / `RIGHT_CAN` in [blueprints.py](/dimos/robot/manipulators/openarm/blueprints.py). +- **Arms swap sides.** USB enumeration order flipped. Swap `LEFT_CAN` / `RIGHT_CAN` in [config.py](/dimos/robot/manipulators/openarm/config.py). --- diff --git a/docs/capabilities/manipulation/planning_groups.md b/docs/capabilities/manipulation/planning_groups.md index 24343b5bc1..7b86182778 100644 --- a/docs/capabilities/manipulation/planning_groups.md +++ b/docs/capabilities/manipulation/planning_groups.md @@ -11,12 +11,24 @@ being planned. | Planning group | A named serial chain of controllable robot joints. | | Planning group ID | Stable API ID in the form `{robot_name}/{group_name}`. | | Global joint name | Boundary-level joint name in the form `{robot_name}/{local_joint_name}`. | +| Local joint name | The joint name as it appears in the robot model. | | Generated plan | Minimal planning artifact containing selected group IDs and one synchronized global-joint path. | | Auxiliary group | A group selected for a pose request without receiving its own pose target. | Local URDF/SRDF joint names stay inside robot-scoped APIs, model parsing, and -backend internals. Flat planning states and generated plan paths require global -joint names so two robots can safely have the same local joint names. +backend internals. Group-scoped APIs, generated plans, preview, execution, and +coordinator boundaries use global joint names so two robots can safely have the +same local joint names. + +`PlanningGroup` descriptors returned by `list_planning_groups()` include both +namespaces: + +- `id`: public `{robot_name}/{group_name}` selector; +- `joint_names`: selected global joint names in group order; +- `local_joint_names`: selected local model joint names in group order; +- `base_link` / `tip_link`: model links for group kinematics. `tip_link=None` + means the group can participate as an auxiliary group but cannot receive a + pose target. ## Planning group sources @@ -64,20 +76,33 @@ name is always `manipulator`. ## Planning APIs Planning APIs select groups explicitly. Descriptors returned by -`WorldSpec.list_planning_groups()` can be passed where a group ID is accepted; -the API normalizes them back to IDs and re-resolves current world state. +`ManipulationModule.list_planning_groups()` can be passed anywhere a group ID is +accepted; the module normalizes descriptors back to IDs and re-resolves current +world state. ```python skip -# Joint-space planning for one group. +# Discover groups. Each item is a PlanningGroup dataclass. +groups = manip.list_planning_groups() +arm = groups[0] + +# Joint-space planning for one group. Named targets may use global names... manip.plan_to_joint_targets({ - "left_arm/manipulator": JointState( + arm.id: JointState( + name=["left_arm/joint1", "left_arm/joint2"], + position=[0.2, -0.1], + ) +}) + +# ...or local model names, but not a mix of both namespaces. +manip.plan_to_joint_targets({ + arm: JointState( name=["joint1", "joint2"], position=[0.2, -0.1], ) }) # Pose planning for an arm while a torso/waist group participates as free DOFs. -manip.plan_to_poses( +manip.plan_to_pose_targets( {"robot/arm": target_pose}, auxiliary_groups=["robot/torso"], ) @@ -87,24 +112,63 @@ manip.preview_plan(plan) manip.execute_plan(plan) ``` -For robot-scoped joint-space planning, unnamed vectors are interpreted in robot -model joint order. If names are provided, they must be local model joint names: -no global names, missing joints, extra joints, or partial joint sets. +### Pose targets + +`plan_to_pose_targets()` accepts `Mapping[PlanningGroupID | PlanningGroup, +Pose]`. It wraps each `Pose` in the world frame before calling the group-scoped +IK path. Every key in `pose_targets` must refer to a pose-targetable group +(`tip_link` is not `None`). `auxiliary_groups` may include non-pose-targeted +groups whose joints should stay in the solve as free DOFs. + +`inverse_kinematics()` is the lower-level RPC. It accepts stamped poses keyed by +group ID plus optional auxiliary group IDs and returns an `IKResult` without +running collision filtering or planning. + +The compatibility wrapper `plan_to_pose(pose, robot_name=None)` still exists. It +selects the default pose-targetable group for the robot, then delegates to +`plan_to_pose_targets()`. + +### Joint targets + +`plan_to_joint_targets()` accepts `Mapping[PlanningGroupID | PlanningGroup, +JointState]`. + +For a group-scoped joint target: + +- an unnamed vector is interpreted in that group's joint order; +- named targets may use all-global names or all-local names; +- global and local names must not be mixed in one target; +- named targets must provide exactly the group's selected joints, with no + missing or extra joints. + +The compatibility wrapper `plan_to_joints(joints, robot_name=None)` still exists. +It selects the robot's default group, then delegates to +`plan_to_joint_targets()`. + +Robot-scoped state helpers such as `set_init_joints()` still use local model +joint names. When unnamed, those vectors are interpreted in full robot model +joint order. ## Generated plans and execution A `GeneratedPlan` stores: - selected planning group IDs; -- a single synchronized path of `JointState` waypoints keyed by global joint - names; +- a single synchronized path of `JointState` waypoints keyed by selected global + joint names; - status, timing, path length, iteration count, and message metadata. -Preview and execution project this path lazily. Preview sends projected joint -paths to the world monitor. Execution splits the path by affected trajectory -task, orders each trajectory by the robot's configured local joint order, writes -global joint names at the coordinator boundary, and invokes each trajectory -controller. Controllers remain planning-group agnostic. +Preview and execution project this path lazily. `preview_plan(plan=None, +duration=None, robot_name=None)` defaults to `_last_plan`; `robot_name` is only a +filter that rejects plans which do not affect the requested robot. Preview sends +the generated global-joint path to the world monitor for animation. + +`execute_plan(plan=None)` also defaults to `_last_plan`. It infers affected +robots from the selected groups, projects each waypoint back into each robot's +full local model joint order, fills unselected robot joints from current state, +then writes a coordinator `JointTrajectory` using global joint names. The +trajectory is dispatched to each robot's configured `coordinator_task_name`. +Controllers remain planning-group agnostic. Multi-task dispatch is not atomic in this change: if one trajectory task accepts and a later task rejects, DimOS reports the rejection but does not roll back the @@ -120,4 +184,7 @@ New planning logic should use model/SRDF structure and planning group base/tip links instead. Robot placement should be encoded in URDF/xacro/MJCF. `joint_names` remains -supported and should describe the ordered controllable local model joint set. +supported and should describe the ordered controllable local model joint set, not +a planning group. `joint_name_mapping` can map external/coordinator joint names +back to local model joint names for adapters that publish scoped hardware names. +`coordinator_task_name` identifies the trajectory task used by `execute_plan()`. diff --git a/docs/capabilities/manipulation/readme.md b/docs/capabilities/manipulation/readme.md index 7aeb9d41de..ca130272c8 100644 --- a/docs/capabilities/manipulation/readme.md +++ b/docs/capabilities/manipulation/readme.md @@ -207,10 +207,13 @@ projection. | File | Description | |------|-------------| | [`manipulation_module.py`](/dimos/manipulation/manipulation_module.py) | Main module (RPC interface, state machine) | -| [`manipulation/blueprints.py`](/dimos/manipulation/blueprints.py) | Planner and perception blueprints | -| [`robot/manipulators/a750/blueprints.py`](/dimos/robot/manipulators/a750/blueprints.py) | A-750 keyboard teleop blueprint | -| [`robot/manipulators/piper/blueprints.py`](/dimos/robot/manipulators/piper/blueprints.py) | Piper keyboard teleop blueprint | -| [`robot/manipulators/xarm/blueprints.py`](/dimos/robot/manipulators/xarm/blueprints.py) | XArm keyboard teleop blueprints | +| [`robot/manipulators/common/blueprints.py`](/dimos/robot/manipulators/common/blueprints.py) | Shared coordinator, planner, and task helpers | +| [`robot/manipulators/a750/config.py`](/dimos/robot/manipulators/a750/config.py) | A-750 model and hardware config | +| [`robot/manipulators/a750/blueprints/teleop.py`](/dimos/robot/manipulators/a750/blueprints/teleop.py) | A-750 keyboard teleop blueprint | +| [`robot/manipulators/piper/blueprints/basic.py`](/dimos/robot/manipulators/piper/blueprints/basic.py) | Piper coordinator blueprint | +| [`robot/manipulators/piper/blueprints/teleop.py`](/dimos/robot/manipulators/piper/blueprints/teleop.py) | Piper teleop blueprints | +| [`robot/manipulators/xarm/blueprints/basic.py`](/dimos/robot/manipulators/xarm/blueprints/basic.py) | XArm coordinator and planner blueprints | +| [`robot/manipulators/xarm/blueprints/perception.py`](/dimos/robot/manipulators/xarm/blueprints/perception.py) | XArm perception blueprint | | [`teleop/keyboard/keyboard_teleop_module.py`](/dimos/teleop/keyboard/keyboard_teleop_module.py) | Keyboard teleop module | | [`planning/world/drake_world.py`](/dimos/manipulation/planning/world/drake_world.py) | Drake physics backend | | [`planning/planners/rrt_planner.py`](/dimos/manipulation/planning/planners/rrt_planner.py) | RRT-Connect motion planner |