diff --git a/dimos/control/blueprints/basic.py b/dimos/control/blueprints/basic.py index f3ffdc3dcc..7f980d93c2 100644 --- a/dimos/control/blueprints/basic.py +++ b/dimos/control/blueprints/basic.py @@ -80,7 +80,9 @@ ) # Piper arm (6-DOF, CAN bus) -_piper_cfg = _catalog_piper(name="arm", adapter_type="piper", address=global_config.can_port) +_piper_cfg = _catalog_piper( + name="arm", adapter_type="piper", address=global_config.can_port or "can0" +) coordinator_piper = ControlCoordinator.blueprint( hardware=[_piper_cfg.to_hardware_component()], diff --git a/dimos/control/blueprints/dual.py b/dimos/control/blueprints/dual.py index 72d20998f3..d5ec44a0be 100644 --- a/dimos/control/blueprints/dual.py +++ b/dimos/control/blueprints/dual.py @@ -67,7 +67,9 @@ _xarm6_dual = _catalog_xarm6( name="xarm_arm", adapter_type="xarm", address=global_config.xarm6_ip, add_gripper=False ) -_piper_dual = _catalog_piper(name="piper_arm", adapter_type="piper", address=global_config.can_port) +_piper_dual = _catalog_piper( + name="piper_arm", adapter_type="piper", address=global_config.can_port or "can0" +) coordinator_piper_xarm = ControlCoordinator.blueprint( hardware=[_xarm6_dual.to_hardware_component(), _piper_dual.to_hardware_component()], diff --git a/dimos/control/blueprints/teleop.py b/dimos/control/blueprints/teleop.py index aa62e173a7..d4052cc4c0 100644 --- a/dimos/control/blueprints/teleop.py +++ b/dimos/control/blueprints/teleop.py @@ -50,7 +50,9 @@ _xarm7_cfg = _catalog_xarm7( name="arm", adapter_type="xarm", address=global_config.xarm7_ip, add_gripper=True ) -_piper_cfg = _catalog_piper(name="arm", adapter_type="piper", address=global_config.can_port) +_piper_cfg = _catalog_piper( + name="arm", adapter_type="piper", address=global_config.can_port or "can0" +) # -- Servo / velocity --------------------------------------------------------- diff --git a/dimos/control/components.py b/dimos/control/components.py index 42b5d9c9ea..be12aa4fb1 100644 --- a/dimos/control/components.py +++ b/dimos/control/components.py @@ -23,6 +23,17 @@ TaskName = str +def split_joint_name(joint_name: str) -> tuple[str, str]: + """Split a coordinator joint name into (hardware_id, suffix). + + Example: "left_arm/joint1" -> ("left_arm", "joint1") + """ + parts = joint_name.split("/", 1) + if len(parts) != 2: + raise ValueError(f"Joint name '{joint_name}' missing separator '/'") + return parts[0], parts[1] + + class HardwareType(Enum): MANIPULATOR = "manipulator" BASE = "base" @@ -44,7 +55,7 @@ class HardwareComponent: Attributes: hardware_id: Unique identifier, also used as joint name prefix hardware_type: Type of hardware (MANIPULATOR, BASE) - joints: List of joint names (e.g., ["arm_joint1", "arm_joint2", ...]) + joints: List of joint names (e.g., ["arm/joint1", "arm/joint2", ...]) adapter_type: Adapter type ("mock", "xarm", "piper") address: Connection address - IP for TCP, port for CAN auto_enable: Whether to auto-enable servos @@ -73,9 +84,9 @@ def make_gripper_joints(hardware_id: HardwareId) -> list[JointName]: hardware_id: The hardware identifier (e.g., "arm") Returns: - List of joint names like ["arm_gripper"] + List of joint names like ["arm/gripper"] """ - return [f"{hardware_id}_gripper"] + return [f"{hardware_id}/gripper"] def make_joints(hardware_id: HardwareId, dof: int) -> list[JointName]: @@ -86,9 +97,9 @@ def make_joints(hardware_id: HardwareId, dof: int) -> list[JointName]: dof: Degrees of freedom Returns: - List of joint names like ["left_arm_joint1", "left_arm_joint2", ...] + List of joint names like ["left_arm/joint1", "left_arm/joint2", ...] """ - return [f"{hardware_id}_joint{i + 1}" for i in range(dof)] + return [f"{hardware_id}/joint{i + 1}" for i in range(dof)] # Maps virtual joint suffix → (Twist group, Twist field) @@ -115,13 +126,13 @@ def make_twist_base_joints( suffixes: Velocity DOF suffixes. Defaults to ["vx", "vy", "wz"] (holonomic). Returns: - List of joint names like ["base_vx", "base_vy", "base_wz"] + List of joint names like ["base/vx", "base/vy", "base/wz"] """ suffixes = suffixes or _DEFAULT_TWIST_SUFFIXES for s in suffixes: if s not in TWIST_SUFFIX_MAP: raise ValueError(f"Unknown twist suffix '{s}'. Valid: {list(TWIST_SUFFIX_MAP)}") - return [f"{hardware_id}_{s}" for s in suffixes] + return [f"{hardware_id}/{s}" for s in suffixes] __all__ = [ @@ -135,4 +146,5 @@ def make_twist_base_joints( "make_gripper_joints", "make_joints", "make_twist_base_joints", + "split_joint_name", ] diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index ef5655036f..c36dc480fa 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -38,6 +38,7 @@ HardwareType, JointName, TaskName, + split_joint_name, ) from dimos.control.hardware_interface import ConnectedHardware, ConnectedTwistBase from dimos.control.task import ControlTask @@ -536,8 +537,8 @@ def _on_twist_command(self, msg: Twist) -> None: if hw.component.hardware_type != HardwareType.BASE: continue for joint_name in hw.joint_names: - # Extract suffix (e.g., "base_vx" → "vx") - suffix = joint_name.rsplit("_", 1)[-1] + # Extract suffix (e.g., "base/vx" → "vx") + _, suffix = split_joint_name(joint_name) mapping = TWIST_SUFFIX_MAP.get(suffix) if mapping is None: continue diff --git a/dimos/control/task.py b/dimos/control/task.py index afad70bb05..56c2c4d426 100644 --- a/dimos/control/task.py +++ b/dimos/control/task.py @@ -48,7 +48,7 @@ class ResourceClaim: Attributes: joints: Set of joint names this task wants to control. - Example: frozenset({"left_joint1", "left_joint2"}) + Example: frozenset({"left/joint1", "left/joint2"}) priority: Priority level for conflict resolution. Higher wins. Typical values: 10 (trajectory), 50 (WBC), 100 (safety) mode: Control mode (POSITION, VELOCITY, TORQUE) @@ -189,7 +189,7 @@ class ControlTask(Protocol): ... ... def claim(self) -> ResourceClaim: ... return ResourceClaim( - ... joints=frozenset(["left_joint1", "left_joint2"]), + ... joints=frozenset(["left/joint1", "left/joint2"]), ... priority=10, ... ) ... @@ -201,7 +201,7 @@ class ControlTask(Protocol): ... t_elapsed = state.t_now - self._start_time ... positions = self._trajectory.sample(t_elapsed) ... return JointCommandOutput( - ... joint_names=["left_joint1", "left_joint2"], + ... joint_names=["left/joint1", "left/joint2"], ... positions=positions, ... ) ... diff --git a/dimos/control/tasks/servo_task.py b/dimos/control/tasks/servo_task.py index 50805bfa2c..ac83a16244 100644 --- a/dimos/control/tasks/servo_task.py +++ b/dimos/control/tasks/servo_task.py @@ -65,7 +65,7 @@ class JointServoTask(BaseControlTask): >>> task = JointServoTask( ... name="servo_arm", ... config=JointServoTaskConfig( - ... joint_names=["arm_joint1", "arm_joint2", "arm_joint3"], + ... joint_names=["arm/joint1", "arm/joint2", "arm/joint3"], ... priority=10, ... timeout=0.5, ... ), diff --git a/dimos/control/tasks/teleop_task.py b/dimos/control/tasks/teleop_task.py index 3f20502759..a37c83c941 100644 --- a/dimos/control/tasks/teleop_task.py +++ b/dimos/control/tasks/teleop_task.py @@ -70,7 +70,7 @@ class TeleopIKTaskConfig: timeout: If no command received for this many seconds, go inactive (0 = never) max_joint_delta_deg: Maximum allowed joint change per tick (safety limit) hand: "left" or "right" — which controller's primary button to listen to - gripper_joint: Optional joint name for the gripper (e.g. "arm_gripper"). + gripper_joint: Optional joint name for the gripper (e.g. "arm/gripper"). gripper_open_pos: Gripper position (adapter units) at trigger value 0.0 (no press). gripper_closed_pos: Gripper position (adapter units) at trigger value 1.0 (full press). """ diff --git a/dimos/control/tasks/trajectory_task.py b/dimos/control/tasks/trajectory_task.py index fd0a9fda6e..90a7d57479 100644 --- a/dimos/control/tasks/trajectory_task.py +++ b/dimos/control/tasks/trajectory_task.py @@ -71,7 +71,7 @@ class JointTrajectoryTask(BaseControlTask): >>> task = JointTrajectoryTask( ... name="traj_left", ... config=JointTrajectoryTaskConfig( - ... joint_names=["left_joint1", "left_joint2"], + ... joint_names=["left/joint1", "left/joint2"], ... priority=10, ... ), ... ) diff --git a/dimos/control/tasks/velocity_task.py b/dimos/control/tasks/velocity_task.py index 5da475114d..21f67a7293 100644 --- a/dimos/control/tasks/velocity_task.py +++ b/dimos/control/tasks/velocity_task.py @@ -71,7 +71,7 @@ class JointVelocityTask(BaseControlTask): >>> task = JointVelocityTask( ... name="velocity_arm", ... config=JointVelocityTaskConfig( - ... joint_names=["arm_joint1", "arm_joint2", "arm_joint3"], + ... joint_names=["arm/joint1", "arm/joint2", "arm/joint3"], ... priority=10, ... timeout=0.2, ... zero_on_timeout=True, diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index 3de7865ae3..270748a6a1 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -71,7 +71,7 @@ def connected_hardware(mock_adapter): def trajectory_task(): """Create a JointTrajectoryTask for testing.""" config = JointTrajectoryTaskConfig( - joint_names=["arm_joint1", "arm_joint2", "arm_joint3"], + joint_names=["arm/joint1", "arm/joint2", "arm/joint3"], priority=10, ) return JointTrajectoryTask(name="test_traj", config=config) @@ -81,7 +81,7 @@ def trajectory_task(): def simple_trajectory(): """Create a simple 2-point trajectory.""" return JointTrajectory( - joint_names=["arm_joint1", "arm_joint2", "arm_joint3"], + joint_names=["arm/joint1", "arm/joint2", "arm/joint3"], points=[ TrajectoryPoint( positions=[0.0, 0.0, 0.0], @@ -101,9 +101,9 @@ def simple_trajectory(): def coordinator_state(): """Create a sample CoordinatorState.""" joints = JointStateSnapshot( - joint_positions={"arm_joint1": 0.0, "arm_joint2": 0.0, "arm_joint3": 0.0}, - joint_velocities={"arm_joint1": 0.0, "arm_joint2": 0.0, "arm_joint3": 0.0}, - joint_efforts={"arm_joint1": 0.0, "arm_joint2": 0.0, "arm_joint3": 0.0}, + joint_positions={"arm/joint1": 0.0, "arm/joint2": 0.0, "arm/joint3": 0.0}, + joint_velocities={"arm/joint1": 0.0, "arm/joint2": 0.0, "arm/joint3": 0.0}, + joint_efforts={"arm/joint1": 0.0, "arm/joint2": 0.0, "arm/joint3": 0.0}, timestamp=time.perf_counter(), ) return CoordinatorState(joints=joints, t_now=time.perf_counter(), dt=0.01) @@ -162,27 +162,27 @@ class TestConnectedHardware: def test_joint_names_prefixed(self, connected_hardware): names = connected_hardware.joint_names assert names == [ - "arm_joint1", - "arm_joint2", - "arm_joint3", - "arm_joint4", - "arm_joint5", - "arm_joint6", + "arm/joint1", + "arm/joint2", + "arm/joint3", + "arm/joint4", + "arm/joint5", + "arm/joint6", ] def test_read_state(self, connected_hardware): state = connected_hardware.read_state() - assert "arm_joint1" in state + assert "arm/joint1" in state assert len(state) == 6 - joint_state = state["arm_joint1"] + joint_state = state["arm/joint1"] assert joint_state.position == 0.0 assert joint_state.velocity == 0.0 assert joint_state.effort == 0.0 def test_write_command(self, connected_hardware, mock_adapter): commands = { - "arm_joint1": 0.5, - "arm_joint2": 1.0, + "arm/joint1": 0.5, + "arm/joint2": 1.0, } connected_hardware.write_command(commands, ControlMode.POSITION) mock_adapter.write_joint_positions.assert_called() @@ -197,9 +197,9 @@ def test_initial_state(self, trajectory_task): def test_claim(self, trajectory_task): claim = trajectory_task.claim() assert claim.priority == 10 - assert "arm_joint1" in claim.joints - assert "arm_joint2" in claim.joints - assert "arm_joint3" in claim.joints + assert "arm/joint1" in claim.joints + assert "arm/joint2" in claim.joints + assert "arm/joint3" in claim.joints def test_execute_trajectory(self, trajectory_task, simple_trajectory): time.perf_counter() @@ -270,7 +270,7 @@ def test_cancel_trajectory(self, trajectory_task, simple_trajectory): def test_preemption(self, trajectory_task, simple_trajectory): trajectory_task.execute(simple_trajectory) - trajectory_task.on_preempted("safety_task", frozenset({"arm_joint1"})) + trajectory_task.on_preempted("safety_task", frozenset({"arm/joint1"})) assert trajectory_task.get_state() == TrajectoryState.ABORTED assert not trajectory_task.is_active() @@ -404,7 +404,7 @@ def test_tick_loop_starts_and_stops(self, mock_adapter): hw = ConnectedHardware(mock_adapter, component) hardware = {"arm": hw} tasks: dict = {} - joint_to_hardware = {f"arm_joint{i + 1}": "arm" for i in range(6)} + joint_to_hardware = {f"arm/joint{i + 1}": "arm" for i in range(6)} tick_loop = TickLoop( tick_rate=100.0, @@ -437,17 +437,17 @@ def test_tick_loop_calls_compute(self, mock_adapter): mock_task.name = "test_task" mock_task.is_active.return_value = True mock_task.claim.return_value = ResourceClaim( - joints=frozenset({"arm_joint1"}), + joints=frozenset({"arm/joint1"}), priority=10, ) mock_task.compute.return_value = JointCommandOutput( - joint_names=["arm_joint1"], + joint_names=["arm/joint1"], positions=[0.5], mode=ControlMode.POSITION, ) tasks = {"test_task": mock_task} - joint_to_hardware = {f"arm_joint{i + 1}": "arm" for i in range(6)} + joint_to_hardware = {f"arm/joint{i + 1}": "arm" for i in range(6)} tick_loop = TickLoop( tick_rate=100.0, @@ -476,13 +476,13 @@ def test_full_trajectory_execution(self, mock_adapter): hardware = {"arm": hw} config = JointTrajectoryTaskConfig( - joint_names=[f"arm_joint{i + 1}" for i in range(6)], + joint_names=[f"arm/joint{i + 1}" for i in range(6)], priority=10, ) traj_task = JointTrajectoryTask(name="traj_arm", config=config) tasks = {"traj_arm": traj_task} - joint_to_hardware = {f"arm_joint{i + 1}": "arm" for i in range(6)} + joint_to_hardware = {f"arm/joint{i + 1}": "arm" for i in range(6)} tick_loop = TickLoop( tick_rate=100.0, @@ -494,7 +494,7 @@ def test_full_trajectory_execution(self, mock_adapter): ) trajectory = JointTrajectory( - joint_names=[f"arm_joint{i + 1}" for i in range(6)], + joint_names=[f"arm/joint{i + 1}" for i in range(6)], points=[ TrajectoryPoint( positions=[0.0] * 6, diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index ce08c74705..a3f42b4bd7 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -31,7 +31,7 @@ class GlobalConfig(BaseSettings): robot_ips: str | None = None xarm7_ip: str | None = None xarm6_ip: str | None = None - can_port: str = "can0" + can_port: str | None = None simulation: bool = False replay: bool = False replay_dir: str = "go2_sf_office" diff --git a/dimos/e2e_tests/test_control_coordinator.py b/dimos/e2e_tests/test_control_coordinator.py index 80b63c529f..853b86e57d 100644 --- a/dimos/e2e_tests/test_control_coordinator.py +++ b/dimos/e2e_tests/test_control_coordinator.py @@ -56,7 +56,7 @@ def test_coordinator_starts_and_responds_to_rpc(self, lcm_spy, start_blueprint) joints = client.list_joints() assert joints is not None assert len(joints) == 7 # Mock arm has 7 DOF - assert "arm_joint1" in joints + assert "arm/joint1" in joints # Test list_tasks RPC tasks = client.list_tasks() @@ -90,7 +90,7 @@ def test_coordinator_executes_trajectory(self, lcm_spy, start_blueprint) -> None # Create a simple trajectory trajectory = JointTrajectory( - joint_names=[f"arm_joint{i + 1}" for i in range(7)], + joint_names=[f"arm/joint{i + 1}" for i in range(7)], points=[ TrajectoryPoint( time_from_start=0.0, @@ -153,7 +153,7 @@ def test_coordinator_joint_state_published(self, lcm_spy, start_blueprint) -> No joint_state = JointState.lcm_decode(raw_msg) assert len(joint_state.name) == 7 assert len(joint_state.position) == 7 - assert "arm_joint1" in joint_state.name + assert "arm/joint1" in joint_state.name def test_coordinator_cancel_trajectory(self, lcm_spy, start_blueprint) -> None: """Test that a running trajectory can be cancelled.""" @@ -167,7 +167,7 @@ def test_coordinator_cancel_trajectory(self, lcm_spy, start_blueprint) -> None: try: # Create a long trajectory (5 seconds) trajectory = JointTrajectory( - joint_names=[f"arm_joint{i + 1}" for i in range(7)], + joint_names=[f"arm/joint{i + 1}" for i in range(7)], points=[ TrajectoryPoint( time_from_start=0.0, @@ -210,8 +210,8 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: try: # Verify both arms present joints = client.list_joints() - assert "left_arm_joint1" in joints - assert "right_arm_joint1" in joints + assert "left_arm/joint1" in joints + assert "right_arm/joint1" in joints tasks = client.list_tasks() assert "traj_left" in tasks @@ -219,7 +219,7 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: # Create trajectories for both arms left_trajectory = JointTrajectory( - joint_names=[f"left_arm_joint{i + 1}" for i in range(7)], + joint_names=[f"left_arm/joint{i + 1}" for i in range(7)], points=[ TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 7), TrajectoryPoint(time_from_start=0.5, positions=[0.2] * 7), @@ -227,7 +227,7 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: ) right_trajectory = JointTrajectory( - joint_names=[f"right_arm_joint{i + 1}" for i in range(6)], + joint_names=[f"right_arm/joint{i + 1}" for i in range(6)], points=[ TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 6), TrajectoryPoint(time_from_start=0.5, positions=[0.3] * 6), diff --git a/dimos/manipulation/blueprints.py b/dimos/manipulation/blueprints.py index d906b27e3b..e97b979cbd 100644 --- a/dimos/manipulation/blueprints.py +++ b/dimos/manipulation/blueprints.py @@ -33,6 +33,7 @@ from dimos.agents.mcp.mcp_server import McpServer from dimos.control.coordinator import ControlCoordinator 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 @@ -46,7 +47,11 @@ from dimos.robot.foxglove_bridge import FoxgloveBridge # TODO: migrate to rerun # Single XArm6 planner (standalone, no coordinator) -_xarm6_planner_cfg = _catalog_xarm6(name="arm") +_xarm6_planner_cfg = _catalog_xarm6( + name="arm", + adapter_type="xarm" if global_config.xarm6_ip else "mock", + address=global_config.xarm6_ip or None, +) xarm6_planner_only = ManipulationModule.blueprint( robots=[_xarm6_planner_cfg.to_robot_model_config()], @@ -61,8 +66,18 @@ # Dual XArm6 planner with coordinator integration # Usage: Start with coordinator_dual_mock, then plan/execute via RPC -_left_arm_cfg = _catalog_xarm6(name="left_arm", y_offset=0.5) -_right_arm_cfg = _catalog_xarm6(name="right_arm", y_offset=-0.5) +_left_arm_cfg = _catalog_xarm6( + name="left_arm", + adapter_type="xarm" if global_config.xarm6_ip else "mock", + address=global_config.xarm6_ip or None, + y_offset=0.5, +) +_right_arm_cfg = _catalog_xarm6( + name="right_arm", + adapter_type="xarm" if global_config.xarm6_ip else "mock", + address=global_config.xarm6_ip or None, + y_offset=-0.5, +) dual_xarm6_planner = ManipulationModule.blueprint( robots=[ @@ -78,9 +93,13 @@ ) -# Single XArm7 planner + mock coordinator (standalone, no external coordinator needed) -# Usage: dimos run xarm7-planner-coordinator -_xarm7_cfg = _catalog_xarm7(name="arm") +# Single XArm7 planner + coordinator (uses real hardware when XARM7_IP is set) +# Usage: XARM7_IP= dimos run xarm7-planner-coordinator +_xarm7_cfg = _catalog_xarm7( + name="arm", + adapter_type="xarm" if global_config.xarm7_ip else "mock", + address=global_config.xarm7_ip or None, +) xarm7_planner_coordinator = autoconnect( ManipulationModule.blueprint( @@ -150,6 +169,8 @@ _xarm7_perception_cfg = _catalog_xarm7( name="arm", + adapter_type="xarm" if global_config.xarm7_ip else "mock", + address=global_config.xarm7_ip or None, pitch=math.radians(45), add_gripper=True, tf_extra_links=["link7"], diff --git a/dimos/manipulation/control/coordinator_client.py b/dimos/manipulation/control/coordinator_client.py index dfa99371a6..ed552e0846 100644 --- a/dimos/manipulation/control/coordinator_client.py +++ b/dimos/manipulation/control/coordinator_client.py @@ -47,6 +47,7 @@ import time from typing import TYPE_CHECKING, Any +from dimos.control.components import split_joint_name from dimos.control.coordinator import ControlCoordinator from dimos.core.rpc_client import RPCClient from dimos.manipulation.planning.trajectory_generator.joint_trajectory_generator import ( @@ -157,11 +158,11 @@ def select_task(self, task_name: str) -> bool: # Try to infer hardware_id from task name if "_" in task_name: suffix = task_name.split("_", 1)[1] # "traj_left" -> "left" - # Try both patterns: exact suffix (e.g., "arm_") and with "_arm" suffix (e.g., "left_arm_") - task_joints = [j for j in all_joints if j.startswith(suffix + "_")] + # Try both patterns: exact suffix (e.g., "arm/") and with "_arm" suffix (e.g., "left_arm/") + task_joints = [j for j in all_joints if j.startswith(suffix + "/")] if not task_joints: # Try with "_arm" suffix for dual-arm setups (left -> left_arm) - task_joints = [j for j in all_joints if j.startswith(suffix + "_arm_")] + task_joints = [j for j in all_joints if j.startswith(suffix + "_arm/")] else: task_joints = all_joints @@ -272,7 +273,7 @@ def preview_waypoints(waypoints: list[list[float]], joint_names: list[str]) -> N print("-" * 70) # Header with joint names (truncated) - headers = [j.split("_")[-1][:6] for j in joint_names] # e.g., "joint1" -> "joint1" + headers = [split_joint_name(j)[1][:6] for j in joint_names] # e.g., "joint1" -> "joint1" header_str = " ".join(f"{h:>7}" for h in headers) print(f" # | {header_str} (degrees)") print("-" * 70) @@ -285,7 +286,7 @@ def preview_waypoints(waypoints: list[list[float]], joint_names: list[str]) -> N def preview_trajectory(trajectory: JointTrajectory, joint_names: list[str]) -> None: """Show generated trajectory preview.""" - headers = [j.split("_")[-1][:6] for j in joint_names] + headers = [split_joint_name(j)[1][:6] for j in joint_names] header_str = " ".join(f"{h:>7}" for h in headers) print("\n" + "=" * 70) diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 35d1f0c43c..b53e6c2e98 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -132,8 +132,8 @@ def __init__(self, **kwargs: Any) -> None: # Coordinator integration (lazy initialized) self._coordinator_client: RPCClient | None = None - # Init joints: captured from first joint state received, used by go_init - self._init_joints: JointState | None = None + # Init joints: captured from first joint state per robot, used by go_init + self._init_joints: dict[RobotName, JointState] = {} # TF publishing thread self._tf_stop_event = threading.Event() @@ -244,21 +244,52 @@ def _get_robot( return (robot_name, robot_id, config, traj_gen) def _on_joint_state(self, msg: JointState) -> None: - """Callback when joint state received from driver.""" + """Callback when joint state received from driver. + + Splits the aggregated JointState by robot using each robot's + coordinator joint names, then routes to the correct monitor. + """ try: - # Forward to world monitor for state synchronization. - # Pass robot_id=None to broadcast to all monitors - each monitor - # extracts only its robot's joints based on joint_name_mapping. - if self._world_monitor is not None: - self._world_monitor.on_joint_state(msg, robot_id=None) - - # Capture initial joint positions on first callback - if self._init_joints is None and msg.position: - self._init_joints = JointState(name=list(msg.name), position=list(msg.position)) - logger.info( - f"Init joints captured: [{', '.join(f'{j:.3f}' for j in msg.position)}]" + if self._world_monitor is None: + return + + # Build name → index map once for the whole message + name_to_idx = {name: i for i, name in enumerate(msg.name)} + + for robot_name, (robot_id, config, _) in self._robots.items(): + coord_names = config.get_coordinator_joint_names() + indices = [name_to_idx.get(cn) for cn in coord_names] + if any(idx is None for idx in indices): + missing = [ + cn for cn, idx in zip(coord_names, indices, strict=False) if idx is None + ] + logger.warning(f"Skipping '{robot_name}': missing joints {missing}") + continue + + # Build per-robot sub-message (coordinator namespace) + sub_positions = [msg.position[idx] for idx in indices] # type: ignore[index] + sub_velocities = ( + [msg.velocity[idx] for idx in indices] # type: ignore[index] + if msg.velocity and len(msg.velocity) == len(msg.name) + else [] + ) + sub_msg = JointState( + name=list(coord_names), + position=sub_positions, + velocity=sub_velocities, ) + # Route to specific monitor + self._world_monitor.on_joint_state(sub_msg, robot_id=robot_id) + + # Capture per-robot init joints on first receipt + if robot_name not in self._init_joints: + self._init_joints[robot_name] = sub_msg + logger.info( + f"Init joints captured for '{robot_name}': " + f"[{', '.join(f'{j:.3f}' for j in sub_positions)}]" + ) + except Exception as e: logger.error(f"Exception in _on_joint_state: {e}") import traceback @@ -601,23 +632,39 @@ def get_robot_info(self, robot_name: RobotName | None = None) -> dict[str, Any] "coordinator_task_name": config.coordinator_task_name, "home_joints": config.home_joints, "pre_grasp_offset": config.pre_grasp_offset, - "init_joints": list(self._init_joints.position) if self._init_joints else None, + "init_joints": list(init.position) + if (init := self._init_joints.get(robot_name)) + else None, } @rpc - def get_init_joints(self) -> JointState | None: - """Get the init joint state (captured at startup or set manually).""" - return self._init_joints + def get_init_joints(self, robot_name: RobotName | None = None) -> JointState | None: + """Get the init joint state (captured at startup or set manually). + + Args: + robot_name: Robot name (uses default if None and only one robot) + """ + robot = self._get_robot(robot_name) + if robot is None: + return None + return self._init_joints.get(robot[0]) @rpc - def set_init_joints(self, joint_state: JointState) -> bool: + def set_init_joints(self, joint_state: JointState, robot_name: RobotName | None = None) -> bool: """Set the init joint state. Args: joint_state: New init joint state (names + positions) + robot_name: Robot name (uses default if None and only one robot) """ - self._init_joints = joint_state - logger.info(f"Init joints set: [{', '.join(f'{j:.3f}' for j in joint_state.position)}]") + robot = self._get_robot(robot_name) + if robot is None: + return False + self._init_joints[robot[0]] = joint_state + logger.info( + f"Init joints set for '{robot[0]}': " + f"[{', '.join(f'{j:.3f}' for j in joint_state.position)}]" + ) return True @rpc @@ -630,16 +677,17 @@ def set_init_joints_to_current(self, robot_name: RobotName | None = None) -> boo robot = self._get_robot(robot_name) if robot is None: return False - _, robot_id, _, _ = robot + robot_name_resolved, robot_id, _, _ = robot if self._world_monitor is None: return False current = self._world_monitor.get_current_joint_state(robot_id) if current is None: logger.error("Cannot capture init joints — no current joint state") return False - self._init_joints = current + self._init_joints[robot_name_resolved] = current logger.info( - f"Init joints set to current: [{', '.join(f'{j:.3f}' for j in current.position)}]" + f"Init joints set to current for '{robot_name_resolved}': " + f"[{', '.join(f'{j:.3f}' for j in current.position)}]" ) return True @@ -1135,7 +1183,13 @@ def go_init(self, robot_name: str | None = None) -> str: Args: robot_name: Robot to move (only needed for multi-arm setups). """ - if self._init_joints is None: + robot = self._get_robot(robot_name) + if robot is None: + return "Error: Robot not found" + rname, robot_id, _, _ = robot + + init = self._init_joints.get(rname) + if init is None: return "Error: No init joints captured — robot may not have reported joint state yet" # Lift if EE is low before moving to init @@ -1145,10 +1199,8 @@ def go_init(self, robot_name: str | None = None) -> str: # Move through a safe waypoint: 10cm above and 5cm in front of init pose. # This avoids direct paths through the workspace that could collide with objects. - robot = self._get_robot(robot_name) - if robot is not None and self._world_monitor is not None: - _, robot_id, _, _ = robot - init_ee = self._world_monitor.get_ee_pose(robot_id, joint_state=self._init_joints) + if self._world_monitor is not None: + init_ee = self._world_monitor.get_ee_pose(robot_id, joint_state=init) if init_ee is not None: wp = Pose( Vector3( @@ -1166,9 +1218,9 @@ def go_init(self, robot_name: str | None = None) -> str: logger.warning("Safe waypoint unreachable, going directly to init") logger.info( - f"Planning motion to init position [{', '.join(f'{j:.3f}' for j in self._init_joints.position)}]..." + f"Planning motion to init position [{', '.join(f'{j:.3f}' for j in init.position)}]..." ) - if not self.plan_to_joints(self._init_joints, robot_name): + if not self.plan_to_joints(init, robot_name): return "Error: Failed to plan path to init position" err = self._preview_execute_wait(robot_name) diff --git a/dimos/manipulation/planning/monitor/world_state_monitor.py b/dimos/manipulation/planning/monitor/robot_state_monitor.py similarity index 96% rename from dimos/manipulation/planning/monitor/world_state_monitor.py rename to dimos/manipulation/planning/monitor/robot_state_monitor.py index 8548251c73..f395a1499a 100644 --- a/dimos/manipulation/planning/monitor/world_state_monitor.py +++ b/dimos/manipulation/planning/monitor/robot_state_monitor.py @@ -13,13 +13,13 @@ # limitations under the License. """ -World State Monitor +Robot State Monitor -Monitors joint state updates and syncs them to a WorldSpec instance. +Per-robot monitor that tracks joint state and syncs it to a WorldSpec instance. This is the WorldSpec-based replacement for StateMonitor. Example: - monitor = WorldStateMonitor(world, lock, robot_id, joint_names) + monitor = RobotStateMonitor(world, lock, robot_id, joint_names) monitor.start() monitor.on_joint_state(joint_state_msg) # Called by subscriber """ @@ -45,7 +45,7 @@ logger = setup_logger() -class WorldStateMonitor: +class RobotStateMonitor: """Monitors joint state updates and syncs them to WorldSpec. This class subscribes to joint state messages and calls @@ -60,7 +60,7 @@ class WorldStateMonitor: ## Comparison with StateMonitor - StateMonitor: Works with PlanningScene ABC - - WorldStateMonitor: Works with WorldSpec Protocol + - RobotStateMonitor: Works with WorldSpec Protocol """ def __init__( @@ -80,7 +80,7 @@ def __init__( robot_id: ID of the robot to monitor joint_names: Ordered list of joint names for this robot (URDF names) joint_name_mapping: Maps coordinator joint names to URDF joint names. - Example: {"left_joint1": "joint1"} means messages with "left_joint1" + Example: {"left/joint1": "joint1"} means messages with "left/joint1" will be mapped to URDF "joint1". If None, names must match exactly. timeout: Timeout for waiting for initial state (seconds) """ @@ -142,7 +142,7 @@ def on_joint_state(self, msg: JointState) -> None: positions = self._extract_positions(msg) if positions is None: logger.debug( - "[WorldStateMonitor] Failed to extract positions - joint names mismatch" + "[RobotStateMonitor] Failed to extract positions - joint names mismatch" ) logger.debug(f" Expected joints: {self._joint_names}") logger.debug(f" Received joints: {msg.name}") @@ -182,7 +182,7 @@ def on_joint_state(self, msg: JointState) -> None: logger.error(f"State callback error: {e}") except Exception as e: - logger.error(f"[WorldStateMonitor] Unexpected exception in on_joint_state: {e}") + logger.error(f"[RobotStateMonitor] Unexpected exception in on_joint_state: {e}") import traceback logger.error(traceback.format_exc()) diff --git a/dimos/manipulation/planning/monitor/world_monitor.py b/dimos/manipulation/planning/monitor/world_monitor.py index 33664b40d4..9e6dc844df 100644 --- a/dimos/manipulation/planning/monitor/world_monitor.py +++ b/dimos/manipulation/planning/monitor/world_monitor.py @@ -22,8 +22,8 @@ from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.manipulation.planning.factory import create_world +from dimos.manipulation.planning.monitor.robot_state_monitor import RobotStateMonitor from dimos.manipulation.planning.monitor.world_obstacle_monitor import WorldObstacleMonitor -from dimos.manipulation.planning.monitor.world_state_monitor import WorldStateMonitor from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.logging_config import setup_logger @@ -61,7 +61,7 @@ def __init__( self._world: WorldSpec = create_world(backend=backend, enable_viz=enable_viz, **kwargs) self._lock = threading.RLock() self._robot_joints: dict[WorldRobotID, list[str]] = {} - self._state_monitors: dict[WorldRobotID, WorldStateMonitor] = {} + self._state_monitors: dict[WorldRobotID, RobotStateMonitor] = {} self._obstacle_monitor: WorldObstacleMonitor | None = None self._viz_thread: threading.Thread | None = None self._viz_stop_event = threading.Event() @@ -139,7 +139,7 @@ def start_state_monitor( if joint_name_mapping is None and config.joint_name_mapping: joint_name_mapping = config.joint_name_mapping - monitor = WorldStateMonitor( + monitor = RobotStateMonitor( world=self._world, lock=self._lock, robot_id=robot_id, @@ -480,7 +480,7 @@ def world(self) -> WorldSpec: """Get underlying WorldSpec. Not thread-safe for modifications.""" return self._world - def get_state_monitor(self, robot_id: str) -> WorldStateMonitor | None: + def get_state_monitor(self, robot_id: str) -> RobotStateMonitor | None: """Get state monitor for a robot (may be None).""" return self._state_monitors.get(robot_id) diff --git a/dimos/manipulation/planning/spec/config.py b/dimos/manipulation/planning/spec/config.py index e278a645ab..59c7f05382 100644 --- a/dimos/manipulation/planning/spec/config.py +++ b/dimos/manipulation/planning/spec/config.py @@ -47,7 +47,7 @@ class RobotModelConfig(ModuleConfig): max_velocity: Maximum joint velocity for trajectory generation (rad/s) max_acceleration: Maximum joint acceleration for trajectory generation (rad/s^2) joint_name_mapping: Maps coordinator joint names to URDF joint names. - Example: {"left_joint1": "joint1"} means coordinator's "left_joint1" + Example: {"left/joint1": "joint1"} means coordinator's "left/joint1" corresponds to URDF's "joint1". If empty, names are assumed to match. coordinator_task_name: Task name for executing trajectories via coordinator RPC. If set, trajectories can be executed via execute_trajectory() RPC. diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index b7a18225f5..c610fc329a 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -183,6 +183,8 @@ def __init__(self, time_step: float = 0.0, enable_viz: bool = False): self._robot_counter = 0 self._obstacle_counter = 0 + # Cache: (resolved_path, base_pose) → model_instance (shared URDF at same pose) + # Built diagram and contexts (created after finalize) self._diagram: Any = None self._live_context: Context | None = None @@ -194,7 +196,10 @@ def __init__(self, time_step: float = 0.0, enable_viz: bool = False): self._obstacle_source_id: Any = None def add_robot(self, config: RobotModelConfig) -> WorldRobotID: - """Add a robot to the world. Returns robot_id.""" + """Add a robot to the world. Returns robot_id. + + Same model_path + base_pose reuses the model instance (e.g. two arms in one URDF). + """ if self._finalized: raise RuntimeError("Cannot add robot after world is finalized") @@ -202,8 +207,21 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: self._robot_counter += 1 robot_id = f"robot_{self._robot_counter}" + config.model_path.resolve() + tuple( + [ + config.base_pose.position.x, + config.base_pose.position.y, + config.base_pose.position.z, + config.base_pose.orientation.x, + config.base_pose.orientation.y, + config.base_pose.orientation.z, + config.base_pose.orientation.w, + ] + ) model_instance = self._load_model(config) self._weld_base_if_needed(config, model_instance) + self._validate_joints(config, model_instance) ee_frame = self._plant.GetBodyByName( @@ -211,7 +229,7 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: ).body_frame() base_frame = self._plant.GetBodyByName(config.base_link, model_instance).body_frame() - # Load a second copy of the URDF as the preview (yellow ghost) robot + # Preview (yellow ghost) — always a separate instance per robot preview_model_instance = None if self._enable_viz: preview_model_instance = self._load_model(config) diff --git a/dimos/manipulation/test_manipulation_module.py b/dimos/manipulation/test_manipulation_module.py index a5780d94d8..3d9f22db9a 100644 --- a/dimos/manipulation/test_manipulation_module.py +++ b/dimos/manipulation/test_manipulation_module.py @@ -68,13 +68,13 @@ def _get_xarm7_config() -> RobotModelConfig: max_velocity=1.0, max_acceleration=2.0, joint_name_mapping={ - "arm_joint1": "joint1", - "arm_joint2": "joint2", - "arm_joint3": "joint3", - "arm_joint4": "joint4", - "arm_joint5": "joint5", - "arm_joint6": "joint6", - "arm_joint7": "joint7", + "arm/joint1": "joint1", + "arm/joint2": "joint2", + "arm/joint3": "joint3", + "arm/joint4": "joint4", + "arm/joint5": "joint5", + "arm/joint6": "joint6", + "arm/joint7": "joint7", }, coordinator_task_name="traj_arm", ) @@ -90,13 +90,13 @@ def joint_state_zeros(): """Create a JointState message with zeros for XArm7.""" return JointState( name=[ - "arm_joint1", - "arm_joint2", - "arm_joint3", - "arm_joint4", - "arm_joint5", - "arm_joint6", - "arm_joint7", + "arm/joint1", + "arm/joint2", + "arm/joint3", + "arm/joint4", + "arm/joint5", + "arm/joint6", + "arm/joint7", ], position=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], velocity=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index d7c9583fa8..0fd0690136 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -61,9 +61,9 @@ def robot_config_with_mapping(): end_effector_link="link_tcp", base_link="link_base", joint_name_mapping={ - "left_joint1": "joint1", - "left_joint2": "joint2", - "left_joint3": "joint3", + "left/joint1": "joint1", + "left/joint2": "joint2", + "left/joint3": "joint3", }, coordinator_task_name="traj_left", ) @@ -207,7 +207,7 @@ def test_mapping_translates_names(self, robot_config_with_mapping, simple_trajec result = module._translate_trajectory_to_coordinator( simple_trajectory, robot_config_with_mapping ) - assert result.joint_names == ["left_joint1", "left_joint2", "left_joint3"] + assert result.joint_names == ["left/joint1", "left/joint2", "left/joint3"] assert len(result.points) == 2 # Points preserved @@ -275,9 +275,9 @@ def test_bidirectional_mapping(self, robot_config_with_mapping): config = robot_config_with_mapping # Coordinator -> URDF - assert config.get_urdf_joint_name("left_joint1") == "joint1" + assert config.get_urdf_joint_name("left/joint1") == "joint1" assert config.get_urdf_joint_name("unknown") == "unknown" # URDF -> Coordinator - assert config.get_coordinator_joint_name("joint1") == "left_joint1" + assert config.get_coordinator_joint_name("joint1") == "left/joint1" assert config.get_coordinator_joint_name("unknown") == "unknown" diff --git a/dimos/robot/catalog/piper.py b/dimos/robot/catalog/piper.py index 1d09b6791b..a57e1ceb0f 100644 --- a/dimos/robot/catalog/piper.py +++ b/dimos/robot/catalog/piper.py @@ -19,7 +19,7 @@ from typing import Any from dimos.core.global_config import global_config -from dimos.robot.config import RobotConfig +from dimos.robot.config import GripperConfig, RobotConfig from dimos.utils.data import LfsPath # Pre-built MJCF for Pinocchio FK (xacro not supported by Pinocchio) @@ -75,6 +75,13 @@ def piper( "xacro_args": {}, "auto_convert_meshes": True, "collision_exclusion_pairs": PIPER_GRIPPER_COLLISION_EXCLUSIONS, + "gripper": GripperConfig( + type="piper", + joints=["gripper"], + collision_exclusions=PIPER_GRIPPER_COLLISION_EXCLUSIONS, + open_position=0.08, + close_position=0.0, + ), } if global_config.simulation and adapter_type == "mock": defaults.update(adapter_type="sim_mujoco", address=str(PIPER_SIM_PATH)) diff --git a/dimos/robot/catalog/ufactory.py b/dimos/robot/catalog/ufactory.py index 9271ef55c6..1c66fdd7bd 100644 --- a/dimos/robot/catalog/ufactory.py +++ b/dimos/robot/catalog/ufactory.py @@ -19,7 +19,7 @@ from typing import Any from dimos.core.global_config import global_config -from dimos.robot.config import RobotConfig +from dimos.robot.config import GripperConfig, RobotConfig from dimos.utils.data import LfsPath # Pre-built URDFs for Pinocchio FK (xacro not supported by Pinocchio) @@ -90,6 +90,15 @@ def xarm7( "auto_convert_meshes": True, "collision_exclusion_pairs": XARM_GRIPPER_COLLISION_EXCLUSIONS if add_gripper else [], "tf_extra_links": tf_extra_links or [], + "gripper": GripperConfig( + type="xarm", + joints=["gripper"], + collision_exclusions=XARM_GRIPPER_COLLISION_EXCLUSIONS, + open_position=0.85, + close_position=0.0, + ) + if add_gripper + else None, } if global_config.simulation and adapter_type == "mock": defaults.update(adapter_type="sim_mujoco", address=str(XARM7_SIM_PATH)) @@ -137,6 +146,15 @@ def xarm6( "auto_convert_meshes": True, "collision_exclusion_pairs": XARM_GRIPPER_COLLISION_EXCLUSIONS if add_gripper else [], "tf_extra_links": tf_extra_links or [], + "gripper": GripperConfig( + type="xarm", + joints=["gripper"], + collision_exclusions=XARM_GRIPPER_COLLISION_EXCLUSIONS, + open_position=0.85, + close_position=0.0, + ) + if add_gripper + else None, } if global_config.simulation and adapter_type == "mock": defaults.update(adapter_type="sim_mujoco", address=str(XARM6_SIM_PATH)) diff --git a/dimos/robot/config.py b/dimos/robot/config.py index 3af6867918..1f225284bf 100644 --- a/dimos/robot/config.py +++ b/dimos/robot/config.py @@ -101,7 +101,7 @@ class RobotConfig(BaseModel): def _ensure_prefix(self) -> None: """Ensure joint_prefix is set (no model parsing needed).""" if self.joint_prefix is None: - self.joint_prefix = f"{self.name}_" + self.joint_prefix = f"{self.name}/" def _ensure_parsed(self) -> ModelDescription: """Parse model lazily on first access.""" @@ -218,6 +218,7 @@ def to_robot_model_config(self) -> RobotModelConfig: def to_hardware_component(self) -> HardwareComponent: """Generate HardwareComponent for ControlCoordinator.""" + self._ensure_prefix() gripper_joints: list[str] = [] if self.gripper and self.gripper.joints: gripper_joints = [f"{self.joint_prefix}{j}" for j in self.gripper.joints] diff --git a/dimos/robot/manipulators/piper/blueprints.py b/dimos/robot/manipulators/piper/blueprints.py index d0e5f7d827..d64f982b22 100644 --- a/dimos/robot/manipulators/piper/blueprints.py +++ b/dimos/robot/manipulators/piper/blueprints.py @@ -24,6 +24,7 @@ from dimos.control.coordinator import ControlCoordinator from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config from dimos.core.transport import LCMTransport from dimos.manipulation.manipulation_module import ManipulationModule from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -31,7 +32,11 @@ from dimos.robot.catalog.piper import PIPER_FK_MODEL, piper as _catalog_piper from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -_piper_cfg = _catalog_piper(name="arm") +_piper_cfg = _catalog_piper( + name="arm", + adapter_type="piper" if global_config.can_port else "mock", + address=global_config.can_port or "can0", +) # Piper 6-DOF mock sim + keyboard teleop + Drake visualization keyboard_teleop_piper = autoconnect(