diff --git a/dimos/control/blueprints.py b/dimos/control/blueprints.py deleted file mode 100644 index fff2083322..0000000000 --- a/dimos/control/blueprints.py +++ /dev/null @@ -1,692 +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 module 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.components import ( - HardwareComponent, - HardwareType, - make_gripper_joints, - 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.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 - -_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") - - -# 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="192.168.2.235", - 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="192.168.1.210", - 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="can0", - 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="192.168.2.235", - auto_enable=True, - ), - HardwareComponent( - hardware_id="right_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("right_arm", 6), - adapter_type="xarm", - address="192.168.1.210", - 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="192.168.1.210", - auto_enable=True, - ), - HardwareComponent( - hardware_id="piper_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("piper_arm", 6), - adapter_type="piper", - address="can0", - 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), - } -) - - -# XArm6 teleop - streaming position control -coordinator_teleop_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="192.168.1.210", - auto_enable=True, - ), - ], - tasks=[ - TaskConfig( - name="servo_arm", - type="servo", - joint_names=[f"arm_joint{i + 1}" for i in range(6)], - priority=10, - ), - ], -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - ("joint_command", JointState): LCMTransport("/teleop/joint_command", JointState), - } -) - -# 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=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 6), - adapter_type="xarm", - address="192.168.1.210", - auto_enable=True, - ), - ], - tasks=[ - TaskConfig( - name="velocity_arm", - type="velocity", - joint_names=[f"arm_joint{i + 1}" for i in range(6)], - priority=10, - ), - ], -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - ("joint_command", JointState): LCMTransport("/joystick/joint_command", JointState), - } -) - -# XArm6 combined (servo + velocity tasks) -coordinator_combined_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="192.168.1.210", - auto_enable=True, - ), - ], - tasks=[ - TaskConfig( - name="servo_arm", - type="servo", - joint_names=[f"arm_joint{i + 1}" for i in range(6)], - priority=10, - ), - TaskConfig( - name="velocity_arm", - type="velocity", - joint_names=[f"arm_joint{i + 1}" for i in range(6)], - priority=10, - ), - ], -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - ("joint_command", JointState): LCMTransport("/control/joint_command", JointState), - } -) - - -# 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=[ - HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 6), - adapter_type="mock", - ), - ], - 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, - ee_joint_id=6, - ), - ], -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - ("cartesian_command", PoseStamped): LCMTransport( - "/coordinator/cartesian_command", PoseStamped - ), - } -) - -# Piper arm with CartesianIK -coordinator_cartesian_ik_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="can0", - auto_enable=True, - ), - ], - 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, - ee_joint_id=6, - ), - ], -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - ("cartesian_command", PoseStamped): LCMTransport( - "/coordinator/cartesian_command", PoseStamped - ), - } -) - - -# Single XArm7 with TeleopIK -coordinator_teleop_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="192.168.2.235", - auto_enable=True, - gripper_joints=make_gripper_joints("arm"), - ), - ], - 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, - ee_joint_id=7, - hand="right", - gripper_joint=make_gripper_joints("arm")[0], - gripper_open_pos=0.85, # xArm gripper range - gripper_closed_pos=0.0, - ), - ], -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - ("cartesian_command", PoseStamped): LCMTransport( - "/coordinator/cartesian_command", PoseStamped - ), - ("buttons", Buttons): LCMTransport("/teleop/buttons", Buttons), - } -) - -# Single Piper with TeleopIK -coordinator_teleop_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="can0", - auto_enable=True, - ), - ], - 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, - ee_joint_id=6, - hand="left", - ), - ], -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - ("cartesian_command", PoseStamped): LCMTransport( - "/coordinator/cartesian_command", PoseStamped - ), - ("buttons", Buttons): LCMTransport("/teleop/buttons", Buttons), - } -) - -# 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=[ - HardwareComponent( - hardware_id="xarm_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("xarm_arm", 6), - adapter_type="xarm", - address="192.168.1.210", - auto_enable=True, - ), - HardwareComponent( - hardware_id="piper_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("piper_arm", 6), - adapter_type="piper", - address="can0", - auto_enable=True, - ), - ], - 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, - ee_joint_id=6, - hand="left", - ), - TaskConfig( - name="teleop_piper", - type="teleop_ik", - joint_names=[f"piper_arm_joint{i + 1}" for i in range(6)], - priority=10, - model_path=_PIPER_MODEL_PATH, - ee_joint_id=6, - hand="right", - ), - ], -).transports( - { - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - ("cartesian_command", PoseStamped): LCMTransport( - "/coordinator/cartesian_command", PoseStamped - ), - ("buttons", Buttons): LCMTransport("/teleop/buttons", Buttons), - } -) - - -# 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", - # 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", - "coordinator_teleop_xarm7", - "coordinator_velocity_xarm6", - "coordinator_xarm6", - "coordinator_xarm7", -] diff --git a/dimos/control/blueprints/_hardware.py b/dimos/control/blueprints/_hardware.py new file mode 100644 index 0000000000..a36027865a --- /dev/null +++ b/dimos/control/blueprints/_hardware.py @@ -0,0 +1,93 @@ +# 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 + +from dimos.control.components import ( + HardwareComponent, + HardwareType, + make_gripper_joints, + make_joints, + make_twist_base_joints, +) +from dimos.core.global_config import global_config +from dimos.utils.data import LfsPath + +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") +XARM7_MODEL_PATH = LfsPath("xarm_description/urdf/xarm7/xarm7.urdf") + + +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 [], + ) + + +def xarm6(hw_id: str = "arm", *, gripper: bool = False) -> 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, + gripper_joints=make_gripper_joints(hw_id) if gripper else [], + ) + + +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 new file mode 100644 index 0000000000..7ad441ed70 --- /dev/null +++ b/dimos/control/blueprints/basic.py @@ -0,0 +1,117 @@ +# 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 + +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 + +# 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( + hardware=[mock_arm()], + 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( + hardware=[xarm7()], + 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( + hardware=[xarm6()], + 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( + hardware=[piper()], + 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_piper", + "coordinator_xarm6", + "coordinator_xarm7", +] diff --git a/dimos/control/blueprints/dual.py b/dimos/control/blueprints/dual.py new file mode 100644 index 0000000000..8482316ba5 --- /dev/null +++ b/dimos/control/blueprints/dual.py @@ -0,0 +1,104 @@ +# 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 + +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 + +# Dual mock arms (7-DOF left, 6-DOF right) +coordinator_dual_mock = control_coordinator( + hardware=[mock_arm("left_arm", 7), mock_arm("right_arm", 6)], + 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( + hardware=[xarm7("left_arm"), xarm6("right_arm")], + 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( + hardware=[xarm6("xarm_arm"), piper("piper_arm")], + 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..4ed3410b8f --- /dev/null +++ b/dimos/control/blueprints/mobile.py @@ -0,0 +1,79 @@ +# 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.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 + +_base_joints = make_twist_base_joints("base") + +# Mock holonomic twist base (3-DOF: vx, vy, wz) +coordinator_mock_twist_base = control_coordinator( + hardware=[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) +coordinator_mobile_manip_mock = control_coordinator( + hardware=[mock_arm(), 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=_base_joints, + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("twist_command", Twist): LCMTransport("/cmd_vel", Twist), + } +) + + +__all__ = [ + "coordinator_mobile_manip_mock", + "coordinator_mock_twist_base", +] diff --git a/dimos/control/blueprints/teleop.py b/dimos/control/blueprints/teleop.py new file mode 100644 index 0000000000..2e922bbcbf --- /dev/null +++ b/dimos/control/blueprints/teleop.py @@ -0,0 +1,249 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Advanced control coordinator blueprints: servo, velocity, cartesian IK, and teleop IK. + +Usage: + 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 + +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 + +# XArm6 teleop - streaming position control +coordinator_teleop_xarm6 = control_coordinator( + hardware=[xarm6()], + tasks=[ + TaskConfig( + name="servo_arm", + type="servo", + joint_names=[f"arm_joint{i + 1}" for i in range(6)], + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("joint_command", JointState): LCMTransport("/teleop/joint_command", JointState), + } +) + +# XArm6 velocity control - streaming velocity for joystick +coordinator_velocity_xarm6 = control_coordinator( + hardware=[xarm6()], + tasks=[ + TaskConfig( + name="velocity_arm", + type="velocity", + joint_names=[f"arm_joint{i + 1}" for i in range(6)], + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("joint_command", JointState): LCMTransport("/joystick/joint_command", JointState), + } +) + +# XArm6 combined (servo + velocity tasks) +coordinator_combined_xarm6 = control_coordinator( + hardware=[xarm6()], + tasks=[ + TaskConfig( + name="servo_arm", + type="servo", + joint_names=[f"arm_joint{i + 1}" for i in range(6)], + priority=10, + ), + TaskConfig( + name="velocity_arm", + type="velocity", + joint_names=[f"arm_joint{i + 1}" for i in range(6)], + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("joint_command", JointState): LCMTransport("/control/joint_command", JointState), + } +) + + +# Mock 6-DOF arm with CartesianIK +coordinator_cartesian_ik_mock = control_coordinator( + 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, + ee_joint_id=6, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("cartesian_command", PoseStamped): LCMTransport( + "/coordinator/cartesian_command", PoseStamped + ), + } +) + +# Piper arm with CartesianIK +coordinator_cartesian_ik_piper = control_coordinator( + 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, + ee_joint_id=6, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("cartesian_command", PoseStamped): LCMTransport( + "/coordinator/cartesian_command", PoseStamped + ), + } +) + + +# Single XArm7 with TeleopIK +coordinator_teleop_xarm7 = control_coordinator( + 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, + ee_joint_id=7, + hand="right", + gripper_joint=make_gripper_joints("arm")[0], + gripper_open_pos=0.85, + gripper_closed_pos=0.0, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("cartesian_command", PoseStamped): LCMTransport( + "/coordinator/cartesian_command", PoseStamped + ), + ("buttons", Buttons): LCMTransport("/teleop/buttons", Buttons), + } +) + +# Single Piper with TeleopIK +coordinator_teleop_piper = control_coordinator( + 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, + ee_joint_id=6, + hand="left", + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("cartesian_command", PoseStamped): LCMTransport( + "/coordinator/cartesian_command", PoseStamped + ), + ("buttons", Buttons): LCMTransport("/teleop/buttons", Buttons), + } +) + +# Dual arm teleop: XArm6 + Piper with TeleopIK +coordinator_teleop_dual = control_coordinator( + 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, + ee_joint_id=6, + hand="left", + ), + TaskConfig( + name="teleop_piper", + type="teleop_ik", + joint_names=[f"piper_arm_joint{i + 1}" for i in range(6)], + priority=10, + model_path=PIPER_MODEL_PATH, + ee_joint_id=6, + hand="right", + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("cartesian_command", PoseStamped): LCMTransport( + "/coordinator/cartesian_command", PoseStamped + ), + ("buttons", Buttons): LCMTransport("/teleop/buttons", Buttons), + } +) + + +__all__ = [ + # 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_velocity_xarm6", +] 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/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" diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 0d4225e463..5fd61891c2 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -16,24 +16,24 @@ # Run `pytest dimos/robot/test_all_blueprints_generation.py` to regenerate. all_blueprints = { - "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 da07a1bdd4..71e16c2da8 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,