From 1d6b0862d7a22339a36feb8678142fdffe7b1a17 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 6 Jan 2026 14:31:54 -0800 Subject: [PATCH 01/42] archive old driver to manipulators_old for redesign --- dimos/hardware/{manipulators => manipulators_old}/README.md | 0 dimos/hardware/{manipulators => manipulators_old}/__init__.py | 0 .../hardware/{manipulators => manipulators_old}/base/__init__.py | 0 .../base/component_based_architecture.md | 0 .../base/components/__init__.py | 0 .../{manipulators => manipulators_old}/base/components/motion.py | 0 .../{manipulators => manipulators_old}/base/components/servo.py | 0 .../{manipulators => manipulators_old}/base/components/status.py | 0 dimos/hardware/{manipulators => manipulators_old}/base/driver.py | 0 .../{manipulators => manipulators_old}/base/sdk_interface.py | 0 dimos/hardware/{manipulators => manipulators_old}/base/spec.py | 0 .../{manipulators => manipulators_old}/base/tests/__init__.py | 0 .../{manipulators => manipulators_old}/base/tests/conftest.py | 0 .../base/tests/test_driver_unit.py | 0 .../{manipulators => manipulators_old}/base/utils/__init__.py | 0 .../{manipulators => manipulators_old}/base/utils/converters.py | 0 .../{manipulators => manipulators_old}/base/utils/shared_state.py | 0 .../{manipulators => manipulators_old}/base/utils/validators.py | 0 dimos/hardware/{manipulators => manipulators_old}/piper/README.md | 0 .../hardware/{manipulators => manipulators_old}/piper/__init__.py | 0 .../{manipulators => manipulators_old}/piper/can_activate.sh | 0 .../piper/components/__init__.py | 0 .../piper/components/configuration.py | 0 .../piper/components/gripper_control.py | 0 .../piper/components/kinematics.py | 0 .../piper/components/motion_control.py | 0 .../piper/components/state_queries.py | 0 .../piper/components/system_control.py | 0 .../{manipulators => manipulators_old}/piper/piper_blueprints.py | 0 .../piper/piper_description.urdf | 0 .../{manipulators => manipulators_old}/piper/piper_driver.py | 0 .../{manipulators => manipulators_old}/piper/piper_wrapper.py | 0 .../{manipulators => manipulators_old}/test_integration_runner.py | 0 dimos/hardware/{manipulators => manipulators_old}/xarm/README.md | 0 .../hardware/{manipulators => manipulators_old}/xarm/__init__.py | 0 .../xarm/components/__init__.py | 0 .../xarm/components/gripper_control.py | 0 .../xarm/components/kinematics.py | 0 .../xarm/components/motion_control.py | 0 .../xarm/components/state_queries.py | 0 .../xarm/components/system_control.py | 0 dimos/hardware/{manipulators => manipulators_old}/xarm/spec.py | 0 .../{manipulators => manipulators_old}/xarm/xarm_blueprints.py | 0 .../{manipulators => manipulators_old}/xarm/xarm_driver.py | 0 .../{manipulators => manipulators_old}/xarm/xarm_wrapper.py | 0 45 files changed, 0 insertions(+), 0 deletions(-) rename dimos/hardware/{manipulators => manipulators_old}/README.md (100%) rename dimos/hardware/{manipulators => manipulators_old}/__init__.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/base/__init__.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/base/component_based_architecture.md (100%) rename dimos/hardware/{manipulators => manipulators_old}/base/components/__init__.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/base/components/motion.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/base/components/servo.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/base/components/status.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/base/driver.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/base/sdk_interface.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/base/spec.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/base/tests/__init__.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/base/tests/conftest.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/base/tests/test_driver_unit.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/base/utils/__init__.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/base/utils/converters.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/base/utils/shared_state.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/base/utils/validators.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/piper/README.md (100%) rename dimos/hardware/{manipulators => manipulators_old}/piper/__init__.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/piper/can_activate.sh (100%) rename dimos/hardware/{manipulators => manipulators_old}/piper/components/__init__.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/piper/components/configuration.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/piper/components/gripper_control.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/piper/components/kinematics.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/piper/components/motion_control.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/piper/components/state_queries.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/piper/components/system_control.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/piper/piper_blueprints.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/piper/piper_description.urdf (100%) rename dimos/hardware/{manipulators => manipulators_old}/piper/piper_driver.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/piper/piper_wrapper.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/test_integration_runner.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/xarm/README.md (100%) rename dimos/hardware/{manipulators => manipulators_old}/xarm/__init__.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/xarm/components/__init__.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/xarm/components/gripper_control.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/xarm/components/kinematics.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/xarm/components/motion_control.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/xarm/components/state_queries.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/xarm/components/system_control.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/xarm/spec.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/xarm/xarm_blueprints.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/xarm/xarm_driver.py (100%) rename dimos/hardware/{manipulators => manipulators_old}/xarm/xarm_wrapper.py (100%) diff --git a/dimos/hardware/manipulators/README.md b/dimos/hardware/manipulators_old/README.md similarity index 100% rename from dimos/hardware/manipulators/README.md rename to dimos/hardware/manipulators_old/README.md diff --git a/dimos/hardware/manipulators/__init__.py b/dimos/hardware/manipulators_old/__init__.py similarity index 100% rename from dimos/hardware/manipulators/__init__.py rename to dimos/hardware/manipulators_old/__init__.py diff --git a/dimos/hardware/manipulators/base/__init__.py b/dimos/hardware/manipulators_old/base/__init__.py similarity index 100% rename from dimos/hardware/manipulators/base/__init__.py rename to dimos/hardware/manipulators_old/base/__init__.py diff --git a/dimos/hardware/manipulators/base/component_based_architecture.md b/dimos/hardware/manipulators_old/base/component_based_architecture.md similarity index 100% rename from dimos/hardware/manipulators/base/component_based_architecture.md rename to dimos/hardware/manipulators_old/base/component_based_architecture.md diff --git a/dimos/hardware/manipulators/base/components/__init__.py b/dimos/hardware/manipulators_old/base/components/__init__.py similarity index 100% rename from dimos/hardware/manipulators/base/components/__init__.py rename to dimos/hardware/manipulators_old/base/components/__init__.py diff --git a/dimos/hardware/manipulators/base/components/motion.py b/dimos/hardware/manipulators_old/base/components/motion.py similarity index 100% rename from dimos/hardware/manipulators/base/components/motion.py rename to dimos/hardware/manipulators_old/base/components/motion.py diff --git a/dimos/hardware/manipulators/base/components/servo.py b/dimos/hardware/manipulators_old/base/components/servo.py similarity index 100% rename from dimos/hardware/manipulators/base/components/servo.py rename to dimos/hardware/manipulators_old/base/components/servo.py diff --git a/dimos/hardware/manipulators/base/components/status.py b/dimos/hardware/manipulators_old/base/components/status.py similarity index 100% rename from dimos/hardware/manipulators/base/components/status.py rename to dimos/hardware/manipulators_old/base/components/status.py diff --git a/dimos/hardware/manipulators/base/driver.py b/dimos/hardware/manipulators_old/base/driver.py similarity index 100% rename from dimos/hardware/manipulators/base/driver.py rename to dimos/hardware/manipulators_old/base/driver.py diff --git a/dimos/hardware/manipulators/base/sdk_interface.py b/dimos/hardware/manipulators_old/base/sdk_interface.py similarity index 100% rename from dimos/hardware/manipulators/base/sdk_interface.py rename to dimos/hardware/manipulators_old/base/sdk_interface.py diff --git a/dimos/hardware/manipulators/base/spec.py b/dimos/hardware/manipulators_old/base/spec.py similarity index 100% rename from dimos/hardware/manipulators/base/spec.py rename to dimos/hardware/manipulators_old/base/spec.py diff --git a/dimos/hardware/manipulators/base/tests/__init__.py b/dimos/hardware/manipulators_old/base/tests/__init__.py similarity index 100% rename from dimos/hardware/manipulators/base/tests/__init__.py rename to dimos/hardware/manipulators_old/base/tests/__init__.py diff --git a/dimos/hardware/manipulators/base/tests/conftest.py b/dimos/hardware/manipulators_old/base/tests/conftest.py similarity index 100% rename from dimos/hardware/manipulators/base/tests/conftest.py rename to dimos/hardware/manipulators_old/base/tests/conftest.py diff --git a/dimos/hardware/manipulators/base/tests/test_driver_unit.py b/dimos/hardware/manipulators_old/base/tests/test_driver_unit.py similarity index 100% rename from dimos/hardware/manipulators/base/tests/test_driver_unit.py rename to dimos/hardware/manipulators_old/base/tests/test_driver_unit.py diff --git a/dimos/hardware/manipulators/base/utils/__init__.py b/dimos/hardware/manipulators_old/base/utils/__init__.py similarity index 100% rename from dimos/hardware/manipulators/base/utils/__init__.py rename to dimos/hardware/manipulators_old/base/utils/__init__.py diff --git a/dimos/hardware/manipulators/base/utils/converters.py b/dimos/hardware/manipulators_old/base/utils/converters.py similarity index 100% rename from dimos/hardware/manipulators/base/utils/converters.py rename to dimos/hardware/manipulators_old/base/utils/converters.py diff --git a/dimos/hardware/manipulators/base/utils/shared_state.py b/dimos/hardware/manipulators_old/base/utils/shared_state.py similarity index 100% rename from dimos/hardware/manipulators/base/utils/shared_state.py rename to dimos/hardware/manipulators_old/base/utils/shared_state.py diff --git a/dimos/hardware/manipulators/base/utils/validators.py b/dimos/hardware/manipulators_old/base/utils/validators.py similarity index 100% rename from dimos/hardware/manipulators/base/utils/validators.py rename to dimos/hardware/manipulators_old/base/utils/validators.py diff --git a/dimos/hardware/manipulators/piper/README.md b/dimos/hardware/manipulators_old/piper/README.md similarity index 100% rename from dimos/hardware/manipulators/piper/README.md rename to dimos/hardware/manipulators_old/piper/README.md diff --git a/dimos/hardware/manipulators/piper/__init__.py b/dimos/hardware/manipulators_old/piper/__init__.py similarity index 100% rename from dimos/hardware/manipulators/piper/__init__.py rename to dimos/hardware/manipulators_old/piper/__init__.py diff --git a/dimos/hardware/manipulators/piper/can_activate.sh b/dimos/hardware/manipulators_old/piper/can_activate.sh similarity index 100% rename from dimos/hardware/manipulators/piper/can_activate.sh rename to dimos/hardware/manipulators_old/piper/can_activate.sh diff --git a/dimos/hardware/manipulators/piper/components/__init__.py b/dimos/hardware/manipulators_old/piper/components/__init__.py similarity index 100% rename from dimos/hardware/manipulators/piper/components/__init__.py rename to dimos/hardware/manipulators_old/piper/components/__init__.py diff --git a/dimos/hardware/manipulators/piper/components/configuration.py b/dimos/hardware/manipulators_old/piper/components/configuration.py similarity index 100% rename from dimos/hardware/manipulators/piper/components/configuration.py rename to dimos/hardware/manipulators_old/piper/components/configuration.py diff --git a/dimos/hardware/manipulators/piper/components/gripper_control.py b/dimos/hardware/manipulators_old/piper/components/gripper_control.py similarity index 100% rename from dimos/hardware/manipulators/piper/components/gripper_control.py rename to dimos/hardware/manipulators_old/piper/components/gripper_control.py diff --git a/dimos/hardware/manipulators/piper/components/kinematics.py b/dimos/hardware/manipulators_old/piper/components/kinematics.py similarity index 100% rename from dimos/hardware/manipulators/piper/components/kinematics.py rename to dimos/hardware/manipulators_old/piper/components/kinematics.py diff --git a/dimos/hardware/manipulators/piper/components/motion_control.py b/dimos/hardware/manipulators_old/piper/components/motion_control.py similarity index 100% rename from dimos/hardware/manipulators/piper/components/motion_control.py rename to dimos/hardware/manipulators_old/piper/components/motion_control.py diff --git a/dimos/hardware/manipulators/piper/components/state_queries.py b/dimos/hardware/manipulators_old/piper/components/state_queries.py similarity index 100% rename from dimos/hardware/manipulators/piper/components/state_queries.py rename to dimos/hardware/manipulators_old/piper/components/state_queries.py diff --git a/dimos/hardware/manipulators/piper/components/system_control.py b/dimos/hardware/manipulators_old/piper/components/system_control.py similarity index 100% rename from dimos/hardware/manipulators/piper/components/system_control.py rename to dimos/hardware/manipulators_old/piper/components/system_control.py diff --git a/dimos/hardware/manipulators/piper/piper_blueprints.py b/dimos/hardware/manipulators_old/piper/piper_blueprints.py similarity index 100% rename from dimos/hardware/manipulators/piper/piper_blueprints.py rename to dimos/hardware/manipulators_old/piper/piper_blueprints.py diff --git a/dimos/hardware/manipulators/piper/piper_description.urdf b/dimos/hardware/manipulators_old/piper/piper_description.urdf similarity index 100% rename from dimos/hardware/manipulators/piper/piper_description.urdf rename to dimos/hardware/manipulators_old/piper/piper_description.urdf diff --git a/dimos/hardware/manipulators/piper/piper_driver.py b/dimos/hardware/manipulators_old/piper/piper_driver.py similarity index 100% rename from dimos/hardware/manipulators/piper/piper_driver.py rename to dimos/hardware/manipulators_old/piper/piper_driver.py diff --git a/dimos/hardware/manipulators/piper/piper_wrapper.py b/dimos/hardware/manipulators_old/piper/piper_wrapper.py similarity index 100% rename from dimos/hardware/manipulators/piper/piper_wrapper.py rename to dimos/hardware/manipulators_old/piper/piper_wrapper.py diff --git a/dimos/hardware/manipulators/test_integration_runner.py b/dimos/hardware/manipulators_old/test_integration_runner.py similarity index 100% rename from dimos/hardware/manipulators/test_integration_runner.py rename to dimos/hardware/manipulators_old/test_integration_runner.py diff --git a/dimos/hardware/manipulators/xarm/README.md b/dimos/hardware/manipulators_old/xarm/README.md similarity index 100% rename from dimos/hardware/manipulators/xarm/README.md rename to dimos/hardware/manipulators_old/xarm/README.md diff --git a/dimos/hardware/manipulators/xarm/__init__.py b/dimos/hardware/manipulators_old/xarm/__init__.py similarity index 100% rename from dimos/hardware/manipulators/xarm/__init__.py rename to dimos/hardware/manipulators_old/xarm/__init__.py diff --git a/dimos/hardware/manipulators/xarm/components/__init__.py b/dimos/hardware/manipulators_old/xarm/components/__init__.py similarity index 100% rename from dimos/hardware/manipulators/xarm/components/__init__.py rename to dimos/hardware/manipulators_old/xarm/components/__init__.py diff --git a/dimos/hardware/manipulators/xarm/components/gripper_control.py b/dimos/hardware/manipulators_old/xarm/components/gripper_control.py similarity index 100% rename from dimos/hardware/manipulators/xarm/components/gripper_control.py rename to dimos/hardware/manipulators_old/xarm/components/gripper_control.py diff --git a/dimos/hardware/manipulators/xarm/components/kinematics.py b/dimos/hardware/manipulators_old/xarm/components/kinematics.py similarity index 100% rename from dimos/hardware/manipulators/xarm/components/kinematics.py rename to dimos/hardware/manipulators_old/xarm/components/kinematics.py diff --git a/dimos/hardware/manipulators/xarm/components/motion_control.py b/dimos/hardware/manipulators_old/xarm/components/motion_control.py similarity index 100% rename from dimos/hardware/manipulators/xarm/components/motion_control.py rename to dimos/hardware/manipulators_old/xarm/components/motion_control.py diff --git a/dimos/hardware/manipulators/xarm/components/state_queries.py b/dimos/hardware/manipulators_old/xarm/components/state_queries.py similarity index 100% rename from dimos/hardware/manipulators/xarm/components/state_queries.py rename to dimos/hardware/manipulators_old/xarm/components/state_queries.py diff --git a/dimos/hardware/manipulators/xarm/components/system_control.py b/dimos/hardware/manipulators_old/xarm/components/system_control.py similarity index 100% rename from dimos/hardware/manipulators/xarm/components/system_control.py rename to dimos/hardware/manipulators_old/xarm/components/system_control.py diff --git a/dimos/hardware/manipulators/xarm/spec.py b/dimos/hardware/manipulators_old/xarm/spec.py similarity index 100% rename from dimos/hardware/manipulators/xarm/spec.py rename to dimos/hardware/manipulators_old/xarm/spec.py diff --git a/dimos/hardware/manipulators/xarm/xarm_blueprints.py b/dimos/hardware/manipulators_old/xarm/xarm_blueprints.py similarity index 100% rename from dimos/hardware/manipulators/xarm/xarm_blueprints.py rename to dimos/hardware/manipulators_old/xarm/xarm_blueprints.py diff --git a/dimos/hardware/manipulators/xarm/xarm_driver.py b/dimos/hardware/manipulators_old/xarm/xarm_driver.py similarity index 100% rename from dimos/hardware/manipulators/xarm/xarm_driver.py rename to dimos/hardware/manipulators_old/xarm/xarm_driver.py diff --git a/dimos/hardware/manipulators/xarm/xarm_wrapper.py b/dimos/hardware/manipulators_old/xarm/xarm_wrapper.py similarity index 100% rename from dimos/hardware/manipulators/xarm/xarm_wrapper.py rename to dimos/hardware/manipulators_old/xarm/xarm_wrapper.py From e2ba169dfbfadb5fe1553cb4690c495c3e5eee45 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 6 Jan 2026 18:09:55 -0800 Subject: [PATCH 02/42] spec.py defining minimal protocol for an arm driver --- dimos/hardware/manipulators/spec.py | 265 ++++++++++++++++++++++++++++ 1 file changed, 265 insertions(+) create mode 100644 dimos/hardware/manipulators/spec.py diff --git a/dimos/hardware/manipulators/spec.py b/dimos/hardware/manipulators/spec.py new file mode 100644 index 0000000000..ea68e115d7 --- /dev/null +++ b/dimos/hardware/manipulators/spec.py @@ -0,0 +1,265 @@ +# 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. + +"""Manipulator specifications: Protocol and shared types. + +This file defines: +1. Shared enums and dataclasses used by all arms +2. ManipulatorBackend Protocol that backends must implement + +Note: No ABC for drivers. Each arm implements its own driver +with full control over threading and logic. +""" + +from dataclasses import dataclass +from enum import Enum +from typing import Protocol + +from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 + +# ============================================================================ +# SHARED TYPES +# ============================================================================ + + +class DriverStatus(Enum): + """Status returned by driver operations.""" + + DISCONNECTED = "disconnected" + CONNECTED = "connected" + ENABLED = "enabled" + MOVING = "moving" + ERROR = "error" + + +class ControlMode(Enum): + """Control modes for manipulator.""" + + POSITION = "position" + VELOCITY = "velocity" + TORQUE = "torque" + CARTESIAN = "cartesian" + CARTESIAN_VELOCITY = "cartesian_velocity" + IMPEDANCE = "impedance" + + +@dataclass +class ManipulatorInfo: + """Information about the manipulator.""" + + vendor: str + model: str + dof: int + firmware_version: str | None = None + serial_number: str | None = None + + +@dataclass +class JointLimits: + """Joint position and velocity limits.""" + + position_lower: list[float] # radians + position_upper: list[float] # radians + velocity_max: list[float] # rad/s + + +def default_base_transform() -> Transform: + """Default identity transform for arm mounting.""" + return Transform( + translation=Vector3(0.0, 0.0, 0.0), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + ) + + +# ============================================================================ +# BACKEND PROTOCOL +# ============================================================================ + + +class ManipulatorBackend(Protocol): + """Protocol for hardware-specific IO. + + Implement this per vendor SDK. All methods use SI units: + - Angles: radians + - Angular velocity: rad/s + - Torque: Nm + - Position: meters + - Force: Newtons + + Benefits of Protocol over ABC: + - Structural typing (duck typing) - no inheritance required + - Backends just need matching methods, no base class + - Easy to create MockBackend for testing + - Type checkers validate compatibility + """ + + # --- Connection --- + + def connect(self, config: dict) -> bool: + """Connect to hardware. Returns True on success.""" + ... + + def disconnect(self) -> None: + """Disconnect from hardware.""" + ... + + def is_connected(self) -> bool: + """Check if connected.""" + ... + + # --- Info --- + + def get_info(self) -> ManipulatorInfo: + """Get manipulator info (vendor, model, DOF).""" + ... + + def get_dof(self) -> int: + """Get degrees of freedom.""" + ... + + def get_limits(self) -> JointLimits: + """Get joint limits.""" + ... + + # --- Control Mode --- + + def set_control_mode(self, mode: ControlMode) -> bool: + """Set control mode (position, velocity, torque, cartesian, etc). + + Args: + mode: Target control mode + + Returns: + True if mode switch successful, False otherwise + + Note: Some arms (like XArm) may accept commands in any mode, + while others (like Piper) require explicit mode switching. + """ + ... + + def get_control_mode(self) -> ControlMode: + """Get current control mode. + + Returns: + Current control mode + """ + ... + + # --- State Reading --- + + def read_joint_positions(self) -> list[float]: + """Read current joint positions (radians).""" + ... + + def read_joint_velocities(self) -> list[float]: + """Read current joint velocities (rad/s).""" + ... + + def read_joint_efforts(self) -> list[float]: + """Read current joint efforts (Nm).""" + ... + + def read_state(self) -> dict: + """Read robot state (mode, state code, etc).""" + ... + + def read_error(self) -> tuple[int, str]: + """Read error code and message. (0, '') means no error.""" + ... + + # --- Motion Control (Joint Space) --- + + def write_joint_positions( + self, + positions: list[float], + velocity: float = 1.0, + ) -> bool: + """Command joint positions (radians). Returns success.""" + ... + + def write_joint_velocities(self, velocities: list[float]) -> bool: + """Command joint velocities (rad/s). Returns success.""" + ... + + def write_stop(self) -> bool: + """Stop all motion immediately.""" + ... + + # --- Servo Control --- + + def write_enable(self, enable: bool) -> bool: + """Enable or disable servos. Returns success.""" + ... + + def read_enabled(self) -> bool: + """Check if servos are enabled.""" + ... + + def write_clear_errors(self) -> bool: + """Clear error state. Returns success.""" + ... + + # --- Optional: Cartesian Control --- + # Return None/False if not supported + + def read_cartesian_position(self) -> dict[str, float] | None: + """Read end-effector pose. + + Returns: + Dict with keys: x, y, z (meters), roll, pitch, yaw (radians) + None if not supported + """ + ... + + def write_cartesian_position( + self, + pose: dict[str, float], + velocity: float = 1.0, + ) -> bool: + """Command end-effector pose. + + Args: + pose: Dict with keys: x, y, z (meters), roll, pitch, yaw (radians) + velocity: Speed as fraction of max (0-1) + + Returns: + True if command accepted, False if not supported + """ + ... + + # --- Optional: Gripper --- + + def read_gripper_position(self) -> float | None: + """Read gripper position (meters). None if no gripper.""" + ... + + def write_gripper_position(self, position: float) -> bool: + """Command gripper position. False if no gripper.""" + ... + + # --- Optional: Force/Torque Sensor --- + + def read_force_torque(self) -> list[float] | None: + """Read F/T sensor [fx, fy, fz, tx, ty, tz]. None if no sensor.""" + ... + + +__all__ = [ + "ControlMode", + "DriverStatus", + "JointLimits", + "ManipulatorBackend", + "ManipulatorInfo", + "default_base_transform", +] From 919691cdbff6be4419e38a277b2a9b5864441526 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 6 Jan 2026 18:11:54 -0800 Subject: [PATCH 03/42] xarm driver driver added - driver owns control thread and robot state threads also invokes rpc calls to arm specific SDK backends --- dimos/hardware/manipulators/xarm/arm.py | 427 ++++++++++++++++++++++++ 1 file changed, 427 insertions(+) create mode 100644 dimos/hardware/manipulators/xarm/arm.py diff --git a/dimos/hardware/manipulators/xarm/arm.py b/dimos/hardware/manipulators/xarm/arm.py new file mode 100644 index 0000000000..574ffb5ed9 --- /dev/null +++ b/dimos/hardware/manipulators/xarm/arm.py @@ -0,0 +1,427 @@ +# 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. + +"""XArm manipulator driver. + +This driver: +- Uses XArmBackend by default (injectable for testing) +- Has full control over threading and timing +- Can be customized without affecting other arms +""" + +from dataclasses import dataclass, field +import threading +import time + +from dimos.core import In, Module, ModuleConfig, Out, rpc +from dimos.hardware.manipulators.spec import ( + ControlMode, + DriverStatus, + JointLimits, + ManipulatorBackend, + ManipulatorInfo, + default_base_transform, +) +from dimos.hardware.manipulators.xarm.backend import XArmBackend +from dimos.msgs.geometry_msgs import Transform +from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState + +# ============================================================================ +# CONFIGURATION +# ============================================================================ + + +@dataclass +class XArmConfig(ModuleConfig): + """XArm-specific configuration. + + Type-safe config with XArm-specific fields. + """ + + ip: str = "192.168.1.185" + dof: int = 6 + control_rate: float = 100.0 # Hz - XArm can handle 100Hz + monitor_rate: float = 10.0 # Hz + has_gripper: bool = False + has_ft_sensor: bool = False + base_frame_id: str = "base_link" + base_transform: Transform | None = field(default_factory=default_base_transform) + connection_type: str = "real" # "real" or "sim" + + +# ============================================================================ +# DRIVER +# ============================================================================ + + +class XArm(Module[XArmConfig]): + """XArm manipulator driver. + + Features: + - Injectable backend (defaults to XArmBackend) + - 100Hz control loop for joint state + - 10Hz monitor loop for robot state + - Full RPC interface for control + + Example: + >>> arm = XArm(ip="192.168.1.185") + >>> arm.start() + >>> arm.enable_servos() + >>> arm.move_joint([0, 0, 0, 0, 0, 0]) + + Testing: + >>> from dimos.hardware.manipulators.mock import MockBackend + >>> arm = XArm(backend=MockBackend()) + >>> arm.start() # No hardware needed! + """ + + # Input topics (commands from controllers) + joint_position_command: In[JointCommand] + joint_velocity_command: In[JointCommand] + + # Output topics + joint_state: Out[JointState] + robot_state: Out[RobotState] + + # Config + config: XArmConfig + default_config = XArmConfig + + def __init__( + self, + backend: ManipulatorBackend | None = None, + *args, + **kwargs, + ) -> None: + super().__init__(*args, **kwargs) + + # Backend is injectable for testing + self.backend: ManipulatorBackend = backend or XArmBackend() + + # Threading state + self._running = False + self._control_thread: threading.Thread | None = None + self._monitor_thread: threading.Thread | None = None + + # Cached info + self._dof: int = self.config.dof + self._joint_names: list[str] = [f"joint{i + 1}" for i in range(self._dof)] + + # Auto-connect on initialization + self._auto_start() + + def _auto_start(self) -> None: + """Auto-connect to hardware on initialization.""" + config_dict = { + "ip": self.config.ip, + "dof": self.config.dof, + } + + if not self.backend.connect(config_dict): + print(f"WARNING: Failed to connect to XArm at {self.config.ip}") + return + + # Update DOF from backend (in case it differs) + self._dof = self.backend.get_dof() + self._joint_names = [f"joint{i + 1}" for i in range(self._dof)] + + # Start threads + self._running = True + + self._control_thread = threading.Thread( + target=self._control_loop, + name="XArm-Control", + daemon=True, + ) + self._monitor_thread = threading.Thread( + target=self._monitor_loop, + name="XArm-Monitor", + daemon=True, + ) + + self._control_thread.start() + self._monitor_thread.start() + + print(f"XArm connected at {self.config.ip}, DOF={self._dof}") + + def _subscribe_to_commands(self) -> None: + """Subscribe to command input topics.""" + try: + if self.joint_position_command: + self.joint_position_command.subscribe(self._on_joint_position_command) + print("Subscribed to joint_position_command") + except Exception as e: + print(f"Could not subscribe to joint_position_command: {e}") + + try: + if self.joint_velocity_command: + self.joint_velocity_command.subscribe(self._on_joint_velocity_command) + print("Subscribed to joint_velocity_command") + except Exception as e: + print(f"Could not subscribe to joint_velocity_command: {e}") + + def _on_joint_position_command(self, cmd: JointCommand) -> None: + """Handle incoming joint position command.""" + if not self._running: + return + + positions = list(cmd.positions) if cmd.positions else [] + if len(positions) != self._dof: + print(f"WARNING: Position command has {len(positions)} joints, expected {self._dof}") + return + + self.backend.write_joint_positions(positions) + + def _on_joint_velocity_command(self, cmd: JointCommand) -> None: + """Handle incoming joint velocity command.""" + if not self._running: + return + + # JointCommand uses 'positions' field for velocity commands too + velocities = list(cmd.positions) if cmd.positions else [] + if len(velocities) != self._dof: + print(f"WARNING: Velocity command has {len(velocities)} joints, expected {self._dof}") + return + + self.backend.write_joint_velocities(velocities) + + # ========================================================================= + # Lifecycle + # ========================================================================= + + @rpc + def start(self) -> DriverStatus: + """Connect to XArm and start control loops (if not already running).""" + super().start() # Important: sets up transports + + if self._running: + # Already connected, just subscribe to commands + self._subscribe_to_commands() + return DriverStatus.CONNECTED + + self._auto_start() + self._subscribe_to_commands() + return DriverStatus.CONNECTED if self._running else DriverStatus.ERROR + + @rpc + def stop(self) -> None: + """Stop XArm and disconnect.""" + self._running = False + + # Stop motion first + try: + self.backend.write_stop() + except Exception: + pass + + # Wait for threads + if self._control_thread and self._control_thread.is_alive(): + self._control_thread.join(timeout=1.0) + if self._monitor_thread and self._monitor_thread.is_alive(): + self._monitor_thread.join(timeout=1.0) + + # Disconnect + try: + self.backend.disconnect() + except Exception: + pass + + super().stop() + + # ========================================================================= + # Control Loop (100Hz) + # ========================================================================= + + def _control_loop(self) -> None: + """High-frequency loop for joint state publishing.""" + period = 1.0 / self.config.control_rate + + while self._running: + start = time.perf_counter() + + try: + self._publish_joint_state() + except Exception as e: + print(f"XArm control loop error: {e}") + + # Rate control + elapsed = time.perf_counter() - start + if elapsed < period: + time.sleep(period - elapsed) + + def _publish_joint_state(self) -> None: + """Read and publish joint state.""" + positions = self.backend.read_joint_positions() + velocities = self.backend.read_joint_velocities() + efforts = self.backend.read_joint_efforts() + + msg = JointState( + ts=time.time(), + frame_id="joint_state", + name=self._joint_names, + position=positions, + velocity=velocities, + effort=efforts, + ) + self.joint_state.publish(msg) + + # ========================================================================= + # Monitor Loop (10Hz) + # ========================================================================= + + def _monitor_loop(self) -> None: + """Low-frequency loop for robot state monitoring.""" + period = 1.0 / self.config.monitor_rate + + while self._running: + start = time.perf_counter() + + try: + self._publish_robot_state() + except Exception as e: + print(f"XArm monitor loop error: {e}") + + # Rate control + elapsed = time.perf_counter() - start + if elapsed < period: + time.sleep(period - elapsed) + + def _publish_robot_state(self) -> None: + """Read and publish robot state.""" + state = self.backend.read_state() + error_code, _ = self.backend.read_error() + + msg = RobotState( + state=state.get("state", 0), + mode=state.get("mode", 0), + error_code=error_code, + warn_code=0, + ) + self.robot_state.publish(msg) + + # ========================================================================= + # RPC Methods - Servo Control + # ========================================================================= + + @rpc + def enable_servos(self) -> bool: + """Enable motor control.""" + return self.backend.write_enable(True) + + @rpc + def disable_servos(self) -> bool: + """Disable motor control.""" + return self.backend.write_enable(False) + + @rpc + def clear_errors(self) -> bool: + """Clear error state.""" + return self.backend.write_clear_errors() + + # ========================================================================= + # RPC Methods - Mode Control + # ========================================================================= + + @rpc + def set_control_mode(self, mode: ControlMode) -> bool: + """Set control mode (position, velocity, cartesian, etc).""" + return self.backend.set_control_mode(mode) + + @rpc + def get_control_mode(self) -> ControlMode: + """Get current control mode.""" + return self.backend.get_control_mode() + + # ========================================================================= + # RPC Methods - Joint Space Motion + # ========================================================================= + + @rpc + def move_joint(self, positions: list[float], velocity: float = 0.5) -> bool: + """Move to joint positions (radians).""" + return self.backend.write_joint_positions(positions, velocity) + + @rpc + def move_velocity(self, velocities: list[float]) -> bool: + """Move with joint velocities (rad/s).""" + return self.backend.write_joint_velocities(velocities) + + @rpc + def stop_motion(self) -> bool: + """Stop all motion.""" + return self.backend.write_stop() + + # ========================================================================= + # RPC Methods - Cartesian Motion + # ========================================================================= + + @rpc + def get_cartesian_position(self) -> dict[str, float] | None: + """Get end-effector pose (x, y, z in meters; roll, pitch, yaw in radians).""" + return self.backend.read_cartesian_position() + + @rpc + def move_cartesian( + self, + pose: dict[str, float], + velocity: float = 0.5, + ) -> bool: + """Move to cartesian pose. + + Args: + pose: Dict with x, y, z (meters), roll, pitch, yaw (radians) + velocity: Speed as fraction of max (0-1) + """ + return self.backend.write_cartesian_position(pose, velocity) + + # ========================================================================= + # RPC Methods - Info + # ========================================================================= + + @rpc + def get_info(self) -> ManipulatorInfo: + """Get manipulator info.""" + return self.backend.get_info() + + @rpc + def get_limits(self) -> JointLimits: + """Get joint limits.""" + return self.backend.get_limits() + + # ========================================================================= + # RPC Methods - Optional Features (Gripper) + # ========================================================================= + + @rpc + def get_gripper_position(self) -> float | None: + """Get gripper position (meters). None if no gripper.""" + if not self.config.has_gripper: + return None + return self.backend.read_gripper_position() + + @rpc + def set_gripper_position(self, position: float) -> bool: + """Set gripper position (meters).""" + if not self.config.has_gripper: + return False + return self.backend.write_gripper_position(position) + + +# ============================================================================ +# EXPORTS +# ============================================================================ + +xarm = XArm.blueprint + +__all__ = ["XArm", "XArmConfig", "xarm"] From 41d7b828190876558cd005cf314a6d18d522d0d4 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 6 Jan 2026 18:13:21 -0800 Subject: [PATCH 04/42] xarm SDK specific wrapper to interface with dimos RPC calls from the driver --- dimos/hardware/manipulators/xarm/__init__.py | 48 +++ dimos/hardware/manipulators/xarm/backend.py | 368 ++++++++++++++++++ .../hardware/manipulators/xarm/blueprints.py | 191 +++++++++ 3 files changed, 607 insertions(+) create mode 100644 dimos/hardware/manipulators/xarm/__init__.py create mode 100644 dimos/hardware/manipulators/xarm/backend.py create mode 100644 dimos/hardware/manipulators/xarm/blueprints.py diff --git a/dimos/hardware/manipulators/xarm/__init__.py b/dimos/hardware/manipulators/xarm/__init__.py new file mode 100644 index 0000000000..71a1fbf112 --- /dev/null +++ b/dimos/hardware/manipulators/xarm/__init__.py @@ -0,0 +1,48 @@ +# 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. + +"""XArm manipulator driver. + +Usage: + >>> from dimos.hardware.manipulators.xarm import XArm + >>> arm = XArm(ip="192.168.1.185", dof=6) + >>> arm.start() + >>> arm.enable_servos() + >>> arm.move_joint([0, 0, 0, 0, 0, 0]) +""" + +from dimos.hardware.manipulators.xarm.arm import XArm, XArmConfig, xarm +from dimos.hardware.manipulators.xarm.backend import XArmBackend +from dimos.hardware.manipulators.xarm.blueprints import ( + xarm5_servo, + xarm7_servo, + xarm7_trajectory, + xarm_cartesian, + xarm_servo, + xarm_trajectory, +) + +__all__ = [ + "XArm", + "XArmBackend", + "XArmConfig", + "xarm", + "xarm5_servo", + "xarm7_servo", + "xarm7_trajectory", + "xarm_cartesian", + # Blueprints + "xarm_servo", + "xarm_trajectory", +] diff --git a/dimos/hardware/manipulators/xarm/backend.py b/dimos/hardware/manipulators/xarm/backend.py new file mode 100644 index 0000000000..aff088cc27 --- /dev/null +++ b/dimos/hardware/manipulators/xarm/backend.py @@ -0,0 +1,368 @@ +# 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. + +"""XArm backend - implements ManipulatorBackend protocol. + +Handles all XArm SDK communication and unit conversion. +""" + +import math + +from xarm.wrapper import XArmAPI + +from dimos.hardware.manipulators.spec import ( + ControlMode, + JointLimits, + ManipulatorInfo, +) + +# XArm mode codes +_XARM_MODE_POSITION = 0 +_XARM_MODE_SERVO_CARTESIAN = 1 +_XARM_MODE_JOINT_VELOCITY = 4 +_XARM_MODE_CARTESIAN_VELOCITY = 5 +_XARM_MODE_JOINT_TORQUE = 6 + + +class XArmBackend: + """XArm-specific backend. + + Implements ManipulatorBackend protocol via duck typing. + No inheritance required - just matching method signatures. + + Unit conversions: + - Angles: XArm uses degrees, we use radians + - Positions: XArm uses mm, we use meters + - Velocities: XArm uses deg/s, we use rad/s + """ + + def __init__(self) -> None: + self._arm: XArmAPI | None = None + self._dof: int = 6 + self._control_mode: ControlMode = ControlMode.POSITION + + # ========================================================================= + # Connection + # ========================================================================= + + def connect(self, config: dict) -> bool: + """Connect to XArm via TCP/IP.""" + ip = config.get("ip", "192.168.1.185") + self._dof = config.get("dof", 6) + + try: + self._arm = XArmAPI(ip) + self._arm.connect() + + if not self._arm.connected: + print(f"ERROR: XArm at {ip} not reachable (connected=False)") + return False + + # Initialize to servo mode for high-frequency control + self._arm.set_mode(_XARM_MODE_SERVO_CARTESIAN) # Mode 1 = servo mode + self._arm.set_state(0) + self._control_mode = ControlMode.POSITION + + return True + except Exception as e: + print(f"ERROR: Failed to connect to XArm at {ip}: {e}") + return False + + def disconnect(self) -> None: + """Disconnect from XArm.""" + if self._arm: + self._arm.disconnect() + self._arm = None + + def is_connected(self) -> bool: + """Check if connected to XArm.""" + return self._arm is not None and self._arm.connected + + # ========================================================================= + # Info + # ========================================================================= + + def get_info(self) -> ManipulatorInfo: + """Get XArm information.""" + return ManipulatorInfo( + vendor="UFACTORY", + model=f"xArm{self._dof}", + dof=self._dof, + ) + + def get_dof(self) -> int: + """Get degrees of freedom.""" + return self._dof + + def get_limits(self) -> JointLimits: + """Get joint limits (default XArm limits).""" + # XArm typical joint limits (varies by joint, using conservative values) + limit = 2 * math.pi + return JointLimits( + position_lower=[-limit] * self._dof, + position_upper=[limit] * self._dof, + velocity_max=[math.pi] * self._dof, # ~180 deg/s + ) + + # ========================================================================= + # Control Mode + # ========================================================================= + + def set_control_mode(self, mode: ControlMode) -> bool: + """Set XArm control mode. + + Note: XArm is flexible and often accepts commands without explicit + mode switching, but some operations require the correct mode. + """ + if not self._arm: + return False + + mode_map = { + ControlMode.POSITION: _XARM_MODE_POSITION, + ControlMode.VELOCITY: _XARM_MODE_JOINT_VELOCITY, + ControlMode.TORQUE: _XARM_MODE_JOINT_TORQUE, + ControlMode.CARTESIAN: _XARM_MODE_SERVO_CARTESIAN, + ControlMode.CARTESIAN_VELOCITY: _XARM_MODE_CARTESIAN_VELOCITY, + } + + xarm_mode = mode_map.get(mode) + if xarm_mode is None: + return False + + code = self._arm.set_mode(xarm_mode) + if code == 0: + self._arm.set_state(0) + self._control_mode = mode + return True + return False + + def get_control_mode(self) -> ControlMode: + """Get current control mode.""" + return self._control_mode + + # ========================================================================= + # State Reading + # ========================================================================= + + def read_joint_positions(self) -> list[float]: + """Read joint positions (degrees -> radians).""" + if not self._arm: + return [0.0] * self._dof + + _, angles = self._arm.get_servo_angle() + if angles: + return [math.radians(a) for a in angles[: self._dof]] + return [0.0] * self._dof + + def read_joint_velocities(self) -> list[float]: + """Read joint velocities. + + Note: XArm doesn't provide real-time velocity feedback directly. + Returns zeros. For velocity estimation, use finite differences + on positions in the driver. + """ + return [0.0] * self._dof + + def read_joint_efforts(self) -> list[float]: + """Read joint torques in Nm.""" + if not self._arm: + return [0.0] * self._dof + + code, torques = self._arm.get_joints_torque() + if code == 0 and torques: + return list(torques[: self._dof]) + return [0.0] * self._dof + + def read_state(self) -> dict: + """Read robot state.""" + if not self._arm: + return {"state": 0, "mode": 0} + + return { + "state": self._arm.state, + "mode": self._arm.mode, + } + + def read_error(self) -> tuple[int, str]: + """Read error code and message.""" + if not self._arm: + return 0, "" + + code = self._arm.error_code + if code == 0: + return 0, "" + return code, f"XArm error {code}" + + # ========================================================================= + # Motion Control (Joint Space) + # ========================================================================= + + def write_joint_positions( + self, + positions: list[float], + velocity: float = 1.0, + ) -> bool: + """Write joint positions for servo mode (radians -> degrees). + + Uses set_servo_angle_j() for high-frequency servo control. + Requires mode 1 (servo mode) to be active. + + Args: + positions: Target positions in radians + velocity: Speed as fraction of max (0-1) - not used in servo mode + """ + if not self._arm: + return False + + # Convert radians to degrees + angles = [math.degrees(p) for p in positions] + + # Use set_servo_angle_j for high-frequency servo control (100Hz+) + # This only executes the last instruction, suitable for real-time control + return self._arm.set_servo_angle_j(angles, speed=100, mvacc=500) == 0 + + def write_joint_velocities(self, velocities: list[float]) -> bool: + """Write joint velocities (rad/s -> deg/s). + + Note: Requires velocity mode to be active. + """ + if not self._arm: + return False + + # Convert rad/s to deg/s + speeds = [math.degrees(v) for v in velocities] + return self._arm.vc_set_joint_velocity(speeds) == 0 + + def write_stop(self) -> bool: + """Emergency stop.""" + if not self._arm: + return False + return self._arm.emergency_stop() == 0 + + # ========================================================================= + # Servo Control + # ========================================================================= + + def write_enable(self, enable: bool) -> bool: + """Enable or disable servos.""" + if not self._arm: + return False + return self._arm.motion_enable(enable=enable) == 0 + + def read_enabled(self) -> bool: + """Check if servos are enabled.""" + if not self._arm: + return False + # XArm state 0 = ready/enabled + return self._arm.state == 0 + + def write_clear_errors(self) -> bool: + """Clear error state.""" + if not self._arm: + return False + return self._arm.clean_error() == 0 + + # ========================================================================= + # Cartesian Control (Optional) + # ========================================================================= + + def read_cartesian_position(self) -> dict[str, float] | None: + """Read end-effector pose (mm -> meters, degrees -> radians).""" + if not self._arm: + return None + + _, pose = self._arm.get_position() + if pose and len(pose) >= 6: + return { + "x": pose[0] / 1000.0, # mm -> m + "y": pose[1] / 1000.0, + "z": pose[2] / 1000.0, + "roll": math.radians(pose[3]), + "pitch": math.radians(pose[4]), + "yaw": math.radians(pose[5]), + } + return None + + def write_cartesian_position( + self, + pose: dict[str, float], + velocity: float = 1.0, + ) -> bool: + """Write end-effector pose (meters -> mm, radians -> degrees).""" + if not self._arm: + return False + + # Convert to XArm units + x = pose.get("x", 0) * 1000.0 # m -> mm + y = pose.get("y", 0) * 1000.0 + z = pose.get("z", 0) * 1000.0 + roll = math.degrees(pose.get("roll", 0)) + pitch = math.degrees(pose.get("pitch", 0)) + yaw = math.degrees(pose.get("yaw", 0)) + + # Speed in mm/s (max ~1000 mm/s) + speed = velocity * 500 + + return ( + self._arm.set_position( + x=x, + y=y, + z=z, + roll=roll, + pitch=pitch, + yaw=yaw, + speed=speed, + wait=False, + ) + == 0 + ) + + # ========================================================================= + # Gripper (Optional) + # ========================================================================= + + def read_gripper_position(self) -> float | None: + """Read gripper position (mm -> meters).""" + if not self._arm: + return None + + code, pos = self._arm.get_gripper_position() + if code == 0 and pos is not None: + return pos / 1000.0 # mm -> m + return None + + def write_gripper_position(self, position: float) -> bool: + """Write gripper position (meters -> mm).""" + if not self._arm: + return False + + pos_mm = position * 1000.0 # m -> mm + return self._arm.set_gripper_position(pos_mm) == 0 + + # ========================================================================= + # Force/Torque Sensor (Optional) + # ========================================================================= + + def read_force_torque(self) -> list[float] | None: + """Read F/T sensor data if available.""" + if not self._arm: + return None + + code, ft = self._arm.get_ft_sensor_data() + if code == 0 and ft: + return list(ft) + return None + + +__all__ = ["XArmBackend"] diff --git a/dimos/hardware/manipulators/xarm/blueprints.py b/dimos/hardware/manipulators/xarm/blueprints.py new file mode 100644 index 0000000000..5e25827050 --- /dev/null +++ b/dimos/hardware/manipulators/xarm/blueprints.py @@ -0,0 +1,191 @@ +# 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. + +"""Blueprints for XArm manipulator using B-lite architecture. + +Usage: + # Run via CLI: + dimos run xarm-servo # Driver only + dimos run xarm-trajectory # Driver + Joint trajectory controller + + # Or programmatically: + from dimos.hardware.manipulators.xarm.blueprints import xarm_trajectory + coordinator = xarm_trajectory.build() + coordinator.loop() +""" + +from dimos.core.blueprints import autoconnect +from dimos.core.transport import LCMTransport +from dimos.hardware.manipulators.xarm.arm import xarm as xarm_blueprint +from dimos.manipulation.control import cartesian_motion_controller, joint_trajectory_controller +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.sensor_msgs import ( + JointCommand, + JointState, + RobotState, +) +from dimos.msgs.trajectory_msgs import JointTrajectory + +# ============================================================================= +# XArm6 Servo Control Blueprint +# ============================================================================= +# XArm driver in servo mode - publishes joint states, accepts joint commands. +# ============================================================================= + +xarm_servo = xarm_blueprint( + ip="192.168.1.185", + dof=6, + control_rate=100.0, + monitor_rate=10.0, +).transports( + { + # Joint state feedback (position, velocity, effort) + ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), + # Robot state feedback (mode, state, errors) + ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), + } +) + +# ============================================================================= +# XArm5 Servo Control Blueprint +# ============================================================================= + +xarm5_servo = xarm_blueprint( + ip="192.168.1.185", + dof=5, + control_rate=100.0, + monitor_rate=10.0, +).transports( + { + ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), + ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), + } +) + +# ============================================================================= +# XArm7 Servo Control Blueprint +# ============================================================================= + +xarm7_servo = xarm_blueprint( + ip="192.168.1.185", + dof=7, + control_rate=100.0, + monitor_rate=10.0, +).transports( + { + ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), + ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), + } +) + +# ============================================================================= +# XArm Trajectory Control Blueprint (Driver + Trajectory Controller) +# ============================================================================= +# Combines XArm driver with JointTrajectoryController for trajectory execution. +# The controller receives JointTrajectory messages and executes them. +# ============================================================================= + +xarm_trajectory = autoconnect( + xarm_blueprint( + ip="192.168.2.235", + dof=7, + control_rate=100.0, # Higher rate for smoother trajectory execution + monitor_rate=10.0, + ), + joint_trajectory_controller( + control_frequency=100.0, + ), +).transports( + { + # Shared topics between driver and controller + ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), + ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), + ("joint_position_command", JointCommand): LCMTransport( + "/xarm/joint_position_command", JointCommand + ), + # Trajectory input topic + ("trajectory", JointTrajectory): LCMTransport("/trajectory", JointTrajectory), + } +) + +# ============================================================================= +# XArm7 Trajectory Control Blueprint +# ============================================================================= + +xarm7_trajectory = autoconnect( + xarm_blueprint( + ip="192.168.1.185", + dof=7, + control_rate=100.0, + monitor_rate=10.0, + ), + joint_trajectory_controller( + control_frequency=100.0, + ), +).transports( + { + ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), + ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), + ("joint_position_command", JointCommand): LCMTransport( + "/xarm/joint_position_command", JointCommand + ), + ("trajectory", JointTrajectory): LCMTransport("/trajectory", JointTrajectory), + } +) + +# ============================================================================= +# XArm Cartesian Control Blueprint (Driver + Cartesian Controller) +# ============================================================================= +# Combines XArm driver with CartesianMotionController for Cartesian space control. +# The controller receives target_pose and converts to joint commands via IK. +# ============================================================================= + +xarm_cartesian = autoconnect( + xarm_blueprint( + ip="192.168.1.185", + dof=6, + control_rate=100.0, + monitor_rate=10.0, + ), + cartesian_motion_controller( + control_frequency=20.0, + position_kp=5.0, + position_ki=0.0, + position_kd=0.1, + max_linear_velocity=0.2, + max_angular_velocity=1.0, + ), +).transports( + { + # Shared topics between driver and controller + ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), + ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), + ("joint_position_command", JointCommand): LCMTransport( + "/xarm/joint_position_command", JointCommand + ), + # Controller-specific topics + ("target_pose", PoseStamped): LCMTransport("/target_pose", PoseStamped), + ("current_pose", PoseStamped): LCMTransport("/xarm/current_pose", PoseStamped), + } +) + + +__all__ = [ + "xarm5_servo", + "xarm7_servo", + "xarm7_trajectory", + "xarm_cartesian", + "xarm_servo", + "xarm_trajectory", +] From a95037c9d4a22a07d89e563d63b227e9edd02f35 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 6 Jan 2026 18:14:12 -0800 Subject: [PATCH 05/42] removed type checking for old armdriver spec from the cartesian controller --- .../control/servo_control/cartesian_motion_controller.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/dimos/manipulation/control/servo_control/cartesian_motion_controller.py b/dimos/manipulation/control/servo_control/cartesian_motion_controller.py index cfbdb77cbf..f5a0810803 100644 --- a/dimos/manipulation/control/servo_control/cartesian_motion_controller.py +++ b/dimos/manipulation/control/servo_control/cartesian_motion_controller.py @@ -34,7 +34,6 @@ from dimos.core import In, Module, Out, rpc from dimos.core.module import ModuleConfig -from dimos.hardware.manipulators.xarm.spec import ArmDriverSpec from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Twist, Vector3 from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState from dimos.utils.logging_config import setup_logger @@ -90,7 +89,7 @@ class CartesianMotionController(Module): 5. Publishing joint commands to the driver The controller is hardware-agnostic: it works with any arm driver that - implements the ArmDriverSpec protocol (provides IK/FK RPC methods). + provides IK/FK RPC methods and JointState/RobotState outputs. """ default_config = CartesianMotionControllerConfig @@ -112,12 +111,12 @@ class CartesianMotionController(Module): cartesian_velocity: Out[Twist] = None # type: ignore[assignment] current_pose: Out[PoseStamped] = None # type: ignore[assignment] - def __init__(self, arm_driver: ArmDriverSpec | None = None, *args: Any, **kwargs: Any) -> None: + def __init__(self, arm_driver: Any = None, *args: Any, **kwargs: Any) -> None: """ Initialize the Cartesian motion controller. Args: - arm_driver: (Optional) Hardware driver implementing ArmDriverSpec protocol. + arm_driver: (Optional) Hardware driver reference (legacy mode). When using blueprints, this is resolved automatically via rpc_calls. """ super().__init__(*args, **kwargs) From 885cc4b92137bb67ebcfcffc5ee6603d70be31b5 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 6 Jan 2026 18:15:08 -0800 Subject: [PATCH 06/42] replicated piper driver to meet the new architecture --- dimos/hardware/manipulators/piper/__init__.py | 34 ++ dimos/hardware/manipulators/piper/arm.py | 431 +++++++++++++++ dimos/hardware/manipulators/piper/backend.py | 500 ++++++++++++++++++ .../hardware/manipulators/piper/blueprints.py | 141 +++++ 4 files changed, 1106 insertions(+) create mode 100644 dimos/hardware/manipulators/piper/__init__.py create mode 100644 dimos/hardware/manipulators/piper/arm.py create mode 100644 dimos/hardware/manipulators/piper/backend.py create mode 100644 dimos/hardware/manipulators/piper/blueprints.py diff --git a/dimos/hardware/manipulators/piper/__init__.py b/dimos/hardware/manipulators/piper/__init__.py new file mode 100644 index 0000000000..3a3c1bbe11 --- /dev/null +++ b/dimos/hardware/manipulators/piper/__init__.py @@ -0,0 +1,34 @@ +# 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. + +"""Piper manipulator driver. + +Usage: + >>> from dimos.hardware.manipulators.piper import Piper + >>> arm = Piper(can_port="can0") + >>> arm.start() + >>> arm.enable_servos() + >>> arm.move_joint([0, 0, 0, 0, 0, 0]) + +Testing: + >>> from dimos.hardware.manipulators.mock import MockBackend + >>> from dimos.hardware.manipulators.piper import Piper + >>> arm = Piper(backend=MockBackend()) + >>> arm.start() # No hardware needed! +""" + +from dimos.hardware.manipulators.piper.arm import Piper, PiperConfig, piper +from dimos.hardware.manipulators.piper.backend import PiperBackend + +__all__ = ["Piper", "PiperBackend", "PiperConfig", "piper"] diff --git a/dimos/hardware/manipulators/piper/arm.py b/dimos/hardware/manipulators/piper/arm.py new file mode 100644 index 0000000000..1dd0207c35 --- /dev/null +++ b/dimos/hardware/manipulators/piper/arm.py @@ -0,0 +1,431 @@ +# 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. + +"""Piper manipulator driver. + +This driver: +- Uses PiperBackend by default (injectable for testing) +- Has full control over threading and timing +- Can be customized without affecting other arms +""" + +from dataclasses import dataclass, field +import threading +import time + +from dimos.core import In, Module, ModuleConfig, Out, rpc +from dimos.hardware.manipulators.piper.backend import PiperBackend +from dimos.hardware.manipulators.spec import ( + ControlMode, + DriverStatus, + JointLimits, + ManipulatorBackend, + ManipulatorInfo, + default_base_transform, +) +from dimos.msgs.geometry_msgs import Transform +from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState + +# ============================================================================ +# CONFIGURATION +# ============================================================================ + + +@dataclass +class PiperConfig(ModuleConfig): + """Piper-specific configuration. + + Type-safe config with Piper-specific fields. + """ + + can_port: str = "can0" + dof: int = 6 # Piper is always 6-DOF + control_rate: float = 100.0 # Hz + monitor_rate: float = 10.0 # Hz + has_gripper: bool = False + base_frame_id: str = "base_link" + base_transform: Transform | None = field(default_factory=default_base_transform) + + +# ============================================================================ +# DRIVER +# ============================================================================ + + +class Piper(Module[PiperConfig]): + """Piper manipulator driver. + + Features: + - Injectable backend (defaults to PiperBackend) + - 100Hz control loop for joint state + - 10Hz monitor loop for robot state + - Full RPC interface for control + + Example: + >>> arm = Piper(can_port="can0") + >>> arm.start() + >>> arm.enable_servos() + >>> arm.move_joint([0, 0, 0, 0, 0, 0]) + + Testing: + >>> from dimos.hardware.manipulators.mock import MockBackend + >>> arm = Piper(backend=MockBackend()) + >>> arm.start() # No hardware needed! + """ + + # Input topics (commands from controllers) + joint_position_command: In[JointCommand] + joint_velocity_command: In[JointCommand] + + # Output topics + joint_state: Out[JointState] + robot_state: Out[RobotState] + + # Config + config: PiperConfig + default_config = PiperConfig + + def __init__( + self, + backend: ManipulatorBackend | None = None, + *args, + **kwargs, + ) -> None: + super().__init__(*args, **kwargs) + + # Backend is injectable for testing + self.backend: ManipulatorBackend = backend or PiperBackend() + + # Threading state + self._running = False + self._control_thread: threading.Thread | None = None + self._monitor_thread: threading.Thread | None = None + + # Cached info + self._dof: int = self.config.dof + self._joint_names: list[str] = [f"joint{i + 1}" for i in range(self._dof)] + + # Auto-connect on initialization + self._auto_start() + + def _auto_start(self) -> None: + """Auto-connect to hardware on initialization.""" + config_dict = { + "can_port": self.config.can_port, + "dof": self.config.dof, + } + + if not self.backend.connect(config_dict): + print(f"WARNING: Failed to connect to Piper on {self.config.can_port}") + return + + # Update DOF from backend (in case it differs) + self._dof = self.backend.get_dof() + self._joint_names = [f"joint{i + 1}" for i in range(self._dof)] + + # Start threads + self._running = True + + self._control_thread = threading.Thread( + target=self._control_loop, + name="Piper-Control", + daemon=True, + ) + self._monitor_thread = threading.Thread( + target=self._monitor_loop, + name="Piper-Monitor", + daemon=True, + ) + + self._control_thread.start() + self._monitor_thread.start() + + print(f"Piper connected on {self.config.can_port}, DOF={self._dof}") + + def _subscribe_to_commands(self) -> None: + """Subscribe to command input topics.""" + try: + if self.joint_position_command: + self.joint_position_command.subscribe(self._on_joint_position_command) + print("Subscribed to joint_position_command") + except Exception as e: + print(f"Could not subscribe to joint_position_command: {e}") + + try: + if self.joint_velocity_command: + self.joint_velocity_command.subscribe(self._on_joint_velocity_command) + print("Subscribed to joint_velocity_command") + except Exception as e: + print(f"Could not subscribe to joint_velocity_command: {e}") + + def _on_joint_position_command(self, cmd: JointCommand) -> None: + """Handle incoming joint position command.""" + if not self._running: + return + + positions = list(cmd.positions) if cmd.positions else [] + if len(positions) != self._dof: + print(f"WARNING: Position command has {len(positions)} joints, expected {self._dof}") + return + + self.backend.write_joint_positions(positions) + + def _on_joint_velocity_command(self, cmd: JointCommand) -> None: + """Handle incoming joint velocity command.""" + if not self._running: + return + + # JointCommand uses 'positions' field for velocity commands too + velocities = list(cmd.positions) if cmd.positions else [] + if len(velocities) != self._dof: + print(f"WARNING: Velocity command has {len(velocities)} joints, expected {self._dof}") + return + + self.backend.write_joint_velocities(velocities) + + # ========================================================================= + # Lifecycle + # ========================================================================= + + @rpc + def start(self) -> DriverStatus: + """Connect to Piper and start control loops (if not already running).""" + super().start() # Important: sets up transports + + if self._running: + # Already connected, just subscribe to commands + self._subscribe_to_commands() + return DriverStatus.CONNECTED + + self._auto_start() + self._subscribe_to_commands() + return DriverStatus.CONNECTED if self._running else DriverStatus.ERROR + + @rpc + def stop(self) -> None: + """Stop Piper and disconnect.""" + self._running = False + + # Stop motion first + try: + self.backend.write_stop() + except Exception: + pass + + # Wait for threads + if self._control_thread and self._control_thread.is_alive(): + self._control_thread.join(timeout=1.0) + if self._monitor_thread and self._monitor_thread.is_alive(): + self._monitor_thread.join(timeout=1.0) + + # Disconnect + try: + self.backend.disconnect() + except Exception: + pass + + super().stop() + + # ========================================================================= + # Control Loop (100Hz) + # ========================================================================= + + def _control_loop(self) -> None: + """High-frequency loop for joint state publishing.""" + period = 1.0 / self.config.control_rate + + while self._running: + start = time.perf_counter() + + try: + self._publish_joint_state() + except Exception as e: + print(f"Piper control loop error: {e}") + + # Rate control + elapsed = time.perf_counter() - start + if elapsed < period: + time.sleep(period - elapsed) + + def _publish_joint_state(self) -> None: + """Read and publish joint state.""" + positions = self.backend.read_joint_positions() + velocities = self.backend.read_joint_velocities() + efforts = self.backend.read_joint_efforts() + + msg = JointState( + ts=time.time(), + frame_id="joint_state", + name=self._joint_names, + position=positions, + velocity=velocities, + effort=efforts, + ) + self.joint_state.publish(msg) + + # ========================================================================= + # Monitor Loop (10Hz) + # ========================================================================= + + def _monitor_loop(self) -> None: + """Low-frequency loop for robot state monitoring.""" + period = 1.0 / self.config.monitor_rate + + while self._running: + start = time.perf_counter() + + try: + self._publish_robot_state() + except Exception as e: + print(f"Piper monitor loop error: {e}") + + # Rate control + elapsed = time.perf_counter() - start + if elapsed < period: + time.sleep(period - elapsed) + + def _publish_robot_state(self) -> None: + """Read and publish robot state.""" + state = self.backend.read_state() + error_code, _ = self.backend.read_error() + + msg = RobotState( + state=state.get("state", 0), + mode=state.get("mode", 0), + error_code=error_code, + warn_code=0, + ) + self.robot_state.publish(msg) + + # ========================================================================= + # RPC Methods - Servo Control + # ========================================================================= + + @rpc + def enable_servos(self) -> bool: + """Enable motor control.""" + return self.backend.write_enable(True) + + @rpc + def disable_servos(self) -> bool: + """Disable motor control.""" + return self.backend.write_enable(False) + + @rpc + def clear_errors(self) -> bool: + """Clear error state.""" + return self.backend.write_clear_errors() + + # ========================================================================= + # RPC Methods - Mode Control + # ========================================================================= + + @rpc + def set_control_mode(self, mode: ControlMode) -> bool: + """Set control mode (position, velocity, etc).""" + return self.backend.set_control_mode(mode) + + @rpc + def get_control_mode(self) -> ControlMode: + """Get current control mode.""" + return self.backend.get_control_mode() + + # ========================================================================= + # RPC Methods - Joint Space Motion + # ========================================================================= + + @rpc + def move_joint(self, positions: list[float], velocity: float = 0.5) -> bool: + """Move to joint positions (radians).""" + return self.backend.write_joint_positions(positions, velocity) + + @rpc + def move_velocity(self, velocities: list[float]) -> bool: + """Move with joint velocities (rad/s). + + Note: Piper doesn't support native velocity control. + This will return False. + """ + return self.backend.write_joint_velocities(velocities) + + @rpc + def stop_motion(self) -> bool: + """Stop all motion.""" + return self.backend.write_stop() + + # ========================================================================= + # RPC Methods - Cartesian Motion + # ========================================================================= + + @rpc + def get_cartesian_position(self) -> dict[str, float] | None: + """Get end-effector pose (x, y, z in meters; roll, pitch, yaw in radians).""" + return self.backend.read_cartesian_position() + + @rpc + def move_cartesian( + self, + pose: dict[str, float], + velocity: float = 0.5, + ) -> bool: + """Move to cartesian pose. + + Args: + pose: Dict with x, y, z (meters), roll, pitch, yaw (radians) + velocity: Speed as fraction of max (0-1) + + Note: Piper may not support direct cartesian control. + """ + return self.backend.write_cartesian_position(pose, velocity) + + # ========================================================================= + # RPC Methods - Info + # ========================================================================= + + @rpc + def get_info(self) -> ManipulatorInfo: + """Get manipulator info.""" + return self.backend.get_info() + + @rpc + def get_limits(self) -> JointLimits: + """Get joint limits.""" + return self.backend.get_limits() + + # ========================================================================= + # RPC Methods - Optional Features (Gripper) + # ========================================================================= + + @rpc + def get_gripper_position(self) -> float | None: + """Get gripper position (meters). None if no gripper.""" + if not self.config.has_gripper: + return None + return self.backend.read_gripper_position() + + @rpc + def set_gripper_position(self, position: float) -> bool: + """Set gripper position (meters).""" + if not self.config.has_gripper: + return False + return self.backend.write_gripper_position(position) + + +# ============================================================================ +# EXPORTS +# ============================================================================ + +piper = Piper.blueprint + +__all__ = ["Piper", "PiperConfig", "piper"] diff --git a/dimos/hardware/manipulators/piper/backend.py b/dimos/hardware/manipulators/piper/backend.py new file mode 100644 index 0000000000..01e52c2645 --- /dev/null +++ b/dimos/hardware/manipulators/piper/backend.py @@ -0,0 +1,500 @@ +# 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. + +"""Piper backend - implements ManipulatorBackend protocol. + +Handles all Piper SDK communication and unit conversion. +""" + +import math +import time +from typing import Any + +from dimos.hardware.manipulators.spec import ( + ControlMode, + JointLimits, + ManipulatorInfo, +) + +# Unit conversion constants +# Piper uses 0.001 degrees internally +RAD_TO_PIPER = 57295.7795 # radians to Piper units (0.001 degrees) +PIPER_TO_RAD = 1.0 / RAD_TO_PIPER # Piper units to radians + + +class PiperBackend: + """Piper-specific backend. + + Implements ManipulatorBackend protocol via duck typing. + No inheritance required - just matching method signatures. + + Unit conversions: + - Angles: Piper uses 0.001 degrees, we use radians + - Velocities: Piper uses internal units, we use rad/s + """ + + def __init__(self) -> None: + self._sdk: Any = None + self._dof: int = 6 # Piper is always 6-DOF + self._connected: bool = False + self._enabled: bool = False + self._control_mode: ControlMode = ControlMode.POSITION + + # ========================================================================= + # Connection + # ========================================================================= + + def connect(self, config: dict) -> bool: + """Connect to Piper via CAN bus.""" + can_port = config.get("can_port", "can0") + + try: + from piper_sdk import C_PiperInterface_V2 + + self._sdk = C_PiperInterface_V2( + can_name=can_port, + judge_flag=True, # Enable safety checks + can_auto_init=True, # Let SDK handle CAN initialization + dh_is_offset=False, + ) + + # Connect to CAN port + self._sdk.ConnectPort(piper_init=True, start_thread=True) + + # Wait for initialization + time.sleep(0.025) + + # Check connection by trying to get status + status = self._sdk.GetArmStatus() + if status is not None: + self._connected = True + print(f"Piper connected via CAN port {can_port}") + return True + else: + print(f"ERROR: Failed to connect to Piper on {can_port} - no status received") + return False + + except ImportError: + print("ERROR: Piper SDK not installed. Please install piper_sdk") + return False + except Exception as e: + print(f"ERROR: Failed to connect to Piper on {can_port}: {e}") + return False + + def disconnect(self) -> None: + """Disconnect from Piper.""" + if self._sdk: + try: + if self._enabled: + self._sdk.DisablePiper() + self._enabled = False + self._sdk.DisconnectPort() + except Exception: + pass + finally: + self._sdk = None + self._connected = False + + def is_connected(self) -> bool: + """Check if connected to Piper.""" + if not self._connected or not self._sdk: + return False + + try: + status = self._sdk.GetArmStatus() + return status is not None + except Exception: + return False + + # ========================================================================= + # Info + # ========================================================================= + + def get_info(self) -> ManipulatorInfo: + """Get Piper information.""" + firmware_version = None + if self._sdk: + try: + firmware_version = self._sdk.GetPiperFirmwareVersion() + except Exception: + pass + + return ManipulatorInfo( + vendor="Agilex", + model="Piper", + dof=self._dof, + firmware_version=firmware_version, + ) + + def get_dof(self) -> int: + """Get degrees of freedom.""" + return self._dof + + def get_limits(self) -> JointLimits: + """Get joint limits.""" + # Piper joint limits (approximate, in radians) + lower = [-3.14, -2.35, -2.35, -3.14, -2.35, -3.14] + upper = [3.14, 2.35, 2.35, 3.14, 2.35, 3.14] + max_vel = [math.pi] * self._dof # ~180 deg/s + + return JointLimits( + position_lower=lower, + position_upper=upper, + velocity_max=max_vel, + ) + + # ========================================================================= + # Control Mode + # ========================================================================= + + def set_control_mode(self, mode: ControlMode) -> bool: + """Set Piper control mode via MotionCtrl_2.""" + if not self._sdk: + return False + + # Piper move modes: 0x01=position, 0x02=velocity + move_mode = 0x01 # Default position mode + if mode == ControlMode.VELOCITY: + move_mode = 0x02 + + try: + self._sdk.MotionCtrl_2( + ctrl_mode=0x01, # CAN control mode + move_mode=move_mode, + move_spd_rate_ctrl=50, # Speed rate (0-100) + is_mit_mode=0x00, # Not MIT mode + ) + self._control_mode = mode + return True + except Exception: + return False + + def get_control_mode(self) -> ControlMode: + """Get current control mode.""" + return self._control_mode + + # ========================================================================= + # State Reading + # ========================================================================= + + def read_joint_positions(self) -> list[float]: + """Read joint positions (Piper units -> radians).""" + if not self._sdk: + return [0.0] * self._dof + + try: + joint_msgs = self._sdk.GetArmJointMsgs() + if joint_msgs and joint_msgs.joint_state: + js = joint_msgs.joint_state + return [ + js.joint_1 * PIPER_TO_RAD, + js.joint_2 * PIPER_TO_RAD, + js.joint_3 * PIPER_TO_RAD, + js.joint_4 * PIPER_TO_RAD, + js.joint_5 * PIPER_TO_RAD, + js.joint_6 * PIPER_TO_RAD, + ] + except Exception: + pass + return [0.0] * self._dof + + def read_joint_velocities(self) -> list[float]: + """Read joint velocities. + + Note: Piper doesn't provide real-time velocity feedback. + Returns zeros. For velocity estimation, use finite differences. + """ + return [0.0] * self._dof + + def read_joint_efforts(self) -> list[float]: + """Read joint efforts/torques. + + Note: Piper doesn't provide torque feedback by default. + """ + return [0.0] * self._dof + + def read_state(self) -> dict: + """Read robot state.""" + if not self._sdk: + return {"state": 0, "mode": 0} + + try: + status = self._sdk.GetArmStatus() + if status and status.arm_status: + arm_status = status.arm_status + error_code = getattr(arm_status, "err_code", 0) + state = 2 if error_code != 0 else 0 # 2=error, 0=idle + return { + "state": state, + "mode": 0, # Piper doesn't expose mode + "error_code": error_code, + } + except Exception: + pass + + return {"state": 0, "mode": 0} + + def read_error(self) -> tuple[int, str]: + """Read error code and message.""" + if not self._sdk: + return 0, "" + + try: + status = self._sdk.GetArmStatus() + if status and status.arm_status: + error_code = getattr(status.arm_status, "err_code", 0) + if error_code == 0: + return 0, "" + + # Piper error codes + error_map = { + 1: "Communication error", + 2: "Motor error", + 3: "Encoder error", + 4: "Overtemperature", + 5: "Overcurrent", + 6: "Joint limit error", + 7: "Emergency stop", + 8: "Power error", + } + return error_code, error_map.get(error_code, f"Unknown error {error_code}") + except Exception: + pass + + return 0, "" + + # ========================================================================= + # Motion Control (Joint Space) + # ========================================================================= + + def write_joint_positions( + self, + positions: list[float], + velocity: float = 1.0, + ) -> bool: + """Write joint positions (radians -> Piper units). + + Args: + positions: Target positions in radians + velocity: Speed as fraction of max (0-1) + """ + if not self._sdk: + return False + + # Convert radians to Piper units (0.001 degrees) + piper_joints = [round(rad * RAD_TO_PIPER) for rad in positions] + + # Set speed rate if not full speed + if velocity < 1.0: + speed_rate = int(velocity * 100) + try: + self._sdk.MotionCtrl_2( + ctrl_mode=0x01, + move_mode=0x01, + move_spd_rate_ctrl=speed_rate, + is_mit_mode=0x00, + ) + except Exception: + pass + + try: + self._sdk.JointCtrl( + piper_joints[0], + piper_joints[1], + piper_joints[2], + piper_joints[3], + piper_joints[4], + piper_joints[5], + ) + return True + except Exception as e: + print(f"Piper joint control error: {e}") + return False + + def write_joint_velocities(self, velocities: list[float]) -> bool: + """Write joint velocities. + + Note: Piper doesn't have native velocity control at SDK level. + Returns False - the driver should implement this via position integration. + """ + return False + + def write_stop(self) -> bool: + """Emergency stop.""" + if not self._sdk: + return False + + try: + if hasattr(self._sdk, "EmergencyStop"): + self._sdk.EmergencyStop() + return True + except Exception: + pass + + # Fallback: disable arm + return self.write_enable(False) + + # ========================================================================= + # Servo Control + # ========================================================================= + + def write_enable(self, enable: bool) -> bool: + """Enable or disable servos.""" + if not self._sdk: + return False + + try: + if enable: + # Enable with retries + attempts = 0 + max_attempts = 100 + while not self._sdk.EnablePiper() and attempts < max_attempts: + time.sleep(0.01) + attempts += 1 + + if attempts < max_attempts: + self._enabled = True + # Set control mode + self._sdk.MotionCtrl_2( + ctrl_mode=0x01, + move_mode=0x01, + move_spd_rate_ctrl=30, + is_mit_mode=0x00, + ) + return True + return False + else: + self._sdk.DisablePiper() + self._enabled = False + return True + except Exception: + return False + + def read_enabled(self) -> bool: + """Check if servos are enabled.""" + return self._enabled + + def write_clear_errors(self) -> bool: + """Clear error state.""" + if not self._sdk: + return False + + try: + if hasattr(self._sdk, "ClearError"): + self._sdk.ClearError() + return True + except Exception: + pass + + # Alternative: disable and re-enable + self.write_enable(False) + time.sleep(0.1) + return self.write_enable(True) + + # ========================================================================= + # Cartesian Control (Optional) + # ========================================================================= + + def read_cartesian_position(self) -> dict[str, float] | None: + """Read end-effector pose. + + Note: Piper may not support direct cartesian feedback. + Returns None if not available. + """ + if not self._sdk: + return None + + try: + if hasattr(self._sdk, "GetArmEndPoseMsgs"): + pose_msgs = self._sdk.GetArmEndPoseMsgs() + if pose_msgs and pose_msgs.end_pose: + ep = pose_msgs.end_pose + return { + "x": ep.X_axis / 1000.0, # mm -> m + "y": ep.Y_axis / 1000.0, + "z": ep.Z_axis / 1000.0, + "roll": ep.RX_axis * PIPER_TO_RAD, + "pitch": ep.RY_axis * PIPER_TO_RAD, + "yaw": ep.RZ_axis * PIPER_TO_RAD, + } + except Exception: + pass + + return None + + def write_cartesian_position( + self, + pose: dict[str, float], + velocity: float = 1.0, + ) -> bool: + """Write end-effector pose. + + Note: Piper may not support direct cartesian control. + """ + # Cartesian control not commonly supported in Piper SDK + return False + + # ========================================================================= + # Gripper (Optional) + # ========================================================================= + + def read_gripper_position(self) -> float | None: + """Read gripper position (percentage -> meters).""" + if not self._sdk: + return None + + try: + if hasattr(self._sdk, "GetArmGripperMsgs"): + gripper_msgs = self._sdk.GetArmGripperMsgs() + if gripper_msgs and gripper_msgs.gripper_state: + # Piper gripper position is 0-100 percentage + # Convert to meters (assume max opening 0.08m) + pos = gripper_msgs.gripper_state.grippers_angle + return float(pos / 100.0) * 0.08 + except Exception: + pass + + return None + + def write_gripper_position(self, position: float) -> bool: + """Write gripper position (meters -> percentage).""" + if not self._sdk: + return False + + try: + if hasattr(self._sdk, "GripperCtrl"): + # Convert meters to percentage (0-100) + # Assume max opening 0.08m + percentage = int((position / 0.08) * 100) + percentage = max(0, min(100, percentage)) + self._sdk.GripperCtrl(percentage, 1000, 0x01, 0) + return True + except Exception: + pass + + return False + + # ========================================================================= + # Force/Torque Sensor (Optional) + # ========================================================================= + + def read_force_torque(self) -> list[float] | None: + """Read F/T sensor data. + + Note: Piper doesn't typically have F/T sensor. + """ + return None + + +__all__ = ["PiperBackend"] diff --git a/dimos/hardware/manipulators/piper/blueprints.py b/dimos/hardware/manipulators/piper/blueprints.py new file mode 100644 index 0000000000..c3858ee0eb --- /dev/null +++ b/dimos/hardware/manipulators/piper/blueprints.py @@ -0,0 +1,141 @@ +# 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. + +"""Blueprints for Piper manipulator using B-lite architecture. + +Usage: + # Run via CLI: + dimos run piper-servo # Driver only + dimos run piper-trajectory # Driver + Joint trajectory controller + + # Or programmatically: + from dimos.hardware.manipulators.piper.blueprints import piper_trajectory + coordinator = piper_trajectory.build() + coordinator.loop() +""" + +from dimos.core.blueprints import autoconnect +from dimos.core.transport import LCMTransport +from dimos.hardware.manipulators.piper.arm import piper as piper_blueprint +from dimos.manipulation.control import joint_trajectory_controller +from dimos.msgs.sensor_msgs import ( + JointCommand, + JointState, + RobotState, +) +from dimos.msgs.trajectory_msgs import JointTrajectory + +# ============================================================================= +# Piper Servo Control Blueprint +# ============================================================================= +# Piper driver in servo mode - publishes joint states, accepts joint commands. +# ============================================================================= + +piper_servo = piper_blueprint( + can_port="can0", + dof=6, + control_rate=100.0, + monitor_rate=10.0, +).transports( + { + # Joint state feedback (position, velocity, effort) + ("joint_state", JointState): LCMTransport("/piper/joint_states", JointState), + # Robot state feedback (mode, state, errors) + ("robot_state", RobotState): LCMTransport("/piper/robot_state", RobotState), + } +) + +# ============================================================================= +# Piper Servo with Gripper Blueprint +# ============================================================================= + +piper_servo_gripper = piper_blueprint( + can_port="can0", + dof=6, + control_rate=100.0, + monitor_rate=10.0, + has_gripper=True, +).transports( + { + ("joint_state", JointState): LCMTransport("/piper/joint_states", JointState), + ("robot_state", RobotState): LCMTransport("/piper/robot_state", RobotState), + } +) + +# ============================================================================= +# Piper Trajectory Control Blueprint (Driver + Trajectory Controller) +# ============================================================================= +# Combines Piper driver with JointTrajectoryController for trajectory execution. +# The controller receives JointTrajectory messages and executes them. +# ============================================================================= + +piper_trajectory = autoconnect( + piper_blueprint( + can_port="can0", + dof=6, + control_rate=100.0, # Higher rate for smoother trajectory execution + monitor_rate=10.0, + ), + joint_trajectory_controller( + control_frequency=100.0, + ), +).transports( + { + # Shared topics between driver and controller + ("joint_state", JointState): LCMTransport("/piper/joint_states", JointState), + ("robot_state", RobotState): LCMTransport("/piper/robot_state", RobotState), + ("joint_position_command", JointCommand): LCMTransport( + "/piper/joint_position_command", JointCommand + ), + # Trajectory input topic + ("trajectory", JointTrajectory): LCMTransport("/trajectory", JointTrajectory), + } +) + +# ============================================================================= +# Piper Dual Arm Blueprint (for dual-arm robots) +# ============================================================================= + +piper_left = piper_blueprint( + can_port="can0", + dof=6, + control_rate=100.0, + monitor_rate=10.0, +).transports( + { + ("joint_state", JointState): LCMTransport("/piper/left/joint_states", JointState), + ("robot_state", RobotState): LCMTransport("/piper/left/robot_state", RobotState), + } +) + +piper_right = piper_blueprint( + can_port="can1", + dof=6, + control_rate=100.0, + monitor_rate=10.0, +).transports( + { + ("joint_state", JointState): LCMTransport("/piper/right/joint_states", JointState), + ("robot_state", RobotState): LCMTransport("/piper/right/robot_state", RobotState), + } +) + + +__all__ = [ + "piper_left", + "piper_right", + "piper_servo", + "piper_servo_gripper", + "piper_trajectory", +] From 1472ecd9da558357541ada077b028fbbaf1004c8 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 6 Jan 2026 18:15:25 -0800 Subject: [PATCH 07/42] added mock backend --- dimos/hardware/manipulators/mock/__init__.py | 28 +++ dimos/hardware/manipulators/mock/backend.py | 252 +++++++++++++++++++ 2 files changed, 280 insertions(+) create mode 100644 dimos/hardware/manipulators/mock/__init__.py create mode 100644 dimos/hardware/manipulators/mock/backend.py diff --git a/dimos/hardware/manipulators/mock/__init__.py b/dimos/hardware/manipulators/mock/__init__.py new file mode 100644 index 0000000000..87428973a4 --- /dev/null +++ b/dimos/hardware/manipulators/mock/__init__.py @@ -0,0 +1,28 @@ +# 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. + +"""Mock backend for testing manipulator drivers without hardware. + +Usage: + >>> from dimos.hardware.manipulators.xarm import XArm + >>> from dimos.hardware.manipulators.mock import MockBackend + >>> arm = XArm(backend=MockBackend()) + >>> arm.start() # No hardware needed! + >>> arm.move_joint([0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) + >>> assert arm.backend.read_joint_positions() == [0.1, 0.2, 0.3, 0.4, 0.5, 0.6] +""" + +from dimos.hardware.manipulators.mock.backend import MockBackend + +__all__ = ["MockBackend"] diff --git a/dimos/hardware/manipulators/mock/backend.py b/dimos/hardware/manipulators/mock/backend.py new file mode 100644 index 0000000000..da3696421e --- /dev/null +++ b/dimos/hardware/manipulators/mock/backend.py @@ -0,0 +1,252 @@ +# 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. + +"""Mock backend for testing - no hardware required. + +Usage: + >>> from dimos.hardware.manipulators.xarm import XArm + >>> from dimos.hardware.manipulators.mock import MockBackend + >>> arm = XArm(backend=MockBackend()) + >>> arm.start() # No hardware! +""" + +import math + +from dimos.hardware.manipulators.spec import ( + ControlMode, + JointLimits, + ManipulatorInfo, +) + + +class MockBackend: + """Fake backend for unit tests. + + Implements ManipulatorBackend protocol with in-memory state. + Useful for: + - Unit testing driver logic without hardware + - Integration testing with predictable behavior + - Development without physical robot + """ + + def __init__(self, dof: int = 6) -> None: + self._dof = dof + self._positions = [0.0] * dof + self._velocities = [0.0] * dof + self._efforts = [0.0] * dof + self._enabled = False + self._connected = False + self._control_mode = ControlMode.POSITION + self._cartesian_position: dict[str, float] = { + "x": 0.3, + "y": 0.0, + "z": 0.3, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + } + self._gripper_position: float = 0.0 + self._error_code: int = 0 + self._error_message: str = "" + + # ========================================================================= + # Connection + # ========================================================================= + + def connect(self, config: dict) -> bool: + """Simulate connection.""" + self._dof = config.get("dof", self._dof) + self._positions = [0.0] * self._dof + self._velocities = [0.0] * self._dof + self._efforts = [0.0] * self._dof + self._connected = True + return True + + def disconnect(self) -> None: + """Simulate disconnection.""" + self._connected = False + + def is_connected(self) -> bool: + """Check mock connection status.""" + return self._connected + + # ========================================================================= + # Info + # ========================================================================= + + def get_info(self) -> ManipulatorInfo: + """Return mock info.""" + return ManipulatorInfo( + vendor="Mock", + model="MockArm", + dof=self._dof, + firmware_version="1.0.0", + serial_number="MOCK-001", + ) + + def get_dof(self) -> int: + """Return DOF.""" + return self._dof + + def get_limits(self) -> JointLimits: + """Return mock joint limits.""" + return JointLimits( + position_lower=[-math.pi] * self._dof, + position_upper=[math.pi] * self._dof, + velocity_max=[1.0] * self._dof, + ) + + # ========================================================================= + # Control Mode + # ========================================================================= + + def set_control_mode(self, mode: ControlMode) -> bool: + """Set mock control mode.""" + self._control_mode = mode + return True + + def get_control_mode(self) -> ControlMode: + """Get mock control mode.""" + return self._control_mode + + # ========================================================================= + # State Reading + # ========================================================================= + + def read_joint_positions(self) -> list[float]: + """Return mock joint positions.""" + return self._positions.copy() + + def read_joint_velocities(self) -> list[float]: + """Return mock joint velocities.""" + return self._velocities.copy() + + def read_joint_efforts(self) -> list[float]: + """Return mock joint efforts.""" + return self._efforts.copy() + + def read_state(self) -> dict: + """Return mock state.""" + return { + "state": 0 if self._enabled else 1, + "mode": self._control_mode.value, + } + + def read_error(self) -> tuple[int, str]: + """Return mock error.""" + return self._error_code, self._error_message + + # ========================================================================= + # Motion Control + # ========================================================================= + + def write_joint_positions( + self, + positions: list[float], + velocity: float = 1.0, + ) -> bool: + """Set mock joint positions (instant move).""" + if len(positions) != self._dof: + return False + self._positions = list(positions) + return True + + def write_joint_velocities(self, velocities: list[float]) -> bool: + """Set mock joint velocities.""" + if len(velocities) != self._dof: + return False + self._velocities = list(velocities) + return True + + def write_stop(self) -> bool: + """Stop mock motion.""" + self._velocities = [0.0] * self._dof + return True + + # ========================================================================= + # Servo Control + # ========================================================================= + + def write_enable(self, enable: bool) -> bool: + """Enable/disable mock servos.""" + self._enabled = enable + return True + + def read_enabled(self) -> bool: + """Check mock servo state.""" + return self._enabled + + def write_clear_errors(self) -> bool: + """Clear mock errors.""" + self._error_code = 0 + self._error_message = "" + return True + + # ========================================================================= + # Cartesian Control (Optional) + # ========================================================================= + + def read_cartesian_position(self) -> dict[str, float] | None: + """Return mock cartesian position.""" + return self._cartesian_position.copy() + + def write_cartesian_position( + self, + pose: dict[str, float], + velocity: float = 1.0, + ) -> bool: + """Set mock cartesian position.""" + self._cartesian_position.update(pose) + return True + + # ========================================================================= + # Gripper (Optional) + # ========================================================================= + + def read_gripper_position(self) -> float | None: + """Return mock gripper position.""" + return self._gripper_position + + def write_gripper_position(self, position: float) -> bool: + """Set mock gripper position.""" + self._gripper_position = position + return True + + # ========================================================================= + # Force/Torque (Optional) + # ========================================================================= + + def read_force_torque(self) -> list[float] | None: + """Return mock F/T sensor data (not supported in mock).""" + return None + + # ========================================================================= + # Test Helpers (not part of Protocol) + # ========================================================================= + + def set_error(self, code: int, message: str) -> None: + """Inject an error for testing error handling.""" + self._error_code = code + self._error_message = message + + def set_positions(self, positions: list[float]) -> None: + """Set positions directly for testing.""" + self._positions = list(positions) + + def set_efforts(self, efforts: list[float]) -> None: + """Set efforts directly for testing.""" + self._efforts = list(efforts) + + +__all__ = ["MockBackend"] From 6e2a20d704b33e54043bd92dcd302f344f7af707 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 6 Jan 2026 18:16:02 -0800 Subject: [PATCH 08/42] updated all blueprints to add new arm module --- dimos/hardware/manipulators/__init__.py | 52 +++++++++++++++++++++++++ dimos/robot/all_blueprints.py | 13 ++++--- 2 files changed, 59 insertions(+), 6 deletions(-) create mode 100644 dimos/hardware/manipulators/__init__.py diff --git a/dimos/hardware/manipulators/__init__.py b/dimos/hardware/manipulators/__init__.py new file mode 100644 index 0000000000..e4133dbb51 --- /dev/null +++ b/dimos/hardware/manipulators/__init__.py @@ -0,0 +1,52 @@ +# 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. + +"""Manipulator drivers for robotic arms. + +Architecture: B-lite (Protocol-based backends with per-arm drivers) + +- spec.py: ManipulatorBackend Protocol and shared types +- xarm/: XArm driver and backend +- piper/: Piper driver and backend +- mock/: Mock backend for testing + +Usage: + >>> from dimos.hardware.manipulators.xarm import XArm + >>> arm = XArm(ip="192.168.1.185") + >>> arm.start() + >>> arm.enable_servos() + >>> arm.move_joint([0, 0, 0, 0, 0, 0]) + +Testing: + >>> from dimos.hardware.manipulators.xarm import XArm + >>> from dimos.hardware.manipulators.mock import MockBackend + >>> arm = XArm(backend=MockBackend()) + >>> arm.start() # No hardware needed! +""" + +from dimos.hardware.manipulators.spec import ( + ControlMode, + DriverStatus, + JointLimits, + ManipulatorBackend, + ManipulatorInfo, +) + +__all__ = [ + "ControlMode", + "DriverStatus", + "JointLimits", + "ManipulatorBackend", + "ManipulatorInfo", +] diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index f989098f05..9c06a6011c 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -37,11 +37,12 @@ "unitree-g1-full": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:full_featured", "unitree-g1-detection": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:detection", # xArm manipulator blueprints - "xarm-servo": "dimos.hardware.manipulators.xarm.xarm_blueprints:xarm_servo", - "xarm5-servo": "dimos.hardware.manipulators.xarm.xarm_blueprints:xarm5_servo", - "xarm7-servo": "dimos.hardware.manipulators.xarm.xarm_blueprints:xarm7_servo", - "xarm-cartesian": "dimos.hardware.manipulators.xarm.xarm_blueprints:xarm_cartesian", - "xarm-trajectory": "dimos.hardware.manipulators.xarm.xarm_blueprints:xarm_trajectory", + "xarm-servo": "dimos.hardware.manipulators.xarm.blueprints:xarm_servo", + "xarm5-servo": "dimos.hardware.manipulators.xarm.blueprints:xarm5_servo", + "xarm7-servo": "dimos.hardware.manipulators.xarm.blueprints:xarm7_servo", + "xarm7-trajectory": "dimos.hardware.manipulators.xarm.blueprints:xarm7_trajectory", + "xarm-cartesian": "dimos.hardware.manipulators.xarm.blueprints:xarm_cartesian", + "xarm-trajectory": "dimos.hardware.manipulators.xarm.blueprints:xarm_trajectory", # Piper manipulator blueprints "piper-servo": "dimos.hardware.manipulators.piper.piper_blueprints:piper_servo", "piper-cartesian": "dimos.hardware.manipulators.piper.piper_blueprints:piper_cartesian", @@ -84,7 +85,7 @@ "websocket_vis": "dimos.web.websocket_vis.websocket_vis_module", "web_input": "dimos.agents.cli.web", # xArm manipulator modules - "xarm_driver": "dimos.hardware.manipulators.xarm.xarm_driver", + "xarm_driver": "dimos.hardware.manipulators.xarm.arm", "cartesian_motion_controller": "dimos.manipulation.control.servo_control.cartesian_motion_controller", "joint_trajectory_controller": "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller", } From c427fa51246966d10a0e0d4ecce8d9d367c785f2 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 6 Jan 2026 18:20:10 -0800 Subject: [PATCH 09/42] Added readme explaining new driver architecture overview --- dimos/hardware/manipulators/README.md | 158 ++++++++++++++++++++++++++ 1 file changed, 158 insertions(+) create mode 100644 dimos/hardware/manipulators/README.md diff --git a/dimos/hardware/manipulators/README.md b/dimos/hardware/manipulators/README.md new file mode 100644 index 0000000000..4d8b01efeb --- /dev/null +++ b/dimos/hardware/manipulators/README.md @@ -0,0 +1,158 @@ +# Manipulator Drivers + +This module provides manipulator arm drivers using the **B-lite architecture**: Protocol-only with injectable backends. + +## Architecture Overview + +``` +┌─────────────────────────────────────────────────────────────┐ +│ Driver (Module) │ +│ - Owns threading (control loop, monitor loop) │ +│ - Publishes joint_state, robot_state │ +│ - Subscribes to joint_position_command, joint_velocity_cmd │ +│ - Exposes RPC methods (move_joint, enable_servos, etc.) │ +└─────────────────────┬───────────────────────────────────────┘ + │ uses +┌─────────────────────▼───────────────────────────────────────┐ +│ Backend (implements Protocol) │ +│ - Handles SDK communication │ +│ - Unit conversions (radians ↔ vendor units) │ +│ - Swappable: XArmBackend, PiperBackend, MockBackend │ +└─────────────────────────────────────────────────────────────┘ +``` + +## Key Benefits + +- **Testable**: Inject `MockBackend` for unit tests without hardware +- **Flexible**: Each arm controls its own threading/timing +- **Simple**: No ABC inheritance required - just implement the Protocol +- **Type-safe**: Full type checking via `ManipulatorBackend` Protocol + +## Directory Structure + +``` +manipulators/ +├── spec.py # ManipulatorBackend Protocol + shared types +├── mock/ +│ └── backend.py # MockBackend for testing +├── xarm/ +│ ├── backend.py # XArmBackend (SDK wrapper) +│ ├── arm.py # XArm driver module +│ └── blueprints.py # Pre-configured blueprints +└── piper/ + ├── backend.py # PiperBackend (SDK wrapper) + ├── arm.py # Piper driver module + └── blueprints.py # Pre-configured blueprints +``` + +## Quick Start + +### Using a Driver Directly + +```python +from dimos.hardware.manipulators.xarm import XArm + +arm = XArm(ip="192.168.1.185", dof=6) +arm.start() +arm.enable_servos() +arm.move_joint([0, 0, 0, 0, 0, 0]) +arm.stop() +``` + +### Using Blueprints + +```python +from dimos.hardware.manipulators.xarm.blueprints import xarm_trajectory + +coordinator = xarm_trajectory.build() +coordinator.loop() +``` + +### Testing Without Hardware + +```python +from dimos.hardware.manipulators.mock import MockBackend +from dimos.hardware.manipulators.xarm import XArm + +arm = XArm(backend=MockBackend(dof=6)) +arm.start() # No hardware needed! +arm.move_joint([0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) +``` + +## Adding a New Arm + +1. **Create the backend** (`backend.py`): + +```python +from dimos.hardware.manipulators.spec import ManipulatorBackend, ControlMode + +class MyArmBackend: # No inheritance needed - just match the Protocol + def connect(self, config: dict) -> bool: ... + def disconnect(self) -> None: ... + def read_joint_positions(self) -> list[float]: ... + def write_joint_positions(self, positions: list[float], velocity: float = 1.0) -> bool: ... + # ... implement other Protocol methods +``` + +2. **Create the driver** (`arm.py`): + +```python +from dimos.core import Module, ModuleConfig, In, Out, rpc +from .backend import MyArmBackend + +class MyArm(Module[MyArmConfig]): + joint_state: Out[JointState] + robot_state: Out[RobotState] + joint_position_command: In[JointCommand] + + def __init__(self, backend=None, **kwargs): + super().__init__(**kwargs) + self.backend = backend or MyArmBackend() + # ... setup control loops +``` + +3. **Create blueprints** (`blueprints.py`) for common configurations. + +## ManipulatorBackend Protocol + +All backends must implement these core methods: + +| Category | Methods | +|----------|---------| +| Connection | `connect()`, `disconnect()`, `is_connected()` | +| Info | `get_info()`, `get_dof()`, `get_limits()` | +| State | `read_joint_positions()`, `read_joint_velocities()`, `read_joint_efforts()` | +| Motion | `write_joint_positions()`, `write_joint_velocities()`, `write_stop()` | +| Servo | `write_enable()`, `read_enabled()`, `write_clear_errors()` | +| Mode | `set_control_mode()`, `get_control_mode()` | + +Optional methods (return `None`/`False` if unsupported): +- `read_cartesian_position()`, `write_cartesian_position()` +- `read_gripper_position()`, `write_gripper_position()` +- `read_force_torque()` + +## Unit Conventions + +All backends convert to/from SI units: + +| Quantity | Unit | +|----------|------| +| Angles | radians | +| Angular velocity | rad/s | +| Torque | Nm | +| Position | meters | +| Force | Newtons | + +## Available Blueprints + +### XArm +- `xarm_servo` - Basic servo control (6-DOF) +- `xarm5_servo`, `xarm7_servo` - 5/7-DOF variants +- `xarm_trajectory` - Driver + trajectory controller +- `xarm_cartesian` - Driver + cartesian controller + +### Piper +- `piper_servo` - Basic servo control +- `piper_servo_gripper` - With gripper support +- `piper_trajectory` - Driver + trajectory controller +- `piper_left`, `piper_right` - Dual arm configurations From f3b95f5d7a4b8c6b3c6edc89816cef9d60e9942d Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 7 Jan 2026 10:58:14 -0800 Subject: [PATCH 10/42] config now parsed in backend init instead of connect method --- dimos/hardware/manipulators/README.md | 13 +++++++++---- dimos/hardware/manipulators/mock/backend.py | 6 +----- dimos/hardware/manipulators/piper/arm.py | 14 ++++++-------- dimos/hardware/manipulators/piper/backend.py | 17 ++++++++--------- dimos/hardware/manipulators/spec.py | 2 +- dimos/hardware/manipulators/xarm/arm.py | 12 +++++------- dimos/hardware/manipulators/xarm/backend.py | 16 +++++++--------- 7 files changed, 37 insertions(+), 43 deletions(-) diff --git a/dimos/hardware/manipulators/README.md b/dimos/hardware/manipulators/README.md index 4d8b01efeb..d3e54d4cb0 100644 --- a/dimos/hardware/manipulators/README.md +++ b/dimos/hardware/manipulators/README.md @@ -84,10 +84,12 @@ arm.move_joint([0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) 1. **Create the backend** (`backend.py`): ```python -from dimos.hardware.manipulators.spec import ManipulatorBackend, ControlMode - class MyArmBackend: # No inheritance needed - just match the Protocol - def connect(self, config: dict) -> bool: ... + def __init__(self, ip: str = "192.168.1.100", dof: int = 6) -> None: + self._ip = ip + self._dof = dof + + def connect(self) -> bool: ... def disconnect(self) -> None: ... def read_joint_positions(self) -> list[float]: ... def write_joint_positions(self, positions: list[float], velocity: float = 1.0) -> bool: ... @@ -107,7 +109,10 @@ class MyArm(Module[MyArmConfig]): def __init__(self, backend=None, **kwargs): super().__init__(**kwargs) - self.backend = backend or MyArmBackend() + self.backend = backend or MyArmBackend( + ip=self.config.ip, + dof=self.config.dof, + ) # ... setup control loops ``` diff --git a/dimos/hardware/manipulators/mock/backend.py b/dimos/hardware/manipulators/mock/backend.py index da3696421e..9357b5458d 100644 --- a/dimos/hardware/manipulators/mock/backend.py +++ b/dimos/hardware/manipulators/mock/backend.py @@ -64,12 +64,8 @@ def __init__(self, dof: int = 6) -> None: # Connection # ========================================================================= - def connect(self, config: dict) -> bool: + def connect(self) -> bool: """Simulate connection.""" - self._dof = config.get("dof", self._dof) - self._positions = [0.0] * self._dof - self._velocities = [0.0] * self._dof - self._efforts = [0.0] * self._dof self._connected = True return True diff --git a/dimos/hardware/manipulators/piper/arm.py b/dimos/hardware/manipulators/piper/arm.py index 1dd0207c35..306505302b 100644 --- a/dimos/hardware/manipulators/piper/arm.py +++ b/dimos/hardware/manipulators/piper/arm.py @@ -104,8 +104,11 @@ def __init__( ) -> None: super().__init__(*args, **kwargs) - # Backend is injectable for testing - self.backend: ManipulatorBackend = backend or PiperBackend() + # Backend is injectable for testing (pass config to backend __init__) + self.backend: ManipulatorBackend = backend or PiperBackend( + can_port=self.config.can_port, + dof=self.config.dof, + ) # Threading state self._running = False @@ -121,12 +124,7 @@ def __init__( def _auto_start(self) -> None: """Auto-connect to hardware on initialization.""" - config_dict = { - "can_port": self.config.can_port, - "dof": self.config.dof, - } - - if not self.backend.connect(config_dict): + if not self.backend.connect(): print(f"WARNING: Failed to connect to Piper on {self.config.can_port}") return diff --git a/dimos/hardware/manipulators/piper/backend.py b/dimos/hardware/manipulators/piper/backend.py index 01e52c2645..9b19ff069a 100644 --- a/dimos/hardware/manipulators/piper/backend.py +++ b/dimos/hardware/manipulators/piper/backend.py @@ -44,9 +44,10 @@ class PiperBackend: - Velocities: Piper uses internal units, we use rad/s """ - def __init__(self) -> None: + def __init__(self, can_port: str = "can0", dof: int = 6) -> None: + self._can_port = can_port + self._dof = dof self._sdk: Any = None - self._dof: int = 6 # Piper is always 6-DOF self._connected: bool = False self._enabled: bool = False self._control_mode: ControlMode = ControlMode.POSITION @@ -55,15 +56,13 @@ def __init__(self) -> None: # Connection # ========================================================================= - def connect(self, config: dict) -> bool: + def connect(self) -> bool: """Connect to Piper via CAN bus.""" - can_port = config.get("can_port", "can0") - try: from piper_sdk import C_PiperInterface_V2 self._sdk = C_PiperInterface_V2( - can_name=can_port, + can_name=self._can_port, judge_flag=True, # Enable safety checks can_auto_init=True, # Let SDK handle CAN initialization dh_is_offset=False, @@ -79,17 +78,17 @@ def connect(self, config: dict) -> bool: status = self._sdk.GetArmStatus() if status is not None: self._connected = True - print(f"Piper connected via CAN port {can_port}") + print(f"Piper connected via CAN port {self._can_port}") return True else: - print(f"ERROR: Failed to connect to Piper on {can_port} - no status received") + print(f"ERROR: Failed to connect to Piper on {self._can_port} - no status received") return False except ImportError: print("ERROR: Piper SDK not installed. Please install piper_sdk") return False except Exception as e: - print(f"ERROR: Failed to connect to Piper on {can_port}: {e}") + print(f"ERROR: Failed to connect to Piper on {self._can_port}: {e}") return False def disconnect(self) -> None: diff --git a/dimos/hardware/manipulators/spec.py b/dimos/hardware/manipulators/spec.py index ea68e115d7..5e21ac5845 100644 --- a/dimos/hardware/manipulators/spec.py +++ b/dimos/hardware/manipulators/spec.py @@ -106,7 +106,7 @@ class ManipulatorBackend(Protocol): # --- Connection --- - def connect(self, config: dict) -> bool: + def connect(self) -> bool: """Connect to hardware. Returns True on success.""" ... diff --git a/dimos/hardware/manipulators/xarm/arm.py b/dimos/hardware/manipulators/xarm/arm.py index 574ffb5ed9..e60633a096 100644 --- a/dimos/hardware/manipulators/xarm/arm.py +++ b/dimos/hardware/manipulators/xarm/arm.py @@ -107,7 +107,10 @@ def __init__( super().__init__(*args, **kwargs) # Backend is injectable for testing - self.backend: ManipulatorBackend = backend or XArmBackend() + self.backend: ManipulatorBackend = backend or XArmBackend( + ip=self.config.ip, + dof=self.config.dof, + ) # Threading state self._running = False @@ -123,12 +126,7 @@ def __init__( def _auto_start(self) -> None: """Auto-connect to hardware on initialization.""" - config_dict = { - "ip": self.config.ip, - "dof": self.config.dof, - } - - if not self.backend.connect(config_dict): + if not self.backend.connect(): print(f"WARNING: Failed to connect to XArm at {self.config.ip}") return diff --git a/dimos/hardware/manipulators/xarm/backend.py b/dimos/hardware/manipulators/xarm/backend.py index aff088cc27..bd6152900e 100644 --- a/dimos/hardware/manipulators/xarm/backend.py +++ b/dimos/hardware/manipulators/xarm/backend.py @@ -47,26 +47,24 @@ class XArmBackend: - Velocities: XArm uses deg/s, we use rad/s """ - def __init__(self) -> None: + def __init__(self, ip: str = "192.168.1.185", dof: int = 6) -> None: + self._ip = ip + self._dof = dof self._arm: XArmAPI | None = None - self._dof: int = 6 self._control_mode: ControlMode = ControlMode.POSITION # ========================================================================= # Connection # ========================================================================= - def connect(self, config: dict) -> bool: + def connect(self) -> bool: """Connect to XArm via TCP/IP.""" - ip = config.get("ip", "192.168.1.185") - self._dof = config.get("dof", 6) - try: - self._arm = XArmAPI(ip) + self._arm = XArmAPI(self._ip) self._arm.connect() if not self._arm.connected: - print(f"ERROR: XArm at {ip} not reachable (connected=False)") + print(f"ERROR: XArm at {self._ip} not reachable (connected=False)") return False # Initialize to servo mode for high-frequency control @@ -76,7 +74,7 @@ def connect(self, config: dict) -> bool: return True except Exception as e: - print(f"ERROR: Failed to connect to XArm at {ip}: {e}") + print(f"ERROR: Failed to connect to XArm at {self._ip}: {e}") return False def disconnect(self) -> None: From d6ea830c40aed74a17caa4f273bbc25463d4e177 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 7 Jan 2026 14:53:58 -0800 Subject: [PATCH 11/42] addded dual arm control blueprint using trajectory controller --- .../hardware/manipulators/xarm/blueprints.py | 188 +++++- .../control/dual_trajectory_setter.py | 540 ++++++++++++++++++ dimos/robot/all_blueprints.py | 7 +- 3 files changed, 730 insertions(+), 5 deletions(-) create mode 100644 dimos/manipulation/control/dual_trajectory_setter.py diff --git a/dimos/hardware/manipulators/xarm/blueprints.py b/dimos/hardware/manipulators/xarm/blueprints.py index 5e25827050..a03353ebb2 100644 --- a/dimos/hardware/manipulators/xarm/blueprints.py +++ b/dimos/hardware/manipulators/xarm/blueprints.py @@ -27,8 +27,12 @@ from dimos.core.blueprints import autoconnect from dimos.core.transport import LCMTransport -from dimos.hardware.manipulators.xarm.arm import xarm as xarm_blueprint -from dimos.manipulation.control import cartesian_motion_controller, joint_trajectory_controller +from dimos.hardware.manipulators.xarm.arm import XArm, xarm as xarm_blueprint +from dimos.manipulation.control import ( + JointTrajectoryController, + cartesian_motion_controller, + joint_trajectory_controller, +) from dimos.msgs.geometry_msgs import PoseStamped from dimos.msgs.sensor_msgs import ( JointCommand, @@ -37,6 +41,40 @@ ) from dimos.msgs.trajectory_msgs import JointTrajectory +# ============================================================================= +# Dual-Arm Subclasses (workaround for module deduplication) +# ============================================================================= +# To run multiple arms in the same blueprint, we need distinct class names. +# This gives each arm its own RPC namespace: +# - XArmLeft → /rpc/XArmLeft/start/req +# - XArmRight → /rpc/XArmRight/start/req +# ============================================================================= + + +class XArmLeft(XArm): + """Left arm instance for dual-arm setups (XArm7).""" + + pass + + +class XArmRight(XArm): + """Right arm instance for dual-arm setups (XArm6).""" + + pass + + +class JointTrajectoryControllerLeft(JointTrajectoryController): + """Left arm trajectory controller.""" + + pass + + +class JointTrajectoryControllerRight(JointTrajectoryController): + """Right arm trajectory controller.""" + + pass + + # ============================================================================= # XArm6 Servo Control Blueprint # ============================================================================= @@ -181,11 +219,157 @@ ) +# ============================================================================= +# Dual-Arm Blueprint (XArm7 Left + XArm6 Right) +# ============================================================================= +# Uses subclasses for separate RPC namespaces + remappings for separate topics. +# Each arm has its own topics for joint states and commands. +# ============================================================================= + +xarm_dual = ( + autoconnect( + XArmLeft.blueprint( + ip="192.168.2.235", # XArm7 - Left arm + dof=7, + control_rate=100.0, + monitor_rate=10.0, + ), + XArmRight.blueprint( + ip="192.168.1.210", # XArm6 - Right arm + dof=6, + control_rate=100.0, + monitor_rate=10.0, + ), + ) + .remappings( + [ + # Remap left arm connections to unique names + (XArmLeft, "joint_state", "left_joint_state"), + (XArmLeft, "robot_state", "left_robot_state"), + (XArmLeft, "joint_position_command", "left_joint_position_command"), + # Remap right arm connections to unique names + (XArmRight, "joint_state", "right_joint_state"), + (XArmRight, "robot_state", "right_robot_state"), + (XArmRight, "joint_position_command", "right_joint_position_command"), + ] + ) + .transports( + { + # Left arm (XArm7) topics - use remapped names + ("left_joint_state", JointState): LCMTransport("/xarm/left/joint_states", JointState), + ("left_robot_state", RobotState): LCMTransport("/xarm/left/robot_state", RobotState), + ("left_joint_position_command", JointCommand): LCMTransport( + "/xarm/left/joint_position_command", JointCommand + ), + # Right arm (XArm6) topics - use remapped names + ("right_joint_state", JointState): LCMTransport("/xarm/right/joint_states", JointState), + ("right_robot_state", RobotState): LCMTransport("/xarm/right/robot_state", RobotState), + ("right_joint_position_command", JointCommand): LCMTransport( + "/xarm/right/joint_position_command", JointCommand + ), + } + ) +) + + +# ============================================================================= +# Dual-Arm Trajectory Blueprint (XArm7 Left + XArm6 Right + Controllers) +# ============================================================================= +# Full dual-arm setup with trajectory controllers for each arm. +# Each arm has independent trajectory execution. +# ============================================================================= + +xarm_dual_trajectory = ( + autoconnect( + # Left arm (XArm7) + XArmLeft.blueprint( + ip="192.168.2.235", + dof=7, + control_rate=100.0, + monitor_rate=10.0, + ), + # Right arm (XArm6) + XArmRight.blueprint( + ip="192.168.1.210", + dof=6, + control_rate=100.0, + monitor_rate=10.0, + ), + # Left trajectory controller + JointTrajectoryControllerLeft.blueprint( + control_frequency=100.0, + ), + # Right trajectory controller + JointTrajectoryControllerRight.blueprint( + control_frequency=100.0, + ), + ) + .remappings( + [ + # Left arm connections + (XArmLeft, "joint_state", "left_joint_state"), + (XArmLeft, "robot_state", "left_robot_state"), + (XArmLeft, "joint_position_command", "left_joint_position_command"), + # Right arm connections + (XArmRight, "joint_state", "right_joint_state"), + (XArmRight, "robot_state", "right_robot_state"), + (XArmRight, "joint_position_command", "right_joint_position_command"), + # Left controller connections - connect to left arm + (JointTrajectoryControllerLeft, "joint_state", "left_joint_state"), + (JointTrajectoryControllerLeft, "robot_state", "left_robot_state"), + ( + JointTrajectoryControllerLeft, + "joint_position_command", + "left_joint_position_command", + ), + (JointTrajectoryControllerLeft, "trajectory", "left_trajectory"), + # Right controller connections - connect to right arm + (JointTrajectoryControllerRight, "joint_state", "right_joint_state"), + (JointTrajectoryControllerRight, "robot_state", "right_robot_state"), + ( + JointTrajectoryControllerRight, + "joint_position_command", + "right_joint_position_command", + ), + (JointTrajectoryControllerRight, "trajectory", "right_trajectory"), + ] + ) + .transports( + { + # Left arm topics + ("left_joint_state", JointState): LCMTransport("/xarm/left/joint_states", JointState), + ("left_robot_state", RobotState): LCMTransport("/xarm/left/robot_state", RobotState), + ("left_joint_position_command", JointCommand): LCMTransport( + "/xarm/left/joint_position_command", JointCommand + ), + ("left_trajectory", JointTrajectory): LCMTransport( + "/xarm/left/trajectory", JointTrajectory + ), + # Right arm topics + ("right_joint_state", JointState): LCMTransport("/xarm/right/joint_states", JointState), + ("right_robot_state", RobotState): LCMTransport("/xarm/right/robot_state", RobotState), + ("right_joint_position_command", JointCommand): LCMTransport( + "/xarm/right/joint_position_command", JointCommand + ), + ("right_trajectory", JointTrajectory): LCMTransport( + "/xarm/right/trajectory", JointTrajectory + ), + } + ) +) + + __all__ = [ + "JointTrajectoryControllerLeft", + "JointTrajectoryControllerRight", + "XArmLeft", + "XArmRight", "xarm5_servo", "xarm7_servo", "xarm7_trajectory", "xarm_cartesian", + "xarm_dual", + "xarm_dual_trajectory", "xarm_servo", "xarm_trajectory", ] diff --git a/dimos/manipulation/control/dual_trajectory_setter.py b/dimos/manipulation/control/dual_trajectory_setter.py new file mode 100644 index 0000000000..c6ae215a8d --- /dev/null +++ b/dimos/manipulation/control/dual_trajectory_setter.py @@ -0,0 +1,540 @@ +#!/usr/bin/env python3 +# 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 Interactive Trajectory Publisher. + +Interactive terminal UI for creating joint trajectories for two arms independently. +Supports running trajectories on left arm, right arm, or both simultaneously. + +Workflow: +1. Add waypoints to left or right arm (or both) +2. Generator applies trapezoidal velocity profiles +3. Preview the generated trajectories +4. Run on left, right, or both arms + +Use with xarm-dual-trajectory blueprint running in another terminal. +""" + +from dataclasses import dataclass +import math +import sys +import time + +from dimos import core +from dimos.manipulation.planning import JointTrajectoryGenerator +from dimos.msgs.sensor_msgs import JointState +from dimos.msgs.trajectory_msgs import JointTrajectory + + +@dataclass +class ArmState: + """State for a single arm.""" + + name: str + num_joints: int | None = None + latest_joint_state: JointState | None = None + generator: JointTrajectoryGenerator | None = None + waypoints: list[list[float]] | None = None + generated_trajectory: JointTrajectory | None = None + + def __post_init__(self) -> None: + self.waypoints = [] + + +class DualTrajectorySetter: + """ + Creates and publishes JointTrajectory for dual-arm setups. + + Manages two arms independently with separate waypoints and trajectories. + Supports running trajectories on one or both arms. + """ + + def __init__( + self, + left_joint_topic: str = "/xarm/left/joint_states", + right_joint_topic: str = "/xarm/right/joint_states", + left_trajectory_topic: str = "/xarm/left/trajectory", + right_trajectory_topic: str = "/xarm/right/trajectory", + ): + """ + Initialize the dual trajectory setter. + + Args: + left_joint_topic: Topic for left arm joint states + right_joint_topic: Topic for right arm joint states + left_trajectory_topic: Topic to publish left arm trajectories + right_trajectory_topic: Topic to publish right arm trajectories + """ + # Arm states + self.left = ArmState(name="left") + self.right = ArmState(name="right") + + # Publishers for trajectories + self.left_trajectory_pub: core.LCMTransport[JointTrajectory] = core.LCMTransport( + left_trajectory_topic, JointTrajectory + ) + self.right_trajectory_pub: core.LCMTransport[JointTrajectory] = core.LCMTransport( + right_trajectory_topic, JointTrajectory + ) + + # Subscribers for joint states + self.left_joint_sub: core.LCMTransport[JointState] = core.LCMTransport( + left_joint_topic, JointState + ) + self.right_joint_sub: core.LCMTransport[JointState] = core.LCMTransport( + right_joint_topic, JointState + ) + + print("DualTrajectorySetter initialized") + print(f" Left arm: {left_joint_topic} -> {left_trajectory_topic}") + print(f" Right arm: {right_joint_topic} -> {right_trajectory_topic}") + + def start(self) -> bool: + """Start subscribing to joint states.""" + self.left_joint_sub.subscribe(self._on_left_joint_state) + self.right_joint_sub.subscribe(self._on_right_joint_state) + print(" Waiting for joint states...") + + # Wait for both arms + left_ready = False + right_ready = False + + for _ in range(50): # 5 second timeout + if not left_ready and self.left.latest_joint_state is not None: + self.left.num_joints = len(self.left.latest_joint_state.position) + self.left.generator = JointTrajectoryGenerator( + num_joints=self.left.num_joints, + max_velocity=1.0, + max_acceleration=2.0, + points_per_segment=50, + ) + print(f" Left arm ready ({self.left.num_joints} joints)") + left_ready = True + + if not right_ready and self.right.latest_joint_state is not None: + self.right.num_joints = len(self.right.latest_joint_state.position) + self.right.generator = JointTrajectoryGenerator( + num_joints=self.right.num_joints, + max_velocity=1.0, + max_acceleration=2.0, + points_per_segment=50, + ) + print(f" Right arm ready ({self.right.num_joints} joints)") + right_ready = True + + if left_ready and right_ready: + return True + + time.sleep(0.1) + + if not left_ready: + print(" Warning: Left arm not responding") + if not right_ready: + print(" Warning: Right arm not responding") + + return left_ready or right_ready + + def _on_left_joint_state(self, msg: JointState) -> None: + """Callback for left arm joint state.""" + self.left.latest_joint_state = msg + + def _on_right_joint_state(self, msg: JointState) -> None: + """Callback for right arm joint state.""" + self.right.latest_joint_state = msg + + def get_current_joints(self, arm: ArmState) -> list[float] | None: + """Get current joint positions for an arm.""" + if arm.latest_joint_state is None or arm.num_joints is None: + return None + return list(arm.latest_joint_state.position[: arm.num_joints]) + + def generate_trajectory(self, arm: ArmState) -> JointTrajectory | None: + """Generate trajectory for an arm from its waypoints.""" + if arm.generator is None or not arm.waypoints or len(arm.waypoints) < 2: + return None + return arm.generator.generate(arm.waypoints) + + def publish_trajectory(self, arm: ArmState, trajectory: JointTrajectory) -> None: + """Publish trajectory to an arm.""" + if arm.name == "left": + self.left_trajectory_pub.broadcast(None, trajectory) + else: + self.right_trajectory_pub.broadcast(None, trajectory) + print( + f" Published to {arm.name}: {len(trajectory.points)} points, " + f"duration={trajectory.duration:.2f}s" + ) + + +def parse_joint_input(line: str, num_joints: int) -> list[float] | None: + """Parse joint positions from user input (degrees by default, 'r' suffix for radians).""" + parts = line.strip().split() + if len(parts) != num_joints: + return None + + positions = [] + for part in parts: + try: + if part.endswith("r"): + positions.append(float(part[:-1])) + else: + positions.append(math.radians(float(part))) + except ValueError: + return None + + return positions + + +def preview_waypoints(arm: ArmState) -> None: + """Show waypoints for an arm.""" + if not arm.waypoints or arm.num_joints is None: + print(f" {arm.name.upper()}: No waypoints") + return + + joint_headers = " ".join([f"{'J' + str(i + 1):>7}" for i in range(arm.num_joints)]) + line_width = 6 + 3 + arm.num_joints * 8 + 10 + + print(f"\n{arm.name.upper()} Waypoints ({len(arm.waypoints)}):") + print("-" * line_width) + print(f" # | {joint_headers} (degrees)") + print("-" * line_width) + for i, joints in enumerate(arm.waypoints): + deg = [f"{math.degrees(j):7.1f}" for j in joints] + print(f" {i + 1:2} | {' '.join(deg)}") + print("-" * line_width) + + +def preview_trajectory(arm: ArmState) -> None: + """Show generated trajectory for an arm.""" + if arm.generated_trajectory is None or arm.num_joints is None: + print(f" {arm.name.upper()}: No trajectory") + return + + traj = arm.generated_trajectory + joint_headers = " ".join([f"{'J' + str(i + 1):>7}" for i in range(arm.num_joints)]) + line_width = 9 + 3 + arm.num_joints * 8 + 10 + + print(f"\n{'=' * line_width}") + print(f"{arm.name.upper()} TRAJECTORY") + print(f"{'=' * line_width}") + print(f"Duration: {traj.duration:.3f}s | Points: {len(traj.points)}") + print("-" * line_width) + print(f"{'Time':>6} | {joint_headers} (degrees)") + print("-" * line_width) + + num_samples = min(10, max(len(traj.points) // 10, 5)) + for i in range(num_samples + 1): + t = (i / num_samples) * traj.duration + q_ref, _ = traj.sample(t) + q_deg = [f"{math.degrees(q):7.1f}" for q in q_ref] + print(f"{t:6.2f} | {' '.join(q_deg)}") + + print("-" * line_width) + + +def interactive_mode(setter: DualTrajectorySetter) -> None: + """Interactive mode for creating dual-arm trajectories.""" + left = setter.left + right = setter.right + + print("\n" + "=" * 80) + print("Dual-Arm Interactive Trajectory Setter") + print("=" * 80) + + if left.num_joints: + print(f" Left arm: {left.num_joints} joints") + else: + print(" Left arm: NOT CONNECTED") + + if right.num_joints: + print(f" Right arm: {right.num_joints} joints") + else: + print(" Right arm: NOT CONNECTED") + + print("\nCommands:") + print(" left add ... - Add waypoint to left arm (degrees)") + print(" right add ... - Add waypoint to right arm (degrees)") + print(" left here - Add current position as waypoint (left)") + print(" right here - Add current position as waypoint (right)") + print(" left current - Show current left arm joints") + print(" right current - Show current right arm joints") + print(" left list - List left arm waypoints") + print(" right list - List right arm waypoints") + print(" left delete - Delete waypoint n from left") + print(" right delete - Delete waypoint n from right") + print(" left clear - Clear left arm waypoints") + print(" right clear - Clear right arm waypoints") + print(" preview - Preview both trajectories") + print(" run left - Run trajectory on left arm only") + print(" run right - Run trajectory on right arm only") + print(" run both - Run trajectories on both arms") + print(" vel - Set max velocity (rad/s)") + print(" quit - Exit") + print("=" * 80) + + try: + while True: + left_wp = len(left.waypoints) if left.waypoints else 0 + right_wp = len(right.waypoints) if right.waypoints else 0 + prompt = f"[L:{left_wp} R:{right_wp}] > " + line = input(prompt).strip() + + if not line: + continue + + parts = line.split() + cmd = parts[0].lower() + + # Determine which arm (if applicable) + arm: ArmState | None = None + if cmd in ("left", "l"): + arm = left + parts = parts[1:] # Remove arm selector + cmd = parts[0].lower() if parts else "" + elif cmd in ("right", "r"): + arm = right + parts = parts[1:] + cmd = parts[0].lower() if parts else "" + + # ARM-SPECIFIC COMMANDS + if arm is not None: + if arm.num_joints is None: + print(f" {arm.name.upper()} arm not connected") + continue + + # ADD waypoint + if cmd == "add" and len(parts) >= arm.num_joints + 1: + joints = parse_joint_input( + " ".join(parts[1 : arm.num_joints + 1]), arm.num_joints + ) + if joints: + arm.waypoints.append(joints) # type: ignore[union-attr] + arm.generated_trajectory = None + deg = [f"{math.degrees(j):.1f}" for j in joints] + print( + f" {arm.name.upper()} waypoint {len(arm.waypoints)}: [{', '.join(deg)}] deg" # type: ignore[arg-type] + ) + else: + print(f" Invalid values (need {arm.num_joints} in degrees)") + + # HERE - add current position + elif cmd == "here": + joints = setter.get_current_joints(arm) + if joints: + arm.waypoints.append(joints) # type: ignore[union-attr] + arm.generated_trajectory = None + deg = [f"{math.degrees(j):.1f}" for j in joints] + print( + f" {arm.name.upper()} waypoint {len(arm.waypoints)}: [{', '.join(deg)}] deg" # type: ignore[arg-type] + ) + else: + print(" No joint state available") + + # CURRENT + elif cmd == "current": + joints = setter.get_current_joints(arm) + if joints: + deg = [f"{math.degrees(j):.1f}" for j in joints] + print(f" {arm.name.upper()}: [{', '.join(deg)}] deg") + else: + print(" No joint state available") + + # LIST + elif cmd == "list": + preview_waypoints(arm) + + # DELETE + elif cmd == "delete" and len(parts) >= 2: + try: + idx = int(parts[1]) - 1 + if arm.waypoints and 0 <= idx < len(arm.waypoints): + arm.waypoints.pop(idx) + arm.generated_trajectory = None + print(f" Deleted {arm.name} waypoint {idx + 1}") + else: + wp_count = len(arm.waypoints) if arm.waypoints else 0 + print(f" Invalid index (1-{wp_count})") + except ValueError: + print(" Invalid index") + + # CLEAR + elif cmd == "clear": + if arm.waypoints: + arm.waypoints.clear() + arm.generated_trajectory = None + print(f" {arm.name.upper()} waypoints cleared") + + else: + print(f" Unknown command for {arm.name}: {cmd}") + + # GLOBAL COMMANDS + elif cmd == "preview": + # Generate trajectories if needed + for a in [left, right]: + if a.waypoints and len(a.waypoints) >= 2: + try: + a.generated_trajectory = setter.generate_trajectory(a) + except Exception as e: + print(f" Error generating {a.name} trajectory: {e}") + a.generated_trajectory = None + + preview_trajectory(left) + preview_trajectory(right) + + elif cmd == "run" and len(parts) >= 2: + target = parts[1].lower() + + # Determine which arms to run + arms_to_run: list[ArmState] = [] + if target in ("left", "l"): + arms_to_run = [left] + elif target in ("right", "r"): + arms_to_run = [right] + elif target == "both": + arms_to_run = [left, right] + else: + print(" Usage: run left|right|both") + continue + + # Generate trajectories if needed + for a in arms_to_run: + if not a.waypoints or len(a.waypoints) < 2: + print(f" {a.name.upper()}: Need at least 2 waypoints") + continue + + if a.generated_trajectory is None: + try: + a.generated_trajectory = setter.generate_trajectory(a) + except Exception as e: + print(f" Error generating {a.name} trajectory: {e}") + continue + + # Preview and confirm + valid_arms = [a for a in arms_to_run if a.generated_trajectory is not None] + if not valid_arms: + print(" No valid trajectories to run") + continue + + for a in valid_arms: + preview_trajectory(a) + + arm_names = ", ".join(a.name.upper() for a in valid_arms) + confirm = input(f"\n Run on {arm_names}? [y/N]: ").strip().lower() + if confirm == "y": + print("\n Publishing trajectories...") + for a in valid_arms: + if a.generated_trajectory: + setter.publish_trajectory(a, a.generated_trajectory) + + elif cmd == "vel" and len(parts) >= 3: + arm_name = parts[1].lower() + a = ( + left + if arm_name in ("left", "l") + else right + if arm_name in ("right", "r") + else None + ) + if a is None or a.generator is None: + print(" Usage: vel left|right ") + continue + try: + vel = float(parts[2]) + if vel <= 0: + print(" Velocity must be positive") + else: + a.generator.set_limits(vel, a.generator.max_acceleration) + a.generated_trajectory = None + print(f" {a.name.upper()} max velocity: {vel:.2f} rad/s") + except ValueError: + print(" Invalid velocity value") + + elif cmd in ("quit", "exit", "q"): + break + + else: + print(f" Unknown command: {cmd}") + + except KeyboardInterrupt: + print("\n\nExiting...") + + +def main() -> int: + """Main entry point.""" + import argparse + + parser = argparse.ArgumentParser(description="Dual-Arm Interactive Trajectory Setter") + parser.add_argument( + "--left-joint-topic", + type=str, + default="/xarm/left/joint_states", + help="Left arm joint state topic", + ) + parser.add_argument( + "--right-joint-topic", + type=str, + default="/xarm/right/joint_states", + help="Right arm joint state topic", + ) + parser.add_argument( + "--left-trajectory-topic", + type=str, + default="/xarm/left/trajectory", + help="Left arm trajectory topic", + ) + parser.add_argument( + "--right-trajectory-topic", + type=str, + default="/xarm/right/trajectory", + help="Right arm trajectory topic", + ) + args = parser.parse_args() + + print("\n" + "=" * 80) + print("Dual-Arm Trajectory Setter") + print("=" * 80) + print("\nRun 'dimos run xarm-dual-trajectory' in another terminal first!") + print("=" * 80) + + setter = DualTrajectorySetter( + left_joint_topic=args.left_joint_topic, + right_joint_topic=args.right_joint_topic, + left_trajectory_topic=args.left_trajectory_topic, + right_trajectory_topic=args.right_trajectory_topic, + ) + + if not setter.start(): + print("\nWarning: Could not connect to both arms") + response = input("Continue anyway? [y/N]: ").strip().lower() + if response != "y": + return 0 + + interactive_mode(setter) + return 0 + + +if __name__ == "__main__": + try: + sys.exit(main()) + except KeyboardInterrupt: + print("\n\nInterrupted by user") + sys.exit(0) + except Exception as e: + print(f"\nError: {e}") + import traceback + + traceback.print_exc() + sys.exit(1) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 9c06a6011c..920aa1254f 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -43,10 +43,11 @@ "xarm7-trajectory": "dimos.hardware.manipulators.xarm.blueprints:xarm7_trajectory", "xarm-cartesian": "dimos.hardware.manipulators.xarm.blueprints:xarm_cartesian", "xarm-trajectory": "dimos.hardware.manipulators.xarm.blueprints:xarm_trajectory", + "xarm-dual": "dimos.hardware.manipulators.xarm.blueprints:xarm_dual", + "xarm-dual-trajectory": "dimos.hardware.manipulators.xarm.blueprints:xarm_dual_trajectory", # Piper manipulator blueprints - "piper-servo": "dimos.hardware.manipulators.piper.piper_blueprints:piper_servo", - "piper-cartesian": "dimos.hardware.manipulators.piper.piper_blueprints:piper_cartesian", - "piper-trajectory": "dimos.hardware.manipulators.piper.piper_blueprints:piper_trajectory", + "piper-servo": "dimos.hardware.manipulators.piper.blueprints:piper_servo", + "piper-trajectory": "dimos.hardware.manipulators.piper.blueprints:piper_trajectory", # Demo blueprints "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", From f69337d5e36906409a50b387c89a06199d6e22da Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 8 Jan 2026 12:59:37 -0800 Subject: [PATCH 12/42] adding a control orchestrator for single control loop for multiple arms and joint control - added dataclasses for orchestrator and protocol for ControlTask --- dimos/control/task.py | 297 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 297 insertions(+) create mode 100644 dimos/control/task.py diff --git a/dimos/control/task.py b/dimos/control/task.py new file mode 100644 index 0000000000..03303e07f1 --- /dev/null +++ b/dimos/control/task.py @@ -0,0 +1,297 @@ +# 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. + +"""ControlTask protocol and types for the ControlOrchestrator. + +This module defines: +- Data types used by tasks and the orchestrator (ResourceClaim, JointStateSnapshot, etc.) +- ControlTask protocol that all tasks must implement + +Tasks are "passive" - they don't own threads. The orchestrator calls +compute() at each tick, passing current state and time. + +CRITICAL: Tasks must NEVER call time.time() directly. +Use the t_now passed in OrchestratorState. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Protocol, runtime_checkable + +from dimos.hardware.manipulators.spec import ControlMode + +# ============================================================================= +# Data Types +# ============================================================================= + + +@dataclass(frozen=True) +class ResourceClaim: + """Declares which joints a task wants to control. + + Used by the orchestrator to determine resource ownership and + resolve conflicts between competing tasks. + + Attributes: + joints: Set of joint names this task wants to control. + Example: frozenset({"left_joint1", "left_joint2"}) + priority: Priority level for conflict resolution. Higher wins. + Typical values: 10 (trajectory), 50 (WBC), 100 (safety) + mode: Control mode (POSITION, VELOCITY, TORQUE) + """ + + joints: frozenset[str] + priority: int = 0 + mode: ControlMode = ControlMode.POSITION + + def conflicts_with(self, other: ResourceClaim) -> bool: + """Check if two claims compete for the same joints.""" + return bool(self.joints & other.joints) + + +@dataclass +class JointStateSnapshot: + """Aggregated joint states from all hardware. + + Provides a unified view of all joint states across all hardware + interfaces, indexed by fully-qualified joint name. + + Attributes: + joint_positions: Joint name -> position (radians) + joint_velocities: Joint name -> velocity (rad/s) + joint_efforts: Joint name -> effort (Nm) + timestamp: Unix timestamp when state was read + """ + + joint_positions: dict[str, float] = field(default_factory=dict) + joint_velocities: dict[str, float] = field(default_factory=dict) + joint_efforts: dict[str, float] = field(default_factory=dict) + timestamp: float = 0.0 + + def get_position(self, joint_name: str) -> float | None: + """Get position for a specific joint.""" + return self.joint_positions.get(joint_name) + + def get_velocity(self, joint_name: str) -> float | None: + """Get velocity for a specific joint.""" + return self.joint_velocities.get(joint_name) + + def get_effort(self, joint_name: str) -> float | None: + """Get effort for a specific joint.""" + return self.joint_efforts.get(joint_name) + + +@dataclass +class OrchestratorState: + """Complete state snapshot for tasks to read. + + Passed to each task's compute() method every tick. Contains + all information a task needs to compute its output. + + CRITICAL: Tasks should use t_now for timing, never time.time()! + + Attributes: + joints: Aggregated joint states from all hardware + t_now: Current tick time (time.perf_counter()) + dt: Time since last tick (seconds) + """ + + joints: JointStateSnapshot + t_now: float # Orchestrator time (perf_counter) - USE THIS, NOT time.time()! + dt: float # Time since last tick + + +@dataclass +class JointCommandOutput: + """Joint-centric command output from a task. + + Commands are addressed by joint name, NOT by hardware ID. + The orchestrator routes commands to the appropriate hardware. + + This design enables: + - WBC spanning multiple hardware interfaces + - Partial joint ownership + - Per-joint arbitration + + Attributes: + joint_names: Which joints this command is for + positions: Position commands (radians), or None + velocities: Velocity commands (rad/s), or None + efforts: Effort commands (Nm), or None + mode: Control mode - must match which field is populated + """ + + joint_names: list[str] + positions: list[float] | None = None + velocities: list[float] | None = None + efforts: list[float] | None = None + mode: ControlMode = ControlMode.POSITION + + def __post_init__(self) -> None: + """Validate that lengths match and at least one value field is set.""" + n = len(self.joint_names) + + if self.positions is not None and len(self.positions) != n: + raise ValueError(f"positions length {len(self.positions)} != joint_names length {n}") + if self.velocities is not None and len(self.velocities) != n: + raise ValueError(f"velocities length {len(self.velocities)} != joint_names length {n}") + if self.efforts is not None and len(self.efforts) != n: + raise ValueError(f"efforts length {len(self.efforts)} != joint_names length {n}") + + def get_values(self) -> list[float] | None: + """Get the active values based on mode.""" + if self.mode == ControlMode.POSITION: + return self.positions + elif self.mode == ControlMode.VELOCITY: + return self.velocities + elif self.mode == ControlMode.TORQUE: + return self.efforts + return None + + +# ============================================================================= +# ControlTask Protocol +# ============================================================================= + + +@runtime_checkable +class ControlTask(Protocol): + """Protocol for passive tasks that run within the orchestrator. + + Tasks are "passive" - they don't own threads. The orchestrator + calls compute() at each tick, passing current state and time. + + Lifecycle: + 1. Task is added to orchestrator via add_task() + 2. Orchestrator calls claim() to understand resource needs + 3. Each tick: is_active() → compute() → output merged via arbitration + 4. Task removed via remove_task() or transitions to inactive + + CRITICAL: Tasks must NEVER call time.time() directly. + Use state.t_now passed to compute() for all timing. + + Example: + >>> class MyTask: + ... @property + ... def name(self) -> str: + ... return "my_task" + ... + ... def claim(self) -> ResourceClaim: + ... return ResourceClaim( + ... joints=frozenset(["left_joint1", "left_joint2"]), + ... priority=10, + ... ) + ... + ... def is_active(self) -> bool: + ... return self._executing + ... + ... def compute(self, state: OrchestratorState) -> JointCommandOutput | None: + ... # Use state.t_now, NOT time.time()! + ... t_elapsed = state.t_now - self._start_time + ... positions = self._trajectory.sample(t_elapsed) + ... return JointCommandOutput( + ... joint_names=["left_joint1", "left_joint2"], + ... positions=positions, + ... ) + ... + ... def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: + ... print(f"Preempted by {by_task} on {joints}") + """ + + @property + def name(self) -> str: + """Unique identifier for this task instance. + + Used for logging, debugging, and task management. + Must be unique across all tasks in the orchestrator. + """ + ... + + def claim(self) -> ResourceClaim: + """Declare resource requirements. + + Called by orchestrator to determine: + - Which joints this task wants to control + - Priority for conflict resolution + - Control mode (position/velocity/effort) + + Returns: + ResourceClaim with joints (frozenset) and priority (int) + + Note: + The claim can change dynamically - orchestrator calls this + every tick for active tasks. + """ + ... + + def is_active(self) -> bool: + """Check if task should run this tick. + + Inactive tasks: + - Skip compute() call + - Don't participate in arbitration + - Don't consume resources + + Returns: + True if task should execute this tick + """ + ... + + def compute(self, state: OrchestratorState) -> JointCommandOutput | None: + """Compute output command given current state. + + Called by orchestrator for active tasks each tick. + + CRITICAL: Use state.t_now for timing, NEVER time.time()! + This ensures deterministic behavior and enables simulation. + + Args: + state: OrchestratorState containing: + - joints: JointStateSnapshot with all joint states + - t_now: Current tick time (use this for all timing!) + - dt: Time since last tick + + Returns: + JointCommandOutput with joint_names and values, or None if + no command should be sent this tick. + """ + ... + + def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: + """Called ONCE per tick with ALL preempted joints aggregated. + + Called when a higher-priority task takes control of some of this + task's joints. Allows task to gracefully handle being overridden. + + This is called ONCE per tick with ALL preempted joints, not once + per joint. This reduces noise and improves performance. + + Args: + by_task: Name of the preempting task (or "arbitration" if multiple) + joints: All joints that were preempted this tick + """ + ... + + +__all__ = [ + # Types + "ControlMode", + # Protocol + "ControlTask", + "JointCommandOutput", + "JointStateSnapshot", + "OrchestratorState", + "ResourceClaim", +] From 526519171a2ce3beb18b4ad08ecb8809b6bed62a Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 8 Jan 2026 13:01:33 -0800 Subject: [PATCH 13/42] hardware interface protocol that wraps specific arm SDK to work with orchestrator. Also solves namespace for multiple arm and hardware --- dimos/control/hardware_interface.py | 196 ++++++++++++++++++++++++++++ 1 file changed, 196 insertions(+) create mode 100644 dimos/control/hardware_interface.py diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py new file mode 100644 index 0000000000..ec67d1ae8d --- /dev/null +++ b/dimos/control/hardware_interface.py @@ -0,0 +1,196 @@ +# 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 interface for the ControlOrchestrator. + +Wraps ManipulatorBackend with orchestrator-specific features: +- Namespaced joint names (e.g., "left_joint1") +- Unified read/write interface +- Hold-last-value for partial commands +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Protocol, runtime_checkable + +from dimos.hardware.manipulators.spec import ControlMode + +if TYPE_CHECKING: + from dimos.hardware.manipulators.spec import ManipulatorBackend + + +@runtime_checkable +class HardwareInterface(Protocol): + """Protocol for hardware that the orchestrator can control. + + This wraps ManipulatorBackend with orchestrator-specific features: + - Namespaced joint names (e.g., "left_arm_joint1") + - Unified read/write interface + - State caching + """ + + @property + def hardware_id(self) -> str: + """Unique ID for this hardware (e.g., 'left_arm').""" + ... + + @property + def joint_names(self) -> list[str]: + """Ordered list of fully-qualified joint names this hardware controls.""" + ... + + def read_state(self) -> dict[str, tuple[float, float, float]]: + """Read current state. + + Returns: + Dict of joint_name -> (position, velocity, effort) + """ + ... + + def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: + """Write commands to hardware. + + IMPORTANT: Accepts partial joint sets. Missing joints hold last value. + + Args: + commands: {joint_name: value} - can be partial + mode: Control mode (POSITION, VELOCITY, TORQUE) + + Returns: + True if command was sent successfully + """ + ... + + +class BackendHardwareInterface: + """Concrete implementation wrapping a ManipulatorBackend. + + Features: + - Generates namespaced joint names (prefix_joint1, prefix_joint2, ...) + - Holds last commanded value for partial commands + - On first tick, reads current position from hardware for missing joints + """ + + def __init__( + self, + backend: ManipulatorBackend, + hardware_id: str, + joint_prefix: str | None = None, + ) -> None: + """Initialize hardware interface. + + Args: + backend: ManipulatorBackend instance (XArmBackend, PiperBackend, etc.) + hardware_id: Unique identifier for this hardware + joint_prefix: Prefix for joint names (defaults to hardware_id) + """ + self._backend = backend + self._hardware_id = hardware_id + self._prefix = joint_prefix or hardware_id + self._dof = backend.get_dof() + + # Generate joint names: prefix_joint1, prefix_joint2, ... + self._joint_names = [f"{self._prefix}_joint{i + 1}" for i in range(self._dof)] + + # Track last commanded values for hold-last behavior + self._last_commanded: dict[str, float] = {} + self._initialized = False + + @property + def hardware_id(self) -> str: + """Unique ID for this hardware.""" + return self._hardware_id + + @property + def joint_names(self) -> list[str]: + """Ordered list of joint names.""" + return self._joint_names + + @property + def dof(self) -> int: + """Degrees of freedom.""" + return self._dof + + def read_state(self) -> dict[str, tuple[float, float, float]]: + """Read state as {joint_name: (position, velocity, effort)}. + + Returns: + Dict mapping joint name to (position, velocity, effort) tuple + """ + positions = self._backend.read_joint_positions() + velocities = self._backend.read_joint_velocities() + efforts = self._backend.read_joint_efforts() + + return { + name: (positions[i], velocities[i], efforts[i]) + for i, name in enumerate(self._joint_names) + } + + def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: + """Write commands - allows partial joint sets, holds last for missing. + + This is critical for: + - Partial WBC overrides + - Safety controllers + - Mixed task ownership + + Args: + commands: {joint_name: value} - can be partial + mode: Control mode + + Returns: + True if command was sent successfully + """ + # Initialize on first write if needed + if not self._initialized: + self._initialize_last_commanded() + + # Update last commanded for joints we received + for joint_name, value in commands.items(): + if joint_name in self._joint_names: + self._last_commanded[joint_name] = value + + # Build ordered list for backend + ordered = self._build_ordered_command() + + # Send to backend + if mode == ControlMode.POSITION: + return self._backend.write_joint_positions(ordered) + elif mode == ControlMode.VELOCITY: + return self._backend.write_joint_velocities(ordered) + # TORQUE mode not universally supported + return False + + def _initialize_last_commanded(self) -> None: + """Initialize last_commanded with current hardware positions.""" + try: + current = self._backend.read_joint_positions() + for i, name in enumerate(self._joint_names): + self._last_commanded[name] = current[i] + self._initialized = True + except Exception: + # If read fails, initialize to zeros + for name in self._joint_names: + self._last_commanded[name] = 0.0 + self._initialized = True + + def _build_ordered_command(self) -> list[float]: + """Build ordered command list from last_commanded dict.""" + return [self._last_commanded[name] for name in self._joint_names] + + +__all__ = [ + "BackendHardwareInterface", + "HardwareInterface", +] From 3500ba5001a4a5208f894a8dfc7719c6d4bc9342 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 8 Jan 2026 13:04:05 -0800 Subject: [PATCH 14/42] main orchestrator module and control loop that claims resources computes next commands, and arbitrates priority of different tasks and controllers --- dimos/control/__init__.py | 92 +++++++ dimos/control/orchestrator.py | 469 ++++++++++++++++++++++++++++++++++ dimos/control/tick_loop.py | 362 ++++++++++++++++++++++++++ 3 files changed, 923 insertions(+) create mode 100644 dimos/control/__init__.py create mode 100644 dimos/control/orchestrator.py create mode 100644 dimos/control/tick_loop.py diff --git a/dimos/control/__init__.py b/dimos/control/__init__.py new file mode 100644 index 0000000000..3d7d647cd4 --- /dev/null +++ b/dimos/control/__init__.py @@ -0,0 +1,92 @@ +# 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. + +"""ControlOrchestrator - Centralized control for multi-arm coordination. + +This module provides a centralized control orchestrator that replaces +per-driver/per-controller loops with a single deterministic tick-based system. + +Features: +- Single tick loop (read → compute → arbitrate → route → write) +- Per-joint arbitration (highest priority wins) +- Mode conflict detection +- Partial command support (hold last value) +- Aggregated preemption notifications + +Example: + >>> from dimos.control import ControlOrchestrator + >>> from dimos.control.tasks import JointTrajectoryTask, JointTrajectoryTaskConfig + >>> from dimos.hardware.manipulators.xarm import XArmBackend + >>> + >>> # Create orchestrator + >>> orch = ControlOrchestrator(tick_rate=100.0) + >>> + >>> # Add hardware + >>> backend = XArmBackend(ip="192.168.1.185", dof=7) + >>> backend.connect() + >>> orch.add_hardware("left_arm", backend, joint_prefix="left") + >>> + >>> # Add task + >>> joints = [f"left_joint{i+1}" for i in range(7)] + >>> task = JointTrajectoryTask( + ... "traj_left", + ... JointTrajectoryTaskConfig(joint_names=joints, priority=10), + ... ) + >>> orch.add_task(task) + >>> + >>> # Start + >>> orch.start() +""" + +from dimos.control.hardware_interface import ( + BackendHardwareInterface, + HardwareInterface, +) +from dimos.control.orchestrator import ( + ControlOrchestrator, + ControlOrchestratorConfig, + HardwareConfig, + TaskConfig, + control_orchestrator, +) +from dimos.control.task import ( + ControlMode, + ControlTask, + JointCommandOutput, + JointStateSnapshot, + OrchestratorState, + ResourceClaim, +) +from dimos.control.tick_loop import TickLoop + +__all__ = [ + # Hardware interface + "BackendHardwareInterface", + "ControlMode", + # Orchestrator + "ControlOrchestrator", + "ControlOrchestratorConfig", + # Task protocol and types + "ControlTask", + "HardwareConfig", + "HardwareInterface", + "JointCommandOutput", + "JointStateSnapshot", + "OrchestratorState", + "ResourceClaim", + "TaskConfig", + # Tick loop + "TickLoop", + "control_orchestrator", +] diff --git a/dimos/control/orchestrator.py b/dimos/control/orchestrator.py new file mode 100644 index 0000000000..0ce84e6b77 --- /dev/null +++ b/dimos/control/orchestrator.py @@ -0,0 +1,469 @@ +# 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. + +"""ControlOrchestrator module. + +Centralized control orchestrator that replaces per-driver/per-controller +loops with a single deterministic tick-based system. + +Features: +- Single tick loop (read → compute → arbitrate → route → write) +- Per-joint arbitration (highest priority wins) +- Mode conflict detection +- Partial command support (hold last value) +- Aggregated preemption notifications +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +import threading +import time +from typing import TYPE_CHECKING, Any + +from dimos.control.hardware_interface import BackendHardwareInterface, HardwareInterface +from dimos.control.tick_loop import TickLoop +from dimos.core import Module, Out, rpc +from dimos.core.module import ModuleConfig +from dimos.msgs.trajectory_msgs import JointTrajectory, TrajectoryState +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from dimos.control.task import ControlTask + from dimos.hardware.manipulators.spec import ManipulatorBackend + from dimos.msgs.sensor_msgs import JointState + +logger = setup_logger() + + +# ============================================================================= +# Configuration +# ============================================================================= + + +@dataclass +class HardwareConfig: + """Configuration for a hardware backend. + + Attributes: + id: Unique hardware identifier (e.g., "arm", "left_arm") + type: Backend type ("mock", "xarm", "piper") + dof: Degrees of freedom + joint_prefix: Prefix for joint names (defaults to id) + ip: IP address (required for xarm) + can_port: CAN port (for piper, default "can0") + auto_enable: Whether to auto-enable servos (default True) + """ + + id: str + type: str = "mock" + dof: int = 7 + joint_prefix: str | None = None + ip: str | None = None + can_port: str | None = None + auto_enable: bool = True + + +@dataclass +class TaskConfig: + """Configuration for a control task. + + Attributes: + name: Task name (e.g., "traj_arm") + type: Task type ("trajectory") + joint_names: List of joint names this task controls + priority: Task priority (higher wins arbitration) + """ + + name: str + type: str = "trajectory" + joint_names: list[str] = field(default_factory=lambda: []) + priority: int = 10 + + +@dataclass +class ControlOrchestratorConfig(ModuleConfig): + """Configuration for the ControlOrchestrator. + + Attributes: + tick_rate: Control loop frequency in Hz (default: 100) + publish_joint_state: Whether to publish aggregated JointState + joint_state_frame_id: Frame ID for published JointState + log_ticks: Whether to log tick information (verbose) + hardware: List of hardware configurations to create on start + tasks: List of task configurations to create on start + """ + + tick_rate: float = 100.0 + publish_joint_state: bool = True + joint_state_frame_id: str = "orchestrator" + log_ticks: bool = False + hardware: list[HardwareConfig] = field(default_factory=lambda: []) + tasks: list[TaskConfig] = field(default_factory=lambda: []) + + +# ============================================================================= +# ControlOrchestrator Module +# ============================================================================= + + +class ControlOrchestrator(Module[ControlOrchestratorConfig]): + """Centralized control orchestrator with per-joint arbitration. + + Single tick loop that: + 1. Reads state from all hardware + 2. Runs all active tasks + 3. Arbitrates conflicts per-joint (highest priority wins) + 4. Routes commands to hardware + 5. Publishes aggregated joint state + + Key design decisions: + - Joint-centric commands (not hardware-centric) + - Per-joint arbitration (not per-hardware) + - Centralized time (tasks use state.t_now, never time.time()) + - Partial commands OK (hardware holds last value) + - Aggregated preemption (one notification per task per tick) + + Example: + >>> from dimos.control import ControlOrchestrator + >>> from dimos.hardware.manipulators.xarm import XArmBackend + >>> + >>> orch = ControlOrchestrator(tick_rate=100.0) + >>> backend = XArmBackend(ip="192.168.1.185", dof=7) + >>> backend.connect() + >>> orch.add_hardware("left_arm", backend, joint_prefix="left") + >>> orch.start() + """ + + # Output: Aggregated joint state for external consumers + joint_state: Out[JointState] + + config: ControlOrchestratorConfig + default_config = ControlOrchestratorConfig + + def __init__(self, *args: Any, **kwargs: Any) -> None: + super().__init__(*args, **kwargs) + + # Hardware interfaces (keyed by hardware_id) + self._hardware: dict[str, HardwareInterface] = {} + self._hardware_lock = threading.Lock() + + # Joint -> hardware mapping (built when hardware added) + self._joint_to_hardware: dict[str, str] = {} + + # Registered tasks + self._tasks: dict[str, ControlTask] = {} + self._task_lock = threading.Lock() + + # Tick loop (created on start) + self._tick_loop: TickLoop | None = None + + logger.info(f"ControlOrchestrator initialized at {self.config.tick_rate}Hz") + + # ========================================================================= + # Config-based Setup + # ========================================================================= + + def _setup_from_config(self) -> None: + """Create hardware and tasks from config (called on start).""" + for hw_cfg in self.config.hardware: + try: + backend = self._create_backend_from_config(hw_cfg) + if not backend.connect(): + raise RuntimeError(f"Failed to connect to {hw_cfg.type} backend") + + if hw_cfg.auto_enable and hasattr(backend, "write_enable"): + backend.write_enable(True) + + self.add_hardware( + hw_cfg.id, + backend, + joint_prefix=hw_cfg.joint_prefix or hw_cfg.id, + ) + except Exception as e: + logger.error(f"Failed to setup hardware {hw_cfg.id}: {e}") + raise + + for task_cfg in self.config.tasks: + try: + task = self._create_task_from_config(task_cfg) + self.add_task(task) + except Exception as e: + logger.error(f"Failed to setup task {task_cfg.name}: {e}") + raise + + def _create_backend_from_config(self, cfg: HardwareConfig) -> ManipulatorBackend: + """Create a manipulator backend from config.""" + backend_type = cfg.type.lower() + + if backend_type == "mock": + from dimos.hardware.manipulators.mock import MockBackend + + return MockBackend(dof=cfg.dof) + + elif backend_type == "xarm": + if cfg.ip is None: + raise ValueError("ip is required for xarm backend") + from dimos.hardware.manipulators.xarm import XArmBackend + + return XArmBackend(ip=cfg.ip, dof=cfg.dof) + + elif backend_type == "piper": + from dimos.hardware.manipulators.piper import PiperBackend + + return PiperBackend(can_port=cfg.can_port or "can0", dof=cfg.dof) + + else: + raise ValueError(f"Unknown backend type: {backend_type}") + + def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask: + """Create a control task from config.""" + task_type = cfg.type.lower() + + if task_type == "trajectory": + from dimos.control.tasks import JointTrajectoryTask, JointTrajectoryTaskConfig + + return JointTrajectoryTask( + cfg.name, + JointTrajectoryTaskConfig( + joint_names=cfg.joint_names, + priority=cfg.priority, + ), + ) + + else: + raise ValueError(f"Unknown task type: {task_type}") + + # ========================================================================= + # Hardware Management (RPC) + # ========================================================================= + + @rpc + def add_hardware( + self, + hardware_id: str, + backend: ManipulatorBackend, + joint_prefix: str | None = None, + ) -> bool: + """Register a hardware backend with the orchestrator.""" + with self._hardware_lock: + if hardware_id in self._hardware: + logger.warning(f"Hardware {hardware_id} already registered") + return False + + interface = BackendHardwareInterface( + backend=backend, + hardware_id=hardware_id, + joint_prefix=joint_prefix, + ) + self._hardware[hardware_id] = interface + + for joint_name in interface.joint_names: + self._joint_to_hardware[joint_name] = hardware_id + + logger.info(f"Added hardware {hardware_id} with joints: {interface.joint_names}") + return True + + @rpc + def remove_hardware(self, hardware_id: str) -> bool: + """Remove a hardware interface.""" + with self._hardware_lock: + if hardware_id not in self._hardware: + return False + + interface = self._hardware[hardware_id] + for joint_name in interface.joint_names: + del self._joint_to_hardware[joint_name] + + del self._hardware[hardware_id] + logger.info(f"Removed hardware {hardware_id}") + return True + + @rpc + def list_hardware(self) -> list[str]: + """List registered hardware IDs.""" + with self._hardware_lock: + return list(self._hardware.keys()) + + @rpc + def list_joints(self) -> list[str]: + """List all joint names across all hardware.""" + with self._hardware_lock: + return list(self._joint_to_hardware.keys()) + + # ========================================================================= + # Task Management (RPC) + # ========================================================================= + + @rpc + def add_task(self, task: ControlTask) -> bool: + """Register a task with the orchestrator.""" + with self._task_lock: + if task.name in self._tasks: + logger.warning(f"Task {task.name} already registered") + return False + self._tasks[task.name] = task + logger.info(f"Added task {task.name}") + return True + + @rpc + def remove_task(self, task_name: str) -> bool: + """Remove a task by name.""" + with self._task_lock: + if task_name in self._tasks: + del self._tasks[task_name] + logger.info(f"Removed task {task_name}") + return True + return False + + @rpc + def get_task(self, task_name: str) -> ControlTask | None: + """Get a task by name.""" + with self._task_lock: + return self._tasks.get(task_name) + + @rpc + def list_tasks(self) -> list[str]: + """List registered task names.""" + with self._task_lock: + return list(self._tasks.keys()) + + @rpc + def get_active_tasks(self) -> list[str]: + """List currently active task names.""" + with self._task_lock: + return [name for name, task in self._tasks.items() if task.is_active()] + + # ========================================================================= + # Trajectory Execution (RPC) + # ========================================================================= + + @rpc + def execute_trajectory(self, task_name: str, trajectory: JointTrajectory) -> bool: + """Execute a trajectory on a named task.""" + with self._task_lock: + task = self._tasks.get(task_name) + if task is None: + logger.warning(f"Task {task_name} not found") + return False + + if not hasattr(task, "execute"): + logger.warning(f"Task {task_name} doesn't support execute()") + return False + + t_now = time.perf_counter() + logger.info( + f"Executing trajectory on {task_name}: " + f"{len(trajectory.points)} points, duration={trajectory.duration:.3f}s" + ) + return task.execute(trajectory, t_now) + + @rpc + def get_trajectory_status(self, task_name: str) -> dict[str, Any]: + """Get the status of a trajectory task.""" + with self._task_lock: + task = self._tasks.get(task_name) + if task is None: + return {} + + result: dict[str, Any] = {"active": task.is_active()} + + if hasattr(task, "get_state"): + state = task.get_state() + result["state"] = state.name if isinstance(state, TrajectoryState) else str(state) + + if hasattr(task, "get_progress"): + t_now = time.perf_counter() + result["progress"] = task.get_progress(t_now) + + return result + + @rpc + def cancel_trajectory(self, task_name: str) -> bool: + """Cancel an active trajectory on a task.""" + with self._task_lock: + task = self._tasks.get(task_name) + if task is None: + logger.warning(f"Task {task_name} not found") + return False + + if not hasattr(task, "cancel"): + logger.warning(f"Task {task_name} doesn't support cancel()") + return False + + logger.info(f"Cancelling trajectory on {task_name}") + return task.cancel() + + # ========================================================================= + # Lifecycle + # ========================================================================= + + @rpc + def start(self) -> None: + """Start the orchestrator control loop.""" + super().start() + + if self._tick_loop and self._tick_loop.tick_count > 0: + logger.warning("Orchestrator already running") + return + + # Setup hardware and tasks from config (if any) + if self.config.hardware or self.config.tasks: + self._setup_from_config() + + # Create and start tick loop + publish_cb = self.joint_state.publish if self.config.publish_joint_state else None + self._tick_loop = TickLoop( + tick_rate=self.config.tick_rate, + hardware=self._hardware, + hardware_lock=self._hardware_lock, + tasks=self._tasks, + task_lock=self._task_lock, + joint_to_hardware=self._joint_to_hardware, + publish_callback=publish_cb, + frame_id=self.config.joint_state_frame_id, + log_ticks=self.config.log_ticks, + ) + self._tick_loop.start() + + logger.info(f"ControlOrchestrator started at {self.config.tick_rate}Hz") + + @rpc + def stop(self) -> None: + """Stop the orchestrator.""" + logger.info("Stopping ControlOrchestrator...") + + if self._tick_loop: + self._tick_loop.stop() + + super().stop() + logger.info("ControlOrchestrator stopped") + + @rpc + def get_tick_count(self) -> int: + """Get the number of ticks since start.""" + return self._tick_loop.tick_count if self._tick_loop else 0 + + +# Blueprint export +control_orchestrator = ControlOrchestrator.blueprint + + +__all__ = [ + "ControlOrchestrator", + "ControlOrchestratorConfig", + "HardwareConfig", + "TaskConfig", + "control_orchestrator", +] diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py new file mode 100644 index 0000000000..bf687f0b2c --- /dev/null +++ b/dimos/control/tick_loop.py @@ -0,0 +1,362 @@ +# 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. + +"""Tick loop for the ControlOrchestrator. + +This module contains the core control loop logic: +- Read state from all hardware +- Compute outputs from all active tasks +- Arbitrate conflicts per-joint (highest priority wins) +- Route commands to hardware +- Publish aggregated joint state + +Separated from orchestrator.py following the DimOS pattern of +splitting coordination logic from module wrapper. +""" + +from __future__ import annotations + +import threading +import time +from typing import TYPE_CHECKING + +from dimos.control.task import ( + ControlTask, + JointCommandOutput, + JointStateSnapshot, + OrchestratorState, + ResourceClaim, +) +from dimos.msgs.sensor_msgs import JointState +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from collections.abc import Callable + + from dimos.control.hardware_interface import HardwareInterface + from dimos.hardware.manipulators.spec import ControlMode + +logger = setup_logger() + + +class TickLoop: + """Core tick loop for the control orchestrator. + + Runs the deterministic control cycle: + 1. READ: Collect joint state from all hardware + 2. COMPUTE: Run all active tasks + 3. ARBITRATE: Per-joint conflict resolution (highest priority wins) + 4. NOTIFY: Send preemption notifications to affected tasks + 5. ROUTE: Convert joint-centric commands to hardware-centric + 6. WRITE: Send commands to hardware + 7. PUBLISH: Output aggregated JointState + + Args: + tick_rate: Control loop frequency in Hz + hardware: Dict of hardware_id -> HardwareInterface + hardware_lock: Lock protecting hardware dict + tasks: Dict of task_name -> ControlTask + task_lock: Lock protecting tasks dict + joint_to_hardware: Dict mapping joint_name -> hardware_id + publish_callback: Optional callback to publish JointState + frame_id: Frame ID for published JointState + log_ticks: Whether to log tick information + """ + + def __init__( + self, + tick_rate: float, + hardware: dict[str, HardwareInterface], + hardware_lock: threading.Lock, + tasks: dict[str, ControlTask], + task_lock: threading.Lock, + joint_to_hardware: dict[str, str], + publish_callback: Callable[[JointState], None] | None = None, + frame_id: str = "orchestrator", + log_ticks: bool = False, + ) -> None: + self._tick_rate = tick_rate + self._hardware = hardware + self._hardware_lock = hardware_lock + self._tasks = tasks + self._task_lock = task_lock + self._joint_to_hardware = joint_to_hardware + self._publish_callback = publish_callback + self._frame_id = frame_id + self._log_ticks = log_ticks + + self._running = False + self._tick_thread: threading.Thread | None = None + self._last_tick_time: float = 0.0 + self._tick_count: int = 0 + + @property + def tick_count(self) -> int: + """Number of ticks since start.""" + return self._tick_count + + def start(self) -> None: + """Start the tick loop in a daemon thread.""" + if self._running: + logger.warning("TickLoop already running") + return + + self._running = True + self._last_tick_time = time.perf_counter() + self._tick_count = 0 + + self._tick_thread = threading.Thread( + target=self._loop, + name="ControlOrchestrator-Tick", + daemon=True, + ) + self._tick_thread.start() + logger.info(f"TickLoop started at {self._tick_rate}Hz") + + def stop(self) -> None: + """Stop the tick loop.""" + self._running = False + if self._tick_thread and self._tick_thread.is_alive(): + self._tick_thread.join(timeout=2.0) + logger.info("TickLoop stopped") + + def _loop(self) -> None: + """Main control loop - deterministic read → compute → arbitrate → write.""" + period = 1.0 / self._tick_rate + + while self._running: + tick_start = time.perf_counter() + + try: + self._tick() + except Exception as e: + logger.error(f"TickLoop tick error: {e}") + + # Rate control + elapsed = time.perf_counter() - tick_start + if elapsed < period: + time.sleep(period - elapsed) + + def _tick(self) -> None: + """Single tick: read → compute → arbitrate → route → write.""" + t_now = time.perf_counter() + dt = t_now - self._last_tick_time + self._last_tick_time = t_now + self._tick_count += 1 + + # === PHASE 1: READ ALL HARDWARE === + joint_states = self._read_all_hardware() + state = OrchestratorState(joints=joint_states, t_now=t_now, dt=dt) + + # === PHASE 2: COMPUTE ALL ACTIVE TASKS === + commands = self._compute_all_tasks(state) + + # === PHASE 3: ARBITRATE (with mode validation) === + joint_commands, preemptions = self._arbitrate(commands) + + # === PHASE 4: NOTIFY PREEMPTIONS (once per task) === + self._notify_preemptions(preemptions) + + # === PHASE 5: ROUTE TO HARDWARE === + hw_commands = self._route_to_hardware(joint_commands) + + # === PHASE 6: WRITE TO HARDWARE === + self._write_all_hardware(hw_commands) + + # === PHASE 7: PUBLISH AGGREGATED STATE === + if self._publish_callback: + self._publish_joint_state(joint_states) + + # Optional logging + if self._log_ticks: + active = len([c for c in commands if c[2] is not None]) + logger.debug( + f"Tick {self._tick_count}: dt={dt:.4f}s, " + f"{len(joint_states.joint_positions)} joints, " + f"{active} active tasks" + ) + + def _read_all_hardware(self) -> JointStateSnapshot: + """Read state from all hardware interfaces.""" + joint_positions: dict[str, float] = {} + joint_velocities: dict[str, float] = {} + joint_efforts: dict[str, float] = {} + + with self._hardware_lock: + for hw in self._hardware.values(): + try: + state = hw.read_state() + for joint_name, (pos, vel, eff) in state.items(): + joint_positions[joint_name] = pos + joint_velocities[joint_name] = vel + joint_efforts[joint_name] = eff + except Exception as e: + logger.error(f"Failed to read {hw.hardware_id}: {e}") + + return JointStateSnapshot( + joint_positions=joint_positions, + joint_velocities=joint_velocities, + joint_efforts=joint_efforts, + timestamp=time.time(), + ) + + def _compute_all_tasks( + self, state: OrchestratorState + ) -> list[tuple[ControlTask, ResourceClaim, JointCommandOutput | None]]: + """Compute outputs from all active tasks.""" + results: list[tuple[ControlTask, ResourceClaim, JointCommandOutput | None]] = [] + + with self._task_lock: + for task in self._tasks.values(): + if not task.is_active(): + continue + + try: + claim = task.claim() + output = task.compute(state) + results.append((task, claim, output)) + except Exception as e: + logger.error(f"Task {task.name} compute error: {e}") + + return results + + def _arbitrate( + self, + commands: list[tuple[ControlTask, ResourceClaim, JointCommandOutput | None]], + ) -> tuple[ + dict[str, tuple[float, ControlMode, str]], + dict[str, set[str]], + ]: + """Per-joint arbitration with mode conflict detection. + + Returns: + Tuple of: + - joint_commands: {joint_name: (value, mode, task_name)} + - preemptions: {task_name: set of preempted joints} + """ + # Track winner per joint: joint -> (priority, value, mode, task_name) + winners: dict[str, tuple[int, float, ControlMode, str]] = {} + preemptions: dict[str, set[str]] = {} + + for task, claim, output in commands: + if output is None: + continue + + values = output.get_values() + if values is None: + continue + + for i, joint_name in enumerate(output.joint_names): + value = values[i] + priority = claim.priority + mode = output.mode + + if joint_name not in winners: + # First claim on this joint + winners[joint_name] = (priority, value, mode, task.name) + + elif priority > winners[joint_name][0]: + # Higher priority wins - track preemption + old_task = winners[joint_name][3] + if old_task not in preemptions: + preemptions[old_task] = set() + preemptions[old_task].add(joint_name) + winners[joint_name] = (priority, value, mode, task.name) + + elif priority == winners[joint_name][0]: + # Same priority + if mode != winners[joint_name][2]: + # Mode conflict at same priority - log and drop + logger.warning( + f"Mode conflict on {joint_name}: {task.name} wants " + f"{mode.value}, but {winners[joint_name][3]} wants " + f"{winners[joint_name][2].value}. Dropping {task.name}." + ) + # If same mode and same priority, first wins (keep existing) + + # Convert to simplified output: joint -> (value, mode, task_name) + joint_commands = { + joint: (value, mode, task_name) + for joint, (_, value, mode, task_name) in winners.items() + } + + return joint_commands, preemptions + + def _notify_preemptions(self, preemptions: dict[str, set[str]]) -> None: + """Notify each preempted task ONCE with all affected joints.""" + with self._task_lock: + for task_name, joints in preemptions.items(): + task = self._tasks.get(task_name) + if task: + try: + task.on_preempted( + by_task="higher_priority", + joints=frozenset(joints), + ) + except Exception as e: + logger.error(f"Error notifying {task_name} of preemption: {e}") + + def _route_to_hardware( + self, + joint_commands: dict[str, tuple[float, ControlMode, str]], + ) -> dict[str, tuple[dict[str, float], ControlMode]]: + """Route joint-centric commands to hardware. + + Returns: + {hardware_id: ({joint: value}, mode)} + """ + hw_commands: dict[str, tuple[dict[str, float], ControlMode]] = {} + + for joint_name, (value, mode, _) in joint_commands.items(): + hw_id = self._joint_to_hardware.get(joint_name) + if hw_id is None: + logger.warning(f"Unknown joint {joint_name}, cannot route") + continue + + if hw_id not in hw_commands: + hw_commands[hw_id] = ({}, mode) + + hw_commands[hw_id][0][joint_name] = value + + return hw_commands + + def _write_all_hardware( + self, + hw_commands: dict[str, tuple[dict[str, float], ControlMode]], + ) -> None: + """Write commands to all hardware interfaces.""" + with self._hardware_lock: + for hw_id, (positions, mode) in hw_commands.items(): + if hw_id in self._hardware: + try: + self._hardware[hw_id].write_command(positions, mode) + except Exception as e: + logger.error(f"Failed to write to {hw_id}: {e}") + + def _publish_joint_state(self, snapshot: JointStateSnapshot) -> None: + """Publish aggregated JointState for external consumers.""" + names = list(snapshot.joint_positions.keys()) + msg = JointState( + ts=snapshot.timestamp, + frame_id=self._frame_id, + name=names, + position=[snapshot.joint_positions[n] for n in names], + velocity=[snapshot.joint_velocities.get(n, 0.0) for n in names], + effort=[snapshot.joint_efforts.get(n, 0.0) for n in names], + ) + if self._publish_callback: + self._publish_callback(msg) + + +__all__ = ["TickLoop"] From f33e2c7ad8b20b8b93307c442697c5d2aa204b3d Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 8 Jan 2026 13:05:00 -0800 Subject: [PATCH 15/42] added a trajectory task implementation that performs trajecotry control --- dimos/control/tasks/__init__.py | 25 +++ dimos/control/tasks/trajectory_task.py | 249 +++++++++++++++++++++++++ 2 files changed, 274 insertions(+) create mode 100644 dimos/control/tasks/__init__.py create mode 100644 dimos/control/tasks/trajectory_task.py diff --git a/dimos/control/tasks/__init__.py b/dimos/control/tasks/__init__.py new file mode 100644 index 0000000000..75460ffa26 --- /dev/null +++ b/dimos/control/tasks/__init__.py @@ -0,0 +1,25 @@ +# 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. + +"""Task implementations for the ControlOrchestrator.""" + +from dimos.control.tasks.trajectory_task import ( + JointTrajectoryTask, + JointTrajectoryTaskConfig, +) + +__all__ = [ + "JointTrajectoryTask", + "JointTrajectoryTaskConfig", +] diff --git a/dimos/control/tasks/trajectory_task.py b/dimos/control/tasks/trajectory_task.py new file mode 100644 index 0000000000..c2d08e4934 --- /dev/null +++ b/dimos/control/tasks/trajectory_task.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. + +"""Joint trajectory task for the ControlOrchestrator. + +Passive trajectory execution - called by orchestrator each tick. +Unlike JointTrajectoryController which owns a thread, this task +is compute-only and relies on the orchestrator for timing. + +CRITICAL: Uses t_now from OrchestratorState, never calls time.time() +""" + +from __future__ import annotations + +from dataclasses import dataclass + +from dimos.control.task import ( + ControlMode, + ControlTask, + JointCommandOutput, + OrchestratorState, + ResourceClaim, +) +from dimos.msgs.trajectory_msgs import JointTrajectory, TrajectoryState +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +@dataclass +class JointTrajectoryTaskConfig: + """Configuration for trajectory task. + + Attributes: + joint_names: List of joint names this task controls + priority: Priority for arbitration (higher wins) + """ + + joint_names: list[str] + priority: int = 10 + + +class JointTrajectoryTask: + """Passive trajectory execution task. + + Unlike JointTrajectoryController which owns a thread, this task + is called by the orchestrator at each tick. + + CRITICAL: Uses t_now from OrchestratorState, never calls time.time() + + State Machine: + IDLE ──execute()──► EXECUTING ──done──► COMPLETED + ▲ │ │ + │ cancel() reset() + │ ▼ │ + └─────reset()───── ABORTED ◄──────────────┘ + + Example: + >>> task = JointTrajectoryTask( + ... name="traj_left", + ... config=JointTrajectoryTaskConfig( + ... joint_names=["left_joint1", "left_joint2"], + ... priority=10, + ... ), + ... ) + >>> orchestrator.add_task(task) + >>> task.execute(my_trajectory, t_now=orchestrator_t_now) + """ + + def __init__(self, name: str, config: JointTrajectoryTaskConfig) -> None: + """Initialize trajectory task. + + Args: + name: Unique task name + config: Task configuration + """ + self._name = name + self._config = config + self._joint_names = frozenset(config.joint_names) + self._joint_names_list = list(config.joint_names) + + # State machine + self._state = TrajectoryState.IDLE + self._trajectory: JointTrajectory | None = None + self._start_time: float = 0.0 # In orchestrator's t_now space + + logger.info(f"JointTrajectoryTask {name} initialized for joints: {config.joint_names}") + + @property + def name(self) -> str: + """Unique task identifier.""" + return self._name + + def claim(self) -> ResourceClaim: + """Declare resource requirements.""" + return ResourceClaim( + joints=self._joint_names, + priority=self._config.priority, + mode=ControlMode.POSITION, + ) + + def is_active(self) -> bool: + """Check if task should run this tick.""" + return self._state == TrajectoryState.EXECUTING + + def compute(self, state: OrchestratorState) -> JointCommandOutput | None: + """Compute trajectory output for this tick. + + CRITICAL: Uses state.t_now for timing, NOT time.time()! + + Args: + state: Current orchestrator state + + Returns: + JointCommandOutput with positions, or None if not executing + """ + if self._trajectory is None: + return None + + # Use orchestrator's time, NOT time.time() + t_elapsed = state.t_now - self._start_time + + # Check completion + if t_elapsed >= self._trajectory.duration: + self._state = TrajectoryState.COMPLETED + logger.info(f"Trajectory {self._name} completed after {t_elapsed:.3f}s") + return None + + # Sample trajectory + q_ref, _ = self._trajectory.sample(t_elapsed) + + return JointCommandOutput( + joint_names=self._joint_names_list, + positions=list(q_ref), + mode=ControlMode.POSITION, + ) + + def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: + """Handle preemption by higher-priority task. + + Args: + by_task: Name of preempting task + joints: Joints that were preempted + """ + logger.warning(f"Trajectory {self._name} preempted by {by_task} on joints {joints}") + # Abort if any of our joints were preempted + if joints & self._joint_names: + self._state = TrajectoryState.ABORTED + + # ========================================================================= + # Task-specific methods + # ========================================================================= + + def execute(self, trajectory: JointTrajectory, t_now: float) -> bool: + """Start executing a trajectory. + + Args: + trajectory: Trajectory to execute + t_now: Current orchestrator time (from state.t_now) + + Returns: + True if accepted, False if invalid or in FAULT state + """ + if self._state == TrajectoryState.FAULT: + logger.warning(f"Cannot execute: {self._name} in FAULT state") + return False + + if trajectory is None or trajectory.duration <= 0: + logger.warning(f"Invalid trajectory for {self._name}") + return False + + if not trajectory.points: + logger.warning(f"Empty trajectory for {self._name}") + return False + + # Preempt any active trajectory + if self._state == TrajectoryState.EXECUTING: + logger.info(f"Preempting active trajectory on {self._name}") + + self._trajectory = trajectory + self._start_time = t_now # Use orchestrator's time + self._state = TrajectoryState.EXECUTING + + logger.info( + f"Executing trajectory on {self._name}: " + f"{len(trajectory.points)} points, duration={trajectory.duration:.3f}s" + ) + return True + + def cancel(self) -> bool: + """Cancel current trajectory. + + Returns: + True if cancelled, False if not executing + """ + if self._state != TrajectoryState.EXECUTING: + return False + self._state = TrajectoryState.ABORTED + logger.info(f"Trajectory {self._name} cancelled") + return True + + def reset(self) -> bool: + """Reset to idle state. + + Returns: + True if reset, False if currently executing + """ + if self._state == TrajectoryState.EXECUTING: + logger.warning(f"Cannot reset {self._name} while executing") + return False + self._state = TrajectoryState.IDLE + self._trajectory = None + logger.info(f"Trajectory {self._name} reset to IDLE") + return True + + def get_state(self) -> TrajectoryState: + """Get current state.""" + return self._state + + def get_progress(self, t_now: float) -> float: + """Get execution progress (0.0 to 1.0). + + Args: + t_now: Current orchestrator time + + Returns: + Progress as fraction, or 0.0 if not executing + """ + if self._state != TrajectoryState.EXECUTING or self._trajectory is None: + return 0.0 + t_elapsed = t_now - self._start_time + return min(1.0, t_elapsed / self._trajectory.duration) + + +__all__ = [ + "JointTrajectoryTask", + "JointTrajectoryTaskConfig", +] From b36f13b983bd31ba1b064c7adc737b69abeb95b1 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 8 Jan 2026 13:05:28 -0800 Subject: [PATCH 16/42] added blueprints to launch orchestratory module with differnt arms for testing --- dimos/control/blueprints.py | 323 ++++++++++++++++++++++++++++++++++ dimos/robot/all_blueprints.py | 9 + 2 files changed, 332 insertions(+) create mode 100644 dimos/control/blueprints.py diff --git a/dimos/control/blueprints.py b/dimos/control/blueprints.py new file mode 100644 index 0000000000..96463cd746 --- /dev/null +++ b/dimos/control/blueprints.py @@ -0,0 +1,323 @@ +# 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 ControlOrchestrator. + +This module provides ready-to-use orchestrator blueprints for common setups. + +Usage: + # Run via CLI: + dimos run orchestrator-mock # Mock 7-DOF arm + dimos run orchestrator-xarm7 # XArm7 real hardware + dimos run orchestrator-dual-mock # Dual mock arms + + # Or programmatically: + from dimos.control.blueprints import orchestrator_mock + coordinator = orchestrator_mock.build() + coordinator.loop() + +Example with trajectory setter: + # Terminal 1: Run the orchestrator + dimos run orchestrator-mock + + # Terminal 2: Send trajectories via RPC + python -m dimos.control.examples.orchestrator_trajectory_setter --task traj_arm +""" + +from __future__ import annotations + +from dimos.control.orchestrator import ( + ControlOrchestrator, + HardwareConfig, + TaskConfig, + control_orchestrator, +) +from dimos.core.transport import LCMTransport +from dimos.msgs.sensor_msgs import JointState + +# ============================================================================= +# Helper function to generate joint names +# ============================================================================= + + +def _joint_names(prefix: str, dof: int) -> list[str]: + """Generate joint names with prefix.""" + return [f"{prefix}_joint{i + 1}" for i in range(dof)] + + +# ============================================================================= +# Single Arm Blueprints +# ============================================================================= + +# Mock 7-DOF arm (for testing) +orchestrator_mock = control_orchestrator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="orchestrator", + hardware=[ + HardwareConfig( + id="arm", + type="mock", + dof=7, + joint_prefix="arm", + ), + ], + tasks=[ + TaskConfig( + name="traj_arm", + type="trajectory", + joint_names=_joint_names("arm", 7), + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState), + } +) + +# XArm7 real hardware (requires IP configuration) +orchestrator_xarm7 = control_orchestrator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="orchestrator", + hardware=[ + HardwareConfig( + id="arm", + type="xarm", + dof=7, + joint_prefix="arm", + ip="192.168.2.235", # Default IP, override via env or config + auto_enable=True, + ), + ], + tasks=[ + TaskConfig( + name="traj_arm", + type="trajectory", + joint_names=_joint_names("arm", 7), + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState), + } +) + +# XArm6 real hardware +orchestrator_xarm6 = control_orchestrator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="orchestrator", + hardware=[ + HardwareConfig( + id="arm", + type="xarm", + dof=6, + joint_prefix="arm", + ip="192.168.1.185", + auto_enable=True, + ), + ], + tasks=[ + TaskConfig( + name="traj_arm", + type="trajectory", + joint_names=_joint_names("arm", 6), + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState), + } +) + +# Piper arm (6-DOF, CAN bus) +orchestrator_piper = control_orchestrator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="orchestrator", + hardware=[ + HardwareConfig( + id="arm", + type="piper", + dof=6, + joint_prefix="arm", + can_port="can0", + auto_enable=True, + ), + ], + tasks=[ + TaskConfig( + name="traj_arm", + type="trajectory", + joint_names=_joint_names("arm", 6), + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState), + } +) + +# ============================================================================= +# Dual Arm Blueprints +# ============================================================================= + +# Dual mock arms (7-DOF left, 6-DOF right) for testing +orchestrator_dual_mock = control_orchestrator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="orchestrator", + hardware=[ + HardwareConfig( + id="left_arm", + type="mock", + dof=7, + joint_prefix="left", + ), + HardwareConfig( + id="right_arm", + type="mock", + dof=6, + joint_prefix="right", + ), + ], + tasks=[ + TaskConfig( + name="traj_left", + type="trajectory", + joint_names=_joint_names("left", 7), + priority=10, + ), + TaskConfig( + name="traj_right", + type="trajectory", + joint_names=_joint_names("right", 6), + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState), + } +) + +# Dual XArm setup (XArm7 left, XArm6 right) +orchestrator_dual_xarm = control_orchestrator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="orchestrator", + hardware=[ + HardwareConfig( + id="left_arm", + type="xarm", + dof=7, + joint_prefix="left", + ip="192.168.2.235", + auto_enable=True, + ), + HardwareConfig( + id="right_arm", + type="xarm", + dof=6, + joint_prefix="right", + ip="192.168.1.210", + auto_enable=True, + ), + ], + tasks=[ + TaskConfig( + name="traj_left", + type="trajectory", + joint_names=_joint_names("left", 7), + priority=10, + ), + TaskConfig( + name="traj_right", + type="trajectory", + joint_names=_joint_names("right", 6), + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState), + } +) + +# ============================================================================= +# High-frequency Blueprints (200Hz) +# ============================================================================= + +# High-frequency mock for demanding applications +orchestrator_highfreq_mock = control_orchestrator( + tick_rate=200.0, + publish_joint_state=True, + joint_state_frame_id="orchestrator", + hardware=[ + HardwareConfig( + id="arm", + type="mock", + dof=7, + joint_prefix="arm", + ), + ], + tasks=[ + TaskConfig( + name="traj_arm", + type="trajectory", + joint_names=_joint_names("arm", 7), + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState), + } +) + +# ============================================================================= +# Raw Blueprints (no hardware/tasks configured - for programmatic setup) +# ============================================================================= + +# Basic orchestrator with transport only (add hardware/tasks programmatically) +orchestrator_basic = control_orchestrator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="orchestrator", +).transports( + { + ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState), + } +) + + +__all__ = [ + # Raw blueprints (for programmatic setup) + "orchestrator_basic", + # Dual arm blueprints + "orchestrator_dual_mock", + "orchestrator_dual_xarm", + # High-frequency blueprints + "orchestrator_highfreq_mock", + # Single arm blueprints + "orchestrator_mock", + "orchestrator_piper", + "orchestrator_xarm6", + "orchestrator_xarm7", +] diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 920aa1254f..e5732455e0 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -48,6 +48,13 @@ # Piper manipulator blueprints "piper-servo": "dimos.hardware.manipulators.piper.blueprints:piper_servo", "piper-trajectory": "dimos.hardware.manipulators.piper.blueprints:piper_trajectory", + # Control orchestrator blueprints + "orchestrator-mock": "dimos.control.blueprints:orchestrator_mock", + "orchestrator-xarm7": "dimos.control.blueprints:orchestrator_xarm7", + "orchestrator-xarm6": "dimos.control.blueprints:orchestrator_xarm6", + "orchestrator-piper": "dimos.control.blueprints:orchestrator_piper", + "orchestrator-dual-mock": "dimos.control.blueprints:orchestrator_dual_mock", + "orchestrator-dual-xarm": "dimos.control.blueprints:orchestrator_dual_xarm", # Demo blueprints "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", @@ -85,6 +92,8 @@ "wavefront_frontier_explorer": "dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector", "websocket_vis": "dimos.web.websocket_vis.websocket_vis_module", "web_input": "dimos.agents.cli.web", + # Control orchestrator module + "control_orchestrator": "dimos.control.orchestrator", # xArm manipulator modules "xarm_driver": "dimos.hardware.manipulators.xarm.arm", "cartesian_motion_controller": "dimos.manipulation.control.servo_control.cartesian_motion_controller", From cb9d87c9dd74cfa6f564a65ba26b0fa6fe25520c Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 8 Jan 2026 15:22:55 -0800 Subject: [PATCH 17/42] updated blueprints to add piper + xarm blueprint --- dimos/control/blueprints.py | 50 ++++++++++++++++++++++++++++++++--- dimos/control/orchestrator.py | 11 ++++++++ dimos/robot/all_blueprints.py | 1 + 3 files changed, 59 insertions(+), 3 deletions(-) diff --git a/dimos/control/blueprints.py b/dimos/control/blueprints.py index 96463cd746..5cbc109ece 100644 --- a/dimos/control/blueprints.py +++ b/dimos/control/blueprints.py @@ -127,13 +127,13 @@ def _joint_names(prefix: str, dof: int) -> list[str]: type="xarm", dof=6, joint_prefix="arm", - ip="192.168.1.185", + ip="192.168.1.210", auto_enable=True, ), ], tasks=[ TaskConfig( - name="traj_arm", + name="traj_xarm", type="trajectory", joint_names=_joint_names("arm", 6), priority=10, @@ -162,7 +162,7 @@ def _joint_names(prefix: str, dof: int) -> list[str]: ], tasks=[ TaskConfig( - name="traj_arm", + name="traj_piper", type="trajectory", joint_names=_joint_names("arm", 6), priority=10, @@ -260,6 +260,49 @@ def _joint_names(prefix: str, dof: int) -> list[str]: } ) +# Dual Arm setup (XArm6 , Piper ) +orchestrator_piper_xarm = control_orchestrator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="orchestrator", + hardware=[ + HardwareConfig( + id="xarm_arm", + type="xarm", + dof=6, + joint_prefix="xarm", + ip="192.168.1.210", + auto_enable=True, + ), + HardwareConfig( + id="piper_arm", + type="piper", + dof=6, + joint_prefix="piper", + can_port="can0", + auto_enable=True, + ), + ], + tasks=[ + TaskConfig( + name="traj_xarm", + type="trajectory", + joint_names=_joint_names("xarm", 6), + priority=10, + ), + TaskConfig( + name="traj_piper", + type="trajectory", + joint_names=_joint_names("piper", 6), + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState), + } +) + # ============================================================================= # High-frequency Blueprints (200Hz) # ============================================================================= @@ -318,6 +361,7 @@ def _joint_names(prefix: str, dof: int) -> list[str]: # Single arm blueprints "orchestrator_mock", "orchestrator_piper", + "orchestrator_piper_xarm", "orchestrator_xarm6", "orchestrator_xarm7", ] diff --git a/dimos/control/orchestrator.py b/dimos/control/orchestrator.py index 0ce84e6b77..a6569408b3 100644 --- a/dimos/control/orchestrator.py +++ b/dimos/control/orchestrator.py @@ -302,6 +302,17 @@ def list_joints(self) -> list[str]: with self._hardware_lock: return list(self._joint_to_hardware.keys()) + @rpc + def get_joint_positions(self) -> dict[str, float]: + """Get current joint positions for all joints.""" + with self._hardware_lock: + positions: dict[str, float] = {} + for hw in self._hardware.values(): + state = hw.read_state() # {joint_name: (pos, vel, effort)} + for joint_name, (pos, _vel, _effort) in state.items(): + positions[joint_name] = pos + return positions + # ========================================================================= # Task Management (RPC) # ========================================================================= diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index e5732455e0..3ca80b91d2 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -55,6 +55,7 @@ "orchestrator-piper": "dimos.control.blueprints:orchestrator_piper", "orchestrator-dual-mock": "dimos.control.blueprints:orchestrator_dual_mock", "orchestrator-dual-xarm": "dimos.control.blueprints:orchestrator_dual_xarm", + "orchestrator-piper-xarm": "dimos.control.blueprints:orchestrator_piper_xarm", # Demo blueprints "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", From 300eca2adca90e32812d62a99c6fa1d4c0cb82db Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 8 Jan 2026 15:23:32 -0800 Subject: [PATCH 18/42] orchestrator client that can send tasks to the control orchestrator module --- .../control/orchestrator_client.py | 697 ++++++++++++++++++ 1 file changed, 697 insertions(+) create mode 100644 dimos/manipulation/control/orchestrator_client.py diff --git a/dimos/manipulation/control/orchestrator_client.py b/dimos/manipulation/control/orchestrator_client.py new file mode 100644 index 0000000000..ba65d0750d --- /dev/null +++ b/dimos/manipulation/control/orchestrator_client.py @@ -0,0 +1,697 @@ +#!/usr/bin/env python3 +# 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. + +""" +Interactive client for the ControlOrchestrator. + +Interfaces with a running ControlOrchestrator via RPC to: +- Query hardware and task status +- Plan and execute trajectories on single or multiple arms +- Monitor execution progress + +Usage: + # Terminal 1: Start the orchestrator + dimos run orchestrator-mock # Single arm + dimos run orchestrator-dual-mock # Dual arm + + # Terminal 2: Run this client + python -m dimos.manipulation.control.orchestrator_client + python -m dimos.manipulation.control.orchestrator_client --task traj_left + python -m dimos.manipulation.control.orchestrator_client --task traj_right + +How it works: + 1. Connects to ControlOrchestrator via LCM RPC + 2. Queries available hardware/tasks/joints + 3. You add waypoints (joint positions) + 4. Generates trajectory with trapezoidal velocity profile + 5. Sends trajectory to orchestrator via execute_trajectory() RPC + 6. Orchestrator's tick loop executes it at 100Hz +""" + +from __future__ import annotations + +import math +import sys +import time +from typing import TYPE_CHECKING, Any + +from dimos.control.orchestrator import ControlOrchestrator +from dimos.core.rpc_client import RPCClient +from dimos.manipulation.planning import JointTrajectoryGenerator + +if TYPE_CHECKING: + from dimos.msgs.trajectory_msgs import JointTrajectory + + +class OrchestratorClient: + """ + RPC client for the ControlOrchestrator. + + Connects to a running orchestrator and provides methods to: + - Query state (joints, tasks, hardware) + - Execute trajectories on any task + - Monitor progress + + Example: + client = OrchestratorClient() + + # Query state + print(client.list_hardware()) # ['left_arm', 'right_arm'] + print(client.list_tasks()) # ['traj_left', 'traj_right'] + + # Setup for a task + client.select_task("traj_left") + + # Get current position and create trajectory + current = client.get_current_positions() + target = [0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + trajectory = client.generate_trajectory([current, target]) + + # Execute + client.execute_trajectory("traj_left", trajectory) + """ + + def __init__(self) -> None: + """Initialize connection to orchestrator via RPC.""" + self._rpc = RPCClient(None, ControlOrchestrator) + + # Per-task state + self._current_task: str | None = None + self._task_joints: dict[str, list[str]] = {} # task_name -> joint_names + self._generators: dict[str, JointTrajectoryGenerator] = {} # task_name -> generator + + def stop(self) -> None: + """Stop the RPC client.""" + self._rpc.stop_rpc_client() + + # ========================================================================= + # Query methods (RPC calls) + # ========================================================================= + + def list_hardware(self) -> list[str]: + """List all hardware IDs.""" + return self._rpc.list_hardware() or [] + + def list_joints(self) -> list[str]: + """List all joint names across all hardware.""" + return self._rpc.list_joints() or [] + + def list_tasks(self) -> list[str]: + """List all task names.""" + return self._rpc.list_tasks() or [] + + def get_active_tasks(self) -> list[str]: + """Get currently active task names.""" + return self._rpc.get_active_tasks() or [] + + def get_joint_positions(self) -> dict[str, float]: + """Get current joint positions for all joints.""" + return self._rpc.get_joint_positions() or {} + + def get_trajectory_status(self, task_name: str) -> dict[str, Any]: + """Get status of a trajectory task.""" + return self._rpc.get_trajectory_status(task_name) or {} + + # ========================================================================= + # Trajectory execution (RPC calls) + # ========================================================================= + + def execute_trajectory(self, task_name: str, trajectory: JointTrajectory) -> bool: + """Execute a trajectory on a task.""" + return self._rpc.execute_trajectory(task_name, trajectory) or False + + def cancel_trajectory(self, task_name: str) -> bool: + """Cancel an active trajectory.""" + return self._rpc.cancel_trajectory(task_name) or False + + # ========================================================================= + # Task selection and setup + # ========================================================================= + + def select_task(self, task_name: str) -> bool: + """ + Select a task and setup its trajectory generator. + + This queries the orchestrator to find which joints the task controls, + then creates a trajectory generator for those joints. + """ + tasks = self.list_tasks() + if task_name not in tasks: + print(f"Task '{task_name}' not found. Available: {tasks}") + return False + + self._current_task = task_name + + # Get joints for this task (infer from task name pattern) + # e.g., "traj_left" -> joints starting with "left_" + # e.g., "traj_arm" -> joints starting with "arm_" + all_joints = self.list_joints() + + # Try to infer prefix from task name + if "_" in task_name: + prefix = task_name.split("_", 1)[1] # "traj_left" -> "left" + task_joints = [j for j in all_joints if j.startswith(prefix + "_")] + else: + task_joints = all_joints + + if not task_joints: + # Fallback: use all joints + task_joints = all_joints + + self._task_joints[task_name] = task_joints + + # Create generator if not exists + if task_name not in self._generators: + self._generators[task_name] = JointTrajectoryGenerator( + num_joints=len(task_joints), + max_velocity=1.0, + max_acceleration=2.0, + points_per_segment=50, + ) + + return True + + def get_task_joints(self, task_name: str | None = None) -> list[str]: + """Get joint names for a task.""" + task = task_name or self._current_task + if task is None: + return [] + return self._task_joints.get(task, []) + + def get_current_positions(self, task_name: str | None = None) -> list[float] | None: + """Get current joint positions for a task as a list.""" + task = task_name or self._current_task + if task is None: + return None + + joints = self._task_joints.get(task, []) + if not joints: + return None + + positions = self.get_joint_positions() + if not positions: + return None + + return [positions.get(j, 0.0) for j in joints] + + def generate_trajectory( + self, waypoints: list[list[float]], task_name: str | None = None + ) -> JointTrajectory | None: + """Generate trajectory from waypoints using trapezoidal velocity profile.""" + task = task_name or self._current_task + if task is None: + print("Error: No task selected") + return None + + generator = self._generators.get(task) + if generator is None: + print(f"Error: No generator for task '{task}'. Call select_task() first.") + return None + + return generator.generate(waypoints) + + def set_velocity_limit(self, velocity: float, task_name: str | None = None) -> None: + """Set max velocity for trajectory generation.""" + task = task_name or self._current_task + if task and task in self._generators: + gen = self._generators[task] + gen.set_limits(velocity, gen.max_acceleration) + + def set_acceleration_limit(self, acceleration: float, task_name: str | None = None) -> None: + """Set max acceleration for trajectory generation.""" + task = task_name or self._current_task + if task and task in self._generators: + gen = self._generators[task] + gen.set_limits(gen.max_velocity, acceleration) + + +# ============================================================================= +# Interactive CLI +# ============================================================================= + + +def parse_joint_input(line: str, num_joints: int) -> list[float] | None: + """Parse joint positions from user input (degrees by default, 'r' suffix for radians).""" + parts = line.strip().split() + if len(parts) != num_joints: + return None + + positions = [] + for part in parts: + try: + if part.endswith("r"): + positions.append(float(part[:-1])) + else: + positions.append(math.radians(float(part))) + except ValueError: + return None + + return positions + + +def format_positions(positions: list[float], as_degrees: bool = True) -> str: + """Format positions for display.""" + if as_degrees: + return "[" + ", ".join(f"{math.degrees(p):.1f}" for p in positions) + "] deg" + return "[" + ", ".join(f"{p:.3f}" for p in positions) + "] rad" + + +def preview_waypoints(waypoints: list[list[float]], joint_names: list[str]) -> None: + """Show waypoints list.""" + if not waypoints: + print("No waypoints") + return + + len(joint_names) + print(f"\nWaypoints ({len(waypoints)}):") + print("-" * 70) + + # Header with joint names (truncated) + headers = [j.split("_")[-1][:6] for j in joint_names] # e.g., "joint1" -> "joint1" + header_str = " ".join(f"{h:>7}" for h in headers) + print(f" # | {header_str} (degrees)") + print("-" * 70) + + for i, joints in enumerate(waypoints): + deg = [f"{math.degrees(j):7.1f}" for j in joints] + print(f" {i + 1:2} | {' '.join(deg)}") + print("-" * 70) + + +def preview_trajectory(trajectory: JointTrajectory, joint_names: list[str]) -> None: + """Show generated trajectory preview.""" + len(joint_names) + headers = [j.split("_")[-1][:6] for j in joint_names] + header_str = " ".join(f"{h:>7}" for h in headers) + + print("\n" + "=" * 70) + print("GENERATED TRAJECTORY") + print("=" * 70) + print(f"Duration: {trajectory.duration:.3f}s") + print(f"Points: {len(trajectory.points)}") + print("-" * 70) + print(f"{'Time':>6} | {header_str} (degrees)") + print("-" * 70) + + num_samples = min(10, max(len(trajectory.points) // 10, 5)) + for i in range(num_samples + 1): + t = (i / num_samples) * trajectory.duration + q_ref, _ = trajectory.sample(t) + q_deg = [f"{math.degrees(q):7.1f}" for q in q_ref] + print(f"{t:6.2f} | {' '.join(q_deg)}") + + print("=" * 70) + + +def wait_for_completion(client: OrchestratorClient, task_name: str, timeout: float = 60.0) -> bool: + """Wait for trajectory to complete with progress display.""" + start = time.time() + last_progress = -1.0 + + while time.time() - start < timeout: + status = client.get_trajectory_status(task_name) + if not status.get("active", False): + state = status.get("state", "UNKNOWN") + print(f"\nTrajectory finished: {state}") + return state == "COMPLETED" + + progress = status.get("progress", 0.0) + if progress != last_progress: + bar_len = 30 + filled = int(bar_len * progress) + bar = "=" * filled + "-" * (bar_len - filled) + print(f"\r[{bar}] {progress * 100:.1f}%", end="", flush=True) + last_progress = progress + + time.sleep(0.05) + + print("\nTimeout waiting for trajectory") + return False + + +def print_help(num_joints: int, task_name: str) -> None: + """Print help message.""" + joint_args = " ".join([f"" for i in range(num_joints)]) + + print("\n" + "=" * 70) + print(f"Orchestrator Client - Task: {task_name} ({num_joints} joints)") + print("=" * 70) + print("\nWaypoint Commands:") + print(f" add {joint_args}") + print(" - Add waypoint (values in degrees)") + print(" here - Add current position as waypoint") + print(" list - List all waypoints") + print(" delete - Delete waypoint n") + print(" clear - Clear all waypoints") + print("\nTrajectory Commands:") + print(" preview - Preview generated trajectory") + print(" run - Execute trajectory") + print(" status - Show task status") + print(" cancel - Cancel active trajectory") + print("\nMulti-Arm Commands:") + print(" tasks - List all tasks") + print(" switch - Switch to different task") + print(" hw - List hardware") + print(" joints - List joints for current task") + print("\nSettings:") + print(" current - Show current joint positions") + print(" vel - Set max velocity (rad/s)") + print(" accel - Set max acceleration (rad/s^2)") + print(" help - Show this help") + print(" quit - Exit") + print("=" * 70) + + +def interactive_mode(client: OrchestratorClient, initial_task: str) -> None: + """Interactive mode for trajectory planning and execution.""" + # Setup initial task + if not client.select_task(initial_task): + print(f"Failed to select task: {initial_task}") + return + + current_task = initial_task + waypoints: list[list[float]] = [] + generated_trajectory: JointTrajectory | None = None + + joints = client.get_task_joints(current_task) + num_joints = len(joints) + + print_help(num_joints, current_task) + + try: + while True: + # Prompt shows task and waypoint count + active = client.get_active_tasks() + active_marker = "*" if current_task in active else "" + prompt = f"[{current_task}{active_marker}:{len(waypoints)}wp] > " + + try: + line = input(prompt).strip() + except EOFError: + break + + if not line: + continue + + parts = line.split() + cmd = parts[0].lower() + + # === Waypoint commands === + + if cmd == "add": + if len(parts) < num_joints + 1: + print(f"Need {num_joints} joint values") + continue + values = parse_joint_input(" ".join(parts[1 : num_joints + 1]), num_joints) + if values: + waypoints.append(values) + generated_trajectory = None + print(f"Added waypoint {len(waypoints)}: {format_positions(values)}") + else: + print(f"Invalid values. Need {num_joints} numbers (degrees)") + + elif cmd == "here": + positions = client.get_current_positions(current_task) + if positions: + waypoints.append(positions) + generated_trajectory = None + print(f"Added waypoint {len(waypoints)}: {format_positions(positions)}") + else: + print("Could not get current positions") + + elif cmd == "list": + preview_waypoints(waypoints, joints) + + elif cmd == "delete": + if len(parts) < 2: + print("Usage: delete ") + continue + try: + idx = int(parts[1]) - 1 + if 0 <= idx < len(waypoints): + waypoints.pop(idx) + generated_trajectory = None + print(f"Deleted waypoint {idx + 1}") + else: + print(f"Invalid index (1-{len(waypoints)})") + except ValueError: + print("Invalid index") + + elif cmd == "clear": + waypoints.clear() + generated_trajectory = None + print("Cleared waypoints") + + # === Trajectory commands === + + elif cmd == "preview": + if len(waypoints) < 2: + print("Need at least 2 waypoints") + continue + try: + generated_trajectory = client.generate_trajectory(waypoints, current_task) + if generated_trajectory: + preview_trajectory(generated_trajectory, joints) + except Exception as e: + print(f"Error: {e}") + + elif cmd == "run": + if len(waypoints) < 2: + print("Need at least 2 waypoints") + continue + + if generated_trajectory is None: + generated_trajectory = client.generate_trajectory(waypoints, current_task) + + if generated_trajectory is None: + print("Failed to generate trajectory") + continue + + preview_trajectory(generated_trajectory, joints) + confirm = input("\nExecute? [y/N]: ").strip().lower() + if confirm == "y": + if client.execute_trajectory(current_task, generated_trajectory): + print("Trajectory started...") + wait_for_completion(client, current_task) + else: + print("Failed to start trajectory") + + elif cmd == "status": + status = client.get_trajectory_status(current_task) + print(f"\nTask: {current_task}") + print(f" Active: {status.get('active', False)}") + print(f" State: {status.get('state', 'UNKNOWN')}") + if "progress" in status: + print(f" Progress: {status['progress'] * 100:.1f}%") + + elif cmd == "cancel": + if client.cancel_trajectory(current_task): + print("Cancelled") + else: + print("Cancel failed") + + # === Multi-arm commands === + + elif cmd == "tasks": + tasks = client.list_tasks() + active = client.get_active_tasks() + print("\nTasks:") + for t in tasks: + marker = "* " if t == current_task else " " + status = " [ACTIVE]" if t in active else "" + t_joints = client.get_task_joints(t) if t in client._task_joints else [] + joint_count = len(t_joints) if t_joints else "?" + print(f"{marker}{t} ({joint_count} joints){status}") + + elif cmd == "switch": + if len(parts) < 2: + print("Usage: switch ") + continue + new_task = parts[1] + if client.select_task(new_task): + current_task = new_task + joints = client.get_task_joints(current_task) + num_joints = len(joints) + waypoints.clear() + generated_trajectory = None + print(f"Switched to {current_task} ({num_joints} joints)") + print(f"Joints: {', '.join(joints)}") + + elif cmd == "hw": + hardware = client.list_hardware() + print(f"\nHardware: {', '.join(hardware)}") + + elif cmd == "joints": + print(f"\nJoints for {current_task}:") + for i, j in enumerate(joints): + pos = client.get_joint_positions().get(j, 0.0) + print(f" {i + 1}. {j}: {math.degrees(pos):.1f} deg") + + # === Settings === + + elif cmd == "current": + positions = client.get_current_positions(current_task) + if positions: + print(f"Current: {format_positions(positions)}") + else: + print("Could not get positions") + + elif cmd == "vel": + if len(parts) < 2: + gen = client._generators.get(current_task) + if gen: + print(f"Max velocity: {gen.max_velocity[0]:.2f} rad/s") + continue + try: + vel = float(parts[1]) + if vel <= 0: + print("Velocity must be positive") + else: + client.set_velocity_limit(vel, current_task) + generated_trajectory = None + print(f"Max velocity: {vel:.2f} rad/s") + except ValueError: + print("Invalid velocity") + + elif cmd == "accel": + if len(parts) < 2: + gen = client._generators.get(current_task) + if gen: + print(f"Max acceleration: {gen.max_acceleration[0]:.2f} rad/s^2") + continue + try: + accel = float(parts[1]) + if accel <= 0: + print("Acceleration must be positive") + else: + client.set_acceleration_limit(accel, current_task) + generated_trajectory = None + print(f"Max acceleration: {accel:.2f} rad/s^2") + except ValueError: + print("Invalid acceleration") + + elif cmd == "help": + print_help(num_joints, current_task) + + elif cmd in ("quit", "exit", "q"): + break + + else: + print(f"Unknown command: {cmd} (type 'help' for commands)") + + except KeyboardInterrupt: + print("\n\nExiting...") + + +def main() -> int: + """Main entry point.""" + import argparse + + parser = argparse.ArgumentParser( + description="Interactive client for ControlOrchestrator", + formatter_class=argparse.RawDescriptionHelpFormatter, + epilog=""" +Examples: + # Single arm (with orchestrator-mock running) + python -m dimos.manipulation.control.orchestrator_client + + # Dual arm - control left arm + python -m dimos.manipulation.control.orchestrator_client --task traj_left + + # Dual arm - control right arm + python -m dimos.manipulation.control.orchestrator_client --task traj_right + """, + ) + parser.add_argument( + "--task", + type=str, + default="traj_arm", + help="Initial task to control (default: traj_arm)", + ) + parser.add_argument( + "--vel", + type=float, + default=1.0, + help="Max velocity in rad/s (default: 1.0)", + ) + parser.add_argument( + "--accel", + type=float, + default=2.0, + help="Max acceleration in rad/s^2 (default: 2.0)", + ) + args = parser.parse_args() + + print("\n" + "=" * 70) + print("Orchestrator Client") + print("=" * 70) + print("\nConnecting to ControlOrchestrator via RPC...") + + client = OrchestratorClient() + + # Check connection + try: + hardware = client.list_hardware() + tasks = client.list_tasks() + + if not hardware: + print("\nWarning: No hardware found. Is the orchestrator running?") + print("Start with: dimos run orchestrator-mock") + response = input("Continue anyway? [y/N]: ").strip().lower() + if response != "y": + client.stop() + return 0 + else: + print(f"Hardware: {', '.join(hardware)}") + print(f"Tasks: {', '.join(tasks)}") + + except Exception as e: + print(f"\nConnection error: {e}") + print("Make sure orchestrator is running: dimos run orchestrator-mock") + client.stop() + return 1 + + # Find initial task + if args.task not in tasks and tasks: + print(f"\nTask '{args.task}' not found.") + print(f"Available: {', '.join(tasks)}") + args.task = tasks[0] + print(f"Using '{args.task}'") + + # Set velocity/acceleration limits + if client.select_task(args.task): + client.set_velocity_limit(args.vel, args.task) + client.set_acceleration_limit(args.accel, args.task) + + try: + interactive_mode(client, args.task) + finally: + client.stop() + + return 0 + + +if __name__ == "__main__": + try: + sys.exit(main()) + except KeyboardInterrupt: + print("\n\nInterrupted") + sys.exit(0) + except Exception as e: + print(f"\nError: {e}") + import traceback + + traceback.print_exc() + sys.exit(1) From b4497326d9443004bbed5b4fe96aa58e28c13f58 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 8 Jan 2026 17:10:25 -0800 Subject: [PATCH 19/42] added a readme --- dimos/control/README.md | 195 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 195 insertions(+) create mode 100644 dimos/control/README.md diff --git a/dimos/control/README.md b/dimos/control/README.md new file mode 100644 index 0000000000..58490321fa --- /dev/null +++ b/dimos/control/README.md @@ -0,0 +1,195 @@ +# Control Orchestrator + +Centralized control system for multi-arm robots with per-joint arbitration. + +## Architecture + +``` +┌─────────────────────────────────────────────────────────────┐ +│ ControlOrchestrator │ +│ │ +│ ┌──────────────────────────────────────────────────────┐ │ +│ │ TickLoop (100Hz) │ │ +│ │ │ │ +│ │ READ ──► COMPUTE ──► ARBITRATE ──► ROUTE ──► WRITE │ │ +│ └──────────────────────────────────────────────────────┘ │ +│ │ │ │ │ │ +│ ▼ ▼ ▼ ▼ │ +│ ┌─────────┐ ┌───────┐ ┌─────────┐ ┌──────────┐ │ +│ │Hardware │ │ Tasks │ │Priority │ │ Backends │ │ +│ │Interface│ │ │ │ Winners │ │ │ │ +│ └─────────┘ └───────┘ └─────────┘ └──────────┘ │ +└─────────────────────────────────────────────────────────────┘ +``` + +## Quick Start + +```bash +# Terminal 1: Run orchestrator +dimos run orchestrator-mock # Single 7-DOF mock arm +dimos run orchestrator-dual-mock # Dual arms (7+6 DOF) +dimos run orchestrator-piper-xarm # Real hardware + +# Terminal 2: Control via CLI +python -m dimos.manipulation.control.orchestrator_client +``` + +## Core Concepts + +### Tick Loop +Single deterministic loop at 100Hz: +1. **Read** - Get joint positions from all hardware +2. **Compute** - Each task calculates desired output +3. **Arbitrate** - Per-joint, highest priority wins +4. **Route** - Group commands by hardware +5. **Write** - Send commands to backends + +### Tasks (Controllers) +Tasks are passive controllers called by the orchestrator: + +```python +class MyController: + def claim(self) -> ResourceClaim: + return ResourceClaim(joints={"joint1", "joint2"}, priority=10) + + def compute(self, state: OrchestratorState) -> JointCommandOutput: + # Your control law here (PID, impedance, etc.) + return JointCommandOutput( + joint_names=["joint1", "joint2"], + positions=[0.5, 0.3], + mode=ControlMode.POSITION, + ) +``` + +### Priority & Arbitration +Higher priority always wins. Arbitration happens every tick: + +``` +traj_arm (priority=10) wants joint1 = 0.5 +safety (priority=100) wants joint1 = 0.0 + ↓ + safety wins, traj_arm preempted +``` + +### Preemption +When a task loses a joint to higher priority, it gets notified: + +```python +def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: + self._state = TrajectoryState.PREEMPTED +``` + +## Files + +``` +dimos/control/ +├── orchestrator.py # Module + RPC interface +├── tick_loop.py # 100Hz control loop +├── task.py # ControlTask protocol + types +├── hardware_interface.py # Backend wrapper +├── blueprints.py # Pre-configured setups +└── tasks/ + └── trajectory_task.py # Joint trajectory controller +``` + +## Configuration + +```python +from dimos.control import control_orchestrator, HardwareConfig, TaskConfig + +my_robot = control_orchestrator( + tick_rate=100.0, + hardware=[ + HardwareConfig(id="left", type="xarm", dof=7, joint_prefix="left", ip="192.168.1.100"), + HardwareConfig(id="right", type="piper", dof=6, joint_prefix="right", can_port="can0"), + ], + tasks=[ + TaskConfig(name="traj_left", type="trajectory", joint_names=[...], priority=10), + TaskConfig(name="traj_right", type="trajectory", joint_names=[...], priority=10), + TaskConfig(name="safety", type="trajectory", joint_names=[...], priority=100), + ], +) +``` + +## RPC Methods + +| Method | Description | +|--------|-------------| +| `list_hardware()` | List hardware IDs | +| `list_joints()` | List all joint names | +| `list_tasks()` | List task names | +| `get_joint_positions()` | Get current positions | +| `execute_trajectory(task, traj)` | Execute trajectory | +| `get_trajectory_status(task)` | Get task status | +| `cancel_trajectory(task)` | Cancel active trajectory | + +## Control Modes + +Tasks output commands in one of three modes: + +| Mode | Output | Use Case | +|------|--------|----------| +| POSITION | `q` | Trajectory following | +| VELOCITY | `q_dot` | Joystick teleoperation | +| TORQUE | `tau` | Force control, impedance | + +## Writing a Custom Task + +```python +from dimos.control.task import ControlTask, ResourceClaim, JointCommandOutput, ControlMode + +class PIDController: + def __init__(self, joints: list[str], priority: int = 10): + self._name = "pid_controller" + self._claim = ResourceClaim(joints=frozenset(joints), priority=priority) + self._joints = joints + self.Kp, self.Ki, self.Kd = 10.0, 0.1, 1.0 + self._integral = [0.0] * len(joints) + self._last_error = [0.0] * len(joints) + self.target = [0.0] * len(joints) + + @property + def name(self) -> str: + return self._name + + def claim(self) -> ResourceClaim: + return self._claim + + def is_active(self) -> bool: + return True + + def compute(self, state) -> JointCommandOutput: + positions = [state.joints.joint_positions[j] for j in self._joints] + error = [t - p for t, p in zip(self.target, positions)] + + # PID + self._integral = [i + e * state.dt for i, e in zip(self._integral, error)] + derivative = [(e - le) / state.dt for e, le in zip(error, self._last_error)] + output = [self.Kp*e + self.Ki*i + self.Kd*d + for e, i, d in zip(error, self._integral, derivative)] + self._last_error = error + + return JointCommandOutput( + joint_names=self._joints, + positions=output, + mode=ControlMode.POSITION, + ) + + def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: + pass # Handle preemption +``` + +## Joint State Output + +The orchestrator publishes one aggregated `JointState` message containing all joints: + +```python +JointState( + name=["left_joint1", ..., "right_joint1", ...], # All joints + position=[...], + velocity=[...], + effort=[...], +) +``` + +Subscribe via: `/orchestrator/joint_state` From 7b0e2240e0591db9b2781a609bba07ade71d0461 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 8 Jan 2026 20:04:22 -0800 Subject: [PATCH 20/42] added pytest and e2e test --- dimos/control/orchestrator.py | 4 +- dimos/control/test_control.py | 550 +++++++++++++++++++ dimos/e2e_tests/test_control_orchestrator.py | 263 +++++++++ 3 files changed, 816 insertions(+), 1 deletion(-) create mode 100644 dimos/control/test_control.py create mode 100644 dimos/e2e_tests/test_control_orchestrator.py diff --git a/dimos/control/orchestrator.py b/dimos/control/orchestrator.py index a6569408b3..9e48bfcccf 100644 --- a/dimos/control/orchestrator.py +++ b/dimos/control/orchestrator.py @@ -36,13 +36,15 @@ from dimos.control.tick_loop import TickLoop from dimos.core import Module, Out, rpc from dimos.core.module import ModuleConfig +from dimos.msgs.sensor_msgs import ( + JointState, # noqa: TC001 - needed at runtime for Out[JointState] +) from dimos.msgs.trajectory_msgs import JointTrajectory, TrajectoryState from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: from dimos.control.task import ControlTask from dimos.hardware.manipulators.spec import ManipulatorBackend - from dimos.msgs.sensor_msgs import JointState logger = setup_logger() diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py new file mode 100644 index 0000000000..ffea5b34b2 --- /dev/null +++ b/dimos/control/test_control.py @@ -0,0 +1,550 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Tests for the Control Orchestrator module.""" + +from __future__ import annotations + +import threading +import time +from unittest.mock import MagicMock + +import pytest + +from dimos.control.hardware_interface import BackendHardwareInterface +from dimos.control.task import ( + ControlMode, + JointCommandOutput, + JointStateSnapshot, + OrchestratorState, + ResourceClaim, +) +from dimos.control.tasks.trajectory_task import ( + JointTrajectoryTask, + JointTrajectoryTaskConfig, + TrajectoryState, +) +from dimos.control.tick_loop import TickLoop +from dimos.msgs.trajectory_msgs import JointTrajectory, TrajectoryPoint + +# ============================================================================= +# Fixtures +# ============================================================================= + + +@pytest.fixture +def mock_backend(): + """Create a mock manipulator backend.""" + backend = MagicMock() + backend.get_dof.return_value = 6 + backend.get_joint_names.return_value = [f"joint{i + 1}" for i in range(6)] + backend.read_joint_positions.return_value = [0.0] * 6 + backend.read_joint_velocities.return_value = [0.0] * 6 + backend.read_joint_efforts.return_value = [0.0] * 6 + backend.write_joint_positions.return_value = True + backend.write_joint_velocities.return_value = True + return backend + + +@pytest.fixture +def hardware_interface(mock_backend): + """Create a BackendHardwareInterface with mock backend.""" + return BackendHardwareInterface( + backend=mock_backend, + hardware_id="test_arm", + joint_prefix="arm", + ) + + +@pytest.fixture +def trajectory_task(): + """Create a JointTrajectoryTask for testing.""" + config = JointTrajectoryTaskConfig( + joint_names=["arm_joint1", "arm_joint2", "arm_joint3"], + priority=10, + ) + return JointTrajectoryTask(name="test_traj", config=config) + + +@pytest.fixture +def simple_trajectory(): + """Create a simple 2-point trajectory.""" + return JointTrajectory( + joint_names=["arm_joint1", "arm_joint2", "arm_joint3"], + points=[ + TrajectoryPoint( + positions=[0.0, 0.0, 0.0], + velocities=[0.0, 0.0, 0.0], + time_from_start=0.0, + ), + TrajectoryPoint( + positions=[1.0, 0.5, 0.25], + velocities=[0.0, 0.0, 0.0], + time_from_start=1.0, + ), + ], + ) + + +@pytest.fixture +def orchestrator_state(): + """Create a sample OrchestratorState.""" + joints = JointStateSnapshot( + joint_positions={"arm_joint1": 0.0, "arm_joint2": 0.0, "arm_joint3": 0.0}, + joint_velocities={"arm_joint1": 0.0, "arm_joint2": 0.0, "arm_joint3": 0.0}, + joint_efforts={"arm_joint1": 0.0, "arm_joint2": 0.0, "arm_joint3": 0.0}, + timestamp=time.perf_counter(), + ) + return OrchestratorState(joints=joints, t_now=time.perf_counter(), dt=0.01) + + +# ============================================================================= +# Test ResourceClaim +# ============================================================================= + + +class TestResourceClaim: + def test_create_claim(self): + claim = ResourceClaim( + joints=frozenset({"joint1", "joint2"}), + priority=10, + ) + assert claim.joints == frozenset({"joint1", "joint2"}) + assert claim.priority == 10 + + def test_claim_immutable(self): + claim = ResourceClaim( + joints=frozenset({"joint1"}), + priority=5, + ) + with pytest.raises(AttributeError): + claim.priority = 20 + + +# ============================================================================= +# Test JointCommandOutput +# ============================================================================= + + +class TestJointCommandOutput: + def test_position_output(self): + output = JointCommandOutput( + joint_names=["j1", "j2"], + positions=[0.5, 1.0], + mode=ControlMode.POSITION, + ) + assert output.get_values() == [0.5, 1.0] + assert output.mode == ControlMode.POSITION + + def test_velocity_output(self): + output = JointCommandOutput( + joint_names=["j1", "j2"], + velocities=[0.1, 0.2], + mode=ControlMode.VELOCITY, + ) + assert output.get_values() == [0.1, 0.2] + assert output.mode == ControlMode.VELOCITY + + def test_torque_output(self): + output = JointCommandOutput( + joint_names=["j1", "j2"], + efforts=[5.0, 10.0], + mode=ControlMode.TORQUE, + ) + assert output.get_values() == [5.0, 10.0] + assert output.mode == ControlMode.TORQUE + + def test_no_values_returns_none(self): + output = JointCommandOutput( + joint_names=["j1"], + mode=ControlMode.POSITION, + ) + assert output.get_values() is None + + +# ============================================================================= +# Test JointStateSnapshot +# ============================================================================= + + +class TestJointStateSnapshot: + def test_create_snapshot(self): + snapshot = JointStateSnapshot( + joint_positions={"j1": 0.5, "j2": 1.0}, + joint_velocities={"j1": 0.0, "j2": 0.1}, + joint_efforts={"j1": 1.0, "j2": 2.0}, + timestamp=100.0, + ) + assert snapshot.joint_positions["j1"] == 0.5 + assert snapshot.joint_velocities["j2"] == 0.1 + assert snapshot.timestamp == 100.0 + + def test_get_position(self): + snapshot = JointStateSnapshot( + joint_positions={"j1": 0.5, "j2": 1.0}, + joint_velocities={"j1": 0.0, "j2": 0.1}, + joint_efforts={"j1": 1.0, "j2": 2.0}, + timestamp=100.0, + ) + assert snapshot.get_position("j1") == 0.5 + assert snapshot.get_position("j2") == 1.0 + assert snapshot.get_position("nonexistent") is None + + +# ============================================================================= +# Test BackendHardwareInterface +# ============================================================================= + + +class TestBackendHardwareInterface: + def test_joint_names_prefixed(self, hardware_interface): + names = hardware_interface.joint_names + assert names == [ + "arm_joint1", + "arm_joint2", + "arm_joint3", + "arm_joint4", + "arm_joint5", + "arm_joint6", + ] + + def test_read_state(self, hardware_interface): + state = hardware_interface.read_state() + assert "arm_joint1" in state + assert len(state) == 6 + pos, vel, eff = state["arm_joint1"] + assert pos == 0.0 + assert vel == 0.0 + assert eff == 0.0 + + def test_write_command(self, hardware_interface, mock_backend): + commands = { + "arm_joint1": 0.5, + "arm_joint2": 1.0, + } + hardware_interface.write_command(commands, ControlMode.POSITION) + mock_backend.write_joint_positions.assert_called() + + +# ============================================================================= +# Test JointTrajectoryTask +# ============================================================================= + + +class TestJointTrajectoryTask: + def test_initial_state(self, trajectory_task): + assert trajectory_task.name == "test_traj" + assert not trajectory_task.is_active() + assert trajectory_task.get_state() == TrajectoryState.IDLE + + def test_claim(self, trajectory_task): + claim = trajectory_task.claim() + assert claim.priority == 10 + assert "arm_joint1" in claim.joints + assert "arm_joint2" in claim.joints + assert "arm_joint3" in claim.joints + + def test_execute_trajectory(self, trajectory_task, simple_trajectory): + t_now = time.perf_counter() + result = trajectory_task.execute(simple_trajectory, t_now) + assert result is True + assert trajectory_task.is_active() + assert trajectory_task.get_state() == TrajectoryState.EXECUTING + + def test_compute_during_trajectory( + self, trajectory_task, simple_trajectory, orchestrator_state + ): + t_now = time.perf_counter() + trajectory_task.execute(simple_trajectory, t_now) + + state = OrchestratorState( + joints=orchestrator_state.joints, + t_now=t_now + 0.5, + dt=0.01, + ) + output = trajectory_task.compute(state) + + assert output is not None + assert output.mode == ControlMode.POSITION + assert len(output.positions) == 3 + assert 0.4 < output.positions[0] < 0.6 + + def test_trajectory_completes(self, trajectory_task, simple_trajectory, orchestrator_state): + t_now = time.perf_counter() + trajectory_task.execute(simple_trajectory, t_now) + + state = OrchestratorState( + joints=orchestrator_state.joints, + t_now=t_now + 1.5, + dt=0.01, + ) + output = trajectory_task.compute(state) + + assert output is None + assert not trajectory_task.is_active() + assert trajectory_task.get_state() == TrajectoryState.COMPLETED + + def test_cancel_trajectory(self, trajectory_task, simple_trajectory): + t_now = time.perf_counter() + trajectory_task.execute(simple_trajectory, t_now) + assert trajectory_task.is_active() + + trajectory_task.cancel() + assert not trajectory_task.is_active() + assert trajectory_task.get_state() == TrajectoryState.ABORTED + + def test_preemption(self, trajectory_task, simple_trajectory): + t_now = time.perf_counter() + trajectory_task.execute(simple_trajectory, t_now) + + trajectory_task.on_preempted("safety_task", frozenset({"arm_joint1"})) + assert trajectory_task.get_state() == TrajectoryState.ABORTED + assert not trajectory_task.is_active() + + def test_progress(self, trajectory_task, simple_trajectory): + t_now = time.perf_counter() + trajectory_task.execute(simple_trajectory, t_now) + + assert trajectory_task.get_progress(t_now) == pytest.approx(0.0, abs=0.01) + assert trajectory_task.get_progress(t_now + 0.5) == pytest.approx(0.5, abs=0.01) + assert trajectory_task.get_progress(t_now + 1.0) == pytest.approx(1.0, abs=0.01) + + +# ============================================================================= +# Test Arbitration Logic +# ============================================================================= + + +class TestArbitration: + def test_single_task_wins(self): + outputs = [ + ( + MagicMock(name="task1"), + ResourceClaim(joints=frozenset({"j1"}), priority=10), + JointCommandOutput(joint_names=["j1"], positions=[0.5], mode=ControlMode.POSITION), + ), + ] + + winners = {} + for task, claim, output in outputs: + if output is None: + continue + values = output.get_values() + if values is None: + continue + for i, joint in enumerate(output.joint_names): + if joint not in winners: + winners[joint] = (claim.priority, values[i], output.mode, task.name) + + assert "j1" in winners + assert winners["j1"][1] == 0.5 + + def test_higher_priority_wins(self): + task_low = MagicMock() + task_low.name = "low_priority" + task_high = MagicMock() + task_high.name = "high_priority" + + outputs = [ + ( + task_low, + ResourceClaim(joints=frozenset({"j1"}), priority=10), + JointCommandOutput(joint_names=["j1"], positions=[0.5], mode=ControlMode.POSITION), + ), + ( + task_high, + ResourceClaim(joints=frozenset({"j1"}), priority=100), + JointCommandOutput(joint_names=["j1"], positions=[0.0], mode=ControlMode.POSITION), + ), + ] + + winners = {} + for task, claim, output in outputs: + if output is None: + continue + values = output.get_values() + if values is None: + continue + for i, joint in enumerate(output.joint_names): + if joint not in winners: + winners[joint] = (claim.priority, values[i], output.mode, task.name) + elif claim.priority > winners[joint][0]: + winners[joint] = (claim.priority, values[i], output.mode, task.name) + + assert winners["j1"][3] == "high_priority" + assert winners["j1"][1] == 0.0 + + def test_non_overlapping_joints(self): + task1 = MagicMock() + task1.name = "task1" + task2 = MagicMock() + task2.name = "task2" + + outputs = [ + ( + task1, + ResourceClaim(joints=frozenset({"j1", "j2"}), priority=10), + JointCommandOutput( + joint_names=["j1", "j2"], + positions=[0.5, 0.6], + mode=ControlMode.POSITION, + ), + ), + ( + task2, + ResourceClaim(joints=frozenset({"j3", "j4"}), priority=10), + JointCommandOutput( + joint_names=["j3", "j4"], + positions=[0.7, 0.8], + mode=ControlMode.POSITION, + ), + ), + ] + + winners = {} + for task, claim, output in outputs: + if output is None: + continue + values = output.get_values() + if values is None: + continue + for i, joint in enumerate(output.joint_names): + if joint not in winners: + winners[joint] = (claim.priority, values[i], output.mode, task.name) + + assert winners["j1"][3] == "task1" + assert winners["j2"][3] == "task1" + assert winners["j3"][3] == "task2" + assert winners["j4"][3] == "task2" + + +# ============================================================================= +# Test TickLoop +# ============================================================================= + + +class TestTickLoop: + def test_tick_loop_starts_and_stops(self, mock_backend): + hw = BackendHardwareInterface(mock_backend, "arm", "arm") + hardware = {"arm": hw} + tasks: dict = {} + joint_to_hardware = {f"arm_joint{i + 1}": "arm" for i in range(6)} + + tick_loop = TickLoop( + tick_rate=100.0, + hardware=hardware, + hardware_lock=threading.Lock(), + tasks=tasks, + task_lock=threading.Lock(), + joint_to_hardware=joint_to_hardware, + ) + + tick_loop.start() + time.sleep(0.05) + assert tick_loop.tick_count > 0 + + tick_loop.stop() + final_count = tick_loop.tick_count + time.sleep(0.02) + assert tick_loop.tick_count == final_count + + def test_tick_loop_calls_compute(self, mock_backend): + hw = BackendHardwareInterface(mock_backend, "arm", "arm") + hardware = {"arm": hw} + + mock_task = MagicMock() + mock_task.name = "test_task" + mock_task.is_active.return_value = True + mock_task.claim.return_value = ResourceClaim( + joints=frozenset({"arm_joint1"}), + priority=10, + ) + mock_task.compute.return_value = JointCommandOutput( + joint_names=["arm_joint1"], + positions=[0.5], + mode=ControlMode.POSITION, + ) + + tasks = {"test_task": mock_task} + joint_to_hardware = {f"arm_joint{i + 1}": "arm" for i in range(6)} + + tick_loop = TickLoop( + tick_rate=100.0, + hardware=hardware, + hardware_lock=threading.Lock(), + tasks=tasks, + task_lock=threading.Lock(), + joint_to_hardware=joint_to_hardware, + ) + + tick_loop.start() + time.sleep(0.05) + tick_loop.stop() + + assert mock_task.compute.call_count > 0 + + +# ============================================================================= +# Integration Test +# ============================================================================= + + +class TestIntegration: + def test_full_trajectory_execution(self, mock_backend): + hw = BackendHardwareInterface(mock_backend, "arm", "arm") + hardware = {"arm": hw} + + config = JointTrajectoryTaskConfig( + joint_names=[f"arm_joint{i + 1}" for i in range(6)], + priority=10, + ) + traj_task = JointTrajectoryTask(name="traj_arm", config=config) + tasks = {"traj_arm": traj_task} + + joint_to_hardware = {f"arm_joint{i + 1}": "arm" for i in range(6)} + + tick_loop = TickLoop( + tick_rate=100.0, + hardware=hardware, + hardware_lock=threading.Lock(), + tasks=tasks, + task_lock=threading.Lock(), + joint_to_hardware=joint_to_hardware, + ) + + trajectory = JointTrajectory( + joint_names=[f"arm_joint{i + 1}" for i in range(6)], + points=[ + TrajectoryPoint( + positions=[0.0] * 6, + velocities=[0.0] * 6, + time_from_start=0.0, + ), + TrajectoryPoint( + positions=[0.5] * 6, + velocities=[0.0] * 6, + time_from_start=0.5, + ), + ], + ) + + tick_loop.start() + t_now = time.perf_counter() + traj_task.execute(trajectory, t_now) + + time.sleep(0.6) + tick_loop.stop() + + assert traj_task.get_state() == TrajectoryState.COMPLETED + assert mock_backend.write_joint_positions.call_count > 0 diff --git a/dimos/e2e_tests/test_control_orchestrator.py b/dimos/e2e_tests/test_control_orchestrator.py new file mode 100644 index 0000000000..a97c045ce6 --- /dev/null +++ b/dimos/e2e_tests/test_control_orchestrator.py @@ -0,0 +1,263 @@ +# 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. + +"""End-to-end tests for the ControlOrchestrator. + +These tests start a real orchestrator process and communicate via LCM/RPC. +Unlike unit tests, these verify the full system integration. + +Run with: + pytest dimos/e2e_tests/test_control_orchestrator.py -v -s +""" + +import os +import time + +import pytest + +from dimos.control.orchestrator import ControlOrchestrator +from dimos.core.rpc_client import RPCClient +from dimos.msgs.sensor_msgs import JointState +from dimos.msgs.trajectory_msgs import JointTrajectory, TrajectoryPoint, TrajectoryState + + +@pytest.mark.skipif(bool(os.getenv("CI")), reason="LCM doesn't work in CI.") +class TestControlOrchestratorE2E: + """End-to-end tests for ControlOrchestrator.""" + + def test_orchestrator_starts_and_responds_to_rpc(self, lcm_spy, start_blueprint) -> None: + """Test that orchestrator starts and responds to RPC queries.""" + # Save topics we care about (LCM topics include type suffix) + joint_state_topic = "/orchestrator/joint_state#sensor_msgs.JointState" + lcm_spy.save_topic(joint_state_topic) + lcm_spy.save_topic("/rpc/ControlOrchestrator/list_joints/res") + lcm_spy.save_topic("/rpc/ControlOrchestrator/list_tasks/res") + + # Start the mock orchestrator blueprint + start_blueprint("orchestrator-mock") + + # Wait for joint state to be published (proves tick loop is running) + lcm_spy.wait_for_saved_topic( + joint_state_topic, + timeout=10.0, + ) + + # Create RPC client and query + client = RPCClient(None, ControlOrchestrator) + try: + # Test list_joints RPC + joints = client.list_joints() + assert joints is not None + assert len(joints) == 7 # Mock arm has 7 DOF + assert "arm_joint1" in joints + + # Test list_tasks RPC + tasks = client.list_tasks() + assert tasks is not None + assert "traj_arm" in tasks + + # Test list_hardware RPC + hardware = client.list_hardware() + assert hardware is not None + assert "arm" in hardware + finally: + client.stop_rpc_client() + + def test_orchestrator_executes_trajectory(self, lcm_spy, start_blueprint) -> None: + """Test that orchestrator executes a trajectory via RPC.""" + # Save topics + lcm_spy.save_topic("/orchestrator/joint_state#sensor_msgs.JointState") + lcm_spy.save_topic("/rpc/ControlOrchestrator/execute_trajectory/res") + lcm_spy.save_topic("/rpc/ControlOrchestrator/get_trajectory_status/res") + + # Start orchestrator + start_blueprint("orchestrator-mock") + + # Wait for it to be ready + lcm_spy.wait_for_saved_topic( + "/orchestrator/joint_state#sensor_msgs.JointState", timeout=10.0 + ) + + # Create RPC client + client = RPCClient(None, ControlOrchestrator) + try: + # Get initial joint positions + initial_positions = client.get_joint_positions() + assert initial_positions is not None + + # Create a simple trajectory + trajectory = JointTrajectory( + joint_names=[f"arm_joint{i + 1}" for i in range(7)], + points=[ + TrajectoryPoint( + time_from_start=0.0, + positions=[0.0] * 7, + velocities=[0.0] * 7, + ), + TrajectoryPoint( + time_from_start=0.5, + positions=[0.1] * 7, + velocities=[0.0] * 7, + ), + ], + ) + + # Execute trajectory + result = client.execute_trajectory("traj_arm", trajectory) + assert result is True + + # Poll for completion + timeout = 5.0 + start_time = time.time() + completed = False + + while time.time() - start_time < timeout: + status = client.get_trajectory_status("traj_arm") + if status is not None and status.get("state") == TrajectoryState.COMPLETED.name: + completed = True + break + time.sleep(0.1) + + assert completed, "Trajectory did not complete within timeout" + finally: + client.stop_rpc_client() + + def test_orchestrator_joint_state_published(self, lcm_spy, start_blueprint) -> None: + """Test that joint state messages are published at expected rate.""" + joint_state_topic = "/orchestrator/joint_state#sensor_msgs.JointState" + lcm_spy.save_topic(joint_state_topic) + + # Start orchestrator + start_blueprint("orchestrator-mock") + + # Wait for initial message + lcm_spy.wait_for_saved_topic(joint_state_topic, timeout=10.0) + + # Collect messages for 1 second + time.sleep(1.0) + + # Check we received messages (should be ~100 at 100Hz) + with lcm_spy._messages_lock: + message_count = len(lcm_spy.messages.get(joint_state_topic, [])) + + # Allow some tolerance (at least 50 messages in 1 second) + assert message_count >= 50, f"Expected ~100 messages, got {message_count}" + + # Decode a message to verify structure + with lcm_spy._messages_lock: + raw_msg = lcm_spy.messages[joint_state_topic][0] + + joint_state = JointState.lcm_decode(raw_msg) + assert len(joint_state.name) == 7 + assert len(joint_state.position) == 7 + assert "arm_joint1" in joint_state.name + + def test_orchestrator_cancel_trajectory(self, lcm_spy, start_blueprint) -> None: + """Test that a running trajectory can be cancelled.""" + lcm_spy.save_topic("/orchestrator/joint_state#sensor_msgs.JointState") + + # Start orchestrator + start_blueprint("orchestrator-mock") + lcm_spy.wait_for_saved_topic( + "/orchestrator/joint_state#sensor_msgs.JointState", timeout=10.0 + ) + + client = RPCClient(None, ControlOrchestrator) + try: + # Create a long trajectory (5 seconds) + trajectory = JointTrajectory( + joint_names=[f"arm_joint{i + 1}" for i in range(7)], + points=[ + TrajectoryPoint( + time_from_start=0.0, + positions=[0.0] * 7, + velocities=[0.0] * 7, + ), + TrajectoryPoint( + time_from_start=5.0, + positions=[1.0] * 7, + velocities=[0.0] * 7, + ), + ], + ) + + # Start trajectory + result = client.execute_trajectory("traj_arm", trajectory) + assert result is True + + # Wait a bit then cancel + time.sleep(0.5) + cancel_result = client.cancel_trajectory("traj_arm") + assert cancel_result is True + + # Check status is ABORTED + status = client.get_trajectory_status("traj_arm") + assert status is not None + assert status.get("state") == TrajectoryState.ABORTED.name + finally: + client.stop_rpc_client() + + def test_dual_arm_orchestrator(self, lcm_spy, start_blueprint) -> None: + """Test dual-arm orchestrator with independent trajectories.""" + lcm_spy.save_topic("/orchestrator/joint_state#sensor_msgs.JointState") + + # Start dual-arm mock orchestrator + start_blueprint("orchestrator-dual-mock") + lcm_spy.wait_for_saved_topic( + "/orchestrator/joint_state#sensor_msgs.JointState", timeout=10.0 + ) + + client = RPCClient(None, ControlOrchestrator) + try: + # Verify both arms present + joints = client.list_joints() + assert "left_joint1" in joints + assert "right_joint1" in joints + + tasks = client.list_tasks() + assert "traj_left" in tasks + assert "traj_right" in tasks + + # Create trajectories for both arms + left_trajectory = JointTrajectory( + joint_names=[f"left_joint{i + 1}" for i in range(7)], + points=[ + TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 7), + TrajectoryPoint(time_from_start=0.5, positions=[0.2] * 7), + ], + ) + + right_trajectory = JointTrajectory( + joint_names=[f"right_joint{i + 1}" for i in range(6)], + points=[ + TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 6), + TrajectoryPoint(time_from_start=0.5, positions=[0.3] * 6), + ], + ) + + # Execute both + assert client.execute_trajectory("traj_left", left_trajectory) is True + assert client.execute_trajectory("traj_right", right_trajectory) is True + + # Wait for completion + time.sleep(1.0) + + # Both should complete + left_status = client.get_trajectory_status("traj_left") + right_status = client.get_trajectory_status("traj_right") + + assert left_status.get("state") == TrajectoryState.COMPLETED.name + assert right_status.get("state") == TrajectoryState.COMPLETED.name + finally: + client.stop_rpc_client() From 116d9a60ba080e65748bbe4d9890de688e0067dd Mon Sep 17 00:00:00 2001 From: Mustafa Bhadsorawala <39084056+mustafab0@users.noreply.github.com> Date: Fri, 9 Jan 2026 11:06:58 -0800 Subject: [PATCH 21/42] Update dimos/control/hardware_interface.py explicit false added to the Torque Mode command sent, to avoid silent failing scenario --- dimos/control/hardware_interface.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index ec67d1ae8d..33d24a40d4 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -152,7 +152,7 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: Returns: True if command was sent successfully """ - # Initialize on first write if needed + # Initialize on first write if needed (with simple lock to prevent race) if not self._initialized: self._initialize_last_commanded() @@ -169,9 +169,12 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: return self._backend.write_joint_positions(ordered) elif mode == ControlMode.VELOCITY: return self._backend.write_joint_velocities(ordered) - # TORQUE mode not universally supported - return False - + elif mode == ControlMode.TORQUE: + # Most backends don't support torque mode + # Log and return False rather than silently ignoring + return False + else: + return False def _initialize_last_commanded(self) -> None: """Initialize last_commanded with current hardware positions.""" try: From 0a8865fb7ee2c824a657607243e8738a8c3e7bae Mon Sep 17 00:00:00 2001 From: mustafab0 <39084056+mustafab0@users.noreply.github.com> Date: Fri, 9 Jan 2026 19:07:43 +0000 Subject: [PATCH 22/42] CI code cleanup --- dimos/control/hardware_interface.py | 1 + 1 file changed, 1 insertion(+) diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 33d24a40d4..af99d2fed3 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -175,6 +175,7 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: return False else: return False + def _initialize_last_commanded(self) -> None: """Initialize last_commanded with current hardware positions.""" try: From 17d8453e6a70308d4b5c1e084cb1046b1477267f Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 9 Jan 2026 12:05:25 -0800 Subject: [PATCH 23/42] Fixed issues flagged by greptile Mode conflict detection in routing: Added check in _route_to_hardware Preemption tracking: Changed structure to {preempted_task: {joint: winning_task}} Mode conflict preemption: Tasks dropped due to mode conflict at same priority Trajectory completion edge case: Returns final position instead of None on completion Dead code removal: and Piper backend cleanup --- dimos/control/orchestrator.py | 7 +++ dimos/control/tasks/trajectory_task.py | 10 +++- dimos/control/test_control.py | 4 +- dimos/control/tick_loop.py | 48 ++++++++++++++----- dimos/hardware/manipulators/piper/backend.py | 8 +++- .../control/orchestrator_client.py | 2 - 6 files changed, 61 insertions(+), 18 deletions(-) diff --git a/dimos/control/orchestrator.py b/dimos/control/orchestrator.py index 9e48bfcccf..5a5d0cfb6b 100644 --- a/dimos/control/orchestrator.py +++ b/dimos/control/orchestrator.py @@ -180,6 +180,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: def _setup_from_config(self) -> None: """Create hardware and tasks from config (called on start).""" for hw_cfg in self.config.hardware: + backend = None try: backend = self._create_backend_from_config(hw_cfg) if not backend.connect(): @@ -195,6 +196,12 @@ def _setup_from_config(self) -> None: ) except Exception as e: logger.error(f"Failed to setup hardware {hw_cfg.id}: {e}") + # Clean up connected backend on failure + if backend is not None and hasattr(backend, "disconnect"): + try: + backend.disconnect() + except Exception: + pass # Best effort cleanup raise for task_cfg in self.config.tasks: diff --git a/dimos/control/tasks/trajectory_task.py b/dimos/control/tasks/trajectory_task.py index c2d08e4934..1c9d1bf3f3 100644 --- a/dimos/control/tasks/trajectory_task.py +++ b/dimos/control/tasks/trajectory_task.py @@ -131,11 +131,17 @@ def compute(self, state: OrchestratorState) -> JointCommandOutput | None: # Use orchestrator's time, NOT time.time() t_elapsed = state.t_now - self._start_time - # Check completion + # Check completion - clamp to final position to ensure we reach goal if t_elapsed >= self._trajectory.duration: self._state = TrajectoryState.COMPLETED logger.info(f"Trajectory {self._name} completed after {t_elapsed:.3f}s") - return None + # Return final position to hold at goal + q_ref, _ = self._trajectory.sample(self._trajectory.duration) + return JointCommandOutput( + joint_names=self._joint_names_list, + positions=list(q_ref), + mode=ControlMode.POSITION, + ) # Sample trajectory q_ref, _ = self._trajectory.sample(t_elapsed) diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index ffea5b34b2..d27264e063 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -291,7 +291,9 @@ def test_trajectory_completes(self, trajectory_task, simple_trajectory, orchestr ) output = trajectory_task.compute(state) - assert output is None + # On completion, returns final position (not None) to hold at goal + assert output is not None + assert output.positions == [1.0, 0.5, 0.25] # Final trajectory point assert not trajectory_task.is_active() assert trajectory_task.get_state() == TrajectoryState.COMPLETED diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py index bf687f0b2c..5f9040cab0 100644 --- a/dimos/control/tick_loop.py +++ b/dimos/control/tick_loop.py @@ -243,11 +243,11 @@ def _arbitrate( Returns: Tuple of: - joint_commands: {joint_name: (value, mode, task_name)} - - preemptions: {task_name: set of preempted joints} + - preemptions: {preempted_task: {joint: winning_task}} """ # Track winner per joint: joint -> (priority, value, mode, task_name) winners: dict[str, tuple[int, float, ControlMode, str]] = {} - preemptions: dict[str, set[str]] = {} + preemptions: dict[str, dict[str, str]] = {} for task, claim, output in commands: if output is None: @@ -270,19 +270,24 @@ def _arbitrate( # Higher priority wins - track preemption old_task = winners[joint_name][3] if old_task not in preemptions: - preemptions[old_task] = set() - preemptions[old_task].add(joint_name) + preemptions[old_task] = {} + preemptions[old_task][joint_name] = task.name winners[joint_name] = (priority, value, mode, task.name) elif priority == winners[joint_name][0]: # Same priority if mode != winners[joint_name][2]: # Mode conflict at same priority - log and drop + winning_task = winners[joint_name][3] logger.warning( f"Mode conflict on {joint_name}: {task.name} wants " - f"{mode.value}, but {winners[joint_name][3]} wants " - f"{winners[joint_name][2].value}. Dropping {task.name}." + f"{mode.name}, but {winning_task} wants " + f"{winners[joint_name][2].name}. Dropping {task.name}." ) + # Notify dropped task of preemption + if task.name not in preemptions: + preemptions[task.name] = {} + preemptions[task.name][joint_name] = winning_task # If same mode and same priority, first wins (keep existing) # Convert to simplified output: joint -> (value, mode, task_name) @@ -293,15 +298,26 @@ def _arbitrate( return joint_commands, preemptions - def _notify_preemptions(self, preemptions: dict[str, set[str]]) -> None: - """Notify each preempted task ONCE with all affected joints.""" + def _notify_preemptions(self, preemptions: dict[str, dict[str, str]]) -> None: + """Notify each preempted task with affected joints, grouped by winning task.""" with self._task_lock: - for task_name, joints in preemptions.items(): + for task_name, joint_winners in preemptions.items(): task = self._tasks.get(task_name) - if task: + if not task: + continue + + # Group joints by winning task + by_winner: dict[str, set[str]] = {} + for joint, winner in joint_winners.items(): + if winner not in by_winner: + by_winner[winner] = set() + by_winner[winner].add(joint) + + # Notify once per distinct winning task + for winner, joints in by_winner.items(): try: task.on_preempted( - by_task="higher_priority", + by_task=winner, joints=frozenset(joints), ) except Exception as e: @@ -326,6 +342,16 @@ def _route_to_hardware( if hw_id not in hw_commands: hw_commands[hw_id] = ({}, mode) + else: + # Check for mode conflict across joints on same hardware + existing_mode = hw_commands[hw_id][1] + if mode != existing_mode: + logger.error( + f"Mode conflict for hardware {hw_id}: joint {joint_name} wants " + f"{mode.name} but hardware already has {existing_mode.name}. " + f"Dropping command for {joint_name}." + ) + continue hw_commands[hw_id][0][joint_name] = value diff --git a/dimos/hardware/manipulators/piper/backend.py b/dimos/hardware/manipulators/piper/backend.py index 9b19ff069a..dcb62f0747 100644 --- a/dimos/hardware/manipulators/piper/backend.py +++ b/dimos/hardware/manipulators/piper/backend.py @@ -358,11 +358,15 @@ def write_enable(self, enable: bool) -> bool: # Enable with retries attempts = 0 max_attempts = 100 - while not self._sdk.EnablePiper() and attempts < max_attempts: + success = False + while attempts < max_attempts: + if self._sdk.EnablePiper(): + success = True + break time.sleep(0.01) attempts += 1 - if attempts < max_attempts: + if success: self._enabled = True # Set control mode self._sdk.MotionCtrl_2( diff --git a/dimos/manipulation/control/orchestrator_client.py b/dimos/manipulation/control/orchestrator_client.py index ba65d0750d..dfead11405 100644 --- a/dimos/manipulation/control/orchestrator_client.py +++ b/dimos/manipulation/control/orchestrator_client.py @@ -274,7 +274,6 @@ def preview_waypoints(waypoints: list[list[float]], joint_names: list[str]) -> N print("No waypoints") return - len(joint_names) print(f"\nWaypoints ({len(waypoints)}):") print("-" * 70) @@ -292,7 +291,6 @@ def preview_waypoints(waypoints: list[list[float]], joint_names: list[str]) -> N def preview_trajectory(trajectory: JointTrajectory, joint_names: list[str]) -> None: """Show generated trajectory preview.""" - len(joint_names) headers = [j.split("_")[-1][:6] for j in joint_names] header_str = " ".join(f"{h:>7}" for h in headers) From 00de446efac4ff53310bccda94b1dbb73d192a61 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 9 Jan 2026 12:22:59 -0800 Subject: [PATCH 24/42] Renamed deprecated old manipuialtion test file and Mypy type fixes --- dimos/hardware/manipulators/mock/backend.py | 6 ++- dimos/hardware/manipulators/piper/backend.py | 2 +- dimos/hardware/manipulators/spec.py | 2 +- dimos/hardware/manipulators/xarm/backend.py | 49 +++++++++++-------- ...er.py => integration_runner_deprecated.py} | 2 +- 5 files changed, 35 insertions(+), 26 deletions(-) rename dimos/hardware/manipulators_old/{test_integration_runner.py => integration_runner_deprecated.py} (99%) diff --git a/dimos/hardware/manipulators/mock/backend.py b/dimos/hardware/manipulators/mock/backend.py index 9357b5458d..80b3543739 100644 --- a/dimos/hardware/manipulators/mock/backend.py +++ b/dimos/hardware/manipulators/mock/backend.py @@ -132,11 +132,13 @@ def read_joint_efforts(self) -> list[float]: """Return mock joint efforts.""" return self._efforts.copy() - def read_state(self) -> dict: + def read_state(self) -> dict[str, int]: """Return mock state.""" + # Use index of control mode as int (0=position, 1=velocity, etc.) + mode_int = list(ControlMode).index(self._control_mode) return { "state": 0 if self._enabled else 1, - "mode": self._control_mode.value, + "mode": mode_int, } def read_error(self) -> tuple[int, str]: diff --git a/dimos/hardware/manipulators/piper/backend.py b/dimos/hardware/manipulators/piper/backend.py index dcb62f0747..f673d3f41e 100644 --- a/dimos/hardware/manipulators/piper/backend.py +++ b/dimos/hardware/manipulators/piper/backend.py @@ -223,7 +223,7 @@ def read_joint_efforts(self) -> list[float]: """ return [0.0] * self._dof - def read_state(self) -> dict: + def read_state(self) -> dict[str, int]: """Read robot state.""" if not self._sdk: return {"state": 0, "mode": 0} diff --git a/dimos/hardware/manipulators/spec.py b/dimos/hardware/manipulators/spec.py index 5e21ac5845..bef1e33b26 100644 --- a/dimos/hardware/manipulators/spec.py +++ b/dimos/hardware/manipulators/spec.py @@ -170,7 +170,7 @@ def read_joint_efforts(self) -> list[float]: """Read current joint efforts (Nm).""" ... - def read_state(self) -> dict: + def read_state(self) -> dict[str, int]: """Read robot state (mode, state code, etc).""" ... diff --git a/dimos/hardware/manipulators/xarm/backend.py b/dimos/hardware/manipulators/xarm/backend.py index bd6152900e..9ada032a3f 100644 --- a/dimos/hardware/manipulators/xarm/backend.py +++ b/dimos/hardware/manipulators/xarm/backend.py @@ -182,7 +182,7 @@ def read_joint_efforts(self) -> list[float]: return list(torques[: self._dof]) return [0.0] * self._dof - def read_state(self) -> dict: + def read_state(self) -> dict[str, int]: """Read robot state.""" if not self._arm: return {"state": 0, "mode": 0} @@ -228,7 +228,8 @@ def write_joint_positions( # Use set_servo_angle_j for high-frequency servo control (100Hz+) # This only executes the last instruction, suitable for real-time control - return self._arm.set_servo_angle_j(angles, speed=100, mvacc=500) == 0 + code: int = self._arm.set_servo_angle_j(angles, speed=100, mvacc=500) + return code == 0 def write_joint_velocities(self, velocities: list[float]) -> bool: """Write joint velocities (rad/s -> deg/s). @@ -240,13 +241,15 @@ def write_joint_velocities(self, velocities: list[float]) -> bool: # Convert rad/s to deg/s speeds = [math.degrees(v) for v in velocities] - return self._arm.vc_set_joint_velocity(speeds) == 0 + code: int = self._arm.vc_set_joint_velocity(speeds) + return code == 0 def write_stop(self) -> bool: """Emergency stop.""" if not self._arm: return False - return self._arm.emergency_stop() == 0 + code: int = self._arm.emergency_stop() + return code == 0 # ========================================================================= # Servo Control @@ -256,20 +259,23 @@ def write_enable(self, enable: bool) -> bool: """Enable or disable servos.""" if not self._arm: return False - return self._arm.motion_enable(enable=enable) == 0 + code: int = self._arm.motion_enable(enable=enable) + return code == 0 def read_enabled(self) -> bool: """Check if servos are enabled.""" if not self._arm: return False # XArm state 0 = ready/enabled - return self._arm.state == 0 + state: int = self._arm.state + return state == 0 def write_clear_errors(self) -> bool: """Clear error state.""" if not self._arm: return False - return self._arm.clean_error() == 0 + code: int = self._arm.clean_error() + return code == 0 # ========================================================================= # Cartesian Control (Optional) @@ -312,19 +318,17 @@ def write_cartesian_position( # Speed in mm/s (max ~1000 mm/s) speed = velocity * 500 - return ( - self._arm.set_position( - x=x, - y=y, - z=z, - roll=roll, - pitch=pitch, - yaw=yaw, - speed=speed, - wait=False, - ) - == 0 + code: int = self._arm.set_position( + x=x, + y=y, + z=z, + roll=roll, + pitch=pitch, + yaw=yaw, + speed=speed, + wait=False, ) + return code == 0 # ========================================================================= # Gripper (Optional) @@ -335,7 +339,9 @@ def read_gripper_position(self) -> float | None: if not self._arm: return None - code, pos = self._arm.get_gripper_position() + result = self._arm.get_gripper_position() + code: int = result[0] + pos: float | None = result[1] if code == 0 and pos is not None: return pos / 1000.0 # mm -> m return None @@ -346,7 +352,8 @@ def write_gripper_position(self, position: float) -> bool: return False pos_mm = position * 1000.0 # m -> mm - return self._arm.set_gripper_position(pos_mm) == 0 + code: int = self._arm.set_gripper_position(pos_mm) + return code == 0 # ========================================================================= # Force/Torque Sensor (Optional) diff --git a/dimos/hardware/manipulators_old/test_integration_runner.py b/dimos/hardware/manipulators_old/integration_runner_deprecated.py similarity index 99% rename from dimos/hardware/manipulators_old/test_integration_runner.py rename to dimos/hardware/manipulators_old/integration_runner_deprecated.py index eab6a022da..6766cde692 100644 --- a/dimos/hardware/manipulators_old/test_integration_runner.py +++ b/dimos/hardware/manipulators_old/integration_runner_deprecated.py @@ -49,7 +49,7 @@ import time from dimos.core.transport import LCMTransport -from dimos.hardware.manipulators.base.sdk_interface import BaseManipulatorSDK, ManipulatorInfo +from dimos.hardware.manipulators_old.base.sdk_interface import BaseManipulatorSDK, ManipulatorInfo from dimos.msgs.sensor_msgs import JointState, RobotState From 72caf1671f4e6ea62af6c3e871dc7f2651f2a893 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 9 Jan 2026 12:39:08 -0800 Subject: [PATCH 25/42] fix mypy test --- dimos/control/orchestrator.py | 4 +- dimos/control/tick_loop.py | 2 +- dimos/hardware/manipulators/piper/arm.py | 5 +- dimos/hardware/manipulators/xarm/__init__.py | 4 + dimos/hardware/manipulators/xarm/arm.py | 5 +- .../hardware/manipulators/xarm/blueprints.py | 180 +++++++----------- .../control/orchestrator_client.py | 6 +- pyproject.toml | 2 +- 8 files changed, 83 insertions(+), 125 deletions(-) diff --git a/dimos/control/orchestrator.py b/dimos/control/orchestrator.py index 5a5d0cfb6b..0ecb9e33b9 100644 --- a/dimos/control/orchestrator.py +++ b/dimos/control/orchestrator.py @@ -387,7 +387,7 @@ def execute_trajectory(self, task_name: str, trajectory: JointTrajectory) -> boo f"Executing trajectory on {task_name}: " f"{len(trajectory.points)} points, duration={trajectory.duration:.3f}s" ) - return task.execute(trajectory, t_now) + return task.execute(trajectory, t_now) # type: ignore[attr-defined,no-any-return] @rpc def get_trajectory_status(self, task_name: str) -> dict[str, Any]: @@ -423,7 +423,7 @@ def cancel_trajectory(self, task_name: str) -> bool: return False logger.info(f"Cancelling trajectory on {task_name}") - return task.cancel() + return task.cancel() # type: ignore[attr-defined,no-any-return] # ========================================================================= # Lifecycle diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py index 5f9040cab0..3d5aac5d36 100644 --- a/dimos/control/tick_loop.py +++ b/dimos/control/tick_loop.py @@ -236,7 +236,7 @@ def _arbitrate( commands: list[tuple[ControlTask, ResourceClaim, JointCommandOutput | None]], ) -> tuple[ dict[str, tuple[float, ControlMode, str]], - dict[str, set[str]], + dict[str, dict[str, str]], ]: """Per-joint arbitration with mode conflict detection. diff --git a/dimos/hardware/manipulators/piper/arm.py b/dimos/hardware/manipulators/piper/arm.py index 306505302b..0a096af640 100644 --- a/dimos/hardware/manipulators/piper/arm.py +++ b/dimos/hardware/manipulators/piper/arm.py @@ -23,6 +23,7 @@ from dataclasses import dataclass, field import threading import time +from typing import Any from dimos.core import In, Module, ModuleConfig, Out, rpc from dimos.hardware.manipulators.piper.backend import PiperBackend @@ -99,8 +100,8 @@ class Piper(Module[PiperConfig]): def __init__( self, backend: ManipulatorBackend | None = None, - *args, - **kwargs, + *args: Any, + **kwargs: Any, ) -> None: super().__init__(*args, **kwargs) diff --git a/dimos/hardware/manipulators/xarm/__init__.py b/dimos/hardware/manipulators/xarm/__init__.py index 71a1fbf112..f4a74d4283 100644 --- a/dimos/hardware/manipulators/xarm/__init__.py +++ b/dimos/hardware/manipulators/xarm/__init__.py @@ -24,6 +24,9 @@ from dimos.hardware.manipulators.xarm.arm import XArm, XArmConfig, xarm from dimos.hardware.manipulators.xarm.backend import XArmBackend + +# Backwards compatibility alias +XArmDriver = XArm from dimos.hardware.manipulators.xarm.blueprints import ( xarm5_servo, xarm7_servo, @@ -37,6 +40,7 @@ "XArm", "XArmBackend", "XArmConfig", + "XArmDriver", # Backwards compatibility alias "xarm", "xarm5_servo", "xarm7_servo", diff --git a/dimos/hardware/manipulators/xarm/arm.py b/dimos/hardware/manipulators/xarm/arm.py index e60633a096..5e10c1c9c3 100644 --- a/dimos/hardware/manipulators/xarm/arm.py +++ b/dimos/hardware/manipulators/xarm/arm.py @@ -23,6 +23,7 @@ from dataclasses import dataclass, field import threading import time +from typing import Any from dimos.core import In, Module, ModuleConfig, Out, rpc from dimos.hardware.manipulators.spec import ( @@ -101,8 +102,8 @@ class XArm(Module[XArmConfig]): def __init__( self, backend: ManipulatorBackend | None = None, - *args, - **kwargs, + *args: Any, + **kwargs: Any, ) -> None: super().__init__(*args, **kwargs) diff --git a/dimos/hardware/manipulators/xarm/blueprints.py b/dimos/hardware/manipulators/xarm/blueprints.py index a03353ebb2..e1f4d8473a 100644 --- a/dimos/hardware/manipulators/xarm/blueprints.py +++ b/dimos/hardware/manipulators/xarm/blueprints.py @@ -226,49 +226,34 @@ class JointTrajectoryControllerRight(JointTrajectoryController): # Each arm has its own topics for joint states and commands. # ============================================================================= -xarm_dual = ( - autoconnect( - XArmLeft.blueprint( - ip="192.168.2.235", # XArm7 - Left arm - dof=7, - control_rate=100.0, - monitor_rate=10.0, +xarm_dual = autoconnect( + XArmLeft.blueprint( + ip="192.168.2.235", # XArm7 - Left arm + dof=7, + control_rate=100.0, + monitor_rate=10.0, + ), + XArmRight.blueprint( + ip="192.168.1.210", # XArm6 - Right arm + dof=6, + control_rate=100.0, + monitor_rate=10.0, + ), +).transports( + { + # Left arm (XArm7) topics - use remapped names + ("left_joint_state", JointState): LCMTransport("/xarm/left/joint_states", JointState), + ("left_robot_state", RobotState): LCMTransport("/xarm/left/robot_state", RobotState), + ("left_joint_position_command", JointCommand): LCMTransport( + "/xarm/left/joint_position_command", JointCommand ), - XArmRight.blueprint( - ip="192.168.1.210", # XArm6 - Right arm - dof=6, - control_rate=100.0, - monitor_rate=10.0, + # Right arm (XArm6) topics - use remapped names + ("right_joint_state", JointState): LCMTransport("/xarm/right/joint_states", JointState), + ("right_robot_state", RobotState): LCMTransport("/xarm/right/robot_state", RobotState), + ("right_joint_position_command", JointCommand): LCMTransport( + "/xarm/right/joint_position_command", JointCommand ), - ) - .remappings( - [ - # Remap left arm connections to unique names - (XArmLeft, "joint_state", "left_joint_state"), - (XArmLeft, "robot_state", "left_robot_state"), - (XArmLeft, "joint_position_command", "left_joint_position_command"), - # Remap right arm connections to unique names - (XArmRight, "joint_state", "right_joint_state"), - (XArmRight, "robot_state", "right_robot_state"), - (XArmRight, "joint_position_command", "right_joint_position_command"), - ] - ) - .transports( - { - # Left arm (XArm7) topics - use remapped names - ("left_joint_state", JointState): LCMTransport("/xarm/left/joint_states", JointState), - ("left_robot_state", RobotState): LCMTransport("/xarm/left/robot_state", RobotState), - ("left_joint_position_command", JointCommand): LCMTransport( - "/xarm/left/joint_position_command", JointCommand - ), - # Right arm (XArm6) topics - use remapped names - ("right_joint_state", JointState): LCMTransport("/xarm/right/joint_states", JointState), - ("right_robot_state", RobotState): LCMTransport("/xarm/right/robot_state", RobotState), - ("right_joint_position_command", JointCommand): LCMTransport( - "/xarm/right/joint_position_command", JointCommand - ), - } - ) + } ) @@ -279,83 +264,50 @@ class JointTrajectoryControllerRight(JointTrajectoryController): # Each arm has independent trajectory execution. # ============================================================================= -xarm_dual_trajectory = ( - autoconnect( - # Left arm (XArm7) - XArmLeft.blueprint( - ip="192.168.2.235", - dof=7, - control_rate=100.0, - monitor_rate=10.0, +xarm_dual_trajectory = autoconnect( + # Left arm (XArm7) + XArmLeft.blueprint( + ip="192.168.2.235", + dof=7, + control_rate=100.0, + monitor_rate=10.0, + ), + # Right arm (XArm6) + XArmRight.blueprint( + ip="192.168.1.210", + dof=6, + control_rate=100.0, + monitor_rate=10.0, + ), + # Left trajectory controller + JointTrajectoryControllerLeft.blueprint( + control_frequency=100.0, + ), + # Right trajectory controller + JointTrajectoryControllerRight.blueprint( + control_frequency=100.0, + ), +).transports( + { + # Left arm topics + ("left_joint_state", JointState): LCMTransport("/xarm/left/joint_states", JointState), + ("left_robot_state", RobotState): LCMTransport("/xarm/left/robot_state", RobotState), + ("left_joint_position_command", JointCommand): LCMTransport( + "/xarm/left/joint_position_command", JointCommand ), - # Right arm (XArm6) - XArmRight.blueprint( - ip="192.168.1.210", - dof=6, - control_rate=100.0, - monitor_rate=10.0, + ("left_trajectory", JointTrajectory): LCMTransport( + "/xarm/left/trajectory", JointTrajectory ), - # Left trajectory controller - JointTrajectoryControllerLeft.blueprint( - control_frequency=100.0, + # Right arm topics + ("right_joint_state", JointState): LCMTransport("/xarm/right/joint_states", JointState), + ("right_robot_state", RobotState): LCMTransport("/xarm/right/robot_state", RobotState), + ("right_joint_position_command", JointCommand): LCMTransport( + "/xarm/right/joint_position_command", JointCommand ), - # Right trajectory controller - JointTrajectoryControllerRight.blueprint( - control_frequency=100.0, + ("right_trajectory", JointTrajectory): LCMTransport( + "/xarm/right/trajectory", JointTrajectory ), - ) - .remappings( - [ - # Left arm connections - (XArmLeft, "joint_state", "left_joint_state"), - (XArmLeft, "robot_state", "left_robot_state"), - (XArmLeft, "joint_position_command", "left_joint_position_command"), - # Right arm connections - (XArmRight, "joint_state", "right_joint_state"), - (XArmRight, "robot_state", "right_robot_state"), - (XArmRight, "joint_position_command", "right_joint_position_command"), - # Left controller connections - connect to left arm - (JointTrajectoryControllerLeft, "joint_state", "left_joint_state"), - (JointTrajectoryControllerLeft, "robot_state", "left_robot_state"), - ( - JointTrajectoryControllerLeft, - "joint_position_command", - "left_joint_position_command", - ), - (JointTrajectoryControllerLeft, "trajectory", "left_trajectory"), - # Right controller connections - connect to right arm - (JointTrajectoryControllerRight, "joint_state", "right_joint_state"), - (JointTrajectoryControllerRight, "robot_state", "right_robot_state"), - ( - JointTrajectoryControllerRight, - "joint_position_command", - "right_joint_position_command", - ), - (JointTrajectoryControllerRight, "trajectory", "right_trajectory"), - ] - ) - .transports( - { - # Left arm topics - ("left_joint_state", JointState): LCMTransport("/xarm/left/joint_states", JointState), - ("left_robot_state", RobotState): LCMTransport("/xarm/left/robot_state", RobotState), - ("left_joint_position_command", JointCommand): LCMTransport( - "/xarm/left/joint_position_command", JointCommand - ), - ("left_trajectory", JointTrajectory): LCMTransport( - "/xarm/left/trajectory", JointTrajectory - ), - # Right arm topics - ("right_joint_state", JointState): LCMTransport("/xarm/right/joint_states", JointState), - ("right_robot_state", RobotState): LCMTransport("/xarm/right/robot_state", RobotState), - ("right_joint_position_command", JointCommand): LCMTransport( - "/xarm/right/joint_position_command", JointCommand - ), - ("right_trajectory", JointTrajectory): LCMTransport( - "/xarm/right/trajectory", JointTrajectory - ), - } - ) + } ) diff --git a/dimos/manipulation/control/orchestrator_client.py b/dimos/manipulation/control/orchestrator_client.py index dfead11405..20da6dbe7e 100644 --- a/dimos/manipulation/control/orchestrator_client.py +++ b/dimos/manipulation/control/orchestrator_client.py @@ -321,7 +321,7 @@ def wait_for_completion(client: OrchestratorClient, task_name: str, timeout: flo while time.time() - start < timeout: status = client.get_trajectory_status(task_name) if not status.get("active", False): - state = status.get("state", "UNKNOWN") + state: str = status.get("state", "UNKNOWN") print(f"\nTrajectory finished: {state}") return state == "COMPLETED" @@ -508,10 +508,10 @@ def interactive_mode(client: OrchestratorClient, initial_task: str) -> None: print("\nTasks:") for t in tasks: marker = "* " if t == current_task else " " - status = " [ACTIVE]" if t in active else "" + active_marker = " [ACTIVE]" if t in active else "" t_joints = client.get_task_joints(t) if t in client._task_joints else [] joint_count = len(t_joints) if t_joints else "?" - print(f"{marker}{t} ({joint_count} joints){status}") + print(f"{marker}{t} ({joint_count} joints){active_marker}") elif cmd == "switch": if len(parts) < 2: diff --git a/pyproject.toml b/pyproject.toml index b0a1e61415..c9c4ddfb2c 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -309,7 +309,7 @@ python_version = "3.12" incremental = true strict = true warn_unused_ignores = false -exclude = "^dimos/models/Detic(/|$)|^dimos/rxpy_backpressure(/|$)|.*/test_.|.*/conftest.py*" +exclude = "^dimos/models/Detic(/|$)|^dimos/rxpy_backpressure(/|$)|^dimos/hardware/manipulators_old(/|$)|.*/test_.|.*/conftest.py*" [[tool.mypy.overrides]] module = [ From 6603cc5eac2d9b5a8de3c5fce0976ee3ef4de582 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 9 Jan 2026 14:45:27 -0800 Subject: [PATCH 26/42] mypy test fix added explicit type --- dimos/manipulation/control/dual_trajectory_setter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/manipulation/control/dual_trajectory_setter.py b/dimos/manipulation/control/dual_trajectory_setter.py index c6ae215a8d..eedd1d48a0 100644 --- a/dimos/manipulation/control/dual_trajectory_setter.py +++ b/dimos/manipulation/control/dual_trajectory_setter.py @@ -441,7 +441,7 @@ def interactive_mode(setter: DualTrajectorySetter) -> None: elif cmd == "vel" and len(parts) >= 3: arm_name = parts[1].lower() - a = ( + a: ArmState | None = ( left if arm_name in ("left", "l") else right From a0400fa01ab72c3f9aa9dc4958dc21dad632ecd7 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 9 Jan 2026 15:12:32 -0800 Subject: [PATCH 27/42] Remove deprecated manipulators_old folder --- dimos/hardware/manipulators_old/README.md | 173 ----- dimos/hardware/manipulators_old/__init__.py | 21 - .../manipulators_old/base/__init__.py | 44 -- .../base/component_based_architecture.md | 208 ------ .../base/components/__init__.py | 59 -- .../base/components/motion.py | 591 --------------- .../manipulators_old/base/components/servo.py | 522 -------------- .../base/components/status.py | 595 ---------------- .../hardware/manipulators_old/base/driver.py | 637 ----------------- .../manipulators_old/base/sdk_interface.py | 471 ------------ dimos/hardware/manipulators_old/base/spec.py | 195 ----- .../manipulators_old/base/tests/__init__.py | 15 - .../manipulators_old/base/tests/conftest.py | 362 ---------- .../base/tests/test_driver_unit.py | 577 --------------- .../manipulators_old/base/utils/__init__.py | 40 -- .../manipulators_old/base/utils/converters.py | 266 ------- .../base/utils/shared_state.py | 255 ------- .../manipulators_old/base/utils/validators.py | 254 ------- .../integration_runner_deprecated.py | 626 ---------------- .../hardware/manipulators_old/piper/README.md | 35 - .../manipulators_old/piper/__init__.py | 32 - .../manipulators_old/piper/can_activate.sh | 138 ---- .../piper/components/__init__.py | 17 - .../piper/components/configuration.py | 348 --------- .../piper/components/gripper_control.py | 120 ---- .../piper/components/kinematics.py | 116 --- .../piper/components/motion_control.py | 286 -------- .../piper/components/state_queries.py | 340 --------- .../piper/components/system_control.py | 395 ----------- .../piper/piper_blueprints.py | 172 ----- .../piper/piper_description.urdf | 497 ------------- .../manipulators_old/piper/piper_driver.py | 241 ------- .../manipulators_old/piper/piper_wrapper.py | 671 ------------------ .../hardware/manipulators_old/xarm/README.md | 149 ---- .../manipulators_old/xarm/__init__.py | 29 - .../xarm/components/__init__.py | 15 - .../xarm/components/gripper_control.py | 372 ---------- .../xarm/components/kinematics.py | 85 --- .../xarm/components/motion_control.py | 147 ---- .../xarm/components/state_queries.py | 185 ----- .../xarm/components/system_control.py | 555 --------------- dimos/hardware/manipulators_old/xarm/spec.py | 63 -- .../manipulators_old/xarm/xarm_blueprints.py | 260 ------- .../manipulators_old/xarm/xarm_driver.py | 174 ----- .../manipulators_old/xarm/xarm_wrapper.py | 564 --------------- 45 files changed, 11917 deletions(-) delete mode 100644 dimos/hardware/manipulators_old/README.md delete mode 100644 dimos/hardware/manipulators_old/__init__.py delete mode 100644 dimos/hardware/manipulators_old/base/__init__.py delete mode 100644 dimos/hardware/manipulators_old/base/component_based_architecture.md delete mode 100644 dimos/hardware/manipulators_old/base/components/__init__.py delete mode 100644 dimos/hardware/manipulators_old/base/components/motion.py delete mode 100644 dimos/hardware/manipulators_old/base/components/servo.py delete mode 100644 dimos/hardware/manipulators_old/base/components/status.py delete mode 100644 dimos/hardware/manipulators_old/base/driver.py delete mode 100644 dimos/hardware/manipulators_old/base/sdk_interface.py delete mode 100644 dimos/hardware/manipulators_old/base/spec.py delete mode 100644 dimos/hardware/manipulators_old/base/tests/__init__.py delete mode 100644 dimos/hardware/manipulators_old/base/tests/conftest.py delete mode 100644 dimos/hardware/manipulators_old/base/tests/test_driver_unit.py delete mode 100644 dimos/hardware/manipulators_old/base/utils/__init__.py delete mode 100644 dimos/hardware/manipulators_old/base/utils/converters.py delete mode 100644 dimos/hardware/manipulators_old/base/utils/shared_state.py delete mode 100644 dimos/hardware/manipulators_old/base/utils/validators.py delete mode 100644 dimos/hardware/manipulators_old/integration_runner_deprecated.py delete mode 100644 dimos/hardware/manipulators_old/piper/README.md delete mode 100644 dimos/hardware/manipulators_old/piper/__init__.py delete mode 100644 dimos/hardware/manipulators_old/piper/can_activate.sh delete mode 100644 dimos/hardware/manipulators_old/piper/components/__init__.py delete mode 100644 dimos/hardware/manipulators_old/piper/components/configuration.py delete mode 100644 dimos/hardware/manipulators_old/piper/components/gripper_control.py delete mode 100644 dimos/hardware/manipulators_old/piper/components/kinematics.py delete mode 100644 dimos/hardware/manipulators_old/piper/components/motion_control.py delete mode 100644 dimos/hardware/manipulators_old/piper/components/state_queries.py delete mode 100644 dimos/hardware/manipulators_old/piper/components/system_control.py delete mode 100644 dimos/hardware/manipulators_old/piper/piper_blueprints.py delete mode 100755 dimos/hardware/manipulators_old/piper/piper_description.urdf delete mode 100644 dimos/hardware/manipulators_old/piper/piper_driver.py delete mode 100644 dimos/hardware/manipulators_old/piper/piper_wrapper.py delete mode 100644 dimos/hardware/manipulators_old/xarm/README.md delete mode 100644 dimos/hardware/manipulators_old/xarm/__init__.py delete mode 100644 dimos/hardware/manipulators_old/xarm/components/__init__.py delete mode 100644 dimos/hardware/manipulators_old/xarm/components/gripper_control.py delete mode 100644 dimos/hardware/manipulators_old/xarm/components/kinematics.py delete mode 100644 dimos/hardware/manipulators_old/xarm/components/motion_control.py delete mode 100644 dimos/hardware/manipulators_old/xarm/components/state_queries.py delete mode 100644 dimos/hardware/manipulators_old/xarm/components/system_control.py delete mode 100644 dimos/hardware/manipulators_old/xarm/spec.py delete mode 100644 dimos/hardware/manipulators_old/xarm/xarm_blueprints.py delete mode 100644 dimos/hardware/manipulators_old/xarm/xarm_driver.py delete mode 100644 dimos/hardware/manipulators_old/xarm/xarm_wrapper.py diff --git a/dimos/hardware/manipulators_old/README.md b/dimos/hardware/manipulators_old/README.md deleted file mode 100644 index d4bb1cdba7..0000000000 --- a/dimos/hardware/manipulators_old/README.md +++ /dev/null @@ -1,173 +0,0 @@ -# Manipulator Drivers - -Component-based framework for integrating robotic manipulators into DIMOS. - -## Quick Start: Adding a New Manipulator - -Adding support for a new robot arm requires **two files**: -1. **SDK Wrapper** (~200-500 lines) - Translates vendor SDK to standard interface -2. **Driver** (~30-50 lines) - Assembles components and configuration - -## Directory Structure - -``` -manipulators/ -├── base/ # Framework (don't modify) -│ ├── sdk_interface.py # BaseManipulatorSDK abstract class -│ ├── driver.py # BaseManipulatorDriver base class -│ ├── spec.py # ManipulatorCapabilities dataclass -│ └── components/ # Reusable standard components -├── xarm/ # XArm implementation (reference) -└── piper/ # Piper implementation (reference) -``` - -## Hardware Requirements - -Your manipulator **must** support: - -| Requirement | Description | -|-------------|-------------| -| Joint Position Feedback | Read current joint angles | -| Joint Position Control | Command target joint positions | -| Servo Enable/Disable | Enable and disable motor power | -| Error Reporting | Report error codes/states | -| Emergency Stop | Hardware or software e-stop | - -**Optional:** velocity control, torque control, cartesian control, F/T sensor, gripper - -## Step 1: Implement SDK Wrapper - -Create `your_arm/your_arm_wrapper.py` implementing `BaseManipulatorSDK`: - -```python -from dimos.hardware.manipulators.base.sdk_interface import BaseManipulatorSDK, ManipulatorInfo - -class YourArmSDKWrapper(BaseManipulatorSDK): - def __init__(self): - self._sdk = None - - def connect(self, config: dict) -> bool: - self._sdk = YourNativeSDK(config['ip']) - return self._sdk.connect() - - def get_joint_positions(self) -> list[float]: - """Return positions in RADIANS.""" - degrees = self._sdk.get_angles() - return [math.radians(d) for d in degrees] - - def set_joint_positions(self, positions: list[float], - velocity: float, acceleration: float) -> bool: - return self._sdk.move_joints(positions, velocity) - - def enable_servos(self) -> bool: - return self._sdk.motor_on() - - # ... implement remaining required methods (see sdk_interface.py) -``` - -### Unit Conventions - -**All SDK wrappers must use these standard units:** - -| Quantity | Unit | -|----------|------| -| Joint positions | radians | -| Joint velocities | rad/s | -| Joint accelerations | rad/s^2 | -| Joint torques | Nm | -| Cartesian positions | meters | -| Forces | N | - -## Step 2: Create Driver Assembly - -Create `your_arm/your_arm_driver.py`: - -```python -from dimos.hardware.manipulators.base.driver import BaseManipulatorDriver -from dimos.hardware.manipulators.base.spec import ManipulatorCapabilities -from dimos.hardware.manipulators.base.components import ( - StandardMotionComponent, - StandardServoComponent, - StandardStatusComponent, -) -from .your_arm_wrapper import YourArmSDKWrapper - -class YourArmDriver(BaseManipulatorDriver): - def __init__(self, config: dict): - sdk = YourArmSDKWrapper() - - capabilities = ManipulatorCapabilities( - dof=6, - has_gripper=False, - has_force_torque=False, - joint_limits_lower=[-3.14, -2.09, -3.14, -3.14, -3.14, -3.14], - joint_limits_upper=[3.14, 2.09, 3.14, 3.14, 3.14, 3.14], - max_joint_velocity=[2.0] * 6, - max_joint_acceleration=[4.0] * 6, - ) - - components = [ - StandardMotionComponent(), - StandardServoComponent(), - StandardStatusComponent(), - ] - - super().__init__(sdk, components, config, capabilities) -``` - -## Component API Decorator - -Use `@component_api` to expose methods as RPC endpoints: - -```python -from dimos.hardware.manipulators.base.components import component_api - -class StandardMotionComponent: - @component_api - def move_joint(self, positions: list[float], velocity: float = 1.0): - """Auto-exposed as driver.move_joint()""" - ... -``` - -## Threading Architecture - -The driver runs **2 threads**: -1. **Control Loop (100Hz)** - Processes commands, reads joint state, publishes feedback -2. **Monitor Loop (10Hz)** - Reads robot state, errors, optional sensors - -``` -RPC Call → Command Queue → Control Loop → SDK → Hardware - ↓ - SharedState → LCM Publisher -``` - -## Testing Your Driver - -```python -driver = YourArmDriver({"ip": "192.168.1.100"}) -driver.start() -driver.enable_servo() -driver.move_joint([0, 0, 0, 0, 0, 0], velocity=0.5) -state = driver.get_joint_state() -driver.stop() -``` - -## Common Issues - -| Issue | Solution | -|-------|----------| -| Unit mismatch | Verify wrapper converts to radians/meters | -| Commands ignored | Ensure servos are enabled before commanding | -| Velocity not working | Some arms need mode switch via `set_control_mode()` | - -## Architecture Details - -For complete architecture documentation including full SDK interface specification, -component details, and testing strategies, see: - -**[component_based_architecture.md](base/component_based_architecture.md)** - -## Reference Implementations - -- **XArm**: [xarm/xarm_wrapper.py](xarm/xarm_wrapper.py) - Full-featured wrapper -- **Piper**: [piper/piper_wrapper.py](piper/piper_wrapper.py) - Shows velocity workaround diff --git a/dimos/hardware/manipulators_old/__init__.py b/dimos/hardware/manipulators_old/__init__.py deleted file mode 100644 index a54a846afc..0000000000 --- a/dimos/hardware/manipulators_old/__init__.py +++ /dev/null @@ -1,21 +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. - -""" -Manipulator Hardware Drivers - -Drivers for various robotic manipulator arms. -""" - -__all__ = [] diff --git a/dimos/hardware/manipulators_old/base/__init__.py b/dimos/hardware/manipulators_old/base/__init__.py deleted file mode 100644 index 3ed58d9819..0000000000 --- a/dimos/hardware/manipulators_old/base/__init__.py +++ /dev/null @@ -1,44 +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. - -"""Base framework for generalized manipulator drivers. - -This package provides the foundation for building manipulator drivers -that work with any robotic arm (XArm, Piper, UR, Franka, etc.). -""" - -from .components import StandardMotionComponent, StandardServoComponent, StandardStatusComponent -from .driver import BaseManipulatorDriver, Command -from .sdk_interface import BaseManipulatorSDK, ManipulatorInfo -from .spec import ManipulatorCapabilities, ManipulatorDriverSpec, RobotState -from .utils import SharedState - -__all__ = [ - # Driver - "BaseManipulatorDriver", - # SDK Interface - "BaseManipulatorSDK", - "Command", - "ManipulatorCapabilities", - # Spec - "ManipulatorDriverSpec", - "ManipulatorInfo", - "RobotState", - # Utils - "SharedState", - # Components - "StandardMotionComponent", - "StandardServoComponent", - "StandardStatusComponent", -] diff --git a/dimos/hardware/manipulators_old/base/component_based_architecture.md b/dimos/hardware/manipulators_old/base/component_based_architecture.md deleted file mode 100644 index 893ebf1276..0000000000 --- a/dimos/hardware/manipulators_old/base/component_based_architecture.md +++ /dev/null @@ -1,208 +0,0 @@ -# Component-Based Architecture for Manipulator Drivers - -## Overview - -This architecture provides maximum code reuse through standardized SDK wrappers and reusable components. Each new manipulator requires only an SDK wrapper (~200-500 lines) and a thin driver assembly (~30-50 lines). - -## Architecture Layers - -``` -┌─────────────────────────────────────────────────────┐ -│ RPC Interface │ -│ (Standardized across all arms) │ -└─────────────────────────────────────────────────────┘ - ▲ -┌─────────────────────────────────────────────────────┐ -│ Driver Instance (XArmDriver) │ -│ Extends DIMOS Module, assembles components │ -└─────────────────────────────────────────────────────┘ - ▲ -┌─────────────────────────────────────────────────────┐ -│ Standard Components │ -│ (Motion, Servo, Status) - reused everywhere │ -└─────────────────────────────────────────────────────┘ - ▲ -┌─────────────────────────────────────────────────────┐ -│ SDK Wrapper (XArmSDKWrapper) │ -│ Implements BaseManipulatorSDK interface │ -└─────────────────────────────────────────────────────┘ - ▲ -┌─────────────────────────────────────────────────────┐ -│ Native Vendor SDK (XArmAPI) │ -└─────────────────────────────────────────────────────┘ -``` - -## Core Interfaces - -### BaseManipulatorSDK - -Abstract interface that all SDK wrappers must implement. See `sdk_interface.py` for full specification. - -**Required methods:** `connect()`, `disconnect()`, `is_connected()`, `get_joint_positions()`, `get_joint_velocities()`, `set_joint_positions()`, `enable_servos()`, `disable_servos()`, `emergency_stop()`, `get_error_code()`, `clear_errors()`, `get_info()` - -**Optional methods:** `get_force_torque()`, `get_gripper_position()`, `set_cartesian_position()`, etc. - -### ManipulatorCapabilities - -Dataclass defining arm properties: DOF, joint limits, velocity limits, feature flags. - -## Component System - -### @component_api Decorator - -Methods marked with `@component_api` are automatically exposed as RPC endpoints on the driver: - -```python -from dimos.hardware.manipulators.base.components import component_api - -class StandardMotionComponent: - @component_api - def move_joint(self, positions: list[float], velocity: float = 1.0) -> dict: - """Auto-exposed as driver.move_joint()""" - ... -``` - -### Dependency Injection - -Components receive dependencies via setter methods, not constructor: - -```python -class StandardMotionComponent: - def __init__(self): - self.sdk = None - self.shared_state = None - self.command_queue = None - self.capabilities = None - - def set_sdk(self, sdk): self.sdk = sdk - def set_shared_state(self, state): self.shared_state = state - def set_command_queue(self, queue): self.command_queue = queue - def set_capabilities(self, caps): self.capabilities = caps - def initialize(self): pass # Called after all setters -``` - -### Standard Components - -| Component | Purpose | Key Methods | -|-----------|---------|-------------| -| `StandardMotionComponent` | Joint/cartesian motion | `move_joint()`, `move_joint_velocity()`, `get_joint_state()`, `stop_motion()` | -| `StandardServoComponent` | Motor control | `enable_servo()`, `disable_servo()`, `emergency_stop()`, `set_control_mode()` | -| `StandardStatusComponent` | Monitoring | `get_robot_state()`, `get_error_state()`, `get_health_metrics()` | - -## Threading Model - -The driver runs **2 threads**: - -1. **Control Loop (100Hz)** - Process commands, read joint state, publish feedback -2. **Monitor Loop (10Hz)** - Read robot state, errors, optional sensors (F/T, gripper) - -``` -RPC Call → Command Queue → Control Loop → SDK → Hardware - ↓ - SharedState (thread-safe) - ↓ - LCM Publisher → External Systems -``` - -## DIMOS Module Integration - -The driver extends `Module` for pub/sub integration: - -```python -class BaseManipulatorDriver(Module): - def __init__(self, sdk, components, config, capabilities): - super().__init__() - self.shared_state = SharedState() - self.command_queue = Queue(maxsize=10) - - # Inject dependencies into components - for component in components: - component.set_sdk(sdk) - component.set_shared_state(self.shared_state) - component.set_command_queue(self.command_queue) - component.set_capabilities(capabilities) - component.initialize() - - # Auto-expose @component_api methods - self._auto_expose_component_apis() -``` - -## Adding a New Manipulator - -### Step 1: SDK Wrapper - -```python -class YourArmSDKWrapper(BaseManipulatorSDK): - def get_joint_positions(self) -> list[float]: - degrees = self._sdk.get_angles() - return [math.radians(d) for d in degrees] # Convert to radians - - def set_joint_positions(self, positions, velocity, acceleration) -> bool: - return self._sdk.move_joints(positions, velocity) - - # ... implement remaining required methods -``` - -### Step 2: Driver Assembly - -```python -class YourArmDriver(BaseManipulatorDriver): - def __init__(self, config: dict): - sdk = YourArmSDKWrapper() - capabilities = ManipulatorCapabilities( - dof=6, - joint_limits_lower=[-3.14] * 6, - joint_limits_upper=[3.14] * 6, - ) - components = [ - StandardMotionComponent(), - StandardServoComponent(), - StandardStatusComponent(), - ] - super().__init__(sdk, components, config, capabilities) -``` - -## Unit Conventions - -All SDK wrappers must convert to standard units: - -| Quantity | Unit | -|----------|------| -| Positions | radians | -| Velocities | rad/s | -| Accelerations | rad/s^2 | -| Torques | Nm | -| Cartesian | meters | - -## Testing Strategy - -```python -# Test SDK wrapper with mocked native SDK -def test_wrapper_positions(): - mock = Mock() - mock.get_angles.return_value = [0, 90, 180] - wrapper = YourArmSDKWrapper() - wrapper._sdk = mock - assert wrapper.get_joint_positions() == [0, math.pi/2, math.pi] - -# Test component with mocked SDK wrapper -def test_motion_component(): - mock_sdk = Mock(spec=BaseManipulatorSDK) - component = StandardMotionComponent() - component.set_sdk(mock_sdk) - component.move_joint([0, 0, 0]) - # Verify command was queued -``` - -## Advantages - -- **Maximum reuse**: Components tested once, used by 100+ arms -- **Consistent behavior**: All arms identical at RPC level -- **Centralized fixes**: Fix once in component, all arms benefit -- **Team scalability**: Developers work on wrappers independently -- **Strong contracts**: SDK interface defines exact requirements - -## Reference Implementations - -- **XArm**: `xarm/xarm_wrapper.py` - Full-featured, converts degrees→radians -- **Piper**: `piper/piper_wrapper.py` - Shows velocity integration workaround diff --git a/dimos/hardware/manipulators_old/base/components/__init__.py b/dimos/hardware/manipulators_old/base/components/__init__.py deleted file mode 100644 index b04f60f691..0000000000 --- a/dimos/hardware/manipulators_old/base/components/__init__.py +++ /dev/null @@ -1,59 +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. - -"""Standard components for manipulator drivers.""" - -from collections.abc import Callable -from typing import Any, TypeVar - -F = TypeVar("F", bound=Callable[..., Any]) - - -def component_api(fn: F) -> F: - """Decorator to mark component methods that should be exposed as driver RPCs. - - Methods decorated with @component_api will be automatically discovered by the - driver and exposed as @rpc methods on the driver instance. This allows external - code to call these methods via the standard Module RPC system. - - Example: - class MyComponent: - @component_api - def enable_servo(self): - '''Enable servo motors.''' - return self.sdk.enable_servos() - - # The driver will auto-generate: - # @rpc - # def enable_servo(self): - # return component.enable_servo() - - # External code can then call: - # driver.enable_servo() - """ - fn.__component_api__ = True # type: ignore[attr-defined] - return fn - - -# Import components AFTER defining component_api to avoid circular imports -from .motion import StandardMotionComponent -from .servo import StandardServoComponent -from .status import StandardStatusComponent - -__all__ = [ - "StandardMotionComponent", - "StandardServoComponent", - "StandardStatusComponent", - "component_api", -] diff --git a/dimos/hardware/manipulators_old/base/components/motion.py b/dimos/hardware/manipulators_old/base/components/motion.py deleted file mode 100644 index f3205acb01..0000000000 --- a/dimos/hardware/manipulators_old/base/components/motion.py +++ /dev/null @@ -1,591 +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. - -"""Standard motion control component for manipulator drivers.""" - -import logging -from queue import Queue -import time -from typing import Any - -from ..driver import Command -from ..sdk_interface import BaseManipulatorSDK -from ..spec import ManipulatorCapabilities -from ..utils import SharedState, scale_velocities, validate_joint_limits, validate_velocity_limits -from . import component_api - - -class StandardMotionComponent: - """Motion control component that works with any SDK wrapper. - - This component provides standard motion control methods that work - consistently across all manipulator types. Methods decorated with @component_api - are automatically exposed as RPC methods on the driver. It handles: - - Joint position control - - Joint velocity control - - Joint effort/torque control (if supported) - - Trajectory execution (if supported) - - Motion safety validation - """ - - def __init__( - self, - sdk: BaseManipulatorSDK | None = None, - shared_state: SharedState | None = None, - command_queue: Queue[Any] | None = None, - capabilities: ManipulatorCapabilities | None = None, - ) -> None: - """Initialize the motion component. - - Args: - sdk: SDK wrapper instance (can be set later) - shared_state: Shared state instance (can be set later) - command_queue: Command queue (can be set later) - capabilities: Manipulator capabilities (can be set later) - """ - self.sdk = sdk - self.shared_state = shared_state - self.command_queue = command_queue - self.capabilities = capabilities - self.logger = logging.getLogger(self.__class__.__name__) - - # Motion limits - self.velocity_scale = 1.0 # Global velocity scaling (0-1) - self.acceleration_scale = 1.0 # Global acceleration scaling (0-1) - - # ============= Initialization Methods (called by BaseDriver) ============= - - def set_sdk(self, sdk: BaseManipulatorSDK) -> None: - """Set the SDK wrapper instance.""" - self.sdk = sdk - - def set_shared_state(self, shared_state: SharedState) -> None: - """Set the shared state instance.""" - self.shared_state = shared_state - - def set_command_queue(self, command_queue: "Queue[Any]") -> None: - """Set the command queue instance.""" - self.command_queue = command_queue - - def set_capabilities(self, capabilities: ManipulatorCapabilities) -> None: - """Set the capabilities instance.""" - self.capabilities = capabilities - - def initialize(self) -> None: - """Initialize the component after all resources are set.""" - self.logger.debug("Motion component initialized") - - # ============= Component API Methods ============= - - @component_api - def move_joint( - self, - positions: list[float], - velocity: float = 1.0, - acceleration: float = 1.0, - wait: bool = False, - validate: bool = True, - ) -> dict[str, Any]: - """Move joints to target positions. - - Args: - positions: Target joint positions in radians - velocity: Velocity scaling factor (0-1) - acceleration: Acceleration scaling factor (0-1) - wait: If True, block until motion completes - validate: If True, validate against joint limits - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - # Validate inputs - if validate and self.capabilities: - if len(positions) != self.capabilities.dof: - return { - "success": False, - "error": f"Expected {self.capabilities.dof} positions, got {len(positions)}", - } - - # Check joint limits - if self.capabilities.joint_limits_lower and self.capabilities.joint_limits_upper: - valid, error = validate_joint_limits( - positions, - self.capabilities.joint_limits_lower, - self.capabilities.joint_limits_upper, - ) - if not valid: - return {"success": False, "error": error} - - # Apply global scaling - velocity = velocity * self.velocity_scale - acceleration = acceleration * self.acceleration_scale - - # Queue command for async execution - if self.command_queue and not wait: - command = Command( - type="position", - data={ - "positions": positions, - "velocity": velocity, - "acceleration": acceleration, - "wait": False, - }, - timestamp=time.time(), - ) - self.command_queue.put(command) - return {"success": True, "queued": True} - - # Execute directly (blocking or wait mode) - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - success = self.sdk.set_joint_positions(positions, velocity, acceleration, wait) - - if success and self.shared_state: - self.shared_state.set_target_joints(positions=positions) - - return {"success": success} - - except Exception as e: - self.logger.error(f"Error in move_joint: {e}") - return {"success": False, "error": str(e)} - - @component_api - def move_joint_velocity( - self, velocities: list[float], acceleration: float = 1.0, validate: bool = True - ) -> dict[str, Any]: - """Set joint velocities. - - Args: - velocities: Target joint velocities in rad/s - acceleration: Acceleration scaling factor (0-1) - validate: If True, validate against velocity limits - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - # Validate inputs - if validate and self.capabilities: - if len(velocities) != self.capabilities.dof: - return { - "success": False, - "error": f"Expected {self.capabilities.dof} velocities, got {len(velocities)}", - } - - # Check velocity limits - if self.capabilities.max_joint_velocity: - valid, _error = validate_velocity_limits( - velocities, self.capabilities.max_joint_velocity, self.velocity_scale - ) - if not valid: - # Scale velocities to stay within limits - velocities = scale_velocities( - velocities, self.capabilities.max_joint_velocity, self.velocity_scale - ) - self.logger.warning("Velocities scaled to stay within limits") - - # Queue command for async execution - if self.command_queue: - command = Command( - type="velocity", data={"velocities": velocities}, timestamp=time.time() - ) - self.command_queue.put(command) - return {"success": True, "queued": True} - - # Execute directly - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - success = self.sdk.set_joint_velocities(velocities) - - if success and self.shared_state: - self.shared_state.set_target_joints(velocities=velocities) - - return {"success": success} - - except Exception as e: - self.logger.error(f"Error in move_joint_velocity: {e}") - return {"success": False, "error": str(e)} - - @component_api - def move_joint_effort(self, efforts: list[float], validate: bool = True) -> dict[str, Any]: - """Set joint efforts/torques. - - Args: - efforts: Target joint efforts in Nm - validate: If True, validate inputs - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - # Check if effort control is supported - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - if not hasattr(self.sdk, "set_joint_efforts"): - return {"success": False, "error": "Effort control not supported"} - - # Validate inputs - if validate and self.capabilities: - if len(efforts) != self.capabilities.dof: - return { - "success": False, - "error": f"Expected {self.capabilities.dof} efforts, got {len(efforts)}", - } - - # Queue command for async execution - if self.command_queue: - command = Command(type="effort", data={"efforts": efforts}, timestamp=time.time()) - self.command_queue.put(command) - return {"success": True, "queued": True} - - # Execute directly - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - success = self.sdk.set_joint_efforts(efforts) - - if success and self.shared_state: - self.shared_state.set_target_joints(efforts=efforts) - - return {"success": success} - - except Exception as e: - self.logger.error(f"Error in move_joint_effort: {e}") - return {"success": False, "error": str(e)} - - @component_api - def stop_motion(self) -> dict[str, Any]: - """Stop all ongoing motion immediately. - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - # Queue stop command with high priority - if self.command_queue: - command = Command(type="stop", data={}, timestamp=time.time()) - # Clear queue and add stop command - while not self.command_queue.empty(): - try: - self.command_queue.get_nowait() - except: - break - self.command_queue.put(command) - - # Also execute directly for immediate stop - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - success = self.sdk.stop_motion() - - # Clear targets - if self.shared_state: - self.shared_state.set_target_joints(positions=None, velocities=None, efforts=None) - - return {"success": success} - - except Exception as e: - self.logger.error(f"Error in stop_motion: {e}") - return {"success": False, "error": str(e)} - - @component_api - def get_joint_state(self) -> dict[str, Any]: - """Get current joint state. - - Returns: - Dict with joint positions, velocities, efforts, and timestamp - """ - try: - if self.shared_state: - # Get from shared state (updated by reader thread) - positions, velocities, efforts = self.shared_state.get_joint_state() - else: - # Get directly from SDK - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - positions = self.sdk.get_joint_positions() - velocities = self.sdk.get_joint_velocities() - efforts = self.sdk.get_joint_efforts() - - return { - "positions": positions, - "velocities": velocities, - "efforts": efforts, - "timestamp": time.time(), - "success": True, - } - - except Exception as e: - self.logger.error(f"Error in get_joint_state: {e}") - return {"success": False, "error": str(e)} - - @component_api - def get_joint_limits(self) -> dict[str, Any]: - """Get joint position limits. - - Returns: - Dict with lower and upper limits in radians - """ - try: - if self.capabilities: - return { - "lower": self.capabilities.joint_limits_lower, - "upper": self.capabilities.joint_limits_upper, - "success": True, - } - else: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - lower, upper = self.sdk.get_joint_limits() - return {"lower": lower, "upper": upper, "success": True} - - except Exception as e: - self.logger.error(f"Error in get_joint_limits: {e}") - return {"success": False, "error": str(e)} - - @component_api - def get_velocity_limits(self) -> dict[str, Any]: - """Get joint velocity limits. - - Returns: - Dict with maximum velocities in rad/s - """ - try: - if self.capabilities: - return {"limits": self.capabilities.max_joint_velocity, "success": True} - else: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - limits = self.sdk.get_velocity_limits() - return {"limits": limits, "success": True} - - except Exception as e: - self.logger.error(f"Error in get_velocity_limits: {e}") - return {"success": False, "error": str(e)} - - @component_api - def set_velocity_scale(self, scale: float) -> dict[str, Any]: - """Set global velocity scaling factor. - - Args: - scale: Velocity scale factor (0-1) - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - if scale <= 0 or scale > 1: - return {"success": False, "error": f"Invalid scale {scale}, must be in (0, 1]"} - - self.velocity_scale = scale - return {"success": True, "scale": scale} - - except Exception as e: - self.logger.error(f"Error in set_velocity_scale: {e}") - return {"success": False, "error": str(e)} - - @component_api - def set_acceleration_scale(self, scale: float) -> dict[str, Any]: - """Set global acceleration scaling factor. - - Args: - scale: Acceleration scale factor (0-1) - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - if scale <= 0 or scale > 1: - return {"success": False, "error": f"Invalid scale {scale}, must be in (0, 1]"} - - self.acceleration_scale = scale - return {"success": True, "scale": scale} - - except Exception as e: - self.logger.error(f"Error in set_acceleration_scale: {e}") - return {"success": False, "error": str(e)} - - # ============= Cartesian Control (Optional) ============= - - @component_api - def move_cartesian( - self, - pose: dict[str, float], - velocity: float = 1.0, - acceleration: float = 1.0, - wait: bool = False, - ) -> dict[str, Any]: - """Move end-effector to target pose. - - Args: - pose: Target pose with keys: x, y, z (meters), roll, pitch, yaw (radians) - velocity: Velocity scaling factor (0-1) - acceleration: Acceleration scaling factor (0-1) - wait: If True, block until motion completes - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - # Check if Cartesian control is supported - if not self.capabilities or not self.capabilities.has_cartesian_control: - return {"success": False, "error": "Cartesian control not supported"} - - # Apply global scaling - velocity = velocity * self.velocity_scale - acceleration = acceleration * self.acceleration_scale - - # Queue command for async execution - if self.command_queue and not wait: - command = Command( - type="cartesian", - data={ - "pose": pose, - "velocity": velocity, - "acceleration": acceleration, - "wait": False, - }, - timestamp=time.time(), - ) - self.command_queue.put(command) - return {"success": True, "queued": True} - - # Execute directly - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - success = self.sdk.set_cartesian_position(pose, velocity, acceleration, wait) - - if success and self.shared_state: - self.shared_state.target_cartesian_position = pose - - return {"success": success} - - except Exception as e: - self.logger.error(f"Error in move_cartesian: {e}") - return {"success": False, "error": str(e)} - - @component_api - def get_cartesian_state(self) -> dict[str, Any]: - """Get current end-effector pose. - - Returns: - Dict with pose (x, y, z, roll, pitch, yaw) and timestamp - """ - try: - # Check if Cartesian control is supported - if not self.capabilities or not self.capabilities.has_cartesian_control: - return {"success": False, "error": "Cartesian control not supported"} - - pose: dict[str, float] | None = None - if self.shared_state and self.shared_state.cartesian_position: - # Get from shared state - pose = self.shared_state.cartesian_position - else: - # Get directly from SDK - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - pose = self.sdk.get_cartesian_position() - - if pose: - return {"pose": pose, "timestamp": time.time(), "success": True} - else: - return {"success": False, "error": "Failed to get Cartesian state"} - - except Exception as e: - self.logger.error(f"Error in get_cartesian_state: {e}") - return {"success": False, "error": str(e)} - - # ============= Trajectory Execution (Optional) ============= - - @component_api - def execute_trajectory( - self, trajectory: list[dict[str, Any]], wait: bool = True - ) -> dict[str, Any]: - """Execute a joint trajectory. - - Args: - trajectory: List of waypoints, each with: - - 'positions': list[float] in radians - - 'velocities': Optional list[float] in rad/s - - 'time': float seconds from start - wait: If True, block until trajectory completes - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - # Check if trajectory execution is supported - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - if not hasattr(self.sdk, "execute_trajectory"): - return {"success": False, "error": "Trajectory execution not supported"} - - # Validate trajectory if capabilities available - if self.capabilities: - from ..utils import validate_trajectory - - # Only validate if all required capability fields are present - jl_lower = self.capabilities.joint_limits_lower - jl_upper = self.capabilities.joint_limits_upper - max_vel = self.capabilities.max_joint_velocity - max_acc = self.capabilities.max_joint_acceleration - - if ( - jl_lower is not None - and jl_upper is not None - and max_vel is not None - and max_acc is not None - ): - valid, error = validate_trajectory( - trajectory, - jl_lower, - jl_upper, - max_vel, - max_acc, - ) - if not valid: - return {"success": False, "error": error} - else: - self.logger.debug("Skipping trajectory validation; capabilities incomplete") - - # Execute trajectory - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - success = self.sdk.execute_trajectory(trajectory, wait) - - return {"success": success} - - except Exception as e: - self.logger.error(f"Error in execute_trajectory: {e}") - return {"success": False, "error": str(e)} - - @component_api - def stop_trajectory(self) -> dict[str, Any]: - """Stop any executing trajectory. - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - # Check if trajectory execution is supported - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - if not hasattr(self.sdk, "stop_trajectory"): - return {"success": False, "error": "Trajectory execution not supported"} - - success = self.sdk.stop_trajectory() - return {"success": success} - - except Exception as e: - self.logger.error(f"Error in stop_trajectory: {e}") - return {"success": False, "error": str(e)} diff --git a/dimos/hardware/manipulators_old/base/components/servo.py b/dimos/hardware/manipulators_old/base/components/servo.py deleted file mode 100644 index c773f10723..0000000000 --- a/dimos/hardware/manipulators_old/base/components/servo.py +++ /dev/null @@ -1,522 +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. - -"""Standard servo control component for manipulator drivers.""" - -import logging -import time -from typing import Any - -from ..sdk_interface import BaseManipulatorSDK -from ..spec import ManipulatorCapabilities -from ..utils import SharedState -from . import component_api - - -class StandardServoComponent: - """Servo control component that works with any SDK wrapper. - - This component provides standard servo/motor control methods that work - consistently across all manipulator types. Methods decorated with @component_api - are automatically exposed as RPC methods on the driver. It handles: - - Servo enable/disable - - Control mode switching - - Emergency stop - - Error recovery - - Homing operations - """ - - def __init__( - self, - sdk: BaseManipulatorSDK | None = None, - shared_state: SharedState | None = None, - capabilities: ManipulatorCapabilities | None = None, - ): - """Initialize the servo component. - - Args: - sdk: SDK wrapper instance (can be set later) - shared_state: Shared state instance (can be set later) - capabilities: Manipulator capabilities (can be set later) - """ - self.sdk = sdk - self.shared_state = shared_state - self.capabilities = capabilities - self.logger = logging.getLogger(self.__class__.__name__) - - # State tracking - self.last_enable_time = 0.0 - self.last_disable_time = 0.0 - - # ============= Initialization Methods (called by BaseDriver) ============= - - def set_sdk(self, sdk: BaseManipulatorSDK) -> None: - """Set the SDK wrapper instance.""" - self.sdk = sdk - - def set_shared_state(self, shared_state: SharedState) -> None: - """Set the shared state instance.""" - self.shared_state = shared_state - - def set_capabilities(self, capabilities: ManipulatorCapabilities) -> None: - """Set the capabilities instance.""" - self.capabilities = capabilities - - def initialize(self) -> None: - """Initialize the component after all resources are set.""" - self.logger.debug("Servo component initialized") - - # ============= Component API Methods ============= - - @component_api - def enable_servo(self, check_errors: bool = True) -> dict[str, Any]: - """Enable servo/motor control. - - Args: - check_errors: If True, check for errors before enabling - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - # Check if already enabled - if self.sdk.are_servos_enabled(): - return {"success": True, "message": "Servos already enabled"} - - # Check for errors if requested - if check_errors: - error_code = self.sdk.get_error_code() - if error_code != 0: - error_msg = self.sdk.get_error_message() - return { - "success": False, - "error": f"Cannot enable servos with active error: {error_msg} (code: {error_code})", - } - - # Enable servos - success = self.sdk.enable_servos() - - if success: - self.last_enable_time = time.time() - if self.shared_state: - self.shared_state.is_enabled = True - self.logger.info("Servos enabled successfully") - else: - self.logger.error("Failed to enable servos") - - return {"success": success} - - except Exception as e: - self.logger.error(f"Error in enable_servo: {e}") - return {"success": False, "error": str(e)} - - @component_api - def disable_servo(self, stop_motion: bool = True) -> dict[str, Any]: - """Disable servo/motor control. - - Args: - stop_motion: If True, stop any ongoing motion first - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - # Check if already disabled - if not self.sdk.are_servos_enabled(): - return {"success": True, "message": "Servos already disabled"} - - # Stop motion if requested - if stop_motion: - self.sdk.stop_motion() - time.sleep(0.1) # Brief delay to ensure motion stopped - - # Disable servos - success = self.sdk.disable_servos() - - if success: - self.last_disable_time = time.time() - if self.shared_state: - self.shared_state.is_enabled = False - self.logger.info("Servos disabled successfully") - else: - self.logger.error("Failed to disable servos") - - return {"success": success} - - except Exception as e: - self.logger.error(f"Error in disable_servo: {e}") - return {"success": False, "error": str(e)} - - @component_api - def toggle_servo(self) -> dict[str, Any]: - """Toggle servo enable/disable state. - - Returns: - Dict with 'success', 'enabled' state, and optional 'error' keys - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - current_state = self.sdk.are_servos_enabled() - - if current_state: - result = self.disable_servo() - else: - result = self.enable_servo() - - if result["success"]: - result["enabled"] = not current_state - - return result - - except Exception as e: - self.logger.error(f"Error in toggle_servo: {e}") - return {"success": False, "error": str(e)} - - @component_api - def get_servo_state(self) -> dict[str, Any]: - """Get current servo state. - - Returns: - Dict with servo state information - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - enabled = self.sdk.are_servos_enabled() - robot_state = self.sdk.get_robot_state() - - return { - "enabled": enabled, - "mode": robot_state.get("mode", 0), - "state": robot_state.get("state", 0), - "is_moving": robot_state.get("is_moving", False), - "last_enable_time": self.last_enable_time, - "last_disable_time": self.last_disable_time, - "success": True, - } - - except Exception as e: - self.logger.error(f"Error in get_servo_state: {e}") - return {"success": False, "error": str(e)} - - @component_api - def emergency_stop(self) -> dict[str, Any]: - """Execute emergency stop. - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - # Execute e-stop - success = self.sdk.emergency_stop() - - if success: - # Update shared state - if self.shared_state: - self.shared_state.update_robot_state(state=3) # 3 = e-stop state - self.shared_state.is_enabled = False - self.shared_state.is_moving = False - - self.logger.warning("Emergency stop executed") - else: - self.logger.error("Failed to execute emergency stop") - - return {"success": success} - - except Exception as e: - self.logger.error(f"Error in emergency_stop: {e}") - # Try to stop motion as fallback - try: - if self.sdk is not None: - self.sdk.stop_motion() - except: - pass - return {"success": False, "error": str(e)} - - @component_api - def reset_emergency_stop(self) -> dict[str, Any]: - """Reset from emergency stop state. - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - # Clear errors first - self.sdk.clear_errors() - - # Re-enable servos - success = self.sdk.enable_servos() - - if success: - if self.shared_state: - self.shared_state.update_robot_state(state=0) # 0 = idle - self.shared_state.is_enabled = True - - self.logger.info("Emergency stop reset successfully") - else: - self.logger.error("Failed to reset emergency stop") - - return {"success": success} - - except Exception as e: - self.logger.error(f"Error in reset_emergency_stop: {e}") - return {"success": False, "error": str(e)} - - @component_api - def set_control_mode(self, mode: str) -> dict[str, Any]: - """Set control mode. - - Args: - mode: Control mode ('position', 'velocity', 'torque', 'impedance') - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - # Validate mode - valid_modes = ["position", "velocity", "torque", "impedance"] - if mode not in valid_modes: - return { - "success": False, - "error": f"Invalid mode '{mode}'. Valid modes: {valid_modes}", - } - - # Check if mode is supported - if mode == "impedance" and self.capabilities: - if not self.capabilities.has_impedance_control: - return {"success": False, "error": "Impedance control not supported"} - - # Set control mode - success = self.sdk.set_control_mode(mode) - - if success: - # Map mode string to integer - mode_map = {"position": 0, "velocity": 1, "torque": 2, "impedance": 3} - if self.shared_state: - self.shared_state.update_robot_state(mode=mode_map.get(mode, 0)) - - self.logger.info(f"Control mode set to '{mode}'") - - return {"success": success} - - except Exception as e: - self.logger.error(f"Error in set_control_mode: {e}") - return {"success": False, "error": str(e)} - - @component_api - def get_control_mode(self) -> dict[str, Any]: - """Get current control mode. - - Returns: - Dict with current mode and success status - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - mode = self.sdk.get_control_mode() - - if mode: - return {"mode": mode, "success": True} - else: - # Try to get from robot state - robot_state = self.sdk.get_robot_state() - mode_int = robot_state.get("mode", 0) - - # Map integer to string - mode_map = {0: "position", 1: "velocity", 2: "torque", 3: "impedance"} - mode_str = mode_map.get(mode_int, "unknown") - - return {"mode": mode_str, "success": True} - - except Exception as e: - self.logger.error(f"Error in get_control_mode: {e}") - return {"success": False, "error": str(e)} - - @component_api - def clear_errors(self) -> dict[str, Any]: - """Clear any error states. - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - # Clear errors via SDK - success = self.sdk.clear_errors() - - if success: - # Update shared state - if self.shared_state: - self.shared_state.clear_errors() - - self.logger.info("Errors cleared successfully") - else: - self.logger.error("Failed to clear errors") - - return {"success": success} - - except Exception as e: - self.logger.error(f"Error in clear_errors: {e}") - return {"success": False, "error": str(e)} - - @component_api - def reset_fault(self) -> dict[str, Any]: - """Reset from fault state. - - This typically involves: - 1. Clearing errors - 2. Disabling servos - 3. Brief delay - 4. Re-enabling servos - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - self.logger.info("Resetting fault state...") - - # Step 1: Clear errors - if not self.sdk.clear_errors(): - return {"success": False, "error": "Failed to clear errors"} - - # Step 2: Disable servos if enabled - if self.sdk.are_servos_enabled(): - if not self.sdk.disable_servos(): - return {"success": False, "error": "Failed to disable servos"} - - # Step 3: Brief delay - time.sleep(0.5) - - # Step 4: Re-enable servos - if not self.sdk.enable_servos(): - return {"success": False, "error": "Failed to re-enable servos"} - - # Update shared state - if self.shared_state: - self.shared_state.update_robot_state( - state=0, # idle - error_code=0, - error_message="", - ) - self.shared_state.is_enabled = True - - self.logger.info("Fault reset successfully") - return {"success": True} - - except Exception as e: - self.logger.error(f"Error in reset_fault: {e}") - return {"success": False, "error": str(e)} - - @component_api - def home_robot(self, position: list[float] | None = None) -> dict[str, Any]: - """Move robot to home position. - - Args: - position: Optional home position in radians. - If None, uses zero position or configured home. - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - # Determine home position - if position is None: - # Use configured home or zero position - if self.capabilities: - position = [0.0] * self.capabilities.dof - else: - # Get current DOF from joint state - current = self.sdk.get_joint_positions() - position = [0.0] * len(current) - - # Enable servos if needed - if not self.sdk.are_servos_enabled(): - if not self.sdk.enable_servos(): - return {"success": False, "error": "Failed to enable servos"} - - # Move to home position - success = self.sdk.set_joint_positions( - position, - velocity=0.3, # Slower speed for homing - acceleration=0.3, - wait=True, # Wait for completion - ) - - if success: - if self.shared_state: - self.shared_state.is_homed = True - self.logger.info("Robot homed successfully") - - return {"success": success} - - except Exception as e: - self.logger.error(f"Error in home_robot: {e}") - return {"success": False, "error": str(e)} - - @component_api - def brake_release(self) -> dict[str, Any]: - """Release motor brakes (if applicable). - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - # This is typically the same as enabling servos - return self.enable_servo() - - except Exception as e: - self.logger.error(f"Error in brake_release: {e}") - return {"success": False, "error": str(e)} - - @component_api - def brake_engage(self) -> dict[str, Any]: - """Engage motor brakes (if applicable). - - Returns: - Dict with 'success' and optional 'error' keys - """ - try: - # This is typically the same as disabling servos - return self.disable_servo() - - except Exception as e: - self.logger.error(f"Error in brake_engage: {e}") - return {"success": False, "error": str(e)} diff --git a/dimos/hardware/manipulators_old/base/components/status.py b/dimos/hardware/manipulators_old/base/components/status.py deleted file mode 100644 index b20897ac65..0000000000 --- a/dimos/hardware/manipulators_old/base/components/status.py +++ /dev/null @@ -1,595 +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. - -"""Standard status monitoring component for manipulator drivers.""" - -from collections import deque -from dataclasses import dataclass -import logging -import time -from typing import Any - -from ..sdk_interface import BaseManipulatorSDK -from ..spec import ManipulatorCapabilities -from ..utils import SharedState -from . import component_api - - -@dataclass -class HealthMetrics: - """Health metrics for monitoring.""" - - update_rate: float = 0.0 # Hz - command_rate: float = 0.0 # Hz - error_rate: float = 0.0 # errors/minute - uptime: float = 0.0 # seconds - total_errors: int = 0 - total_commands: int = 0 - total_updates: int = 0 - - -class StandardStatusComponent: - """Status monitoring component that works with any SDK wrapper. - - This component provides standard status monitoring methods that work - consistently across all manipulator types. Methods decorated with @component_api - are automatically exposed as RPC methods on the driver. It handles: - - Robot state queries - - Error monitoring - - Health metrics - - System information - - Force/torque monitoring (if supported) - - Temperature monitoring (if supported) - """ - - def __init__( - self, - sdk: BaseManipulatorSDK | None = None, - shared_state: SharedState | None = None, - capabilities: ManipulatorCapabilities | None = None, - ): - """Initialize the status component. - - Args: - sdk: SDK wrapper instance (can be set later) - shared_state: Shared state instance (can be set later) - capabilities: Manipulator capabilities (can be set later) - """ - self.sdk = sdk - self.shared_state = shared_state - self.capabilities = capabilities - self.logger = logging.getLogger(self.__class__.__name__) - - # Health monitoring - self.start_time = time.time() - self.health_metrics = HealthMetrics() - - # Rate calculation - self.update_timestamps: deque[float] = deque(maxlen=100) - self.command_timestamps: deque[float] = deque(maxlen=100) - self.error_timestamps: deque[float] = deque(maxlen=100) - - # Error history - self.error_history: deque[dict[str, Any]] = deque(maxlen=50) - - # ============= Initialization Methods (called by BaseDriver) ============= - - def set_sdk(self, sdk: BaseManipulatorSDK) -> None: - """Set the SDK wrapper instance.""" - self.sdk = sdk - - def set_shared_state(self, shared_state: SharedState) -> None: - """Set the shared state instance.""" - self.shared_state = shared_state - - def set_capabilities(self, capabilities: ManipulatorCapabilities) -> None: - """Set the capabilities instance.""" - self.capabilities = capabilities - - def initialize(self) -> None: - """Initialize the component after all resources are set.""" - self.start_time = time.time() - self.logger.debug("Status component initialized") - - def publish_state(self) -> None: - """Called periodically to update metrics (by publisher thread).""" - current_time = time.time() - self.update_timestamps.append(current_time) - self._update_health_metrics() - - # ============= Component API Methods ============= - - @component_api - def get_robot_state(self) -> dict[str, Any]: - """Get comprehensive robot state. - - Returns: - Dict with complete state information - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - current_time = time.time() - - # Get state from SDK - robot_state = self.sdk.get_robot_state() - - # Get additional info - error_msg = ( - self.sdk.get_error_message() if robot_state.get("error_code", 0) != 0 else "" - ) - - # Map state integer to string - state_map = {0: "idle", 1: "moving", 2: "error", 3: "emergency_stop"} - state_str = state_map.get(robot_state.get("state", 0), "unknown") - - # Map mode integer to string - mode_map = {0: "position", 1: "velocity", 2: "torque", 3: "impedance"} - mode_str = mode_map.get(robot_state.get("mode", 0), "unknown") - - result = { - "state": state_str, - "state_code": robot_state.get("state", 0), - "mode": mode_str, - "mode_code": robot_state.get("mode", 0), - "error_code": robot_state.get("error_code", 0), - "error_message": error_msg, - "is_moving": robot_state.get("is_moving", False), - "is_connected": self.sdk.is_connected(), - "is_enabled": self.sdk.are_servos_enabled(), - "timestamp": current_time, - "success": True, - } - - # Add shared state info if available - if self.shared_state: - result["is_homed"] = self.shared_state.is_homed - result["last_update"] = self.shared_state.last_state_update - result["last_command"] = self.shared_state.last_command_sent - - return result - - except Exception as e: - self.logger.error(f"Error in get_robot_state: {e}") - return {"success": False, "error": str(e)} - - @component_api - def get_system_info(self) -> dict[str, Any]: - """Get system information. - - Returns: - Dict with system information - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - # Get manipulator info - info = self.sdk.get_info() - - result = { - "vendor": info.vendor, - "model": info.model, - "dof": info.dof, - "firmware_version": info.firmware_version, - "serial_number": info.serial_number, - "success": True, - } - - # Add capabilities if available - if self.capabilities: - result["capabilities"] = { - "dof": self.capabilities.dof, - "has_gripper": self.capabilities.has_gripper, - "has_force_torque": self.capabilities.has_force_torque, - "has_impedance_control": self.capabilities.has_impedance_control, - "has_cartesian_control": self.capabilities.has_cartesian_control, - "payload_mass": self.capabilities.payload_mass, - "reach": self.capabilities.reach, - } - - return result - - except Exception as e: - self.logger.error(f"Error in get_system_info: {e}") - return {"success": False, "error": str(e)} - - @component_api - def get_capabilities(self) -> dict[str, Any]: - """Get manipulator capabilities. - - Returns: - Dict with capability information - """ - try: - if not self.capabilities: - return {"success": False, "error": "Capabilities not available"} - - return { - "dof": self.capabilities.dof, - "has_gripper": self.capabilities.has_gripper, - "has_force_torque": self.capabilities.has_force_torque, - "has_impedance_control": self.capabilities.has_impedance_control, - "has_cartesian_control": self.capabilities.has_cartesian_control, - "joint_limits_lower": self.capabilities.joint_limits_lower, - "joint_limits_upper": self.capabilities.joint_limits_upper, - "max_joint_velocity": self.capabilities.max_joint_velocity, - "max_joint_acceleration": self.capabilities.max_joint_acceleration, - "payload_mass": self.capabilities.payload_mass, - "reach": self.capabilities.reach, - "success": True, - } - - except Exception as e: - self.logger.error(f"Error in get_capabilities: {e}") - return {"success": False, "error": str(e)} - - @component_api - def get_error_state(self) -> dict[str, Any]: - """Get detailed error state. - - Returns: - Dict with error information - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - error_code = self.sdk.get_error_code() - error_msg = self.sdk.get_error_message() - - result = { - "has_error": error_code != 0, - "error_code": error_code, - "error_message": error_msg, - "error_history": list(self.error_history), - "total_errors": self.health_metrics.total_errors, - "success": True, - } - - # Add last error time from shared state - if self.shared_state: - result["last_error_time"] = self.shared_state.last_error_time - - return result - - except Exception as e: - self.logger.error(f"Error in get_error_state: {e}") - return {"success": False, "error": str(e)} - - @component_api - def get_health_metrics(self) -> dict[str, Any]: - """Get health metrics. - - Returns: - Dict with health metrics - """ - try: - self._update_health_metrics() - - return { - "uptime": self.health_metrics.uptime, - "update_rate": self.health_metrics.update_rate, - "command_rate": self.health_metrics.command_rate, - "error_rate": self.health_metrics.error_rate, - "total_updates": self.health_metrics.total_updates, - "total_commands": self.health_metrics.total_commands, - "total_errors": self.health_metrics.total_errors, - "is_healthy": self._is_healthy(), - "timestamp": time.time(), - "success": True, - } - - except Exception as e: - self.logger.error(f"Error in get_health_metrics: {e}") - return {"success": False, "error": str(e)} - - @component_api - def get_statistics(self) -> dict[str, Any]: - """Get operation statistics. - - Returns: - Dict with statistics - """ - try: - stats = {} - - # Get stats from shared state - if self.shared_state: - stats.update(self.shared_state.get_statistics()) - - # Add component stats - stats["uptime"] = time.time() - self.start_time - stats["health_metrics"] = { - "update_rate": self.health_metrics.update_rate, - "command_rate": self.health_metrics.command_rate, - "error_rate": self.health_metrics.error_rate, - } - - stats["success"] = True - return stats - - except Exception as e: - self.logger.error(f"Error in get_statistics: {e}") - return {"success": False, "error": str(e)} - - @component_api - def check_connection(self) -> dict[str, Any]: - """Check connection status. - - Returns: - Dict with connection status - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - connected = self.sdk.is_connected() - - result: dict[str, Any] = { - "connected": connected, - "timestamp": time.time(), - "success": True, - } - - # Try to get more info if connected - if connected: - try: - # Try a simple query to verify connection - self.sdk.get_error_code() - result["verified"] = True - except: - result["verified"] = False - result["message"] = "Connected but cannot communicate" - - return result - - except Exception as e: - self.logger.error(f"Error in check_connection: {e}") - return {"success": False, "error": str(e)} - - # ============= Force/Torque Monitoring (Optional) ============= - - @component_api - def get_force_torque(self) -> dict[str, Any]: - """Get force/torque sensor data. - - Returns: - Dict with F/T data if available - """ - try: - # Check if F/T is supported - if not self.capabilities or not self.capabilities.has_force_torque: - return {"success": False, "error": "Force/torque sensor not available"} - - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - ft_data = self.sdk.get_force_torque() - - if ft_data: - return { - "force": ft_data[:3] if len(ft_data) >= 3 else None, # [fx, fy, fz] - "torque": ft_data[3:6] if len(ft_data) >= 6 else None, # [tx, ty, tz] - "data": ft_data, - "timestamp": time.time(), - "success": True, - } - else: - return {"success": False, "error": "Failed to read F/T sensor"} - - except Exception as e: - self.logger.error(f"Error in get_force_torque: {e}") - return {"success": False, "error": str(e)} - - @component_api - def zero_force_torque(self) -> dict[str, Any]: - """Zero the force/torque sensor. - - Returns: - Dict with success status - """ - try: - # Check if F/T is supported - if not self.capabilities or not self.capabilities.has_force_torque: - return {"success": False, "error": "Force/torque sensor not available"} - - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - success = self.sdk.zero_force_torque() - return {"success": success} - - except Exception as e: - self.logger.error(f"Error in zero_force_torque: {e}") - return {"success": False, "error": str(e)} - - # ============= I/O Monitoring (Optional) ============= - - @component_api - def get_digital_inputs(self) -> dict[str, Any]: - """Get digital input states. - - Returns: - Dict with digital input states - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - inputs = self.sdk.get_digital_inputs() - - if inputs is not None: - return {"inputs": inputs, "timestamp": time.time(), "success": True} - else: - return {"success": False, "error": "Digital inputs not available"} - - except Exception as e: - self.logger.error(f"Error in get_digital_inputs: {e}") - return {"success": False, "error": str(e)} - - @component_api - def set_digital_outputs(self, outputs: dict[str, bool]) -> dict[str, Any]: - """Set digital output states. - - Args: - outputs: Dict of output_id: bool - - Returns: - Dict with success status - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - success = self.sdk.set_digital_outputs(outputs) - return {"success": success} - - except Exception as e: - self.logger.error(f"Error in set_digital_outputs: {e}") - return {"success": False, "error": str(e)} - - @component_api - def get_analog_inputs(self) -> dict[str, Any]: - """Get analog input values. - - Returns: - Dict with analog input values - """ - try: - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - inputs = self.sdk.get_analog_inputs() - - if inputs is not None: - return {"inputs": inputs, "timestamp": time.time(), "success": True} - else: - return {"success": False, "error": "Analog inputs not available"} - - except Exception as e: - self.logger.error(f"Error in get_analog_inputs: {e}") - return {"success": False, "error": str(e)} - - # ============= Gripper Status (Optional) ============= - - @component_api - def get_gripper_state(self) -> dict[str, Any]: - """Get gripper state. - - Returns: - Dict with gripper state - """ - try: - # Check if gripper is supported - if not self.capabilities or not self.capabilities.has_gripper: - return {"success": False, "error": "Gripper not available"} - - if self.sdk is None: - return {"success": False, "error": "SDK not configured"} - - position = self.sdk.get_gripper_position() - - if position is not None: - result: dict[str, Any] = { - "position": position, # meters - "timestamp": time.time(), - "success": True, - } - - # Add from shared state if available - if self.shared_state and self.shared_state.gripper_force is not None: - result["force"] = self.shared_state.gripper_force - - return result - else: - return {"success": False, "error": "Failed to get gripper state"} - - except Exception as e: - self.logger.error(f"Error in get_gripper_state: {e}") - return {"success": False, "error": str(e)} - - # ============= Helper Methods ============= - - def _update_health_metrics(self) -> None: - """Update health metrics based on recent data.""" - current_time = time.time() - - # Update uptime - self.health_metrics.uptime = current_time - self.start_time - - # Calculate update rate - if len(self.update_timestamps) > 1: - time_span = self.update_timestamps[-1] - self.update_timestamps[0] - if time_span > 0: - self.health_metrics.update_rate = len(self.update_timestamps) / time_span - - # Calculate command rate - if len(self.command_timestamps) > 1: - time_span = self.command_timestamps[-1] - self.command_timestamps[0] - if time_span > 0: - self.health_metrics.command_rate = len(self.command_timestamps) / time_span - - # Calculate error rate (errors per minute) - recent_errors = [t for t in self.error_timestamps if current_time - t < 60] - self.health_metrics.error_rate = len(recent_errors) - - # Update totals from shared state - if self.shared_state: - stats = self.shared_state.get_statistics() - self.health_metrics.total_updates = stats.get("state_read_count", 0) - self.health_metrics.total_commands = stats.get("command_sent_count", 0) - self.health_metrics.total_errors = stats.get("error_count", 0) - - def _is_healthy(self) -> bool: - """Check if system is healthy based on metrics.""" - # Check update rate (should be > 10 Hz) - if self.health_metrics.update_rate < 10: - return False - - # Check error rate (should be < 10 per minute) - if self.health_metrics.error_rate > 10: - return False - - # Check SDK is configured - if self.sdk is None: - return False - - # Check connection - if not self.sdk.is_connected(): - return False - - # Check for persistent errors - if self.sdk.get_error_code() != 0: - return False - - return True - - def record_error(self, error_code: int, error_msg: str) -> None: - """Record an error occurrence. - - Args: - error_code: Error code - error_msg: Error message - """ - current_time = time.time() - self.error_timestamps.append(current_time) - self.error_history.append( - {"code": error_code, "message": error_msg, "timestamp": current_time} - ) - - def record_command(self) -> None: - """Record a command occurrence.""" - self.command_timestamps.append(time.time()) diff --git a/dimos/hardware/manipulators_old/base/driver.py b/dimos/hardware/manipulators_old/base/driver.py deleted file mode 100644 index be68be5a23..0000000000 --- a/dimos/hardware/manipulators_old/base/driver.py +++ /dev/null @@ -1,637 +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. - -"""Base manipulator driver with threading and component management.""" - -from dataclasses import dataclass -import logging -from queue import Empty, Queue -from threading import Event, Thread -import time -from typing import Any - -from dimos.core import In, Module, Out, rpc -from dimos.msgs.geometry_msgs import WrenchStamped -from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState - -from .sdk_interface import BaseManipulatorSDK -from .spec import ManipulatorCapabilities -from .utils import SharedState - - -@dataclass -class Command: - """Command to be sent to the manipulator.""" - - type: str # 'position', 'velocity', 'effort', 'cartesian', etc. - data: Any - timestamp: float = 0.0 - - -class BaseManipulatorDriver(Module): - """Base driver providing threading and component management. - - This class handles: - - Thread management (state reader, command sender, state publisher) - - Component registration and lifecycle - - RPC method registration - - Shared state management - - Error handling and recovery - - Pub/Sub with LCM transport for real-time control - """ - - # Input topics (commands from controllers - initialized by Module) - joint_position_command: In[JointCommand] = None # type: ignore[assignment] - joint_velocity_command: In[JointCommand] = None # type: ignore[assignment] - - # Output topics (state publishing - initialized by Module) - joint_state: Out[JointState] = None # type: ignore[assignment] - robot_state: Out[RobotState] = None # type: ignore[assignment] - ft_sensor: Out[WrenchStamped] = None # type: ignore[assignment] - - def __init__( - self, - sdk: BaseManipulatorSDK, - components: list[Any], - config: dict[str, Any], - name: str | None = None, - *args: Any, - **kwargs: Any, - ) -> None: - """Initialize the base manipulator driver. - - Args: - sdk: SDK wrapper instance - components: List of component instances - config: Configuration dictionary - name: Optional driver name for logging - *args, **kwargs: Additional arguments for Module - """ - # Initialize Module parent class - super().__init__(*args, **kwargs) - - self.sdk = sdk - self.components = components - self.config: Any = config # Config dict accessed as object - self.name = name or self.__class__.__name__ - - # Logging - self.logger = logging.getLogger(self.name) - - # Shared state - self.shared_state = SharedState() - - # Threading - self.stop_event = Event() - self.threads: list[Thread] = [] - self.command_queue: Queue[Any] = Queue(maxsize=10) - - # RPC registry - self.rpc_methods: dict[str, Any] = {} - self._exposed_component_apis: set[str] = set() # Track auto-exposed method names - - # Capabilities - self.capabilities = self._get_capabilities() - - # Rate control - self.control_rate = config.get("control_rate", 100) # Hz - control loop + joint feedback - self.monitor_rate = config.get("monitor_rate", 10) # Hz - robot state monitoring - - # Pre-allocate reusable objects (optimization: avoid per-cycle allocation) - # Note: _joint_names is populated after _get_capabilities() sets self.capabilities - self._joint_names: list[str] = [f"joint{i + 1}" for i in range(self.capabilities.dof)] - - # Initialize components with shared resources - self._initialize_components() - - # Auto-expose component API methods as RPCs on the driver - self._auto_expose_component_apis() - - # Connect to hardware - self._connect() - - def _get_capabilities(self) -> ManipulatorCapabilities: - """Get manipulator capabilities from config or SDK. - - Returns: - ManipulatorCapabilities instance - """ - # Try to get from SDK info - info = self.sdk.get_info() - - # Get joint limits - lower_limits, upper_limits = self.sdk.get_joint_limits() - velocity_limits = self.sdk.get_velocity_limits() - acceleration_limits = self.sdk.get_acceleration_limits() - - return ManipulatorCapabilities( - dof=info.dof, - has_gripper=self.config.get("has_gripper", False), - has_force_torque=self.config.get("has_force_torque", False), - has_impedance_control=self.config.get("has_impedance_control", False), - has_cartesian_control=self.config.get("has_cartesian_control", False), - max_joint_velocity=velocity_limits, - max_joint_acceleration=acceleration_limits, - joint_limits_lower=lower_limits, - joint_limits_upper=upper_limits, - payload_mass=self.config.get("payload_mass", 0.0), - reach=self.config.get("reach", 0.0), - ) - - def _initialize_components(self) -> None: - """Initialize components with shared resources.""" - for component in self.components: - # Provide access to shared state - if hasattr(component, "set_shared_state"): - component.set_shared_state(self.shared_state) - - # Provide access to SDK - if hasattr(component, "set_sdk"): - component.set_sdk(self.sdk) - - # Provide access to command queue - if hasattr(component, "set_command_queue"): - component.set_command_queue(self.command_queue) - - # Provide access to capabilities - if hasattr(component, "set_capabilities"): - component.set_capabilities(self.capabilities) - - # Initialize component - if hasattr(component, "initialize"): - component.initialize() - - def _auto_expose_component_apis(self) -> None: - """Auto-expose @component_api methods from components as RPC methods on the driver. - - This scans all components for methods decorated with @component_api and creates - corresponding @rpc wrapper methods on the driver instance. This allows external - code to call these methods via the standard Module RPC system. - - Example: - # Component defines: - @component_api - def enable_servo(self): ... - - # Driver auto-generates an RPC wrapper, so external code can call: - driver.enable_servo() - - # And the method is discoverable via: - driver.rpcs # Lists 'enable_servo' among available RPCs - """ - for component in self.components: - for method_name in dir(component): - if method_name.startswith("_"): - continue - - method = getattr(component, method_name, None) - if not callable(method) or not getattr(method, "__component_api__", False): - continue - - # Skip if driver already has a non-wrapper method with this name - existing = getattr(self, method_name, None) - if existing is not None and not getattr( - existing, "__component_api_wrapper__", False - ): - self.logger.warning( - f"Driver already has method '{method_name}', skipping component API" - ) - continue - - # Create RPC wrapper - use factory to properly capture method reference - wrapper = self._create_component_api_wrapper(method) - - # Attach to driver instance - setattr(self, method_name, wrapper) - - # Store in rpc_methods dict for backward compatibility - self.rpc_methods[method_name] = wrapper - - # Track exposed method name for cleanup - self._exposed_component_apis.add(method_name) - - self.logger.debug(f"Exposed component API as RPC: {method_name}") - - def _create_component_api_wrapper(self, component_method: Any) -> Any: - """Create an RPC wrapper for a component API method. - - Args: - component_method: The component method to wrap - - Returns: - RPC-decorated wrapper function - """ - import functools - - @rpc - @functools.wraps(component_method) - def wrapper(*args: Any, **kwargs: Any) -> Any: - return component_method(*args, **kwargs) - - wrapper.__component_api_wrapper__ = True # type: ignore[attr-defined] - return wrapper - - def _connect(self) -> None: - """Connect to the manipulator hardware.""" - self.logger.info(f"Connecting to {self.name}...") - - # Connect via SDK - if not self.sdk.connect(self.config): - raise RuntimeError(f"Failed to connect to {self.name}") - - self.shared_state.is_connected = True - self.logger.info(f"Successfully connected to {self.name}") - - # Get initial state - self._update_joint_state() - self._update_robot_state() - - def _update_joint_state(self) -> None: - """Update joint state from hardware (high frequency - 100Hz). - - Reads joint positions, velocities, efforts and publishes to LCM immediately. - """ - try: - # Get joint state feedback - positions = self.sdk.get_joint_positions() - velocities = self.sdk.get_joint_velocities() - efforts = self.sdk.get_joint_efforts() - - self.shared_state.update_joint_state( - positions=positions, velocities=velocities, efforts=efforts - ) - - # Publish joint state immediately at control rate - if self.joint_state and hasattr(self.joint_state, "publish"): - joint_state_msg = JointState( - ts=time.time(), - frame_id="joint-state", - name=self._joint_names, # Pre-allocated list (optimization) - position=positions or [0.0] * self.capabilities.dof, - velocity=velocities or [0.0] * self.capabilities.dof, - effort=efforts or [0.0] * self.capabilities.dof, - ) - self.joint_state.publish(joint_state_msg) - - except Exception as e: - self.logger.error(f"Error updating joint state: {e}") - - def _update_robot_state(self) -> None: - """Update robot state from hardware (low frequency - 10Hz). - - Reads robot mode, errors, warnings, optional states and publishes to LCM immediately. - """ - try: - # Get robot state (mode, errors, warnings) - robot_state = self.sdk.get_robot_state() - self.shared_state.update_robot_state( - state=robot_state.get("state", 0), - mode=robot_state.get("mode", 0), - error_code=robot_state.get("error_code", 0), - error_message=self.sdk.get_error_message(), - ) - - # Update status flags - self.shared_state.is_moving = robot_state.get("is_moving", False) - self.shared_state.is_enabled = self.sdk.are_servos_enabled() - - # Get optional states (cartesian, force/torque, gripper) - if self.capabilities.has_cartesian_control: - cart_pos = self.sdk.get_cartesian_position() - if cart_pos: - self.shared_state.cartesian_position = cart_pos - - if self.capabilities.has_force_torque: - ft = self.sdk.get_force_torque() - if ft: - self.shared_state.force_torque = ft - - if self.capabilities.has_gripper: - gripper_pos = self.sdk.get_gripper_position() - if gripper_pos is not None: - self.shared_state.gripper_position = gripper_pos - - # Publish robot state immediately at monitor rate - if self.robot_state and hasattr(self.robot_state, "publish"): - robot_state_msg = RobotState( - state=self.shared_state.robot_state, - mode=self.shared_state.control_mode, - error_code=self.shared_state.error_code, - warn_code=0, - ) - self.robot_state.publish(robot_state_msg) - - # Publish force/torque if available - if ( - self.ft_sensor - and hasattr(self.ft_sensor, "publish") - and self.capabilities.has_force_torque - ): - if self.shared_state.force_torque: - ft_msg = WrenchStamped.from_force_torque_array( - ft_data=self.shared_state.force_torque, - frame_id="ft_sensor", - ts=time.time(), - ) - self.ft_sensor.publish(ft_msg) - - except Exception as e: - self.logger.error(f"Error updating robot state: {e}") - self.shared_state.update_robot_state(error_code=999, error_message=str(e)) - - # ============= Threading ============= - - @rpc - def start(self) -> None: - """Start all driver threads and subscribe to input topics.""" - super().start() - self.logger.info(f"Starting {self.name} driver threads...") - - # Subscribe to input topics if they have transports - try: - if self.joint_position_command and hasattr(self.joint_position_command, "subscribe"): - self.joint_position_command.subscribe(self._on_joint_position_command) - self.logger.debug("Subscribed to joint_position_command") - except (AttributeError, ValueError) as e: - self.logger.debug(f"joint_position_command transport not configured: {e}") - - try: - if self.joint_velocity_command and hasattr(self.joint_velocity_command, "subscribe"): - self.joint_velocity_command.subscribe(self._on_joint_velocity_command) - self.logger.debug("Subscribed to joint_velocity_command") - except (AttributeError, ValueError) as e: - self.logger.debug(f"joint_velocity_command transport not configured: {e}") - - self.threads = [ - Thread(target=self._control_loop_thread, name=f"{self.name}-ControlLoop", daemon=True), - Thread( - target=self._robot_state_monitor_thread, - name=f"{self.name}-StateMonitor", - daemon=True, - ), - ] - - for thread in self.threads: - thread.start() - self.logger.debug(f"Started thread: {thread.name}") - - self.logger.info(f"{self.name} driver started successfully") - - def _control_loop_thread(self) -> None: - """Control loop: send commands AND read joint feedback (100Hz). - - This tight loop ensures synchronized command/feedback for real-time control. - """ - self.logger.debug("Control loop thread started") - period = 1.0 / self.control_rate - next_time = time.perf_counter() + period # perf_counter for precise timing - - while not self.stop_event.is_set(): - try: - # 1. Process all pending commands (non-blocking) - while True: - try: - command = self.command_queue.get_nowait() # Non-blocking (optimization) - self._process_command(command) - except Empty: - break # No more commands - - # 2. Read joint state feedback (critical for control) - self._update_joint_state() - - except Exception as e: - self.logger.error(f"Control loop error: {e}") - - # Rate control - maintain precise timing - next_time += period - sleep_time = next_time - time.perf_counter() - if sleep_time > 0: - time.sleep(sleep_time) - else: - # Fell behind - reset timing - next_time = time.perf_counter() + period - if sleep_time < -period: - self.logger.warning(f"Control loop fell behind by {-sleep_time:.3f}s") - - self.logger.debug("Control loop thread stopped") - - def _robot_state_monitor_thread(self) -> None: - """Monitor robot state: mode, errors, warnings (10-20Hz). - - Lower frequency monitoring for high-level planning and error handling. - """ - self.logger.debug("Robot state monitor thread started") - period = 1.0 / self.monitor_rate - next_time = time.perf_counter() + period # perf_counter for precise timing - - while not self.stop_event.is_set(): - try: - # Read robot state, mode, errors, optional states - self._update_robot_state() - except Exception as e: - self.logger.error(f"Robot state monitor error: {e}") - - # Rate control - next_time += period - sleep_time = next_time - time.perf_counter() - if sleep_time > 0: - time.sleep(sleep_time) - else: - next_time = time.perf_counter() + period - - self.logger.debug("Robot state monitor thread stopped") - - def _process_command(self, command: Command) -> None: - """Process a command from the queue. - - Args: - command: Command to process - """ - try: - if command.type == "position": - success = self.sdk.set_joint_positions( - command.data["positions"], - command.data.get("velocity", 1.0), - command.data.get("acceleration", 1.0), - command.data.get("wait", False), - ) - if success: - self.shared_state.target_positions = command.data["positions"] - - elif command.type == "velocity": - success = self.sdk.set_joint_velocities(command.data["velocities"]) - if success: - self.shared_state.target_velocities = command.data["velocities"] - - elif command.type == "effort": - success = self.sdk.set_joint_efforts(command.data["efforts"]) - if success: - self.shared_state.target_efforts = command.data["efforts"] - - elif command.type == "cartesian": - success = self.sdk.set_cartesian_position( - command.data["pose"], - command.data.get("velocity", 1.0), - command.data.get("acceleration", 1.0), - command.data.get("wait", False), - ) - if success: - self.shared_state.target_cartesian_position = command.data["pose"] - - elif command.type == "stop": - self.sdk.stop_motion() - - else: - self.logger.warning(f"Unknown command type: {command.type}") - - except Exception as e: - self.logger.error(f"Error processing command {command.type}: {e}") - - # ============= Input Callbacks ============= - - def _on_joint_position_command(self, cmd_msg: JointCommand) -> None: - """Callback when joint position command is received. - - Args: - cmd_msg: JointCommand message containing positions - """ - command = Command( - type="position", data={"positions": list(cmd_msg.positions)}, timestamp=time.time() - ) - try: - self.command_queue.put_nowait(command) - except: - self.logger.warning("Command queue full, dropping position command") - - def _on_joint_velocity_command(self, cmd_msg: JointCommand) -> None: - """Callback when joint velocity command is received. - - Args: - cmd_msg: JointCommand message containing velocities - """ - command = Command( - type="velocity", - data={"velocities": list(cmd_msg.positions)}, # JointCommand uses 'positions' field - timestamp=time.time(), - ) - try: - self.command_queue.put_nowait(command) - except: - self.logger.warning("Command queue full, dropping velocity command") - - # ============= Lifecycle Management ============= - - @rpc - def stop(self) -> None: - """Stop all threads and disconnect from hardware.""" - self.logger.info(f"Stopping {self.name} driver...") - - # Signal threads to stop - self.stop_event.set() - - # Stop any ongoing motion - try: - self.sdk.stop_motion() - except: - pass - - # Wait for threads to stop - for thread in self.threads: - thread.join(timeout=2.0) - if thread.is_alive(): - self.logger.warning(f"Thread {thread.name} did not stop cleanly") - - # Disconnect from hardware - try: - self.sdk.disconnect() - except: - pass - - self.shared_state.is_connected = False - self.logger.info(f"{self.name} driver stopped") - - # Call Module's stop - super().stop() - - def __del__(self) -> None: - """Cleanup on deletion.""" - if self.shared_state.is_connected: - self.stop() - - # ============= RPC Method Access ============= - - def get_rpc_method(self, method_name: str) -> Any: - """Get an RPC method by name. - - Args: - method_name: Name of the RPC method - - Returns: - The method if found, None otherwise - """ - return self.rpc_methods.get(method_name) - - def list_rpc_methods(self) -> list[str]: - """List all available RPC methods. - - Returns: - List of RPC method names - """ - return list(self.rpc_methods.keys()) - - # ============= Component Access ============= - - def get_component(self, component_type: type[Any]) -> Any: - """Get a component by type. - - Args: - component_type: Type of component to find - - Returns: - The component if found, None otherwise - """ - for component in self.components: - if isinstance(component, component_type): - return component - return None - - def add_component(self, component: Any) -> None: - """Add a component at runtime. - - Args: - component: Component instance to add - """ - self.components.append(component) - self._initialize_components() - self._auto_expose_component_apis() - - def remove_component(self, component: Any) -> None: - """Remove a component at runtime. - - Args: - component: Component instance to remove - """ - if component in self.components: - self.components.remove(component) - # Clean up old exposed methods and re-expose for remaining components - self._cleanup_exposed_component_apis() - self._auto_expose_component_apis() - - def _cleanup_exposed_component_apis(self) -> None: - """Remove all auto-exposed component API methods from the driver.""" - for method_name in self._exposed_component_apis: - if hasattr(self, method_name): - delattr(self, method_name) - self._exposed_component_apis.clear() - self.rpc_methods.clear() diff --git a/dimos/hardware/manipulators_old/base/sdk_interface.py b/dimos/hardware/manipulators_old/base/sdk_interface.py deleted file mode 100644 index f20d35bd50..0000000000 --- a/dimos/hardware/manipulators_old/base/sdk_interface.py +++ /dev/null @@ -1,471 +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. - -"""Base SDK interface that all manipulator SDK wrappers must implement. - -This interface defines the standard methods and units that all SDK wrappers -must provide, ensuring consistent behavior across different manipulator types. - -Standard Units: -- Angles: radians -- Angular velocity: rad/s -- Linear position: meters -- Linear velocity: m/s -- Force: Newtons -- Torque: Nm -- Time: seconds -""" - -from abc import ABC, abstractmethod -from dataclasses import dataclass -from typing import Any - - -@dataclass -class ManipulatorInfo: - """Information about the manipulator.""" - - vendor: str - model: str - dof: int - firmware_version: str | None = None - serial_number: str | None = None - - -class BaseManipulatorSDK(ABC): - """Abstract base class for manipulator SDK wrappers. - - All SDK wrappers must implement this interface to ensure compatibility - with the standard components. Methods should handle unit conversions - internally to always work with standard units. - """ - - # ============= Connection Management ============= - - @abstractmethod - def connect(self, config: dict[str, Any]) -> bool: - """Establish connection to the manipulator. - - Args: - config: Configuration dict with connection parameters - (e.g., ip, port, can_interface, etc.) - - Returns: - True if connection successful, False otherwise - """ - pass - - @abstractmethod - def disconnect(self) -> None: - """Disconnect from the manipulator. - - Should cleanly close all connections and free resources. - """ - pass - - @abstractmethod - def is_connected(self) -> bool: - """Check if currently connected to the manipulator. - - Returns: - True if connected, False otherwise - """ - pass - - # ============= Joint State Query ============= - - @abstractmethod - def get_joint_positions(self) -> list[float]: - """Get current joint positions. - - Returns: - Joint positions in RADIANS - """ - pass - - @abstractmethod - def get_joint_velocities(self) -> list[float]: - """Get current joint velocities. - - Returns: - Joint velocities in RAD/S - """ - pass - - @abstractmethod - def get_joint_efforts(self) -> list[float]: - """Get current joint efforts/torques. - - Returns: - Joint efforts in Nm (torque) or N (force) - """ - pass - - # ============= Joint Motion Control ============= - - @abstractmethod - def set_joint_positions( - self, - positions: list[float], - velocity: float = 1.0, - acceleration: float = 1.0, - wait: bool = False, - ) -> bool: - """Move joints to target positions. - - Args: - positions: Target positions in RADIANS - velocity: Max velocity as fraction of maximum (0-1) - acceleration: Max acceleration as fraction of maximum (0-1) - wait: If True, block until motion completes - - Returns: - True if command accepted, False otherwise - """ - pass - - @abstractmethod - def set_joint_velocities(self, velocities: list[float]) -> bool: - """Set joint velocity targets. - - Args: - velocities: Target velocities in RAD/S - - Returns: - True if command accepted, False otherwise - """ - pass - - @abstractmethod - def set_joint_efforts(self, efforts: list[float]) -> bool: - """Set joint effort/torque targets. - - Args: - efforts: Target efforts in Nm (torque) or N (force) - - Returns: - True if command accepted, False otherwise - """ - pass - - @abstractmethod - def stop_motion(self) -> bool: - """Stop all ongoing motion immediately. - - Returns: - True if stop successful, False otherwise - """ - pass - - # ============= Servo Control ============= - - @abstractmethod - def enable_servos(self) -> bool: - """Enable motor control (servos/brakes released). - - Returns: - True if servos enabled, False otherwise - """ - pass - - @abstractmethod - def disable_servos(self) -> bool: - """Disable motor control (servos/brakes engaged). - - Returns: - True if servos disabled, False otherwise - """ - pass - - @abstractmethod - def are_servos_enabled(self) -> bool: - """Check if servos are currently enabled. - - Returns: - True if enabled, False if disabled - """ - pass - - # ============= System State ============= - - @abstractmethod - def get_robot_state(self) -> dict[str, Any]: - """Get current robot state information. - - Returns: - Dict with at least these keys: - - 'state': int (0=idle, 1=moving, 2=error, 3=e-stop) - - 'mode': int (0=position, 1=velocity, 2=torque) - - 'error_code': int (0 = no error) - - 'is_moving': bool - """ - pass - - @abstractmethod - def get_error_code(self) -> int: - """Get current error code. - - Returns: - Error code (0 = no error) - """ - pass - - @abstractmethod - def get_error_message(self) -> str: - """Get human-readable error message. - - Returns: - Error message string (empty if no error) - """ - pass - - @abstractmethod - def clear_errors(self) -> bool: - """Clear any error states. - - Returns: - True if errors cleared, False otherwise - """ - pass - - @abstractmethod - def emergency_stop(self) -> bool: - """Execute emergency stop. - - Returns: - True if e-stop executed, False otherwise - """ - pass - - # ============= Information ============= - - @abstractmethod - def get_info(self) -> ManipulatorInfo: - """Get manipulator information. - - Returns: - ManipulatorInfo object with vendor, model, DOF, etc. - """ - pass - - @abstractmethod - def get_joint_limits(self) -> tuple[list[float], list[float]]: - """Get joint position limits. - - Returns: - Tuple of (lower_limits, upper_limits) in RADIANS - """ - pass - - @abstractmethod - def get_velocity_limits(self) -> list[float]: - """Get joint velocity limits. - - Returns: - Maximum velocities in RAD/S - """ - pass - - @abstractmethod - def get_acceleration_limits(self) -> list[float]: - """Get joint acceleration limits. - - Returns: - Maximum accelerations in RAD/S² - """ - pass - - # ============= Optional Methods (Override if Supported) ============= - # These have default implementations that indicate feature not available - - def get_cartesian_position(self) -> dict[str, float] | None: - """Get current end-effector pose. - - Returns: - Dict with keys: x, y, z (meters), roll, pitch, yaw (radians) - None if not supported - """ - return None - - def set_cartesian_position( - self, - pose: dict[str, float], - velocity: float = 1.0, - acceleration: float = 1.0, - wait: bool = False, - ) -> bool: - """Move end-effector to target pose. - - Args: - pose: Target pose with keys: x, y, z (meters), roll, pitch, yaw (radians) - velocity: Max velocity as fraction (0-1) - acceleration: Max acceleration as fraction (0-1) - wait: If True, block until motion completes - - Returns: - False (not supported by default) - """ - return False - - def get_cartesian_velocity(self) -> dict[str, float] | None: - """Get current end-effector velocity. - - Returns: - Dict with keys: vx, vy, vz (m/s), wx, wy, wz (rad/s) - None if not supported - """ - return None - - def set_cartesian_velocity(self, twist: dict[str, float]) -> bool: - """Set end-effector velocity. - - Args: - twist: Velocity with keys: vx, vy, vz (m/s), wx, wy, wz (rad/s) - - Returns: - False (not supported by default) - """ - return False - - def get_force_torque(self) -> list[float] | None: - """Get force/torque sensor reading. - - Returns: - List of [fx, fy, fz (N), tx, ty, tz (Nm)] - None if not supported - """ - return None - - def zero_force_torque(self) -> bool: - """Zero the force/torque sensor. - - Returns: - False (not supported by default) - """ - return False - - def set_impedance_parameters(self, stiffness: list[float], damping: list[float]) -> bool: - """Set impedance control parameters. - - Args: - stiffness: Stiffness values [x, y, z, rx, ry, rz] - damping: Damping values [x, y, z, rx, ry, rz] - - Returns: - False (not supported by default) - """ - return False - - def get_digital_inputs(self) -> dict[str, bool] | None: - """Get digital input states. - - Returns: - Dict of input_id: bool - None if not supported - """ - return None - - def set_digital_outputs(self, outputs: dict[str, bool]) -> bool: - """Set digital output states. - - Args: - outputs: Dict of output_id: bool - - Returns: - False (not supported by default) - """ - return False - - def get_analog_inputs(self) -> dict[str, float] | None: - """Get analog input values. - - Returns: - Dict of input_id: float - None if not supported - """ - return None - - def set_analog_outputs(self, outputs: dict[str, float]) -> bool: - """Set analog output values. - - Args: - outputs: Dict of output_id: float - - Returns: - False (not supported by default) - """ - return False - - def execute_trajectory(self, trajectory: list[dict[str, Any]], wait: bool = True) -> bool: - """Execute a joint trajectory. - - Args: - trajectory: List of waypoints, each with: - - 'positions': list[float] in radians - - 'velocities': Optional list[float] in rad/s - - 'time': float seconds from start - wait: If True, block until trajectory completes - - Returns: - False (not supported by default) - """ - return False - - def stop_trajectory(self) -> bool: - """Stop any executing trajectory. - - Returns: - False (not supported by default) - """ - return False - - def get_gripper_position(self) -> float | None: - """Get gripper position. - - Returns: - Position in meters (0=closed, max=fully open) - None if no gripper - """ - return None - - def set_gripper_position(self, position: float, force: float = 1.0) -> bool: - """Set gripper position. - - Args: - position: Target position in meters - force: Gripping force as fraction (0-1) - - Returns: - False (not supported by default) - """ - return False - - def set_control_mode(self, mode: str) -> bool: - """Set control mode. - - Args: - mode: One of 'position', 'velocity', 'torque', 'impedance' - - Returns: - False (not supported by default) - """ - return False - - def get_control_mode(self) -> str | None: - """Get current control mode. - - Returns: - Current mode string or None if not supported - """ - return None diff --git a/dimos/hardware/manipulators_old/base/spec.py b/dimos/hardware/manipulators_old/base/spec.py deleted file mode 100644 index 8a0722cf09..0000000000 --- a/dimos/hardware/manipulators_old/base/spec.py +++ /dev/null @@ -1,195 +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. - -from dataclasses import dataclass -from typing import Any, Protocol - -from dimos.core import In, Out -from dimos.msgs.geometry_msgs import WrenchStamped -from dimos.msgs.sensor_msgs import JointCommand, JointState - - -@dataclass -class RobotState: - """Universal robot state compatible with all manipulators.""" - - # Core state fields (all manipulators must provide these) - state: int = 0 # 0: idle, 1: moving, 2: error, 3: e-stop - mode: int = 0 # 0: position, 1: velocity, 2: torque, 3: impedance - error_code: int = 0 # Standardized error codes across all arms - warn_code: int = 0 # Standardized warning codes - - # Extended state (optional, arm-specific) - is_connected: bool = False - is_enabled: bool = False - is_moving: bool = False - is_collision: bool = False - - # Vendor-specific data (if needed) - vendor_data: dict[str, Any] | None = None - - -@dataclass -class ManipulatorCapabilities: - """Describes what a manipulator can do.""" - - dof: int # Degrees of freedom - has_gripper: bool = False - has_force_torque: bool = False - has_impedance_control: bool = False - has_cartesian_control: bool = False - max_joint_velocity: list[float] | None = None # rad/s - max_joint_acceleration: list[float] | None = None # rad/s² - joint_limits_lower: list[float] | None = None # rad - joint_limits_upper: list[float] | None = None # rad - payload_mass: float = 0.0 # kg - reach: float = 0.0 # meters - - -class ManipulatorDriverSpec(Protocol): - """Universal protocol specification for ALL manipulator drivers. - - This defines the standard interface that every manipulator driver - must implement, regardless of the underlying hardware (XArm, Piper, - UR, Franka, etc.). - - ## Component-Based Architecture - - Drivers use a **component-based architecture** where functionality is provided - by composable components: - - - **StandardMotionComponent**: Joint/cartesian motion, trajectory execution - - **StandardServoComponent**: Servo control, modes, emergency stop, error handling - - **StandardStatusComponent**: State monitoring, capabilities, diagnostics - - RPC methods are provided by components and registered with the driver. - Access them via: - - ```python - # Method 1: Via component (direct access) - motion = driver.get_component(StandardMotionComponent) - motion.rpc_move_joint(positions=[0, 0, 0, 0, 0, 0]) - - # Method 2: Via driver's RPC registry - move_fn = driver.get_rpc_method('rpc_move_joint') - move_fn(positions=[0, 0, 0, 0, 0, 0]) - - # Method 3: Via blueprints (recommended - automatic routing) - # Commands sent to input topics are automatically routed to components - driver.joint_position_command.publish(JointCommand(positions=[0, 0, 0, 0, 0, 0])) - ``` - - ## Required Components - - Every driver must include these standard components: - - `StandardMotionComponent` - Provides motion control RPC methods - - `StandardServoComponent` - Provides servo control RPC methods - - `StandardStatusComponent` - Provides status monitoring RPC methods - - ## Available RPC Methods (via Components) - - ### Motion Control (StandardMotionComponent) - - `rpc_move_joint()` - Move to joint positions - - `rpc_move_joint_velocity()` - Set joint velocities - - `rpc_move_joint_effort()` - Set joint efforts (optional) - - `rpc_stop_motion()` - Stop all motion - - `rpc_get_joint_state()` - Get current joint state - - `rpc_get_joint_limits()` - Get joint limits - - `rpc_move_cartesian()` - Cartesian motion (optional) - - `rpc_execute_trajectory()` - Execute trajectory (optional) - - ### Servo Control (StandardServoComponent) - - `rpc_enable_servo()` - Enable motor control - - `rpc_disable_servo()` - Disable motor control - - `rpc_set_control_mode()` - Set control mode - - `rpc_emergency_stop()` - Execute emergency stop - - `rpc_clear_errors()` - Clear error states - - `rpc_home_robot()` - Home the robot - - ### Status Monitoring (StandardStatusComponent) - - `rpc_get_robot_state()` - Get robot state - - `rpc_get_capabilities()` - Get capabilities - - `rpc_get_system_info()` - Get system information - - `rpc_check_connection()` - Check connection status - - ## Standardized Units - - All units are standardized: - - Angles: radians - - Angular velocity: rad/s - - Linear position: meters - - Linear velocity: m/s - - Force: Newtons - - Torque: Nm - - Time: seconds - """ - - # ============= Capabilities Declaration ============= - capabilities: ManipulatorCapabilities - - # ============= Input Topics (Commands) ============= - # Core control inputs (all manipulators must support these) - joint_position_command: In[JointCommand] # Target joint positions (rad) - joint_velocity_command: In[JointCommand] # Target joint velocities (rad/s) - - # ============= Output Topics (Feedback) ============= - # Core feedback (all manipulators must provide these) - joint_state: Out[JointState] # Current positions, velocities, efforts - robot_state: Out[RobotState] # System state and health - - # Optional feedback (capability-dependent) - ft_sensor: Out[WrenchStamped] | None # Force/torque sensor data - - # ============= Component Access ============= - def get_component(self, component_type: type) -> Any: - """Get a component by type. - - Args: - component_type: Type of component to retrieve - - Returns: - Component instance if found, None otherwise - - Example: - motion = driver.get_component(StandardMotionComponent) - motion.rpc_move_joint([0, 0, 0, 0, 0, 0]) - """ - pass - - def get_rpc_method(self, method_name: str) -> Any: - """Get an RPC method by name. - - Args: - method_name: Name of the RPC method (e.g., 'rpc_move_joint') - - Returns: - Callable method if found, None otherwise - - Example: - move_fn = driver.get_rpc_method('rpc_move_joint') - result = move_fn(positions=[0, 0, 0, 0, 0, 0]) - """ - ... - - def list_rpc_methods(self) -> list[str]: - """List all available RPC methods from all components. - - Returns: - List of RPC method names - - Example: - methods = driver.list_rpc_methods() - # ['rpc_move_joint', 'rpc_enable_servo', 'rpc_get_robot_state', ...] - """ - ... diff --git a/dimos/hardware/manipulators_old/base/tests/__init__.py b/dimos/hardware/manipulators_old/base/tests/__init__.py deleted file mode 100644 index f863fa5120..0000000000 --- a/dimos/hardware/manipulators_old/base/tests/__init__.py +++ /dev/null @@ -1,15 +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. - -"""Tests for manipulator base module.""" diff --git a/dimos/hardware/manipulators_old/base/tests/conftest.py b/dimos/hardware/manipulators_old/base/tests/conftest.py deleted file mode 100644 index d3e6a4c66d..0000000000 --- a/dimos/hardware/manipulators_old/base/tests/conftest.py +++ /dev/null @@ -1,362 +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. - -"""Pytest fixtures and mocks for manipulator driver tests. - -This module contains MockSDK which implements BaseManipulatorSDK with controllable -behavior for testing driver logic without requiring hardware. - -Features: -- Configurable initial state (positions, DOF, vendor, model) -- Call tracking for verification -- Configurable error injection -- Simulated behavior (e.g., position updates) -""" - -from dataclasses import dataclass, field -import math - -import pytest - -from ..sdk_interface import BaseManipulatorSDK, ManipulatorInfo - - -@dataclass -class MockSDKConfig: - """Configuration for MockSDK behavior.""" - - dof: int = 6 - vendor: str = "Mock" - model: str = "TestArm" - initial_positions: list[float] | None = None - initial_velocities: list[float] | None = None - initial_efforts: list[float] | None = None - - # Error injection - connect_fails: bool = False - enable_fails: bool = False - motion_fails: bool = False - error_code: int = 0 - - # Behavior options - simulate_motion: bool = False # If True, set_joint_positions updates internal state - - -@dataclass -class CallRecord: - """Record of a method call for verification.""" - - method: str - args: tuple = field(default_factory=tuple) - kwargs: dict = field(default_factory=dict) - - -class MockSDK(BaseManipulatorSDK): - """Mock SDK for unit testing. Implements BaseManipulatorSDK interface. - - Usage: - # Basic usage - mock = MockSDK() - driver = create_driver_with_sdk(mock) - driver.enable_servo() - assert mock.enable_servos_called - - # With custom config - config = MockSDKConfig(dof=7, connect_fails=True) - mock = MockSDK(config=config) - - # With initial positions - mock = MockSDK(positions=[0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) - - # Verify calls - mock.set_joint_positions([0.1] * 6) - assert mock.was_called("set_joint_positions") - assert mock.call_count("set_joint_positions") == 1 - """ - - def __init__( - self, - config: MockSDKConfig | None = None, - *, - dof: int = 6, - vendor: str = "Mock", - model: str = "TestArm", - positions: list[float] | None = None, - ): - """Initialize MockSDK. - - Args: - config: Full configuration object (takes precedence) - dof: Degrees of freedom (ignored if config provided) - vendor: Vendor name (ignored if config provided) - model: Model name (ignored if config provided) - positions: Initial joint positions (ignored if config provided) - """ - if config is None: - config = MockSDKConfig( - dof=dof, - vendor=vendor, - model=model, - initial_positions=positions, - ) - - self._config = config - self._dof = config.dof - self._vendor = config.vendor - self._model = config.model - - # State - self._connected = False - self._servos_enabled = False - self._positions = list(config.initial_positions or [0.0] * self._dof) - self._velocities = list(config.initial_velocities or [0.0] * self._dof) - self._efforts = list(config.initial_efforts or [0.0] * self._dof) - self._mode = 0 - self._state = 0 - self._error_code = config.error_code - - # Call tracking - self._calls: list[CallRecord] = [] - - # Convenience flags for simple assertions - self.connect_called = False - self.disconnect_called = False - self.enable_servos_called = False - self.disable_servos_called = False - self.set_joint_positions_called = False - self.set_joint_velocities_called = False - self.stop_motion_called = False - self.emergency_stop_called = False - self.clear_errors_called = False - - def _record_call(self, method: str, *args, **kwargs): - """Record a method call.""" - self._calls.append(CallRecord(method=method, args=args, kwargs=kwargs)) - - def was_called(self, method: str) -> bool: - """Check if a method was called.""" - return any(c.method == method for c in self._calls) - - def call_count(self, method: str) -> int: - """Get the number of times a method was called.""" - return sum(1 for c in self._calls if c.method == method) - - def get_calls(self, method: str) -> list[CallRecord]: - """Get all calls to a specific method.""" - return [c for c in self._calls if c.method == method] - - def get_last_call(self, method: str) -> CallRecord | None: - """Get the last call to a specific method.""" - calls = self.get_calls(method) - return calls[-1] if calls else None - - def reset_calls(self): - """Reset call tracking.""" - self._calls.clear() - self.connect_called = False - self.disconnect_called = False - self.enable_servos_called = False - self.disable_servos_called = False - self.set_joint_positions_called = False - self.set_joint_velocities_called = False - self.stop_motion_called = False - self.emergency_stop_called = False - self.clear_errors_called = False - - # ============= State Manipulation (for test setup) ============= - - def set_positions(self, positions: list[float]): - """Set internal positions (test helper).""" - self._positions = list(positions) - - def set_error(self, code: int, message: str = ""): - """Inject an error state (test helper).""" - self._error_code = code - - def set_enabled(self, enabled: bool): - """Set servo enabled state (test helper).""" - self._servos_enabled = enabled - - # ============= BaseManipulatorSDK Implementation ============= - - def connect(self, config: dict) -> bool: - self._record_call("connect", config) - self.connect_called = True - - if self._config.connect_fails: - return False - - self._connected = True - return True - - def disconnect(self) -> None: - self._record_call("disconnect") - self.disconnect_called = True - self._connected = False - - def is_connected(self) -> bool: - self._record_call("is_connected") - return self._connected - - def get_joint_positions(self) -> list[float]: - self._record_call("get_joint_positions") - return self._positions.copy() - - def get_joint_velocities(self) -> list[float]: - self._record_call("get_joint_velocities") - return self._velocities.copy() - - def get_joint_efforts(self) -> list[float]: - self._record_call("get_joint_efforts") - return self._efforts.copy() - - def set_joint_positions( - self, - positions: list[float], - velocity: float = 1.0, - acceleration: float = 1.0, - wait: bool = False, - ) -> bool: - self._record_call( - "set_joint_positions", - positions, - velocity=velocity, - acceleration=acceleration, - wait=wait, - ) - self.set_joint_positions_called = True - - if self._config.motion_fails: - return False - - if not self._servos_enabled: - return False - - if self._config.simulate_motion: - self._positions = list(positions) - - return True - - def set_joint_velocities(self, velocities: list[float]) -> bool: - self._record_call("set_joint_velocities", velocities) - self.set_joint_velocities_called = True - - if self._config.motion_fails: - return False - - if not self._servos_enabled: - return False - - self._velocities = list(velocities) - return True - - def set_joint_efforts(self, efforts: list[float]) -> bool: - self._record_call("set_joint_efforts", efforts) - return False # Not supported in mock - - def stop_motion(self) -> bool: - self._record_call("stop_motion") - self.stop_motion_called = True - self._velocities = [0.0] * self._dof - return True - - def enable_servos(self) -> bool: - self._record_call("enable_servos") - self.enable_servos_called = True - - if self._config.enable_fails: - return False - - self._servos_enabled = True - return True - - def disable_servos(self) -> bool: - self._record_call("disable_servos") - self.disable_servos_called = True - self._servos_enabled = False - return True - - def are_servos_enabled(self) -> bool: - self._record_call("are_servos_enabled") - return self._servos_enabled - - def get_robot_state(self) -> dict: - self._record_call("get_robot_state") - return { - "state": self._state, - "mode": self._mode, - "error_code": self._error_code, - "is_moving": any(v != 0 for v in self._velocities), - } - - def get_error_code(self) -> int: - self._record_call("get_error_code") - return self._error_code - - def get_error_message(self) -> str: - self._record_call("get_error_message") - return "" if self._error_code == 0 else f"Mock error {self._error_code}" - - def clear_errors(self) -> bool: - self._record_call("clear_errors") - self.clear_errors_called = True - self._error_code = 0 - return True - - def emergency_stop(self) -> bool: - self._record_call("emergency_stop") - self.emergency_stop_called = True - self._velocities = [0.0] * self._dof - self._servos_enabled = False - return True - - def get_info(self) -> ManipulatorInfo: - self._record_call("get_info") - return ManipulatorInfo( - vendor=self._vendor, - model=f"{self._model} (Mock)", - dof=self._dof, - firmware_version="mock-1.0.0", - serial_number="MOCK-001", - ) - - def get_joint_limits(self) -> tuple[list[float], list[float]]: - self._record_call("get_joint_limits") - lower = [-2 * math.pi] * self._dof - upper = [2 * math.pi] * self._dof - return lower, upper - - def get_velocity_limits(self) -> list[float]: - self._record_call("get_velocity_limits") - return [math.pi] * self._dof - - def get_acceleration_limits(self) -> list[float]: - self._record_call("get_acceleration_limits") - return [math.pi * 2] * self._dof - - -# ============= Pytest Fixtures ============= - - -@pytest.fixture -def mock_sdk(): - """Create a basic MockSDK.""" - return MockSDK(dof=6) - - -@pytest.fixture -def mock_sdk_with_positions(): - """Create MockSDK with initial positions.""" - positions = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6] - return MockSDK(positions=positions) diff --git a/dimos/hardware/manipulators_old/base/tests/test_driver_unit.py b/dimos/hardware/manipulators_old/base/tests/test_driver_unit.py deleted file mode 100644 index b305d8cd15..0000000000 --- a/dimos/hardware/manipulators_old/base/tests/test_driver_unit.py +++ /dev/null @@ -1,577 +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. - -"""Unit tests for BaseManipulatorDriver. - -These tests use MockSDK to test driver logic in isolation without hardware. -Run with: pytest dimos/hardware/manipulators/base/tests/test_driver_unit.py -v -""" - -import math -import time - -import pytest - -from ..components import ( - StandardMotionComponent, - StandardServoComponent, - StandardStatusComponent, -) -from ..driver import BaseManipulatorDriver -from .conftest import MockSDK, MockSDKConfig - -# ============================================================================= -# Fixtures -# ============================================================================= -# Note: mock_sdk and mock_sdk_with_positions fixtures are defined in conftest.py - - -@pytest.fixture -def standard_components(): - """Create standard component set.""" - return [ - StandardMotionComponent(), - StandardServoComponent(), - StandardStatusComponent(), - ] - - -@pytest.fixture -def driver(mock_sdk, standard_components): - """Create a driver with MockSDK and standard components.""" - config = {"dof": 6} - driver = BaseManipulatorDriver( - sdk=mock_sdk, - components=standard_components, - config=config, - name="TestDriver", - ) - yield driver - # Cleanup - stop driver if running - try: - driver.stop() - except Exception: - pass - - -@pytest.fixture -def started_driver(driver): - """Create and start a driver.""" - driver.start() - time.sleep(0.05) # Allow threads to start - yield driver - - -# ============================================================================= -# Connection Tests -# ============================================================================= - - -class TestConnection: - """Tests for driver connection behavior.""" - - def test_driver_connects_on_init(self, mock_sdk, standard_components): - """Driver should connect to SDK during initialization.""" - config = {"dof": 6} - driver = BaseManipulatorDriver( - sdk=mock_sdk, - components=standard_components, - config=config, - name="TestDriver", - ) - - assert mock_sdk.connect_called - assert mock_sdk.is_connected() - assert driver.shared_state.is_connected - - driver.stop() - - @pytest.mark.skip( - reason="Driver init failure leaks LCM threads - needs cleanup fix in Module base class" - ) - def test_connection_failure_raises(self, standard_components): - """Driver should raise if SDK connection fails.""" - config_fail = MockSDKConfig(connect_fails=True) - mock_sdk = MockSDK(config=config_fail) - - with pytest.raises(RuntimeError, match="Failed to connect"): - BaseManipulatorDriver( - sdk=mock_sdk, - components=standard_components, - config={"dof": 6}, - name="TestDriver", - ) - - def test_disconnect_on_stop(self, started_driver, mock_sdk): - """Driver should disconnect SDK on stop.""" - started_driver.stop() - - assert mock_sdk.disconnect_called - assert not started_driver.shared_state.is_connected - - -# ============================================================================= -# Joint State Tests -# ============================================================================= - - -class TestJointState: - """Tests for joint state reading.""" - - def test_get_joint_state_returns_positions(self, driver): - """get_joint_state should return current positions.""" - result = driver.get_joint_state() - - assert result["success"] is True - assert len(result["positions"]) == 6 - assert len(result["velocities"]) == 6 - assert len(result["efforts"]) == 6 - - def test_get_joint_state_with_custom_positions(self, standard_components): - """get_joint_state should return SDK positions.""" - expected_positions = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6] - mock_sdk = MockSDK(positions=expected_positions) - - driver = BaseManipulatorDriver( - sdk=mock_sdk, - components=standard_components, - config={"dof": 6}, - name="TestDriver", - ) - - result = driver.get_joint_state() - - assert result["positions"] == expected_positions - - driver.stop() - - def test_shared_state_updated_on_joint_read(self, driver): - """Shared state should be updated when reading joints.""" - # Manually trigger joint state update - driver._update_joint_state() - - assert driver.shared_state.joint_positions is not None - assert len(driver.shared_state.joint_positions) == 6 - - -# ============================================================================= -# Servo Control Tests -# ============================================================================= - - -class TestServoControl: - """Tests for servo enable/disable.""" - - def test_enable_servo_calls_sdk(self, driver, mock_sdk): - """enable_servo should call SDK's enable_servos.""" - result = driver.enable_servo() - - assert result["success"] is True - assert mock_sdk.enable_servos_called - - def test_enable_servo_updates_shared_state(self, driver): - """enable_servo should update shared state.""" - driver.enable_servo() - - # Trigger state update to sync - driver._update_robot_state() - - assert driver.shared_state.is_enabled is True - - def test_disable_servo_calls_sdk(self, driver, mock_sdk): - """disable_servo should call SDK's disable_servos.""" - driver.enable_servo() # Enable first - result = driver.disable_servo() - - assert result["success"] is True - assert mock_sdk.disable_servos_called - - def test_enable_fails_with_error(self, standard_components): - """enable_servo should return failure when SDK fails.""" - config = MockSDKConfig(enable_fails=True) - mock_sdk = MockSDK(config=config) - - driver = BaseManipulatorDriver( - sdk=mock_sdk, - components=standard_components, - config={"dof": 6}, - name="TestDriver", - ) - - result = driver.enable_servo() - - assert result["success"] is False - - driver.stop() - - -# ============================================================================= -# Motion Control Tests -# ============================================================================= - - -class TestMotionControl: - """Tests for motion commands.""" - - def test_move_joint_blocking_calls_sdk(self, driver, mock_sdk): - """move_joint with wait=True should call SDK directly.""" - # Enable servos first (required for motion) - driver.enable_servo() - - target = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6] - # Use wait=True to bypass queue and call SDK directly - result = driver.move_joint(target, velocity=0.5, wait=True) - - assert result["success"] is True - assert mock_sdk.set_joint_positions_called - - # Verify arguments - call = mock_sdk.get_last_call("set_joint_positions") - assert call is not None - assert list(call.args[0]) == target - assert call.kwargs["velocity"] == 0.5 - - def test_move_joint_async_queues_command(self, driver, mock_sdk): - """move_joint with wait=False should queue command.""" - # Enable servos first - driver.enable_servo() - - target = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6] - # Default wait=False queues command - result = driver.move_joint(target, velocity=0.5) - - assert result["success"] is True - assert result.get("queued") is True - # SDK not called yet (command is in queue) - assert not mock_sdk.set_joint_positions_called - # But command is in the queue - assert not driver.command_queue.empty() - - def test_move_joint_fails_without_enable(self, driver, mock_sdk): - """move_joint should fail if servos not enabled (blocking mode).""" - target = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6] - # Use wait=True to test synchronous failure - result = driver.move_joint(target, wait=True) - - assert result["success"] is False - - def test_move_joint_with_simulated_motion(self, standard_components): - """With simulate_motion, positions should update (blocking mode).""" - config = MockSDKConfig(simulate_motion=True) - mock_sdk = MockSDK(config=config) - - driver = BaseManipulatorDriver( - sdk=mock_sdk, - components=standard_components, - config={"dof": 6}, - name="TestDriver", - ) - - driver.enable_servo() - target = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6] - # Use wait=True to execute directly - driver.move_joint(target, wait=True) - - # Check SDK internal state updated - assert mock_sdk.get_joint_positions() == target - - driver.stop() - - def test_stop_motion_calls_sdk(self, driver, mock_sdk): - """stop_motion should call SDK's stop_motion.""" - result = driver.stop_motion() - - # stop_motion may return success=False if not moving, but should not error - assert result is not None - assert mock_sdk.stop_motion_called - - def test_process_command_calls_sdk(self, driver, mock_sdk): - """_process_command should execute queued commands.""" - from ..driver import Command - - driver.enable_servo() - - # Create a position command directly - command = Command( - type="position", - data={"positions": [0.1, 0.2, 0.3, 0.4, 0.5, 0.6], "velocity": 0.5}, - ) - - # Process it directly - driver._process_command(command) - - assert mock_sdk.set_joint_positions_called - - -# ============================================================================= -# Robot State Tests -# ============================================================================= - - -class TestRobotState: - """Tests for robot state reading.""" - - def test_get_robot_state_returns_state(self, driver): - """get_robot_state should return state info.""" - result = driver.get_robot_state() - - assert result["success"] is True - assert "state" in result - assert "mode" in result - assert "error_code" in result - - def test_get_robot_state_with_error(self, standard_components): - """get_robot_state should report errors from SDK.""" - config = MockSDKConfig(error_code=42) - mock_sdk = MockSDK(config=config) - - driver = BaseManipulatorDriver( - sdk=mock_sdk, - components=standard_components, - config={"dof": 6}, - name="TestDriver", - ) - - result = driver.get_robot_state() - - assert result["error_code"] == 42 - - driver.stop() - - def test_clear_errors_calls_sdk(self, driver, mock_sdk): - """clear_errors should call SDK's clear_errors.""" - result = driver.clear_errors() - - assert result["success"] is True - assert mock_sdk.clear_errors_called - - -# ============================================================================= -# Joint Limits Tests -# ============================================================================= - - -class TestJointLimits: - """Tests for joint limit queries.""" - - def test_get_joint_limits_returns_limits(self, driver): - """get_joint_limits should return lower and upper limits.""" - result = driver.get_joint_limits() - - assert result["success"] is True - assert len(result["lower"]) == 6 - assert len(result["upper"]) == 6 - - def test_joint_limits_are_reasonable(self, driver): - """Joint limits should be reasonable values.""" - result = driver.get_joint_limits() - - for lower, upper in zip(result["lower"], result["upper"], strict=False): - assert lower < upper - assert lower >= -2 * math.pi - assert upper <= 2 * math.pi - - -# ============================================================================= -# Capabilities Tests -# ============================================================================= - - -class TestCapabilities: - """Tests for driver capabilities.""" - - def test_capabilities_from_sdk(self, driver): - """Driver should get capabilities from SDK.""" - assert driver.capabilities.dof == 6 - assert len(driver.capabilities.max_joint_velocity) == 6 - assert len(driver.capabilities.joint_limits_lower) == 6 - - def test_capabilities_with_different_dof(self, standard_components): - """Driver should support different DOF arms.""" - mock_sdk = MockSDK(dof=7) - - driver = BaseManipulatorDriver( - sdk=mock_sdk, - components=standard_components, - config={"dof": 7}, - name="TestDriver", - ) - - assert driver.capabilities.dof == 7 - assert len(driver.capabilities.max_joint_velocity) == 7 - - driver.stop() - - -# ============================================================================= -# Component API Exposure Tests -# ============================================================================= - - -class TestComponentAPIExposure: - """Tests for auto-exposed component APIs.""" - - def test_motion_component_api_exposed(self, driver): - """Motion component APIs should be exposed on driver.""" - assert hasattr(driver, "move_joint") - assert hasattr(driver, "stop_motion") - assert callable(driver.move_joint) - - def test_servo_component_api_exposed(self, driver): - """Servo component APIs should be exposed on driver.""" - assert hasattr(driver, "enable_servo") - assert hasattr(driver, "disable_servo") - assert callable(driver.enable_servo) - - def test_status_component_api_exposed(self, driver): - """Status component APIs should be exposed on driver.""" - assert hasattr(driver, "get_joint_state") - assert hasattr(driver, "get_robot_state") - assert hasattr(driver, "get_joint_limits") - assert callable(driver.get_joint_state) - - -# ============================================================================= -# Threading Tests -# ============================================================================= - - -class TestThreading: - """Tests for driver threading behavior.""" - - def test_start_creates_threads(self, driver): - """start() should create control threads.""" - driver.start() - time.sleep(0.05) - - assert len(driver.threads) >= 2 - assert all(t.is_alive() for t in driver.threads) - - driver.stop() - - def test_stop_terminates_threads(self, started_driver): - """stop() should terminate all threads.""" - started_driver.stop() - time.sleep(0.1) - - assert all(not t.is_alive() for t in started_driver.threads) - - def test_stop_calls_sdk_stop_motion(self, started_driver, mock_sdk): - """stop() should call SDK stop_motion.""" - started_driver.stop() - - assert mock_sdk.stop_motion_called - - -# ============================================================================= -# Call Verification Tests (MockSDK features) -# ============================================================================= - - -class TestMockSDKCallTracking: - """Tests for MockSDK call tracking features.""" - - def test_call_count(self, mock_sdk): - """MockSDK should count method calls.""" - mock_sdk.get_joint_positions() - mock_sdk.get_joint_positions() - mock_sdk.get_joint_positions() - - assert mock_sdk.call_count("get_joint_positions") == 3 - - def test_was_called(self, mock_sdk): - """MockSDK.was_called should report if method called.""" - assert not mock_sdk.was_called("enable_servos") - - mock_sdk.enable_servos() - - assert mock_sdk.was_called("enable_servos") - - def test_get_last_call_args(self, mock_sdk): - """MockSDK should record call arguments.""" - positions = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] - mock_sdk.enable_servos() - mock_sdk.set_joint_positions(positions, velocity=0.5, wait=True) - - call = mock_sdk.get_last_call("set_joint_positions") - - assert call is not None - assert list(call.args[0]) == positions - assert call.kwargs["velocity"] == 0.5 - assert call.kwargs["wait"] is True - - def test_reset_calls(self, mock_sdk): - """MockSDK.reset_calls should clear call history.""" - mock_sdk.enable_servos() - mock_sdk.get_joint_positions() - - mock_sdk.reset_calls() - - assert mock_sdk.call_count("enable_servos") == 0 - assert mock_sdk.call_count("get_joint_positions") == 0 - assert not mock_sdk.enable_servos_called - - -# ============================================================================= -# Edge Case Tests -# ============================================================================= - - -class TestEdgeCases: - """Tests for edge cases and error handling.""" - - def test_multiple_enable_calls_optimized(self, driver): - """Multiple enable calls should only call SDK once (optimization).""" - result1 = driver.enable_servo() - result2 = driver.enable_servo() - result3 = driver.enable_servo() - - # All calls succeed - assert result1["success"] is True - assert result2["success"] is True - assert result3["success"] is True - - # But SDK only called once (component optimizes redundant calls) - assert driver.sdk.call_count("enable_servos") == 1 - - # Second and third calls should indicate already enabled - assert result2.get("message") == "Servos already enabled" - assert result3.get("message") == "Servos already enabled" - - def test_disable_when_already_disabled(self, driver): - """Disable when already disabled should return success without SDK call.""" - # MockSDK starts with servos disabled - result = driver.disable_servo() - - assert result["success"] is True - assert result.get("message") == "Servos already disabled" - # SDK not called since already disabled - assert not driver.sdk.disable_servos_called - - def test_disable_after_enable(self, driver): - """Disable after enable should call SDK.""" - driver.enable_servo() - result = driver.disable_servo() - - assert result["success"] is True - assert driver.sdk.disable_servos_called - - def test_emergency_stop(self, driver): - """emergency_stop should disable servos.""" - driver.enable_servo() - - driver.sdk.emergency_stop() - - assert driver.sdk.emergency_stop_called - assert not driver.sdk.are_servos_enabled() diff --git a/dimos/hardware/manipulators_old/base/utils/__init__.py b/dimos/hardware/manipulators_old/base/utils/__init__.py deleted file mode 100644 index a2dcb2f82e..0000000000 --- a/dimos/hardware/manipulators_old/base/utils/__init__.py +++ /dev/null @@ -1,40 +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. - -"""Shared utilities for manipulator drivers.""" - -from .converters import degrees_to_radians, meters_to_mm, mm_to_meters, radians_to_degrees -from .shared_state import SharedState -from .validators import ( - clamp_positions, - scale_velocities, - validate_acceleration_limits, - validate_joint_limits, - validate_trajectory, - validate_velocity_limits, -) - -__all__ = [ - "SharedState", - "clamp_positions", - "degrees_to_radians", - "meters_to_mm", - "mm_to_meters", - "radians_to_degrees", - "scale_velocities", - "validate_acceleration_limits", - "validate_joint_limits", - "validate_trajectory", - "validate_velocity_limits", -] diff --git a/dimos/hardware/manipulators_old/base/utils/converters.py b/dimos/hardware/manipulators_old/base/utils/converters.py deleted file mode 100644 index dff5956f8e..0000000000 --- a/dimos/hardware/manipulators_old/base/utils/converters.py +++ /dev/null @@ -1,266 +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. - -"""Unit conversion utilities for manipulator drivers.""" - -import math - - -def degrees_to_radians(degrees: float | list[float]) -> float | list[float]: - """Convert degrees to radians. - - Args: - degrees: Angle(s) in degrees - - Returns: - Angle(s) in radians - """ - if isinstance(degrees, list): - return [math.radians(d) for d in degrees] - return math.radians(degrees) - - -def radians_to_degrees(radians: float | list[float]) -> float | list[float]: - """Convert radians to degrees. - - Args: - radians: Angle(s) in radians - - Returns: - Angle(s) in degrees - """ - if isinstance(radians, list): - return [math.degrees(r) for r in radians] - return math.degrees(radians) - - -def mm_to_meters(mm: float | list[float]) -> float | list[float]: - """Convert millimeters to meters. - - Args: - mm: Distance(s) in millimeters - - Returns: - Distance(s) in meters - """ - if isinstance(mm, list): - return [m / 1000.0 for m in mm] - return mm / 1000.0 - - -def meters_to_mm(meters: float | list[float]) -> float | list[float]: - """Convert meters to millimeters. - - Args: - meters: Distance(s) in meters - - Returns: - Distance(s) in millimeters - """ - if isinstance(meters, list): - return [m * 1000.0 for m in meters] - return meters * 1000.0 - - -def rpm_to_rad_per_sec(rpm: float | list[float]) -> float | list[float]: - """Convert RPM to rad/s. - - Args: - rpm: Angular velocity in RPM - - Returns: - Angular velocity in rad/s - """ - factor = (2 * math.pi) / 60.0 - if isinstance(rpm, list): - return [r * factor for r in rpm] - return rpm * factor - - -def rad_per_sec_to_rpm(rad_per_sec: float | list[float]) -> float | list[float]: - """Convert rad/s to RPM. - - Args: - rad_per_sec: Angular velocity in rad/s - - Returns: - Angular velocity in RPM - """ - factor = 60.0 / (2 * math.pi) - if isinstance(rad_per_sec, list): - return [r * factor for r in rad_per_sec] - return rad_per_sec * factor - - -def quaternion_to_euler(qx: float, qy: float, qz: float, qw: float) -> tuple[float, float, float]: - """Convert quaternion to Euler angles (roll, pitch, yaw). - - Args: - qx, qy, qz, qw: Quaternion components - - Returns: - Tuple of (roll, pitch, yaw) in radians - """ - # Roll (x-axis rotation) - sinr_cosp = 2 * (qw * qx + qy * qz) - cosr_cosp = 1 - 2 * (qx * qx + qy * qy) - roll = math.atan2(sinr_cosp, cosr_cosp) - - # Pitch (y-axis rotation) - sinp = 2 * (qw * qy - qz * qx) - if abs(sinp) >= 1: - pitch = math.copysign(math.pi / 2, sinp) # Use 90 degrees if out of range - else: - pitch = math.asin(sinp) - - # Yaw (z-axis rotation) - siny_cosp = 2 * (qw * qz + qx * qy) - cosy_cosp = 1 - 2 * (qy * qy + qz * qz) - yaw = math.atan2(siny_cosp, cosy_cosp) - - return roll, pitch, yaw - - -def euler_to_quaternion(roll: float, pitch: float, yaw: float) -> tuple[float, float, float, float]: - """Convert Euler angles to quaternion. - - Args: - roll, pitch, yaw: Euler angles in radians - - Returns: - Tuple of (qx, qy, qz, qw) quaternion components - """ - cy = math.cos(yaw * 0.5) - sy = math.sin(yaw * 0.5) - cp = math.cos(pitch * 0.5) - sp = math.sin(pitch * 0.5) - cr = math.cos(roll * 0.5) - sr = math.sin(roll * 0.5) - - qw = cr * cp * cy + sr * sp * sy - qx = sr * cp * cy - cr * sp * sy - qy = cr * sp * cy + sr * cp * sy - qz = cr * cp * sy - sr * sp * cy - - return qx, qy, qz, qw - - -def pose_dict_to_list(pose: dict[str, float]) -> list[float]: - """Convert pose dictionary to list format. - - Args: - pose: Dict with keys: x, y, z, roll, pitch, yaw - - Returns: - List [x, y, z, roll, pitch, yaw] - """ - return [ - pose.get("x", 0.0), - pose.get("y", 0.0), - pose.get("z", 0.0), - pose.get("roll", 0.0), - pose.get("pitch", 0.0), - pose.get("yaw", 0.0), - ] - - -def pose_list_to_dict(pose: list[float]) -> dict[str, float]: - """Convert pose list to dictionary format. - - Args: - pose: List [x, y, z, roll, pitch, yaw] - - Returns: - Dict with keys: x, y, z, roll, pitch, yaw - """ - if len(pose) < 6: - raise ValueError(f"Pose list must have 6 elements, got {len(pose)}") - - return { - "x": pose[0], - "y": pose[1], - "z": pose[2], - "roll": pose[3], - "pitch": pose[4], - "yaw": pose[5], - } - - -def twist_dict_to_list(twist: dict[str, float]) -> list[float]: - """Convert twist dictionary to list format. - - Args: - twist: Dict with keys: vx, vy, vz, wx, wy, wz - - Returns: - List [vx, vy, vz, wx, wy, wz] - """ - return [ - twist.get("vx", 0.0), - twist.get("vy", 0.0), - twist.get("vz", 0.0), - twist.get("wx", 0.0), - twist.get("wy", 0.0), - twist.get("wz", 0.0), - ] - - -def twist_list_to_dict(twist: list[float]) -> dict[str, float]: - """Convert twist list to dictionary format. - - Args: - twist: List [vx, vy, vz, wx, wy, wz] - - Returns: - Dict with keys: vx, vy, vz, wx, wy, wz - """ - if len(twist) < 6: - raise ValueError(f"Twist list must have 6 elements, got {len(twist)}") - - return { - "vx": twist[0], - "vy": twist[1], - "vz": twist[2], - "wx": twist[3], - "wy": twist[4], - "wz": twist[5], - } - - -def normalize_angle(angle: float) -> float: - """Normalize angle to [-pi, pi]. - - Args: - angle: Angle in radians - - Returns: - Normalized angle in [-pi, pi] - """ - while angle > math.pi: - angle -= 2 * math.pi - while angle < -math.pi: - angle += 2 * math.pi - return angle - - -def normalize_angles(angles: list[float]) -> list[float]: - """Normalize angles to [-pi, pi]. - - Args: - angles: Angles in radians - - Returns: - Normalized angles in [-pi, pi] - """ - return [normalize_angle(a) for a in angles] diff --git a/dimos/hardware/manipulators_old/base/utils/shared_state.py b/dimos/hardware/manipulators_old/base/utils/shared_state.py deleted file mode 100644 index 8af275ea17..0000000000 --- a/dimos/hardware/manipulators_old/base/utils/shared_state.py +++ /dev/null @@ -1,255 +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. - -"""Thread-safe shared state for manipulator drivers.""" - -from dataclasses import dataclass, field -from threading import Lock -import time -from typing import Any - - -@dataclass -class SharedState: - """Thread-safe shared state for manipulator drivers. - - This class holds the current state of the manipulator that needs to be - shared between multiple threads (state reader, command sender, publisher). - All access should be protected by the lock. - """ - - # Thread synchronization - lock: Lock = field(default_factory=Lock) - - # Joint state (current values from hardware) - joint_positions: list[float] | None = None # radians - joint_velocities: list[float] | None = None # rad/s - joint_efforts: list[float] | None = None # Nm - - # Joint targets (commanded values) - target_positions: list[float] | None = None # radians - target_velocities: list[float] | None = None # rad/s - target_efforts: list[float] | None = None # Nm - - # Cartesian state (if available) - cartesian_position: dict[str, float] | None = None # x,y,z,roll,pitch,yaw - cartesian_velocity: dict[str, float] | None = None # vx,vy,vz,wx,wy,wz - - # Cartesian targets - target_cartesian_position: dict[str, float] | None = None - target_cartesian_velocity: dict[str, float] | None = None - - # Force/torque sensor (if available) - force_torque: list[float] | None = None # [fx,fy,fz,tx,ty,tz] - - # System state - robot_state: int = 0 # 0=idle, 1=moving, 2=error, 3=e-stop - control_mode: int = 0 # 0=position, 1=velocity, 2=torque - error_code: int = 0 # 0 = no error - error_message: str = "" # Human-readable error - - # Connection and enable status - is_connected: bool = False - is_enabled: bool = False - is_moving: bool = False - is_homed: bool = False - - # Gripper state (if available) - gripper_position: float | None = None # meters - gripper_force: float | None = None # Newtons - - # Timestamps - last_state_update: float = 0.0 - last_command_sent: float = 0.0 - last_error_time: float = 0.0 - - # Statistics - state_read_count: int = 0 - command_sent_count: int = 0 - error_count: int = 0 - - def update_joint_state( - self, - positions: list[float] | None = None, - velocities: list[float] | None = None, - efforts: list[float] | None = None, - ) -> None: - """Thread-safe update of joint state. - - Args: - positions: Joint positions in radians - velocities: Joint velocities in rad/s - efforts: Joint efforts in Nm - """ - with self.lock: - if positions is not None: - self.joint_positions = positions - if velocities is not None: - self.joint_velocities = velocities - if efforts is not None: - self.joint_efforts = efforts - self.last_state_update = time.time() - self.state_read_count += 1 - - def update_robot_state( - self, - state: int | None = None, - mode: int | None = None, - error_code: int | None = None, - error_message: str | None = None, - ) -> None: - """Thread-safe update of robot state. - - Args: - state: Robot state code - mode: Control mode code - error_code: Error code (0 = no error) - error_message: Human-readable error message - """ - with self.lock: - if state is not None: - self.robot_state = state - if mode is not None: - self.control_mode = mode - if error_code is not None: - self.error_code = error_code - if error_code != 0: - self.error_count += 1 - self.last_error_time = time.time() - if error_message is not None: - self.error_message = error_message - - def update_cartesian_state( - self, position: dict[str, float] | None = None, velocity: dict[str, float] | None = None - ) -> None: - """Thread-safe update of Cartesian state. - - Args: - position: End-effector pose (x,y,z,roll,pitch,yaw) - velocity: End-effector twist (vx,vy,vz,wx,wy,wz) - """ - with self.lock: - if position is not None: - self.cartesian_position = position - if velocity is not None: - self.cartesian_velocity = velocity - - def set_target_joints( - self, - positions: list[float] | None = None, - velocities: list[float] | None = None, - efforts: list[float] | None = None, - ) -> None: - """Thread-safe update of joint targets. - - Args: - positions: Target positions in radians - velocities: Target velocities in rad/s - efforts: Target efforts in Nm - """ - with self.lock: - if positions is not None: - self.target_positions = positions - if velocities is not None: - self.target_velocities = velocities - if efforts is not None: - self.target_efforts = efforts - self.last_command_sent = time.time() - self.command_sent_count += 1 - - def get_joint_state( - self, - ) -> tuple[list[float] | None, list[float] | None, list[float] | None]: - """Thread-safe read of joint state. - - Returns: - Tuple of (positions, velocities, efforts) - """ - with self.lock: - return ( - self.joint_positions.copy() if self.joint_positions else None, - self.joint_velocities.copy() if self.joint_velocities else None, - self.joint_efforts.copy() if self.joint_efforts else None, - ) - - def get_robot_state(self) -> dict[str, Any]: - """Thread-safe read of robot state. - - Returns: - Dict with state information - """ - with self.lock: - return { - "state": self.robot_state, - "mode": self.control_mode, - "error_code": self.error_code, - "error_message": self.error_message, - "is_connected": self.is_connected, - "is_enabled": self.is_enabled, - "is_moving": self.is_moving, - "last_update": self.last_state_update, - } - - def get_statistics(self) -> dict[str, Any]: - """Get statistics about state updates. - - Returns: - Dict with statistics - """ - with self.lock: - return { - "state_read_count": self.state_read_count, - "command_sent_count": self.command_sent_count, - "error_count": self.error_count, - "last_state_update": self.last_state_update, - "last_command_sent": self.last_command_sent, - "last_error_time": self.last_error_time, - } - - def clear_errors(self) -> None: - """Clear error state.""" - with self.lock: - self.error_code = 0 - self.error_message = "" - - def reset(self) -> None: - """Reset all state to initial values.""" - with self.lock: - self.joint_positions = None - self.joint_velocities = None - self.joint_efforts = None - self.target_positions = None - self.target_velocities = None - self.target_efforts = None - self.cartesian_position = None - self.cartesian_velocity = None - self.target_cartesian_position = None - self.target_cartesian_velocity = None - self.force_torque = None - self.robot_state = 0 - self.control_mode = 0 - self.error_code = 0 - self.error_message = "" - self.is_connected = False - self.is_enabled = False - self.is_moving = False - self.is_homed = False - self.gripper_position = None - self.gripper_force = None - self.last_state_update = 0.0 - self.last_command_sent = 0.0 - self.last_error_time = 0.0 - self.state_read_count = 0 - self.command_sent_count = 0 - self.error_count = 0 diff --git a/dimos/hardware/manipulators_old/base/utils/validators.py b/dimos/hardware/manipulators_old/base/utils/validators.py deleted file mode 100644 index 3fabdcd306..0000000000 --- a/dimos/hardware/manipulators_old/base/utils/validators.py +++ /dev/null @@ -1,254 +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. - -"""Validation utilities for manipulator drivers.""" - -from typing import cast - - -def validate_joint_limits( - positions: list[float], - lower_limits: list[float], - upper_limits: list[float], - tolerance: float = 0.0, -) -> tuple[bool, str | None]: - """Validate joint positions are within limits. - - Args: - positions: Joint positions to validate (radians) - lower_limits: Lower joint limits (radians) - upper_limits: Upper joint limits (radians) - tolerance: Optional tolerance for soft limits (radians) - - Returns: - Tuple of (is_valid, error_message) - If valid, error_message is None - """ - if len(positions) != len(lower_limits) or len(positions) != len(upper_limits): - return False, f"Dimension mismatch: {len(positions)} positions, {len(lower_limits)} limits" - - for i, pos in enumerate(positions): - lower = lower_limits[i] - tolerance - upper = upper_limits[i] + tolerance - - if pos < lower: - return False, f"Joint {i} position {pos:.3f} below limit {lower_limits[i]:.3f}" - - if pos > upper: - return False, f"Joint {i} position {pos:.3f} above limit {upper_limits[i]:.3f}" - - return True, None - - -def validate_velocity_limits( - velocities: list[float], max_velocities: list[float], scale_factor: float = 1.0 -) -> tuple[bool, str | None]: - """Validate joint velocities are within limits. - - Args: - velocities: Joint velocities to validate (rad/s) - max_velocities: Maximum allowed velocities (rad/s) - scale_factor: Optional scaling factor (0-1) to reduce max velocity - - Returns: - Tuple of (is_valid, error_message) - If valid, error_message is None - """ - if len(velocities) != len(max_velocities): - return ( - False, - f"Dimension mismatch: {len(velocities)} velocities, {len(max_velocities)} limits", - ) - - if scale_factor <= 0 or scale_factor > 1: - return False, f"Invalid scale factor: {scale_factor} (must be in (0, 1])" - - for i, vel in enumerate(velocities): - max_vel = max_velocities[i] * scale_factor - - if abs(vel) > max_vel: - return False, f"Joint {i} velocity {abs(vel):.3f} exceeds limit {max_vel:.3f}" - - return True, None - - -def validate_acceleration_limits( - accelerations: list[float], max_accelerations: list[float], scale_factor: float = 1.0 -) -> tuple[bool, str | None]: - """Validate joint accelerations are within limits. - - Args: - accelerations: Joint accelerations to validate (rad/s²) - max_accelerations: Maximum allowed accelerations (rad/s²) - scale_factor: Optional scaling factor (0-1) to reduce max acceleration - - Returns: - Tuple of (is_valid, error_message) - If valid, error_message is None - """ - if len(accelerations) != len(max_accelerations): - return ( - False, - f"Dimension mismatch: {len(accelerations)} accelerations, {len(max_accelerations)} limits", - ) - - if scale_factor <= 0 or scale_factor > 1: - return False, f"Invalid scale factor: {scale_factor} (must be in (0, 1])" - - for i, acc in enumerate(accelerations): - max_acc = max_accelerations[i] * scale_factor - - if abs(acc) > max_acc: - return False, f"Joint {i} acceleration {abs(acc):.3f} exceeds limit {max_acc:.3f}" - - return True, None - - -def validate_trajectory( - trajectory: list[dict[str, float | list[float]]], - lower_limits: list[float], - upper_limits: list[float], - max_velocities: list[float] | None = None, - max_accelerations: list[float] | None = None, -) -> tuple[bool, str | None]: - """Validate a joint trajectory. - - Args: - trajectory: List of waypoints, each with: - - 'positions': list[float] in radians - - 'velocities': Optional list[float] in rad/s - - 'time': float seconds from start - lower_limits: Lower joint limits (radians) - upper_limits: Upper joint limits (radians) - max_velocities: Optional maximum velocities (rad/s) - max_accelerations: Optional maximum accelerations (rad/s²) - - Returns: - Tuple of (is_valid, error_message) - If valid, error_message is None - """ - if not trajectory: - return False, "Empty trajectory" - - # Check first waypoint starts at time 0 - if trajectory[0].get("time", 0) != 0: - return False, "Trajectory must start at time 0" - - # Check waypoints are time-ordered - prev_time: float = -1.0 - for i, waypoint in enumerate(trajectory): - curr_time = cast("float", waypoint.get("time", 0)) - if curr_time <= prev_time: - return False, f"Waypoint {i} time {curr_time} not after previous {prev_time}" - prev_time = curr_time - - # Validate each waypoint - for i, waypoint in enumerate(trajectory): - # Check required fields - if "positions" not in waypoint: - return False, f"Waypoint {i} missing positions" - - positions = cast("list[float]", waypoint["positions"]) - - # Validate position limits - valid, error = validate_joint_limits(positions, lower_limits, upper_limits) - if not valid: - return False, f"Waypoint {i}: {error}" - - # Validate velocity limits if provided - if "velocities" in waypoint and max_velocities: - velocities = cast("list[float]", waypoint["velocities"]) - valid, error = validate_velocity_limits(velocities, max_velocities) - if not valid: - return False, f"Waypoint {i}: {error}" - - # Check acceleration limits between waypoints - if max_accelerations and len(trajectory) > 1: - for i in range(1, len(trajectory)): - prev = trajectory[i - 1] - curr = trajectory[i] - - dt = cast("float", curr["time"]) - cast("float", prev["time"]) - if dt <= 0: - continue - - # Estimate acceleration from position change - prev_pos = cast("list[float]", prev["positions"]) - curr_pos = cast("list[float]", curr["positions"]) - for j in range(len(prev_pos)): - pos_change = curr_pos[j] - prev_pos[j] - pos_change / dt - - # If velocities provided, use them for better estimate - if "velocities" in prev and "velocities" in curr: - prev_vel = cast("list[float]", prev["velocities"]) - curr_vel = cast("list[float]", curr["velocities"]) - vel_change = curr_vel[j] - prev_vel[j] - acc = vel_change / dt - if abs(acc) > max_accelerations[j]: - return ( - False, - f"Acceleration between waypoint {i - 1} and {i} joint {j}: {abs(acc):.3f} exceeds limit {max_accelerations[j]:.3f}", - ) - - return True, None - - -def scale_velocities( - velocities: list[float], max_velocities: list[float], scale_factor: float = 0.8 -) -> list[float]: - """Scale velocities to stay within limits. - - Args: - velocities: Desired velocities (rad/s) - max_velocities: Maximum allowed velocities (rad/s) - scale_factor: Safety factor (0-1) to stay below limits - - Returns: - Scaled velocities that respect limits - """ - if not velocities or not max_velocities: - return velocities - - # Find the joint that requires most scaling - max_scale = 1.0 - for vel, max_vel in zip(velocities, max_velocities, strict=False): - if max_vel > 0 and abs(vel) > 0: - required_scale = abs(vel) / (max_vel * scale_factor) - max_scale = max(max_scale, required_scale) - - # Apply uniform scaling to maintain direction - if max_scale > 1.0: - return [v / max_scale for v in velocities] - - return velocities - - -def clamp_positions( - positions: list[float], lower_limits: list[float], upper_limits: list[float] -) -> list[float]: - """Clamp positions to stay within limits. - - Args: - positions: Desired positions (radians) - lower_limits: Lower joint limits (radians) - upper_limits: Upper joint limits (radians) - - Returns: - Clamped positions within limits - """ - clamped = [] - for pos, lower, upper in zip(positions, lower_limits, upper_limits, strict=False): - clamped.append(max(lower, min(upper, pos))) - return clamped diff --git a/dimos/hardware/manipulators_old/integration_runner_deprecated.py b/dimos/hardware/manipulators_old/integration_runner_deprecated.py deleted file mode 100644 index 6766cde692..0000000000 --- a/dimos/hardware/manipulators_old/integration_runner_deprecated.py +++ /dev/null @@ -1,626 +0,0 @@ -#!/usr/bin/env python3 -# 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. - -""" -Integration test runner for manipulator drivers. - -This is a standalone script (NOT a pytest test file) that tests the common -BaseManipulatorDriver interface that all arms implement. -Supports both mock mode (for CI/CD) and hardware mode (for real testing). - -NOTE: This file is intentionally NOT named test_*.py to avoid pytest auto-discovery. -For pytest-based unit tests, see: dimos/hardware/manipulators/base/tests/test_driver_unit.py - -Usage: - # Run with mock (CI/CD safe, default) - python -m dimos.hardware.manipulators.integration_test_runner - - # Run specific arm with mock - python -m dimos.hardware.manipulators.integration_test_runner --arm piper - - # Run with real hardware (xArm) - python -m dimos.hardware.manipulators.integration_test_runner --hardware --ip 192.168.1.210 - - # Run with real hardware (Piper) - python -m dimos.hardware.manipulators.integration_test_runner --hardware --arm piper --can can0 - - # Run specific test - python -m dimos.hardware.manipulators.integration_test_runner --test connection - - # Skip motion tests (safer for hardware) - python -m dimos.hardware.manipulators.integration_test_runner --hardware --skip-motion -""" - -import argparse -import math -import sys -import time - -from dimos.core.transport import LCMTransport -from dimos.hardware.manipulators_old.base.sdk_interface import BaseManipulatorSDK, ManipulatorInfo -from dimos.msgs.sensor_msgs import JointState, RobotState - - -class MockSDK(BaseManipulatorSDK): - """Mock SDK for testing without hardware. Works for any arm type.""" - - def __init__(self, dof: int = 6, vendor: str = "Mock", model: str = "TestArm"): - self._connected = True - self._dof = dof - self._vendor = vendor - self._model = model - self._positions = [0.0] * dof - self._velocities = [0.0] * dof - self._efforts = [0.0] * dof - self._servos_enabled = False - self._mode = 0 - self._state = 0 - self._error_code = 0 - - def connect(self, config: dict) -> bool: - self._connected = True - return True - - def disconnect(self) -> None: - self._connected = False - - def is_connected(self) -> bool: - return self._connected - - def get_joint_positions(self) -> list[float]: - return self._positions.copy() - - def get_joint_velocities(self) -> list[float]: - return self._velocities.copy() - - def get_joint_efforts(self) -> list[float]: - return self._efforts.copy() - - def set_joint_positions( - self, - positions: list[float], - velocity: float = 1.0, - acceleration: float = 1.0, - wait: bool = False, - ) -> bool: - if not self._servos_enabled: - return False - self._positions = list(positions) - return True - - def set_joint_velocities(self, velocities: list[float]) -> bool: - if not self._servos_enabled: - return False - self._velocities = list(velocities) - return True - - def set_joint_efforts(self, efforts: list[float]) -> bool: - return False # Not supported in mock - - def stop_motion(self) -> bool: - self._velocities = [0.0] * self._dof - return True - - def enable_servos(self) -> bool: - self._servos_enabled = True - return True - - def disable_servos(self) -> bool: - self._servos_enabled = False - return True - - def are_servos_enabled(self) -> bool: - return self._servos_enabled - - def get_robot_state(self) -> dict: - return { - "state": self._state, - "mode": self._mode, - "error_code": self._error_code, - "is_moving": any(v != 0 for v in self._velocities), - } - - def get_error_code(self) -> int: - return self._error_code - - def get_error_message(self) -> str: - return "" if self._error_code == 0 else f"Error {self._error_code}" - - def clear_errors(self) -> bool: - self._error_code = 0 - return True - - def emergency_stop(self) -> bool: - self._velocities = [0.0] * self._dof - self._servos_enabled = False - return True - - def get_info(self) -> ManipulatorInfo: - return ManipulatorInfo( - vendor=self._vendor, - model=f"{self._model} (Mock)", - dof=self._dof, - firmware_version="mock-1.0.0", - serial_number="MOCK-001", - ) - - def get_joint_limits(self) -> tuple[list[float], list[float]]: - lower = [-2 * math.pi] * self._dof - upper = [2 * math.pi] * self._dof - return lower, upper - - def get_velocity_limits(self) -> list[float]: - return [math.pi] * self._dof - - def get_acceleration_limits(self) -> list[float]: - return [math.pi * 2] * self._dof - - -# ============================================================================= -# Test Functions (work with any driver implementing BaseManipulatorDriver) -# ============================================================================= - - -def check_connection(driver, hardware: bool) -> bool: - """Test that driver connects to hardware/mock.""" - print("Testing connection...") - - if not driver.sdk.is_connected(): - print(" FAIL: SDK not connected") - return False - - info = driver.sdk.get_info() - print(f" Connected to: {info.vendor} {info.model}") - print(f" DOF: {info.dof}") - print(f" Firmware: {info.firmware_version}") - print(f" Mode: {'HARDWARE' if hardware else 'MOCK'}") - print(" PASS") - return True - - -def check_read_joint_state(driver, hardware: bool) -> bool: - """Test reading joint state.""" - print("Testing read joint state...") - - result = driver.get_joint_state() - if not result.get("success"): - print(f" FAIL: {result.get('error')}") - return False - - positions = result["positions"] - velocities = result["velocities"] - efforts = result["efforts"] - - print(f" Positions (deg): {[f'{math.degrees(p):.1f}' for p in positions]}") - print(f" Velocities: {[f'{v:.3f}' for v in velocities]}") - print(f" Efforts: {[f'{e:.2f}' for e in efforts]}") - - if len(positions) != driver.capabilities.dof: - print(f" FAIL: Expected {driver.capabilities.dof} joints, got {len(positions)}") - return False - - print(" PASS") - return True - - -def check_get_robot_state(driver, hardware: bool) -> bool: - """Test getting robot state.""" - print("Testing robot state...") - - result = driver.get_robot_state() - if not result.get("success"): - print(f" FAIL: {result.get('error')}") - return False - - print(f" State: {result.get('state')}") - print(f" Mode: {result.get('mode')}") - print(f" Error code: {result.get('error_code')}") - print(f" Is moving: {result.get('is_moving')}") - print(" PASS") - return True - - -def check_servo_enable_disable(driver, hardware: bool) -> bool: - """Test enabling and disabling servos.""" - print("Testing servo enable/disable...") - - # Enable - result = driver.enable_servo() - if not result.get("success"): - print(f" FAIL enable: {result.get('error')}") - return False - print(" Enabled servos") - - # Hardware needs more time for state to propagate - time.sleep(1.0 if hardware else 0.01) - - # Check state with retry for hardware - enabled = driver.sdk.are_servos_enabled() - if not enabled and hardware: - # Retry after additional delay - time.sleep(0.5) - enabled = driver.sdk.are_servos_enabled() - - if not enabled: - print(" FAIL: Servos not enabled after enable_servo()") - return False - print(" Verified servos enabled") - - # # Disable - # result = driver.disable_servo() - # if not result.get("success"): - # print(f" FAIL disable: {result.get('error')}") - # return False - # print(" Disabled servos") - - print(" PASS") - return True - - -def check_joint_limits(driver, hardware: bool) -> bool: - """Test getting joint limits.""" - print("Testing joint limits...") - - result = driver.get_joint_limits() - if not result.get("success"): - print(f" FAIL: {result.get('error')}") - return False - - lower = result["lower"] - upper = result["upper"] - - print(f" Lower (deg): {[f'{math.degrees(l):.1f}' for l in lower]}") - print(f" Upper (deg): {[f'{math.degrees(u):.1f}' for u in upper]}") - - if len(lower) != driver.capabilities.dof: - print(" FAIL: Wrong number of limits") - return False - - print(" PASS") - return True - - -def check_stop_motion(driver, hardware: bool) -> bool: - """Test stop motion command.""" - print("Testing stop motion...") - - result = driver.stop_motion() - # Note: stop_motion may return success=False if arm isn't moving, - # which is expected behavior. We just verify no exception occurred. - if result is None: - print(" FAIL: stop_motion returned None") - return False - - if result.get("error"): - print(f" FAIL: {result.get('error')}") - return False - - # success=False when not moving is OK, success=True is also OK - print(f" stop_motion returned success={result.get('success')}") - print(" PASS") - return True - - -def check_small_motion(driver, hardware: bool) -> bool: - """Test a small joint motion (5 degrees on joint 1). - - WARNING: With --hardware, this MOVES the real robot! - """ - print("Testing small motion (5 deg on J1)...") - if hardware: - print(" WARNING: Robot will move!") - - # Get current position - result = driver.get_joint_state() - if not result.get("success"): - print(f" FAIL: Cannot read state: {result.get('error')}") - return False - - current_pos = list(result["positions"]) - print(f" Current J1: {math.degrees(current_pos[0]):.2f} deg") - - driver.clear_errors() - # print(driver.get_state()) - - # Enable servos - result = driver.enable_servo() - print(result) - if not result.get("success"): - print(f" FAIL: Cannot enable servos: {result.get('error')}") - return False - - time.sleep(0.5 if hardware else 0.01) - - # Move +5 degrees on joint 1 - target_pos = current_pos.copy() - target_pos[0] += math.radians(5.0) - print(f" Target J1: {math.degrees(target_pos[0]):.2f} deg") - - result = driver.move_joint(target_pos, velocity=0.3, wait=True) - if not result.get("success"): - print(f" FAIL: Motion failed: {result.get('error')}") - return False - - time.sleep(1.0 if hardware else 0.01) - - # Verify position - result = driver.get_joint_state() - new_pos = result["positions"] - error = abs(new_pos[0] - target_pos[0]) - print( - f" Reached J1: {math.degrees(new_pos[0]):.2f} deg (error: {math.degrees(error):.3f} deg)" - ) - - if hardware and error > math.radians(1.0): # Allow 1 degree error for real hardware - print(" FAIL: Position error too large") - return False - - # Move back - print(" Moving back to original position...") - driver.move_joint(current_pos, velocity=0.3, wait=True) - time.sleep(1.0 if hardware else 0.01) - - print(" PASS") - return True - - -# ============================================================================= -# Driver Factory -# ============================================================================= - - -def create_driver(arm: str, hardware: bool, config: dict): - """Create driver for the specified arm type. - - Args: - arm: Arm type ('xarm', 'piper', etc.) - hardware: If True, use real hardware; if False, use mock SDK - config: Configuration dict (ip, dof, etc.) - - Returns: - Driver instance - """ - if arm == "xarm": - from dimos.hardware.manipulators.xarm.xarm_driver import XArmDriver - - if hardware: - return XArmDriver(config=config) - else: - # Create driver with mock SDK - driver = XArmDriver.__new__(XArmDriver) - # Manually initialize with mock - from dimos.hardware.manipulators.base import ( - BaseManipulatorDriver, - StandardMotionComponent, - StandardServoComponent, - StandardStatusComponent, - ) - - mock_sdk = MockSDK(dof=config.get("dof", 6), vendor="UFactory", model="xArm") - components = [ - StandardMotionComponent(), - StandardServoComponent(), - StandardStatusComponent(), - ] - BaseManipulatorDriver.__init__( - driver, sdk=mock_sdk, components=components, config=config, name="XArmDriver" - ) - return driver - - elif arm == "piper": - from dimos.hardware.manipulators.piper.piper_driver import PiperDriver - - if hardware: - return PiperDriver(config=config) - else: - # Create driver with mock SDK - driver = PiperDriver.__new__(PiperDriver) - from dimos.hardware.manipulators.base import ( - BaseManipulatorDriver, - StandardMotionComponent, - StandardServoComponent, - StandardStatusComponent, - ) - - mock_sdk = MockSDK(dof=6, vendor="Agilex", model="Piper") - components = [ - StandardMotionComponent(), - StandardServoComponent(), - StandardStatusComponent(), - ] - BaseManipulatorDriver.__init__( - driver, sdk=mock_sdk, components=components, config=config, name="PiperDriver" - ) - return driver - - else: - raise ValueError(f"Unknown arm type: {arm}. Supported: xarm, piper") - - -# ============================================================================= -# Test Runner -# ============================================================================= - - -def configure_transports(driver, arm: str): - """Configure LCM transports for the driver (like production does). - - Args: - driver: The driver instance - arm: Arm type for topic naming - """ - # Create LCM transports for state publishing - joint_state_transport = LCMTransport(f"/test/{arm}/joint_state", JointState) - robot_state_transport = LCMTransport(f"/test/{arm}/robot_state", RobotState) - - # Set transports on driver's Out streams - if driver.joint_state: - driver.joint_state._transport = joint_state_transport - if driver.robot_state: - driver.robot_state._transport = robot_state_transport - - -def run_tests( - arm: str, - hardware: bool, - config: dict, - test_name: str | None = None, - skip_motion: bool = False, -): - """Run integration tests.""" - mode = "HARDWARE" if hardware else "MOCK" - print("=" * 60) - print(f"Manipulator Driver Integration Tests ({mode})") - print("=" * 60) - print(f"Arm: {arm}") - print(f"Config: {config}") - print() - - # Create driver - print("Creating driver...") - try: - driver = create_driver(arm, hardware, config) - except Exception as e: - print(f"FATAL: Failed to create driver: {e}") - return False - - # Configure transports (like production does) - print("Configuring transports...") - configure_transports(driver, arm) - - # Start driver - print("Starting driver...") - try: - driver.start() - # Piper needs more initialization time before commands work - wait_time = 3.0 if (hardware and arm == "piper") else (1.0 if hardware else 0.1) - time.sleep(wait_time) - except Exception as e: - print(f"FATAL: Failed to start driver: {e}") - return False - - # Define tests (stop_motion last since it leaves arm in stopped state) - tests = [ - ("connection", check_connection), - ("read_state", check_read_joint_state), - ("robot_state", check_get_robot_state), - ("joint_limits", check_joint_limits), - # ("servo", check_servo_enable_disable), - ] - - if not skip_motion: - tests.append(("motion", check_small_motion)) - - # Stop test always last (leaves arm in stopped state) - tests.append(("stop", check_stop_motion)) - - # Run tests - results = {} - print() - print("-" * 60) - - for name, test_func in tests: - if test_name and name != test_name: - continue - - try: - results[name] = test_func(driver, hardware) - except Exception as e: - print(f" EXCEPTION: {e}") - import traceback - - traceback.print_exc() - results[name] = False - - print() - - # Stop driver - print("Stopping driver...") - try: - driver.stop() - except Exception as e: - print(f"Warning: Error stopping driver: {e}") - - # Summary - print("-" * 60) - print("SUMMARY") - print("-" * 60) - passed = sum(1 for r in results.values() if r) - total = len(results) - - for name, result in results.items(): - status = "PASS" if result else "FAIL" - print(f" {name}: {status}") - - print() - print(f"Result: {passed}/{total} tests passed") - - return passed == total - - -def main(): - parser = argparse.ArgumentParser( - description="Generic manipulator driver integration tests", - formatter_class=argparse.RawDescriptionHelpFormatter, - epilog=""" -Examples: - # Mock mode (CI/CD safe, default) - python -m dimos.hardware.manipulators.integration_test_runner - - # xArm hardware mode - python -m dimos.hardware.manipulators.integration_test_runner --hardware --ip 192.168.1.210 - - # Piper hardware mode - python -m dimos.hardware.manipulators.integration_test_runner --hardware --arm piper --can can0 - - # Skip motion tests - python -m dimos.hardware.manipulators.integration_test_runner --hardware --skip-motion -""", - ) - parser.add_argument( - "--arm", default="xarm", choices=["xarm", "piper"], help="Arm type to test (default: xarm)" - ) - parser.add_argument( - "--hardware", action="store_true", help="Use real hardware (default: mock mode)" - ) - parser.add_argument( - "--ip", default="192.168.1.210", help="IP address for xarm (default: 192.168.1.210)" - ) - parser.add_argument("--can", default="can0", help="CAN interface for piper (default: can0)") - parser.add_argument( - "--dof", type=int, help="Degrees of freedom (auto-detected in hardware mode)" - ) - parser.add_argument("--test", help="Run specific test only") - parser.add_argument("--skip-motion", action="store_true", help="Skip motion tests") - args = parser.parse_args() - - # Build config - DOF auto-detected from hardware if not specified - config = {} - if args.arm == "xarm" and args.ip: - config["ip"] = args.ip - if args.arm == "piper" and args.can: - config["can_port"] = args.can - if args.dof: - config["dof"] = args.dof - elif not args.hardware: - # Mock mode needs explicit DOF - config["dof"] = 6 - - success = run_tests(args.arm, args.hardware, config, args.test, args.skip_motion) - sys.exit(0 if success else 1) - - -if __name__ == "__main__": - main() diff --git a/dimos/hardware/manipulators_old/piper/README.md b/dimos/hardware/manipulators_old/piper/README.md deleted file mode 100644 index 89ff2161ac..0000000000 --- a/dimos/hardware/manipulators_old/piper/README.md +++ /dev/null @@ -1,35 +0,0 @@ -# Piper Driver - -Driver for the Piper 6-DOF manipulator with CAN bus communication. - -## Supported Features - -✅ **Joint Control** -- Position control -- Velocity control (integration-based) -- Joint state feedback at 100Hz - -✅ **System Control** -- Enable/disable motors -- Emergency stop -- Error recovery - -✅ **Gripper Control** -- Position and force control -- Gripper state feedback - -## Cartesian Control Limitation - -⚠️ **Cartesian control is currently NOT available for the Piper arm.** - -### Why? -The Piper SDK doesn't expose an inverse kinematics (IK) solver that can be called without moving the robot. While the robot can execute Cartesian commands internally, we cannot: -- Pre-compute joint trajectories for Cartesian paths -- Validate if a pose is reachable without trying to move there -- Plan complex Cartesian trajectories offline - -### Future Solution -We will implement a universal IK solver that sits outside the driver layer and works with all arms (XArm, Piper, and future robots), regardless of whether they expose internal IK. - -### Current Workaround -Use joint-space control for now. If you need Cartesian planning, consider using external IK libraries like ikpy or robotics-toolbox-python with the Piper's URDF file. diff --git a/dimos/hardware/manipulators_old/piper/__init__.py b/dimos/hardware/manipulators_old/piper/__init__.py deleted file mode 100644 index acead9f7fb..0000000000 --- a/dimos/hardware/manipulators_old/piper/__init__.py +++ /dev/null @@ -1,32 +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. - -""" -Piper Arm Driver - -Real-time driver for Piper manipulator with CAN bus communication. -""" - -from .piper_blueprints import piper_cartesian, piper_servo, piper_trajectory -from .piper_driver import PiperDriver, piper_driver -from .piper_wrapper import PiperSDKWrapper - -__all__ = [ - "PiperDriver", - "PiperSDKWrapper", - "piper_cartesian", - "piper_driver", - "piper_servo", - "piper_trajectory", -] diff --git a/dimos/hardware/manipulators_old/piper/can_activate.sh b/dimos/hardware/manipulators_old/piper/can_activate.sh deleted file mode 100644 index addb892557..0000000000 --- a/dimos/hardware/manipulators_old/piper/can_activate.sh +++ /dev/null @@ -1,138 +0,0 @@ -#!/bin/bash - -# The default CAN name can be set by the user via command-line parameters. -DEFAULT_CAN_NAME="${1:-can0}" - -# The default bitrate for a single CAN module can be set by the user via command-line parameters. -DEFAULT_BITRATE="${2:-1000000}" - -# USB hardware address (optional parameter) -USB_ADDRESS="${3}" -echo "-------------------START-----------------------" -# Check if ethtool is installed. -if ! dpkg -l | grep -q "ethtool"; then - echo "\e[31mError: ethtool not detected in the system.\e[0m" - echo "Please use the following command to install ethtool:" - echo "sudo apt update && sudo apt install ethtool" - exit 1 -fi - -# Check if can-utils is installed. -if ! dpkg -l | grep -q "can-utils"; then - echo "\e[31mError: can-utils not detected in the system.\e[0m" - echo "Please use the following command to install ethtool:" - echo "sudo apt update && sudo apt install can-utils" - exit 1 -fi - -echo "Both ethtool and can-utils are installed." - -# Retrieve the number of CAN modules in the current system. -CURRENT_CAN_COUNT=$(ip link show type can | grep -c "link/can") - -# Verify if the number of CAN modules in the current system matches the expected value. -if [ "$CURRENT_CAN_COUNT" -ne "1" ]; then - if [ -z "$USB_ADDRESS" ]; then - # Iterate through all CAN interfaces. - for iface in $(ip -br link show type can | awk '{print $1}'); do - # Use ethtool to retrieve bus-info. - BUS_INFO=$(sudo ethtool -i "$iface" | grep "bus-info" | awk '{print $2}') - - if [ -z "$BUS_INFO" ];then - echo "Error: Unable to retrieve bus-info for interface $iface." - continue - fi - - echo "Interface $iface is inserted into USB port $BUS_INFO" - done - echo -e " \e[31m Error: The number of CAN modules detected by the system ($CURRENT_CAN_COUNT) does not match the expected number (1). \e[0m" - echo -e " \e[31m Please add the USB hardware address parameter, such as: \e[0m" - echo -e " bash can_activate.sh can0 1000000 1-2:1.0" - echo "-------------------ERROR-----------------------" - exit 1 - fi -fi - -# Load the gs_usb module. -# sudo modprobe gs_usb -# if [ $? -ne 0 ]; then -# echo "Error: Unable to load the gs_usb module." -# exit 1 -# fi - -if [ -n "$USB_ADDRESS" ]; then - echo "Detected USB hardware address parameter: $USB_ADDRESS" - - # Use ethtool to find the CAN interface corresponding to the USB hardware address. - INTERFACE_NAME="" - for iface in $(ip -br link show type can | awk '{print $1}'); do - BUS_INFO=$(sudo ethtool -i "$iface" | grep "bus-info" | awk '{print $2}') - if [ "$BUS_INFO" = "$USB_ADDRESS" ]; then - INTERFACE_NAME="$iface" - break - fi - done - - if [ -z "$INTERFACE_NAME" ]; then - echo "Error: Unable to find CAN interface corresponding to USB hardware address $USB_ADDRESS." - exit 1 - else - echo "Found the interface corresponding to USB hardware address $USB_ADDRESS: $INTERFACE_NAME." - fi -else - # Retrieve the unique CAN interface. - INTERFACE_NAME=$(ip -br link show type can | awk '{print $1}') - - # Check if the interface name has been retrieved. - if [ -z "$INTERFACE_NAME" ]; then - echo "Error: Unable to detect CAN interface." - exit 1 - fi - BUS_INFO=$(sudo ethtool -i "$INTERFACE_NAME" | grep "bus-info" | awk '{print $2}') - echo "Expected to configure a single CAN module, detected interface $INTERFACE_NAME with corresponding USB address $BUS_INFO." -fi - -# Check if the current interface is already activated. -IS_LINK_UP=$(ip link show "$INTERFACE_NAME" | grep -q "UP" && echo "yes" || echo "no") - -# Retrieve the bitrate of the current interface. -CURRENT_BITRATE=$(ip -details link show "$INTERFACE_NAME" | grep -oP 'bitrate \K\d+') - -if [ "$IS_LINK_UP" = "yes" ] && [ "$CURRENT_BITRATE" -eq "$DEFAULT_BITRATE" ]; then - echo "Interface $INTERFACE_NAME is already activated with a bitrate of $DEFAULT_BITRATE." - - # Check if the interface name matches the default name. - if [ "$INTERFACE_NAME" != "$DEFAULT_CAN_NAME" ]; then - echo "Rename interface $INTERFACE_NAME to $DEFAULT_CAN_NAME." - sudo ip link set "$INTERFACE_NAME" down - sudo ip link set "$INTERFACE_NAME" name "$DEFAULT_CAN_NAME" - sudo ip link set "$DEFAULT_CAN_NAME" up - echo "The interface has been renamed to $DEFAULT_CAN_NAME and reactivated." - else - echo "The interface name is already $DEFAULT_CAN_NAME." - fi -else - # If the interface is not activated or the bitrate is different, configure it. - if [ "$IS_LINK_UP" = "yes" ]; then - echo "Interface $INTERFACE_NAME is already activated, but the bitrate is $CURRENT_BITRATE, which does not match the set value of $DEFAULT_BITRATE." - else - echo "Interface $INTERFACE_NAME is not activated or bitrate is not set." - fi - - # Set the interface bitrate and activate it. - sudo ip link set "$INTERFACE_NAME" down - sudo ip link set "$INTERFACE_NAME" type can bitrate $DEFAULT_BITRATE - sudo ip link set "$INTERFACE_NAME" up - echo "Interface $INTERFACE_NAME has been reset to bitrate $DEFAULT_BITRATE and activated." - - # Rename the interface to the default name. - if [ "$INTERFACE_NAME" != "$DEFAULT_CAN_NAME" ]; then - echo "Rename interface $INTERFACE_NAME to $DEFAULT_CAN_NAME." - sudo ip link set "$INTERFACE_NAME" down - sudo ip link set "$INTERFACE_NAME" name "$DEFAULT_CAN_NAME" - sudo ip link set "$DEFAULT_CAN_NAME" up - echo "The interface has been renamed to $DEFAULT_CAN_NAME and reactivated." - fi -fi - -echo "-------------------OVER------------------------" diff --git a/dimos/hardware/manipulators_old/piper/components/__init__.py b/dimos/hardware/manipulators_old/piper/components/__init__.py deleted file mode 100644 index 2c6d863ca1..0000000000 --- a/dimos/hardware/manipulators_old/piper/components/__init__.py +++ /dev/null @@ -1,17 +0,0 @@ -"""Component classes for PiperDriver.""" - -from .configuration import ConfigurationComponent -from .gripper_control import GripperControlComponent -from .kinematics import KinematicsComponent -from .motion_control import MotionControlComponent -from .state_queries import StateQueryComponent -from .system_control import SystemControlComponent - -__all__ = [ - "ConfigurationComponent", - "GripperControlComponent", - "KinematicsComponent", - "MotionControlComponent", - "StateQueryComponent", - "SystemControlComponent", -] diff --git a/dimos/hardware/manipulators_old/piper/components/configuration.py b/dimos/hardware/manipulators_old/piper/components/configuration.py deleted file mode 100644 index b7ac53c371..0000000000 --- a/dimos/hardware/manipulators_old/piper/components/configuration.py +++ /dev/null @@ -1,348 +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. - -""" -Configuration Component for PiperDriver. - -Provides RPC methods for configuring robot parameters including: -- Joint parameters (limits, speeds, acceleration) -- End-effector parameters (speed, acceleration) -- Collision protection -- Motor configuration -""" - -from typing import Any - -from dimos.core import rpc -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -class ConfigurationComponent: - """ - Component providing configuration RPC methods for PiperDriver. - - This component assumes the parent class has: - - self.piper: C_PiperInterface_V2 instance - - self.config: PiperDriverConfig instance - """ - - # Type hints for attributes provided by parent class - piper: Any - config: Any - - @rpc - def set_joint_config( - self, - motor_num: int, - kp_factor: int, - ki_factor: int, - kd_factor: int, - ke_factor: int = 0, - ) -> tuple[bool, str]: - """ - Configure joint control parameters. - - Args: - motor_num: Motor number (1-6) - kp_factor: Proportional gain factor - ki_factor: Integral gain factor - kd_factor: Derivative gain factor - ke_factor: Error gain factor - - Returns: - Tuple of (success, message) - """ - try: - if motor_num not in range(1, 7): - return (False, f"Invalid motor_num: {motor_num}. Must be 1-6") - - result = self.piper.JointConfig(motor_num, kp_factor, ki_factor, kd_factor, ke_factor) - - if result: - return (True, f"Joint {motor_num} configuration set successfully") - else: - return (False, f"Failed to configure joint {motor_num}") - - except Exception as e: - logger.error(f"set_joint_config failed: {e}") - return (False, str(e)) - - @rpc - def set_joint_max_acc(self, motor_num: int, max_joint_acc: int) -> tuple[bool, str]: - """ - Set joint maximum acceleration. - - Args: - motor_num: Motor number (1-6) - max_joint_acc: Maximum joint acceleration - - Returns: - Tuple of (success, message) - """ - try: - if motor_num not in range(1, 7): - return (False, f"Invalid motor_num: {motor_num}. Must be 1-6") - - result = self.piper.JointMaxAccConfig(motor_num, max_joint_acc) - - if result: - return (True, f"Joint {motor_num} max acceleration set to {max_joint_acc}") - else: - return (False, f"Failed to set max acceleration for joint {motor_num}") - - except Exception as e: - logger.error(f"set_joint_max_acc failed: {e}") - return (False, str(e)) - - @rpc - def set_motor_angle_limit_max_speed( - self, - motor_num: int, - min_joint_angle: int, - max_joint_angle: int, - max_joint_speed: int, - ) -> tuple[bool, str]: - """ - Set motor angle limits and maximum speed. - - Args: - motor_num: Motor number (1-6) - min_joint_angle: Minimum joint angle (in Piper units: 0.001 degrees) - max_joint_angle: Maximum joint angle (in Piper units: 0.001 degrees) - max_joint_speed: Maximum joint speed - - Returns: - Tuple of (success, message) - """ - try: - if motor_num not in range(1, 7): - return (False, f"Invalid motor_num: {motor_num}. Must be 1-6") - - result = self.piper.MotorAngleLimitMaxSpdSet( - motor_num, min_joint_angle, max_joint_angle, max_joint_speed - ) - - if result: - return ( - True, - f"Joint {motor_num} angle limits and max speed set successfully", - ) - else: - return (False, f"Failed to set angle limits for joint {motor_num}") - - except Exception as e: - logger.error(f"set_motor_angle_limit_max_speed failed: {e}") - return (False, str(e)) - - @rpc - def set_motor_max_speed(self, motor_num: int, max_joint_spd: int) -> tuple[bool, str]: - """ - Set motor maximum speed. - - Args: - motor_num: Motor number (1-6) - max_joint_spd: Maximum joint speed - - Returns: - Tuple of (success, message) - """ - try: - if motor_num not in range(1, 7): - return (False, f"Invalid motor_num: {motor_num}. Must be 1-6") - - result = self.piper.MotorMaxSpdSet(motor_num, max_joint_spd) - - if result: - return (True, f"Joint {motor_num} max speed set to {max_joint_spd}") - else: - return (False, f"Failed to set max speed for joint {motor_num}") - - except Exception as e: - logger.error(f"set_motor_max_speed failed: {e}") - return (False, str(e)) - - @rpc - def set_end_speed_and_acc( - self, - end_max_linear_vel: int, - end_max_angular_vel: int, - end_max_linear_acc: int, - end_max_angular_acc: int, - ) -> tuple[bool, str]: - """ - Set end-effector speed and acceleration parameters. - - Args: - end_max_linear_vel: Maximum linear velocity - end_max_angular_vel: Maximum angular velocity - end_max_linear_acc: Maximum linear acceleration - end_max_angular_acc: Maximum angular acceleration - - Returns: - Tuple of (success, message) - """ - try: - result = self.piper.EndSpdAndAccParamSet( - end_max_linear_vel, - end_max_angular_vel, - end_max_linear_acc, - end_max_angular_acc, - ) - - if result: - return (True, "End-effector speed and acceleration parameters set successfully") - else: - return (False, "Failed to set end-effector parameters") - - except Exception as e: - logger.error(f"set_end_speed_and_acc failed: {e}") - return (False, str(e)) - - @rpc - def set_crash_protection_level(self, level: int) -> tuple[bool, str]: - """ - Set collision/crash protection level. - - Args: - level: Protection level (0=disabled, higher values = more sensitive) - - Returns: - Tuple of (success, message) - """ - try: - result = self.piper.CrashProtectionConfig(level) - - if result: - return (True, f"Crash protection level set to {level}") - else: - return (False, "Failed to set crash protection level") - - except Exception as e: - logger.error(f"set_crash_protection_level failed: {e}") - return (False, str(e)) - - @rpc - def search_motor_max_angle_speed_acc_limit(self, motor_num: int) -> tuple[bool, str]: - """ - Search for motor maximum angle, speed, and acceleration limits. - - Args: - motor_num: Motor number (1-6) - - Returns: - Tuple of (success, message) - """ - try: - if motor_num not in range(1, 7): - return (False, f"Invalid motor_num: {motor_num}. Must be 1-6") - - result = self.piper.SearchMotorMaxAngleSpdAccLimit(motor_num) - - if result: - return (True, f"Search initiated for motor {motor_num} limits") - else: - return (False, f"Failed to search limits for motor {motor_num}") - - except Exception as e: - logger.error(f"search_motor_max_angle_speed_acc_limit failed: {e}") - return (False, str(e)) - - @rpc - def search_all_motor_max_angle_speed(self) -> tuple[bool, str]: - """ - Search for all motors' maximum angle and speed limits. - - Returns: - Tuple of (success, message) - """ - try: - result = self.piper.SearchAllMotorMaxAngleSpd() - - if result: - return (True, "Search initiated for all motor angle/speed limits") - else: - return (False, "Failed to search all motor limits") - - except Exception as e: - logger.error(f"search_all_motor_max_angle_speed failed: {e}") - return (False, str(e)) - - @rpc - def search_all_motor_max_acc_limit(self) -> tuple[bool, str]: - """ - Search for all motors' maximum acceleration limits. - - Returns: - Tuple of (success, message) - """ - try: - result = self.piper.SearchAllMotorMaxAccLimit() - - if result: - return (True, "Search initiated for all motor acceleration limits") - else: - return (False, "Failed to search all motor acceleration limits") - - except Exception as e: - logger.error(f"search_all_motor_max_acc_limit failed: {e}") - return (False, str(e)) - - @rpc - def set_sdk_joint_limit_param( - self, joint_limits: list[tuple[float, float]] - ) -> tuple[bool, str]: - """ - Set SDK joint limit parameters. - - Args: - joint_limits: List of (min_angle, max_angle) tuples for each joint in radians - - Returns: - Tuple of (success, message) - """ - try: - if len(joint_limits) != 6: - return (False, f"Expected 6 joint limit tuples, got {len(joint_limits)}") - - # Convert to Piper units and call SDK method - # Note: Actual SDK method signature may vary - logger.info(f"Setting SDK joint limits: {joint_limits}") - return (True, "SDK joint limits set (method may vary by SDK version)") - - except Exception as e: - logger.error(f"set_sdk_joint_limit_param failed: {e}") - return (False, str(e)) - - @rpc - def set_sdk_gripper_range_param(self, min_range: int, max_range: int) -> tuple[bool, str]: - """ - Set SDK gripper range parameters. - - Args: - min_range: Minimum gripper range - max_range: Maximum gripper range - - Returns: - Tuple of (success, message) - """ - try: - # Note: Actual SDK method signature may vary - logger.info(f"Setting SDK gripper range: {min_range} - {max_range}") - return (True, "SDK gripper range set (method may vary by SDK version)") - - except Exception as e: - logger.error(f"set_sdk_gripper_range_param failed: {e}") - return (False, str(e)) diff --git a/dimos/hardware/manipulators_old/piper/components/gripper_control.py b/dimos/hardware/manipulators_old/piper/components/gripper_control.py deleted file mode 100644 index 5f500097cd..0000000000 --- a/dimos/hardware/manipulators_old/piper/components/gripper_control.py +++ /dev/null @@ -1,120 +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. - -""" -Gripper Control Component for PiperDriver. - -Provides RPC methods for gripper control operations. -""" - -from typing import Any - -from dimos.core import rpc -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -class GripperControlComponent: - """ - Component providing gripper control RPC methods for PiperDriver. - - This component assumes the parent class has: - - self.piper: C_PiperInterface_V2 instance - - self.config: PiperDriverConfig instance - """ - - # Type hints for attributes provided by parent class - piper: Any - config: Any - - @rpc - def set_gripper( - self, - gripper_angle: int, - gripper_effort: int = 100, - gripper_enable: int = 0x01, - gripper_state: int = 0x00, - ) -> tuple[bool, str]: - """ - Set gripper position and parameters. - - Args: - gripper_angle: Gripper angle (0-1000, 0=closed, 1000=open) - gripper_effort: Gripper effort/force (0-1000) - gripper_enable: Gripper enable (0x00=disabled, 0x01=enabled) - gripper_state: Gripper state - - Returns: - Tuple of (success, message) - """ - try: - result = self.piper.GripperCtrl( - gripper_angle, gripper_effort, gripper_enable, gripper_state - ) - - if result: - return (True, f"Gripper set to angle={gripper_angle}, effort={gripper_effort}") - else: - return (False, "Failed to set gripper") - - except Exception as e: - logger.error(f"set_gripper failed: {e}") - return (False, str(e)) - - @rpc - def open_gripper(self, effort: int = 100) -> tuple[bool, str]: - """ - Open gripper. - - Args: - effort: Gripper effort (0-1000) - - Returns: - Tuple of (success, message) - """ - result: tuple[bool, str] = self.set_gripper(gripper_angle=1000, gripper_effort=effort) - return result - - @rpc - def close_gripper(self, effort: int = 100) -> tuple[bool, str]: - """ - Close gripper. - - Args: - effort: Gripper effort (0-1000) - - Returns: - Tuple of (success, message) - """ - result: tuple[bool, str] = self.set_gripper(gripper_angle=0, gripper_effort=effort) - return result - - @rpc - def set_gripper_zero(self) -> tuple[bool, str]: - """ - Set gripper zero position. - - Returns: - Tuple of (success, message) - """ - try: - # This method may require specific SDK implementation - # For now, we'll just document it - logger.info("set_gripper_zero called - implementation may vary by SDK version") - return (True, "Gripper zero set (if supported by SDK)") - - except Exception as e: - logger.error(f"set_gripper_zero failed: {e}") - return (False, str(e)) diff --git a/dimos/hardware/manipulators_old/piper/components/kinematics.py b/dimos/hardware/manipulators_old/piper/components/kinematics.py deleted file mode 100644 index 51be97a764..0000000000 --- a/dimos/hardware/manipulators_old/piper/components/kinematics.py +++ /dev/null @@ -1,116 +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. - -""" -Kinematics Component for PiperDriver. - -Provides RPC methods for kinematic calculations including: -- Forward kinematics -""" - -from typing import Any - -from dimos.core import rpc -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -class KinematicsComponent: - """ - Component providing kinematics RPC methods for PiperDriver. - - This component assumes the parent class has: - - self.piper: C_PiperInterface_V2 instance - - self.config: PiperDriverConfig instance - - PIPER_TO_RAD: conversion constant (0.001 degrees → radians) - """ - - # Type hints for attributes provided by parent class - piper: Any - config: Any - - @rpc - def get_forward_kinematics( - self, mode: str = "feedback" - ) -> tuple[bool, dict[str, float] | None]: - """ - Compute forward kinematics. - - Args: - mode: "feedback" for current joint angles, "control" for commanded angles - - Returns: - Tuple of (success, pose_dict) with keys: x, y, z, rx, ry, rz - """ - try: - fk_result = self.piper.GetFK(mode=mode) - - if fk_result is not None: - # Convert from Piper units - pose_dict = { - "x": fk_result[0] * 0.001, # 0.001 mm → mm - "y": fk_result[1] * 0.001, - "z": fk_result[2] * 0.001, - "rx": fk_result[3] * 0.001 * (3.14159 / 180.0), # → rad - "ry": fk_result[4] * 0.001 * (3.14159 / 180.0), - "rz": fk_result[5] * 0.001 * (3.14159 / 180.0), - } - return (True, pose_dict) - else: - return (False, None) - - except Exception as e: - logger.error(f"get_forward_kinematics failed: {e}") - return (False, None) - - @rpc - def enable_fk_calculation(self) -> tuple[bool, str]: - """ - Enable forward kinematics calculation. - - Returns: - Tuple of (success, message) - """ - try: - result = self.piper.EnableFkCal() - - if result: - return (True, "FK calculation enabled") - else: - return (False, "Failed to enable FK calculation") - - except Exception as e: - logger.error(f"enable_fk_calculation failed: {e}") - return (False, str(e)) - - @rpc - def disable_fk_calculation(self) -> tuple[bool, str]: - """ - Disable forward kinematics calculation. - - Returns: - Tuple of (success, message) - """ - try: - result = self.piper.DisableFkCal() - - if result: - return (True, "FK calculation disabled") - else: - return (False, "Failed to disable FK calculation") - - except Exception as e: - logger.error(f"disable_fk_calculation failed: {e}") - return (False, str(e)) diff --git a/dimos/hardware/manipulators_old/piper/components/motion_control.py b/dimos/hardware/manipulators_old/piper/components/motion_control.py deleted file mode 100644 index 7a0dc36eed..0000000000 --- a/dimos/hardware/manipulators_old/piper/components/motion_control.py +++ /dev/null @@ -1,286 +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. - -""" -Motion Control Component for PiperDriver. - -Provides RPC methods for motion control operations including: -- Joint position control -- Joint velocity control -- End-effector pose control -- Emergency stop -- Circular motion -""" - -import math -import time -from typing import Any - -from dimos.core import rpc -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -class MotionControlComponent: - """ - Component providing motion control RPC methods for PiperDriver. - - This component assumes the parent class has: - - self.piper: C_PiperInterface_V2 instance - - self.config: PiperDriverConfig instance - - RAD_TO_PIPER: conversion constant (radians → 0.001 degrees) - - PIPER_TO_RAD: conversion constant (0.001 degrees → radians) - """ - - # Type hints for attributes expected from parent class - piper: Any - config: Any - RAD_TO_PIPER: float - PIPER_TO_RAD: float - _joint_cmd_lock: Any - _joint_cmd_: Any - _vel_cmd_: Any - _last_cmd_time: float - - @rpc - def set_joint_angles(self, angles: list[float], gripper_state: int = 0x00) -> tuple[bool, str]: - """ - Set joint angles (RPC method). - - Args: - angles: List of joint angles in radians - gripper_state: Gripper state (0x00 = no change, 0x01 = open, 0x02 = close) - - Returns: - Tuple of (success, message) - """ - try: - if len(angles) != 6: - return (False, f"Expected 6 joint angles, got {len(angles)}") - - # Convert radians to Piper units (0.001 degrees) - piper_joints = [round(rad * self.RAD_TO_PIPER) for rad in angles] - - # Send joint control command - result = self.piper.JointCtrl( - piper_joints[0], - piper_joints[1], - piper_joints[2], - piper_joints[3], - piper_joints[4], - piper_joints[5], - gripper_state, - ) - - if result: - return (True, "Joint angles set successfully") - else: - return (False, "Failed to set joint angles") - - except Exception as e: - logger.error(f"set_joint_angles failed: {e}") - return (False, str(e)) - - @rpc - def set_joint_command(self, positions: list[float]) -> tuple[bool, str]: - """ - Manually set the joint command (for testing). - This updates the shared joint_cmd that the control loop reads. - - Args: - positions: List of joint positions in radians - - Returns: - Tuple of (success, message) - """ - try: - if len(positions) != 6: - return (False, f"Expected 6 joint positions, got {len(positions)}") - - with self._joint_cmd_lock: - self._joint_cmd_ = list(positions) - - logger.info(f"✓ Joint command set: {[f'{math.degrees(p):.2f}°' for p in positions]}") - return (True, "Joint command updated") - except Exception as e: - return (False, str(e)) - - @rpc - def set_end_pose( - self, x: float, y: float, z: float, rx: float, ry: float, rz: float - ) -> tuple[bool, str]: - """ - Set end-effector pose. - - Args: - x: X position in millimeters - y: Y position in millimeters - z: Z position in millimeters - rx: Roll in radians - ry: Pitch in radians - rz: Yaw in radians - - Returns: - Tuple of (success, message) - """ - try: - # Convert to Piper units - # Position: mm → 0.001 mm - x_piper = round(x * 1000) - y_piper = round(y * 1000) - z_piper = round(z * 1000) - - # Rotation: radians → 0.001 degrees - rx_piper = round(math.degrees(rx) * 1000) - ry_piper = round(math.degrees(ry) * 1000) - rz_piper = round(math.degrees(rz) * 1000) - - # Send end pose control command - result = self.piper.EndPoseCtrl(x_piper, y_piper, z_piper, rx_piper, ry_piper, rz_piper) - - if result: - return (True, "End pose set successfully") - else: - return (False, "Failed to set end pose") - - except Exception as e: - logger.error(f"set_end_pose failed: {e}") - return (False, str(e)) - - @rpc - def emergency_stop(self) -> tuple[bool, str]: - """Emergency stop the arm.""" - try: - result = self.piper.EmergencyStop() - - if result: - logger.warning("Emergency stop activated") - return (True, "Emergency stop activated") - else: - return (False, "Failed to activate emergency stop") - - except Exception as e: - logger.error(f"emergency_stop failed: {e}") - return (False, str(e)) - - @rpc - def move_c_axis_update(self, instruction_num: int = 0x00) -> tuple[bool, str]: - """ - Update circular motion axis. - - Args: - instruction_num: Instruction number (0x00, 0x01, 0x02, 0x03) - - Returns: - Tuple of (success, message) - """ - try: - if instruction_num not in [0x00, 0x01, 0x02, 0x03]: - return (False, f"Invalid instruction_num: {instruction_num}") - - result = self.piper.MoveCAxisUpdateCtrl(instruction_num) - - if result: - return (True, f"Move C axis updated with instruction {instruction_num}") - else: - return (False, "Failed to update Move C axis") - - except Exception as e: - logger.error(f"move_c_axis_update failed: {e}") - return (False, str(e)) - - @rpc - def set_joint_mit_ctrl( - self, - motor_num: int, - pos_target: float, - vel_target: float, - torq_target: float, - kp: int, - kd: int, - ) -> tuple[bool, str]: - """ - Set joint MIT (Model-based Inverse Torque) control. - - Args: - motor_num: Motor number (1-6) - pos_target: Target position in radians - vel_target: Target velocity in rad/s - torq_target: Target torque in Nm - kp: Proportional gain (0-100) - kd: Derivative gain (0-100) - - Returns: - Tuple of (success, message) - """ - try: - if motor_num not in range(1, 7): - return (False, f"Invalid motor_num: {motor_num}. Must be 1-6") - - # Convert to Piper units - pos_piper = round(pos_target * self.RAD_TO_PIPER) - vel_piper = round(vel_target * self.RAD_TO_PIPER) - torq_piper = round(torq_target * 1000) # Torque in millinewton-meters - - result = self.piper.JointMitCtrl(motor_num, pos_piper, vel_piper, torq_piper, kp, kd) - - if result: - return (True, f"Joint {motor_num} MIT control set successfully") - else: - return (False, f"Failed to set MIT control for joint {motor_num}") - - except Exception as e: - logger.error(f"set_joint_mit_ctrl failed: {e}") - return (False, str(e)) - - @rpc - def set_joint_velocities(self, velocities: list[float]) -> tuple[bool, str]: - """ - Set joint velocities (RPC method). - - Requires velocity control mode to be enabled. - - The control loop integrates velocities to positions: - - position_target += velocity * dt - - Integrated positions are sent to JointCtrl - - This provides smooth velocity control while using the proven position API. - - Args: - velocities: List of 6 joint velocities in rad/s - - Returns: - Tuple of (success, message) - """ - try: - if len(velocities) != 6: - return (False, f"Expected 6 velocities, got {len(velocities)}") - - if not self.config.velocity_control: - return ( - False, - "Velocity control mode not enabled. Call enable_velocity_control_mode() first.", - ) - - with self._joint_cmd_lock: - self._vel_cmd_ = list(velocities) - self._last_cmd_time = time.time() - - logger.info(f"✓ Velocity command set: {[f'{v:.3f} rad/s' for v in velocities]}") - return (True, "Velocity command updated") - - except Exception as e: - logger.error(f"set_joint_velocities failed: {e}") - return (False, str(e)) diff --git a/dimos/hardware/manipulators_old/piper/components/state_queries.py b/dimos/hardware/manipulators_old/piper/components/state_queries.py deleted file mode 100644 index 3fe00fffc6..0000000000 --- a/dimos/hardware/manipulators_old/piper/components/state_queries.py +++ /dev/null @@ -1,340 +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. - -""" -State Query Component for PiperDriver. - -Provides RPC methods for querying robot state including: -- Joint state -- Robot state -- End-effector pose -- Gripper state -- Motor information -- Firmware version -""" - -import threading -from typing import Any - -from dimos.core import rpc -from dimos.msgs.sensor_msgs import JointState, RobotState -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -class StateQueryComponent: - """ - Component providing state query RPC methods for PiperDriver. - - This component assumes the parent class has: - - self.piper: C_PiperInterface_V2 instance - - self.config: PiperDriverConfig instance - - self._joint_state_lock: threading.Lock - - self._joint_states_: Optional[JointState] - - self._robot_state_: Optional[RobotState] - - PIPER_TO_RAD: conversion constant (0.001 degrees → radians) - """ - - # Type hints for attributes expected from parent class - piper: Any # C_PiperInterface_V2 instance - config: Any # Config dict accessed as object - _joint_state_lock: threading.Lock - _joint_states_: JointState | None - _robot_state_: RobotState | None - PIPER_TO_RAD: float - - @rpc - def get_joint_state(self) -> JointState | None: - """ - Get the current joint state (RPC method). - - Returns: - Current JointState or None - """ - with self._joint_state_lock: - return self._joint_states_ - - @rpc - def get_robot_state(self) -> RobotState | None: - """ - Get the current robot state (RPC method). - - Returns: - Current RobotState or None - """ - with self._joint_state_lock: - return self._robot_state_ - - @rpc - def get_arm_status(self) -> tuple[bool, dict[str, Any] | None]: - """ - Get arm status. - - Returns: - Tuple of (success, status_dict) - """ - try: - status = self.piper.GetArmStatus() - - if status is not None: - status_dict = { - "time_stamp": status.time_stamp, - "Hz": status.Hz, - "motion_mode": status.arm_status.motion_mode, - "mode_feedback": status.arm_status.mode_feedback, - "teach_status": status.arm_status.teach_status, - "motion_status": status.arm_status.motion_status, - "trajectory_num": status.arm_status.trajectory_num, - } - return (True, status_dict) - else: - return (False, None) - - except Exception as e: - logger.error(f"get_arm_status failed: {e}") - return (False, None) - - @rpc - def get_arm_joint_angles(self) -> tuple[bool, list[float] | None]: - """ - Get arm joint angles in radians. - - Returns: - Tuple of (success, joint_angles) - """ - try: - arm_joint = self.piper.GetArmJointMsgs() - - if arm_joint is not None: - # Convert from Piper units (0.001 degrees) to radians - angles = [ - arm_joint.joint_state.joint_1 * self.PIPER_TO_RAD, - arm_joint.joint_state.joint_2 * self.PIPER_TO_RAD, - arm_joint.joint_state.joint_3 * self.PIPER_TO_RAD, - arm_joint.joint_state.joint_4 * self.PIPER_TO_RAD, - arm_joint.joint_state.joint_5 * self.PIPER_TO_RAD, - arm_joint.joint_state.joint_6 * self.PIPER_TO_RAD, - ] - return (True, angles) - else: - return (False, None) - - except Exception as e: - logger.error(f"get_arm_joint_angles failed: {e}") - return (False, None) - - @rpc - def get_end_pose(self) -> tuple[bool, dict[str, float] | None]: - """ - Get end-effector pose. - - Returns: - Tuple of (success, pose_dict) with keys: x, y, z, rx, ry, rz - """ - try: - end_pose = self.piper.GetArmEndPoseMsgs() - - if end_pose is not None: - # Convert from Piper units - pose_dict = { - "x": end_pose.end_pose.end_pose_x * 0.001, # 0.001 mm → mm - "y": end_pose.end_pose.end_pose_y * 0.001, - "z": end_pose.end_pose.end_pose_z * 0.001, - "rx": end_pose.end_pose.end_pose_rx * 0.001 * (3.14159 / 180.0), # → rad - "ry": end_pose.end_pose.end_pose_ry * 0.001 * (3.14159 / 180.0), - "rz": end_pose.end_pose.end_pose_rz * 0.001 * (3.14159 / 180.0), - "time_stamp": end_pose.time_stamp, - "Hz": end_pose.Hz, - } - return (True, pose_dict) - else: - return (False, None) - - except Exception as e: - logger.error(f"get_end_pose failed: {e}") - return (False, None) - - @rpc - def get_gripper_state(self) -> tuple[bool, dict[str, Any] | None]: - """ - Get gripper state. - - Returns: - Tuple of (success, gripper_dict) - """ - try: - gripper = self.piper.GetArmGripperMsgs() - - if gripper is not None: - gripper_dict = { - "gripper_angle": gripper.gripper_state.grippers_angle, - "gripper_effort": gripper.gripper_state.grippers_effort, - "gripper_enable": gripper.gripper_state.grippers_enabled, - "time_stamp": gripper.time_stamp, - "Hz": gripper.Hz, - } - return (True, gripper_dict) - else: - return (False, None) - - except Exception as e: - logger.error(f"get_gripper_state failed: {e}") - return (False, None) - - @rpc - def get_arm_enable_status(self) -> tuple[bool, list[int] | None]: - """ - Get arm enable status for all joints. - - Returns: - Tuple of (success, enable_status_list) - """ - try: - enable_status = self.piper.GetArmEnableStatus() - - if enable_status is not None: - return (True, enable_status) - else: - return (False, None) - - except Exception as e: - logger.error(f"get_arm_enable_status failed: {e}") - return (False, None) - - @rpc - def get_firmware_version(self) -> tuple[bool, str | None]: - """ - Get Piper firmware version. - - Returns: - Tuple of (success, version_string) - """ - try: - version = self.piper.GetPiperFirmwareVersion() - - if version is not None: - return (True, version) - else: - return (False, None) - - except Exception as e: - logger.error(f"get_firmware_version failed: {e}") - return (False, None) - - @rpc - def get_sdk_version(self) -> tuple[bool, str | None]: - """ - Get Piper SDK version. - - Returns: - Tuple of (success, version_string) - """ - try: - version = self.piper.GetCurrentSDKVersion() - - if version is not None: - return (True, version) - else: - return (False, None) - - except Exception: - return (False, None) - - @rpc - def get_interface_version(self) -> tuple[bool, str | None]: - """ - Get Piper interface version. - - Returns: - Tuple of (success, version_string) - """ - try: - version = self.piper.GetCurrentInterfaceVersion() - - if version is not None: - return (True, version) - else: - return (False, None) - - except Exception: - return (False, None) - - @rpc - def get_protocol_version(self) -> tuple[bool, str | None]: - """ - Get Piper protocol version. - - Returns: - Tuple of (success, version_string) - """ - try: - version = self.piper.GetCurrentProtocolVersion() - - if version is not None: - return (True, version) - else: - return (False, None) - - except Exception: - return (False, None) - - @rpc - def get_can_fps(self) -> tuple[bool, float | None]: - """ - Get CAN bus FPS (frames per second). - - Returns: - Tuple of (success, fps_value) - """ - try: - fps = self.piper.GetCanFps() - - if fps is not None: - return (True, fps) - else: - return (False, None) - - except Exception as e: - logger.error(f"get_can_fps failed: {e}") - return (False, None) - - @rpc - def get_motor_max_acc_limit(self) -> tuple[bool, dict[str, Any] | None]: - """ - Get maximum acceleration limit for all motors. - - Returns: - Tuple of (success, acc_limit_dict) - """ - try: - acc_limit = self.piper.GetCurrentMotorMaxAccLimit() - - if acc_limit is not None: - acc_dict = { - "motor_1": acc_limit.current_motor_max_acc_limit.motor_1_max_acc_limit, - "motor_2": acc_limit.current_motor_max_acc_limit.motor_2_max_acc_limit, - "motor_3": acc_limit.current_motor_max_acc_limit.motor_3_max_acc_limit, - "motor_4": acc_limit.current_motor_max_acc_limit.motor_4_max_acc_limit, - "motor_5": acc_limit.current_motor_max_acc_limit.motor_5_max_acc_limit, - "motor_6": acc_limit.current_motor_max_acc_limit.motor_6_max_acc_limit, - "time_stamp": acc_limit.time_stamp, - } - return (True, acc_dict) - else: - return (False, None) - - except Exception as e: - logger.error(f"get_motor_max_acc_limit failed: {e}") - return (False, None) diff --git a/dimos/hardware/manipulators_old/piper/components/system_control.py b/dimos/hardware/manipulators_old/piper/components/system_control.py deleted file mode 100644 index a15eb29133..0000000000 --- a/dimos/hardware/manipulators_old/piper/components/system_control.py +++ /dev/null @@ -1,395 +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. - -""" -System Control Component for PiperDriver. - -Provides RPC methods for system-level control operations including: -- Enable/disable arm -- Mode control (drag teach, MIT control, etc.) -- Motion control -- Master/slave configuration -""" - -from typing import Any - -from dimos.core import rpc -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -class SystemControlComponent: - """ - Component providing system control RPC methods for PiperDriver. - - This component assumes the parent class has: - - self.piper: C_PiperInterface_V2 instance - - self.config: PiperDriverConfig instance - """ - - # Type hints for attributes expected from parent class - piper: Any # C_PiperInterface_V2 instance - config: Any # Config dict accessed as object - - @rpc - def enable_servo_mode(self) -> tuple[bool, str]: - """ - Enable servo mode. - This enables the arm to receive motion commands. - - Returns: - Tuple of (success, message) - """ - try: - result = self.piper.EnableArm() - - if result: - logger.info("Servo mode enabled") - return (True, "Servo mode enabled") - else: - logger.warning("Failed to enable servo mode") - return (False, "Failed to enable servo mode") - - except Exception as e: - logger.error(f"enable_servo_mode failed: {e}") - return (False, str(e)) - - @rpc - def disable_servo_mode(self) -> tuple[bool, str]: - """ - Disable servo mode. - - Returns: - Tuple of (success, message) - """ - try: - result = self.piper.DisableArm() - - if result: - logger.info("Servo mode disabled") - return (True, "Servo mode disabled") - else: - logger.warning("Failed to disable servo mode") - return (False, "Failed to disable servo mode") - - except Exception as e: - logger.error(f"disable_servo_mode failed: {e}") - return (False, str(e)) - - @rpc - def motion_enable(self, enable: bool = True) -> tuple[bool, str]: - """Enable or disable arm motion.""" - try: - if enable: - result = self.piper.EnableArm() - msg = "Motion enabled" - else: - result = self.piper.DisableArm() - msg = "Motion disabled" - - if result: - return (True, msg) - else: - return (False, f"Failed to {msg.lower()}") - - except Exception as e: - return (False, str(e)) - - @rpc - def set_motion_ctrl_1( - self, - ctrl_mode: int = 0x00, - move_mode: int = 0x00, - move_spd_rate: int = 50, - coor_mode: int = 0x00, - reference_joint: int = 0x00, - ) -> tuple[bool, str]: - """ - Set motion control parameters (MotionCtrl_1). - - Args: - ctrl_mode: Control mode - move_mode: Movement mode - move_spd_rate: Movement speed rate (0-100) - coor_mode: Coordinate mode - reference_joint: Reference joint - - Returns: - Tuple of (success, message) - """ - try: - result = self.piper.MotionCtrl_1( - ctrl_mode, move_mode, move_spd_rate, coor_mode, reference_joint - ) - - if result: - return (True, "Motion control 1 parameters set successfully") - else: - return (False, "Failed to set motion control 1 parameters") - - except Exception as e: - logger.error(f"set_motion_ctrl_1 failed: {e}") - return (False, str(e)) - - @rpc - def set_motion_ctrl_2( - self, - limit_fun_en: int = 0x00, - collis_detect_en: int = 0x00, - friction_feed_en: int = 0x00, - gravity_feed_en: int = 0x00, - is_mit_mode: int = 0x00, - ) -> tuple[bool, str]: - """ - Set motion control parameters (MotionCtrl_2). - - Args: - limit_fun_en: Limit function enable (0x00 = disabled, 0x01 = enabled) - collis_detect_en: Collision detection enable - friction_feed_en: Friction compensation enable - gravity_feed_en: Gravity compensation enable - is_mit_mode: MIT mode enable (0x00 = disabled, 0x01 = enabled) - - Returns: - Tuple of (success, message) - """ - try: - result = self.piper.MotionCtrl_2( - limit_fun_en, - collis_detect_en, - friction_feed_en, - gravity_feed_en, - is_mit_mode, - ) - - if result: - return (True, "Motion control 2 parameters set successfully") - else: - return (False, "Failed to set motion control 2 parameters") - - except Exception as e: - logger.error(f"set_motion_ctrl_2 failed: {e}") - return (False, str(e)) - - @rpc - def set_mode_ctrl( - self, - drag_teach_en: int = 0x00, - teach_record_en: int = 0x00, - ) -> tuple[bool, str]: - """ - Set mode control (drag teaching, recording, etc.). - - Args: - drag_teach_en: Drag teaching enable (0x00 = disabled, 0x01 = enabled) - teach_record_en: Teaching record enable - - Returns: - Tuple of (success, message) - """ - try: - result = self.piper.ModeCtrl(drag_teach_en, teach_record_en) - - if result: - mode_str = [] - if drag_teach_en == 0x01: - mode_str.append("drag teaching") - if teach_record_en == 0x01: - mode_str.append("recording") - - if mode_str: - return (True, f"Mode control set: {', '.join(mode_str)} enabled") - else: - return (True, "Mode control set: all modes disabled") - else: - return (False, "Failed to set mode control") - - except Exception as e: - logger.error(f"set_mode_ctrl failed: {e}") - return (False, str(e)) - - @rpc - def configure_master_slave( - self, - linkage_config: int, - feedback_offset: int, - ctrl_offset: int, - linkage_offset: int, - ) -> tuple[bool, str]: - """ - Configure master/slave linkage. - - Args: - linkage_config: Linkage configuration - feedback_offset: Feedback offset - ctrl_offset: Control offset - linkage_offset: Linkage offset - - Returns: - Tuple of (success, message) - """ - try: - result = self.piper.MasterSlaveConfig( - linkage_config, feedback_offset, ctrl_offset, linkage_offset - ) - - if result: - return (True, "Master/slave configuration set successfully") - else: - return (False, "Failed to set master/slave configuration") - - except Exception as e: - logger.error(f"configure_master_slave failed: {e}") - return (False, str(e)) - - @rpc - def search_firmware_version(self) -> tuple[bool, str]: - """ - Search for firmware version. - - Returns: - Tuple of (success, message) - """ - try: - result = self.piper.SearchPiperFirmwareVersion() - - if result: - return (True, "Firmware version search initiated") - else: - return (False, "Failed to search firmware version") - - except Exception as e: - logger.error(f"search_firmware_version failed: {e}") - return (False, str(e)) - - @rpc - def piper_init(self) -> tuple[bool, str]: - """ - Initialize Piper arm. - - Returns: - Tuple of (success, message) - """ - try: - result = self.piper.PiperInit() - - if result: - logger.info("Piper initialized") - return (True, "Piper initialized successfully") - else: - logger.warning("Failed to initialize Piper") - return (False, "Failed to initialize Piper") - - except Exception as e: - logger.error(f"piper_init failed: {e}") - return (False, str(e)) - - @rpc - def enable_piper(self) -> tuple[bool, str]: - """ - Enable Piper (convenience method). - - Returns: - Tuple of (success, message) - """ - try: - result = self.piper.EnablePiper() - - if result: - logger.info("Piper enabled") - return (True, "Piper enabled") - else: - logger.warning("Failed to enable Piper") - return (False, "Failed to enable Piper") - - except Exception as e: - logger.error(f"enable_piper failed: {e}") - return (False, str(e)) - - @rpc - def disable_piper(self) -> tuple[bool, str]: - """ - Disable Piper (convenience method). - - Returns: - Tuple of (success, message) - """ - try: - result = self.piper.DisablePiper() - - if result: - logger.info("Piper disabled") - return (True, "Piper disabled") - else: - logger.warning("Failed to disable Piper") - return (False, "Failed to disable Piper") - - except Exception as e: - logger.error(f"disable_piper failed: {e}") - return (False, str(e)) - - # ========================================================================= - # Velocity Control Mode - # ========================================================================= - - @rpc - def enable_velocity_control_mode(self) -> tuple[bool, str]: - """ - Enable velocity control mode (integration-based). - - This switches the control loop to use velocity integration: - - Velocity commands are integrated: position_target += velocity * dt - - Integrated positions are sent to JointCtrl (standard position control) - - Provides smooth velocity control interface while using proven position API - - Returns: - Tuple of (success, message) - """ - try: - # Set config flag to enable velocity control - # The control loop will integrate velocities to positions - self.config.velocity_control = True - - logger.info("Velocity control mode enabled (integration-based)") - return (True, "Velocity control mode enabled") - - except Exception as e: - logger.error(f"enable_velocity_control_mode failed: {e}") - self.config.velocity_control = False # Revert on exception - return (False, str(e)) - - @rpc - def disable_velocity_control_mode(self) -> tuple[bool, str]: - """ - Disable velocity control mode and return to position control. - - Returns: - Tuple of (success, message) - """ - try: - # Set config flag to disable velocity control - # The control loop will switch back to standard position control mode - self.config.velocity_control = False - - # Reset position target to allow re-initialization when re-enabled - self._position_target_ = None - - logger.info("Position control mode enabled (velocity mode disabled)") - return (True, "Position control mode enabled") - - except Exception as e: - logger.error(f"disable_velocity_control_mode failed: {e}") - self.config.velocity_control = True # Revert on exception - return (False, str(e)) diff --git a/dimos/hardware/manipulators_old/piper/piper_blueprints.py b/dimos/hardware/manipulators_old/piper/piper_blueprints.py deleted file mode 100644 index 1145616841..0000000000 --- a/dimos/hardware/manipulators_old/piper/piper_blueprints.py +++ /dev/null @@ -1,172 +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. - -""" -Blueprints for Piper manipulator control systems. - -This module provides declarative blueprints for configuring Piper servo control, -following the same pattern used for xArm and other manipulators. - -Usage: - # Run via CLI: - dimos run piper-servo # Driver only - dimos run piper-cartesian # Driver + Cartesian motion controller - dimos run piper-trajectory # Driver + Joint trajectory controller - - # Or programmatically: - from dimos.hardware.manipulators.piper.piper_blueprints import piper_servo - coordinator = piper_servo.build() - coordinator.loop() -""" - -from typing import Any - -from dimos.core.blueprints import autoconnect -from dimos.core.transport import LCMTransport -from dimos.hardware.manipulators.piper.piper_driver import piper_driver as piper_driver_blueprint -from dimos.manipulation.control import cartesian_motion_controller, joint_trajectory_controller -from dimos.msgs.geometry_msgs import PoseStamped -from dimos.msgs.sensor_msgs import ( - JointCommand, - JointState, - RobotState, -) -from dimos.msgs.trajectory_msgs import JointTrajectory - - -# Create a blueprint wrapper for the component-based driver -def piper_driver(**config: Any) -> Any: - """Create a blueprint for PiperDriver. - - Args: - **config: Configuration parameters passed to PiperDriver - - can_port: CAN interface name (default: "can0") - - has_gripper: Whether gripper is attached (default: True) - - enable_on_start: Whether to enable servos on start (default: True) - - control_rate: Control loop + joint feedback rate in Hz (default: 100) - - monitor_rate: Robot state monitoring rate in Hz (default: 10) - - Returns: - Blueprint configuration for PiperDriver - """ - # Set defaults - config.setdefault("can_port", "can0") - config.setdefault("has_gripper", True) - config.setdefault("enable_on_start", True) - config.setdefault("control_rate", 100) - config.setdefault("monitor_rate", 10) - - # Return the piper_driver blueprint with the config - return piper_driver_blueprint(**config) - - -# ============================================================================= -# Piper Servo Control Blueprint -# ============================================================================= -# PiperDriver configured for servo control mode using component-based architecture. -# Publishes joint states and robot state, listens for joint commands. -# ============================================================================= - -piper_servo = piper_driver( - can_port="can0", - has_gripper=True, - enable_on_start=True, - control_rate=100, - monitor_rate=10, -).transports( - { - # Joint state feedback (position, velocity, effort) - ("joint_state", JointState): LCMTransport("/piper/joint_states", JointState), - # Robot state feedback (mode, state, errors) - ("robot_state", RobotState): LCMTransport("/piper/robot_state", RobotState), - # Position commands input - ("joint_position_command", JointCommand): LCMTransport( - "/piper/joint_position_command", JointCommand - ), - # Velocity commands input - ("joint_velocity_command", JointCommand): LCMTransport( - "/piper/joint_velocity_command", JointCommand - ), - } -) - -# ============================================================================= -# Piper Cartesian Control Blueprint (Driver + Controller) -# ============================================================================= -# Combines PiperDriver with CartesianMotionController for Cartesian space control. -# The controller receives target_pose and converts to joint commands via IK. -# ============================================================================= - -piper_cartesian = autoconnect( - piper_driver( - can_port="can0", - has_gripper=True, - enable_on_start=True, - control_rate=100, - monitor_rate=10, - ), - cartesian_motion_controller( - control_frequency=20.0, - position_kp=5.0, - position_ki=0.0, - position_kd=0.1, - max_linear_velocity=0.2, - max_angular_velocity=1.0, - ), -).transports( - { - # Shared topics between driver and controller - ("joint_state", JointState): LCMTransport("/piper/joint_states", JointState), - ("robot_state", RobotState): LCMTransport("/piper/robot_state", RobotState), - ("joint_position_command", JointCommand): LCMTransport( - "/piper/joint_position_command", JointCommand - ), - # Controller-specific topics - ("target_pose", PoseStamped): LCMTransport("/target_pose", PoseStamped), - ("current_pose", PoseStamped): LCMTransport("/piper/current_pose", PoseStamped), - } -) - -# ============================================================================= -# Piper Trajectory Control Blueprint (Driver + Trajectory Controller) -# ============================================================================= -# Combines PiperDriver with JointTrajectoryController for trajectory execution. -# The controller receives JointTrajectory messages and executes them at 100Hz. -# ============================================================================= - -piper_trajectory = autoconnect( - piper_driver( - can_port="can0", - has_gripper=True, - enable_on_start=True, - control_rate=100, - monitor_rate=10, - ), - joint_trajectory_controller( - control_frequency=100.0, - ), -).transports( - { - # Shared topics between driver and controller - ("joint_state", JointState): LCMTransport("/piper/joint_states", JointState), - ("robot_state", RobotState): LCMTransport("/piper/robot_state", RobotState), - ("joint_position_command", JointCommand): LCMTransport( - "/piper/joint_position_command", JointCommand - ), - # Trajectory input topic - ("trajectory", JointTrajectory): LCMTransport("/trajectory", JointTrajectory), - } -) - -__all__ = ["piper_cartesian", "piper_servo", "piper_trajectory"] diff --git a/dimos/hardware/manipulators_old/piper/piper_description.urdf b/dimos/hardware/manipulators_old/piper/piper_description.urdf deleted file mode 100755 index c8a5a11ded..0000000000 --- a/dimos/hardware/manipulators_old/piper/piper_description.urdf +++ /dev/null @@ -1,497 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/dimos/hardware/manipulators_old/piper/piper_driver.py b/dimos/hardware/manipulators_old/piper/piper_driver.py deleted file mode 100644 index 5730a4394a..0000000000 --- a/dimos/hardware/manipulators_old/piper/piper_driver.py +++ /dev/null @@ -1,241 +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. - -"""Piper driver using the generalized component-based architecture.""" - -import logging -import time -from typing import Any - -from dimos.hardware.manipulators.base import ( - BaseManipulatorDriver, - StandardMotionComponent, - StandardServoComponent, - StandardStatusComponent, -) - -from .piper_wrapper import PiperSDKWrapper - -logger = logging.getLogger(__name__) - - -class PiperDriver(BaseManipulatorDriver): - """Piper driver using component-based architecture. - - This driver supports the Piper 6-DOF manipulator via CAN bus. - All the complex logic is handled by the base class and standard components. - This file just assembles the pieces. - """ - - def __init__(self, **kwargs: Any) -> None: - """Initialize the Piper driver. - - Args: - **kwargs: Arguments for Module initialization. - Driver configuration can be passed via 'config' keyword arg: - - can_port: CAN interface name (e.g., 'can0') - - has_gripper: Whether gripper is attached - - enable_on_start: Whether to enable servos on start - """ - # Extract driver-specific config from kwargs - config: dict[str, Any] = kwargs.pop("config", {}) - - # Extract driver-specific params that might be passed directly - driver_params = [ - "can_port", - "has_gripper", - "enable_on_start", - "control_rate", - "monitor_rate", - ] - for param in driver_params: - if param in kwargs: - config[param] = kwargs.pop(param) - - logger.info(f"Initializing PiperDriver with config: {config}") - - # Create SDK wrapper - sdk = PiperSDKWrapper() - - # Create standard components - components = [ - StandardMotionComponent(sdk), - StandardServoComponent(sdk), - StandardStatusComponent(sdk), - ] - - # Optional: Add gripper component if configured - # if config.get('has_gripper', False): - # from dimos.hardware.manipulators.base.components import StandardGripperComponent - # components.append(StandardGripperComponent(sdk)) - - # Remove any kwargs that would conflict with explicit arguments - kwargs.pop("sdk", None) - kwargs.pop("components", None) - kwargs.pop("name", None) - - # Initialize base driver with SDK and components - super().__init__( - sdk=sdk, components=components, config=config, name="PiperDriver", **kwargs - ) - - # Initialize position target for velocity integration - self._position_target: list[float] | None = None - self._last_velocity_time: float = 0.0 - - # Enable on start if configured - if config.get("enable_on_start", False): - logger.info("Enabling Piper servos on start...") - servo_component = self.get_component(StandardServoComponent) - if servo_component: - result = servo_component.enable_servo() - if result["success"]: - logger.info("Piper servos enabled successfully") - else: - logger.warning(f"Failed to enable servos: {result.get('error')}") - - logger.info("PiperDriver initialized successfully") - - def _process_command(self, command: Any) -> None: - """Override to implement velocity control via position integration. - - Args: - command: Command to process - """ - # Handle velocity commands specially for Piper - if command.type == "velocity": - # Piper doesn't have native velocity control - integrate to position - current_time = time.time() - - # Initialize position target from current state on first velocity command - if self._position_target is None: - positions = self.shared_state.joint_positions - if positions: - self._position_target = list(positions) - logger.info( - f"Velocity control: Initialized position target from current state: {self._position_target}" - ) - else: - logger.warning("Cannot start velocity control - no current position available") - return - - # Calculate dt since last velocity command - if self._last_velocity_time > 0: - dt = current_time - self._last_velocity_time - else: - dt = 1.0 / self.control_rate # Use nominal period for first command - - self._last_velocity_time = current_time - - # Integrate velocity to position: pos += vel * dt - velocities = command.data["velocities"] - for i in range(min(len(velocities), len(self._position_target))): - self._position_target[i] += velocities[i] * dt - - # Send integrated position command - success = self.sdk.set_joint_positions( - self._position_target, - velocity=1.0, # Use max velocity for responsiveness - acceleration=1.0, - wait=False, - ) - - if success: - self.shared_state.target_positions = self._position_target - self.shared_state.target_velocities = velocities - - else: - # Reset velocity integration when switching to position mode - if command.type == "position": - self._position_target = None - self._last_velocity_time = 0.0 - - # Use base implementation for other command types - super()._process_command(command) - - -# Blueprint configuration for the driver -def get_blueprint() -> dict[str, Any]: - """Get the blueprint configuration for the Piper driver. - - Returns: - Dictionary with blueprint configuration - """ - return { - "name": "PiperDriver", - "class": PiperDriver, - "config": { - "can_port": "can0", # Default CAN interface - "has_gripper": True, # Piper usually has gripper - "enable_on_start": True, # Enable servos on startup - "control_rate": 100, # Hz - control loop + joint feedback - "monitor_rate": 10, # Hz - robot state monitoring - }, - "inputs": { - "joint_position_command": "JointCommand", - "joint_velocity_command": "JointCommand", - }, - "outputs": { - "joint_state": "JointState", - "robot_state": "RobotState", - }, - "rpc_methods": [ - # Motion control - "move_joint", - "move_joint_velocity", - "move_joint_effort", - "stop_motion", - "get_joint_state", - "get_joint_limits", - "get_velocity_limits", - "set_velocity_scale", - "set_acceleration_scale", - "move_cartesian", - "get_cartesian_state", - "execute_trajectory", - "stop_trajectory", - # Servo control - "enable_servo", - "disable_servo", - "toggle_servo", - "get_servo_state", - "emergency_stop", - "reset_emergency_stop", - "set_control_mode", - "get_control_mode", - "clear_errors", - "reset_fault", - "home_robot", - "brake_release", - "brake_engage", - # Status monitoring - "get_robot_state", - "get_system_info", - "get_capabilities", - "get_error_state", - "get_health_metrics", - "get_statistics", - "check_connection", - "get_force_torque", - "zero_force_torque", - "get_digital_inputs", - "set_digital_outputs", - "get_analog_inputs", - "get_gripper_state", - ], - } - - -# Expose blueprint for declarative composition (compatible with dimos framework) -piper_driver = PiperDriver.blueprint diff --git a/dimos/hardware/manipulators_old/piper/piper_wrapper.py b/dimos/hardware/manipulators_old/piper/piper_wrapper.py deleted file mode 100644 index 7384f6c06e..0000000000 --- a/dimos/hardware/manipulators_old/piper/piper_wrapper.py +++ /dev/null @@ -1,671 +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. - -"""Piper SDK wrapper implementation.""" - -import logging -import time -from typing import Any - -from ..base.sdk_interface import BaseManipulatorSDK, ManipulatorInfo - -# Unit conversion constants -RAD_TO_PIPER = 57295.7795 # radians to Piper units (0.001 degrees) -PIPER_TO_RAD = 1.0 / RAD_TO_PIPER # Piper units to radians - - -class PiperSDKWrapper(BaseManipulatorSDK): - """SDK wrapper for Piper manipulators. - - This wrapper translates Piper's native SDK (which uses radians but 1-indexed joints) - to our standard interface (0-indexed). - """ - - def __init__(self) -> None: - """Initialize the Piper SDK wrapper.""" - self.logger = logging.getLogger(self.__class__.__name__) - self.native_sdk: Any = None - self.dof = 6 # Piper is always 6-DOF - self._connected = False - self._enabled = False - - # ============= Connection Management ============= - - def connect(self, config: dict[str, Any]) -> bool: - """Connect to Piper via CAN bus. - - Args: - config: Configuration with 'can_port' (e.g., 'can0') - - Returns: - True if connection successful - """ - try: - from piper_sdk import C_PiperInterface_V2 - - can_port = config.get("can_port", "can0") - self.logger.info(f"Connecting to Piper via CAN port {can_port}...") - - # Create Piper SDK instance - self.native_sdk = C_PiperInterface_V2( - can_name=can_port, - judge_flag=True, # Enable safety checks - can_auto_init=True, # Let SDK handle CAN initialization - dh_is_offset=False, - ) - - # Connect to CAN port - self.native_sdk.ConnectPort(piper_init=True, start_thread=True) - - # Wait for initialization - time.sleep(0.025) - - # Check connection by trying to get status - status = self.native_sdk.GetArmStatus() - if status is not None: - self._connected = True - - # Get firmware version - try: - version = self.native_sdk.GetPiperFirmwareVersion() - self.logger.info(f"Connected to Piper (firmware: {version})") - except: - self.logger.info("Connected to Piper") - - return True - else: - self.logger.error("Failed to connect to Piper - no status received") - return False - - except ImportError: - self.logger.error("Piper SDK not installed. Please install piper_sdk") - return False - except Exception as e: - self.logger.error(f"Connection failed: {e}") - return False - - def disconnect(self) -> None: - """Disconnect from Piper.""" - if self.native_sdk: - try: - # Disable arm first - if self._enabled: - self.native_sdk.DisablePiper() - self._enabled = False - - # Disconnect - self.native_sdk.DisconnectPort() - self._connected = False - self.logger.info("Disconnected from Piper") - except: - pass - finally: - self.native_sdk = None - - def is_connected(self) -> bool: - """Check if connected to Piper. - - Returns: - True if connected - """ - if not self._connected or not self.native_sdk: - return False - - # Try to get status to verify connection - try: - status = self.native_sdk.GetArmStatus() - return status is not None - except: - return False - - # ============= Joint State Query ============= - - def get_joint_positions(self) -> list[float]: - """Get current joint positions. - - Returns: - Joint positions in RADIANS (0-indexed) - """ - joint_msgs = self.native_sdk.GetArmJointMsgs() - if not joint_msgs or not joint_msgs.joint_state: - raise RuntimeError("Failed to get Piper joint positions") - - # Get joint positions from joint_state (values are in Piper units: 0.001 degrees) - # Convert to radians using PIPER_TO_RAD conversion factor - joint_state = joint_msgs.joint_state - positions = [ - joint_state.joint_1 * PIPER_TO_RAD, # Convert Piper units to radians - joint_state.joint_2 * PIPER_TO_RAD, - joint_state.joint_3 * PIPER_TO_RAD, - joint_state.joint_4 * PIPER_TO_RAD, - joint_state.joint_5 * PIPER_TO_RAD, - joint_state.joint_6 * PIPER_TO_RAD, - ] - return positions - - def get_joint_velocities(self) -> list[float]: - """Get current joint velocities. - - Returns: - Joint velocities in RAD/S (0-indexed) - """ - # TODO: Get actual velocities from Piper SDK - # For now return zeros as velocity feedback may not be available - return [0.0] * self.dof - - def get_joint_efforts(self) -> list[float]: - """Get current joint efforts/torques. - - Returns: - Joint efforts in Nm (0-indexed) - """ - # TODO: Get actual efforts/torques from Piper SDK if available - # For now return zeros as effort feedback may not be available - return [0.0] * self.dof - - # ============= Joint Motion Control ============= - - def set_joint_positions( - self, - positions: list[float], - velocity: float = 1.0, - acceleration: float = 1.0, - wait: bool = False, - ) -> bool: - """Move joints to target positions. - - Args: - positions: Target positions in RADIANS (0-indexed) - velocity: Max velocity fraction (0-1) - acceleration: Max acceleration fraction (0-1) - wait: If True, block until motion completes - - Returns: - True if command accepted - """ - # Convert radians to Piper units (0.001 degrees) - piper_joints = [round(rad * RAD_TO_PIPER) for rad in positions] - - # Optionally set motion control parameters based on velocity/acceleration - if velocity < 1.0 or acceleration < 1.0: - # Scale speed rate based on velocity parameter (0-100) - speed_rate = int(velocity * 100) - self.native_sdk.MotionCtrl_2( - ctrl_mode=0x01, # CAN control mode - move_mode=0x01, # Move mode - move_spd_rate_ctrl=speed_rate, # Speed rate - is_mit_mode=0x00, # Not MIT mode - ) - - # Send joint control command using JointCtrl with 6 individual parameters - try: - self.native_sdk.JointCtrl( - piper_joints[0], # Joint 1 - piper_joints[1], # Joint 2 - piper_joints[2], # Joint 3 - piper_joints[3], # Joint 4 - piper_joints[4], # Joint 5 - piper_joints[5], # Joint 6 - ) - result = True - except Exception as e: - self.logger.error(f"Error setting joint positions: {e}") - result = False - - # If wait requested, poll until motion completes - if wait and result: - start_time = time.time() - timeout = 30.0 # 30 second timeout - - while time.time() - start_time < timeout: - try: - # Check if reached target (within tolerance) - current = self.get_joint_positions() - tolerance = 0.01 # radians - if all(abs(current[i] - positions[i]) < tolerance for i in range(6)): - break - except: - pass # Continue waiting - time.sleep(0.01) - - return result - - def set_joint_velocities(self, velocities: list[float]) -> bool: - """Set joint velocity targets. - - Note: Piper doesn't have native velocity control. The driver should - implement velocity control via position integration if needed. - - Args: - velocities: Target velocities in RAD/S (0-indexed) - - Returns: - False - velocity control not supported at SDK level - """ - # Piper doesn't have native velocity control - # The driver layer should implement this via position integration - self.logger.debug("Velocity control not supported at SDK level - use position integration") - return False - - def set_joint_efforts(self, efforts: list[float]) -> bool: - """Set joint effort/torque targets. - - Args: - efforts: Target efforts in Nm (0-indexed) - - Returns: - True if command accepted - """ - # Check if torque control is supported - if not hasattr(self.native_sdk, "SetJointTorque"): - self.logger.warning("Torque control not available in this Piper version") - return False - - # Convert 0-indexed to 1-indexed dict - torque_dict = {i + 1: torque for i, torque in enumerate(efforts)} - - # Send torque command - self.native_sdk.SetJointTorque(torque_dict) - return True - - def stop_motion(self) -> bool: - """Stop all ongoing motion. - - Returns: - True if stop successful - """ - # Piper emergency stop - if hasattr(self.native_sdk, "EmergencyStop"): - self.native_sdk.EmergencyStop() - else: - # Alternative: set zero velocities - zero_vel = {i: 0.0 for i in range(1, 7)} - if hasattr(self.native_sdk, "SetJointSpeed"): - self.native_sdk.SetJointSpeed(zero_vel) - - return True - - # ============= Servo Control ============= - - def enable_servos(self) -> bool: - """Enable motor control. - - Returns: - True if servos enabled - """ - # Enable Piper - attempts = 0 - max_attempts = 100 - - while not self.native_sdk.EnablePiper() and attempts < max_attempts: - time.sleep(0.01) - attempts += 1 - - if attempts < max_attempts: - self._enabled = True - - # Set control mode - self.native_sdk.MotionCtrl_2( - ctrl_mode=0x01, # CAN control mode - move_mode=0x01, # Move mode - move_spd_rate_ctrl=30, # Speed rate - is_mit_mode=0x00, # Not MIT mode - ) - - return True - - return False - - def disable_servos(self) -> bool: - """Disable motor control. - - Returns: - True if servos disabled - """ - self.native_sdk.DisablePiper() - self._enabled = False - return True - - def are_servos_enabled(self) -> bool: - """Check if servos are enabled. - - Returns: - True if enabled - """ - return self._enabled - - # ============= System State ============= - - def get_robot_state(self) -> dict[str, Any]: - """Get current robot state. - - Returns: - State dictionary - """ - status = self.native_sdk.GetArmStatus() - - if status and status.arm_status: - # Map Piper states to standard states - # Use the nested arm_status object - arm_status = status.arm_status - - # Default state mapping - state = 0 # idle - mode = 0 # position mode - error_code = 0 - - # Check for error status - if hasattr(arm_status, "err_code"): - error_code = arm_status.err_code - if error_code != 0: - state = 2 # error state - - # Check motion status if available - if hasattr(arm_status, "motion_status"): - # Could check if moving - pass - - return { - "state": state, - "mode": mode, - "error_code": error_code, - "warn_code": 0, # Piper doesn't have warn codes - "is_moving": False, # Would need to track this - "cmd_num": 0, # Piper doesn't expose command queue - } - - return { - "state": 2, # Error if can't get status - "mode": 0, - "error_code": 999, - "warn_code": 0, - "is_moving": False, - "cmd_num": 0, - } - - def get_error_code(self) -> int: - """Get current error code. - - Returns: - Error code (0 = no error) - """ - status = self.native_sdk.GetArmStatus() - if status and hasattr(status, "error_code"): - return int(status.error_code) - return 0 - - def get_error_message(self) -> str: - """Get human-readable error message. - - Returns: - Error message string - """ - error_code = self.get_error_code() - if error_code == 0: - return "" - - # Piper error codes (approximate) - error_map = { - 1: "Communication error", - 2: "Motor error", - 3: "Encoder error", - 4: "Overtemperature", - 5: "Overcurrent", - 6: "Joint limit error", - 7: "Emergency stop", - 8: "Power error", - } - - return error_map.get(error_code, f"Unknown error {error_code}") - - def clear_errors(self) -> bool: - """Clear error states. - - Returns: - True if errors cleared - """ - if hasattr(self.native_sdk, "ClearError"): - self.native_sdk.ClearError() - return True - - # Alternative: disable and re-enable - self.disable_servos() - time.sleep(0.1) - return self.enable_servos() - - def emergency_stop(self) -> bool: - """Execute emergency stop. - - Returns: - True if e-stop executed - """ - if hasattr(self.native_sdk, "EmergencyStop"): - self.native_sdk.EmergencyStop() - return True - - # Alternative: disable servos - return self.disable_servos() - - # ============= Information ============= - - def get_info(self) -> ManipulatorInfo: - """Get manipulator information. - - Returns: - ManipulatorInfo object - """ - firmware_version = None - try: - firmware_version = self.native_sdk.GetPiperFirmwareVersion() - except: - pass - - return ManipulatorInfo( - vendor="Agilex", - model="Piper", - dof=self.dof, - firmware_version=firmware_version, - serial_number=None, # Piper doesn't expose serial number - ) - - def get_joint_limits(self) -> tuple[list[float], list[float]]: - """Get joint position limits. - - Returns: - Tuple of (lower_limits, upper_limits) in RADIANS - """ - # Piper joint limits (approximate, in radians) - lower_limits = [-3.14, -2.35, -2.35, -3.14, -2.35, -3.14] - upper_limits = [3.14, 2.35, 2.35, 3.14, 2.35, 3.14] - - return (lower_limits, upper_limits) - - def get_velocity_limits(self) -> list[float]: - """Get joint velocity limits. - - Returns: - Maximum velocities in RAD/S - """ - # Piper max velocities (approximate) - max_vel = 3.14 # rad/s - return [max_vel] * self.dof - - def get_acceleration_limits(self) -> list[float]: - """Get joint acceleration limits. - - Returns: - Maximum accelerations in RAD/S² - """ - # Piper max accelerations (approximate) - max_acc = 10.0 # rad/s² - return [max_acc] * self.dof - - # ============= Optional Methods ============= - - def get_cartesian_position(self) -> dict[str, float] | None: - """Get current end-effector pose. - - Returns: - Pose dict or None if not supported - """ - if hasattr(self.native_sdk, "GetEndPose"): - pose = self.native_sdk.GetEndPose() - if pose: - return { - "x": pose.x, - "y": pose.y, - "z": pose.z, - "roll": pose.roll, - "pitch": pose.pitch, - "yaw": pose.yaw, - } - return None - - def set_cartesian_position( - self, - pose: dict[str, float], - velocity: float = 1.0, - acceleration: float = 1.0, - wait: bool = False, - ) -> bool: - """Move end-effector to target pose. - - Args: - pose: Target pose dict - velocity: Max velocity fraction (0-1) - acceleration: Max acceleration fraction (0-1) - wait: Block until complete - - Returns: - True if command accepted - """ - if not hasattr(self.native_sdk, "MoveL"): - self.logger.warning("Cartesian control not available") - return False - - # Create pose object for Piper - target = { - "x": pose["x"], - "y": pose["y"], - "z": pose["z"], - "roll": pose["roll"], - "pitch": pose["pitch"], - "yaw": pose["yaw"], - } - - # Send Cartesian command - self.native_sdk.MoveL(target) - - # Wait if requested - if wait: - start_time = time.time() - timeout = 30.0 - - while time.time() - start_time < timeout: - current = self.get_cartesian_position() - if current: - # Check if reached target (within tolerance) - tol_pos = 0.005 # 5mm - tol_rot = 0.05 # ~3 degrees - - if ( - abs(current["x"] - pose["x"]) < tol_pos - and abs(current["y"] - pose["y"]) < tol_pos - and abs(current["z"] - pose["z"]) < tol_pos - and abs(current["roll"] - pose["roll"]) < tol_rot - and abs(current["pitch"] - pose["pitch"]) < tol_rot - and abs(current["yaw"] - pose["yaw"]) < tol_rot - ): - break - - time.sleep(0.01) - - return True - - def get_gripper_position(self) -> float | None: - """Get gripper position. - - Returns: - Position in meters or None - """ - if hasattr(self.native_sdk, "GetGripperState"): - state = self.native_sdk.GetGripperState() - if state: - # Piper gripper position is 0-100 (percentage) - # Convert to meters (assume max opening 0.08m) - return float(state / 100.0) * 0.08 - return None - - def set_gripper_position(self, position: float, force: float = 1.0) -> bool: - """Set gripper position. - - Args: - position: Target position in meters - force: Force fraction (0-1) - - Returns: - True if successful - """ - if not hasattr(self.native_sdk, "GripperCtrl"): - self.logger.warning("Gripper control not available") - return False - - # Convert meters to percentage (0-100) - # Assume max opening 0.08m - percentage = int((position / 0.08) * 100) - percentage = max(0, min(100, percentage)) - - # Control gripper - self.native_sdk.GripperCtrl(percentage) - return True - - def set_control_mode(self, mode: str) -> bool: - """Set control mode. - - Args: - mode: 'position', 'velocity', 'torque', or 'impedance' - - Returns: - True if successful - """ - # Piper modes via MotionCtrl_2 - # ctrl_mode: 0x01=CAN control - # move_mode: 0x01=position, 0x02=velocity? - - if not hasattr(self.native_sdk, "MotionCtrl_2"): - return False - - move_mode = 0x01 # Default position - if mode == "velocity": - move_mode = 0x02 - - self.native_sdk.MotionCtrl_2( - ctrl_mode=0x01, move_mode=move_mode, move_spd_rate_ctrl=30, is_mit_mode=0x00 - ) - - return True - - def get_control_mode(self) -> str | None: - """Get current control mode. - - Returns: - Mode string or None - """ - status = self.native_sdk.GetArmStatus() - if status and hasattr(status, "arm_mode"): - # Map Piper modes - mode_map = {0x01: "position", 0x02: "velocity"} - return mode_map.get(status.arm_mode, "unknown") - - return "position" # Default assumption diff --git a/dimos/hardware/manipulators_old/xarm/README.md b/dimos/hardware/manipulators_old/xarm/README.md deleted file mode 100644 index ff7a797cad..0000000000 --- a/dimos/hardware/manipulators_old/xarm/README.md +++ /dev/null @@ -1,149 +0,0 @@ -# xArm Driver for dimos - -Real-time driver for UFACTORY xArm5/6/7 manipulators integrated with the dimos framework. - -## Quick Start - -### 1. Specify Robot IP - -**On boot** (Important) -```bash -sudo ifconfig lo multicast -sudo route add -net 224.0.0.0 netmask 240.0.0.0 dev lo -``` - -**Option A: Command-line argument** (recommended) -```bash -python test_xarm_driver.py --ip 192.168.1.235 -python interactive_control.py --ip 192.168.1.235 -``` - -**Option B: Environment variable** -```bash -export XARM_IP=192.168.1.235 -python test_xarm_driver.py -``` - -**Option C: Use default** (192.168.1.235) -```bash -python test_xarm_driver.py # Uses default -``` - -**Note:** Command-line `--ip` takes precedence over `XARM_IP` environment variable. - -### 2. Basic Usage - -```python -from dimos import core -from dimos.hardware.manipulators.xarm.xarm_driver import XArmDriver -from dimos.msgs.sensor_msgs import JointState, JointCommand - -# Start dimos and deploy driver -dimos = core.start(1) -xarm = dimos.deploy(XArmDriver, ip_address="192.168.1.235", xarm_type="xarm6") - -# Configure LCM transports -xarm.joint_state.transport = core.LCMTransport("/xarm/joint_states", JointState) -xarm.joint_position_command.transport = core.LCMTransport("/xarm/joint_commands", JointCommand) - -# Start and enable servo mode -xarm.start() -xarm.enable_servo_mode() - -# Control via RPC -xarm.set_joint_angles([0, 0, 0, 0, 0, 0], speed=50, mvacc=100, mvtime=0) - -# Cleanup -xarm.stop() -dimos.stop() -``` - -## Key Features - -- **100Hz control loop** for real-time position/velocity control -- **LCM pub/sub** for distributed system integration -- **RPC methods** for direct hardware control -- **Position mode** (radians) and **velocity mode** (deg/s) -- **Component-based API**: motion, kinematics, system, gripper control - -## Topics - -**Subscribed:** -- `/xarm/joint_position_command` - JointCommand (positions in radians) -- `/xarm/joint_velocity_command` - JointCommand (velocities in deg/s) - -**Published:** -- `/xarm/joint_states` - JointState (100Hz) -- `/xarm/robot_state` - RobotState (10Hz) -- `/xarm/ft_ext`, `/xarm/ft_raw` - WrenchStamped (force/torque) - -## Common RPC Methods - -```python -# System control -xarm.enable_servo_mode() # Enable position control (mode 1) -xarm.enable_velocity_control_mode() # Enable velocity control (mode 4) -xarm.motion_enable(True) # Enable motors -xarm.clean_error() # Clear errors - -# Motion control -xarm.set_joint_angles([...], speed=50, mvacc=100, mvtime=0) -xarm.set_servo_angle(joint_id=5, angle=0.5, speed=50) - -# State queries -state = xarm.get_joint_state() -position = xarm.get_position() -``` - -## Configuration - -Key parameters for `XArmDriver`: -- `ip_address`: Robot IP (default: "192.168.1.235") -- `xarm_type`: Robot model - "xarm5", "xarm6", or "xarm7" (default: "xarm6") -- `control_frequency`: Control loop rate in Hz (default: 100.0) -- `is_radian`: Use radians vs degrees (default: True) -- `enable_on_start`: Auto-enable servo mode (default: True) -- `velocity_control`: Use velocity vs position mode (default: False) - -## Testing - -### With Mock Hardware (No Physical Robot) - -```bash -# Unit tests with mocked xArm hardware -python tests/test_xarm_rt_driver.py -``` - -### With Real Hardware - -**⚠️ Note:** Interactive control and hardware tests require a physical xArm connected to the network. Interactive control, and sample_trajectory_generator are part of test suite, and will be deprecated. - -**Using Alfred Embodiment:** - -To test with real hardware using the current Alfred embodiment: - -1. **Turn on the Flowbase** (xArm controller) -2. **SSH into dimensional-cpu-2:** - ``` -3. **Verify PC is connected to the controller:** - ```bash - ping 192.168.1.235 # Should respond - ``` -4. **Run the interactive control:** - ```bash - # Interactive control (recommended) - venv/bin/python dimos/hardware/manipulators/xarm/interactive_control.py --ip 192.168.1.235 - - # Run driver standalone - venv/bin/python dimos/hardware/manipulators/xarm/test_xarm_driver.py --ip 192.168.1.235 - - # Run automated test suite - venv/bin/python dimos/hardware/manipulators/xarm/test_xarm_driver.py --ip 192.168.1.235 --run-tests - - # Specify xArm model type (if using xArm7) - venv/bin/python dimos/hardware/manipulators/xarm/interactive_control.py --ip 192.168.1.235 --type xarm7 - ``` - -## License - -Copyright 2025 Dimensional Inc. - Apache License 2.0 diff --git a/dimos/hardware/manipulators_old/xarm/__init__.py b/dimos/hardware/manipulators_old/xarm/__init__.py deleted file mode 100644 index ef0c6763c1..0000000000 --- a/dimos/hardware/manipulators_old/xarm/__init__.py +++ /dev/null @@ -1,29 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -""" -xArm Manipulator Driver Module - -Real-time driver and components for xArm5/6/7 manipulators. -""" - -from dimos.hardware.manipulators.xarm.spec import ArmDriverSpec -from dimos.hardware.manipulators.xarm.xarm_driver import XArmDriver -from dimos.hardware.manipulators.xarm.xarm_wrapper import XArmSDKWrapper - -__all__ = [ - "ArmDriverSpec", - "XArmDriver", - "XArmSDKWrapper", -] diff --git a/dimos/hardware/manipulators_old/xarm/components/__init__.py b/dimos/hardware/manipulators_old/xarm/components/__init__.py deleted file mode 100644 index 4592560cda..0000000000 --- a/dimos/hardware/manipulators_old/xarm/components/__init__.py +++ /dev/null @@ -1,15 +0,0 @@ -"""Component classes for XArmDriver.""" - -from .gripper_control import GripperControlComponent -from .kinematics import KinematicsComponent -from .motion_control import MotionControlComponent -from .state_queries import StateQueryComponent -from .system_control import SystemControlComponent - -__all__ = [ - "GripperControlComponent", - "KinematicsComponent", - "MotionControlComponent", - "StateQueryComponent", - "SystemControlComponent", -] diff --git a/dimos/hardware/manipulators_old/xarm/components/gripper_control.py b/dimos/hardware/manipulators_old/xarm/components/gripper_control.py deleted file mode 100644 index 13b8347978..0000000000 --- a/dimos/hardware/manipulators_old/xarm/components/gripper_control.py +++ /dev/null @@ -1,372 +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. - -""" -Gripper Control Component for XArmDriver. - -Provides RPC methods for controlling various grippers: -- Standard xArm gripper -- Bio gripper -- Vacuum gripper -- Robotiq gripper -""" - -from typing import TYPE_CHECKING, Any - -from dimos.core import rpc -from dimos.utils.logging_config import setup_logger - -if TYPE_CHECKING: - from xarm.wrapper import XArmAPI - -logger = setup_logger() - - -class GripperControlComponent: - """ - Component providing gripper control RPC methods for XArmDriver. - - This component assumes the parent class has: - - self.arm: XArmAPI instance - - self.config: XArmDriverConfig instance - """ - - # Type hints for attributes expected from parent class - arm: "XArmAPI" - config: Any # Config dict accessed as object (dict with attribute access) - - # ========================================================================= - # Standard xArm Gripper - # ========================================================================= - - @rpc - def set_gripper_enable(self, enable: int) -> tuple[int, str]: - """Enable/disable gripper.""" - try: - code = self.arm.set_gripper_enable(enable) - return ( - code, - f"Gripper {'enabled' if enable else 'disabled'}" - if code == 0 - else f"Error code: {code}", - ) - except Exception as e: - return (-1, str(e)) - - @rpc - def set_gripper_mode(self, mode: int) -> tuple[int, str]: - """Set gripper mode (0=location mode, 1=speed mode, 2=current mode).""" - try: - code = self.arm.set_gripper_mode(mode) - return (code, f"Gripper mode set to {mode}" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def set_gripper_speed(self, speed: float) -> tuple[int, str]: - """Set gripper speed (r/min).""" - try: - code = self.arm.set_gripper_speed(speed) - return (code, f"Gripper speed set to {speed}" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def set_gripper_position( - self, - position: float, - wait: bool = False, - speed: float | None = None, - timeout: float | None = None, - ) -> tuple[int, str]: - """ - Set gripper position. - - Args: - position: Target position (0-850) - wait: Wait for completion - speed: Optional speed override - timeout: Optional timeout for wait - """ - try: - code = self.arm.set_gripper_position(position, wait=wait, speed=speed, timeout=timeout) - return ( - code, - f"Gripper position set to {position}" if code == 0 else f"Error code: {code}", - ) - except Exception as e: - return (-1, str(e)) - - @rpc - def get_gripper_position(self) -> tuple[int, float | None]: - """Get current gripper position.""" - try: - code, position = self.arm.get_gripper_position() - return (code, position if code == 0 else None) - except Exception: - return (-1, None) - - @rpc - def get_gripper_err_code(self) -> tuple[int, int | None]: - """Get gripper error code.""" - try: - code, err = self.arm.get_gripper_err_code() - return (code, err if code == 0 else None) - except Exception: - return (-1, None) - - @rpc - def clean_gripper_error(self) -> tuple[int, str]: - """Clear gripper error.""" - try: - code = self.arm.clean_gripper_error() - return (code, "Gripper error cleared" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - # ========================================================================= - # Bio Gripper - # ========================================================================= - - @rpc - def set_bio_gripper_enable(self, enable: int, wait: bool = True) -> tuple[int, str]: - """Enable/disable bio gripper.""" - try: - code = self.arm.set_bio_gripper_enable(enable, wait=wait) - return ( - code, - f"Bio gripper {'enabled' if enable else 'disabled'}" - if code == 0 - else f"Error code: {code}", - ) - except Exception as e: - return (-1, str(e)) - - @rpc - def set_bio_gripper_speed(self, speed: int) -> tuple[int, str]: - """Set bio gripper speed (1-100).""" - try: - code = self.arm.set_bio_gripper_speed(speed) - return ( - code, - f"Bio gripper speed set to {speed}" if code == 0 else f"Error code: {code}", - ) - except Exception as e: - return (-1, str(e)) - - @rpc - def open_bio_gripper( - self, speed: int = 0, wait: bool = True, timeout: float = 5 - ) -> tuple[int, str]: - """Open bio gripper.""" - try: - code = self.arm.open_bio_gripper(speed=speed, wait=wait, timeout=timeout) - return (code, "Bio gripper opened" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def close_bio_gripper( - self, speed: int = 0, wait: bool = True, timeout: float = 5 - ) -> tuple[int, str]: - """Close bio gripper.""" - try: - code = self.arm.close_bio_gripper(speed=speed, wait=wait, timeout=timeout) - return (code, "Bio gripper closed" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def get_bio_gripper_status(self) -> tuple[int, int | None]: - """Get bio gripper status.""" - try: - code, status = self.arm.get_bio_gripper_status() - return (code, status if code == 0 else None) - except Exception: - return (-1, None) - - @rpc - def get_bio_gripper_error(self) -> tuple[int, int | None]: - """Get bio gripper error code.""" - try: - code, error = self.arm.get_bio_gripper_error() - return (code, error if code == 0 else None) - except Exception: - return (-1, None) - - @rpc - def clean_bio_gripper_error(self) -> tuple[int, str]: - """Clear bio gripper error.""" - try: - code = self.arm.clean_bio_gripper_error() - return (code, "Bio gripper error cleared" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - # ========================================================================= - # Vacuum Gripper - # ========================================================================= - - @rpc - def set_vacuum_gripper(self, on: int) -> tuple[int, str]: - """Turn vacuum gripper on/off (0=off, 1=on).""" - try: - code = self.arm.set_vacuum_gripper(on) - return ( - code, - f"Vacuum gripper {'on' if on else 'off'}" if code == 0 else f"Error code: {code}", - ) - except Exception as e: - return (-1, str(e)) - - @rpc - def get_vacuum_gripper(self) -> tuple[int, int | None]: - """Get vacuum gripper state.""" - try: - code, state = self.arm.get_vacuum_gripper() - return (code, state if code == 0 else None) - except Exception: - return (-1, None) - - # ========================================================================= - # Robotiq Gripper - # ========================================================================= - - @rpc - def robotiq_reset(self) -> tuple[int, str]: - """Reset Robotiq gripper.""" - try: - code = self.arm.robotiq_reset() - return (code, "Robotiq gripper reset" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def robotiq_set_activate(self, wait: bool = True, timeout: float = 3) -> tuple[int, str]: - """Activate Robotiq gripper.""" - try: - code = self.arm.robotiq_set_activate(wait=wait, timeout=timeout) - return (code, "Robotiq gripper activated" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def robotiq_set_position( - self, - position: int, - speed: int = 0xFF, - force: int = 0xFF, - wait: bool = True, - timeout: float = 5, - ) -> tuple[int, str]: - """ - Set Robotiq gripper position. - - Args: - position: Target position (0-255, 0=open, 255=closed) - speed: Gripper speed (0-255) - force: Gripper force (0-255) - wait: Wait for completion - timeout: Timeout for wait - """ - try: - code = self.arm.robotiq_set_position( - position, speed=speed, force=force, wait=wait, timeout=timeout - ) - return ( - code, - f"Robotiq position set to {position}" if code == 0 else f"Error code: {code}", - ) - except Exception as e: - return (-1, str(e)) - - @rpc - def robotiq_open( - self, speed: int = 0xFF, force: int = 0xFF, wait: bool = True, timeout: float = 5 - ) -> tuple[int, str]: - """Open Robotiq gripper.""" - try: - code = self.arm.robotiq_open(speed=speed, force=force, wait=wait, timeout=timeout) - return (code, "Robotiq gripper opened" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def robotiq_close( - self, speed: int = 0xFF, force: int = 0xFF, wait: bool = True, timeout: float = 5 - ) -> tuple[int, str]: - """Close Robotiq gripper.""" - try: - code = self.arm.robotiq_close(speed=speed, force=force, wait=wait, timeout=timeout) - return (code, "Robotiq gripper closed" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def robotiq_get_status(self) -> tuple[int, dict[str, Any] | None]: - """Get Robotiq gripper status.""" - try: - ret = self.arm.robotiq_get_status() - if isinstance(ret, tuple) and len(ret) >= 2: - code = ret[0] - if code == 0: - # Return status as dict if successful - status = { - "gOBJ": ret[1] if len(ret) > 1 else None, # Object detection status - "gSTA": ret[2] if len(ret) > 2 else None, # Gripper status - "gGTO": ret[3] if len(ret) > 3 else None, # Go to requested position - "gACT": ret[4] if len(ret) > 4 else None, # Activation status - "kFLT": ret[5] if len(ret) > 5 else None, # Fault status - "gFLT": ret[6] if len(ret) > 6 else None, # Fault status - "gPR": ret[7] if len(ret) > 7 else None, # Requested position echo - "gPO": ret[8] if len(ret) > 8 else None, # Actual position - "gCU": ret[9] if len(ret) > 9 else None, # Current - } - return (code, status) - return (code, None) - return (-1, None) - except Exception as e: - logger.error(f"robotiq_get_status failed: {e}") - return (-1, None) - - # ========================================================================= - # Lite6 Gripper - # ========================================================================= - - @rpc - def open_lite6_gripper(self) -> tuple[int, str]: - """Open Lite6 gripper.""" - try: - code = self.arm.open_lite6_gripper() - return (code, "Lite6 gripper opened" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def close_lite6_gripper(self) -> tuple[int, str]: - """Close Lite6 gripper.""" - try: - code = self.arm.close_lite6_gripper() - return (code, "Lite6 gripper closed" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def stop_lite6_gripper(self) -> tuple[int, str]: - """Stop Lite6 gripper.""" - try: - code = self.arm.stop_lite6_gripper() - return (code, "Lite6 gripper stopped" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) diff --git a/dimos/hardware/manipulators_old/xarm/components/kinematics.py b/dimos/hardware/manipulators_old/xarm/components/kinematics.py deleted file mode 100644 index c29007a426..0000000000 --- a/dimos/hardware/manipulators_old/xarm/components/kinematics.py +++ /dev/null @@ -1,85 +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. - -""" -Kinematics Component for XArmDriver. - -Provides RPC methods for kinematic calculations including: -- Forward kinematics -- Inverse kinematics -""" - -from typing import TYPE_CHECKING, Any - -from dimos.core import rpc -from dimos.utils.logging_config import setup_logger - -if TYPE_CHECKING: - from xarm.wrapper import XArmAPI - -logger = setup_logger() - - -class KinematicsComponent: - """ - Component providing kinematics RPC methods for XArmDriver. - - This component assumes the parent class has: - - self.arm: XArmAPI instance - - self.config: XArmDriverConfig instance - """ - - # Type hints for attributes expected from parent class - arm: "XArmAPI" - config: Any # Config dict accessed as object (dict with attribute access) - - @rpc - def get_inverse_kinematics(self, pose: list[float]) -> tuple[int, list[float] | None]: - """ - Compute inverse kinematics. - - Args: - pose: [x, y, z, roll, pitch, yaw] - - Returns: - Tuple of (code, joint_angles) - """ - try: - code, angles = self.arm.get_inverse_kinematics( - pose, input_is_radian=self.config.is_radian, return_is_radian=self.config.is_radian - ) - return (code, list(angles) if code == 0 else None) - except Exception: - return (-1, None) - - @rpc - def get_forward_kinematics(self, angles: list[float]) -> tuple[int, list[float] | None]: - """ - Compute forward kinematics. - - Args: - angles: Joint angles - - Returns: - Tuple of (code, pose) - """ - try: - code, pose = self.arm.get_forward_kinematics( - angles, - input_is_radian=self.config.is_radian, - return_is_radian=self.config.is_radian, - ) - return (code, list(pose) if code == 0 else None) - except Exception: - return (-1, None) diff --git a/dimos/hardware/manipulators_old/xarm/components/motion_control.py b/dimos/hardware/manipulators_old/xarm/components/motion_control.py deleted file mode 100644 index 64aaa861e0..0000000000 --- a/dimos/hardware/manipulators_old/xarm/components/motion_control.py +++ /dev/null @@ -1,147 +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. - -""" -Motion Control Component for XArmDriver. - -Provides RPC methods for motion control operations including: -- Joint position control -- Joint velocity control -- Cartesian position control -- Home positioning -""" - -import math -import threading -from typing import TYPE_CHECKING, Any - -from dimos.core import rpc -from dimos.utils.logging_config import setup_logger - -if TYPE_CHECKING: - from xarm.wrapper import XArmAPI - -logger = setup_logger() - - -class MotionControlComponent: - """ - Component providing motion control RPC methods for XArmDriver. - - This component assumes the parent class has: - - self.arm: XArmAPI instance - - self.config: XArmDriverConfig instance - - self._joint_cmd_lock: threading.Lock - - self._joint_cmd_: Optional[list[float]] - """ - - # Type hints for attributes expected from parent class - arm: "XArmAPI" - config: Any # Config dict accessed as object (dict with attribute access) - _joint_cmd_lock: threading.Lock - _joint_cmd_: list[float] | None - - @rpc - def set_joint_angles(self, angles: list[float]) -> tuple[int, str]: - """ - Set joint angles (RPC method). - - Args: - angles: List of joint angles (in radians if is_radian=True) - - Returns: - Tuple of (code, message) - """ - try: - code = self.arm.set_servo_angle_j(angles=angles, is_radian=self.config.is_radian) - msg = "Success" if code == 0 else f"Error code: {code}" - return (code, msg) - except Exception as e: - logger.error(f"set_joint_angles failed: {e}") - return (-1, str(e)) - - @rpc - def set_joint_velocities(self, velocities: list[float]) -> tuple[int, str]: - """ - Set joint velocities (RPC method). - Note: Requires velocity control mode. - - Args: - velocities: List of joint velocities (rad/s) - - Returns: - Tuple of (code, message) - """ - try: - # For velocity control, you would use vc_set_joint_velocity - # This requires mode 4 (joint velocity control) - code = self.arm.vc_set_joint_velocity( - speeds=velocities, is_radian=self.config.is_radian - ) - msg = "Success" if code == 0 else f"Error code: {code}" - return (code, msg) - except Exception as e: - logger.error(f"set_joint_velocities failed: {e}") - return (-1, str(e)) - - @rpc - def set_position(self, position: list[float], wait: bool = False) -> tuple[int, str]: - """ - Set TCP position [x, y, z, roll, pitch, yaw]. - - Args: - position: Target position - wait: Wait for motion to complete - - Returns: - Tuple of (code, message) - """ - try: - code = self.arm.set_position(*position, is_radian=self.config.is_radian, wait=wait) - return (code, "Success" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def move_gohome(self, wait: bool = False) -> tuple[int, str]: - """Move to home position.""" - try: - code = self.arm.move_gohome(wait=wait, is_radian=self.config.is_radian) - return (code, "Moving home" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def set_joint_command(self, positions: list[float]) -> tuple[int, str]: - """ - Manually set the joint command (for testing). - This updates the shared joint_cmd that the control loop reads. - - Args: - positions: List of joint positions in radians - - Returns: - Tuple of (code, message) - """ - try: - if len(positions) != self.config.num_joints: - return (-1, f"Expected {self.config.num_joints} positions, got {len(positions)}") - - with self._joint_cmd_lock: - self._joint_cmd_ = list(positions) - - logger.info(f"✓ Joint command set: {[f'{math.degrees(p):.2f}°' for p in positions]}") - return (0, "Joint command updated") - except Exception as e: - return (-1, str(e)) diff --git a/dimos/hardware/manipulators_old/xarm/components/state_queries.py b/dimos/hardware/manipulators_old/xarm/components/state_queries.py deleted file mode 100644 index 5615763cc4..0000000000 --- a/dimos/hardware/manipulators_old/xarm/components/state_queries.py +++ /dev/null @@ -1,185 +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. - -""" -State Query Component for XArmDriver. - -Provides RPC methods for querying robot state including: -- Joint state -- Robot state -- Cartesian position -- Firmware version -""" - -import threading -from typing import TYPE_CHECKING, Any - -from dimos.core import rpc -from dimos.msgs.sensor_msgs import JointState, RobotState -from dimos.utils.logging_config import setup_logger - -if TYPE_CHECKING: - from xarm.wrapper import XArmAPI - -logger = setup_logger() - - -class StateQueryComponent: - """ - Component providing state query RPC methods for XArmDriver. - - This component assumes the parent class has: - - self.arm: XArmAPI instance - - self.config: XArmDriverConfig instance - - self._joint_state_lock: threading.Lock - - self._joint_states_: Optional[JointState] - - self._robot_state_: Optional[RobotState] - """ - - # Type hints for attributes expected from parent class - arm: "XArmAPI" - config: Any # Config dict accessed as object (dict with attribute access) - _joint_state_lock: threading.Lock - _joint_states_: JointState | None - _robot_state_: RobotState | None - - @rpc - def get_joint_state(self) -> JointState | None: - """ - Get the current joint state (RPC method). - - Returns: - Current JointState or None - """ - with self._joint_state_lock: - return self._joint_states_ - - @rpc - def get_robot_state(self) -> RobotState | None: - """ - Get the current robot state (RPC method). - - Returns: - Current RobotState or None - """ - with self._joint_state_lock: - return self._robot_state_ - - @rpc - def get_position(self) -> tuple[int, list[float] | None]: - """ - Get TCP position [x, y, z, roll, pitch, yaw]. - - Returns: - Tuple of (code, position) - """ - try: - code, position = self.arm.get_position(is_radian=self.config.is_radian) - return (code, list(position) if code == 0 else None) - except Exception as e: - logger.error(f"get_position failed: {e}") - return (-1, None) - - @rpc - def get_version(self) -> tuple[int, str | None]: - """Get firmware version.""" - try: - code, version = self.arm.get_version() - return (code, version if code == 0 else None) - except Exception: - return (-1, None) - - @rpc - def get_servo_angle(self) -> tuple[int, list[float] | None]: - """Get joint angles.""" - try: - code, angles = self.arm.get_servo_angle(is_radian=self.config.is_radian) - return (code, list(angles) if code == 0 else None) - except Exception as e: - logger.error(f"get_servo_angle failed: {e}") - return (-1, None) - - @rpc - def get_position_aa(self) -> tuple[int, list[float] | None]: - """Get TCP position in axis-angle format.""" - try: - code, position = self.arm.get_position_aa(is_radian=self.config.is_radian) - return (code, list(position) if code == 0 else None) - except Exception as e: - logger.error(f"get_position_aa failed: {e}") - return (-1, None) - - # ========================================================================= - # Robot State Queries - # ========================================================================= - - @rpc - def get_state(self) -> tuple[int, int | None]: - """Get robot state (0=ready, 3=pause, 4=stop).""" - try: - code, state = self.arm.get_state() - return (code, state if code == 0 else None) - except Exception: - return (-1, None) - - @rpc - def get_cmdnum(self) -> tuple[int, int | None]: - """Get command queue length.""" - try: - code, cmdnum = self.arm.get_cmdnum() - return (code, cmdnum if code == 0 else None) - except Exception: - return (-1, None) - - @rpc - def get_err_warn_code(self) -> tuple[int, list[int] | None]: - """Get error and warning codes.""" - try: - err_warn = [0, 0] - code = self.arm.get_err_warn_code(err_warn) - return (code, err_warn if code == 0 else None) - except Exception: - return (-1, None) - - # ========================================================================= - # Force/Torque Sensor Queries - # ========================================================================= - - @rpc - def get_ft_sensor_data(self) -> tuple[int, list[float] | None]: - """Get force/torque sensor data [fx, fy, fz, tx, ty, tz].""" - try: - code, ft_data = self.arm.get_ft_sensor_data() - return (code, list(ft_data) if code == 0 else None) - except Exception as e: - logger.error(f"get_ft_sensor_data failed: {e}") - return (-1, None) - - @rpc - def get_ft_sensor_error(self) -> tuple[int, int | None]: - """Get FT sensor error code.""" - try: - code, error = self.arm.get_ft_sensor_error() - return (code, error if code == 0 else None) - except Exception: - return (-1, None) - - @rpc - def get_ft_sensor_mode(self) -> tuple[int, int | None]: - """Get FT sensor application mode.""" - try: - code, mode = self.arm.get_ft_sensor_app_get() - return (code, mode if code == 0 else None) - except Exception: - return (-1, None) diff --git a/dimos/hardware/manipulators_old/xarm/components/system_control.py b/dimos/hardware/manipulators_old/xarm/components/system_control.py deleted file mode 100644 index a04e9a94a0..0000000000 --- a/dimos/hardware/manipulators_old/xarm/components/system_control.py +++ /dev/null @@ -1,555 +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. - -""" -System Control Component for XArmDriver. - -Provides RPC methods for system-level control operations including: -- Mode control (servo, velocity) -- State management -- Error handling -- Emergency stop -""" - -from typing import TYPE_CHECKING, Any, Protocol - -from dimos.core import rpc -from dimos.utils.logging_config import setup_logger - -if TYPE_CHECKING: - from xarm.wrapper import XArmAPI - - class XArmConfig(Protocol): - """Protocol for XArm configuration.""" - - is_radian: bool - velocity_control: bool - - -logger = setup_logger() - - -class SystemControlComponent: - """ - Component providing system control RPC methods for XArmDriver. - - This component assumes the parent class has: - - self.arm: XArmAPI instance - - self.config: XArmDriverConfig instance - """ - - # Type hints for attributes expected from parent class - arm: "XArmAPI" - config: Any # Should be XArmConfig but accessed as dict - - @rpc - def enable_servo_mode(self) -> tuple[int, str]: - """ - Enable servo mode (mode 1). - Required for set_servo_angle_j to work. - - Returns: - Tuple of (code, message) - """ - try: - code = self.arm.set_mode(1) - if code == 0: - logger.info("Servo mode enabled") - return (code, "Servo mode enabled") - else: - logger.warning(f"Failed to enable servo mode: code={code}") - return (code, f"Error code: {code}") - except Exception as e: - logger.error(f"enable_servo_mode failed: {e}") - return (-1, str(e)) - - @rpc - def disable_servo_mode(self) -> tuple[int, str]: - """ - Disable servo mode (set to position mode). - - Returns: - Tuple of (code, message) - """ - try: - code = self.arm.set_mode(0) - if code == 0: - logger.info("Servo mode disabled (position mode)") - return (code, "Position mode enabled") - else: - logger.warning(f"Failed to disable servo mode: code={code}") - return (code, f"Error code: {code}") - except Exception as e: - logger.error(f"disable_servo_mode failed: {e}") - return (-1, str(e)) - - @rpc - def enable_velocity_control_mode(self) -> tuple[int, str]: - """ - Enable velocity control mode (mode 4). - Required for vc_set_joint_velocity to work. - - Returns: - Tuple of (code, message) - """ - try: - # IMPORTANT: Set config flag BEFORE changing robot mode - # This prevents control loop from sending wrong command type during transition - self.config.velocity_control = True - - # Step 1: Set mode to 4 (velocity control) - code = self.arm.set_mode(4) - if code != 0: - logger.warning(f"Failed to set mode to 4: code={code}") - self.config.velocity_control = False # Revert on failure - return (code, f"Failed to set mode: code={code}") - - # Step 2: Set state to 0 (ready/sport mode) - this activates the mode! - code = self.arm.set_state(0) - if code == 0: - logger.info("Velocity control mode enabled (mode=4, state=0)") - return (code, "Velocity control mode enabled") - else: - logger.warning(f"Failed to set state to 0: code={code}") - self.config.velocity_control = False # Revert on failure - return (code, f"Failed to set state: code={code}") - except Exception as e: - logger.error(f"enable_velocity_control_mode failed: {e}") - self.config.velocity_control = False # Revert on exception - return (-1, str(e)) - - @rpc - def disable_velocity_control_mode(self) -> tuple[int, str]: - """ - Disable velocity control mode and return to position control (mode 1). - - Returns: - Tuple of (code, message) - """ - try: - # IMPORTANT: Set config flag BEFORE changing robot mode - # This prevents control loop from sending velocity commands after mode change - self.config.velocity_control = False - - # Step 1: Clear any errors that may have occurred - self.arm.clean_error() - self.arm.clean_warn() - - # Step 2: Set mode to 1 (servo/position control) - code = self.arm.set_mode(1) - if code != 0: - logger.warning(f"Failed to set mode to 1: code={code}") - self.config.velocity_control = True # Revert on failure - return (code, f"Failed to set mode: code={code}") - - # Step 3: Set state to 0 (ready) - CRITICAL for accepting new commands - code = self.arm.set_state(0) - if code == 0: - logger.info("Position control mode enabled (state=0, mode=1)") - return (code, "Position control mode enabled") - else: - logger.warning(f"Failed to set state to 0: code={code}") - self.config.velocity_control = True # Revert on failure - return (code, f"Failed to set state: code={code}") - except Exception as e: - logger.error(f"disable_velocity_control_mode failed: {e}") - self.config.velocity_control = True # Revert on exception - return (-1, str(e)) - - @rpc - def motion_enable(self, enable: bool = True) -> tuple[int, str]: - """Enable or disable arm motion.""" - try: - code = self.arm.motion_enable(enable=enable) - msg = f"Motion {'enabled' if enable else 'disabled'}" - return (code, msg if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def set_state(self, state: int) -> tuple[int, str]: - """ - Set robot state. - - Args: - state: 0=ready, 3=pause, 4=stop - """ - try: - code = self.arm.set_state(state=state) - return (code, "Success" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def clean_error(self) -> tuple[int, str]: - """Clear error codes.""" - try: - code = self.arm.clean_error() - return (code, "Errors cleared" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def clean_warn(self) -> tuple[int, str]: - """Clear warning codes.""" - try: - code = self.arm.clean_warn() - return (code, "Warnings cleared" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def emergency_stop(self) -> tuple[int, str]: - """Emergency stop the arm.""" - try: - code = self.arm.emergency_stop() - return (code, "Emergency stop" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - # ========================================================================= - # Configuration & Persistence - # ========================================================================= - - @rpc - def clean_conf(self) -> tuple[int, str]: - """Clean configuration.""" - try: - code = self.arm.clean_conf() - return (code, "Configuration cleaned" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def save_conf(self) -> tuple[int, str]: - """Save current configuration to robot.""" - try: - code = self.arm.save_conf() - return (code, "Configuration saved" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def reload_dynamics(self) -> tuple[int, str]: - """Reload dynamics parameters.""" - try: - code = self.arm.reload_dynamics() - return (code, "Dynamics reloaded" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - # ========================================================================= - # Mode & State Control - # ========================================================================= - - @rpc - def set_mode(self, mode: int) -> tuple[int, str]: - """ - Set control mode. - - Args: - mode: 0=position, 1=servo, 4=velocity, etc. - """ - try: - code = self.arm.set_mode(mode) - return (code, f"Mode set to {mode}" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - # ========================================================================= - # Collision & Safety - # ========================================================================= - - @rpc - def set_collision_sensitivity(self, sensitivity: int) -> tuple[int, str]: - """Set collision sensitivity (0-5, 0=least sensitive).""" - try: - code = self.arm.set_collision_sensitivity(sensitivity) - return ( - code, - f"Collision sensitivity set to {sensitivity}" - if code == 0 - else f"Error code: {code}", - ) - except Exception as e: - return (-1, str(e)) - - @rpc - def set_teach_sensitivity(self, sensitivity: int) -> tuple[int, str]: - """Set teach sensitivity (1-5).""" - try: - code = self.arm.set_teach_sensitivity(sensitivity) - return ( - code, - f"Teach sensitivity set to {sensitivity}" if code == 0 else f"Error code: {code}", - ) - except Exception as e: - return (-1, str(e)) - - @rpc - def set_collision_rebound(self, enable: int) -> tuple[int, str]: - """Enable/disable collision rebound (0=disable, 1=enable).""" - try: - code = self.arm.set_collision_rebound(enable) - return ( - code, - f"Collision rebound {'enabled' if enable else 'disabled'}" - if code == 0 - else f"Error code: {code}", - ) - except Exception as e: - return (-1, str(e)) - - @rpc - def set_self_collision_detection(self, enable: int) -> tuple[int, str]: - """Enable/disable self collision detection.""" - try: - code = self.arm.set_self_collision_detection(enable) - return ( - code, - f"Self collision detection {'enabled' if enable else 'disabled'}" - if code == 0 - else f"Error code: {code}", - ) - except Exception as e: - return (-1, str(e)) - - # ========================================================================= - # Reduced Mode & Boundaries - # ========================================================================= - - @rpc - def set_reduced_mode(self, enable: int) -> tuple[int, str]: - """Enable/disable reduced mode.""" - try: - code = self.arm.set_reduced_mode(enable) - return ( - code, - f"Reduced mode {'enabled' if enable else 'disabled'}" - if code == 0 - else f"Error code: {code}", - ) - except Exception as e: - return (-1, str(e)) - - @rpc - def set_reduced_max_tcp_speed(self, speed: float) -> tuple[int, str]: - """Set maximum TCP speed in reduced mode.""" - try: - code = self.arm.set_reduced_max_tcp_speed(speed) - return ( - code, - f"Reduced max TCP speed set to {speed}" if code == 0 else f"Error code: {code}", - ) - except Exception as e: - return (-1, str(e)) - - @rpc - def set_reduced_max_joint_speed(self, speed: float) -> tuple[int, str]: - """Set maximum joint speed in reduced mode.""" - try: - code = self.arm.set_reduced_max_joint_speed(speed) - return ( - code, - f"Reduced max joint speed set to {speed}" if code == 0 else f"Error code: {code}", - ) - except Exception as e: - return (-1, str(e)) - - @rpc - def set_fence_mode(self, enable: int) -> tuple[int, str]: - """Enable/disable fence mode.""" - try: - code = self.arm.set_fence_mode(enable) - return ( - code, - f"Fence mode {'enabled' if enable else 'disabled'}" - if code == 0 - else f"Error code: {code}", - ) - except Exception as e: - return (-1, str(e)) - - # ========================================================================= - # TCP & Dynamics Configuration - # ========================================================================= - - @rpc - def set_tcp_offset(self, offset: list[float]) -> tuple[int, str]: - """Set TCP offset [x, y, z, roll, pitch, yaw].""" - try: - code = self.arm.set_tcp_offset(offset) - return (code, "TCP offset set" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def set_tcp_load(self, weight: float, center_of_gravity: list[float]) -> tuple[int, str]: - """Set TCP load (payload).""" - try: - code = self.arm.set_tcp_load(weight, center_of_gravity) - return (code, f"TCP load set: {weight}kg" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def set_gravity_direction(self, direction: list[float]) -> tuple[int, str]: - """Set gravity direction vector.""" - try: - code = self.arm.set_gravity_direction(direction) - return (code, "Gravity direction set" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def set_world_offset(self, offset: list[float]) -> tuple[int, str]: - """Set world coordinate offset.""" - try: - code = self.arm.set_world_offset(offset) - return (code, "World offset set" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - # ========================================================================= - # Motion Parameters - # ========================================================================= - - @rpc - def set_tcp_jerk(self, jerk: float) -> tuple[int, str]: - """Set TCP jerk (mm/s³).""" - try: - code = self.arm.set_tcp_jerk(jerk) - return (code, f"TCP jerk set to {jerk}" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def set_tcp_maxacc(self, acc: float) -> tuple[int, str]: - """Set TCP maximum acceleration (mm/s²).""" - try: - code = self.arm.set_tcp_maxacc(acc) - return ( - code, - f"TCP max acceleration set to {acc}" if code == 0 else f"Error code: {code}", - ) - except Exception as e: - return (-1, str(e)) - - @rpc - def set_joint_jerk(self, jerk: float) -> tuple[int, str]: - """Set joint jerk (rad/s³ or °/s³).""" - try: - code = self.arm.set_joint_jerk(jerk, is_radian=self.config.is_radian) - return (code, f"Joint jerk set to {jerk}" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - @rpc - def set_joint_maxacc(self, acc: float) -> tuple[int, str]: - """Set joint maximum acceleration (rad/s² or °/s²).""" - try: - code = self.arm.set_joint_maxacc(acc, is_radian=self.config.is_radian) - return ( - code, - f"Joint max acceleration set to {acc}" if code == 0 else f"Error code: {code}", - ) - except Exception as e: - return (-1, str(e)) - - @rpc - def set_pause_time(self, seconds: float) -> tuple[int, str]: - """Set pause time for motion commands.""" - try: - code = self.arm.set_pause_time(seconds) - return (code, f"Pause time set to {seconds}s" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - # ========================================================================= - # Digital I/O (Tool GPIO) - # ========================================================================= - - @rpc - def get_tgpio_digital(self, io_num: int) -> tuple[int, int | None]: - """Get tool GPIO digital input value.""" - try: - code, value = self.arm.get_tgpio_digital(io_num) - return (code, value if code == 0 else None) - except Exception: - return (-1, None) - - @rpc - def set_tgpio_digital(self, io_num: int, value: int) -> tuple[int, str]: - """Set tool GPIO digital output value (0 or 1).""" - try: - code = self.arm.set_tgpio_digital(io_num, value) - return (code, f"TGPIO {io_num} set to {value}" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - # ========================================================================= - # Digital I/O (Controller GPIO) - # ========================================================================= - - @rpc - def get_cgpio_digital(self, io_num: int) -> tuple[int, int | None]: - """Get controller GPIO digital input value.""" - try: - code, value = self.arm.get_cgpio_digital(io_num) - return (code, value if code == 0 else None) - except Exception: - return (-1, None) - - @rpc - def set_cgpio_digital(self, io_num: int, value: int) -> tuple[int, str]: - """Set controller GPIO digital output value (0 or 1).""" - try: - code = self.arm.set_cgpio_digital(io_num, value) - return (code, f"CGPIO {io_num} set to {value}" if code == 0 else f"Error code: {code}") - except Exception as e: - return (-1, str(e)) - - # ========================================================================= - # Analog I/O - # ========================================================================= - - @rpc - def get_tgpio_analog(self, io_num: int) -> tuple[int, float | None]: - """Get tool GPIO analog input value.""" - try: - code, value = self.arm.get_tgpio_analog(io_num) - return (code, value if code == 0 else None) - except Exception: - return (-1, None) - - @rpc - def get_cgpio_analog(self, io_num: int) -> tuple[int, float | None]: - """Get controller GPIO analog input value.""" - try: - code, value = self.arm.get_cgpio_analog(io_num) - return (code, value if code == 0 else None) - except Exception: - return (-1, None) - - @rpc - def set_cgpio_analog(self, io_num: int, value: float) -> tuple[int, str]: - """Set controller GPIO analog output value.""" - try: - code = self.arm.set_cgpio_analog(io_num, value) - return ( - code, - f"CGPIO analog {io_num} set to {value}" if code == 0 else f"Error code: {code}", - ) - except Exception as e: - return (-1, str(e)) diff --git a/dimos/hardware/manipulators_old/xarm/spec.py b/dimos/hardware/manipulators_old/xarm/spec.py deleted file mode 100644 index 625f036a0b..0000000000 --- a/dimos/hardware/manipulators_old/xarm/spec.py +++ /dev/null @@ -1,63 +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. - -from dataclasses import dataclass -from typing import Protocol - -from dimos.core import In, Out -from dimos.msgs.geometry_msgs import WrenchStamped -from dimos.msgs.sensor_msgs import JointCommand, JointState - - -@dataclass -class RobotState: - """Custom message containing full robot state (deprecated - use RobotStateMsg).""" - - state: int = 0 # Robot state (0: ready, 3: paused, 4: stopped, etc.) - mode: int = 0 # Control mode (0: position, 1: servo, 4: joint velocity, 5: cartesian velocity) - error_code: int = 0 # Error code - warn_code: int = 0 # Warning code - cmdnum: int = 0 # Command queue length - mt_brake: int = 0 # Motor brake state - mt_able: int = 0 # Motor enable state - - -class ArmDriverSpec(Protocol): - """Protocol specification for xArm manipulator driver. - - Compatible with xArm5, xArm6, and xArm7 models. - """ - - # Input topics (commands) - joint_position_command: In[JointCommand] # Desired joint positions (radians) - joint_velocity_command: In[JointCommand] # Desired joint velocities (rad/s) - - # Output topics - joint_state: Out[JointState] # Current joint positions, velocities, and efforts - robot_state: Out[RobotState] # Full robot state (errors, modes, etc.) - ft_ext: Out[WrenchStamped] # External force/torque (compensated) - ft_raw: Out[WrenchStamped] # Raw force/torque sensor data - - # RPC Methods - def set_joint_angles(self, angles: list[float]) -> tuple[int, str]: ... - - def set_joint_velocities(self, velocities: list[float]) -> tuple[int, str]: ... - - def get_joint_state(self) -> JointState: ... - - def get_robot_state(self) -> RobotState: ... - - def enable_servo_mode(self) -> tuple[int, str]: ... - - def disable_servo_mode(self) -> tuple[int, str]: ... diff --git a/dimos/hardware/manipulators_old/xarm/xarm_blueprints.py b/dimos/hardware/manipulators_old/xarm/xarm_blueprints.py deleted file mode 100644 index 4e84c9c991..0000000000 --- a/dimos/hardware/manipulators_old/xarm/xarm_blueprints.py +++ /dev/null @@ -1,260 +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. - -""" -Blueprints for xArm manipulator control using component-based architecture. - -This module provides declarative blueprints for configuring xArm with the new -generalized component-based driver architecture. - -Usage: - # Run via CLI: - dimos run xarm-servo # Driver only - dimos run xarm-trajectory # Driver + Joint trajectory controller - dimos run xarm-cartesian # Driver + Cartesian motion controller - - # Or programmatically: - from dimos.hardware.manipulators.xarm.xarm_blueprints import xarm_trajectory - coordinator = xarm_trajectory.build() - coordinator.loop() -""" - -from typing import Any - -from dimos.core.blueprints import autoconnect -from dimos.core.transport import LCMTransport -from dimos.hardware.manipulators.xarm.xarm_driver import xarm_driver as xarm_driver_blueprint -from dimos.manipulation.control import cartesian_motion_controller, joint_trajectory_controller -from dimos.msgs.geometry_msgs import PoseStamped -from dimos.msgs.sensor_msgs import ( - JointCommand, - JointState, - RobotState, -) -from dimos.msgs.trajectory_msgs import JointTrajectory - - -# Create a blueprint wrapper for the component-based driver -def xarm_driver(**config: Any) -> Any: - """Create a blueprint for XArmDriver. - - Args: - **config: Configuration parameters passed to XArmDriver - - ip: IP address of XArm controller (default: "192.168.1.210") - - dof: Degrees of freedom - 5, 6, or 7 (default: 6) - - has_gripper: Whether gripper is attached (default: False) - - has_force_torque: Whether F/T sensor is attached (default: False) - - control_rate: Control loop + joint feedback rate in Hz (default: 100) - - monitor_rate: Robot state monitoring rate in Hz (default: 10) - - Returns: - Blueprint configuration for XArmDriver - """ - # Set defaults - config.setdefault("ip", "192.168.1.210") - config.setdefault("dof", 6) - config.setdefault("has_gripper", False) - config.setdefault("has_force_torque", False) - config.setdefault("control_rate", 100) - config.setdefault("monitor_rate", 10) - - # Return the xarm_driver blueprint with the config - return xarm_driver_blueprint(**config) - - -# ============================================================================= -# xArm6 Servo Control Blueprint -# ============================================================================= -# XArmDriver configured for servo control mode using component-based architecture. -# Publishes joint states and robot state, listens for joint commands. -# ============================================================================= - -xarm_servo = xarm_driver( - ip="192.168.1.210", - dof=6, # XArm6 - has_gripper=False, - has_force_torque=False, - control_rate=100, - monitor_rate=10, -).transports( - { - # Joint state feedback (position, velocity, effort) - ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), - # Robot state feedback (mode, state, errors) - ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), - # Position commands input - ("joint_position_command", JointCommand): LCMTransport( - "/xarm/joint_position_command", JointCommand - ), - # Velocity commands input - ("joint_velocity_command", JointCommand): LCMTransport( - "/xarm/joint_velocity_command", JointCommand - ), - } -) - -# ============================================================================= -# xArm7 Servo Control Blueprint -# ============================================================================= - -xarm7_servo = xarm_driver( - ip="192.168.1.210", - dof=7, # XArm7 - has_gripper=False, - has_force_torque=False, - control_rate=100, - monitor_rate=10, -).transports( - { - ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), - ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), - ("joint_position_command", JointCommand): LCMTransport( - "/xarm/joint_position_command", JointCommand - ), - ("joint_velocity_command", JointCommand): LCMTransport( - "/xarm/joint_velocity_command", JointCommand - ), - } -) - -# ============================================================================= -# xArm5 Servo Control Blueprint -# ============================================================================= - -xarm5_servo = xarm_driver( - ip="192.168.1.210", - dof=5, # XArm5 - has_gripper=False, - has_force_torque=False, - control_rate=100, - monitor_rate=10, -).transports( - { - ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), - ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), - ("joint_position_command", JointCommand): LCMTransport( - "/xarm/joint_position_command", JointCommand - ), - ("joint_velocity_command", JointCommand): LCMTransport( - "/xarm/joint_velocity_command", JointCommand - ), - } -) - -# ============================================================================= -# xArm Trajectory Control Blueprint (Driver + Trajectory Controller) -# ============================================================================= -# Combines XArmDriver with JointTrajectoryController for trajectory execution. -# The controller receives JointTrajectory messages and executes them at 100Hz. -# ============================================================================= - -xarm_trajectory = autoconnect( - xarm_driver( - ip="192.168.1.210", - dof=6, # XArm6 - has_gripper=False, - has_force_torque=False, - control_rate=500, - monitor_rate=10, - ), - joint_trajectory_controller( - control_frequency=100.0, - ), -).transports( - { - # Shared topics between driver and controller - ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), - ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), - ("joint_position_command", JointCommand): LCMTransport( - "/xarm/joint_position_command", JointCommand - ), - # Trajectory input topic - ("trajectory", JointTrajectory): LCMTransport("/trajectory", JointTrajectory), - } -) - -# ============================================================================= -# xArm7 Trajectory Control Blueprint -# ============================================================================= - -xarm7_trajectory = autoconnect( - xarm_driver( - ip="192.168.1.210", - dof=7, # XArm7 - has_gripper=False, - has_force_torque=False, - control_rate=100, - monitor_rate=10, - ), - joint_trajectory_controller( - control_frequency=100.0, - ), -).transports( - { - ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), - ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), - ("joint_position_command", JointCommand): LCMTransport( - "/xarm/joint_position_command", JointCommand - ), - ("trajectory", JointTrajectory): LCMTransport("/trajectory", JointTrajectory), - } -) - -# ============================================================================= -# xArm Cartesian Control Blueprint (Driver + Controller) -# ============================================================================= -# Combines XArmDriver with CartesianMotionController for Cartesian space control. -# The controller receives target_pose and converts to joint commands via IK. -# ============================================================================= - -xarm_cartesian = autoconnect( - xarm_driver( - ip="192.168.1.210", - dof=6, # XArm6 - has_gripper=False, - has_force_torque=False, - control_rate=100, - monitor_rate=10, - ), - cartesian_motion_controller( - control_frequency=20.0, - position_kp=5.0, - position_ki=0.0, - position_kd=0.1, - max_linear_velocity=0.2, - max_angular_velocity=1.0, - ), -).transports( - { - # Shared topics between driver and controller - ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), - ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), - ("joint_position_command", JointCommand): LCMTransport( - "/xarm/joint_position_command", JointCommand - ), - # Controller-specific topics - ("target_pose", PoseStamped): LCMTransport("/target_pose", PoseStamped), - ("current_pose", PoseStamped): LCMTransport("/xarm/current_pose", PoseStamped), - } -) - - -__all__ = [ - "xarm5_servo", - "xarm7_servo", - "xarm7_trajectory", - "xarm_cartesian", - "xarm_servo", - "xarm_trajectory", -] diff --git a/dimos/hardware/manipulators_old/xarm/xarm_driver.py b/dimos/hardware/manipulators_old/xarm/xarm_driver.py deleted file mode 100644 index f6d950938c..0000000000 --- a/dimos/hardware/manipulators_old/xarm/xarm_driver.py +++ /dev/null @@ -1,174 +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. - -"""XArm driver using the generalized component-based architecture.""" - -import logging -from typing import Any - -from dimos.hardware.manipulators.base import ( - BaseManipulatorDriver, - StandardMotionComponent, - StandardServoComponent, - StandardStatusComponent, -) - -from .xarm_wrapper import XArmSDKWrapper - -logger = logging.getLogger(__name__) - - -class XArmDriver(BaseManipulatorDriver): - """XArm driver using component-based architecture. - - This driver supports XArm5, XArm6, and XArm7 models. - All the complex logic is handled by the base class and standard components. - This file just assembles the pieces. - """ - - def __init__(self, **kwargs: Any) -> None: - """Initialize the XArm driver. - - Args: - **kwargs: Arguments for Module initialization. - Driver configuration can be passed via 'config' keyword arg: - - ip: IP address of the XArm controller - - dof: Degrees of freedom (5, 6, or 7) - - has_gripper: Whether gripper is attached - - has_force_torque: Whether F/T sensor is attached - """ - # Extract driver-specific config from kwargs - config: dict[str, Any] = kwargs.pop("config", {}) - - # Extract driver-specific params that might be passed directly - driver_params = [ - "ip", - "dof", - "has_gripper", - "has_force_torque", - "control_rate", - "monitor_rate", - ] - for param in driver_params: - if param in kwargs: - config[param] = kwargs.pop(param) - - logger.info(f"Initializing XArmDriver with config: {config}") - - # Create SDK wrapper - sdk = XArmSDKWrapper() - - # Create standard components - components = [ - StandardMotionComponent(sdk), - StandardServoComponent(sdk), - StandardStatusComponent(sdk), - ] - - # Optional: Add gripper component if configured - # if config.get('has_gripper', False): - # from dimos.hardware.manipulators.base.components import StandardGripperComponent - # components.append(StandardGripperComponent(sdk)) - - # Optional: Add force/torque component if configured - # if config.get('has_force_torque', False): - # from dimos.hardware.manipulators.base.components import StandardForceTorqueComponent - # components.append(StandardForceTorqueComponent(sdk)) - - # Remove any kwargs that would conflict with explicit arguments - kwargs.pop("sdk", None) - kwargs.pop("components", None) - kwargs.pop("name", None) - - # Initialize base driver with SDK and components - super().__init__(sdk=sdk, components=components, config=config, name="XArmDriver", **kwargs) - - logger.info("XArmDriver initialized successfully") - - -# Blueprint configuration for the driver -def get_blueprint() -> dict[str, Any]: - """Get the blueprint configuration for the XArm driver. - - Returns: - Dictionary with blueprint configuration - """ - return { - "name": "XArmDriver", - "class": XArmDriver, - "config": { - "ip": "192.168.1.210", # Default IP - "dof": 7, # Default to 7-DOF - "has_gripper": False, - "has_force_torque": False, - "control_rate": 100, # Hz - control loop + joint feedback - "monitor_rate": 10, # Hz - robot state monitoring - }, - "inputs": { - "joint_position_command": "JointCommand", - "joint_velocity_command": "JointCommand", - }, - "outputs": { - "joint_state": "JointState", - "robot_state": "RobotState", - }, - "rpc_methods": [ - # Motion control - "move_joint", - "move_joint_velocity", - "move_joint_effort", - "stop_motion", - "get_joint_state", - "get_joint_limits", - "get_velocity_limits", - "set_velocity_scale", - "set_acceleration_scale", - "move_cartesian", - "get_cartesian_state", - "execute_trajectory", - "stop_trajectory", - # Servo control - "enable_servo", - "disable_servo", - "toggle_servo", - "get_servo_state", - "emergency_stop", - "reset_emergency_stop", - "set_control_mode", - "get_control_mode", - "clear_errors", - "reset_fault", - "home_robot", - "brake_release", - "brake_engage", - # Status monitoring - "get_robot_state", - "get_system_info", - "get_capabilities", - "get_error_state", - "get_health_metrics", - "get_statistics", - "check_connection", - "get_force_torque", - "zero_force_torque", - "get_digital_inputs", - "set_digital_outputs", - "get_analog_inputs", - "get_gripper_state", - ], - } - - -# Expose blueprint for declarative composition (compatible with dimos framework) -xarm_driver = XArmDriver.blueprint diff --git a/dimos/hardware/manipulators_old/xarm/xarm_wrapper.py b/dimos/hardware/manipulators_old/xarm/xarm_wrapper.py deleted file mode 100644 index a743c0e3c7..0000000000 --- a/dimos/hardware/manipulators_old/xarm/xarm_wrapper.py +++ /dev/null @@ -1,564 +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. - -"""XArm SDK wrapper implementation.""" - -import logging -import math -from typing import Any - -from ..base.sdk_interface import BaseManipulatorSDK, ManipulatorInfo - - -class XArmSDKWrapper(BaseManipulatorSDK): - """SDK wrapper for XArm manipulators. - - This wrapper translates XArm's native SDK (which uses degrees and mm) - to our standard interface (radians and meters). - """ - - def __init__(self) -> None: - """Initialize the XArm SDK wrapper.""" - self.logger = logging.getLogger(self.__class__.__name__) - self.native_sdk: Any = None - self.dof = 7 # Default, will be updated on connect - self._connected = False - - # ============= Connection Management ============= - - def connect(self, config: dict[str, Any]) -> bool: - """Connect to XArm controller. - - Args: - config: Configuration with 'ip' and optionally 'dof' (5, 6, or 7) - - Returns: - True if connection successful - """ - try: - from xarm import XArmAPI - - ip = config.get("ip", "192.168.1.100") - self.dof = config.get("dof", 7) - - self.logger.info(f"Connecting to XArm at {ip} (DOF: {self.dof})...") - - # Create XArm API instance - # XArm SDK uses degrees by default, we'll convert to radians - self.native_sdk = XArmAPI(ip, is_radian=False) - - # Check connection - if self.native_sdk.connected: - # Initialize XArm - self.native_sdk.motion_enable(True) - self.native_sdk.set_mode(1) # Servo mode for high-frequency control - self.native_sdk.set_state(0) # Ready state - - self._connected = True - self.logger.info( - f"Successfully connected to XArm (version: {self.native_sdk.version})" - ) - return True - else: - self.logger.error("Failed to connect to XArm") - return False - - except ImportError: - self.logger.error("XArm SDK not installed. Please install: pip install xArm-Python-SDK") - return False - except Exception as e: - self.logger.error(f"Connection failed: {e}") - return False - - def disconnect(self) -> None: - """Disconnect from XArm controller.""" - if self.native_sdk: - try: - self.native_sdk.disconnect() - self._connected = False - self.logger.info("Disconnected from XArm") - except: - pass - finally: - self.native_sdk = None - - def is_connected(self) -> bool: - """Check if connected to XArm. - - Returns: - True if connected - """ - return self._connected and self.native_sdk and self.native_sdk.connected - - # ============= Joint State Query ============= - - def get_joint_positions(self) -> list[float]: - """Get current joint positions. - - Returns: - Joint positions in RADIANS - """ - code, angles = self.native_sdk.get_servo_angle() - if code != 0: - raise RuntimeError(f"XArm error getting positions: {code}") - - # Convert degrees to radians - positions = [math.radians(angle) for angle in angles[: self.dof]] - return positions - - def get_joint_velocities(self) -> list[float]: - """Get current joint velocities. - - Returns: - Joint velocities in RAD/S - """ - # XArm doesn't directly provide velocities in older versions - # Try to get from realtime data if available - if hasattr(self.native_sdk, "get_joint_speeds"): - code, speeds = self.native_sdk.get_joint_speeds() - if code == 0: - # Convert deg/s to rad/s - return [math.radians(speed) for speed in speeds[: self.dof]] - - # Return zeros if not available - return [0.0] * self.dof - - def get_joint_efforts(self) -> list[float]: - """Get current joint efforts/torques. - - Returns: - Joint efforts in Nm - """ - # Try to get joint torques - if hasattr(self.native_sdk, "get_joint_torques"): - code, torques = self.native_sdk.get_joint_torques() - if code == 0: - return list(torques[: self.dof]) - - # Return zeros if not available - return [0.0] * self.dof - - # ============= Joint Motion Control ============= - - def set_joint_positions( - self, - positions: list[float], - _velocity: float = 1.0, - _acceleration: float = 1.0, - _wait: bool = False, - ) -> bool: - """Move joints to target positions using servo mode. - - Args: - positions: Target positions in RADIANS - _velocity: UNUSED in servo mode (kept for interface compatibility) - _acceleration: UNUSED in servo mode (kept for interface compatibility) - _wait: UNUSED in servo mode (kept for interface compatibility) - - Returns: - True if command accepted - """ - # Convert radians to degrees - degrees = [math.degrees(pos) for pos in positions] - - # Use set_servo_angle_j for high-frequency servo control (100Hz+) - # This sends immediate position commands without trajectory planning - # Requires mode 1 (servo mode) and executes only the last instruction - code = self.native_sdk.set_servo_angle_j(degrees, speed=100, mvacc=500, wait=False) - - return bool(code == 0) - - def set_joint_velocities(self, velocities: list[float]) -> bool: - """Set joint velocity targets. - - Args: - velocities: Target velocities in RAD/S - - Returns: - True if command accepted - """ - # Check if velocity control is supported - if not hasattr(self.native_sdk, "vc_set_joint_velocity"): - self.logger.warning("Velocity control not supported in this XArm version") - return False - - # Convert rad/s to deg/s - deg_velocities = [math.degrees(vel) for vel in velocities] - - # Set to velocity control mode if needed - if self.native_sdk.mode != 4: - self.native_sdk.set_mode(4) # Joint velocity mode - - # Send velocity command - code = self.native_sdk.vc_set_joint_velocity(deg_velocities) - return bool(code == 0) - - def set_joint_efforts(self, efforts: list[float]) -> bool: - """Set joint effort/torque targets. - - Args: - efforts: Target efforts in Nm - - Returns: - True if command accepted - """ - # Check if torque control is supported - if not hasattr(self.native_sdk, "set_joint_torque"): - self.logger.warning("Torque control not supported in this XArm version") - return False - - # Send torque command - code = self.native_sdk.set_joint_torque(efforts) - return bool(code == 0) - - def stop_motion(self) -> bool: - """Stop all ongoing motion. - - Returns: - True if stop successful - """ - # XArm emergency stop - code = self.native_sdk.emergency_stop() - - # Re-enable after stop - if code == 0: - self.native_sdk.set_state(0) # Clear stop state - self.native_sdk.motion_enable(True) - - return bool(code == 0) - - # ============= Servo Control ============= - - def enable_servos(self) -> bool: - """Enable motor control. - - Returns: - True if servos enabled - """ - code1 = self.native_sdk.motion_enable(True) - code2 = self.native_sdk.set_state(0) # Ready state - code3 = self.native_sdk.set_mode(1) # Servo mode - return bool(code1 == 0 and code2 == 0 and code3 == 0) - - def disable_servos(self) -> bool: - """Disable motor control. - - Returns: - True if servos disabled - """ - code = self.native_sdk.motion_enable(False) - return bool(code == 0) - - def are_servos_enabled(self) -> bool: - """Check if servos are enabled. - - Returns: - True if enabled - """ - # Check motor state - return bool(self.native_sdk.mode == 1 and self.native_sdk.mode != 4) - - # ============= System State ============= - - def get_robot_state(self) -> dict[str, Any]: - """Get current robot state. - - Returns: - State dictionary - """ - return { - "state": self.native_sdk.state, # 0=ready, 1=pause, 2=stop, 3=running, 4=error - "mode": self.native_sdk.mode, # 0=position, 1=servo, 4=joint_vel, 5=cart_vel - "error_code": self.native_sdk.error_code, - "warn_code": self.native_sdk.warn_code, - "is_moving": self.native_sdk.state == 3, - "cmd_num": self.native_sdk.cmd_num, - } - - def get_error_code(self) -> int: - """Get current error code. - - Returns: - Error code (0 = no error) - """ - return int(self.native_sdk.error_code) - - def get_error_message(self) -> str: - """Get human-readable error message. - - Returns: - Error message string - """ - if self.native_sdk.error_code == 0: - return "" - - # XArm error codes (partial list) - error_map = { - 1: "Emergency stop button pressed", - 2: "Joint limit exceeded", - 3: "Command reply timeout", - 4: "Power supply error", - 5: "Motor overheated", - 6: "Motor driver error", - 7: "Other error", - 10: "Servo error", - 11: "Joint collision", - 12: "Tool IO error", - 13: "Tool communication error", - 14: "Kinematic error", - 15: "Self collision", - 16: "Joint overheated", - 17: "Planning error", - 19: "Force control error", - 20: "Joint current overlimit", - 21: "TCP command overlimit", - 22: "Overspeed", - } - - return error_map.get( - self.native_sdk.error_code, f"Unknown error {self.native_sdk.error_code}" - ) - - def clear_errors(self) -> bool: - """Clear error states. - - Returns: - True if errors cleared - """ - code = self.native_sdk.clean_error() - if code == 0: - # Reset to ready state - self.native_sdk.set_state(0) - return bool(code == 0) - - def emergency_stop(self) -> bool: - """Execute emergency stop. - - Returns: - True if e-stop executed - """ - code = self.native_sdk.emergency_stop() - return bool(code == 0) - - # ============= Information ============= - - def get_info(self) -> ManipulatorInfo: - """Get manipulator information. - - Returns: - ManipulatorInfo object - """ - return ManipulatorInfo( - vendor="UFACTORY", - model=f"xArm{self.dof}", - dof=self.dof, - firmware_version=self.native_sdk.version if self.native_sdk else None, - serial_number=self.native_sdk.get_servo_version()[1][0] if self.native_sdk else None, - ) - - def get_joint_limits(self) -> tuple[list[float], list[float]]: - """Get joint position limits. - - Returns: - Tuple of (lower_limits, upper_limits) in RADIANS - """ - # XArm joint limits in degrees (approximate, varies by model) - if self.dof == 7: - lower_deg = [-360, -118, -360, -233, -360, -97, -360] - upper_deg = [360, 118, 360, 11, 360, 180, 360] - elif self.dof == 6: - lower_deg = [-360, -118, -225, -11, -360, -97] - upper_deg = [360, 118, 11, 225, 360, 180] - else: # 5 DOF - lower_deg = [-360, -118, -225, -97, -360] - upper_deg = [360, 118, 11, 180, 360] - - # Convert to radians - lower_rad = [math.radians(d) for d in lower_deg[: self.dof]] - upper_rad = [math.radians(d) for d in upper_deg[: self.dof]] - - return (lower_rad, upper_rad) - - def get_velocity_limits(self) -> list[float]: - """Get joint velocity limits. - - Returns: - Maximum velocities in RAD/S - """ - # XArm max velocities in deg/s (default) - max_vel_deg = 180.0 - - # Convert to rad/s - max_vel_rad = math.radians(max_vel_deg) - return [max_vel_rad] * self.dof - - def get_acceleration_limits(self) -> list[float]: - """Get joint acceleration limits. - - Returns: - Maximum accelerations in RAD/S² - """ - # XArm max acceleration in deg/s² (default) - max_acc_deg = 1145.0 - - # Convert to rad/s² - max_acc_rad = math.radians(max_acc_deg) - return [max_acc_rad] * self.dof - - # ============= Optional Methods ============= - - def get_cartesian_position(self) -> dict[str, float] | None: - """Get current end-effector pose. - - Returns: - Pose dict or None if not supported - """ - code, pose = self.native_sdk.get_position() - if code != 0: - return None - - # XArm returns [x, y, z (mm), roll, pitch, yaw (degrees)] - return { - "x": pose[0] / 1000.0, # mm to meters - "y": pose[1] / 1000.0, - "z": pose[2] / 1000.0, - "roll": math.radians(pose[3]), - "pitch": math.radians(pose[4]), - "yaw": math.radians(pose[5]), - } - - def set_cartesian_position( - self, - pose: dict[str, float], - velocity: float = 1.0, - acceleration: float = 1.0, - wait: bool = False, - ) -> bool: - """Move end-effector to target pose. - - Args: - pose: Target pose dict - velocity: Max velocity fraction (0-1) - acceleration: Max acceleration fraction (0-1) - wait: Block until complete - - Returns: - True if command accepted - """ - # Convert to XArm format - xarm_pose = [ - pose["x"] * 1000.0, # meters to mm - pose["y"] * 1000.0, - pose["z"] * 1000.0, - math.degrees(pose["roll"]), - math.degrees(pose["pitch"]), - math.degrees(pose["yaw"]), - ] - - # XArm max Cartesian speed (default 500 mm/s) - max_speed = 500.0 - speed = max_speed * velocity - - # XArm max Cartesian acceleration (default 2000 mm/s²) - max_acc = 2000.0 - acc = max_acc * acceleration - - code = self.native_sdk.set_position(xarm_pose, radius=-1, speed=speed, mvacc=acc, wait=wait) - - return bool(code == 0) - - def get_force_torque(self) -> list[float] | None: - """Get F/T sensor reading. - - Returns: - [fx, fy, fz, tx, ty, tz] or None - """ - if hasattr(self.native_sdk, "get_ft_sensor_data"): - code, ft_data = self.native_sdk.get_ft_sensor_data() - if code == 0: - return list(ft_data) - return None - - def zero_force_torque(self) -> bool: - """Zero the F/T sensor. - - Returns: - True if successful - """ - if hasattr(self.native_sdk, "set_ft_sensor_zero"): - code = self.native_sdk.set_ft_sensor_zero() - return bool(code == 0) - return False - - def get_gripper_position(self) -> float | None: - """Get gripper position. - - Returns: - Position in meters or None - """ - if hasattr(self.native_sdk, "get_gripper_position"): - code, pos = self.native_sdk.get_gripper_position() - if code == 0: - # Convert mm to meters - return float(pos / 1000.0) - return None - - def set_gripper_position(self, position: float, force: float = 1.0) -> bool: - """Set gripper position. - - Args: - position: Target position in meters - force: Force fraction (0-1) - - Returns: - True if successful - """ - if hasattr(self.native_sdk, "set_gripper_position"): - # Convert meters to mm - pos_mm = position * 1000.0 - code = self.native_sdk.set_gripper_position(pos_mm, wait=False) - return bool(code == 0) - return False - - def set_control_mode(self, mode: str) -> bool: - """Set control mode. - - Args: - mode: 'position', 'velocity', 'torque', or 'impedance' - - Returns: - True if successful - """ - mode_map = { - "position": 0, - "velocity": 4, # Joint velocity mode - "servo": 1, # Servo mode (for torque control) - "impedance": 0, # Not directly supported, use position - } - - if mode not in mode_map: - return False - - code = self.native_sdk.set_mode(mode_map[mode]) - return bool(code == 0) - - def get_control_mode(self) -> str | None: - """Get current control mode. - - Returns: - Mode string or None - """ - mode_map = {0: "position", 1: "servo", 4: "velocity", 5: "cartesian_velocity"} - - return mode_map.get(self.native_sdk.mode, "unknown") From 142105b965a1c00e679b8cc1aafdec7b5b2810a1 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 9 Jan 2026 15:57:41 -0800 Subject: [PATCH 28/42] fixed redef error in dual trajectory setter --- dimos/manipulation/control/dual_trajectory_setter.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/dimos/manipulation/control/dual_trajectory_setter.py b/dimos/manipulation/control/dual_trajectory_setter.py index eedd1d48a0..4b54f0e3e5 100644 --- a/dimos/manipulation/control/dual_trajectory_setter.py +++ b/dimos/manipulation/control/dual_trajectory_setter.py @@ -441,14 +441,14 @@ def interactive_mode(setter: DualTrajectorySetter) -> None: elif cmd == "vel" and len(parts) >= 3: arm_name = parts[1].lower() - a: ArmState | None = ( + target_arm: ArmState | None = ( left if arm_name in ("left", "l") else right if arm_name in ("right", "r") else None ) - if a is None or a.generator is None: + if target_arm is None or target_arm.generator is None: print(" Usage: vel left|right ") continue try: @@ -456,9 +456,9 @@ def interactive_mode(setter: DualTrajectorySetter) -> None: if vel <= 0: print(" Velocity must be positive") else: - a.generator.set_limits(vel, a.generator.max_acceleration) - a.generated_trajectory = None - print(f" {a.name.upper()} max velocity: {vel:.2f} rad/s") + target_arm.generator.set_limits(vel, target_arm.generator.max_acceleration) + target_arm.generated_trajectory = None + print(f" {target_arm.name.upper()} max velocity: {vel:.2f} rad/s") except ValueError: print(" Invalid velocity value") From 1c442f70f1b186f60bf37acbee0a270d568e1c3f Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 10 Jan 2026 12:20:08 -0800 Subject: [PATCH 29/42] Fixed bugs identified by greptile overview: 1. tick_loop.py - Race condition in _route_to_hardware 2. orchestrator.py - Added hardware_added tracking list and rollback in outer except block 3. hardware_interface.py - Added disconnect() to both HardwareInterface protocol and BackendHardwareInterface 4. Added disconnect() to both HardwareInterface protocol and BackendHardwareInterface 5. orchestrator.py - Start order fix Moved super().start() to end, after tick loop starts successfully 6. trajectory_task.py - Added Empty joint_names validation --- dimos/control/hardware_interface.py | 21 ++++- dimos/control/orchestrator.py | 90 ++++++++++++-------- dimos/control/tasks/trajectory_task.py | 2 + dimos/control/tick_loop.py | 37 ++++---- dimos/hardware/manipulators/piper/backend.py | 6 +- 5 files changed, 101 insertions(+), 55 deletions(-) diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index af99d2fed3..82db93979c 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -22,10 +22,13 @@ from __future__ import annotations +import logging from typing import TYPE_CHECKING, Protocol, runtime_checkable from dimos.hardware.manipulators.spec import ControlMode +logger = logging.getLogger(__name__) + if TYPE_CHECKING: from dimos.hardware.manipulators.spec import ManipulatorBackend @@ -72,6 +75,10 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: """ ... + def disconnect(self) -> None: + """Disconnect the underlying hardware.""" + ... + class BackendHardwareInterface: """Concrete implementation wrapping a ManipulatorBackend. @@ -106,6 +113,7 @@ def __init__( # Track last commanded values for hold-last behavior self._last_commanded: dict[str, float] = {} self._initialized = False + self._warned_unknown_joints: set[str] = set() @property def hardware_id(self) -> str: @@ -122,6 +130,11 @@ def dof(self) -> int: """Degrees of freedom.""" return self._dof + def disconnect(self) -> None: + """Disconnect the underlying backend.""" + if hasattr(self._backend, "disconnect"): + self._backend.disconnect() + def read_state(self) -> dict[str, tuple[float, float, float]]: """Read state as {joint_name: (position, velocity, effort)}. @@ -152,7 +165,7 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: Returns: True if command was sent successfully """ - # Initialize on first write if needed (with simple lock to prevent race) + # Initialize on first write if needed if not self._initialized: self._initialize_last_commanded() @@ -160,6 +173,12 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: for joint_name, value in commands.items(): if joint_name in self._joint_names: self._last_commanded[joint_name] = value + elif joint_name not in self._warned_unknown_joints: + logger.warning( + f"Hardware {self._hardware_id} received command for unknown joint " + f"{joint_name}. Valid joints: {self._joint_names}" + ) + self._warned_unknown_joints.add(joint_name) # Build ordered list for backend ordered = self._build_ordered_command() diff --git a/dimos/control/orchestrator.py b/dimos/control/orchestrator.py index 0ecb9e33b9..509e0efcc1 100644 --- a/dimos/control/orchestrator.py +++ b/dimos/control/orchestrator.py @@ -179,38 +179,51 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: def _setup_from_config(self) -> None: """Create hardware and tasks from config (called on start).""" - for hw_cfg in self.config.hardware: - backend = None - try: - backend = self._create_backend_from_config(hw_cfg) - if not backend.connect(): - raise RuntimeError(f"Failed to connect to {hw_cfg.type} backend") - - if hw_cfg.auto_enable and hasattr(backend, "write_enable"): - backend.write_enable(True) - - self.add_hardware( - hw_cfg.id, - backend, - joint_prefix=hw_cfg.joint_prefix or hw_cfg.id, - ) - except Exception as e: - logger.error(f"Failed to setup hardware {hw_cfg.id}: {e}") - # Clean up connected backend on failure - if backend is not None and hasattr(backend, "disconnect"): - try: - backend.disconnect() - except Exception: - pass # Best effort cleanup - raise - - for task_cfg in self.config.tasks: - try: - task = self._create_task_from_config(task_cfg) - self.add_task(task) - except Exception as e: - logger.error(f"Failed to setup task {task_cfg.name}: {e}") - raise + hardware_added: list[str] = [] + + try: + for hw_cfg in self.config.hardware: + backend = None + try: + backend = self._create_backend_from_config(hw_cfg) + if not backend.connect(): + raise RuntimeError(f"Failed to connect to {hw_cfg.type} backend") + + if hw_cfg.auto_enable and hasattr(backend, "write_enable"): + backend.write_enable(True) + + self.add_hardware( + hw_cfg.id, + backend, + joint_prefix=hw_cfg.joint_prefix or hw_cfg.id, + ) + hardware_added.append(hw_cfg.id) + except Exception as e: + logger.error(f"Failed to setup hardware {hw_cfg.id}: {e}") + # Clean up connected backend on failure + if backend is not None and hasattr(backend, "disconnect"): + try: + backend.disconnect() + except Exception: + pass # Best effort cleanup + raise + + for task_cfg in self.config.tasks: + try: + task = self._create_task_from_config(task_cfg) + self.add_task(task) + except Exception as e: + logger.error(f"Failed to setup task {task_cfg.name}: {e}") + raise + + except Exception: + # Rollback: clean up all successfully added hardware + for hw_id in hardware_added: + try: + self.remove_hardware(hw_id) + except Exception: + pass # Best effort cleanup + raise def _create_backend_from_config(self, cfg: HardwareConfig) -> ManipulatorBackend: """Create a manipulator backend from config.""" @@ -432,8 +445,6 @@ def cancel_trajectory(self, task_name: str) -> bool: @rpc def start(self) -> None: """Start the orchestrator control loop.""" - super().start() - if self._tick_loop and self._tick_loop.tick_count > 0: logger.warning("Orchestrator already running") return @@ -457,6 +468,8 @@ def start(self) -> None: ) self._tick_loop.start() + # Only mark as started after everything succeeds + super().start() logger.info(f"ControlOrchestrator started at {self.config.tick_rate}Hz") @rpc @@ -467,6 +480,15 @@ def stop(self) -> None: if self._tick_loop: self._tick_loop.stop() + # Disconnect all hardware backends + with self._hardware_lock: + for hw_id, interface in list(self._hardware.items()): + try: + interface.disconnect() + logger.info(f"Disconnected hardware {hw_id}") + except Exception as e: + logger.error(f"Error disconnecting hardware {hw_id}: {e}") + super().stop() logger.info("ControlOrchestrator stopped") diff --git a/dimos/control/tasks/trajectory_task.py b/dimos/control/tasks/trajectory_task.py index 1c9d1bf3f3..639264adb2 100644 --- a/dimos/control/tasks/trajectory_task.py +++ b/dimos/control/tasks/trajectory_task.py @@ -85,6 +85,8 @@ def __init__(self, name: str, config: JointTrajectoryTaskConfig) -> None: name: Unique task name config: Task configuration """ + if not config.joint_names: + raise ValueError(f"JointTrajectoryTask '{name}' requires at least one joint") self._name = name self._config = config self._joint_names = frozenset(config.joint_names) diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py index 3d5aac5d36..da020dad7d 100644 --- a/dimos/control/tick_loop.py +++ b/dimos/control/tick_loop.py @@ -334,26 +334,27 @@ def _route_to_hardware( """ hw_commands: dict[str, tuple[dict[str, float], ControlMode]] = {} - for joint_name, (value, mode, _) in joint_commands.items(): - hw_id = self._joint_to_hardware.get(joint_name) - if hw_id is None: - logger.warning(f"Unknown joint {joint_name}, cannot route") - continue - - if hw_id not in hw_commands: - hw_commands[hw_id] = ({}, mode) - else: - # Check for mode conflict across joints on same hardware - existing_mode = hw_commands[hw_id][1] - if mode != existing_mode: - logger.error( - f"Mode conflict for hardware {hw_id}: joint {joint_name} wants " - f"{mode.name} but hardware already has {existing_mode.name}. " - f"Dropping command for {joint_name}." - ) + with self._hardware_lock: + for joint_name, (value, mode, _) in joint_commands.items(): + hw_id = self._joint_to_hardware.get(joint_name) + if hw_id is None: + logger.warning(f"Unknown joint {joint_name}, cannot route") continue - hw_commands[hw_id][0][joint_name] = value + if hw_id not in hw_commands: + hw_commands[hw_id] = ({}, mode) + else: + # Check for mode conflict across joints on same hardware + existing_mode = hw_commands[hw_id][1] + if mode != existing_mode: + logger.error( + f"Mode conflict for hardware {hw_id}: joint {joint_name} wants " + f"{mode.name} but hardware already has {existing_mode.name}. " + f"Dropping command for {joint_name}." + ) + continue + + hw_commands[hw_id][0][joint_name] = value return hw_commands diff --git a/dimos/hardware/manipulators/piper/backend.py b/dimos/hardware/manipulators/piper/backend.py index f673d3f41e..fe2125ae7f 100644 --- a/dimos/hardware/manipulators/piper/backend.py +++ b/dimos/hardware/manipulators/piper/backend.py @@ -45,6 +45,8 @@ class PiperBackend: """ def __init__(self, can_port: str = "can0", dof: int = 6) -> None: + if dof != 6: + raise ValueError(f"PiperBackend only supports 6 DOF (got {dof})") self._can_port = can_port self._dof = dof self._sdk: Any = None @@ -355,9 +357,9 @@ def write_enable(self, enable: bool) -> bool: try: if enable: - # Enable with retries + # Enable with retries (500ms max) attempts = 0 - max_attempts = 100 + max_attempts = 50 success = False while attempts < max_attempts: if self._sdk.EnablePiper(): From 959e6da61af4ee8c885a3948bfa5f2fcc4808461 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 10 Jan 2026 16:02:30 -0800 Subject: [PATCH 30/42] addressed greptile suggestion: hardware_interface.py - Torque mode logging fix orchestrator.py - Fail hardware removal if joints in use tick_loop.py - Rate control drift fix --- dimos/control/hardware_interface.py | 2 +- dimos/control/orchestrator.py | 17 ++++++++++++++++- dimos/control/tick_loop.py | 14 ++++++++++---- 3 files changed, 27 insertions(+), 6 deletions(-) diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 82db93979c..0f6459b565 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -190,7 +190,7 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: return self._backend.write_joint_velocities(ordered) elif mode == ControlMode.TORQUE: # Most backends don't support torque mode - # Log and return False rather than silently ignoring + logger.warning(f"Hardware {self._hardware_id} does not support torque mode") return False else: return False diff --git a/dimos/control/orchestrator.py b/dimos/control/orchestrator.py index 509e0efcc1..d6e24fad10 100644 --- a/dimos/control/orchestrator.py +++ b/dimos/control/orchestrator.py @@ -305,9 +305,24 @@ def remove_hardware(self, hardware_id: str) -> bool: return False interface = self._hardware[hardware_id] + hw_joints = set(interface.joint_names) + + with self._task_lock: + for task in self._tasks.values(): + if task.is_active(): + claimed_joints = task.claim().joints + overlap = hw_joints & claimed_joints + if overlap: + logger.error( + f"Cannot remove hardware {hardware_id}: " + f"task '{task.name}' is actively using joints {overlap}" + ) + return False + for joint_name in interface.joint_names: del self._joint_to_hardware[joint_name] + interface.disconnect() del self._hardware[hardware_id] logger.info(f"Removed hardware {hardware_id}") return True @@ -445,7 +460,7 @@ def cancel_trajectory(self, task_name: str) -> bool: @rpc def start(self) -> None: """Start the orchestrator control loop.""" - if self._tick_loop and self._tick_loop.tick_count > 0: + if self._tick_loop and self._tick_loop.is_running: logger.warning("Orchestrator already running") return diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py index da020dad7d..17c451cddb 100644 --- a/dimos/control/tick_loop.py +++ b/dimos/control/tick_loop.py @@ -106,6 +106,11 @@ def tick_count(self) -> int: """Number of ticks since start.""" return self._tick_count + @property + def is_running(self) -> bool: + """Whether the tick loop is currently running.""" + return self._running + def start(self) -> None: """Start the tick loop in a daemon thread.""" if self._running: @@ -143,10 +148,11 @@ def _loop(self) -> None: except Exception as e: logger.error(f"TickLoop tick error: {e}") - # Rate control - elapsed = time.perf_counter() - tick_start - if elapsed < period: - time.sleep(period - elapsed) + # Rate control - recalculate sleep time to account for overhead + next_tick_time = tick_start + period + sleep_time = next_tick_time - time.perf_counter() + if sleep_time > 0: + time.sleep(sleep_time) def _tick(self) -> None: """Single tick: read → compute → arbitrate → route → write.""" From 67ca8eb7f57946d290d446f038687d879c09b6a2 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 10 Jan 2026 16:05:06 -0800 Subject: [PATCH 31/42] undo change to pyproject.toml --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index c9c4ddfb2c..b0a1e61415 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -309,7 +309,7 @@ python_version = "3.12" incremental = true strict = true warn_unused_ignores = false -exclude = "^dimos/models/Detic(/|$)|^dimos/rxpy_backpressure(/|$)|^dimos/hardware/manipulators_old(/|$)|.*/test_.|.*/conftest.py*" +exclude = "^dimos/models/Detic(/|$)|^dimos/rxpy_backpressure(/|$)|.*/test_.|.*/conftest.py*" [[tool.mypy.overrides]] module = [ From 5fef95260dd2272c2eea51a8c09535d32051a4bc Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 10 Jan 2026 17:35:04 -0800 Subject: [PATCH 32/42] Replaced _running bool with threading.Event (_stop_event) for thread safety Removed duplicate _auto_start() call from __init__ - connection now only happens in start() orchestrator_client.py IPython conversion --- dimos/hardware/manipulators/xarm/arm.py | 41 +- .../control/orchestrator_client.py | 561 +++++++++--------- 2 files changed, 300 insertions(+), 302 deletions(-) diff --git a/dimos/hardware/manipulators/xarm/arm.py b/dimos/hardware/manipulators/xarm/arm.py index 5e10c1c9c3..78230270df 100644 --- a/dimos/hardware/manipulators/xarm/arm.py +++ b/dimos/hardware/manipulators/xarm/arm.py @@ -21,14 +21,16 @@ """ from dataclasses import dataclass, field +import logging import threading import time from typing import Any +logger = logging.getLogger(__name__) + from dimos.core import In, Module, ModuleConfig, Out, rpc from dimos.hardware.manipulators.spec import ( ControlMode, - DriverStatus, JointLimits, ManipulatorBackend, ManipulatorInfo, @@ -114,7 +116,8 @@ def __init__( ) # Threading state - self._running = False + self._stop_event = threading.Event() + self._stop_event.set() # Initially stopped self._control_thread: threading.Thread | None = None self._monitor_thread: threading.Thread | None = None @@ -122,13 +125,10 @@ def __init__( self._dof: int = self.config.dof self._joint_names: list[str] = [f"joint{i + 1}" for i in range(self._dof)] - # Auto-connect on initialization - self._auto_start() - def _auto_start(self) -> None: """Auto-connect to hardware on initialization.""" if not self.backend.connect(): - print(f"WARNING: Failed to connect to XArm at {self.config.ip}") + logger.warning(f"Failed to connect to XArm at {self.config.ip}") return # Update DOF from backend (in case it differs) @@ -136,7 +136,7 @@ def _auto_start(self) -> None: self._joint_names = [f"joint{i + 1}" for i in range(self._dof)] # Start threads - self._running = True + self._stop_event.clear() # Clear stop event to indicate running self._control_thread = threading.Thread( target=self._control_loop, @@ -172,25 +172,25 @@ def _subscribe_to_commands(self) -> None: def _on_joint_position_command(self, cmd: JointCommand) -> None: """Handle incoming joint position command.""" - if not self._running: + if self._stop_event.is_set(): return positions = list(cmd.positions) if cmd.positions else [] if len(positions) != self._dof: - print(f"WARNING: Position command has {len(positions)} joints, expected {self._dof}") + logger.warning(f"Position command has {len(positions)} joints, expected {self._dof}") return self.backend.write_joint_positions(positions) def _on_joint_velocity_command(self, cmd: JointCommand) -> None: """Handle incoming joint velocity command.""" - if not self._running: + if self._stop_event.is_set(): return # JointCommand uses 'positions' field for velocity commands too velocities = list(cmd.positions) if cmd.positions else [] if len(velocities) != self._dof: - print(f"WARNING: Velocity command has {len(velocities)} joints, expected {self._dof}") + logger.warning(f"Velocity command has {len(velocities)} joints, expected {self._dof}") return self.backend.write_joint_velocities(velocities) @@ -200,23 +200,20 @@ def _on_joint_velocity_command(self, cmd: JointCommand) -> None: # ========================================================================= @rpc - def start(self) -> DriverStatus: + def start(self) -> None: """Connect to XArm and start control loops (if not already running).""" - super().start() # Important: sets up transports - - if self._running: - # Already connected, just subscribe to commands - self._subscribe_to_commands() - return DriverStatus.CONNECTED + if not self._stop_event.is_set(): + # Already running + return + super().start() # Sets up transports self._auto_start() self._subscribe_to_commands() - return DriverStatus.CONNECTED if self._running else DriverStatus.ERROR @rpc def stop(self) -> None: """Stop XArm and disconnect.""" - self._running = False + self._stop_event.set() # Stop motion first try: @@ -246,7 +243,7 @@ def _control_loop(self) -> None: """High-frequency loop for joint state publishing.""" period = 1.0 / self.config.control_rate - while self._running: + while not self._stop_event.is_set(): start = time.perf_counter() try: @@ -283,7 +280,7 @@ def _monitor_loop(self) -> None: """Low-frequency loop for robot state monitoring.""" period = 1.0 / self.config.monitor_rate - while self._running: + while not self._stop_event.is_set(): start = time.perf_counter() try: diff --git a/dimos/manipulation/control/orchestrator_client.py b/dimos/manipulation/control/orchestrator_client.py index 20da6dbe7e..4e857087ca 100644 --- a/dimos/manipulation/control/orchestrator_client.py +++ b/dimos/manipulation/control/orchestrator_client.py @@ -339,258 +339,296 @@ def wait_for_completion(client: OrchestratorClient, task_name: str, timeout: flo return False -def print_help(num_joints: int, task_name: str) -> None: - """Print help message.""" - joint_args = " ".join([f"" for i in range(num_joints)]) +class OrchestratorShell: + """IPython shell interface for orchestrator control.""" + + def __init__(self, client: OrchestratorClient, initial_task: str) -> None: + self._client = client + self._current_task = initial_task + self._waypoints: list[list[float]] = [] + self._generated_trajectory: JointTrajectory | None = None + + if not client.select_task(initial_task): + raise ValueError(f"Failed to select task: {initial_task}") + + def _joints(self) -> list[str]: + return self._client.get_task_joints(self._current_task) + + def _num_joints(self) -> int: + return len(self._joints()) + + def help(self) -> None: + """Show available commands.""" + print("\nOrchestrator Client Commands:") + print("=" * 60) + print("Waypoint Commands:") + print(" here() - Add current position as waypoint") + print(" add(j1, j2, ...) - Add waypoint (degrees)") + print(" waypoints() - List all waypoints") + print(" delete(n) - Delete waypoint n") + print(" clear() - Clear all waypoints") + print("\nTrajectory Commands:") + print(" preview() - Preview generated trajectory") + print(" run() - Execute trajectory") + print(" status() - Show task status") + print(" cancel() - Cancel active trajectory") + print("\nMulti-Arm Commands:") + print(" tasks() - List all tasks") + print(" switch('task_name') - Switch to different task") + print(" hw() - List hardware") + print(" joints() - List joints for current task") + print("\nSettings:") + print(" current() - Show current joint positions") + print(" vel(value) - Set max velocity (rad/s)") + print(" accel(value) - Set max acceleration (rad/s^2)") + print("=" * 60) + + def here(self) -> None: + """Add current position as waypoint.""" + positions = self._client.get_current_positions(self._current_task) + if positions: + self._waypoints.append(positions) + self._generated_trajectory = None + print(f"Added waypoint {len(self._waypoints)}: {format_positions(positions)}") + else: + print("Could not get current positions") + + def add(self, *joints: float) -> None: + """Add waypoint with specified joint values (in degrees).""" + num_joints = self._num_joints() + if len(joints) != num_joints: + print(f"Need {num_joints} joint values, got {len(joints)}") + return + + rad_joints = [math.radians(j) for j in joints] + self._waypoints.append(rad_joints) + self._generated_trajectory = None + print(f"Added waypoint {len(self._waypoints)}: {format_positions(rad_joints)}") + + def waypoints(self) -> None: + """List all waypoints.""" + preview_waypoints(self._waypoints, self._joints()) + + def delete(self, index: int) -> None: + """Delete a waypoint by index (1-based).""" + idx = index - 1 + if 0 <= idx < len(self._waypoints): + self._waypoints.pop(idx) + self._generated_trajectory = None + print(f"Deleted waypoint {index}") + else: + print(f"Invalid index (1-{len(self._waypoints)})") + + def clear(self) -> None: + """Clear all waypoints.""" + self._waypoints.clear() + self._generated_trajectory = None + print("Cleared waypoints") + + def preview(self) -> None: + """Preview generated trajectory.""" + if len(self._waypoints) < 2: + print("Need at least 2 waypoints") + return + try: + self._generated_trajectory = self._client.generate_trajectory( + self._waypoints, self._current_task + ) + if self._generated_trajectory: + preview_trajectory(self._generated_trajectory, self._joints()) + except Exception as e: + print(f"Error: {e}") + + def run(self) -> None: + """Execute trajectory.""" + if len(self._waypoints) < 2: + print("Need at least 2 waypoints") + return + + if self._generated_trajectory is None: + self._generated_trajectory = self._client.generate_trajectory( + self._waypoints, self._current_task + ) - print("\n" + "=" * 70) - print(f"Orchestrator Client - Task: {task_name} ({num_joints} joints)") - print("=" * 70) - print("\nWaypoint Commands:") - print(f" add {joint_args}") - print(" - Add waypoint (values in degrees)") - print(" here - Add current position as waypoint") - print(" list - List all waypoints") - print(" delete - Delete waypoint n") - print(" clear - Clear all waypoints") - print("\nTrajectory Commands:") - print(" preview - Preview generated trajectory") - print(" run - Execute trajectory") - print(" status - Show task status") - print(" cancel - Cancel active trajectory") - print("\nMulti-Arm Commands:") - print(" tasks - List all tasks") - print(" switch - Switch to different task") - print(" hw - List hardware") - print(" joints - List joints for current task") - print("\nSettings:") - print(" current - Show current joint positions") - print(" vel - Set max velocity (rad/s)") - print(" accel - Set max acceleration (rad/s^2)") - print(" help - Show this help") - print(" quit - Exit") - print("=" * 70) + if self._generated_trajectory is None: + print("Failed to generate trajectory") + return + + preview_trajectory(self._generated_trajectory, self._joints()) + confirm = input("\nExecute? [y/N]: ").strip().lower() + if confirm == "y": + if self._client.execute_trajectory(self._current_task, self._generated_trajectory): + print("Trajectory started...") + wait_for_completion(self._client, self._current_task) + else: + print("Failed to start trajectory") + + def status(self) -> None: + """Show task status.""" + status = self._client.get_trajectory_status(self._current_task) + print(f"\nTask: {self._current_task}") + print(f" Active: {status.get('active', False)}") + print(f" State: {status.get('state', 'UNKNOWN')}") + if "progress" in status: + print(f" Progress: {status['progress'] * 100:.1f}%") + + def cancel(self) -> None: + """Cancel active trajectory.""" + if self._client.cancel_trajectory(self._current_task): + print("Cancelled") + else: + print("Cancel failed") + + def tasks(self) -> None: + """List all tasks.""" + all_tasks = self._client.list_tasks() + active = self._client.get_active_tasks() + print("\nTasks:") + for t in all_tasks: + marker = "* " if t == self._current_task else " " + active_marker = " [ACTIVE]" if t in active else "" + t_joints = self._client.get_task_joints(t) + joint_count = len(t_joints) if t_joints else "?" + print(f"{marker}{t} ({joint_count} joints){active_marker}") + + def switch(self, task_name: str) -> None: + """Switch to a different task.""" + if self._client.select_task(task_name): + self._current_task = task_name + self._waypoints.clear() + self._generated_trajectory = None + joints = self._joints() + print(f"Switched to {self._current_task} ({len(joints)} joints)") + print(f"Joints: {', '.join(joints)}") + else: + print(f"Failed to switch to {task_name}") + + def hw(self) -> None: + """List hardware.""" + hardware = self._client.list_hardware() + print(f"\nHardware: {', '.join(hardware)}") + + def joints(self) -> None: + """List joints for current task.""" + joints = self._joints() + print(f"\nJoints for {self._current_task}:") + for i, j in enumerate(joints): + pos = self._client.get_joint_positions().get(j, 0.0) + print(f" {i + 1}. {j}: {math.degrees(pos):.1f} deg") + + def current(self) -> None: + """Show current joint positions.""" + positions = self._client.get_current_positions(self._current_task) + if positions: + print(f"Current: {format_positions(positions)}") + else: + print("Could not get positions") + def vel(self, value: float | None = None) -> None: + """Set or show max velocity (rad/s).""" + if value is None: + gen = self._client._generators.get(self._current_task) + if gen: + print(f"Max velocity: {gen.max_velocity[0]:.2f} rad/s") + return -def interactive_mode(client: OrchestratorClient, initial_task: str) -> None: - """Interactive mode for trajectory planning and execution.""" - # Setup initial task - if not client.select_task(initial_task): - print(f"Failed to select task: {initial_task}") - return + if value <= 0: + print("Velocity must be positive") + return + + self._client.set_velocity_limit(value, self._current_task) + self._generated_trajectory = None + print(f"Max velocity: {value:.2f} rad/s") - current_task = initial_task - waypoints: list[list[float]] = [] - generated_trajectory: JointTrajectory | None = None + def accel(self, value: float | None = None) -> None: + """Set or show max acceleration (rad/s^2).""" + if value is None: + gen = self._client._generators.get(self._current_task) + if gen: + print(f"Max acceleration: {gen.max_acceleration[0]:.2f} rad/s^2") + return - joints = client.get_task_joints(current_task) - num_joints = len(joints) + if value <= 0: + print("Acceleration must be positive") + return - print_help(num_joints, current_task) + self._client.set_acceleration_limit(value, self._current_task) + self._generated_trajectory = None + print(f"Max acceleration: {value:.2f} rad/s^2") + +def interactive_mode(client: OrchestratorClient, initial_task: str) -> None: + """Start IPython interactive mode.""" + import IPython + + shell = OrchestratorShell(client, initial_task) + + print("\n" + "=" * 60) + print(f"Orchestrator Client (IPython) - Task: {initial_task}") + print("=" * 60) + print(f"Joints: {', '.join(shell._joints())}") + print("\nType help() for available commands") + print("=" * 60 + "\n") + + IPython.start_ipython( + argv=[], + user_ns={ + "help": shell.help, + "here": shell.here, + "add": shell.add, + "waypoints": shell.waypoints, + "delete": shell.delete, + "clear": shell.clear, + "preview": shell.preview, + "run": shell.run, + "status": shell.status, + "cancel": shell.cancel, + "tasks": shell.tasks, + "switch": shell.switch, + "hw": shell.hw, + "joints": shell.joints, + "current": shell.current, + "vel": shell.vel, + "accel": shell.accel, + "client": client, + "shell": shell, + }, + ) + + +def _run_client(client: OrchestratorClient, task: str, vel: float, accel: float) -> int: + """Run the client with the given configuration.""" try: - while True: - # Prompt shows task and waypoint count - active = client.get_active_tasks() - active_marker = "*" if current_task in active else "" - prompt = f"[{current_task}{active_marker}:{len(waypoints)}wp] > " - - try: - line = input(prompt).strip() - except EOFError: - break - - if not line: - continue - - parts = line.split() - cmd = parts[0].lower() - - # === Waypoint commands === - - if cmd == "add": - if len(parts) < num_joints + 1: - print(f"Need {num_joints} joint values") - continue - values = parse_joint_input(" ".join(parts[1 : num_joints + 1]), num_joints) - if values: - waypoints.append(values) - generated_trajectory = None - print(f"Added waypoint {len(waypoints)}: {format_positions(values)}") - else: - print(f"Invalid values. Need {num_joints} numbers (degrees)") - - elif cmd == "here": - positions = client.get_current_positions(current_task) - if positions: - waypoints.append(positions) - generated_trajectory = None - print(f"Added waypoint {len(waypoints)}: {format_positions(positions)}") - else: - print("Could not get current positions") - - elif cmd == "list": - preview_waypoints(waypoints, joints) - - elif cmd == "delete": - if len(parts) < 2: - print("Usage: delete ") - continue - try: - idx = int(parts[1]) - 1 - if 0 <= idx < len(waypoints): - waypoints.pop(idx) - generated_trajectory = None - print(f"Deleted waypoint {idx + 1}") - else: - print(f"Invalid index (1-{len(waypoints)})") - except ValueError: - print("Invalid index") - - elif cmd == "clear": - waypoints.clear() - generated_trajectory = None - print("Cleared waypoints") - - # === Trajectory commands === - - elif cmd == "preview": - if len(waypoints) < 2: - print("Need at least 2 waypoints") - continue - try: - generated_trajectory = client.generate_trajectory(waypoints, current_task) - if generated_trajectory: - preview_trajectory(generated_trajectory, joints) - except Exception as e: - print(f"Error: {e}") - - elif cmd == "run": - if len(waypoints) < 2: - print("Need at least 2 waypoints") - continue - - if generated_trajectory is None: - generated_trajectory = client.generate_trajectory(waypoints, current_task) - - if generated_trajectory is None: - print("Failed to generate trajectory") - continue - - preview_trajectory(generated_trajectory, joints) - confirm = input("\nExecute? [y/N]: ").strip().lower() - if confirm == "y": - if client.execute_trajectory(current_task, generated_trajectory): - print("Trajectory started...") - wait_for_completion(client, current_task) - else: - print("Failed to start trajectory") - - elif cmd == "status": - status = client.get_trajectory_status(current_task) - print(f"\nTask: {current_task}") - print(f" Active: {status.get('active', False)}") - print(f" State: {status.get('state', 'UNKNOWN')}") - if "progress" in status: - print(f" Progress: {status['progress'] * 100:.1f}%") - - elif cmd == "cancel": - if client.cancel_trajectory(current_task): - print("Cancelled") - else: - print("Cancel failed") - - # === Multi-arm commands === - - elif cmd == "tasks": - tasks = client.list_tasks() - active = client.get_active_tasks() - print("\nTasks:") - for t in tasks: - marker = "* " if t == current_task else " " - active_marker = " [ACTIVE]" if t in active else "" - t_joints = client.get_task_joints(t) if t in client._task_joints else [] - joint_count = len(t_joints) if t_joints else "?" - print(f"{marker}{t} ({joint_count} joints){active_marker}") - - elif cmd == "switch": - if len(parts) < 2: - print("Usage: switch ") - continue - new_task = parts[1] - if client.select_task(new_task): - current_task = new_task - joints = client.get_task_joints(current_task) - num_joints = len(joints) - waypoints.clear() - generated_trajectory = None - print(f"Switched to {current_task} ({num_joints} joints)") - print(f"Joints: {', '.join(joints)}") - - elif cmd == "hw": - hardware = client.list_hardware() - print(f"\nHardware: {', '.join(hardware)}") - - elif cmd == "joints": - print(f"\nJoints for {current_task}:") - for i, j in enumerate(joints): - pos = client.get_joint_positions().get(j, 0.0) - print(f" {i + 1}. {j}: {math.degrees(pos):.1f} deg") - - # === Settings === - - elif cmd == "current": - positions = client.get_current_positions(current_task) - if positions: - print(f"Current: {format_positions(positions)}") - else: - print("Could not get positions") - - elif cmd == "vel": - if len(parts) < 2: - gen = client._generators.get(current_task) - if gen: - print(f"Max velocity: {gen.max_velocity[0]:.2f} rad/s") - continue - try: - vel = float(parts[1]) - if vel <= 0: - print("Velocity must be positive") - else: - client.set_velocity_limit(vel, current_task) - generated_trajectory = None - print(f"Max velocity: {vel:.2f} rad/s") - except ValueError: - print("Invalid velocity") - - elif cmd == "accel": - if len(parts) < 2: - gen = client._generators.get(current_task) - if gen: - print(f"Max acceleration: {gen.max_acceleration[0]:.2f} rad/s^2") - continue - try: - accel = float(parts[1]) - if accel <= 0: - print("Acceleration must be positive") - else: - client.set_acceleration_limit(accel, current_task) - generated_trajectory = None - print(f"Max acceleration: {accel:.2f} rad/s^2") - except ValueError: - print("Invalid acceleration") - - elif cmd == "help": - print_help(num_joints, current_task) - - elif cmd in ("quit", "exit", "q"): - break + hardware = client.list_hardware() + tasks = client.list_tasks() - else: - print(f"Unknown command: {cmd} (type 'help' for commands)") + if not hardware: + print("\nWarning: No hardware found. Is the orchestrator running?") + print("Start with: dimos run orchestrator-mock") + response = input("Continue anyway? [y/N]: ").strip().lower() + if response != "y": + return 0 + else: + print(f"Hardware: {', '.join(hardware)}") + print(f"Tasks: {', '.join(tasks)}") - except KeyboardInterrupt: - print("\n\nExiting...") + except Exception as e: + print(f"\nConnection error: {e}") + print("Make sure orchestrator is running: dimos run orchestrator-mock") + return 1 + + if task not in tasks and tasks: + print(f"\nTask '{task}' not found.") + print(f"Available: {', '.join(tasks)}") + task = tasks[0] + print(f"Using '{task}'") + + if client.select_task(task): + client.set_velocity_limit(vel, task) + client.set_acceleration_limit(accel, task) + + interactive_mode(client, task) + return 0 def main() -> int: @@ -638,48 +676,11 @@ def main() -> int: print("\nConnecting to ControlOrchestrator via RPC...") client = OrchestratorClient() - - # Check connection - try: - hardware = client.list_hardware() - tasks = client.list_tasks() - - if not hardware: - print("\nWarning: No hardware found. Is the orchestrator running?") - print("Start with: dimos run orchestrator-mock") - response = input("Continue anyway? [y/N]: ").strip().lower() - if response != "y": - client.stop() - return 0 - else: - print(f"Hardware: {', '.join(hardware)}") - print(f"Tasks: {', '.join(tasks)}") - - except Exception as e: - print(f"\nConnection error: {e}") - print("Make sure orchestrator is running: dimos run orchestrator-mock") - client.stop() - return 1 - - # Find initial task - if args.task not in tasks and tasks: - print(f"\nTask '{args.task}' not found.") - print(f"Available: {', '.join(tasks)}") - args.task = tasks[0] - print(f"Using '{args.task}'") - - # Set velocity/acceleration limits - if client.select_task(args.task): - client.set_velocity_limit(args.vel, args.task) - client.set_acceleration_limit(args.accel, args.task) - try: - interactive_mode(client, args.task) + return _run_client(client, args.task, args.vel, args.accel) finally: client.stop() - return 0 - if __name__ == "__main__": try: From 81f412355677522fa374201def4b726182c2d09c Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 10 Jan 2026 17:43:11 -0800 Subject: [PATCH 33/42] added type ignore for ipythin --- dimos/manipulation/control/orchestrator_client.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/manipulation/control/orchestrator_client.py b/dimos/manipulation/control/orchestrator_client.py index 4e857087ca..84e85dfb3d 100644 --- a/dimos/manipulation/control/orchestrator_client.py +++ b/dimos/manipulation/control/orchestrator_client.py @@ -570,7 +570,7 @@ def interactive_mode(client: OrchestratorClient, initial_task: str) -> None: print("\nType help() for available commands") print("=" * 60 + "\n") - IPython.start_ipython( + IPython.start_ipython( # type: ignore[no-untyped-call] argv=[], user_ns={ "help": shell.help, From 449960ad3e19913ca3be955a41ef8d6482c6eb16 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sat, 10 Jan 2026 19:03:49 -0800 Subject: [PATCH 34/42] removed check for has attribute in hardware interface Moved super.start() at the beginning replaced running bool with stop_event in tick_loop to improve thread safety removed default ip from init removed simple dataclasses test --- dimos/control/hardware_interface.py | 3 +- dimos/control/orchestrator.py | 6 ++-- dimos/control/test_control.py | 34 --------------------- dimos/control/tick_loop.py | 13 ++++---- dimos/hardware/manipulators/xarm/backend.py | 2 +- 5 files changed, 12 insertions(+), 46 deletions(-) diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 0f6459b565..7710874547 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -132,8 +132,7 @@ def dof(self) -> int: def disconnect(self) -> None: """Disconnect the underlying backend.""" - if hasattr(self._backend, "disconnect"): - self._backend.disconnect() + self._backend.disconnect() def read_state(self) -> dict[str, tuple[float, float, float]]: """Read state as {joint_name: (position, velocity, effort)}. diff --git a/dimos/control/orchestrator.py b/dimos/control/orchestrator.py index d6e24fad10..f355b3974a 100644 --- a/dimos/control/orchestrator.py +++ b/dimos/control/orchestrator.py @@ -464,6 +464,8 @@ def start(self) -> None: logger.warning("Orchestrator already running") return + super().start() + # Setup hardware and tasks from config (if any) if self.config.hardware or self.config.tasks: self._setup_from_config() @@ -483,8 +485,6 @@ def start(self) -> None: ) self._tick_loop.start() - # Only mark as started after everything succeeds - super().start() logger.info(f"ControlOrchestrator started at {self.config.tick_rate}Hz") @rpc @@ -497,7 +497,7 @@ def stop(self) -> None: # Disconnect all hardware backends with self._hardware_lock: - for hw_id, interface in list(self._hardware.items()): + for hw_id, interface in self._hardware.items(): try: interface.disconnect() logger.info(f"Disconnected hardware {hw_id}") diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index d27264e063..8924b35e43 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -109,29 +109,6 @@ def orchestrator_state(): return OrchestratorState(joints=joints, t_now=time.perf_counter(), dt=0.01) -# ============================================================================= -# Test ResourceClaim -# ============================================================================= - - -class TestResourceClaim: - def test_create_claim(self): - claim = ResourceClaim( - joints=frozenset({"joint1", "joint2"}), - priority=10, - ) - assert claim.joints == frozenset({"joint1", "joint2"}) - assert claim.priority == 10 - - def test_claim_immutable(self): - claim = ResourceClaim( - joints=frozenset({"joint1"}), - priority=5, - ) - with pytest.raises(AttributeError): - claim.priority = 20 - - # ============================================================================= # Test JointCommandOutput # ============================================================================= @@ -179,17 +156,6 @@ def test_no_values_returns_none(self): class TestJointStateSnapshot: - def test_create_snapshot(self): - snapshot = JointStateSnapshot( - joint_positions={"j1": 0.5, "j2": 1.0}, - joint_velocities={"j1": 0.0, "j2": 0.1}, - joint_efforts={"j1": 1.0, "j2": 2.0}, - timestamp=100.0, - ) - assert snapshot.joint_positions["j1"] == 0.5 - assert snapshot.joint_velocities["j2"] == 0.1 - assert snapshot.timestamp == 100.0 - def test_get_position(self): snapshot = JointStateSnapshot( joint_positions={"j1": 0.5, "j2": 1.0}, diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py index 17c451cddb..c8ed25180f 100644 --- a/dimos/control/tick_loop.py +++ b/dimos/control/tick_loop.py @@ -96,7 +96,8 @@ def __init__( self._frame_id = frame_id self._log_ticks = log_ticks - self._running = False + self._stop_event = threading.Event() + self._stop_event.set() # Initially stopped self._tick_thread: threading.Thread | None = None self._last_tick_time: float = 0.0 self._tick_count: int = 0 @@ -109,15 +110,15 @@ def tick_count(self) -> int: @property def is_running(self) -> bool: """Whether the tick loop is currently running.""" - return self._running + return not self._stop_event.is_set() def start(self) -> None: """Start the tick loop in a daemon thread.""" - if self._running: + if not self._stop_event.is_set(): logger.warning("TickLoop already running") return - self._running = True + self._stop_event.clear() self._last_tick_time = time.perf_counter() self._tick_count = 0 @@ -131,7 +132,7 @@ def start(self) -> None: def stop(self) -> None: """Stop the tick loop.""" - self._running = False + self._stop_event.set() if self._tick_thread and self._tick_thread.is_alive(): self._tick_thread.join(timeout=2.0) logger.info("TickLoop stopped") @@ -140,7 +141,7 @@ def _loop(self) -> None: """Main control loop - deterministic read → compute → arbitrate → write.""" period = 1.0 / self._tick_rate - while self._running: + while not self._stop_event.is_set(): tick_start = time.perf_counter() try: diff --git a/dimos/hardware/manipulators/xarm/backend.py b/dimos/hardware/manipulators/xarm/backend.py index 9ada032a3f..33731a6b26 100644 --- a/dimos/hardware/manipulators/xarm/backend.py +++ b/dimos/hardware/manipulators/xarm/backend.py @@ -47,7 +47,7 @@ class XArmBackend: - Velocities: XArm uses deg/s, we use rad/s """ - def __init__(self, ip: str = "192.168.1.185", dof: int = 6) -> None: + def __init__(self, ip: str, dof: int = 6) -> None: self._ip = ip self._dof = dof self._arm: XArmAPI | None = None From 8f76a094c80e1d60ed46e0ccb7c1ce284eff4413 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Mon, 12 Jan 2026 23:15:14 -0800 Subject: [PATCH 35/42] orchestrator.py: Use match statement for backend factory, restructure backend cleanup task.py: Use match statement in get_values() tick_loop.py: Add JointWinner NamedTuple for cleaner arbitration logic xarm/backend.py: Extract unit conversions into static helper methods --- dimos/control/orchestrator.py | 54 ++- dimos/control/task.py | 16 +- dimos/control/tick_loop.py | 80 ++-- dimos/hardware/manipulators/piper/arm.py | 430 -------------------- dimos/hardware/manipulators/xarm/arm.py | 423 ------------------- dimos/hardware/manipulators/xarm/backend.py | 65 +-- 6 files changed, 115 insertions(+), 953 deletions(-) delete mode 100644 dimos/hardware/manipulators/piper/arm.py delete mode 100644 dimos/hardware/manipulators/xarm/arm.py diff --git a/dimos/control/orchestrator.py b/dimos/control/orchestrator.py index f355b3974a..1fbbd6a564 100644 --- a/dimos/control/orchestrator.py +++ b/dimos/control/orchestrator.py @@ -27,6 +27,7 @@ from __future__ import annotations +from collections.abc import Callable from dataclasses import dataclass, field import threading import time @@ -183,9 +184,8 @@ def _setup_from_config(self) -> None: try: for hw_cfg in self.config.hardware: - backend = None + backend = self._create_backend_from_config(hw_cfg) try: - backend = self._create_backend_from_config(hw_cfg) if not backend.connect(): raise RuntimeError(f"Failed to connect to {hw_cfg.type} backend") @@ -200,12 +200,10 @@ def _setup_from_config(self) -> None: hardware_added.append(hw_cfg.id) except Exception as e: logger.error(f"Failed to setup hardware {hw_cfg.id}: {e}") - # Clean up connected backend on failure - if backend is not None and hasattr(backend, "disconnect"): - try: - backend.disconnect() - except Exception: - pass # Best effort cleanup + try: + backend.disconnect() + except Exception: + pass # Best effort cleanup raise for task_cfg in self.config.tasks: @@ -227,27 +225,23 @@ def _setup_from_config(self) -> None: def _create_backend_from_config(self, cfg: HardwareConfig) -> ManipulatorBackend: """Create a manipulator backend from config.""" - backend_type = cfg.type.lower() + match cfg.type.lower(): + case "mock": + from dimos.hardware.manipulators.mock import MockBackend - if backend_type == "mock": - from dimos.hardware.manipulators.mock import MockBackend + return MockBackend(dof=cfg.dof) + case "xarm": + if cfg.ip is None: + raise ValueError("ip is required for xarm backend") + from dimos.hardware.manipulators.xarm import XArmBackend - return MockBackend(dof=cfg.dof) + return XArmBackend(ip=cfg.ip, dof=cfg.dof) + case "piper": + from dimos.hardware.manipulators.piper import PiperBackend - elif backend_type == "xarm": - if cfg.ip is None: - raise ValueError("ip is required for xarm backend") - from dimos.hardware.manipulators.xarm import XArmBackend - - return XArmBackend(ip=cfg.ip, dof=cfg.dof) - - elif backend_type == "piper": - from dimos.hardware.manipulators.piper import PiperBackend - - return PiperBackend(can_port=cfg.can_port or "can0", dof=cfg.dof) - - else: - raise ValueError(f"Unknown backend type: {backend_type}") + return PiperBackend(can_port=cfg.can_port or "can0", dof=cfg.dof) + case _: + raise ValueError(f"Unknown backend type: {cfg.type}") def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask: """Create a control task from config.""" @@ -410,7 +404,7 @@ def execute_trajectory(self, task_name: str, trajectory: JointTrajectory) -> boo logger.warning(f"Task {task_name} doesn't support execute()") return False - t_now = time.perf_counter() + t_now: float = time.perf_counter() logger.info( f"Executing trajectory on {task_name}: " f"{len(trajectory.points)} points, duration={trajectory.duration:.3f}s" @@ -428,12 +422,12 @@ def get_trajectory_status(self, task_name: str) -> dict[str, Any]: result: dict[str, Any] = {"active": task.is_active()} if hasattr(task, "get_state"): - state = task.get_state() + state: TrajectoryState = task.get_state() # type: ignore[attr-defined] result["state"] = state.name if isinstance(state, TrajectoryState) else str(state) if hasattr(task, "get_progress"): - t_now = time.perf_counter() - result["progress"] = task.get_progress(t_now) + t_now: float = time.perf_counter() + result["progress"] = task.get_progress(t_now) # type: ignore[attr-defined] return result diff --git a/dimos/control/task.py b/dimos/control/task.py index 03303e07f1..6f5e153520 100644 --- a/dimos/control/task.py +++ b/dimos/control/task.py @@ -152,13 +152,15 @@ def __post_init__(self) -> None: def get_values(self) -> list[float] | None: """Get the active values based on mode.""" - if self.mode == ControlMode.POSITION: - return self.positions - elif self.mode == ControlMode.VELOCITY: - return self.velocities - elif self.mode == ControlMode.TORQUE: - return self.efforts - return None + match self.mode: + case ControlMode.POSITION: + return self.positions + case ControlMode.VELOCITY: + return self.velocities + case ControlMode.TORQUE: + return self.efforts + case _: + return None # ============================================================================= diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py index c8ed25180f..b2174b9d28 100644 --- a/dimos/control/tick_loop.py +++ b/dimos/control/tick_loop.py @@ -29,7 +29,7 @@ import threading import time -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, NamedTuple from dimos.control.task import ( ControlTask, @@ -50,6 +50,15 @@ logger = setup_logger() +class JointWinner(NamedTuple): + """Tracks the winning task for a joint during arbitration.""" + + priority: int + value: float + mode: ControlMode + task_name: str + + class TickLoop: """Core tick loop for the control orchestrator. @@ -252,9 +261,8 @@ def _arbitrate( - joint_commands: {joint_name: (value, mode, task_name)} - preemptions: {preempted_task: {joint: winning_task}} """ - # Track winner per joint: joint -> (priority, value, mode, task_name) - winners: dict[str, tuple[int, float, ControlMode, str]] = {} - preemptions: dict[str, dict[str, str]] = {} + winners: dict[str, JointWinner] = {} # joint_name -> current winner + preemptions: dict[str, dict[str, str]] = {} # loser_task -> {joint: winner_task} for task, claim, output in commands: if output is None: @@ -265,43 +273,37 @@ def _arbitrate( continue for i, joint_name in enumerate(output.joint_names): - value = values[i] - priority = claim.priority - mode = output.mode + candidate = JointWinner(claim.priority, values[i], output.mode, task.name) + # First claim on this joint if joint_name not in winners: - # First claim on this joint - winners[joint_name] = (priority, value, mode, task.name) - - elif priority > winners[joint_name][0]: - # Higher priority wins - track preemption - old_task = winners[joint_name][3] - if old_task not in preemptions: - preemptions[old_task] = {} - preemptions[old_task][joint_name] = task.name - winners[joint_name] = (priority, value, mode, task.name) - - elif priority == winners[joint_name][0]: - # Same priority - if mode != winners[joint_name][2]: - # Mode conflict at same priority - log and drop - winning_task = winners[joint_name][3] - logger.warning( - f"Mode conflict on {joint_name}: {task.name} wants " - f"{mode.name}, but {winning_task} wants " - f"{winners[joint_name][2].name}. Dropping {task.name}." - ) - # Notify dropped task of preemption - if task.name not in preemptions: - preemptions[task.name] = {} - preemptions[task.name][joint_name] = winning_task - # If same mode and same priority, first wins (keep existing) - - # Convert to simplified output: joint -> (value, mode, task_name) - joint_commands = { - joint: (value, mode, task_name) - for joint, (_, value, mode, task_name) in winners.items() - } + winners[joint_name] = candidate + continue + + current = winners[joint_name] + + # Lower priority - skip + if candidate.priority < current.priority: + continue + + # Higher priority - take over + if candidate.priority > current.priority: + preemptions.setdefault(current.task_name, {})[joint_name] = task.name + winners[joint_name] = candidate + continue + + # Same priority - check for mode conflict + if candidate.mode != current.mode: + logger.warning( + f"Mode conflict on {joint_name}: {task.name} wants " + f"{candidate.mode.name}, but {current.task_name} wants " + f"{current.mode.name}. Dropping {task.name}." + ) + preemptions.setdefault(task.name, {})[joint_name] = current.task_name + # Same priority + same mode: first wins (keep current) + + # Convert to output format: joint -> (value, mode, task_name) + joint_commands = {joint: (w.value, w.mode, w.task_name) for joint, w in winners.items()} return joint_commands, preemptions diff --git a/dimos/hardware/manipulators/piper/arm.py b/dimos/hardware/manipulators/piper/arm.py deleted file mode 100644 index 0a096af640..0000000000 --- a/dimos/hardware/manipulators/piper/arm.py +++ /dev/null @@ -1,430 +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. - -"""Piper manipulator driver. - -This driver: -- Uses PiperBackend by default (injectable for testing) -- Has full control over threading and timing -- Can be customized without affecting other arms -""" - -from dataclasses import dataclass, field -import threading -import time -from typing import Any - -from dimos.core import In, Module, ModuleConfig, Out, rpc -from dimos.hardware.manipulators.piper.backend import PiperBackend -from dimos.hardware.manipulators.spec import ( - ControlMode, - DriverStatus, - JointLimits, - ManipulatorBackend, - ManipulatorInfo, - default_base_transform, -) -from dimos.msgs.geometry_msgs import Transform -from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState - -# ============================================================================ -# CONFIGURATION -# ============================================================================ - - -@dataclass -class PiperConfig(ModuleConfig): - """Piper-specific configuration. - - Type-safe config with Piper-specific fields. - """ - - can_port: str = "can0" - dof: int = 6 # Piper is always 6-DOF - control_rate: float = 100.0 # Hz - monitor_rate: float = 10.0 # Hz - has_gripper: bool = False - base_frame_id: str = "base_link" - base_transform: Transform | None = field(default_factory=default_base_transform) - - -# ============================================================================ -# DRIVER -# ============================================================================ - - -class Piper(Module[PiperConfig]): - """Piper manipulator driver. - - Features: - - Injectable backend (defaults to PiperBackend) - - 100Hz control loop for joint state - - 10Hz monitor loop for robot state - - Full RPC interface for control - - Example: - >>> arm = Piper(can_port="can0") - >>> arm.start() - >>> arm.enable_servos() - >>> arm.move_joint([0, 0, 0, 0, 0, 0]) - - Testing: - >>> from dimos.hardware.manipulators.mock import MockBackend - >>> arm = Piper(backend=MockBackend()) - >>> arm.start() # No hardware needed! - """ - - # Input topics (commands from controllers) - joint_position_command: In[JointCommand] - joint_velocity_command: In[JointCommand] - - # Output topics - joint_state: Out[JointState] - robot_state: Out[RobotState] - - # Config - config: PiperConfig - default_config = PiperConfig - - def __init__( - self, - backend: ManipulatorBackend | None = None, - *args: Any, - **kwargs: Any, - ) -> None: - super().__init__(*args, **kwargs) - - # Backend is injectable for testing (pass config to backend __init__) - self.backend: ManipulatorBackend = backend or PiperBackend( - can_port=self.config.can_port, - dof=self.config.dof, - ) - - # Threading state - self._running = False - self._control_thread: threading.Thread | None = None - self._monitor_thread: threading.Thread | None = None - - # Cached info - self._dof: int = self.config.dof - self._joint_names: list[str] = [f"joint{i + 1}" for i in range(self._dof)] - - # Auto-connect on initialization - self._auto_start() - - def _auto_start(self) -> None: - """Auto-connect to hardware on initialization.""" - if not self.backend.connect(): - print(f"WARNING: Failed to connect to Piper on {self.config.can_port}") - return - - # Update DOF from backend (in case it differs) - self._dof = self.backend.get_dof() - self._joint_names = [f"joint{i + 1}" for i in range(self._dof)] - - # Start threads - self._running = True - - self._control_thread = threading.Thread( - target=self._control_loop, - name="Piper-Control", - daemon=True, - ) - self._monitor_thread = threading.Thread( - target=self._monitor_loop, - name="Piper-Monitor", - daemon=True, - ) - - self._control_thread.start() - self._monitor_thread.start() - - print(f"Piper connected on {self.config.can_port}, DOF={self._dof}") - - def _subscribe_to_commands(self) -> None: - """Subscribe to command input topics.""" - try: - if self.joint_position_command: - self.joint_position_command.subscribe(self._on_joint_position_command) - print("Subscribed to joint_position_command") - except Exception as e: - print(f"Could not subscribe to joint_position_command: {e}") - - try: - if self.joint_velocity_command: - self.joint_velocity_command.subscribe(self._on_joint_velocity_command) - print("Subscribed to joint_velocity_command") - except Exception as e: - print(f"Could not subscribe to joint_velocity_command: {e}") - - def _on_joint_position_command(self, cmd: JointCommand) -> None: - """Handle incoming joint position command.""" - if not self._running: - return - - positions = list(cmd.positions) if cmd.positions else [] - if len(positions) != self._dof: - print(f"WARNING: Position command has {len(positions)} joints, expected {self._dof}") - return - - self.backend.write_joint_positions(positions) - - def _on_joint_velocity_command(self, cmd: JointCommand) -> None: - """Handle incoming joint velocity command.""" - if not self._running: - return - - # JointCommand uses 'positions' field for velocity commands too - velocities = list(cmd.positions) if cmd.positions else [] - if len(velocities) != self._dof: - print(f"WARNING: Velocity command has {len(velocities)} joints, expected {self._dof}") - return - - self.backend.write_joint_velocities(velocities) - - # ========================================================================= - # Lifecycle - # ========================================================================= - - @rpc - def start(self) -> DriverStatus: - """Connect to Piper and start control loops (if not already running).""" - super().start() # Important: sets up transports - - if self._running: - # Already connected, just subscribe to commands - self._subscribe_to_commands() - return DriverStatus.CONNECTED - - self._auto_start() - self._subscribe_to_commands() - return DriverStatus.CONNECTED if self._running else DriverStatus.ERROR - - @rpc - def stop(self) -> None: - """Stop Piper and disconnect.""" - self._running = False - - # Stop motion first - try: - self.backend.write_stop() - except Exception: - pass - - # Wait for threads - if self._control_thread and self._control_thread.is_alive(): - self._control_thread.join(timeout=1.0) - if self._monitor_thread and self._monitor_thread.is_alive(): - self._monitor_thread.join(timeout=1.0) - - # Disconnect - try: - self.backend.disconnect() - except Exception: - pass - - super().stop() - - # ========================================================================= - # Control Loop (100Hz) - # ========================================================================= - - def _control_loop(self) -> None: - """High-frequency loop for joint state publishing.""" - period = 1.0 / self.config.control_rate - - while self._running: - start = time.perf_counter() - - try: - self._publish_joint_state() - except Exception as e: - print(f"Piper control loop error: {e}") - - # Rate control - elapsed = time.perf_counter() - start - if elapsed < period: - time.sleep(period - elapsed) - - def _publish_joint_state(self) -> None: - """Read and publish joint state.""" - positions = self.backend.read_joint_positions() - velocities = self.backend.read_joint_velocities() - efforts = self.backend.read_joint_efforts() - - msg = JointState( - ts=time.time(), - frame_id="joint_state", - name=self._joint_names, - position=positions, - velocity=velocities, - effort=efforts, - ) - self.joint_state.publish(msg) - - # ========================================================================= - # Monitor Loop (10Hz) - # ========================================================================= - - def _monitor_loop(self) -> None: - """Low-frequency loop for robot state monitoring.""" - period = 1.0 / self.config.monitor_rate - - while self._running: - start = time.perf_counter() - - try: - self._publish_robot_state() - except Exception as e: - print(f"Piper monitor loop error: {e}") - - # Rate control - elapsed = time.perf_counter() - start - if elapsed < period: - time.sleep(period - elapsed) - - def _publish_robot_state(self) -> None: - """Read and publish robot state.""" - state = self.backend.read_state() - error_code, _ = self.backend.read_error() - - msg = RobotState( - state=state.get("state", 0), - mode=state.get("mode", 0), - error_code=error_code, - warn_code=0, - ) - self.robot_state.publish(msg) - - # ========================================================================= - # RPC Methods - Servo Control - # ========================================================================= - - @rpc - def enable_servos(self) -> bool: - """Enable motor control.""" - return self.backend.write_enable(True) - - @rpc - def disable_servos(self) -> bool: - """Disable motor control.""" - return self.backend.write_enable(False) - - @rpc - def clear_errors(self) -> bool: - """Clear error state.""" - return self.backend.write_clear_errors() - - # ========================================================================= - # RPC Methods - Mode Control - # ========================================================================= - - @rpc - def set_control_mode(self, mode: ControlMode) -> bool: - """Set control mode (position, velocity, etc).""" - return self.backend.set_control_mode(mode) - - @rpc - def get_control_mode(self) -> ControlMode: - """Get current control mode.""" - return self.backend.get_control_mode() - - # ========================================================================= - # RPC Methods - Joint Space Motion - # ========================================================================= - - @rpc - def move_joint(self, positions: list[float], velocity: float = 0.5) -> bool: - """Move to joint positions (radians).""" - return self.backend.write_joint_positions(positions, velocity) - - @rpc - def move_velocity(self, velocities: list[float]) -> bool: - """Move with joint velocities (rad/s). - - Note: Piper doesn't support native velocity control. - This will return False. - """ - return self.backend.write_joint_velocities(velocities) - - @rpc - def stop_motion(self) -> bool: - """Stop all motion.""" - return self.backend.write_stop() - - # ========================================================================= - # RPC Methods - Cartesian Motion - # ========================================================================= - - @rpc - def get_cartesian_position(self) -> dict[str, float] | None: - """Get end-effector pose (x, y, z in meters; roll, pitch, yaw in radians).""" - return self.backend.read_cartesian_position() - - @rpc - def move_cartesian( - self, - pose: dict[str, float], - velocity: float = 0.5, - ) -> bool: - """Move to cartesian pose. - - Args: - pose: Dict with x, y, z (meters), roll, pitch, yaw (radians) - velocity: Speed as fraction of max (0-1) - - Note: Piper may not support direct cartesian control. - """ - return self.backend.write_cartesian_position(pose, velocity) - - # ========================================================================= - # RPC Methods - Info - # ========================================================================= - - @rpc - def get_info(self) -> ManipulatorInfo: - """Get manipulator info.""" - return self.backend.get_info() - - @rpc - def get_limits(self) -> JointLimits: - """Get joint limits.""" - return self.backend.get_limits() - - # ========================================================================= - # RPC Methods - Optional Features (Gripper) - # ========================================================================= - - @rpc - def get_gripper_position(self) -> float | None: - """Get gripper position (meters). None if no gripper.""" - if not self.config.has_gripper: - return None - return self.backend.read_gripper_position() - - @rpc - def set_gripper_position(self, position: float) -> bool: - """Set gripper position (meters).""" - if not self.config.has_gripper: - return False - return self.backend.write_gripper_position(position) - - -# ============================================================================ -# EXPORTS -# ============================================================================ - -piper = Piper.blueprint - -__all__ = ["Piper", "PiperConfig", "piper"] diff --git a/dimos/hardware/manipulators/xarm/arm.py b/dimos/hardware/manipulators/xarm/arm.py deleted file mode 100644 index 78230270df..0000000000 --- a/dimos/hardware/manipulators/xarm/arm.py +++ /dev/null @@ -1,423 +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. - -"""XArm manipulator driver. - -This driver: -- Uses XArmBackend by default (injectable for testing) -- Has full control over threading and timing -- Can be customized without affecting other arms -""" - -from dataclasses import dataclass, field -import logging -import threading -import time -from typing import Any - -logger = logging.getLogger(__name__) - -from dimos.core import In, Module, ModuleConfig, Out, rpc -from dimos.hardware.manipulators.spec import ( - ControlMode, - JointLimits, - ManipulatorBackend, - ManipulatorInfo, - default_base_transform, -) -from dimos.hardware.manipulators.xarm.backend import XArmBackend -from dimos.msgs.geometry_msgs import Transform -from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState - -# ============================================================================ -# CONFIGURATION -# ============================================================================ - - -@dataclass -class XArmConfig(ModuleConfig): - """XArm-specific configuration. - - Type-safe config with XArm-specific fields. - """ - - ip: str = "192.168.1.185" - dof: int = 6 - control_rate: float = 100.0 # Hz - XArm can handle 100Hz - monitor_rate: float = 10.0 # Hz - has_gripper: bool = False - has_ft_sensor: bool = False - base_frame_id: str = "base_link" - base_transform: Transform | None = field(default_factory=default_base_transform) - connection_type: str = "real" # "real" or "sim" - - -# ============================================================================ -# DRIVER -# ============================================================================ - - -class XArm(Module[XArmConfig]): - """XArm manipulator driver. - - Features: - - Injectable backend (defaults to XArmBackend) - - 100Hz control loop for joint state - - 10Hz monitor loop for robot state - - Full RPC interface for control - - Example: - >>> arm = XArm(ip="192.168.1.185") - >>> arm.start() - >>> arm.enable_servos() - >>> arm.move_joint([0, 0, 0, 0, 0, 0]) - - Testing: - >>> from dimos.hardware.manipulators.mock import MockBackend - >>> arm = XArm(backend=MockBackend()) - >>> arm.start() # No hardware needed! - """ - - # Input topics (commands from controllers) - joint_position_command: In[JointCommand] - joint_velocity_command: In[JointCommand] - - # Output topics - joint_state: Out[JointState] - robot_state: Out[RobotState] - - # Config - config: XArmConfig - default_config = XArmConfig - - def __init__( - self, - backend: ManipulatorBackend | None = None, - *args: Any, - **kwargs: Any, - ) -> None: - super().__init__(*args, **kwargs) - - # Backend is injectable for testing - self.backend: ManipulatorBackend = backend or XArmBackend( - ip=self.config.ip, - dof=self.config.dof, - ) - - # Threading state - self._stop_event = threading.Event() - self._stop_event.set() # Initially stopped - self._control_thread: threading.Thread | None = None - self._monitor_thread: threading.Thread | None = None - - # Cached info - self._dof: int = self.config.dof - self._joint_names: list[str] = [f"joint{i + 1}" for i in range(self._dof)] - - def _auto_start(self) -> None: - """Auto-connect to hardware on initialization.""" - if not self.backend.connect(): - logger.warning(f"Failed to connect to XArm at {self.config.ip}") - return - - # Update DOF from backend (in case it differs) - self._dof = self.backend.get_dof() - self._joint_names = [f"joint{i + 1}" for i in range(self._dof)] - - # Start threads - self._stop_event.clear() # Clear stop event to indicate running - - self._control_thread = threading.Thread( - target=self._control_loop, - name="XArm-Control", - daemon=True, - ) - self._monitor_thread = threading.Thread( - target=self._monitor_loop, - name="XArm-Monitor", - daemon=True, - ) - - self._control_thread.start() - self._monitor_thread.start() - - print(f"XArm connected at {self.config.ip}, DOF={self._dof}") - - def _subscribe_to_commands(self) -> None: - """Subscribe to command input topics.""" - try: - if self.joint_position_command: - self.joint_position_command.subscribe(self._on_joint_position_command) - print("Subscribed to joint_position_command") - except Exception as e: - print(f"Could not subscribe to joint_position_command: {e}") - - try: - if self.joint_velocity_command: - self.joint_velocity_command.subscribe(self._on_joint_velocity_command) - print("Subscribed to joint_velocity_command") - except Exception as e: - print(f"Could not subscribe to joint_velocity_command: {e}") - - def _on_joint_position_command(self, cmd: JointCommand) -> None: - """Handle incoming joint position command.""" - if self._stop_event.is_set(): - return - - positions = list(cmd.positions) if cmd.positions else [] - if len(positions) != self._dof: - logger.warning(f"Position command has {len(positions)} joints, expected {self._dof}") - return - - self.backend.write_joint_positions(positions) - - def _on_joint_velocity_command(self, cmd: JointCommand) -> None: - """Handle incoming joint velocity command.""" - if self._stop_event.is_set(): - return - - # JointCommand uses 'positions' field for velocity commands too - velocities = list(cmd.positions) if cmd.positions else [] - if len(velocities) != self._dof: - logger.warning(f"Velocity command has {len(velocities)} joints, expected {self._dof}") - return - - self.backend.write_joint_velocities(velocities) - - # ========================================================================= - # Lifecycle - # ========================================================================= - - @rpc - def start(self) -> None: - """Connect to XArm and start control loops (if not already running).""" - if not self._stop_event.is_set(): - # Already running - return - - super().start() # Sets up transports - self._auto_start() - self._subscribe_to_commands() - - @rpc - def stop(self) -> None: - """Stop XArm and disconnect.""" - self._stop_event.set() - - # Stop motion first - try: - self.backend.write_stop() - except Exception: - pass - - # Wait for threads - if self._control_thread and self._control_thread.is_alive(): - self._control_thread.join(timeout=1.0) - if self._monitor_thread and self._monitor_thread.is_alive(): - self._monitor_thread.join(timeout=1.0) - - # Disconnect - try: - self.backend.disconnect() - except Exception: - pass - - super().stop() - - # ========================================================================= - # Control Loop (100Hz) - # ========================================================================= - - def _control_loop(self) -> None: - """High-frequency loop for joint state publishing.""" - period = 1.0 / self.config.control_rate - - while not self._stop_event.is_set(): - start = time.perf_counter() - - try: - self._publish_joint_state() - except Exception as e: - print(f"XArm control loop error: {e}") - - # Rate control - elapsed = time.perf_counter() - start - if elapsed < period: - time.sleep(period - elapsed) - - def _publish_joint_state(self) -> None: - """Read and publish joint state.""" - positions = self.backend.read_joint_positions() - velocities = self.backend.read_joint_velocities() - efforts = self.backend.read_joint_efforts() - - msg = JointState( - ts=time.time(), - frame_id="joint_state", - name=self._joint_names, - position=positions, - velocity=velocities, - effort=efforts, - ) - self.joint_state.publish(msg) - - # ========================================================================= - # Monitor Loop (10Hz) - # ========================================================================= - - def _monitor_loop(self) -> None: - """Low-frequency loop for robot state monitoring.""" - period = 1.0 / self.config.monitor_rate - - while not self._stop_event.is_set(): - start = time.perf_counter() - - try: - self._publish_robot_state() - except Exception as e: - print(f"XArm monitor loop error: {e}") - - # Rate control - elapsed = time.perf_counter() - start - if elapsed < period: - time.sleep(period - elapsed) - - def _publish_robot_state(self) -> None: - """Read and publish robot state.""" - state = self.backend.read_state() - error_code, _ = self.backend.read_error() - - msg = RobotState( - state=state.get("state", 0), - mode=state.get("mode", 0), - error_code=error_code, - warn_code=0, - ) - self.robot_state.publish(msg) - - # ========================================================================= - # RPC Methods - Servo Control - # ========================================================================= - - @rpc - def enable_servos(self) -> bool: - """Enable motor control.""" - return self.backend.write_enable(True) - - @rpc - def disable_servos(self) -> bool: - """Disable motor control.""" - return self.backend.write_enable(False) - - @rpc - def clear_errors(self) -> bool: - """Clear error state.""" - return self.backend.write_clear_errors() - - # ========================================================================= - # RPC Methods - Mode Control - # ========================================================================= - - @rpc - def set_control_mode(self, mode: ControlMode) -> bool: - """Set control mode (position, velocity, cartesian, etc).""" - return self.backend.set_control_mode(mode) - - @rpc - def get_control_mode(self) -> ControlMode: - """Get current control mode.""" - return self.backend.get_control_mode() - - # ========================================================================= - # RPC Methods - Joint Space Motion - # ========================================================================= - - @rpc - def move_joint(self, positions: list[float], velocity: float = 0.5) -> bool: - """Move to joint positions (radians).""" - return self.backend.write_joint_positions(positions, velocity) - - @rpc - def move_velocity(self, velocities: list[float]) -> bool: - """Move with joint velocities (rad/s).""" - return self.backend.write_joint_velocities(velocities) - - @rpc - def stop_motion(self) -> bool: - """Stop all motion.""" - return self.backend.write_stop() - - # ========================================================================= - # RPC Methods - Cartesian Motion - # ========================================================================= - - @rpc - def get_cartesian_position(self) -> dict[str, float] | None: - """Get end-effector pose (x, y, z in meters; roll, pitch, yaw in radians).""" - return self.backend.read_cartesian_position() - - @rpc - def move_cartesian( - self, - pose: dict[str, float], - velocity: float = 0.5, - ) -> bool: - """Move to cartesian pose. - - Args: - pose: Dict with x, y, z (meters), roll, pitch, yaw (radians) - velocity: Speed as fraction of max (0-1) - """ - return self.backend.write_cartesian_position(pose, velocity) - - # ========================================================================= - # RPC Methods - Info - # ========================================================================= - - @rpc - def get_info(self) -> ManipulatorInfo: - """Get manipulator info.""" - return self.backend.get_info() - - @rpc - def get_limits(self) -> JointLimits: - """Get joint limits.""" - return self.backend.get_limits() - - # ========================================================================= - # RPC Methods - Optional Features (Gripper) - # ========================================================================= - - @rpc - def get_gripper_position(self) -> float | None: - """Get gripper position (meters). None if no gripper.""" - if not self.config.has_gripper: - return None - return self.backend.read_gripper_position() - - @rpc - def set_gripper_position(self, position: float) -> bool: - """Set gripper position (meters).""" - if not self.config.has_gripper: - return False - return self.backend.write_gripper_position(position) - - -# ============================================================================ -# EXPORTS -# ============================================================================ - -xarm = XArm.blueprint - -__all__ = ["XArm", "XArmConfig", "xarm"] diff --git a/dimos/hardware/manipulators/xarm/backend.py b/dimos/hardware/manipulators/xarm/backend.py index 33731a6b26..a2aa9beef6 100644 --- a/dimos/hardware/manipulators/xarm/backend.py +++ b/dimos/hardware/manipulators/xarm/backend.py @@ -45,8 +45,36 @@ class XArmBackend: - Angles: XArm uses degrees, we use radians - Positions: XArm uses mm, we use meters - Velocities: XArm uses deg/s, we use rad/s + + TODO: Consider creating XArmPose/XArmVelocity types to encapsulate + unit conversions instead of helper methods. See ManipulatorPose discussion. """ + # ========================================================================= + # Unit Conversions (SI <-> XArm units) + # ========================================================================= + + @staticmethod + def _m_to_mm(m: float) -> float: + return m * 1000.0 + + @staticmethod + def _mm_to_m(mm: float) -> float: + return mm / 1000.0 + + @staticmethod + def _rad_to_deg(rad: float) -> float: + return math.degrees(rad) + + @staticmethod + def _deg_to_rad(deg: float) -> float: + return math.radians(deg) + + @staticmethod + def _velocity_to_speed_mm(velocity: float) -> float: + """Convert 0-1 velocity fraction to mm/s (max ~500 mm/s).""" + return velocity * 500 + def __init__(self, ip: str, dof: int = 6) -> None: self._ip = ip self._dof = dof @@ -289,12 +317,12 @@ def read_cartesian_position(self) -> dict[str, float] | None: _, pose = self._arm.get_position() if pose and len(pose) >= 6: return { - "x": pose[0] / 1000.0, # mm -> m - "y": pose[1] / 1000.0, - "z": pose[2] / 1000.0, - "roll": math.radians(pose[3]), - "pitch": math.radians(pose[4]), - "yaw": math.radians(pose[5]), + "x": self._mm_to_m(pose[0]), + "y": self._mm_to_m(pose[1]), + "z": self._mm_to_m(pose[2]), + "roll": self._deg_to_rad(pose[3]), + "pitch": self._deg_to_rad(pose[4]), + "yaw": self._deg_to_rad(pose[5]), } return None @@ -307,25 +335,14 @@ def write_cartesian_position( if not self._arm: return False - # Convert to XArm units - x = pose.get("x", 0) * 1000.0 # m -> mm - y = pose.get("y", 0) * 1000.0 - z = pose.get("z", 0) * 1000.0 - roll = math.degrees(pose.get("roll", 0)) - pitch = math.degrees(pose.get("pitch", 0)) - yaw = math.degrees(pose.get("yaw", 0)) - - # Speed in mm/s (max ~1000 mm/s) - speed = velocity * 500 - code: int = self._arm.set_position( - x=x, - y=y, - z=z, - roll=roll, - pitch=pitch, - yaw=yaw, - speed=speed, + x=self._m_to_mm(pose.get("x", 0)), + y=self._m_to_mm(pose.get("y", 0)), + z=self._m_to_mm(pose.get("z", 0)), + roll=self._rad_to_deg(pose.get("roll", 0)), + pitch=self._rad_to_deg(pose.get("pitch", 0)), + yaw=self._rad_to_deg(pose.get("yaw", 0)), + speed=self._velocity_to_speed_mm(velocity), wait=False, ) return code == 0 From 6143af6ada20571ba569cbea2a4dcd7b518d0468 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Mon, 12 Jan 2026 23:26:18 -0800 Subject: [PATCH 36/42] tick_loop.py: Notify preemption when lower-priority task loses to existing winner hardware_interface.py: Call set_control_mode() before mode-specific writes, Convert if/elif to match statement for control mode dispatch --- dimos/control/hardware_interface.py | 28 ++++++++++++++++++---------- dimos/control/tick_loop.py | 3 ++- 2 files changed, 20 insertions(+), 11 deletions(-) diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 7710874547..7e5f162b2a 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -114,6 +114,7 @@ def __init__( self._last_commanded: dict[str, float] = {} self._initialized = False self._warned_unknown_joints: set[str] = set() + self._current_mode: ControlMode | None = None @property def hardware_id(self) -> str: @@ -182,17 +183,24 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: # Build ordered list for backend ordered = self._build_ordered_command() + # Switch control mode if needed + if mode != self._current_mode: + if not self._backend.set_control_mode(mode): + logger.warning(f"Hardware {self._hardware_id} failed to switch to {mode.name}") + return False + self._current_mode = mode + # Send to backend - if mode == ControlMode.POSITION: - return self._backend.write_joint_positions(ordered) - elif mode == ControlMode.VELOCITY: - return self._backend.write_joint_velocities(ordered) - elif mode == ControlMode.TORQUE: - # Most backends don't support torque mode - logger.warning(f"Hardware {self._hardware_id} does not support torque mode") - return False - else: - return False + match mode: + case ControlMode.POSITION: + return self._backend.write_joint_positions(ordered) + case ControlMode.VELOCITY: + return self._backend.write_joint_velocities(ordered) + case ControlMode.TORQUE: + logger.warning(f"Hardware {self._hardware_id} does not support torque mode") + return False + case _: + return False def _initialize_last_commanded(self) -> None: """Initialize last_commanded with current hardware positions.""" diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py index b2174b9d28..03e4e0ebd0 100644 --- a/dimos/control/tick_loop.py +++ b/dimos/control/tick_loop.py @@ -282,8 +282,9 @@ def _arbitrate( current = winners[joint_name] - # Lower priority - skip + # Lower priority loses - notify preemption if candidate.priority < current.priority: + preemptions.setdefault(task.name, {})[joint_name] = current.task_name continue # Higher priority - take over From 60304d38c83bc5e74154fe323596ae7c6978756f Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 13 Jan 2026 12:17:28 -0800 Subject: [PATCH 37/42] tick_loop.py: Notify preemption when lower-priority task loses to existing winner hardware_interface.py: Call set_control_mode() before mode-specific writes, convert if/elif to match statement trajectory_task.py: Defer start time to first compute() for consistent timing orchestrator.py: Extract _setup_hardware() helper for cleaner config setup piper and xarm/backend.py: Fail fast on read_joint_positions(), map SERVO_POSITION to mode 1 hardware_interface.py: Retry initialization with proper error propagation, spec.py: Add SERVO_POSITION control mode for confusion between position planning and position servo task.py: added SERVO_POSITION to JointCommandOutput helper --- dimos/control/hardware_interface.py | 26 ++++---- dimos/control/orchestrator.py | 63 +++++++++---------- dimos/control/task.py | 2 +- dimos/control/tasks/trajectory_task.py | 20 +++--- dimos/control/test_control.py | 66 +++++++++++++------- dimos/hardware/manipulators/piper/backend.py | 33 +++++----- dimos/hardware/manipulators/spec.py | 3 +- dimos/hardware/manipulators/xarm/backend.py | 11 ++-- 8 files changed, 128 insertions(+), 96 deletions(-) diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 7e5f162b2a..7de7d32339 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -23,6 +23,7 @@ from __future__ import annotations import logging +import time from typing import TYPE_CHECKING, Protocol, runtime_checkable from dimos.hardware.manipulators.spec import ControlMode @@ -192,7 +193,7 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: # Send to backend match mode: - case ControlMode.POSITION: + case ControlMode.POSITION | ControlMode.SERVO_POSITION: return self._backend.write_joint_positions(ordered) case ControlMode.VELOCITY: return self._backend.write_joint_velocities(ordered) @@ -204,16 +205,19 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: def _initialize_last_commanded(self) -> None: """Initialize last_commanded with current hardware positions.""" - try: - current = self._backend.read_joint_positions() - for i, name in enumerate(self._joint_names): - self._last_commanded[name] = current[i] - self._initialized = True - except Exception: - # If read fails, initialize to zeros - for name in self._joint_names: - self._last_commanded[name] = 0.0 - self._initialized = True + for _ in range(10): + try: + current = self._backend.read_joint_positions() + for i, name in enumerate(self._joint_names): + self._last_commanded[name] = current[i] + self._initialized = True + return + except Exception: + time.sleep(0.01) + + raise RuntimeError( + f"Hardware {self._hardware_id} failed to read initial positions after retries" + ) def _build_ordered_command(self) -> list[float]: """Build ordered command list from last_commanded dict.""" diff --git a/dimos/control/orchestrator.py b/dimos/control/orchestrator.py index 1fbbd6a564..885b4df348 100644 --- a/dimos/control/orchestrator.py +++ b/dimos/control/orchestrator.py @@ -184,35 +184,12 @@ def _setup_from_config(self) -> None: try: for hw_cfg in self.config.hardware: - backend = self._create_backend_from_config(hw_cfg) - try: - if not backend.connect(): - raise RuntimeError(f"Failed to connect to {hw_cfg.type} backend") - - if hw_cfg.auto_enable and hasattr(backend, "write_enable"): - backend.write_enable(True) - - self.add_hardware( - hw_cfg.id, - backend, - joint_prefix=hw_cfg.joint_prefix or hw_cfg.id, - ) - hardware_added.append(hw_cfg.id) - except Exception as e: - logger.error(f"Failed to setup hardware {hw_cfg.id}: {e}") - try: - backend.disconnect() - except Exception: - pass # Best effort cleanup - raise + self._setup_hardware(hw_cfg) + hardware_added.append(hw_cfg.id) for task_cfg in self.config.tasks: - try: - task = self._create_task_from_config(task_cfg) - self.add_task(task) - except Exception as e: - logger.error(f"Failed to setup task {task_cfg.name}: {e}") - raise + task = self._create_task_from_config(task_cfg) + self.add_task(task) except Exception: # Rollback: clean up all successfully added hardware @@ -220,7 +197,26 @@ def _setup_from_config(self) -> None: try: self.remove_hardware(hw_id) except Exception: - pass # Best effort cleanup + pass + raise + + def _setup_hardware(self, hw_cfg: HardwareConfig) -> None: + """Connect and add a single hardware backend.""" + backend = self._create_backend_from_config(hw_cfg) + + if not backend.connect(): + raise RuntimeError(f"Failed to connect to {hw_cfg.type} backend") + + try: + if hw_cfg.auto_enable and hasattr(backend, "write_enable"): + backend.write_enable(True) + self.add_hardware( + hw_cfg.id, + backend, + joint_prefix=hw_cfg.joint_prefix or hw_cfg.id, + ) + except Exception: + backend.disconnect() raise def _create_backend_from_config(self, cfg: HardwareConfig) -> ManipulatorBackend: @@ -293,7 +289,11 @@ def add_hardware( @rpc def remove_hardware(self, hardware_id: str) -> bool: - """Remove a hardware interface.""" + """Remove a hardware interface. + + Note: For safety, call this only when no tasks are actively using this + hardware. Consider stopping the orchestrator before removing hardware. + """ with self._hardware_lock: if hardware_id not in self._hardware: return False @@ -404,12 +404,11 @@ def execute_trajectory(self, task_name: str, trajectory: JointTrajectory) -> boo logger.warning(f"Task {task_name} doesn't support execute()") return False - t_now: float = time.perf_counter() logger.info( f"Executing trajectory on {task_name}: " f"{len(trajectory.points)} points, duration={trajectory.duration:.3f}s" ) - return task.execute(trajectory, t_now) # type: ignore[attr-defined,no-any-return] + return task.execute(trajectory) # type: ignore[attr-defined,no-any-return] @rpc def get_trajectory_status(self, task_name: str) -> dict[str, Any]: @@ -426,7 +425,7 @@ def get_trajectory_status(self, task_name: str) -> dict[str, Any]: result["state"] = state.name if isinstance(state, TrajectoryState) else str(state) if hasattr(task, "get_progress"): - t_now: float = time.perf_counter() + t_now = time.perf_counter() result["progress"] = task.get_progress(t_now) # type: ignore[attr-defined] return result diff --git a/dimos/control/task.py b/dimos/control/task.py index 6f5e153520..49589188d9 100644 --- a/dimos/control/task.py +++ b/dimos/control/task.py @@ -153,7 +153,7 @@ def __post_init__(self) -> None: def get_values(self) -> list[float] | None: """Get the active values based on mode.""" match self.mode: - case ControlMode.POSITION: + case ControlMode.POSITION | ControlMode.SERVO_POSITION: return self.positions case ControlMode.VELOCITY: return self.velocities diff --git a/dimos/control/tasks/trajectory_task.py b/dimos/control/tasks/trajectory_task.py index 639264adb2..30faacdad5 100644 --- a/dimos/control/tasks/trajectory_task.py +++ b/dimos/control/tasks/trajectory_task.py @@ -95,7 +95,8 @@ def __init__(self, name: str, config: JointTrajectoryTaskConfig) -> None: # State machine self._state = TrajectoryState.IDLE self._trajectory: JointTrajectory | None = None - self._start_time: float = 0.0 # In orchestrator's t_now space + self._start_time: float = 0.0 + self._pending_start: bool = False # Defer start time to first compute() logger.info(f"JointTrajectoryTask {name} initialized for joints: {config.joint_names}") @@ -109,7 +110,7 @@ def claim(self) -> ResourceClaim: return ResourceClaim( joints=self._joint_names, priority=self._config.priority, - mode=ControlMode.POSITION, + mode=ControlMode.SERVO_POSITION, ) def is_active(self) -> bool: @@ -130,7 +131,11 @@ def compute(self, state: OrchestratorState) -> JointCommandOutput | None: if self._trajectory is None: return None - # Use orchestrator's time, NOT time.time() + # Set start time on first compute() for consistent timing + if self._pending_start: + self._start_time = state.t_now + self._pending_start = False + t_elapsed = state.t_now - self._start_time # Check completion - clamp to final position to ensure we reach goal @@ -142,7 +147,7 @@ def compute(self, state: OrchestratorState) -> JointCommandOutput | None: return JointCommandOutput( joint_names=self._joint_names_list, positions=list(q_ref), - mode=ControlMode.POSITION, + mode=ControlMode.SERVO_POSITION, ) # Sample trajectory @@ -151,7 +156,7 @@ def compute(self, state: OrchestratorState) -> JointCommandOutput | None: return JointCommandOutput( joint_names=self._joint_names_list, positions=list(q_ref), - mode=ControlMode.POSITION, + mode=ControlMode.SERVO_POSITION, ) def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: @@ -170,12 +175,11 @@ def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: # Task-specific methods # ========================================================================= - def execute(self, trajectory: JointTrajectory, t_now: float) -> bool: + def execute(self, trajectory: JointTrajectory) -> bool: """Start executing a trajectory. Args: trajectory: Trajectory to execute - t_now: Current orchestrator time (from state.t_now) Returns: True if accepted, False if invalid or in FAULT state @@ -197,7 +201,7 @@ def execute(self, trajectory: JointTrajectory, t_now: float) -> bool: logger.info(f"Preempting active trajectory on {self._name}") self._trajectory = trajectory - self._start_time = t_now # Use orchestrator's time + self._pending_start = True # Start time set on first compute() self._state = TrajectoryState.EXECUTING logger.info( diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index 8924b35e43..e35a9853d5 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -54,6 +54,7 @@ def mock_backend(): backend.read_joint_efforts.return_value = [0.0] * 6 backend.write_joint_positions.return_value = True backend.write_joint_velocities.return_value = True + backend.set_control_mode.return_value = True return backend @@ -222,8 +223,8 @@ def test_claim(self, trajectory_task): assert "arm_joint3" in claim.joints def test_execute_trajectory(self, trajectory_task, simple_trajectory): - t_now = time.perf_counter() - result = trajectory_task.execute(simple_trajectory, t_now) + time.perf_counter() + result = trajectory_task.execute(simple_trajectory) assert result is True assert trajectory_task.is_active() assert trajectory_task.get_state() == TrajectoryState.EXECUTING @@ -231,28 +232,46 @@ def test_execute_trajectory(self, trajectory_task, simple_trajectory): def test_compute_during_trajectory( self, trajectory_task, simple_trajectory, orchestrator_state ): - t_now = time.perf_counter() - trajectory_task.execute(simple_trajectory, t_now) + t_start = time.perf_counter() + trajectory_task.execute(simple_trajectory) + # First compute sets start time (deferred start) + state0 = OrchestratorState( + joints=orchestrator_state.joints, + t_now=t_start, + dt=0.01, + ) + trajectory_task.compute(state0) + + # Compute at 0.5s into trajectory state = OrchestratorState( joints=orchestrator_state.joints, - t_now=t_now + 0.5, + t_now=t_start + 0.5, dt=0.01, ) output = trajectory_task.compute(state) assert output is not None - assert output.mode == ControlMode.POSITION + assert output.mode == ControlMode.SERVO_POSITION assert len(output.positions) == 3 assert 0.4 < output.positions[0] < 0.6 def test_trajectory_completes(self, trajectory_task, simple_trajectory, orchestrator_state): - t_now = time.perf_counter() - trajectory_task.execute(simple_trajectory, t_now) + t_start = time.perf_counter() + trajectory_task.execute(simple_trajectory) + # First compute sets start time (deferred start) + state0 = OrchestratorState( + joints=orchestrator_state.joints, + t_now=t_start, + dt=0.01, + ) + trajectory_task.compute(state0) + + # Compute past trajectory duration state = OrchestratorState( joints=orchestrator_state.joints, - t_now=t_now + 1.5, + t_now=t_start + 1.5, dt=0.01, ) output = trajectory_task.compute(state) @@ -264,8 +283,7 @@ def test_trajectory_completes(self, trajectory_task, simple_trajectory, orchestr assert trajectory_task.get_state() == TrajectoryState.COMPLETED def test_cancel_trajectory(self, trajectory_task, simple_trajectory): - t_now = time.perf_counter() - trajectory_task.execute(simple_trajectory, t_now) + trajectory_task.execute(simple_trajectory) assert trajectory_task.is_active() trajectory_task.cancel() @@ -273,20 +291,27 @@ def test_cancel_trajectory(self, trajectory_task, simple_trajectory): assert trajectory_task.get_state() == TrajectoryState.ABORTED def test_preemption(self, trajectory_task, simple_trajectory): - t_now = time.perf_counter() - trajectory_task.execute(simple_trajectory, t_now) + trajectory_task.execute(simple_trajectory) trajectory_task.on_preempted("safety_task", frozenset({"arm_joint1"})) assert trajectory_task.get_state() == TrajectoryState.ABORTED assert not trajectory_task.is_active() - def test_progress(self, trajectory_task, simple_trajectory): - t_now = time.perf_counter() - trajectory_task.execute(simple_trajectory, t_now) + def test_progress(self, trajectory_task, simple_trajectory, orchestrator_state): + t_start = time.perf_counter() + trajectory_task.execute(simple_trajectory) + + # First compute sets start time (deferred start) + state0 = OrchestratorState( + joints=orchestrator_state.joints, + t_now=t_start, + dt=0.01, + ) + trajectory_task.compute(state0) - assert trajectory_task.get_progress(t_now) == pytest.approx(0.0, abs=0.01) - assert trajectory_task.get_progress(t_now + 0.5) == pytest.approx(0.5, abs=0.01) - assert trajectory_task.get_progress(t_now + 1.0) == pytest.approx(1.0, abs=0.01) + assert trajectory_task.get_progress(t_start) == pytest.approx(0.0, abs=0.01) + assert trajectory_task.get_progress(t_start + 0.5) == pytest.approx(0.5, abs=0.01) + assert trajectory_task.get_progress(t_start + 1.0) == pytest.approx(1.0, abs=0.01) # ============================================================================= @@ -508,8 +533,7 @@ def test_full_trajectory_execution(self, mock_backend): ) tick_loop.start() - t_now = time.perf_counter() - traj_task.execute(trajectory, t_now) + traj_task.execute(trajectory) time.sleep(0.6) tick_loop.stop() diff --git a/dimos/hardware/manipulators/piper/backend.py b/dimos/hardware/manipulators/piper/backend.py index fe2125ae7f..3b3c0671e5 100644 --- a/dimos/hardware/manipulators/piper/backend.py +++ b/dimos/hardware/manipulators/piper/backend.py @@ -165,6 +165,7 @@ def set_control_mode(self, mode: ControlMode) -> bool: return False # Piper move modes: 0x01=position, 0x02=velocity + # SERVO_POSITION uses position mode for high-freq streaming move_mode = 0x01 # Default position mode if mode == ControlMode.VELOCITY: move_mode = 0x02 @@ -192,23 +193,21 @@ def get_control_mode(self) -> ControlMode: def read_joint_positions(self) -> list[float]: """Read joint positions (Piper units -> radians).""" if not self._sdk: - return [0.0] * self._dof - - try: - joint_msgs = self._sdk.GetArmJointMsgs() - if joint_msgs and joint_msgs.joint_state: - js = joint_msgs.joint_state - return [ - js.joint_1 * PIPER_TO_RAD, - js.joint_2 * PIPER_TO_RAD, - js.joint_3 * PIPER_TO_RAD, - js.joint_4 * PIPER_TO_RAD, - js.joint_5 * PIPER_TO_RAD, - js.joint_6 * PIPER_TO_RAD, - ] - except Exception: - pass - return [0.0] * self._dof + raise RuntimeError("Not connected") + + joint_msgs = self._sdk.GetArmJointMsgs() + if not joint_msgs or not joint_msgs.joint_state: + raise RuntimeError("Failed to read joint positions") + + js = joint_msgs.joint_state + return [ + js.joint_1 * PIPER_TO_RAD, + js.joint_2 * PIPER_TO_RAD, + js.joint_3 * PIPER_TO_RAD, + js.joint_4 * PIPER_TO_RAD, + js.joint_5 * PIPER_TO_RAD, + js.joint_6 * PIPER_TO_RAD, + ] def read_joint_velocities(self) -> list[float]: """Read joint velocities. diff --git a/dimos/hardware/manipulators/spec.py b/dimos/hardware/manipulators/spec.py index bef1e33b26..d6f951456e 100644 --- a/dimos/hardware/manipulators/spec.py +++ b/dimos/hardware/manipulators/spec.py @@ -46,7 +46,8 @@ class DriverStatus(Enum): class ControlMode(Enum): """Control modes for manipulator.""" - POSITION = "position" + POSITION = "position" # Planned position control (slower, smoother) + SERVO_POSITION = "servo_position" # High-freq joint position streaming (100Hz+) VELOCITY = "velocity" TORQUE = "torque" CARTESIAN = "cartesian" diff --git a/dimos/hardware/manipulators/xarm/backend.py b/dimos/hardware/manipulators/xarm/backend.py index a2aa9beef6..2122f64408 100644 --- a/dimos/hardware/manipulators/xarm/backend.py +++ b/dimos/hardware/manipulators/xarm/backend.py @@ -98,7 +98,7 @@ def connect(self) -> bool: # Initialize to servo mode for high-frequency control self._arm.set_mode(_XARM_MODE_SERVO_CARTESIAN) # Mode 1 = servo mode self._arm.set_state(0) - self._control_mode = ControlMode.POSITION + self._control_mode = ControlMode.SERVO_POSITION return True except Exception as e: @@ -156,6 +156,7 @@ def set_control_mode(self, mode: ControlMode) -> bool: mode_map = { ControlMode.POSITION: _XARM_MODE_POSITION, + ControlMode.SERVO_POSITION: _XARM_MODE_SERVO_CARTESIAN, # Mode 1 for high-freq ControlMode.VELOCITY: _XARM_MODE_JOINT_VELOCITY, ControlMode.TORQUE: _XARM_MODE_JOINT_TORQUE, ControlMode.CARTESIAN: _XARM_MODE_SERVO_CARTESIAN, @@ -184,12 +185,12 @@ def get_control_mode(self) -> ControlMode: def read_joint_positions(self) -> list[float]: """Read joint positions (degrees -> radians).""" if not self._arm: - return [0.0] * self._dof + raise RuntimeError("Not connected") _, angles = self._arm.get_servo_angle() - if angles: - return [math.radians(a) for a in angles[: self._dof]] - return [0.0] * self._dof + if not angles: + raise RuntimeError("Failed to read joint positions") + return [math.radians(a) for a in angles[: self._dof]] def read_joint_velocities(self) -> list[float]: """Read joint velocities. From 33e18a6ae38528f174c4124eac76779dcad0cacd Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 13 Jan 2026 12:44:38 -0800 Subject: [PATCH 38/42] cleaned up legacy blueprints for manipulator drivers --- dimos/hardware/manipulators/piper/__init__.py | 20 +- .../hardware/manipulators/piper/blueprints.py | 141 -------- dimos/hardware/manipulators/xarm/__init__.py | 38 +- .../hardware/manipulators/xarm/blueprints.py | 327 ------------------ .../example_cartesian_control.py | 194 ----------- .../example_trajectory_control.py | 189 ---------- dimos/robot/all_blueprints.py | 16 - 7 files changed, 12 insertions(+), 913 deletions(-) delete mode 100644 dimos/hardware/manipulators/piper/blueprints.py delete mode 100644 dimos/hardware/manipulators/xarm/blueprints.py delete mode 100644 dimos/manipulation/control/servo_control/example_cartesian_control.py delete mode 100644 dimos/manipulation/control/trajectory_controller/example_trajectory_control.py diff --git a/dimos/hardware/manipulators/piper/__init__.py b/dimos/hardware/manipulators/piper/__init__.py index 3a3c1bbe11..16c6e451cd 100644 --- a/dimos/hardware/manipulators/piper/__init__.py +++ b/dimos/hardware/manipulators/piper/__init__.py @@ -12,23 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Piper manipulator driver. +"""Piper manipulator hardware backend. Usage: - >>> from dimos.hardware.manipulators.piper import Piper - >>> arm = Piper(can_port="can0") - >>> arm.start() - >>> arm.enable_servos() - >>> arm.move_joint([0, 0, 0, 0, 0, 0]) - -Testing: - >>> from dimos.hardware.manipulators.mock import MockBackend - >>> from dimos.hardware.manipulators.piper import Piper - >>> arm = Piper(backend=MockBackend()) - >>> arm.start() # No hardware needed! + >>> from dimos.hardware.manipulators.piper import PiperBackend + >>> backend = PiperBackend(can_port="can0") + >>> backend.connect() + >>> positions = backend.read_joint_positions() """ -from dimos.hardware.manipulators.piper.arm import Piper, PiperConfig, piper from dimos.hardware.manipulators.piper.backend import PiperBackend -__all__ = ["Piper", "PiperBackend", "PiperConfig", "piper"] +__all__ = ["PiperBackend"] diff --git a/dimos/hardware/manipulators/piper/blueprints.py b/dimos/hardware/manipulators/piper/blueprints.py deleted file mode 100644 index c3858ee0eb..0000000000 --- a/dimos/hardware/manipulators/piper/blueprints.py +++ /dev/null @@ -1,141 +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. - -"""Blueprints for Piper manipulator using B-lite architecture. - -Usage: - # Run via CLI: - dimos run piper-servo # Driver only - dimos run piper-trajectory # Driver + Joint trajectory controller - - # Or programmatically: - from dimos.hardware.manipulators.piper.blueprints import piper_trajectory - coordinator = piper_trajectory.build() - coordinator.loop() -""" - -from dimos.core.blueprints import autoconnect -from dimos.core.transport import LCMTransport -from dimos.hardware.manipulators.piper.arm import piper as piper_blueprint -from dimos.manipulation.control import joint_trajectory_controller -from dimos.msgs.sensor_msgs import ( - JointCommand, - JointState, - RobotState, -) -from dimos.msgs.trajectory_msgs import JointTrajectory - -# ============================================================================= -# Piper Servo Control Blueprint -# ============================================================================= -# Piper driver in servo mode - publishes joint states, accepts joint commands. -# ============================================================================= - -piper_servo = piper_blueprint( - can_port="can0", - dof=6, - control_rate=100.0, - monitor_rate=10.0, -).transports( - { - # Joint state feedback (position, velocity, effort) - ("joint_state", JointState): LCMTransport("/piper/joint_states", JointState), - # Robot state feedback (mode, state, errors) - ("robot_state", RobotState): LCMTransport("/piper/robot_state", RobotState), - } -) - -# ============================================================================= -# Piper Servo with Gripper Blueprint -# ============================================================================= - -piper_servo_gripper = piper_blueprint( - can_port="can0", - dof=6, - control_rate=100.0, - monitor_rate=10.0, - has_gripper=True, -).transports( - { - ("joint_state", JointState): LCMTransport("/piper/joint_states", JointState), - ("robot_state", RobotState): LCMTransport("/piper/robot_state", RobotState), - } -) - -# ============================================================================= -# Piper Trajectory Control Blueprint (Driver + Trajectory Controller) -# ============================================================================= -# Combines Piper driver with JointTrajectoryController for trajectory execution. -# The controller receives JointTrajectory messages and executes them. -# ============================================================================= - -piper_trajectory = autoconnect( - piper_blueprint( - can_port="can0", - dof=6, - control_rate=100.0, # Higher rate for smoother trajectory execution - monitor_rate=10.0, - ), - joint_trajectory_controller( - control_frequency=100.0, - ), -).transports( - { - # Shared topics between driver and controller - ("joint_state", JointState): LCMTransport("/piper/joint_states", JointState), - ("robot_state", RobotState): LCMTransport("/piper/robot_state", RobotState), - ("joint_position_command", JointCommand): LCMTransport( - "/piper/joint_position_command", JointCommand - ), - # Trajectory input topic - ("trajectory", JointTrajectory): LCMTransport("/trajectory", JointTrajectory), - } -) - -# ============================================================================= -# Piper Dual Arm Blueprint (for dual-arm robots) -# ============================================================================= - -piper_left = piper_blueprint( - can_port="can0", - dof=6, - control_rate=100.0, - monitor_rate=10.0, -).transports( - { - ("joint_state", JointState): LCMTransport("/piper/left/joint_states", JointState), - ("robot_state", RobotState): LCMTransport("/piper/left/robot_state", RobotState), - } -) - -piper_right = piper_blueprint( - can_port="can1", - dof=6, - control_rate=100.0, - monitor_rate=10.0, -).transports( - { - ("joint_state", JointState): LCMTransport("/piper/right/joint_states", JointState), - ("robot_state", RobotState): LCMTransport("/piper/right/robot_state", RobotState), - } -) - - -__all__ = [ - "piper_left", - "piper_right", - "piper_servo", - "piper_servo_gripper", - "piper_trajectory", -] diff --git a/dimos/hardware/manipulators/xarm/__init__.py b/dimos/hardware/manipulators/xarm/__init__.py index f4a74d4283..343ebc4e0e 100644 --- a/dimos/hardware/manipulators/xarm/__init__.py +++ b/dimos/hardware/manipulators/xarm/__init__.py @@ -12,41 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""XArm manipulator driver. +"""XArm manipulator hardware backend. Usage: - >>> from dimos.hardware.manipulators.xarm import XArm - >>> arm = XArm(ip="192.168.1.185", dof=6) - >>> arm.start() - >>> arm.enable_servos() - >>> arm.move_joint([0, 0, 0, 0, 0, 0]) + >>> from dimos.hardware.manipulators.xarm import XArmBackend + >>> backend = XArmBackend(ip="192.168.1.185", dof=6) + >>> backend.connect() + >>> positions = backend.read_joint_positions() """ -from dimos.hardware.manipulators.xarm.arm import XArm, XArmConfig, xarm from dimos.hardware.manipulators.xarm.backend import XArmBackend -# Backwards compatibility alias -XArmDriver = XArm -from dimos.hardware.manipulators.xarm.blueprints import ( - xarm5_servo, - xarm7_servo, - xarm7_trajectory, - xarm_cartesian, - xarm_servo, - xarm_trajectory, -) - -__all__ = [ - "XArm", - "XArmBackend", - "XArmConfig", - "XArmDriver", # Backwards compatibility alias - "xarm", - "xarm5_servo", - "xarm7_servo", - "xarm7_trajectory", - "xarm_cartesian", - # Blueprints - "xarm_servo", - "xarm_trajectory", -] +__all__ = ["XArmBackend"] diff --git a/dimos/hardware/manipulators/xarm/blueprints.py b/dimos/hardware/manipulators/xarm/blueprints.py deleted file mode 100644 index e1f4d8473a..0000000000 --- a/dimos/hardware/manipulators/xarm/blueprints.py +++ /dev/null @@ -1,327 +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. - -"""Blueprints for XArm manipulator using B-lite architecture. - -Usage: - # Run via CLI: - dimos run xarm-servo # Driver only - dimos run xarm-trajectory # Driver + Joint trajectory controller - - # Or programmatically: - from dimos.hardware.manipulators.xarm.blueprints import xarm_trajectory - coordinator = xarm_trajectory.build() - coordinator.loop() -""" - -from dimos.core.blueprints import autoconnect -from dimos.core.transport import LCMTransport -from dimos.hardware.manipulators.xarm.arm import XArm, xarm as xarm_blueprint -from dimos.manipulation.control import ( - JointTrajectoryController, - cartesian_motion_controller, - joint_trajectory_controller, -) -from dimos.msgs.geometry_msgs import PoseStamped -from dimos.msgs.sensor_msgs import ( - JointCommand, - JointState, - RobotState, -) -from dimos.msgs.trajectory_msgs import JointTrajectory - -# ============================================================================= -# Dual-Arm Subclasses (workaround for module deduplication) -# ============================================================================= -# To run multiple arms in the same blueprint, we need distinct class names. -# This gives each arm its own RPC namespace: -# - XArmLeft → /rpc/XArmLeft/start/req -# - XArmRight → /rpc/XArmRight/start/req -# ============================================================================= - - -class XArmLeft(XArm): - """Left arm instance for dual-arm setups (XArm7).""" - - pass - - -class XArmRight(XArm): - """Right arm instance for dual-arm setups (XArm6).""" - - pass - - -class JointTrajectoryControllerLeft(JointTrajectoryController): - """Left arm trajectory controller.""" - - pass - - -class JointTrajectoryControllerRight(JointTrajectoryController): - """Right arm trajectory controller.""" - - pass - - -# ============================================================================= -# XArm6 Servo Control Blueprint -# ============================================================================= -# XArm driver in servo mode - publishes joint states, accepts joint commands. -# ============================================================================= - -xarm_servo = xarm_blueprint( - ip="192.168.1.185", - dof=6, - control_rate=100.0, - monitor_rate=10.0, -).transports( - { - # Joint state feedback (position, velocity, effort) - ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), - # Robot state feedback (mode, state, errors) - ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), - } -) - -# ============================================================================= -# XArm5 Servo Control Blueprint -# ============================================================================= - -xarm5_servo = xarm_blueprint( - ip="192.168.1.185", - dof=5, - control_rate=100.0, - monitor_rate=10.0, -).transports( - { - ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), - ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), - } -) - -# ============================================================================= -# XArm7 Servo Control Blueprint -# ============================================================================= - -xarm7_servo = xarm_blueprint( - ip="192.168.1.185", - dof=7, - control_rate=100.0, - monitor_rate=10.0, -).transports( - { - ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), - ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), - } -) - -# ============================================================================= -# XArm Trajectory Control Blueprint (Driver + Trajectory Controller) -# ============================================================================= -# Combines XArm driver with JointTrajectoryController for trajectory execution. -# The controller receives JointTrajectory messages and executes them. -# ============================================================================= - -xarm_trajectory = autoconnect( - xarm_blueprint( - ip="192.168.2.235", - dof=7, - control_rate=100.0, # Higher rate for smoother trajectory execution - monitor_rate=10.0, - ), - joint_trajectory_controller( - control_frequency=100.0, - ), -).transports( - { - # Shared topics between driver and controller - ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), - ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), - ("joint_position_command", JointCommand): LCMTransport( - "/xarm/joint_position_command", JointCommand - ), - # Trajectory input topic - ("trajectory", JointTrajectory): LCMTransport("/trajectory", JointTrajectory), - } -) - -# ============================================================================= -# XArm7 Trajectory Control Blueprint -# ============================================================================= - -xarm7_trajectory = autoconnect( - xarm_blueprint( - ip="192.168.1.185", - dof=7, - control_rate=100.0, - monitor_rate=10.0, - ), - joint_trajectory_controller( - control_frequency=100.0, - ), -).transports( - { - ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), - ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), - ("joint_position_command", JointCommand): LCMTransport( - "/xarm/joint_position_command", JointCommand - ), - ("trajectory", JointTrajectory): LCMTransport("/trajectory", JointTrajectory), - } -) - -# ============================================================================= -# XArm Cartesian Control Blueprint (Driver + Cartesian Controller) -# ============================================================================= -# Combines XArm driver with CartesianMotionController for Cartesian space control. -# The controller receives target_pose and converts to joint commands via IK. -# ============================================================================= - -xarm_cartesian = autoconnect( - xarm_blueprint( - ip="192.168.1.185", - dof=6, - control_rate=100.0, - monitor_rate=10.0, - ), - cartesian_motion_controller( - control_frequency=20.0, - position_kp=5.0, - position_ki=0.0, - position_kd=0.1, - max_linear_velocity=0.2, - max_angular_velocity=1.0, - ), -).transports( - { - # Shared topics between driver and controller - ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState), - ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState), - ("joint_position_command", JointCommand): LCMTransport( - "/xarm/joint_position_command", JointCommand - ), - # Controller-specific topics - ("target_pose", PoseStamped): LCMTransport("/target_pose", PoseStamped), - ("current_pose", PoseStamped): LCMTransport("/xarm/current_pose", PoseStamped), - } -) - - -# ============================================================================= -# Dual-Arm Blueprint (XArm7 Left + XArm6 Right) -# ============================================================================= -# Uses subclasses for separate RPC namespaces + remappings for separate topics. -# Each arm has its own topics for joint states and commands. -# ============================================================================= - -xarm_dual = autoconnect( - XArmLeft.blueprint( - ip="192.168.2.235", # XArm7 - Left arm - dof=7, - control_rate=100.0, - monitor_rate=10.0, - ), - XArmRight.blueprint( - ip="192.168.1.210", # XArm6 - Right arm - dof=6, - control_rate=100.0, - monitor_rate=10.0, - ), -).transports( - { - # Left arm (XArm7) topics - use remapped names - ("left_joint_state", JointState): LCMTransport("/xarm/left/joint_states", JointState), - ("left_robot_state", RobotState): LCMTransport("/xarm/left/robot_state", RobotState), - ("left_joint_position_command", JointCommand): LCMTransport( - "/xarm/left/joint_position_command", JointCommand - ), - # Right arm (XArm6) topics - use remapped names - ("right_joint_state", JointState): LCMTransport("/xarm/right/joint_states", JointState), - ("right_robot_state", RobotState): LCMTransport("/xarm/right/robot_state", RobotState), - ("right_joint_position_command", JointCommand): LCMTransport( - "/xarm/right/joint_position_command", JointCommand - ), - } -) - - -# ============================================================================= -# Dual-Arm Trajectory Blueprint (XArm7 Left + XArm6 Right + Controllers) -# ============================================================================= -# Full dual-arm setup with trajectory controllers for each arm. -# Each arm has independent trajectory execution. -# ============================================================================= - -xarm_dual_trajectory = autoconnect( - # Left arm (XArm7) - XArmLeft.blueprint( - ip="192.168.2.235", - dof=7, - control_rate=100.0, - monitor_rate=10.0, - ), - # Right arm (XArm6) - XArmRight.blueprint( - ip="192.168.1.210", - dof=6, - control_rate=100.0, - monitor_rate=10.0, - ), - # Left trajectory controller - JointTrajectoryControllerLeft.blueprint( - control_frequency=100.0, - ), - # Right trajectory controller - JointTrajectoryControllerRight.blueprint( - control_frequency=100.0, - ), -).transports( - { - # Left arm topics - ("left_joint_state", JointState): LCMTransport("/xarm/left/joint_states", JointState), - ("left_robot_state", RobotState): LCMTransport("/xarm/left/robot_state", RobotState), - ("left_joint_position_command", JointCommand): LCMTransport( - "/xarm/left/joint_position_command", JointCommand - ), - ("left_trajectory", JointTrajectory): LCMTransport( - "/xarm/left/trajectory", JointTrajectory - ), - # Right arm topics - ("right_joint_state", JointState): LCMTransport("/xarm/right/joint_states", JointState), - ("right_robot_state", RobotState): LCMTransport("/xarm/right/robot_state", RobotState), - ("right_joint_position_command", JointCommand): LCMTransport( - "/xarm/right/joint_position_command", JointCommand - ), - ("right_trajectory", JointTrajectory): LCMTransport( - "/xarm/right/trajectory", JointTrajectory - ), - } -) - - -__all__ = [ - "JointTrajectoryControllerLeft", - "JointTrajectoryControllerRight", - "XArmLeft", - "XArmRight", - "xarm5_servo", - "xarm7_servo", - "xarm7_trajectory", - "xarm_cartesian", - "xarm_dual", - "xarm_dual_trajectory", - "xarm_servo", - "xarm_trajectory", -] diff --git a/dimos/manipulation/control/servo_control/example_cartesian_control.py b/dimos/manipulation/control/servo_control/example_cartesian_control.py deleted file mode 100644 index eeff04e424..0000000000 --- a/dimos/manipulation/control/servo_control/example_cartesian_control.py +++ /dev/null @@ -1,194 +0,0 @@ -#!/usr/bin/env python3 -# 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. - -""" -Example: Topic-Based Cartesian Motion Control with xArm - -Demonstrates topic-based Cartesian space motion control. The controller -subscribes to /target_pose and automatically moves to received targets. - -This example shows: -1. Deploy xArm driver with LCM transports -2. Deploy CartesianMotionController with LCM transports -3. Configure controller to subscribe to /target_pose topic -4. Keep system running to process incoming targets - -Use target_setter.py to publish target poses to /target_pose topic. - -Pattern matches: interactive_control.py + sample_trajectory_generator.py -""" - -import signal -import time - -from dimos import core -from dimos.hardware.manipulators.xarm import XArmDriver -from dimos.manipulation.control import CartesianMotionController -from dimos.msgs.geometry_msgs import PoseStamped -from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState - -# Global flag for graceful shutdown -shutdown_requested = False - - -def signal_handler(sig, frame): # type: ignore[no-untyped-def] - """Handle Ctrl+C for graceful shutdown.""" - global shutdown_requested - print("\n\nShutdown requested...") - shutdown_requested = True - - -def main(): # type: ignore[no-untyped-def] - """ - Deploy and run topic-based Cartesian motion control system. - - The system subscribes to /target_pose and automatically moves - the robot to received target poses. - """ - - # Register signal handler for graceful shutdown - signal.signal(signal.SIGINT, signal_handler) - signal.signal(signal.SIGTERM, signal_handler) - - # ========================================================================= - # Step 1: Start dimos cluster - # ========================================================================= - print("=" * 80) - print("Topic-Based Cartesian Motion Control") - print("=" * 80) - print("\nStarting dimos cluster...") - dimos = core.start(1) # Start with 1 worker - - try: - # ========================================================================= - # Step 2: Deploy xArm driver - # ========================================================================= - print("\nDeploying xArm driver...") - arm_driver = dimos.deploy( # type: ignore[attr-defined] - XArmDriver, - ip_address="192.168.1.210", - xarm_type="xarm6", - report_type="dev", - enable_on_start=True, - ) - - # Set up driver transports - arm_driver.joint_state.transport = core.LCMTransport("/xarm/joint_states", JointState) - arm_driver.robot_state.transport = core.LCMTransport("/xarm/robot_state", RobotState) - arm_driver.joint_position_command.transport = core.LCMTransport( - "/xarm/joint_position_command", JointCommand - ) - arm_driver.joint_velocity_command.transport = core.LCMTransport( - "/xarm/joint_velocity_command", JointCommand - ) - - print("Starting xArm driver...") - arm_driver.start() - - # ========================================================================= - # Step 3: Deploy Cartesian motion controller - # ========================================================================= - print("\nDeploying Cartesian motion controller...") - controller = dimos.deploy( # type: ignore[attr-defined] - CartesianMotionController, - arm_driver=arm_driver, - control_frequency=20.0, - position_kp=1.0, - position_kd=0.1, - orientation_kp=2.0, - orientation_kd=0.2, - max_linear_velocity=0.15, - max_angular_velocity=0.8, - position_tolerance=0.002, - orientation_tolerance=0.02, - velocity_control_mode=True, - ) - - # Set up controller transports - controller.joint_state.transport = core.LCMTransport("/xarm/joint_states", JointState) - controller.robot_state.transport = core.LCMTransport("/xarm/robot_state", RobotState) - controller.joint_position_command.transport = core.LCMTransport( - "/xarm/joint_position_command", JointCommand - ) - - # IMPORTANT: Configure controller to subscribe to /target_pose topic - controller.target_pose.transport = core.LCMTransport("/target_pose", PoseStamped) - - # Publish current pose for target setters to use - controller.current_pose.transport = core.LCMTransport("/xarm/current_pose", PoseStamped) - - print("Starting controller...") - controller.start() - - # ========================================================================= - # Step 4: Keep system running - # ========================================================================= - print("\n" + "=" * 80) - print("✓ System ready!") - print("=" * 80) - print("\nController is now listening to /target_pose topic") - print("Use target_setter.py to publish target poses") - print("\nPress Ctrl+C to shutdown") - print("=" * 80 + "\n") - - # Keep running until shutdown requested - while not shutdown_requested: - time.sleep(0.5) - - # ========================================================================= - # Step 5: Clean shutdown - # ========================================================================= - print("\nShutting down...") - print("Stopping controller...") - controller.stop() - print("Stopping driver...") - arm_driver.stop() - print("✓ Shutdown complete") - - finally: - # Always stop dimos cluster - print("Stopping dimos cluster...") - dimos.stop() # type: ignore[attr-defined] - - -if __name__ == "__main__": - """ - Topic-Based Cartesian Control for xArm. - - Usage: - # Terminal 1: Start the controller (this script) - python3 example_cartesian_control.py - - # Terminal 2: Publish target poses - python3 target_setter.py --world 0.4 0.0 0.5 # Absolute world coordinates - python3 target_setter.py --relative 0.05 0 0 # Relative movement (50mm in X) - - The controller subscribes to /target_pose topic and automatically moves - the robot to received target poses. - - Requirements: - - xArm robot connected at 192.168.2.235 - - Robot will be automatically enabled in servo mode - - Proper network configuration - """ - try: - main() # type: ignore[no-untyped-call] - except KeyboardInterrupt: - print("\n\nInterrupted by user") - except Exception as e: - print(f"\nError: {e}") - import traceback - - traceback.print_exc() diff --git a/dimos/manipulation/control/trajectory_controller/example_trajectory_control.py b/dimos/manipulation/control/trajectory_controller/example_trajectory_control.py deleted file mode 100644 index 100e095a45..0000000000 --- a/dimos/manipulation/control/trajectory_controller/example_trajectory_control.py +++ /dev/null @@ -1,189 +0,0 @@ -#!/usr/bin/env python3 -# 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. - -""" -Example: Joint Trajectory Control with xArm - -Demonstrates joint-space trajectory execution. The controller -executes trajectories by sampling at 100Hz and sending joint commands. - -This example shows: -1. Deploy xArm driver with LCM transports -2. Deploy JointTrajectoryController with LCM transports -3. Execute trajectories via RPC or topic -4. Monitor execution status - -Use trajectory_setter.py to interactively create and execute trajectories. -""" - -import signal -import time - -from dimos import core -from dimos.hardware.manipulators.xarm import XArmDriver -from dimos.manipulation.control import JointTrajectoryController -from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState -from dimos.msgs.trajectory_msgs import JointTrajectory, TrajectoryState - -# Global flag for graceful shutdown -shutdown_requested = False - - -def signal_handler(sig, frame): # type: ignore[no-untyped-def] - """Handle Ctrl+C for graceful shutdown.""" - global shutdown_requested - print("\n\nShutdown requested...") - shutdown_requested = True - - -def main(): # type: ignore[no-untyped-def] - """ - Deploy and run joint trajectory control system. - - The system executes joint trajectories at 100Hz by sampling - and forwarding joint positions to the arm driver. - """ - - # Register signal handler for graceful shutdown - signal.signal(signal.SIGINT, signal_handler) - signal.signal(signal.SIGTERM, signal_handler) - - # ========================================================================= - # Step 1: Start dimos cluster - # ========================================================================= - print("=" * 80) - print("Joint Trajectory Control") - print("=" * 80) - print("\nStarting dimos cluster...") - dimos = core.start(1) # Start with 1 worker - - try: - # ========================================================================= - # Step 2: Deploy xArm driver - # ========================================================================= - print("\nDeploying xArm driver...") - arm_driver = dimos.deploy( # type: ignore[attr-defined] - XArmDriver, - ip_address="192.168.1.210", - xarm_type="xarm6", - report_type="dev", - enable_on_start=True, - ) - - # Set up driver transports - arm_driver.joint_state.transport = core.LCMTransport("/xarm/joint_states", JointState) - arm_driver.robot_state.transport = core.LCMTransport("/xarm/robot_state", RobotState) - arm_driver.joint_position_command.transport = core.LCMTransport( - "/xarm/joint_position_command", JointCommand - ) - - print("Starting xArm driver...") - arm_driver.start() - - # ========================================================================= - # Step 3: Deploy Joint Trajectory Controller - # ========================================================================= - print("\nDeploying Joint Trajectory Controller...") - controller = dimos.deploy( # type: ignore[attr-defined] - JointTrajectoryController, - control_frequency=100.0, # 100Hz execution - ) - - # Set up controller transports - controller.joint_state.transport = core.LCMTransport("/xarm/joint_states", JointState) - controller.robot_state.transport = core.LCMTransport("/xarm/robot_state", RobotState) - controller.joint_position_command.transport = core.LCMTransport( - "/xarm/joint_position_command", JointCommand - ) - - # Subscribe to trajectory topic (from trajectory_setter.py) - controller.trajectory.transport = core.LCMTransport("/trajectory", JointTrajectory) - - print("Starting controller...") - controller.start() - - # Wait for joint state - print("\nWaiting for joint state...") - time.sleep(1.0) - - # ========================================================================= - # Step 4: Keep system running - # ========================================================================= - print("\n" + "=" * 80) - print("System ready!") - print("=" * 80) - print("\nJoint Trajectory Controller is running at 100Hz") - print("Listening on /trajectory topic") - print("\nUse trajectory_setter.py in another terminal to publish trajectories") - print("\nPress Ctrl+C to shutdown") - print("=" * 80 + "\n") - - # Keep running until shutdown requested - while not shutdown_requested: - # Print status periodically - status = controller.get_status() - if status.state == TrajectoryState.EXECUTING: - print( - f"\rExecuting: {status.progress:.1%} | " - f"elapsed={status.time_elapsed:.2f}s | " - f"remaining={status.time_remaining:.2f}s", - end="", - ) - time.sleep(0.5) - - # ========================================================================= - # Step 5: Clean shutdown - # ========================================================================= - print("\n\nShutting down...") - print("Stopping controller...") - controller.stop() - print("Stopping driver...") - arm_driver.stop() - print("Shutdown complete") - - finally: - # Always stop dimos cluster - print("Stopping dimos cluster...") - dimos.stop() # type: ignore[attr-defined] - - -if __name__ == "__main__": - """ - Joint Trajectory Control for xArm. - - Usage: - # Terminal 1: Start the controller (this script) - python3 example_trajectory_control.py - - # Terminal 2: Create and execute trajectories - python3 trajectory_setter.py - - The controller executes joint trajectories at 100Hz by sampling - and forwarding joint positions to the arm driver. - - Requirements: - - xArm robot connected at 192.168.1.210 - - Robot will be automatically enabled in servo mode - - Proper network configuration - """ - try: - main() # type: ignore[no-untyped-call] - except KeyboardInterrupt: - print("\n\nInterrupted by user") - except Exception as e: - print(f"\nError: {e}") - import traceback - - traceback.print_exc() diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 3ca80b91d2..5b51caebef 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -36,18 +36,6 @@ "unitree-g1-joystick": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:with_joystick", "unitree-g1-full": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:full_featured", "unitree-g1-detection": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:detection", - # xArm manipulator blueprints - "xarm-servo": "dimos.hardware.manipulators.xarm.blueprints:xarm_servo", - "xarm5-servo": "dimos.hardware.manipulators.xarm.blueprints:xarm5_servo", - "xarm7-servo": "dimos.hardware.manipulators.xarm.blueprints:xarm7_servo", - "xarm7-trajectory": "dimos.hardware.manipulators.xarm.blueprints:xarm7_trajectory", - "xarm-cartesian": "dimos.hardware.manipulators.xarm.blueprints:xarm_cartesian", - "xarm-trajectory": "dimos.hardware.manipulators.xarm.blueprints:xarm_trajectory", - "xarm-dual": "dimos.hardware.manipulators.xarm.blueprints:xarm_dual", - "xarm-dual-trajectory": "dimos.hardware.manipulators.xarm.blueprints:xarm_dual_trajectory", - # Piper manipulator blueprints - "piper-servo": "dimos.hardware.manipulators.piper.blueprints:piper_servo", - "piper-trajectory": "dimos.hardware.manipulators.piper.blueprints:piper_trajectory", # Control orchestrator blueprints "orchestrator-mock": "dimos.control.blueprints:orchestrator_mock", "orchestrator-xarm7": "dimos.control.blueprints:orchestrator_xarm7", @@ -95,10 +83,6 @@ "web_input": "dimos.agents.cli.web", # Control orchestrator module "control_orchestrator": "dimos.control.orchestrator", - # xArm manipulator modules - "xarm_driver": "dimos.hardware.manipulators.xarm.arm", - "cartesian_motion_controller": "dimos.manipulation.control.servo_control.cartesian_motion_controller", - "joint_trajectory_controller": "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller", } From 230b8d7d0724715414c99bf1037ba7279c9b19b3 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 13 Jan 2026 12:49:26 -0800 Subject: [PATCH 39/42] enforce ManipulatorBackend Protocol on the backend.py --- dimos/hardware/manipulators/piper/backend.py | 4 ++++ dimos/hardware/manipulators/xarm/backend.py | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/dimos/hardware/manipulators/piper/backend.py b/dimos/hardware/manipulators/piper/backend.py index 3b3c0671e5..cd8e16717a 100644 --- a/dimos/hardware/manipulators/piper/backend.py +++ b/dimos/hardware/manipulators/piper/backend.py @@ -24,6 +24,7 @@ from dimos.hardware.manipulators.spec import ( ControlMode, JointLimits, + ManipulatorBackend, ManipulatorInfo, ) @@ -501,4 +502,7 @@ def read_force_torque(self) -> list[float] | None: return None +# Static type check: verify PiperBackend implements ManipulatorBackend protocol +_: type[ManipulatorBackend] = PiperBackend + __all__ = ["PiperBackend"] diff --git a/dimos/hardware/manipulators/xarm/backend.py b/dimos/hardware/manipulators/xarm/backend.py index 2122f64408..4606640200 100644 --- a/dimos/hardware/manipulators/xarm/backend.py +++ b/dimos/hardware/manipulators/xarm/backend.py @@ -24,6 +24,7 @@ from dimos.hardware.manipulators.spec import ( ControlMode, JointLimits, + ManipulatorBackend, ManipulatorInfo, ) @@ -388,4 +389,7 @@ def read_force_torque(self) -> list[float] | None: return None +# Static type check: verify XArmBackend implements ManipulatorBackend protocol +_: type[ManipulatorBackend] = XArmBackend + __all__ = ["XArmBackend"] From 32165091559c5caa439c4ac4ff8f5008674ac56b Mon Sep 17 00:00:00 2001 From: stash Date: Wed, 14 Jan 2026 17:31:32 -0800 Subject: [PATCH 40/42] feat: add runtime protocol checks for manipulator backends --- dimos/control/hardware_interface.py | 8 ++++---- dimos/hardware/manipulators/piper/backend.py | 5 +---- dimos/hardware/manipulators/spec.py | 9 ++------- dimos/hardware/manipulators/xarm/backend.py | 5 +---- 4 files changed, 8 insertions(+), 19 deletions(-) diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 7de7d32339..e4fba70e1c 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -26,13 +26,10 @@ import time from typing import TYPE_CHECKING, Protocol, runtime_checkable -from dimos.hardware.manipulators.spec import ControlMode +from dimos.hardware.manipulators.spec import ControlMode, ManipulatorBackend logger = logging.getLogger(__name__) -if TYPE_CHECKING: - from dimos.hardware.manipulators.spec import ManipulatorBackend - @runtime_checkable class HardwareInterface(Protocol): @@ -103,6 +100,9 @@ def __init__( hardware_id: Unique identifier for this hardware joint_prefix: Prefix for joint names (defaults to hardware_id) """ + if not isinstance(backend, ManipulatorBackend): + raise TypeError("backend must implement ManipulatorBackend") + self._backend = backend self._hardware_id = hardware_id self._prefix = joint_prefix or hardware_id diff --git a/dimos/hardware/manipulators/piper/backend.py b/dimos/hardware/manipulators/piper/backend.py index cd8e16717a..1ce91dccd1 100644 --- a/dimos/hardware/manipulators/piper/backend.py +++ b/dimos/hardware/manipulators/piper/backend.py @@ -34,7 +34,7 @@ PIPER_TO_RAD = 1.0 / RAD_TO_PIPER # Piper units to radians -class PiperBackend: +class PiperBackend(ManipulatorBackend): """Piper-specific backend. Implements ManipulatorBackend protocol via duck typing. @@ -502,7 +502,4 @@ def read_force_torque(self) -> list[float] | None: return None -# Static type check: verify PiperBackend implements ManipulatorBackend protocol -_: type[ManipulatorBackend] = PiperBackend - __all__ = ["PiperBackend"] diff --git a/dimos/hardware/manipulators/spec.py b/dimos/hardware/manipulators/spec.py index d6f951456e..585043421e 100644 --- a/dimos/hardware/manipulators/spec.py +++ b/dimos/hardware/manipulators/spec.py @@ -24,7 +24,7 @@ from dataclasses import dataclass from enum import Enum -from typing import Protocol +from typing import Protocol, runtime_checkable from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 @@ -88,6 +88,7 @@ def default_base_transform() -> Transform: # ============================================================================ +@runtime_checkable class ManipulatorBackend(Protocol): """Protocol for hardware-specific IO. @@ -97,12 +98,6 @@ class ManipulatorBackend(Protocol): - Torque: Nm - Position: meters - Force: Newtons - - Benefits of Protocol over ABC: - - Structural typing (duck typing) - no inheritance required - - Backends just need matching methods, no base class - - Easy to create MockBackend for testing - - Type checkers validate compatibility """ # --- Connection --- diff --git a/dimos/hardware/manipulators/xarm/backend.py b/dimos/hardware/manipulators/xarm/backend.py index 4606640200..9adcdca24f 100644 --- a/dimos/hardware/manipulators/xarm/backend.py +++ b/dimos/hardware/manipulators/xarm/backend.py @@ -36,7 +36,7 @@ _XARM_MODE_JOINT_TORQUE = 6 -class XArmBackend: +class XArmBackend(ManipulatorBackend): """XArm-specific backend. Implements ManipulatorBackend protocol via duck typing. @@ -389,7 +389,4 @@ def read_force_torque(self) -> list[float] | None: return None -# Static type check: verify XArmBackend implements ManipulatorBackend protocol -_: type[ManipulatorBackend] = XArmBackend - __all__ = ["XArmBackend"] From 9051c9243db99428d0ab03f741a739574fb3fa60 Mon Sep 17 00:00:00 2001 From: stash Date: Wed, 14 Jan 2026 18:16:06 -0800 Subject: [PATCH 41/42] added runtime checking for controlTask protocol --- dimos/control/orchestrator.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/dimos/control/orchestrator.py b/dimos/control/orchestrator.py index 885b4df348..c194894af8 100644 --- a/dimos/control/orchestrator.py +++ b/dimos/control/orchestrator.py @@ -34,6 +34,7 @@ from typing import TYPE_CHECKING, Any from dimos.control.hardware_interface import BackendHardwareInterface, HardwareInterface +from dimos.control.task import ControlTask from dimos.control.tick_loop import TickLoop from dimos.core import Module, Out, rpc from dimos.core.module import ModuleConfig @@ -44,7 +45,6 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from dimos.control.task import ControlTask from dimos.hardware.manipulators.spec import ManipulatorBackend logger = setup_logger() @@ -351,6 +351,9 @@ def get_joint_positions(self) -> dict[str, float]: @rpc def add_task(self, task: ControlTask) -> bool: """Register a task with the orchestrator.""" + if not isinstance(task, ControlTask): + raise TypeError("task must implement ControlTask") + with self._task_lock: if task.name in self._tasks: logger.warning(f"Task {task.name} already registered") From 26d732b691043293ed842227a2ce9c137096998a Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 14 Jan 2026 19:04:22 -0800 Subject: [PATCH 42/42] Add TaskStatus dataclass, refactor get_trajectory_status and Explicitly inherit from ControlTask protocol --- dimos/control/orchestrator.py | 33 +++++++++++++++++++------- dimos/control/tasks/trajectory_task.py | 2 +- 2 files changed, 26 insertions(+), 9 deletions(-) diff --git a/dimos/control/orchestrator.py b/dimos/control/orchestrator.py index c194894af8..e7ea147dcf 100644 --- a/dimos/control/orchestrator.py +++ b/dimos/control/orchestrator.py @@ -95,6 +95,21 @@ class TaskConfig: priority: int = 10 +@dataclass +class TaskStatus: + """Status of a control task. + + Attributes: + active: Whether the task is currently active + state: Task state name (e.g., "IDLE", "RUNNING", "DONE") + progress: Task progress from 0.0 to 1.0 + """ + + active: bool + state: str | None = None + progress: float | None = None + + @dataclass class ControlOrchestratorConfig(ModuleConfig): """Configuration for the ControlOrchestrator. @@ -414,24 +429,26 @@ def execute_trajectory(self, task_name: str, trajectory: JointTrajectory) -> boo return task.execute(trajectory) # type: ignore[attr-defined,no-any-return] @rpc - def get_trajectory_status(self, task_name: str) -> dict[str, Any]: + def get_trajectory_status(self, task_name: str) -> TaskStatus | None: """Get the status of a trajectory task.""" with self._task_lock: task = self._tasks.get(task_name) if task is None: - return {} - - result: dict[str, Any] = {"active": task.is_active()} + return None + state: str | None = None if hasattr(task, "get_state"): - state: TrajectoryState = task.get_state() # type: ignore[attr-defined] - result["state"] = state.name if isinstance(state, TrajectoryState) else str(state) + task_state: TrajectoryState = task.get_state() # type: ignore[attr-defined] + state = ( + task_state.name if isinstance(task_state, TrajectoryState) else str(task_state) + ) + progress: float | None = None if hasattr(task, "get_progress"): t_now = time.perf_counter() - result["progress"] = task.get_progress(t_now) # type: ignore[attr-defined] + progress = task.get_progress(t_now) # type: ignore[attr-defined] - return result + return TaskStatus(active=task.is_active(), state=state, progress=progress) @rpc def cancel_trajectory(self, task_name: str) -> bool: diff --git a/dimos/control/tasks/trajectory_task.py b/dimos/control/tasks/trajectory_task.py index 30faacdad5..08e3ae337e 100644 --- a/dimos/control/tasks/trajectory_task.py +++ b/dimos/control/tasks/trajectory_task.py @@ -51,7 +51,7 @@ class JointTrajectoryTaskConfig: priority: int = 10 -class JointTrajectoryTask: +class JointTrajectoryTask(ControlTask): """Passive trajectory execution task. Unlike JointTrajectoryController which owns a thread, this task