From 7c46afcb56d7df299330832d5d206411e8c1364f Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 28 Mar 2026 12:17:49 -0700 Subject: [PATCH 01/40] Added robot config factory as single source of truth of robot description for all modules --- dimos/robot/catalog/__init__.py | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 dimos/robot/catalog/__init__.py diff --git a/dimos/robot/catalog/__init__.py b/dimos/robot/catalog/__init__.py new file mode 100644 index 0000000000..5320cbec3b --- /dev/null +++ b/dimos/robot/catalog/__init__.py @@ -0,0 +1,29 @@ +# 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. + +"""Pre-built robot configurations. + +Each function returns a :class:`~dimos.robot.config.RobotConfig` with +robot-specific defaults. Override any field via keyword arguments:: + + from dimos.robot.catalog import xarm7 + + config = xarm7(name="arm", address="192.168.1.185") +""" + +from dimos.robot.catalog.panda import panda +from dimos.robot.catalog.piper import piper +from dimos.robot.catalog.ufactory import xarm6, xarm7 + +__all__ = ["panda", "piper", "xarm6", "xarm7"] From cbd487f0df7f54bdfbcdd4e523e2187badc0863e Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 28 Mar 2026 12:35:09 -0700 Subject: [PATCH 02/40] trigger CI From f25f5b2de178b79071a321226f26589ac7a7d458 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 28 Mar 2026 12:49:57 -0700 Subject: [PATCH 03/40] added a threading lock for xacro files processing also cehcks for root link edge cases --- dimos/robot/model_parser.py | 1 + 1 file changed, 1 insertion(+) diff --git a/dimos/robot/model_parser.py b/dimos/robot/model_parser.py index 1b760d065d..e4423f1d36 100644 --- a/dimos/robot/model_parser.py +++ b/dimos/robot/model_parser.py @@ -16,6 +16,7 @@ from __future__ import annotations +import threading from dataclasses import dataclass, field from pathlib import Path import xml.etree.ElementTree as ET From cf1c485fab55ebe9edbd4ac51ca278bea0ed0f34 Mon Sep 17 00:00:00 2001 From: mustafab0 <39084056+mustafab0@users.noreply.github.com> Date: Sat, 28 Mar 2026 20:00:37 +0000 Subject: [PATCH 04/40] CI code cleanup --- dimos/robot/model_parser.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/robot/model_parser.py b/dimos/robot/model_parser.py index e4423f1d36..9a20113e54 100644 --- a/dimos/robot/model_parser.py +++ b/dimos/robot/model_parser.py @@ -16,9 +16,9 @@ from __future__ import annotations -import threading from dataclasses import dataclass, field from pathlib import Path +import threading import xml.etree.ElementTree as ET from dimos.utils.logging_config import setup_logger From 3e41d1ec96a3c231acb854c283c30388f5d7fe3c Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 28 Mar 2026 14:36:31 -0700 Subject: [PATCH 05/40] trigger CI From 534c890bfbcd1de4e24a26892ce3093d888c955d Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 28 Mar 2026 16:32:44 -0700 Subject: [PATCH 06/40] removed the init files for robot catalog --- dimos/robot/catalog/__init__.py | 29 ----------------------------- 1 file changed, 29 deletions(-) delete mode 100644 dimos/robot/catalog/__init__.py diff --git a/dimos/robot/catalog/__init__.py b/dimos/robot/catalog/__init__.py deleted file mode 100644 index 5320cbec3b..0000000000 --- a/dimos/robot/catalog/__init__.py +++ /dev/null @@ -1,29 +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. - -"""Pre-built robot configurations. - -Each function returns a :class:`~dimos.robot.config.RobotConfig` with -robot-specific defaults. Override any field via keyword arguments:: - - from dimos.robot.catalog import xarm7 - - config = xarm7(name="arm", address="192.168.1.185") -""" - -from dimos.robot.catalog.panda import panda -from dimos.robot.catalog.piper import piper -from dimos.robot.catalog.ufactory import xarm6, xarm7 - -__all__ = ["panda", "piper", "xarm6", "xarm7"] From b192983b286a026a24b0a1174624aed460db0e61 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 28 Mar 2026 19:51:45 -0700 Subject: [PATCH 07/40] Created a fake ament index with symlinks so ament_index_python resolves our LFS packages natively. No monkey-patching. --- dimos/robot/model_parser.py | 1 - 1 file changed, 1 deletion(-) diff --git a/dimos/robot/model_parser.py b/dimos/robot/model_parser.py index 9a20113e54..1b760d065d 100644 --- a/dimos/robot/model_parser.py +++ b/dimos/robot/model_parser.py @@ -18,7 +18,6 @@ from dataclasses import dataclass, field from pathlib import Path -import threading import xml.etree.ElementTree as ET from dimos.utils.logging_config import setup_logger From 0d3aad56c2e746539c75d2c63b8557f0afe159a0 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 28 Mar 2026 12:35:09 -0700 Subject: [PATCH 08/40] trigger CI From b7c175f0d21300a3e9acdc9eb821ef97bd41a0ff Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 28 Mar 2026 14:36:31 -0700 Subject: [PATCH 09/40] trigger CI From 961750647401df824e899bfb501d2f253bf210de Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 31 Mar 2026 19:22:31 -0700 Subject: [PATCH 10/40] updated mobile base blueprint and cleaned up older files --- dimos/control/blueprints/mobile.py | 1 - 1 file changed, 1 deletion(-) diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index aa4f7f5409..2e97033028 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -44,7 +44,6 @@ def _mock_twist_base(hw_id: str = "base") -> HardwareComponent: adapter_type="mock_twist_base", ) - # Mock holonomic twist base (3-DOF: vx, vy, wz) coordinator_mock_twist_base = ControlCoordinator.blueprint( hardware=[_mock_twist_base()], From 5cd8f47d59da47c17f0dba34f2461521ec9cd5c7 Mon Sep 17 00:00:00 2001 From: mustafab0 <39084056+mustafab0@users.noreply.github.com> Date: Wed, 1 Apr 2026 02:34:36 +0000 Subject: [PATCH 11/40] CI code cleanup --- dimos/control/blueprints/mobile.py | 1 + 1 file changed, 1 insertion(+) diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index 2e97033028..aa4f7f5409 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -44,6 +44,7 @@ def _mock_twist_base(hw_id: str = "base") -> HardwareComponent: adapter_type="mock_twist_base", ) + # Mock holonomic twist base (3-DOF: vx, vy, wz) coordinator_mock_twist_base = ControlCoordinator.blueprint( hardware=[_mock_twist_base()], From 302a1c387aaafd1414abbfbc3826bf22d2935d36 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 31 Mar 2026 20:48:20 -0700 Subject: [PATCH 12/40] trigger CI From f774c99c951d72d1b1560230269c39cbf44d550b Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 1 Apr 2026 18:41:35 -0700 Subject: [PATCH 13/40] trigger CI From 93252dc9c78c2bfde7b138fc35e4c6e40840c00b Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 28 Mar 2026 12:35:09 -0700 Subject: [PATCH 14/40] trigger CI From 379440baa0af40dd25684d41d6c9cf80743e68cc Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 28 Mar 2026 14:36:31 -0700 Subject: [PATCH 15/40] trigger CI From 8a4732882a46a2f19c749efde3e1d9cbbc89a477 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 28 Mar 2026 12:35:09 -0700 Subject: [PATCH 16/40] trigger CI From 55f180ca787cdcbc4b36228f0d62ba760156f6dd Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 28 Mar 2026 14:36:31 -0700 Subject: [PATCH 17/40] trigger CI From b1985672b49cfb176fc05e1f01353358d8d19f5d Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 31 Mar 2026 20:48:20 -0700 Subject: [PATCH 18/40] trigger CI From fc1b60d146d77a4a3966d28f70c8c6e32ee39ad7 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 1 Apr 2026 18:41:35 -0700 Subject: [PATCH 19/40] trigger CI From e00ddeb1e0529d604c0dfdc958405c49bbf2de1e Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 28 Mar 2026 12:35:09 -0700 Subject: [PATCH 20/40] trigger CI From fabb06a02a5348996aa97dfb550729bf24b6d859 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 28 Mar 2026 14:36:31 -0700 Subject: [PATCH 21/40] trigger CI From 607edfa238b0ef5759932fe1c3037ff1b9e4f8ac Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 28 Mar 2026 12:35:09 -0700 Subject: [PATCH 22/40] trigger CI From ef4d786239ebd2a12c34ce4e959265c7d9e101c4 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 28 Mar 2026 14:36:31 -0700 Subject: [PATCH 23/40] trigger CI From e078a52fcac5394c6c62d07c67bc9cc9a836293f Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 31 Mar 2026 20:48:20 -0700 Subject: [PATCH 24/40] trigger CI From cfccff2c9590bcb7098e584019067be9993c4639 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 1 Apr 2026 12:50:48 -0700 Subject: [PATCH 25/40] renamed WorldStateMonitor to RobotStateMonitor --- ...rld_state_monitor.py => robot_state_monitor.py} | 14 +++++++------- .../manipulation/planning/monitor/world_monitor.py | 8 ++++---- 2 files changed, 11 insertions(+), 11 deletions(-) rename dimos/manipulation/planning/monitor/{world_state_monitor.py => robot_state_monitor.py} (96%) 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..6bcbd4931b 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__( @@ -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..026b370005 100644 --- a/dimos/manipulation/planning/monitor/world_monitor.py +++ b/dimos/manipulation/planning/monitor/world_monitor.py @@ -23,7 +23,7 @@ from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.manipulation.planning.factory import create_world from dimos.manipulation.planning.monitor.world_obstacle_monitor import WorldObstacleMonitor -from dimos.manipulation.planning.monitor.world_state_monitor import WorldStateMonitor +from dimos.manipulation.planning.monitor.robot_state_monitor import RobotStateMonitor 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) From 19cb3735efc035097f5d8aa776477c1d97d491ac Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 1 Apr 2026 14:16:19 -0700 Subject: [PATCH 26/40] added a split joint name utility using "/" as seperator --- dimos/control/components.py | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/dimos/control/components.py b/dimos/control/components.py index 42b5d9c9ea..ee0be08cd9 100644 --- a/dimos/control/components.py +++ b/dimos/control/components.py @@ -22,6 +22,16 @@ JointName = str 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" @@ -44,7 +54,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 +83,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 +96,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 +125,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 +145,5 @@ def make_twist_base_joints( "make_gripper_joints", "make_joints", "make_twist_base_joints", + "split_joint_name", ] From 52255d53a15c3bdd82466d419e4dab53be94fce5 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 1 Apr 2026 14:16:47 -0700 Subject: [PATCH 27/40] updated all joint mapping reference to use "/" seperator --- dimos/control/coordinator.py | 5 +- dimos/control/task.py | 6 +- dimos/control/tasks/servo_task.py | 2 +- dimos/control/tasks/teleop_task.py | 2 +- dimos/control/tasks/trajectory_task.py | 2 +- dimos/control/tasks/velocity_task.py | 2 +- dimos/control/test_control.py | 52 ++++----- dimos/e2e_tests/test_control_coordinator.py | 16 +-- .../control/coordinator_client.py | 11 +- dimos/manipulation/manipulation_module.py | 101 ++++++++++++------ .../planning/monitor/robot_state_monitor.py | 2 +- dimos/manipulation/planning/spec/config.py | 2 +- .../manipulation/test_manipulation_module.py | 28 ++--- dimos/manipulation/test_manipulation_unit.py | 12 +-- 14 files changed, 141 insertions(+), 102 deletions(-) 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/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/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..60a435a59a 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,20 +244,38 @@ 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): + continue # Not all joints present for this robot + + # Build per-robot sub-message (coordinator namespace) + sub_positions = [msg.position[idx] for idx in indices] # type: ignore[index] + sub_msg = JointState(name=list(coord_names), position=sub_positions) + + # 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}") @@ -601,23 +619,37 @@ 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 +662,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 +1168,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 +1184,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 +1203,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/robot_state_monitor.py b/dimos/manipulation/planning/monitor/robot_state_monitor.py index 6bcbd4931b..f395a1499a 100644 --- a/dimos/manipulation/planning/monitor/robot_state_monitor.py +++ b/dimos/manipulation/planning/monitor/robot_state_monitor.py @@ -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) """ 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/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" From 85db7f3e6da64722e56567d7d7aaf3f6b8372f4c Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 1 Apr 2026 14:48:32 -0700 Subject: [PATCH 28/40] updated seperator after rebase --- dimos/robot/config.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/robot/config.py b/dimos/robot/config.py index 3af6867918..a541c5925d 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.""" From a63e4d6e1030fbca26ef51ae6057cc4a711765c2 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 1 Apr 2026 19:59:38 -0700 Subject: [PATCH 29/40] addded gripper control to catalog --- dimos/robot/catalog/piper.py | 9 ++++++++- dimos/robot/catalog/ufactory.py | 20 +++++++++++++++++++- dimos/robot/config.py | 1 + 3 files changed, 28 insertions(+), 2 deletions(-) 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 a541c5925d..1f225284bf 100644 --- a/dimos/robot/config.py +++ b/dimos/robot/config.py @@ -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] From 620a837fae606da468c9d0036fb91284a192cc47 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 1 Apr 2026 20:01:17 -0700 Subject: [PATCH 30/40] updated blueprints to use global ip config --- dimos/manipulation/blueprints.py | 33 ++++++++++++++++---- dimos/robot/manipulators/piper/blueprints.py | 7 ++++- 2 files changed, 33 insertions(+), 7 deletions(-) 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/robot/manipulators/piper/blueprints.py b/dimos/robot/manipulators/piper/blueprints.py index d0e5f7d827..307d7cc33e 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 != "can0" else "mock", + address=global_config.can_port, +) # Piper 6-DOF mock sim + keyboard teleop + Drake visualization keyboard_teleop_piper = autoconnect( From 354384d095597fead061482701588057bd71c720 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 2 Apr 2026 12:32:05 -0700 Subject: [PATCH 31/40] added 1 urdf, 2 arm support to drake world add robot method --- .../planning/world/drake_world.py | 34 ++++++++++++++++--- 1 file changed, 30 insertions(+), 4 deletions(-) diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index b7a18225f5..3f451c3780 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -183,6 +183,9 @@ 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) + self._loaded_models: dict[tuple[Path, tuple[float, ...]], Any] = {} + # Built diagram and contexts (created after finalize) self._diagram: Any = None self._live_context: Context | None = None @@ -194,7 +197,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 +208,28 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: self._robot_counter += 1 robot_id = f"robot_{self._robot_counter}" - model_instance = self._load_model(config) - self._weld_base_if_needed(config, model_instance) + resolved_path = config.model_path.resolve() + pose_key = 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] + ) + cache_key = (resolved_path, pose_key) + existing_instance = self._loaded_models.get(cache_key) + + if existing_instance is not None: + # Reuse the already-loaded model instance (shared URDF at same pose) + model_instance = existing_instance + logger.info( + f"Reusing shared model instance for '{config.name}' " + f"(same URDF: {resolved_path.name})" + ) + else: + model_instance = self._load_model(config) + self._weld_base_if_needed(config, model_instance) + self._loaded_models[cache_key] = model_instance + self._validate_joints(config, model_instance) ee_frame = self._plant.GetBodyByName( @@ -211,7 +237,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) From bd5d5d85641a85f0b2eeae14a216aacd276d478a Mon Sep 17 00:00:00 2001 From: mustafab0 <39084056+mustafab0@users.noreply.github.com> Date: Thu, 2 Apr 2026 21:35:54 +0000 Subject: [PATCH 32/40] CI code cleanup --- dimos/control/components.py | 1 + dimos/manipulation/manipulation_module.py | 4 +++- .../manipulation/planning/monitor/world_monitor.py | 2 +- dimos/manipulation/planning/world/drake_world.py | 13 +++++++++---- 4 files changed, 14 insertions(+), 6 deletions(-) diff --git a/dimos/control/components.py b/dimos/control/components.py index ee0be08cd9..be12aa4fb1 100644 --- a/dimos/control/components.py +++ b/dimos/control/components.py @@ -22,6 +22,7 @@ JointName = str TaskName = str + def split_joint_name(joint_name: str) -> tuple[str, str]: """Split a coordinator joint name into (hardware_id, suffix). diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 60a435a59a..ffbd86a3da 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -619,7 +619,9 @@ 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(init.position) if (init := self._init_joints.get(robot_name)) else None, + "init_joints": list(init.position) + if (init := self._init_joints.get(robot_name)) + else None, } @rpc diff --git a/dimos/manipulation/planning/monitor/world_monitor.py b/dimos/manipulation/planning/monitor/world_monitor.py index 026b370005..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.world_obstacle_monitor import WorldObstacleMonitor from dimos.manipulation.planning.monitor.robot_state_monitor import RobotStateMonitor +from dimos.manipulation.planning.monitor.world_obstacle_monitor import WorldObstacleMonitor from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.logging_config import setup_logger diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index 3f451c3780..16d75f36ad 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -210,10 +210,15 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: resolved_path = config.model_path.resolve() pose_key = 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] + [ + 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, + ] ) cache_key = (resolved_path, pose_key) existing_instance = self._loaded_models.get(cache_key) From ecf45ef97058f06c0e0d82811361c133e6d17e28 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 2 Apr 2026 14:38:01 -0700 Subject: [PATCH 33/40] trigger CI From 6b520eb323115415f0d14992c2f709d30b62c07e Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 2 Apr 2026 14:51:22 -0700 Subject: [PATCH 34/40] fixed piper global config --- dimos/control/blueprints/basic.py | 2 +- dimos/control/blueprints/dual.py | 2 +- dimos/control/blueprints/teleop.py | 2 +- dimos/core/global_config.py | 2 +- dimos/robot/manipulators/piper/blueprints.py | 4 ++-- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/dimos/control/blueprints/basic.py b/dimos/control/blueprints/basic.py index f3ffdc3dcc..ae37aaa679 100644 --- a/dimos/control/blueprints/basic.py +++ b/dimos/control/blueprints/basic.py @@ -80,7 +80,7 @@ ) # 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..9ddd95dab9 100644 --- a/dimos/control/blueprints/dual.py +++ b/dimos/control/blueprints/dual.py @@ -67,7 +67,7 @@ _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..87f971146b 100644 --- a/dimos/control/blueprints/teleop.py +++ b/dimos/control/blueprints/teleop.py @@ -50,7 +50,7 @@ _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/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/robot/manipulators/piper/blueprints.py b/dimos/robot/manipulators/piper/blueprints.py index 307d7cc33e..d64f982b22 100644 --- a/dimos/robot/manipulators/piper/blueprints.py +++ b/dimos/robot/manipulators/piper/blueprints.py @@ -34,8 +34,8 @@ _piper_cfg = _catalog_piper( name="arm", - adapter_type="piper" if global_config.can_port != "can0" else "mock", - address=global_config.can_port, + 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 From 82ed892321ffa2c47209318fd6989d1ff5255093 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 2 Apr 2026 14:53:52 -0700 Subject: [PATCH 35/40] velocity data is now forwarded with the same index slicing as positions --- dimos/manipulation/manipulation_module.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index ffbd86a3da..c15d5a80ed 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -264,7 +264,16 @@ def _on_joint_state(self, msg: JointState) -> None: # Build per-robot sub-message (coordinator namespace) sub_positions = [msg.position[idx] for idx in indices] # type: ignore[index] - sub_msg = JointState(name=list(coord_names), position=sub_positions) + 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) From 081d38d49d0920c86029c87a8b93f77b505b90a6 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 3 Apr 2026 10:56:17 -0700 Subject: [PATCH 36/40] code logs which joints are missing instead of silently skipping --- dimos/manipulation/manipulation_module.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index c15d5a80ed..95b6d6b886 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -260,7 +260,9 @@ def _on_joint_state(self, msg: JointState) -> None: 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): - continue # Not all joints present for this robot + missing = [cn for cn, idx in zip(coord_names, indices) 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] From 3862710e4454d4182adf37c653d621f940999349 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Mon, 6 Apr 2026 18:55:05 -0700 Subject: [PATCH 37/40] CI cleanup issue --- dimos/control/blueprints/basic.py | 4 +++- dimos/control/blueprints/dual.py | 4 +++- dimos/control/blueprints/teleop.py | 4 +++- dimos/manipulation/manipulation_module.py | 4 +++- 4 files changed, 12 insertions(+), 4 deletions(-) diff --git a/dimos/control/blueprints/basic.py b/dimos/control/blueprints/basic.py index ae37aaa679..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 or "can0") +_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 9ddd95dab9..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 or "can0") +_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 87f971146b..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 or "can0") +_piper_cfg = _catalog_piper( + name="arm", adapter_type="piper", address=global_config.can_port or "can0" +) # -- Servo / velocity --------------------------------------------------------- diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 95b6d6b886..b53e6c2e98 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -260,7 +260,9 @@ def _on_joint_state(self, msg: JointState) -> None: 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) if idx is None] + 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 From 0ead24d19c1b5ef36bb229d7dcbdbc9e10ded538 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Mon, 6 Apr 2026 18:59:05 -0700 Subject: [PATCH 38/40] add_robot() call now always loads a fresh model instance no shared state between robots --- .../manipulation/planning/world/drake_world.py | 17 ++--------------- 1 file changed, 2 insertions(+), 15 deletions(-) diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index 16d75f36ad..1c2a57a4eb 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -184,7 +184,6 @@ def __init__(self, time_step: float = 0.0, enable_viz: bool = False): self._obstacle_counter = 0 # Cache: (resolved_path, base_pose) → model_instance (shared URDF at same pose) - self._loaded_models: dict[tuple[Path, tuple[float, ...]], Any] = {} # Built diagram and contexts (created after finalize) self._diagram: Any = None @@ -220,20 +219,8 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: config.base_pose.orientation.w, ] ) - cache_key = (resolved_path, pose_key) - existing_instance = self._loaded_models.get(cache_key) - - if existing_instance is not None: - # Reuse the already-loaded model instance (shared URDF at same pose) - model_instance = existing_instance - logger.info( - f"Reusing shared model instance for '{config.name}' " - f"(same URDF: {resolved_path.name})" - ) - else: - model_instance = self._load_model(config) - self._weld_base_if_needed(config, model_instance) - self._loaded_models[cache_key] = model_instance + model_instance = self._load_model(config) + self._weld_base_if_needed(config, model_instance) self._validate_joints(config, model_instance) From b817a658f618d6a57f1c13c4afd3108d469ed6e5 Mon Sep 17 00:00:00 2001 From: mustafab0 <39084056+mustafab0@users.noreply.github.com> Date: Tue, 7 Apr 2026 02:02:36 +0000 Subject: [PATCH 39/40] CI code cleanup --- dimos/manipulation/planning/world/drake_world.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index 1c2a57a4eb..c610fc329a 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -207,8 +207,8 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: self._robot_counter += 1 robot_id = f"robot_{self._robot_counter}" - resolved_path = config.model_path.resolve() - pose_key = tuple( + config.model_path.resolve() + tuple( [ config.base_pose.position.x, config.base_pose.position.y, From 23771197ea7ebbfbf27177946be1cf06f273a8c6 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Mon, 6 Apr 2026 19:02:57 -0700 Subject: [PATCH 40/40] trigger CI