From 181fcfc91fcbdbe4aa21b2d5513e465637d421c9 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Mon, 2 Mar 2026 12:41:45 -0800 Subject: [PATCH 01/15] feat: adding arm_ip and can_port to env --- default.env | 2 ++ 1 file changed, 2 insertions(+) diff --git a/default.env b/default.env index 5098a60892..6e4be235dd 100644 --- a/default.env +++ b/default.env @@ -5,6 +5,8 @@ ANTHROPIC_API_KEY= HF_TOKEN= HUGGINGFACE_PRV_ENDPOINT= ROBOT_IP= +ARM_IP= +CAN_PORT=can0 CONN_TYPE=webrtc WEBRTC_SERVER_HOST=0.0.0.0 WEBRTC_SERVER_PORT=9991 From d8845466b7f7994dd99fc67646a9837e8eb6e00c Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Mon, 2 Mar 2026 12:45:22 -0800 Subject: [PATCH 02/15] feat: using env variables in blueprints --- dimos/control/blueprints.py | 35 ++++++++++++++++++++--------------- 1 file changed, 20 insertions(+), 15 deletions(-) diff --git a/dimos/control/blueprints.py b/dimos/control/blueprints.py index fff2083322..4d801b9b9b 100644 --- a/dimos/control/blueprints.py +++ b/dimos/control/blueprints.py @@ -30,6 +30,8 @@ from __future__ import annotations +import os + from dimos.control.components import ( HardwareComponent, HardwareType, @@ -49,6 +51,9 @@ _XARM6_MODEL_PATH = LfsPath("xarm_description/urdf/xarm6/xarm6.urdf") _XARM7_MODEL_PATH = LfsPath("xarm_description/urdf/xarm7/xarm7.urdf") +_ARM_IP = os.getenv("ARM_IP") +_CAN_PORT = os.getenv("CAN_PORT", "can0") + # Mock 7-DOF arm (for testing) coordinator_mock = control_coordinator( @@ -88,7 +93,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 7), adapter_type="xarm", - address="192.168.2.235", + address=_ARM_IP, auto_enable=True, ), ], @@ -117,7 +122,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 6), adapter_type="xarm", - address="192.168.1.210", + address=_ARM_IP, auto_enable=True, ), ], @@ -146,7 +151,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 6), adapter_type="piper", - address="can0", + address=_CAN_PORT, auto_enable=True, ), ], @@ -215,7 +220,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("left_arm", 7), adapter_type="xarm", - address="192.168.2.235", + address=_ARM_IP, auto_enable=True, ), HardwareComponent( @@ -223,7 +228,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("right_arm", 6), adapter_type="xarm", - address="192.168.1.210", + address=_ARM_IP, auto_enable=True, ), ], @@ -258,7 +263,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("xarm_arm", 6), adapter_type="xarm", - address="192.168.1.210", + address=_ARM_IP, auto_enable=True, ), HardwareComponent( @@ -266,7 +271,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("piper_arm", 6), adapter_type="piper", - address="can0", + address=_CAN_PORT, auto_enable=True, ), ], @@ -302,7 +307,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 6), adapter_type="xarm", - address="192.168.1.210", + address=_ARM_IP, auto_enable=True, ), ], @@ -332,7 +337,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 6), adapter_type="xarm", - address="192.168.1.210", + address=_ARM_IP, auto_enable=True, ), ], @@ -362,7 +367,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 6), adapter_type="xarm", - address="192.168.1.210", + address=_ARM_IP, auto_enable=True, ), ], @@ -431,7 +436,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 6), adapter_type="piper", - address="can0", + address=_CAN_PORT, auto_enable=True, ), ], @@ -466,7 +471,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 7), adapter_type="xarm", - address="192.168.2.235", + address=_ARM_IP, auto_enable=True, gripper_joints=make_gripper_joints("arm"), ), @@ -506,7 +511,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 6), adapter_type="piper", - address="can0", + address=_CAN_PORT, auto_enable=True, ), ], @@ -542,7 +547,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("xarm_arm", 6), adapter_type="xarm", - address="192.168.1.210", + address=_ARM_IP, auto_enable=True, ), HardwareComponent( @@ -550,7 +555,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("piper_arm", 6), adapter_type="piper", - address="can0", + address=_CAN_PORT, auto_enable=True, ), ], From 4dcaaef92f1e5f20cc1e72b2203f1d18725212dd Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Sat, 14 Mar 2026 17:22:17 -0700 Subject: [PATCH 03/15] arm_ip env variables --- default.env | 3 ++- dimos/control/blueprints.py | 23 ++++++++++++----------- 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/default.env b/default.env index 6e4be235dd..d4d6a7d6fd 100644 --- a/default.env +++ b/default.env @@ -5,7 +5,8 @@ ANTHROPIC_API_KEY= HF_TOKEN= HUGGINGFACE_PRV_ENDPOINT= ROBOT_IP= -ARM_IP= +XARM7_IP= +XARM6_IP= CAN_PORT=can0 CONN_TYPE=webrtc WEBRTC_SERVER_HOST=0.0.0.0 diff --git a/dimos/control/blueprints.py b/dimos/control/blueprints.py index 4d801b9b9b..9e8bb7377c 100644 --- a/dimos/control/blueprints.py +++ b/dimos/control/blueprints.py @@ -51,7 +51,8 @@ _XARM6_MODEL_PATH = LfsPath("xarm_description/urdf/xarm6/xarm6.urdf") _XARM7_MODEL_PATH = LfsPath("xarm_description/urdf/xarm7/xarm7.urdf") -_ARM_IP = os.getenv("ARM_IP") +_XARM7_IP = os.getenv("XARM7_IP") +_XARM6_IP = os.getenv("XARM6_IP") _CAN_PORT = os.getenv("CAN_PORT", "can0") @@ -93,7 +94,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 7), adapter_type="xarm", - address=_ARM_IP, + address=_XARM7_IP, auto_enable=True, ), ], @@ -122,7 +123,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 6), adapter_type="xarm", - address=_ARM_IP, + address=_XARM6_IP, auto_enable=True, ), ], @@ -220,7 +221,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("left_arm", 7), adapter_type="xarm", - address=_ARM_IP, + address=_XARM7_IP, auto_enable=True, ), HardwareComponent( @@ -228,7 +229,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("right_arm", 6), adapter_type="xarm", - address=_ARM_IP, + address=_XARM6_IP, auto_enable=True, ), ], @@ -263,7 +264,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("xarm_arm", 6), adapter_type="xarm", - address=_ARM_IP, + address=_XARM6_IP, auto_enable=True, ), HardwareComponent( @@ -307,7 +308,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 6), adapter_type="xarm", - address=_ARM_IP, + address=_XARM6_IP, auto_enable=True, ), ], @@ -337,7 +338,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 6), adapter_type="xarm", - address=_ARM_IP, + address=_XARM6_IP, auto_enable=True, ), ], @@ -367,7 +368,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 6), adapter_type="xarm", - address=_ARM_IP, + address=_XARM6_IP, auto_enable=True, ), ], @@ -471,7 +472,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 7), adapter_type="xarm", - address=_ARM_IP, + address=_XARM7_IP, auto_enable=True, gripper_joints=make_gripper_joints("arm"), ), @@ -547,7 +548,7 @@ hardware_type=HardwareType.MANIPULATOR, joints=make_joints("xarm_arm", 6), adapter_type="xarm", - address=_ARM_IP, + address=_XARM6_IP, auto_enable=True, ), HardwareComponent( From e19d9068c3e4ea345885df18e7e1c3e30d5ad952 Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Wed, 18 Mar 2026 12:12:53 -0700 Subject: [PATCH 04/15] misc: control blueprints cleanup --- dimos/control/blueprints/__init__.py | 83 ++++ dimos/control/blueprints/basic.py | 169 ++++++++ dimos/control/blueprints/dual.py | 166 ++++++++ dimos/control/blueprints/mobile.py | 99 +++++ .../{blueprints.py => blueprints/teleop.py} | 381 +----------------- 5 files changed, 539 insertions(+), 359 deletions(-) create mode 100644 dimos/control/blueprints/__init__.py create mode 100644 dimos/control/blueprints/basic.py create mode 100644 dimos/control/blueprints/dual.py create mode 100644 dimos/control/blueprints/mobile.py rename dimos/control/{blueprints.py => blueprints/teleop.py} (51%) diff --git a/dimos/control/blueprints/__init__.py b/dimos/control/blueprints/__init__.py new file mode 100644 index 0000000000..ca7ba7e417 --- /dev/null +++ b/dimos/control/blueprints/__init__.py @@ -0,0 +1,83 @@ +# 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-configured blueprints for the ControlCoordinator. + +This package provides ready-to-use coordinator blueprints for common setups. + +Usage: + # Run via CLI: + dimos run coordinator-mock # Mock 7-DOF arm + dimos run coordinator-xarm7 # XArm7 real hardware + dimos run coordinator-dual-mock # Dual mock arms + + # Or programmatically: + from dimos.control.blueprints import coordinator_mock + coordinator = coordinator_mock.build() + coordinator.loop() +""" + +from __future__ import annotations + +from dimos.control.blueprints.basic import ( + coordinator_basic, + coordinator_mock, + coordinator_piper, + coordinator_xarm6, + coordinator_xarm7, +) +from dimos.control.blueprints.dual import ( + coordinator_dual_mock, + coordinator_dual_xarm, + coordinator_piper_xarm, +) +from dimos.control.blueprints.mobile import ( + coordinator_mobile_manip_mock, + coordinator_mock_twist_base, +) +from dimos.control.blueprints.teleop import ( + coordinator_cartesian_ik_mock, + coordinator_cartesian_ik_piper, + coordinator_combined_xarm6, + coordinator_teleop_dual, + coordinator_teleop_piper, + coordinator_teleop_xarm6, + coordinator_teleop_xarm7, + coordinator_velocity_xarm6, +) + +__all__ = [ + # Basic (single arm, trajectory) + "coordinator_basic", + "coordinator_mock", + "coordinator_xarm7", + "coordinator_xarm6", + "coordinator_piper", + # Dual arm (trajectory) + "coordinator_dual_mock", + "coordinator_dual_xarm", + "coordinator_piper_xarm", + # Teleop (servo, velocity, cartesian IK, teleop IK) + "coordinator_teleop_xarm6", + "coordinator_velocity_xarm6", + "coordinator_combined_xarm6", + "coordinator_cartesian_ik_mock", + "coordinator_cartesian_ik_piper", + "coordinator_teleop_xarm7", + "coordinator_teleop_piper", + "coordinator_teleop_dual", + # Mobile manipulation + "coordinator_mock_twist_base", + "coordinator_mobile_manip_mock", +] diff --git a/dimos/control/blueprints/basic.py b/dimos/control/blueprints/basic.py new file mode 100644 index 0000000000..eebca32596 --- /dev/null +++ b/dimos/control/blueprints/basic.py @@ -0,0 +1,169 @@ +# 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. + +"""Single-arm coordinator blueprints with trajectory control. + +Usage: + dimos run coordinator-mock # Mock 7-DOF arm + dimos run coordinator-xarm7 # XArm7 real hardware + dimos run coordinator-xarm6 # XArm6 real hardware + dimos run coordinator-piper # Piper arm (CAN bus) +""" + +from __future__ import annotations + +import os + +from dimos.control.components import HardwareComponent, HardwareType, make_joints +from dimos.control.coordinator import TaskConfig, control_coordinator +from dimos.core.transport import LCMTransport +from dimos.msgs.sensor_msgs.JointState import JointState + +_XARM7_IP = os.getenv("XARM7_IP") +_XARM6_IP = os.getenv("XARM6_IP") +_CAN_PORT = os.getenv("CAN_PORT", "can0") + +# Minimal blueprint (no hardware, no tasks) +coordinator_basic = control_coordinator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + } +) + +# Mock 7-DOF arm (for testing) +coordinator_mock = control_coordinator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[ + HardwareComponent( + hardware_id="arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("arm", 7), + adapter_type="mock", + ), + ], + tasks=[ + TaskConfig( + name="traj_arm", + type="trajectory", + joint_names=[f"arm_joint{i + 1}" for i in range(7)], + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + } +) + +# XArm7 real hardware +coordinator_xarm7 = control_coordinator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[ + HardwareComponent( + hardware_id="arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("arm", 7), + adapter_type="xarm", + address=_XARM7_IP, + auto_enable=True, + ), + ], + tasks=[ + TaskConfig( + name="traj_arm", + type="trajectory", + joint_names=[f"arm_joint{i + 1}" for i in range(7)], + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + } +) + +# XArm6 real hardware +coordinator_xarm6 = control_coordinator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[ + HardwareComponent( + hardware_id="arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("arm", 6), + adapter_type="xarm", + address=_XARM6_IP, + auto_enable=True, + ), + ], + tasks=[ + TaskConfig( + name="traj_xarm", + type="trajectory", + joint_names=[f"arm_joint{i + 1}" for i in range(6)], + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + } +) + +# Piper arm (6-DOF, CAN bus) +coordinator_piper = control_coordinator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[ + HardwareComponent( + hardware_id="arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("arm", 6), + adapter_type="piper", + address=_CAN_PORT, + auto_enable=True, + ), + ], + tasks=[ + TaskConfig( + name="traj_piper", + type="trajectory", + joint_names=[f"arm_joint{i + 1}" for i in range(6)], + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + } +) + + +__all__ = [ + "coordinator_basic", + "coordinator_mock", + "coordinator_xarm7", + "coordinator_xarm6", + "coordinator_piper", +] diff --git a/dimos/control/blueprints/dual.py b/dimos/control/blueprints/dual.py new file mode 100644 index 0000000000..4dfaad22fd --- /dev/null +++ b/dimos/control/blueprints/dual.py @@ -0,0 +1,166 @@ +# 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. + +"""Dual-arm coordinator blueprints with trajectory control. + +Usage: + dimos run coordinator-dual-mock # Mock 7+6 DOF arms + dimos run coordinator-dual-xarm # XArm7 left + XArm6 right + dimos run coordinator-piper-xarm # XArm6 + Piper +""" + +from __future__ import annotations + +import os + +from dimos.control.components import HardwareComponent, HardwareType, make_joints +from dimos.control.coordinator import TaskConfig, control_coordinator +from dimos.core.transport import LCMTransport +from dimos.msgs.sensor_msgs.JointState import JointState + +_XARM7_IP = os.getenv("XARM7_IP") +_XARM6_IP = os.getenv("XARM6_IP") +_CAN_PORT = os.getenv("CAN_PORT", "can0") + +# Dual mock arms (7-DOF left, 6-DOF right) +coordinator_dual_mock = control_coordinator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[ + HardwareComponent( + hardware_id="left_arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("left_arm", 7), + adapter_type="mock", + ), + HardwareComponent( + hardware_id="right_arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("right_arm", 6), + adapter_type="mock", + ), + ], + tasks=[ + TaskConfig( + name="traj_left", + type="trajectory", + joint_names=[f"left_arm_joint{i + 1}" for i in range(7)], + priority=10, + ), + TaskConfig( + name="traj_right", + type="trajectory", + joint_names=[f"right_arm_joint{i + 1}" for i in range(6)], + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + } +) + +# Dual XArm (XArm7 left, XArm6 right) +coordinator_dual_xarm = control_coordinator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[ + HardwareComponent( + hardware_id="left_arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("left_arm", 7), + adapter_type="xarm", + address=_XARM7_IP, + auto_enable=True, + ), + HardwareComponent( + hardware_id="right_arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("right_arm", 6), + adapter_type="xarm", + address=_XARM6_IP, + auto_enable=True, + ), + ], + tasks=[ + TaskConfig( + name="traj_left", + type="trajectory", + joint_names=[f"left_arm_joint{i + 1}" for i in range(7)], + priority=10, + ), + TaskConfig( + name="traj_right", + type="trajectory", + joint_names=[f"right_arm_joint{i + 1}" for i in range(6)], + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + } +) + +# Dual arm (XArm6 + Piper) +coordinator_piper_xarm = control_coordinator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[ + HardwareComponent( + hardware_id="xarm_arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("xarm_arm", 6), + adapter_type="xarm", + address=_XARM6_IP, + auto_enable=True, + ), + HardwareComponent( + hardware_id="piper_arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("piper_arm", 6), + adapter_type="piper", + address=_CAN_PORT, + auto_enable=True, + ), + ], + tasks=[ + TaskConfig( + name="traj_xarm", + type="trajectory", + joint_names=[f"xarm_arm_joint{i + 1}" for i in range(6)], + priority=10, + ), + TaskConfig( + name="traj_piper", + type="trajectory", + joint_names=[f"piper_arm_joint{i + 1}" for i in range(6)], + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + } +) + + +__all__ = [ + "coordinator_dual_mock", + "coordinator_dual_xarm", + "coordinator_piper_xarm", +] diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py new file mode 100644 index 0000000000..0a387f8812 --- /dev/null +++ b/dimos/control/blueprints/mobile.py @@ -0,0 +1,99 @@ +# 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. + +"""Mobile manipulation coordinator blueprints. + +Usage: + dimos run coordinator-mock-twist-base # Mock holonomic base + dimos run coordinator-mobile-manip-mock # Mock arm + base +""" + +from __future__ import annotations + +from dimos.control.components import HardwareComponent, HardwareType, make_joints, make_twist_base_joints +from dimos.control.coordinator import TaskConfig, control_coordinator +from dimos.core.transport import LCMTransport +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.sensor_msgs.JointState import JointState + +# Mock holonomic twist base (3-DOF: vx, vy, wz) +_base_joints = make_twist_base_joints("base") +coordinator_mock_twist_base = control_coordinator( + hardware=[ + HardwareComponent( + hardware_id="base", + hardware_type=HardwareType.BASE, + joints=_base_joints, + adapter_type="mock_twist_base", + ), + ], + tasks=[ + TaskConfig( + name="vel_base", + type="velocity", + joint_names=_base_joints, + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("twist_command", Twist): LCMTransport("/cmd_vel", Twist), + } +) + + +# Mock arm (7-DOF) + mock holonomic base (3-DOF) +_mm_base_joints = make_twist_base_joints("base") +coordinator_mobile_manip_mock = control_coordinator( + hardware=[ + HardwareComponent( + hardware_id="arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("arm", 7), + adapter_type="mock", + ), + HardwareComponent( + hardware_id="base", + hardware_type=HardwareType.BASE, + joints=_mm_base_joints, + adapter_type="mock_twist_base", + ), + ], + tasks=[ + TaskConfig( + name="traj_arm", + type="trajectory", + joint_names=[f"arm_joint{i + 1}" for i in range(7)], + priority=10, + ), + TaskConfig( + name="vel_base", + type="velocity", + joint_names=_mm_base_joints, + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("twist_command", Twist): LCMTransport("/cmd_vel", Twist), + } +) + + +__all__ = [ + "coordinator_mock_twist_base", + "coordinator_mobile_manip_mock", +] diff --git a/dimos/control/blueprints.py b/dimos/control/blueprints/teleop.py similarity index 51% rename from dimos/control/blueprints.py rename to dimos/control/blueprints/teleop.py index 9e8bb7377c..8571a6e804 100644 --- a/dimos/control/blueprints.py +++ b/dimos/control/blueprints/teleop.py @@ -12,37 +12,27 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Pre-configured blueprints for the ControlCoordinator. - -This module provides ready-to-use coordinator blueprints for common setups. +"""Advanced control coordinator blueprints: servo, velocity, cartesian IK, and teleop IK. Usage: - # Run via CLI: - dimos run coordinator-mock # Mock 7-DOF arm - dimos run coordinator-xarm7 # XArm7 real hardware - dimos run coordinator-dual-mock # Dual mock arms - - # Or programmatically: - from dimos.control.blueprints import coordinator_mock - coordinator = coordinator_mock.build() - coordinator.loop() + dimos run coordinator-teleop-xarm6 # Servo streaming (XArm6) + dimos run coordinator-velocity-xarm6 # Velocity streaming (XArm6) + dimos run coordinator-combined-xarm6 # Servo + velocity (XArm6) + dimos run coordinator-cartesian-ik-mock # Cartesian IK (mock) + dimos run coordinator-cartesian-ik-piper # Cartesian IK (Piper) + dimos run coordinator-teleop-xarm7 # TeleopIK (XArm7) + dimos run coordinator-teleop-piper # TeleopIK (Piper) + dimos run coordinator-teleop-dual # TeleopIK dual arm """ from __future__ import annotations import os -from dimos.control.components import ( - HardwareComponent, - HardwareType, - make_gripper_joints, - make_joints, - make_twist_base_joints, -) +from dimos.control.components import HardwareComponent, HardwareType, make_gripper_joints, make_joints from dimos.control.coordinator import TaskConfig, control_coordinator from dimos.core.transport import LCMTransport from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.sensor_msgs.JointState import JointState from dimos.teleop.quest.quest_types import Buttons from dimos.utils.data import LfsPath @@ -56,246 +46,7 @@ _CAN_PORT = os.getenv("CAN_PORT", "can0") -# Mock 7-DOF arm (for testing) -coordinator_mock = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 7), - adapter_type="mock", - ), - ], - tasks=[ - TaskConfig( - name="traj_arm", - type="trajectory", - joint_names=[f"arm_joint{i + 1}" for i in range(7)], - priority=10, - ), - ], -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - } -) - -# XArm7 real hardware -coordinator_xarm7 = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 7), - adapter_type="xarm", - address=_XARM7_IP, - auto_enable=True, - ), - ], - tasks=[ - TaskConfig( - name="traj_arm", - type="trajectory", - joint_names=[f"arm_joint{i + 1}" for i in range(7)], - priority=10, - ), - ], -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - } -) - -# XArm6 real hardware -coordinator_xarm6 = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 6), - adapter_type="xarm", - address=_XARM6_IP, - auto_enable=True, - ), - ], - tasks=[ - TaskConfig( - name="traj_xarm", - type="trajectory", - joint_names=[f"arm_joint{i + 1}" for i in range(6)], - priority=10, - ), - ], -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - } -) - -# Piper arm (6-DOF, CAN bus) -coordinator_piper = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 6), - adapter_type="piper", - address=_CAN_PORT, - auto_enable=True, - ), - ], - tasks=[ - TaskConfig( - name="traj_piper", - type="trajectory", - joint_names=[f"arm_joint{i + 1}" for i in range(6)], - priority=10, - ), - ], -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - } -) - - -# Dual mock arms (7-DOF left, 6-DOF right) -coordinator_dual_mock = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="left_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("left_arm", 7), - adapter_type="mock", - ), - HardwareComponent( - hardware_id="right_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("right_arm", 6), - adapter_type="mock", - ), - ], - tasks=[ - TaskConfig( - name="traj_left", - type="trajectory", - joint_names=[f"left_arm_joint{i + 1}" for i in range(7)], - priority=10, - ), - TaskConfig( - name="traj_right", - type="trajectory", - joint_names=[f"right_arm_joint{i + 1}" for i in range(6)], - priority=10, - ), - ], -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - } -) - -# Dual XArm (XArm7 left, XArm6 right) -coordinator_dual_xarm = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="left_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("left_arm", 7), - adapter_type="xarm", - address=_XARM7_IP, - auto_enable=True, - ), - HardwareComponent( - hardware_id="right_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("right_arm", 6), - adapter_type="xarm", - address=_XARM6_IP, - auto_enable=True, - ), - ], - tasks=[ - TaskConfig( - name="traj_left", - type="trajectory", - joint_names=[f"left_arm_joint{i + 1}" for i in range(7)], - priority=10, - ), - TaskConfig( - name="traj_right", - type="trajectory", - joint_names=[f"right_arm_joint{i + 1}" for i in range(6)], - priority=10, - ), - ], -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - } -) - -# Dual arm (XArm6 + Piper) -coordinator_piper_xarm = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="xarm_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("xarm_arm", 6), - adapter_type="xarm", - address=_XARM6_IP, - auto_enable=True, - ), - HardwareComponent( - hardware_id="piper_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("piper_arm", 6), - adapter_type="piper", - address=_CAN_PORT, - auto_enable=True, - ), - ], - tasks=[ - TaskConfig( - name="traj_xarm", - type="trajectory", - joint_names=[f"xarm_arm_joint{i + 1}" for i in range(6)], - priority=10, - ), - TaskConfig( - name="traj_piper", - type="trajectory", - joint_names=[f"piper_arm_joint{i + 1}" for i in range(6)], - priority=10, - ), - ], -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - } -) - +# --- Servo / Velocity streaming (XArm6) --- # XArm6 teleop - streaming position control coordinator_teleop_xarm6 = control_coordinator( @@ -394,6 +145,8 @@ ) +# --- Cartesian IK --- + # Mock 6-DOF arm with CartesianIK coordinator_cartesian_ik_mock = control_coordinator( tick_rate=100.0, @@ -461,6 +214,8 @@ ) +# --- TeleopIK --- + # Single XArm7 with TeleopIK coordinator_teleop_xarm7 = control_coordinator( tick_rate=100.0, @@ -591,108 +346,16 @@ ) -# Mock holonomic twist base (3-DOF: vx, vy, wz) -_base_joints = make_twist_base_joints("base") -coordinator_mock_twist_base = control_coordinator( - hardware=[ - HardwareComponent( - hardware_id="base", - hardware_type=HardwareType.BASE, - joints=_base_joints, - adapter_type="mock_twist_base", - ), - ], - tasks=[ - TaskConfig( - name="vel_base", - type="velocity", - joint_names=_base_joints, - priority=10, - ), - ], -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - ("twist_command", Twist): LCMTransport("/cmd_vel", Twist), - } -) - - -# Mock arm (7-DOF) + mock holonomic base (3-DOF) -_mm_base_joints = make_twist_base_joints("base") -coordinator_mobile_manip_mock = control_coordinator( - hardware=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 7), - adapter_type="mock", - ), - HardwareComponent( - hardware_id="base", - hardware_type=HardwareType.BASE, - joints=_mm_base_joints, - adapter_type="mock_twist_base", - ), - ], - tasks=[ - TaskConfig( - name="traj_arm", - type="trajectory", - joint_names=[f"arm_joint{i + 1}" for i in range(7)], - priority=10, - ), - TaskConfig( - name="vel_base", - type="velocity", - joint_names=_mm_base_joints, - priority=10, - ), - ], -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - ("twist_command", Twist): LCMTransport("/cmd_vel", Twist), - } -) - - -coordinator_basic = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - } -) - - __all__ = [ - # Raw - "coordinator_basic", + # Servo / Velocity + "coordinator_teleop_xarm6", + "coordinator_velocity_xarm6", + "coordinator_combined_xarm6", # Cartesian IK "coordinator_cartesian_ik_mock", "coordinator_cartesian_ik_piper", - # Streaming control - "coordinator_combined_xarm6", - # Dual arm - "coordinator_dual_mock", - "coordinator_dual_xarm", - # Mobile manipulation - "coordinator_mobile_manip_mock", - # Single arm - "coordinator_mock", - # Twist base - "coordinator_mock_twist_base", - "coordinator_piper", - "coordinator_piper_xarm", - # Teleop IK - "coordinator_teleop_dual", - "coordinator_teleop_piper", - "coordinator_teleop_xarm6", + # TeleopIK "coordinator_teleop_xarm7", - "coordinator_velocity_xarm6", - "coordinator_xarm6", - "coordinator_xarm7", + "coordinator_teleop_piper", + "coordinator_teleop_dual", ] From bb9c13dbae20e55f01484074b44a749b0e36cfe2 Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Wed, 18 Mar 2026 12:49:22 -0700 Subject: [PATCH 05/15] refactor: hardware factories --- dimos/control/blueprints/_hardware.py | 100 +++++++++++++++++++ dimos/control/blueprints/basic.py | 50 +--------- dimos/control/blueprints/dual.py | 61 +----------- dimos/control/blueprints/mobile.py | 34 ++----- dimos/control/blueprints/teleop.py | 132 +++++--------------------- 5 files changed, 141 insertions(+), 236 deletions(-) create mode 100644 dimos/control/blueprints/_hardware.py diff --git a/dimos/control/blueprints/_hardware.py b/dimos/control/blueprints/_hardware.py new file mode 100644 index 0000000000..ddbd041aa6 --- /dev/null +++ b/dimos/control/blueprints/_hardware.py @@ -0,0 +1,100 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Hardware component factories for coordinator blueprints.""" + +from __future__ import annotations + +import os + +from dimos.control.components import ( + HardwareComponent, + HardwareType, + make_gripper_joints, + make_joints, + make_twist_base_joints, +) +from dimos.utils.data import LfsPath + +# Environment variables + +XARM7_IP = os.getenv("XARM7_IP") +XARM6_IP = os.getenv("XARM6_IP") +CAN_PORT = os.getenv("CAN_PORT", "can0") + +# Model paths + +PIPER_MODEL_PATH = LfsPath("piper_description/mujoco_model/piper_no_gripper_description.xml") +XARM6_MODEL_PATH = LfsPath("xarm_description/urdf/xarm6/xarm6.urdf") +XARM7_MODEL_PATH = LfsPath("xarm_description/urdf/xarm7/xarm7.urdf") + + +# Hardware component factories + + +def mock_arm(hw_id: str = "arm", n_joints: int = 7) -> HardwareComponent: + """Mock manipulator (no real hardware).""" + return HardwareComponent( + hardware_id=hw_id, + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints(hw_id, n_joints), + adapter_type="mock", + ) + + +def xarm7(hw_id: str = "arm", *, gripper: bool = False) -> HardwareComponent: + """XArm7 real hardware (7-DOF).""" + return HardwareComponent( + hardware_id=hw_id, + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints(hw_id, 7), + adapter_type="xarm", + address=XARM7_IP, + auto_enable=True, + gripper_joints=make_gripper_joints(hw_id) if gripper else None, + ) + + +def xarm6(hw_id: str = "arm") -> HardwareComponent: + """XArm6 real hardware (6-DOF).""" + return HardwareComponent( + hardware_id=hw_id, + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints(hw_id, 6), + adapter_type="xarm", + address=XARM6_IP, + auto_enable=True, + ) + + +def piper(hw_id: str = "arm") -> HardwareComponent: + """Piper arm (6-DOF, CAN bus).""" + return HardwareComponent( + hardware_id=hw_id, + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints(hw_id, 6), + adapter_type="piper", + address=CAN_PORT, + auto_enable=True, + ) + + +def mock_twist_base(hw_id: str = "base") -> HardwareComponent: + """Mock holonomic twist base (3-DOF: vx, vy, wz).""" + return HardwareComponent( + hardware_id=hw_id, + hardware_type=HardwareType.BASE, + joints=make_twist_base_joints(hw_id), + adapter_type="mock_twist_base", + ) diff --git a/dimos/control/blueprints/basic.py b/dimos/control/blueprints/basic.py index eebca32596..e073071596 100644 --- a/dimos/control/blueprints/basic.py +++ b/dimos/control/blueprints/basic.py @@ -23,17 +23,11 @@ from __future__ import annotations -import os - -from dimos.control.components import HardwareComponent, HardwareType, make_joints +from dimos.control.blueprints._hardware import mock_arm, piper, xarm6, xarm7 from dimos.control.coordinator import TaskConfig, control_coordinator from dimos.core.transport import LCMTransport from dimos.msgs.sensor_msgs.JointState import JointState -_XARM7_IP = os.getenv("XARM7_IP") -_XARM6_IP = os.getenv("XARM6_IP") -_CAN_PORT = os.getenv("CAN_PORT", "can0") - # Minimal blueprint (no hardware, no tasks) coordinator_basic = control_coordinator( tick_rate=100.0, @@ -50,14 +44,7 @@ tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 7), - adapter_type="mock", - ), - ], + hardware=[mock_arm()], tasks=[ TaskConfig( name="traj_arm", @@ -77,16 +64,7 @@ tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 7), - adapter_type="xarm", - address=_XARM7_IP, - auto_enable=True, - ), - ], + hardware=[xarm7()], tasks=[ TaskConfig( name="traj_arm", @@ -106,16 +84,7 @@ tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 6), - adapter_type="xarm", - address=_XARM6_IP, - auto_enable=True, - ), - ], + hardware=[xarm6()], tasks=[ TaskConfig( name="traj_xarm", @@ -135,16 +104,7 @@ tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 6), - adapter_type="piper", - address=_CAN_PORT, - auto_enable=True, - ), - ], + hardware=[piper()], tasks=[ TaskConfig( name="traj_piper", diff --git a/dimos/control/blueprints/dual.py b/dimos/control/blueprints/dual.py index 4dfaad22fd..1ec44de7cc 100644 --- a/dimos/control/blueprints/dual.py +++ b/dimos/control/blueprints/dual.py @@ -22,36 +22,17 @@ from __future__ import annotations -import os - -from dimos.control.components import HardwareComponent, HardwareType, make_joints +from dimos.control.blueprints._hardware import mock_arm, piper, xarm6, xarm7 from dimos.control.coordinator import TaskConfig, control_coordinator from dimos.core.transport import LCMTransport from dimos.msgs.sensor_msgs.JointState import JointState -_XARM7_IP = os.getenv("XARM7_IP") -_XARM6_IP = os.getenv("XARM6_IP") -_CAN_PORT = os.getenv("CAN_PORT", "can0") - # Dual mock arms (7-DOF left, 6-DOF right) coordinator_dual_mock = control_coordinator( tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="left_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("left_arm", 7), - adapter_type="mock", - ), - HardwareComponent( - hardware_id="right_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("right_arm", 6), - adapter_type="mock", - ), - ], + hardware=[mock_arm("left_arm", 7), mock_arm("right_arm", 6)], tasks=[ TaskConfig( name="traj_left", @@ -77,24 +58,7 @@ tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="left_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("left_arm", 7), - adapter_type="xarm", - address=_XARM7_IP, - auto_enable=True, - ), - HardwareComponent( - hardware_id="right_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("right_arm", 6), - adapter_type="xarm", - address=_XARM6_IP, - auto_enable=True, - ), - ], + hardware=[xarm7("left_arm"), xarm6("right_arm")], tasks=[ TaskConfig( name="traj_left", @@ -120,24 +84,7 @@ tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="xarm_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("xarm_arm", 6), - adapter_type="xarm", - address=_XARM6_IP, - auto_enable=True, - ), - HardwareComponent( - hardware_id="piper_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("piper_arm", 6), - adapter_type="piper", - address=_CAN_PORT, - auto_enable=True, - ), - ], + hardware=[xarm6("xarm_arm"), piper("piper_arm")], tasks=[ TaskConfig( name="traj_xarm", diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index 0a387f8812..ce0426b24c 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -21,23 +21,18 @@ from __future__ import annotations -from dimos.control.components import HardwareComponent, HardwareType, make_joints, make_twist_base_joints +from dimos.control.blueprints._hardware import mock_arm, mock_twist_base +from dimos.control.components import make_twist_base_joints from dimos.control.coordinator import TaskConfig, control_coordinator from dimos.core.transport import LCMTransport from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.sensor_msgs.JointState import JointState -# Mock holonomic twist base (3-DOF: vx, vy, wz) _base_joints = make_twist_base_joints("base") + +# Mock holonomic twist base (3-DOF: vx, vy, wz) coordinator_mock_twist_base = control_coordinator( - hardware=[ - HardwareComponent( - hardware_id="base", - hardware_type=HardwareType.BASE, - joints=_base_joints, - adapter_type="mock_twist_base", - ), - ], + hardware=[mock_twist_base()], tasks=[ TaskConfig( name="vel_base", @@ -53,24 +48,9 @@ } ) - # Mock arm (7-DOF) + mock holonomic base (3-DOF) -_mm_base_joints = make_twist_base_joints("base") coordinator_mobile_manip_mock = control_coordinator( - hardware=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 7), - adapter_type="mock", - ), - HardwareComponent( - hardware_id="base", - hardware_type=HardwareType.BASE, - joints=_mm_base_joints, - adapter_type="mock_twist_base", - ), - ], + hardware=[mock_arm(), mock_twist_base()], tasks=[ TaskConfig( name="traj_arm", @@ -81,7 +61,7 @@ TaskConfig( name="vel_base", type="velocity", - joint_names=_mm_base_joints, + joint_names=_base_joints, priority=10, ), ], diff --git a/dimos/control/blueprints/teleop.py b/dimos/control/blueprints/teleop.py index 8571a6e804..26ccf02c58 100644 --- a/dimos/control/blueprints/teleop.py +++ b/dimos/control/blueprints/teleop.py @@ -27,24 +27,21 @@ from __future__ import annotations -import os - -from dimos.control.components import HardwareComponent, HardwareType, make_gripper_joints, make_joints +from dimos.control.blueprints._hardware import ( + PIPER_MODEL_PATH, + XARM6_MODEL_PATH, + XARM7_MODEL_PATH, + mock_arm, + piper, + xarm6, + xarm7, +) +from dimos.control.components import make_gripper_joints from dimos.control.coordinator import TaskConfig, control_coordinator from dimos.core.transport import LCMTransport from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState from dimos.teleop.quest.quest_types import Buttons -from dimos.utils.data import LfsPath - -_PIPER_MODEL_PATH = LfsPath("piper_description/mujoco_model/piper_no_gripper_description.xml") -_XARM6_MODEL_PATH = LfsPath("xarm_description/urdf/xarm6/xarm6.urdf") -_XARM7_MODEL_PATH = LfsPath("xarm_description/urdf/xarm7/xarm7.urdf") - -_XARM7_IP = os.getenv("XARM7_IP") -_XARM6_IP = os.getenv("XARM6_IP") -_CAN_PORT = os.getenv("CAN_PORT", "can0") - # --- Servo / Velocity streaming (XArm6) --- @@ -53,16 +50,7 @@ tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 6), - adapter_type="xarm", - address=_XARM6_IP, - auto_enable=True, - ), - ], + hardware=[xarm6()], tasks=[ TaskConfig( name="servo_arm", @@ -83,16 +71,7 @@ tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 6), - adapter_type="xarm", - address=_XARM6_IP, - auto_enable=True, - ), - ], + hardware=[xarm6()], tasks=[ TaskConfig( name="velocity_arm", @@ -113,16 +92,7 @@ tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 6), - adapter_type="xarm", - address=_XARM6_IP, - auto_enable=True, - ), - ], + hardware=[xarm6()], tasks=[ TaskConfig( name="servo_arm", @@ -152,21 +122,14 @@ tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 6), - adapter_type="mock", - ), - ], + hardware=[mock_arm("arm", 6)], tasks=[ TaskConfig( name="cartesian_ik_arm", type="cartesian_ik", joint_names=[f"arm_joint{i + 1}" for i in range(6)], priority=10, - model_path=_PIPER_MODEL_PATH, + model_path=PIPER_MODEL_PATH, ee_joint_id=6, ), ], @@ -184,23 +147,14 @@ tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 6), - adapter_type="piper", - address=_CAN_PORT, - auto_enable=True, - ), - ], + hardware=[piper()], tasks=[ TaskConfig( name="cartesian_ik_arm", type="cartesian_ik", joint_names=[f"arm_joint{i + 1}" for i in range(6)], priority=10, - model_path=_PIPER_MODEL_PATH, + model_path=PIPER_MODEL_PATH, ee_joint_id=6, ), ], @@ -221,28 +175,18 @@ tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 7), - adapter_type="xarm", - address=_XARM7_IP, - auto_enable=True, - gripper_joints=make_gripper_joints("arm"), - ), - ], + hardware=[xarm7(gripper=True)], tasks=[ TaskConfig( name="teleop_xarm", type="teleop_ik", joint_names=[f"arm_joint{i + 1}" for i in range(7)], priority=10, - model_path=_XARM7_MODEL_PATH, + model_path=XARM7_MODEL_PATH, ee_joint_id=7, hand="right", gripper_joint=make_gripper_joints("arm")[0], - gripper_open_pos=0.85, # xArm gripper range + gripper_open_pos=0.85, gripper_closed_pos=0.0, ), ], @@ -261,23 +205,14 @@ tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 6), - adapter_type="piper", - address=_CAN_PORT, - auto_enable=True, - ), - ], + hardware=[piper()], tasks=[ TaskConfig( name="teleop_piper", type="teleop_ik", joint_names=[f"arm_joint{i + 1}" for i in range(6)], priority=10, - model_path=_PIPER_MODEL_PATH, + model_path=PIPER_MODEL_PATH, ee_joint_id=6, hand="left", ), @@ -297,31 +232,14 @@ tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[ - HardwareComponent( - hardware_id="xarm_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("xarm_arm", 6), - adapter_type="xarm", - address=_XARM6_IP, - auto_enable=True, - ), - HardwareComponent( - hardware_id="piper_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("piper_arm", 6), - adapter_type="piper", - address=_CAN_PORT, - auto_enable=True, - ), - ], + hardware=[xarm6("xarm_arm"), piper("piper_arm")], tasks=[ TaskConfig( name="teleop_xarm", type="teleop_ik", joint_names=[f"xarm_arm_joint{i + 1}" for i in range(6)], priority=10, - model_path=_XARM6_MODEL_PATH, + model_path=XARM6_MODEL_PATH, ee_joint_id=6, hand="left", ), @@ -330,7 +248,7 @@ type="teleop_ik", joint_names=[f"piper_arm_joint{i + 1}" for i in range(6)], priority=10, - model_path=_PIPER_MODEL_PATH, + model_path=PIPER_MODEL_PATH, ee_joint_id=6, hand="right", ), From dca40dfd9d07018cf7bbe0ce6861f6eadfb0c643 Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Wed, 18 Mar 2026 12:51:05 -0700 Subject: [PATCH 06/15] fix: pre-commit checks --- dimos/control/blueprints/__init__.py | 26 +++++++++++++------------- dimos/control/blueprints/basic.py | 4 ++-- dimos/control/blueprints/mobile.py | 2 +- dimos/control/blueprints/teleop.py | 12 ++++++------ 4 files changed, 22 insertions(+), 22 deletions(-) diff --git a/dimos/control/blueprints/__init__.py b/dimos/control/blueprints/__init__.py index ca7ba7e417..defffb78e3 100644 --- a/dimos/control/blueprints/__init__.py +++ b/dimos/control/blueprints/__init__.py @@ -60,24 +60,24 @@ __all__ = [ # Basic (single arm, trajectory) "coordinator_basic", - "coordinator_mock", - "coordinator_xarm7", - "coordinator_xarm6", - "coordinator_piper", + "coordinator_cartesian_ik_mock", + "coordinator_cartesian_ik_piper", + "coordinator_combined_xarm6", # Dual arm (trajectory) "coordinator_dual_mock", "coordinator_dual_xarm", + "coordinator_mobile_manip_mock", + "coordinator_mock", + # Mobile manipulation + "coordinator_mock_twist_base", + "coordinator_piper", "coordinator_piper_xarm", + "coordinator_teleop_dual", + "coordinator_teleop_piper", # Teleop (servo, velocity, cartesian IK, teleop IK) "coordinator_teleop_xarm6", - "coordinator_velocity_xarm6", - "coordinator_combined_xarm6", - "coordinator_cartesian_ik_mock", - "coordinator_cartesian_ik_piper", "coordinator_teleop_xarm7", - "coordinator_teleop_piper", - "coordinator_teleop_dual", - # Mobile manipulation - "coordinator_mock_twist_base", - "coordinator_mobile_manip_mock", + "coordinator_velocity_xarm6", + "coordinator_xarm6", + "coordinator_xarm7", ] diff --git a/dimos/control/blueprints/basic.py b/dimos/control/blueprints/basic.py index e073071596..d49ba01019 100644 --- a/dimos/control/blueprints/basic.py +++ b/dimos/control/blueprints/basic.py @@ -123,7 +123,7 @@ __all__ = [ "coordinator_basic", "coordinator_mock", - "coordinator_xarm7", - "coordinator_xarm6", "coordinator_piper", + "coordinator_xarm6", + "coordinator_xarm7", ] diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index ce0426b24c..4ed3410b8f 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -74,6 +74,6 @@ __all__ = [ - "coordinator_mock_twist_base", "coordinator_mobile_manip_mock", + "coordinator_mock_twist_base", ] diff --git a/dimos/control/blueprints/teleop.py b/dimos/control/blueprints/teleop.py index 26ccf02c58..2aee5c90e6 100644 --- a/dimos/control/blueprints/teleop.py +++ b/dimos/control/blueprints/teleop.py @@ -265,15 +265,15 @@ __all__ = [ - # Servo / Velocity - "coordinator_teleop_xarm6", - "coordinator_velocity_xarm6", - "coordinator_combined_xarm6", # Cartesian IK "coordinator_cartesian_ik_mock", "coordinator_cartesian_ik_piper", + "coordinator_combined_xarm6", + "coordinator_teleop_dual", + "coordinator_teleop_piper", + # Servo / Velocity + "coordinator_teleop_xarm6", # TeleopIK "coordinator_teleop_xarm7", - "coordinator_teleop_piper", - "coordinator_teleop_dual", + "coordinator_velocity_xarm6", ] From 2f33b357e279516a71220edbb1a263ad0a672a3a Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Wed, 18 Mar 2026 13:17:11 -0700 Subject: [PATCH 07/15] fix: gripper check + comments --- dimos/control/blueprints/__init__.py | 4 ---- dimos/control/blueprints/_hardware.py | 13 +++++++++---- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/dimos/control/blueprints/__init__.py b/dimos/control/blueprints/__init__.py index defffb78e3..8aa5b34670 100644 --- a/dimos/control/blueprints/__init__.py +++ b/dimos/control/blueprints/__init__.py @@ -58,23 +58,19 @@ ) __all__ = [ - # Basic (single arm, trajectory) "coordinator_basic", "coordinator_cartesian_ik_mock", "coordinator_cartesian_ik_piper", "coordinator_combined_xarm6", - # Dual arm (trajectory) "coordinator_dual_mock", "coordinator_dual_xarm", "coordinator_mobile_manip_mock", "coordinator_mock", - # Mobile manipulation "coordinator_mock_twist_base", "coordinator_piper", "coordinator_piper_xarm", "coordinator_teleop_dual", "coordinator_teleop_piper", - # Teleop (servo, velocity, cartesian IK, teleop IK) "coordinator_teleop_xarm6", "coordinator_teleop_xarm7", "coordinator_velocity_xarm6", diff --git a/dimos/control/blueprints/_hardware.py b/dimos/control/blueprints/_hardware.py index ddbd041aa6..405809ebac 100644 --- a/dimos/control/blueprints/_hardware.py +++ b/dimos/control/blueprints/_hardware.py @@ -55,20 +55,22 @@ def mock_arm(hw_id: str = "arm", n_joints: int = 7) -> HardwareComponent: def xarm7(hw_id: str = "arm", *, gripper: bool = False) -> HardwareComponent: """XArm7 real hardware (7-DOF).""" - return HardwareComponent( + kwargs: dict = dict( hardware_id=hw_id, hardware_type=HardwareType.MANIPULATOR, joints=make_joints(hw_id, 7), adapter_type="xarm", address=XARM7_IP, auto_enable=True, - gripper_joints=make_gripper_joints(hw_id) if gripper else None, ) + if gripper: + kwargs["gripper_joints"] = make_gripper_joints(hw_id) + return HardwareComponent(**kwargs) -def xarm6(hw_id: str = "arm") -> HardwareComponent: +def xarm6(hw_id: str = "arm", *, gripper: bool = False) -> HardwareComponent: """XArm6 real hardware (6-DOF).""" - return HardwareComponent( + kwargs: dict = dict( hardware_id=hw_id, hardware_type=HardwareType.MANIPULATOR, joints=make_joints(hw_id, 6), @@ -76,6 +78,9 @@ def xarm6(hw_id: str = "arm") -> HardwareComponent: address=XARM6_IP, auto_enable=True, ) + if gripper: + kwargs["gripper_joints"] = make_gripper_joints(hw_id) + return HardwareComponent(**kwargs) def piper(hw_id: str = "arm") -> HardwareComponent: From f0ebc3564cc648e8ff044c14651beeaf1ad6febf Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Wed, 18 Mar 2026 13:29:37 -0700 Subject: [PATCH 08/15] fix: gripper addition --- dimos/control/blueprints/_hardware.py | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/dimos/control/blueprints/_hardware.py b/dimos/control/blueprints/_hardware.py index 405809ebac..134aba48cd 100644 --- a/dimos/control/blueprints/_hardware.py +++ b/dimos/control/blueprints/_hardware.py @@ -55,32 +55,28 @@ def mock_arm(hw_id: str = "arm", n_joints: int = 7) -> HardwareComponent: def xarm7(hw_id: str = "arm", *, gripper: bool = False) -> HardwareComponent: """XArm7 real hardware (7-DOF).""" - kwargs: dict = dict( + return HardwareComponent( hardware_id=hw_id, hardware_type=HardwareType.MANIPULATOR, joints=make_joints(hw_id, 7), adapter_type="xarm", address=XARM7_IP, auto_enable=True, + gripper_joints=make_gripper_joints(hw_id) if gripper else [], ) - if gripper: - kwargs["gripper_joints"] = make_gripper_joints(hw_id) - return HardwareComponent(**kwargs) def xarm6(hw_id: str = "arm", *, gripper: bool = False) -> HardwareComponent: """XArm6 real hardware (6-DOF).""" - kwargs: dict = dict( + return HardwareComponent( hardware_id=hw_id, hardware_type=HardwareType.MANIPULATOR, joints=make_joints(hw_id, 6), adapter_type="xarm", address=XARM6_IP, auto_enable=True, + gripper_joints=make_gripper_joints(hw_id) if gripper else [], ) - if gripper: - kwargs["gripper_joints"] = make_gripper_joints(hw_id) - return HardwareComponent(**kwargs) def piper(hw_id: str = "arm") -> HardwareComponent: From 6b299fb8b4182f4ad771b152e050e6533086273b Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Wed, 18 Mar 2026 14:53:53 -0700 Subject: [PATCH 09/15] fix: no init needed, blueprint path --- dimos/control/blueprints/__init__.py | 79 ------------------- dimos/control/blueprints/teleop.py | 6 -- .../examples/twist_base_keyboard_teleop.py | 2 +- dimos/robot/all_blueprints.py | 36 ++++----- dimos/teleop/quest/blueprints.py | 5 +- 5 files changed, 23 insertions(+), 105 deletions(-) delete mode 100644 dimos/control/blueprints/__init__.py diff --git a/dimos/control/blueprints/__init__.py b/dimos/control/blueprints/__init__.py deleted file mode 100644 index 8aa5b34670..0000000000 --- a/dimos/control/blueprints/__init__.py +++ /dev/null @@ -1,79 +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-configured blueprints for the ControlCoordinator. - -This package provides ready-to-use coordinator blueprints for common setups. - -Usage: - # Run via CLI: - dimos run coordinator-mock # Mock 7-DOF arm - dimos run coordinator-xarm7 # XArm7 real hardware - dimos run coordinator-dual-mock # Dual mock arms - - # Or programmatically: - from dimos.control.blueprints import coordinator_mock - coordinator = coordinator_mock.build() - coordinator.loop() -""" - -from __future__ import annotations - -from dimos.control.blueprints.basic import ( - coordinator_basic, - coordinator_mock, - coordinator_piper, - coordinator_xarm6, - coordinator_xarm7, -) -from dimos.control.blueprints.dual import ( - coordinator_dual_mock, - coordinator_dual_xarm, - coordinator_piper_xarm, -) -from dimos.control.blueprints.mobile import ( - coordinator_mobile_manip_mock, - coordinator_mock_twist_base, -) -from dimos.control.blueprints.teleop import ( - coordinator_cartesian_ik_mock, - coordinator_cartesian_ik_piper, - coordinator_combined_xarm6, - coordinator_teleop_dual, - coordinator_teleop_piper, - coordinator_teleop_xarm6, - coordinator_teleop_xarm7, - coordinator_velocity_xarm6, -) - -__all__ = [ - "coordinator_basic", - "coordinator_cartesian_ik_mock", - "coordinator_cartesian_ik_piper", - "coordinator_combined_xarm6", - "coordinator_dual_mock", - "coordinator_dual_xarm", - "coordinator_mobile_manip_mock", - "coordinator_mock", - "coordinator_mock_twist_base", - "coordinator_piper", - "coordinator_piper_xarm", - "coordinator_teleop_dual", - "coordinator_teleop_piper", - "coordinator_teleop_xarm6", - "coordinator_teleop_xarm7", - "coordinator_velocity_xarm6", - "coordinator_xarm6", - "coordinator_xarm7", -] diff --git a/dimos/control/blueprints/teleop.py b/dimos/control/blueprints/teleop.py index 2aee5c90e6..70bc3dc837 100644 --- a/dimos/control/blueprints/teleop.py +++ b/dimos/control/blueprints/teleop.py @@ -43,8 +43,6 @@ from dimos.msgs.sensor_msgs.JointState import JointState from dimos.teleop.quest.quest_types import Buttons -# --- Servo / Velocity streaming (XArm6) --- - # XArm6 teleop - streaming position control coordinator_teleop_xarm6 = control_coordinator( tick_rate=100.0, @@ -115,8 +113,6 @@ ) -# --- Cartesian IK --- - # Mock 6-DOF arm with CartesianIK coordinator_cartesian_ik_mock = control_coordinator( tick_rate=100.0, @@ -168,8 +164,6 @@ ) -# --- TeleopIK --- - # Single XArm7 with TeleopIK coordinator_teleop_xarm7 = control_coordinator( tick_rate=100.0, diff --git a/dimos/control/examples/twist_base_keyboard_teleop.py b/dimos/control/examples/twist_base_keyboard_teleop.py index 2d7651145a..610f8679e4 100644 --- a/dimos/control/examples/twist_base_keyboard_teleop.py +++ b/dimos/control/examples/twist_base_keyboard_teleop.py @@ -33,7 +33,7 @@ from __future__ import annotations -from dimos.control.blueprints import coordinator_mock_twist_base +from dimos.control.blueprints.mobile import coordinator_mock_twist_base from dimos.robot.unitree.keyboard_teleop import keyboard_teleop diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index e82cb656ce..8ab85d5294 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -21,24 +21,24 @@ "arm-teleop-piper": "dimos.teleop.quest.blueprints:arm_teleop_piper", "arm-teleop-visualizing": "dimos.teleop.quest.blueprints:arm_teleop_visualizing", "arm-teleop-xarm7": "dimos.teleop.quest.blueprints:arm_teleop_xarm7", - "coordinator-basic": "dimos.control.blueprints:coordinator_basic", - "coordinator-cartesian-ik-mock": "dimos.control.blueprints:coordinator_cartesian_ik_mock", - "coordinator-cartesian-ik-piper": "dimos.control.blueprints:coordinator_cartesian_ik_piper", - "coordinator-combined-xarm6": "dimos.control.blueprints:coordinator_combined_xarm6", - "coordinator-dual-mock": "dimos.control.blueprints:coordinator_dual_mock", - "coordinator-dual-xarm": "dimos.control.blueprints:coordinator_dual_xarm", - "coordinator-mobile-manip-mock": "dimos.control.blueprints:coordinator_mobile_manip_mock", - "coordinator-mock": "dimos.control.blueprints:coordinator_mock", - "coordinator-mock-twist-base": "dimos.control.blueprints:coordinator_mock_twist_base", - "coordinator-piper": "dimos.control.blueprints:coordinator_piper", - "coordinator-piper-xarm": "dimos.control.blueprints:coordinator_piper_xarm", - "coordinator-teleop-dual": "dimos.control.blueprints:coordinator_teleop_dual", - "coordinator-teleop-piper": "dimos.control.blueprints:coordinator_teleop_piper", - "coordinator-teleop-xarm6": "dimos.control.blueprints:coordinator_teleop_xarm6", - "coordinator-teleop-xarm7": "dimos.control.blueprints:coordinator_teleop_xarm7", - "coordinator-velocity-xarm6": "dimos.control.blueprints:coordinator_velocity_xarm6", - "coordinator-xarm6": "dimos.control.blueprints:coordinator_xarm6", - "coordinator-xarm7": "dimos.control.blueprints:coordinator_xarm7", + "coordinator-basic": "dimos.control.blueprints.basic:coordinator_basic", + "coordinator-cartesian-ik-mock": "dimos.control.blueprints.teleop:coordinator_cartesian_ik_mock", + "coordinator-cartesian-ik-piper": "dimos.control.blueprints.teleop:coordinator_cartesian_ik_piper", + "coordinator-combined-xarm6": "dimos.control.blueprints.teleop:coordinator_combined_xarm6", + "coordinator-dual-mock": "dimos.control.blueprints.dual:coordinator_dual_mock", + "coordinator-dual-xarm": "dimos.control.blueprints.dual:coordinator_dual_xarm", + "coordinator-mobile-manip-mock": "dimos.control.blueprints.mobile:coordinator_mobile_manip_mock", + "coordinator-mock": "dimos.control.blueprints.basic:coordinator_mock", + "coordinator-mock-twist-base": "dimos.control.blueprints.mobile:coordinator_mock_twist_base", + "coordinator-piper": "dimos.control.blueprints.basic:coordinator_piper", + "coordinator-piper-xarm": "dimos.control.blueprints.dual:coordinator_piper_xarm", + "coordinator-teleop-dual": "dimos.control.blueprints.teleop:coordinator_teleop_dual", + "coordinator-teleop-piper": "dimos.control.blueprints.teleop:coordinator_teleop_piper", + "coordinator-teleop-xarm6": "dimos.control.blueprints.teleop:coordinator_teleop_xarm6", + "coordinator-teleop-xarm7": "dimos.control.blueprints.teleop:coordinator_teleop_xarm7", + "coordinator-velocity-xarm6": "dimos.control.blueprints.teleop:coordinator_velocity_xarm6", + "coordinator-xarm6": "dimos.control.blueprints.basic:coordinator_xarm6", + "coordinator-xarm7": "dimos.control.blueprints.basic:coordinator_xarm7", "demo-agent": "dimos.agents.demo_agent:demo_agent", "demo-agent-camera": "dimos.agents.demo_agent:demo_agent_camera", "demo-camera": "dimos.hardware.sensors.camera.module:demo_camera", diff --git a/dimos/teleop/quest/blueprints.py b/dimos/teleop/quest/blueprints.py index a3aa54ee08..228676ec4f 100644 --- a/dimos/teleop/quest/blueprints.py +++ b/dimos/teleop/quest/blueprints.py @@ -15,7 +15,7 @@ """Teleop blueprints for testing and deployment.""" -from dimos.control.blueprints import ( +from dimos.control.blueprints.teleop import ( coordinator_teleop_dual, coordinator_teleop_piper, coordinator_teleop_xarm7, @@ -25,10 +25,13 @@ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.teleop.quest.quest_extensions import arm_teleop_module, visualizing_teleop_module from dimos.teleop.quest.quest_types import Buttons +from dimos.visualization.rerun.bridge import rerun_bridge + # Arm teleop with press-and-hold engage arm_teleop = autoconnect( arm_teleop_module(), + rerun_bridge(), ).transports( { ("left_controller_output", PoseStamped): LCMTransport("/teleop/left_delta", PoseStamped), From 39581d37578032dea88d041500c26fd660238dc0 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam <63036454+ruthwikdasyam@users.noreply.github.com> Date: Wed, 18 Mar 2026 21:55:15 +0000 Subject: [PATCH 10/15] CI code cleanup --- dimos/teleop/quest/blueprints.py | 1 - 1 file changed, 1 deletion(-) diff --git a/dimos/teleop/quest/blueprints.py b/dimos/teleop/quest/blueprints.py index 228676ec4f..b835716527 100644 --- a/dimos/teleop/quest/blueprints.py +++ b/dimos/teleop/quest/blueprints.py @@ -27,7 +27,6 @@ from dimos.teleop.quest.quest_types import Buttons from dimos.visualization.rerun.bridge import rerun_bridge - # Arm teleop with press-and-hold engage arm_teleop = autoconnect( arm_teleop_module(), From 19d569401adaca0692ba5a34e0a988d171f09068 Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Wed, 18 Mar 2026 15:03:38 -0700 Subject: [PATCH 11/15] check trigger commit --- dimos/control/blueprints/_hardware.py | 7 ------- 1 file changed, 7 deletions(-) diff --git a/dimos/control/blueprints/_hardware.py b/dimos/control/blueprints/_hardware.py index 134aba48cd..2a243f360b 100644 --- a/dimos/control/blueprints/_hardware.py +++ b/dimos/control/blueprints/_hardware.py @@ -27,22 +27,15 @@ ) from dimos.utils.data import LfsPath -# Environment variables - XARM7_IP = os.getenv("XARM7_IP") XARM6_IP = os.getenv("XARM6_IP") CAN_PORT = os.getenv("CAN_PORT", "can0") -# Model paths - PIPER_MODEL_PATH = LfsPath("piper_description/mujoco_model/piper_no_gripper_description.xml") XARM6_MODEL_PATH = LfsPath("xarm_description/urdf/xarm6/xarm6.urdf") XARM7_MODEL_PATH = LfsPath("xarm_description/urdf/xarm7/xarm7.urdf") -# Hardware component factories - - def mock_arm(hw_id: str = "arm", n_joints: int = 7) -> HardwareComponent: """Mock manipulator (no real hardware).""" return HardwareComponent( From 7fe28891832d0cccffa9fca7622e9c40758a4396 Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Wed, 18 Mar 2026 16:41:45 -0700 Subject: [PATCH 12/15] fix: unwanted changes --- dimos/teleop/quest/blueprints.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/dimos/teleop/quest/blueprints.py b/dimos/teleop/quest/blueprints.py index b835716527..a3aa54ee08 100644 --- a/dimos/teleop/quest/blueprints.py +++ b/dimos/teleop/quest/blueprints.py @@ -15,7 +15,7 @@ """Teleop blueprints for testing and deployment.""" -from dimos.control.blueprints.teleop import ( +from dimos.control.blueprints import ( coordinator_teleop_dual, coordinator_teleop_piper, coordinator_teleop_xarm7, @@ -25,12 +25,10 @@ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.teleop.quest.quest_extensions import arm_teleop_module, visualizing_teleop_module from dimos.teleop.quest.quest_types import Buttons -from dimos.visualization.rerun.bridge import rerun_bridge # Arm teleop with press-and-hold engage arm_teleop = autoconnect( arm_teleop_module(), - rerun_bridge(), ).transports( { ("left_controller_output", PoseStamped): LCMTransport("/teleop/left_delta", PoseStamped), From d6ab60eaa53b6a36c53cd2a73b7ff47f58517213 Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Wed, 18 Mar 2026 17:27:32 -0700 Subject: [PATCH 13/15] fix: blueprint path --- dimos/teleop/quest/blueprints.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/teleop/quest/blueprints.py b/dimos/teleop/quest/blueprints.py index a3aa54ee08..caaaf77fd3 100644 --- a/dimos/teleop/quest/blueprints.py +++ b/dimos/teleop/quest/blueprints.py @@ -15,7 +15,7 @@ """Teleop blueprints for testing and deployment.""" -from dimos.control.blueprints import ( +from dimos.control.blueprints.teleop import ( coordinator_teleop_dual, coordinator_teleop_piper, coordinator_teleop_xarm7, From 0453d06cb790bb2e468319bbf73016a25a3b149a Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 19 Mar 2026 00:10:55 -0700 Subject: [PATCH 14/15] fix: remove duplicates --- dimos/control/blueprints/basic.py | 12 ------------ dimos/control/blueprints/dual.py | 9 --------- dimos/control/blueprints/teleop.py | 24 ------------------------ 3 files changed, 45 deletions(-) diff --git a/dimos/control/blueprints/basic.py b/dimos/control/blueprints/basic.py index d49ba01019..7ad441ed70 100644 --- a/dimos/control/blueprints/basic.py +++ b/dimos/control/blueprints/basic.py @@ -41,9 +41,6 @@ # Mock 7-DOF arm (for testing) coordinator_mock = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", hardware=[mock_arm()], tasks=[ TaskConfig( @@ -61,9 +58,6 @@ # XArm7 real hardware coordinator_xarm7 = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", hardware=[xarm7()], tasks=[ TaskConfig( @@ -81,9 +75,6 @@ # XArm6 real hardware coordinator_xarm6 = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", hardware=[xarm6()], tasks=[ TaskConfig( @@ -101,9 +92,6 @@ # Piper arm (6-DOF, CAN bus) coordinator_piper = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", hardware=[piper()], tasks=[ TaskConfig( diff --git a/dimos/control/blueprints/dual.py b/dimos/control/blueprints/dual.py index 1ec44de7cc..8482316ba5 100644 --- a/dimos/control/blueprints/dual.py +++ b/dimos/control/blueprints/dual.py @@ -29,9 +29,6 @@ # Dual mock arms (7-DOF left, 6-DOF right) coordinator_dual_mock = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", hardware=[mock_arm("left_arm", 7), mock_arm("right_arm", 6)], tasks=[ TaskConfig( @@ -55,9 +52,6 @@ # Dual XArm (XArm7 left, XArm6 right) coordinator_dual_xarm = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", hardware=[xarm7("left_arm"), xarm6("right_arm")], tasks=[ TaskConfig( @@ -81,9 +75,6 @@ # Dual arm (XArm6 + Piper) coordinator_piper_xarm = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", hardware=[xarm6("xarm_arm"), piper("piper_arm")], tasks=[ TaskConfig( diff --git a/dimos/control/blueprints/teleop.py b/dimos/control/blueprints/teleop.py index 70bc3dc837..2e922bbcbf 100644 --- a/dimos/control/blueprints/teleop.py +++ b/dimos/control/blueprints/teleop.py @@ -45,9 +45,6 @@ # XArm6 teleop - streaming position control coordinator_teleop_xarm6 = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", hardware=[xarm6()], tasks=[ TaskConfig( @@ -66,9 +63,6 @@ # XArm6 velocity control - streaming velocity for joystick coordinator_velocity_xarm6 = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", hardware=[xarm6()], tasks=[ TaskConfig( @@ -87,9 +81,6 @@ # XArm6 combined (servo + velocity tasks) coordinator_combined_xarm6 = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", hardware=[xarm6()], tasks=[ TaskConfig( @@ -115,9 +106,6 @@ # Mock 6-DOF arm with CartesianIK coordinator_cartesian_ik_mock = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", hardware=[mock_arm("arm", 6)], tasks=[ TaskConfig( @@ -140,9 +128,6 @@ # Piper arm with CartesianIK coordinator_cartesian_ik_piper = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", hardware=[piper()], tasks=[ TaskConfig( @@ -166,9 +151,6 @@ # Single XArm7 with TeleopIK coordinator_teleop_xarm7 = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", hardware=[xarm7(gripper=True)], tasks=[ TaskConfig( @@ -196,9 +178,6 @@ # Single Piper with TeleopIK coordinator_teleop_piper = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", hardware=[piper()], tasks=[ TaskConfig( @@ -223,9 +202,6 @@ # Dual arm teleop: XArm6 + Piper with TeleopIK coordinator_teleop_dual = control_coordinator( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", hardware=[xarm6("xarm_arm"), piper("piper_arm")], tasks=[ TaskConfig( From 2d8ec35bd6b15347ab059a6ac594ba5d3f7d35e8 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 19 Mar 2026 00:21:48 -0700 Subject: [PATCH 15/15] feat: env var from globalconfig --- default.env | 3 --- dimos/control/blueprints/_hardware.py | 9 ++++----- dimos/core/global_config.py | 3 +++ 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/default.env b/default.env index d4d6a7d6fd..5098a60892 100644 --- a/default.env +++ b/default.env @@ -5,9 +5,6 @@ ANTHROPIC_API_KEY= HF_TOKEN= HUGGINGFACE_PRV_ENDPOINT= ROBOT_IP= -XARM7_IP= -XARM6_IP= -CAN_PORT=can0 CONN_TYPE=webrtc WEBRTC_SERVER_HOST=0.0.0.0 WEBRTC_SERVER_PORT=9991 diff --git a/dimos/control/blueprints/_hardware.py b/dimos/control/blueprints/_hardware.py index 2a243f360b..a36027865a 100644 --- a/dimos/control/blueprints/_hardware.py +++ b/dimos/control/blueprints/_hardware.py @@ -16,8 +16,6 @@ from __future__ import annotations -import os - from dimos.control.components import ( HardwareComponent, HardwareType, @@ -25,11 +23,12 @@ make_joints, make_twist_base_joints, ) +from dimos.core.global_config import global_config from dimos.utils.data import LfsPath -XARM7_IP = os.getenv("XARM7_IP") -XARM6_IP = os.getenv("XARM6_IP") -CAN_PORT = os.getenv("CAN_PORT", "can0") +XARM7_IP = global_config.xarm7_ip +XARM6_IP = global_config.xarm6_ip +CAN_PORT = global_config.can_port PIPER_MODEL_PATH = LfsPath("piper_description/mujoco_model/piper_no_gripper_description.xml") XARM6_MODEL_PATH = LfsPath("xarm_description/urdf/xarm6/xarm6.urdf") diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index 0b070dabd9..42a7fa552a 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -29,6 +29,9 @@ def _get_all_numbers(s: str) -> list[float]: class GlobalConfig(BaseSettings): robot_ip: str | None = None robot_ips: str | None = None + xarm7_ip: str | None = None + xarm6_ip: str | None = None + can_port: str = "can0" simulation: bool = False replay: bool = False replay_dir: str = "go2_sf_office"