diff --git a/.gitignore b/.gitignore index 18fd575c85..4f9c85b321 100644 --- a/.gitignore +++ b/.gitignore @@ -8,6 +8,7 @@ __pycache__/ *venv*/ .venv*/ .ssh/ +.direnv/ # Ignore python tooling dirs *.egg-info/ diff --git a/dimos/hardware/end_effector.py b/dimos/hardware/end_effectors/end_effector.py similarity index 100% rename from dimos/hardware/end_effector.py rename to dimos/hardware/end_effectors/end_effector.py diff --git a/dimos/hardware/can_activate.sh b/dimos/hardware/manipulators/piper/can_activate.sh similarity index 100% rename from dimos/hardware/can_activate.sh rename to dimos/hardware/manipulators/piper/can_activate.sh diff --git a/dimos/hardware/piper_arm.py b/dimos/hardware/manipulators/piper/piper_arm.py similarity index 100% rename from dimos/hardware/piper_arm.py rename to dimos/hardware/manipulators/piper/piper_arm.py diff --git a/dimos/hardware/piper_description.urdf b/dimos/hardware/manipulators/piper/piper_description.urdf similarity index 100% rename from dimos/hardware/piper_description.urdf rename to dimos/hardware/manipulators/piper/piper_description.urdf diff --git a/dimos/hardware/manipulators/xarm/README.md b/dimos/hardware/manipulators/xarm/README.md new file mode 100644 index 0000000000..ff7a797cad --- /dev/null +++ b/dimos/hardware/manipulators/xarm/README.md @@ -0,0 +1,149 @@ +# 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/xarm/components/__init__.py b/dimos/hardware/manipulators/xarm/components/__init__.py new file mode 100644 index 0000000000..4592560cda --- /dev/null +++ b/dimos/hardware/manipulators/xarm/components/__init__.py @@ -0,0 +1,15 @@ +"""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/xarm/components/gripper_control.py b/dimos/hardware/manipulators/xarm/components/gripper_control.py new file mode 100644 index 0000000000..cb8acfb88f --- /dev/null +++ b/dimos/hardware/manipulators/xarm/components/gripper_control.py @@ -0,0 +1,363 @@ +# Copyright 2025 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 dimos.core import rpc +from dimos.utils.logging_config import setup_logger + +logger = setup_logger(__file__) + + +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 + """ + + # ========================================================================= + # 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 | 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/xarm/components/kinematics.py b/dimos/hardware/manipulators/xarm/components/kinematics.py new file mode 100644 index 0000000000..2a3810d769 --- /dev/null +++ b/dimos/hardware/manipulators/xarm/components/kinematics.py @@ -0,0 +1,76 @@ +# Copyright 2025 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 dimos.core import rpc +from dimos.utils.logging_config import setup_logger + +logger = setup_logger(__file__) + + +class KinematicsComponent: + """ + Component providing kinematics RPC methods for XArmDriver. + + This component assumes the parent class has: + - self.arm: XArmAPI instance + - self.config: XArmDriverConfig instance + """ + + @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/xarm/components/motion_control.py b/dimos/hardware/manipulators/xarm/components/motion_control.py new file mode 100644 index 0000000000..31c92de76f --- /dev/null +++ b/dimos/hardware/manipulators/xarm/components/motion_control.py @@ -0,0 +1,136 @@ +# Copyright 2025 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 + +from dimos.core import rpc +from dimos.utils.logging_config import setup_logger + +logger = setup_logger(__file__) + + +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]] + """ + + @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/xarm/components/state_queries.py b/dimos/hardware/manipulators/xarm/components/state_queries.py new file mode 100644 index 0000000000..2f3c0e467c --- /dev/null +++ b/dimos/hardware/manipulators/xarm/components/state_queries.py @@ -0,0 +1,172 @@ +# Copyright 2025 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 +""" + +from dimos.core import rpc +from dimos.msgs.sensor_msgs import JointState, RobotState +from dimos.utils.logging_config import setup_logger + +logger = setup_logger(__file__) + + +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] + """ + + @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/xarm/components/system_control.py b/dimos/hardware/manipulators/xarm/components/system_control.py new file mode 100644 index 0000000000..ba6e65f756 --- /dev/null +++ b/dimos/hardware/manipulators/xarm/components/system_control.py @@ -0,0 +1,539 @@ +# Copyright 2025 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 dimos.core import rpc +from dimos.utils.logging_config import setup_logger + +logger = setup_logger(__file__) + + +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 + """ + + @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/xarm/interactive_control.py b/dimos/hardware/manipulators/xarm/interactive_control.py new file mode 100755 index 0000000000..9b58a3e1ae --- /dev/null +++ b/dimos/hardware/manipulators/xarm/interactive_control.py @@ -0,0 +1,534 @@ +#!/usr/bin/env python3 +# Copyright 2025 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 Terminal UI for xArm Control. + +Provides a menu-driven interface to: +- Select which joint to move +- Specify movement delta in degrees +- Execute smooth trajectories +- View current joint positions + +Usage: + export XARM_IP=192.168.1.235 + venv/bin/python dimos/hardware/manipulators/xarm/interactive_control.py +""" + +import math +import os +import time + +from dimos import core +from dimos.hardware.manipulators.xarm.sample_trajectory_generator import ( + SampleTrajectoryGenerator, +) +from dimos.hardware.manipulators.xarm.xarm_driver import XArmDriver +from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState +from dimos.utils.logging_config import setup_logger + +logger = setup_logger(__file__) + + +def print_banner() -> None: + """Print welcome banner.""" + print("\n" + "=" * 80) + print(" xArm Interactive Control") + print(" Real-time joint control via terminal UI") + print("=" * 80) + + +def print_current_state(traj_gen) -> None: + """Display current joint positions.""" + state = traj_gen.get_current_state() + + print("\n" + "-" * 80) + print("CURRENT JOINT POSITIONS:") + print("-" * 80) + + if state["joint_state"]: + js = state["joint_state"] + for i in range(len(js.position)): + pos_deg = math.degrees(js.position[i]) + vel_deg = math.degrees(js.velocity[i]) + print(f" Joint {i + 1}: {pos_deg:8.2f}° (velocity: {vel_deg:6.2f}°/s)") + else: + print(" ⚠ No joint state available yet") + + if state["robot_state"]: + rs = state["robot_state"] + print( + f"\n Robot Status: state={rs.state} (0=ready), mode={rs.mode} (1=servo), error={rs.error_code}" + ) + + print("-" * 80) + + +def get_control_mode(): + """Get control mode selection from user.""" + while True: + try: + print("\nSelect control mode:") + print(" 1. Position control (move by angle)") + print(" 2. Velocity control (move with velocity)") + choice = input("Mode (1 or 2): ").strip() + + if choice == "1": + print(choice) + return "position" + elif choice == "2": + print(choice) + return "velocity" + else: + print("⚠ Invalid choice. Please enter 1 or 2.") + + except KeyboardInterrupt: + print("\n\nInterrupted by user") + return None + + +def get_joint_selection(num_joints: int): + """Get joint selection from user.""" + while True: + try: + print(f"\nSelect joint to move (1-{num_joints}), or 0 to quit:") + choice = input("Joint number: ").strip() + + if not choice: + continue + + joint_num = int(choice) + + if joint_num == 0: + return None + + if 1 <= joint_num <= num_joints: + return joint_num - 1 # Convert to 0-indexed + else: + print(f"⚠ Invalid joint number. Please enter 1-{num_joints} (or 0 to quit)") + + except ValueError: + print("⚠ Invalid input. Please enter a number.") + except KeyboardInterrupt: + print("\n\nInterrupted by user") + return None + + +def get_delta_degrees(): + """Get movement delta from user.""" + while True: + try: + print("\nEnter movement delta in degrees:") + print(" Positive = counterclockwise") + print(" Negative = clockwise") + delta_str = input("Delta (degrees): ").strip() + + if not delta_str: + continue + + delta = float(delta_str) + + # Sanity check + if abs(delta) > 180: + confirm = input(f"⚠ Large movement ({delta}°). Continue? (y/n): ").strip().lower() + if confirm != "y": + continue + + return delta + + except ValueError: + print("⚠ Invalid input. Please enter a number.") + except KeyboardInterrupt: + print("\n\nInterrupted by user") + return None + + +def get_velocity_deg_s(): + """Get velocity from user.""" + while True: + try: + print("\nEnter target velocity in degrees/second:") + print(" Positive = counterclockwise") + print(" Negative = clockwise") + vel_str = input("Velocity (°/s): ").strip() + + if not vel_str: + continue + + velocity = float(vel_str) + + # Sanity check + if abs(velocity) > 100: + confirm = ( + input(f"⚠ High velocity ({velocity}°/s). Continue? (y/n): ").strip().lower() + ) + if confirm != "y": + continue + + return velocity + + except ValueError: + print("⚠ Invalid input. Please enter a number.") + except KeyboardInterrupt: + print("\n\nInterrupted by user") + return None + + +def get_duration(): + """Get movement duration from user.""" + while True: + try: + print("\nEnter movement duration in seconds (default: 1.0):") + duration_str = input("Duration (s): ").strip() + + if not duration_str: + return 1.0 # Default + + duration = float(duration_str) + + if duration <= 0: + print("⚠ Duration must be positive") + continue + + if duration > 10: + confirm = input(f"⚠ Long duration ({duration}s). Continue? (y/n): ").strip().lower() + if confirm != "y": + continue + + return duration + + except ValueError: + print("⚠ Invalid input. Please enter a number.") + except KeyboardInterrupt: + print("\n\nInterrupted by user") + return None + + +def confirm_motion_position(joint_index, delta_degrees, duration): + """Confirm position motion with user.""" + print("\n" + "=" * 80) + print("POSITION MOTION SUMMARY:") + print(f" Joint: {joint_index + 1}") + print( + f" Delta: {delta_degrees:+.2f}° ({'clockwise' if delta_degrees < 0 else 'counterclockwise'})" + ) + print(f" Duration: {duration:.2f}s") + print("=" * 80) + + confirm = input("\nExecute this motion? (y/n): ").strip().lower() + return confirm == "y" + + +def confirm_motion_velocity(joint_index, velocity_deg_s, duration): + """Confirm velocity motion with user.""" + print("\n" + "=" * 80) + print("VELOCITY MOTION SUMMARY:") + print(f" Joint: {joint_index + 1}") + print( + f" Velocity: {velocity_deg_s:+.2f}°/s ({'clockwise' if velocity_deg_s < 0 else 'counterclockwise'})" + ) + print(f" Duration: {duration:.2f}s (with ramp up/down)") + print(" Profile: 20% ramp up, 60% constant, 20% ramp down") + print("=" * 80) + + confirm = input("\nExecute this motion? (y/n): ").strip().lower() + return confirm == "y" + + +def wait_for_trajectory_completion(traj_gen, duration) -> None: + """Wait for trajectory to complete and show progress.""" + print("\n⚙ Executing motion...") + + # Wait with progress updates + steps = 10 + step_duration = duration / steps + + for i in range(steps): + time.sleep(step_duration) + progress = ((i + 1) / steps) * 100 + print(f" Progress: {progress:.0f}%") + + # Extra time for settling + time.sleep(0.5) + + # Check if completed + state = traj_gen.get_current_state() + if state["trajectory_active"]: + print("⚠ Trajectory still active, waiting...") + time.sleep(duration * 0.5) + + print("✓ Motion complete!") + + +def interactive_control_loop(xarm, traj_gen, num_joints: int) -> None: + """Main interactive control loop.""" + print_banner() + + # Wait for initial state + print("\nInitializing... waiting for robot state...") + time.sleep(2.0) + + # Enable servo mode and set state to ready + state = traj_gen.get_current_state() + if state["robot_state"]: + if state["robot_state"].mode != 1: + print("\n⚙ Enabling servo mode...") + code, msg = xarm.enable_servo_mode() + if code == 0: + print(f"✓ {msg}") + time.sleep(0.5) + + if state["robot_state"].state != 0: + print("⚙ Setting robot to ready state...") + code, msg = xarm.set_state(0) + if code == 0: + print(f"✓ {msg}") + time.sleep(0.5) + + # Enable command publishing + print("\n⚙ Enabling command publishing...") + traj_gen.enable_publishing() + time.sleep(1.0) + print("✓ System ready for motion control") + + # Track current control mode (starts in position mode) + current_mode = "position" + + # Main control loop + while True: + try: + # Display current state + print_current_state(traj_gen) + + # Show current mode + mode_indicator = "POSITION" if current_mode == "position" else "VELOCITY" + print(f"\n[Current Mode: {mode_indicator}]") + + # Get control mode selection + control_mode = get_control_mode() + if control_mode is None: + break + + # Switch modes if user selected a different mode + if control_mode != current_mode: + print(f"\n⚙ Switching from {current_mode} mode to {control_mode} mode...") + + if control_mode == "velocity": + # Switch to velocity control mode (mode 4) + code, msg = xarm.enable_velocity_control_mode() + print(f" {msg} (code: {code})") + if code == 0: + current_mode = "velocity" + # Notify trajectory generator about mode change + traj_gen.set_velocity_mode(True) + time.sleep(0.3) + else: + print(f"⚠ Failed to enable velocity mode, staying in {current_mode} mode") + continue + else: + # Switch to position control mode (mode 1) + code, msg = xarm.disable_velocity_control_mode() + print(f" {msg} (code: {code})") + if code == 0: + current_mode = "position" + # Notify trajectory generator about mode change + traj_gen.set_velocity_mode(False) + time.sleep(0.3) + else: + print(f"⚠ Failed to enable position mode, staying in {current_mode} mode") + continue + + # Get joint selection + joint_index = get_joint_selection(num_joints) + if joint_index is None: + break + + # Get parameters based on mode + if control_mode == "position": + # Position control: get delta + delta_degrees = get_delta_degrees() + if delta_degrees is None: + break + + duration = get_duration() + if duration is None: + break + + # Confirm motion + if not confirm_motion_position(joint_index, delta_degrees, duration): + print("⚠ Motion cancelled") + continue + + # Execute position motion + result = traj_gen.move_joint( + joint_index=joint_index, delta_degrees=delta_degrees, duration=duration + ) + print(f"\n✓ {result}") + + # Wait for completion + wait_for_trajectory_completion(traj_gen, duration) + + else: # velocity mode + # Velocity control: get velocity + velocity_deg_s = get_velocity_deg_s() + if velocity_deg_s is None: + break + + duration = get_duration() + if duration is None: + break + + # Confirm motion + if not confirm_motion_velocity(joint_index, velocity_deg_s, duration): + print("⚠ Motion cancelled") + continue + + # Execute velocity motion (mode already set above if needed) + result = traj_gen.move_joint_velocity( + joint_index=joint_index, velocity_deg_s=velocity_deg_s, duration=duration + ) + print(f"\n✓ {result}") + + # Wait for completion + wait_for_trajectory_completion(traj_gen, duration) + + # Ask to continue + print("\n" + "=" * 80) + continue_choice = input("\nContinue with another motion? (y/n): ").strip().lower() + if continue_choice != "y": + break + + except KeyboardInterrupt: + print("\n\n⚠ Interrupted by user") + break + except Exception as e: + print(f"\n⚠ Error: {e}") + continue_choice = input("\nContinue despite error? (y/n): ").strip().lower() + if continue_choice != "y": + break + + print("\n" + "=" * 80) + print("Shutting down...") + print("=" * 80) + + +def main() -> None: + """Run interactive xArm control.""" + import argparse + + # Parse command-line arguments + parser = argparse.ArgumentParser(description="Interactive xArm Control") + parser.add_argument( + "--ip", type=str, default=None, help="xArm IP address (overrides XARM_IP env var)" + ) + parser.add_argument( + "--type", + type=str, + default="xarm6", + choices=["xarm5", "xarm6", "xarm7"], + help="xArm model type: xarm5, xarm6, or xarm7 (default: xarm6)", + ) + args = parser.parse_args() + + # Determine IP address: command-line arg > env var > default + if args.ip: + ip_address = args.ip + logger.info(f"Using xArm at IP (from --ip): {ip_address}") + else: + ip_address = os.getenv("XARM_IP", "192.168.1.235") + if ip_address == "192.168.1.235": + logger.warning(f"Using default IP: {ip_address}") + logger.warning("Specify IP with: --ip 192.168.1.XXX or export XARM_IP=192.168.1.XXX") + else: + logger.info(f"Using xArm at IP (from XARM_IP env): {ip_address}") + + xarm_type = args.type + logger.info(f"Using {xarm_type.upper()}") + + # Derive num_joints from xarm_type for compatibility with SampleTrajectoryGenerator + num_joints = {"xarm5": 5, "xarm6": 6, "xarm7": 7}[xarm_type] + + # Start dimos + logger.info("Starting dimos...") + dimos = core.start(1) + + # Deploy xArm driver + logger.info("Deploying XArmDriver...") + xarm = dimos.deploy( + XArmDriver, + ip_address=ip_address, + xarm_type=xarm_type, + report_type="dev", + enable_on_start=True, + ) + + # Set up driver transports + xarm.joint_state.transport = core.LCMTransport("/xarm/joint_states", JointState) + xarm.robot_state.transport = core.LCMTransport("/xarm/robot_state", RobotState) + xarm.joint_position_command.transport = core.LCMTransport( + "/xarm/joint_position_command", JointCommand + ) + xarm.joint_velocity_command.transport = core.LCMTransport( + "/xarm/joint_velocity_command", JointCommand + ) + + # Start driver + logger.info("Starting xArm driver...") + xarm.start() + + # Deploy trajectory generator + logger.info("Deploying SampleTrajectoryGenerator...") + traj_gen = dimos.deploy( + SampleTrajectoryGenerator, + num_joints=num_joints, + control_mode="position", + publish_rate=100.0, # 100 Hz + enable_on_start=False, + ) + + # Set up trajectory generator transports + traj_gen.joint_state_input.transport = core.LCMTransport("/xarm/joint_states", JointState) + traj_gen.robot_state_input.transport = core.LCMTransport("/xarm/robot_state", RobotState) + traj_gen.joint_position_command.transport = core.LCMTransport( + "/xarm/joint_position_command", JointCommand + ) + traj_gen.joint_velocity_command.transport = core.LCMTransport( + "/xarm/joint_velocity_command", JointCommand + ) + + # Start trajectory generator + logger.info("Starting trajectory generator...") + traj_gen.start() + + try: + # Run interactive control loop + interactive_control_loop(xarm, traj_gen, num_joints) + + except KeyboardInterrupt: + print("\n\n⚠ Interrupted by user") + + finally: + # Cleanup + print("\nStopping trajectory generator...") + traj_gen.stop() + print("Stopping xArm driver...") + xarm.stop() + print("Stopping dimos...") + dimos.stop() + print("✓ Shutdown complete\n") + + +if __name__ == "__main__": + main() diff --git a/dimos/hardware/manipulators/xarm/sample_trajectory_generator.py b/dimos/hardware/manipulators/xarm/sample_trajectory_generator.py new file mode 100644 index 0000000000..721a2a029c --- /dev/null +++ b/dimos/hardware/manipulators/xarm/sample_trajectory_generator.py @@ -0,0 +1,564 @@ +# Copyright 2025 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. + +""" +Sample Trajectory Generator for xArm Manipulator. + +This module demonstrates how to: +- Subscribe to joint_state and robot_state from the xArm driver +- Publish joint position or velocity commands +- Implement a simple control loop + +Usage: + dimos = core.start(1) + + # Deploy trajectory generator + traj_gen = dimos.deploy( + SampleTrajectoryGenerator, + num_joints=6, + control_mode="position", # or "velocity" + publish_rate=10.0, + ) + + # Set up transports + traj_gen.joint_state_input.transport = core.LCMTransport("/xarm/joint_states", JointState) + traj_gen.robot_state_input.transport = core.LCMTransport("/xarm/robot_state", RobotState) + traj_gen.joint_position_command.transport = core.LCMTransport("/xarm/joint_position_command", List[float]) + + traj_gen.start() +""" + +from dataclasses import dataclass +import math +import threading +import time + +from dimos.core import In, Module, ModuleConfig, Out, rpc +from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState +from dimos.utils.logging_config import setup_logger + +logger = setup_logger(__file__) + + +@dataclass +class TrajectoryGeneratorConfig(ModuleConfig): + """Configuration for trajectory generator.""" + + num_joints: int = 6 # Number of joints (5, 6, or 7) + control_mode: str = "position" # "position" or "velocity" + publish_rate: float = 10.0 # Command publishing rate in Hz + enable_on_start: bool = False # Start publishing commands immediately + + +class SampleTrajectoryGenerator(Module): + """ + Sample trajectory generator for xArm manipulator. + + This module demonstrates command publishing and state monitoring. + Currently sends zero commands, but can be extended for trajectory generation. + + Architecture: + - Subscribes to joint_state and robot_state from xArm driver + - Publishes either joint_position_command OR joint_velocity_command + - Runs a control loop at publish_rate Hz + """ + + default_config = TrajectoryGeneratorConfig + + # Input topics (state feedback from robot) + joint_state_input: In[JointState] = None # Current joint state + robot_state_input: In[RobotState] = None # Current robot state + + # Output topics (commands to robot) - only use ONE at a time + joint_position_command: Out[JointCommand] = None # Position commands (radians) + joint_velocity_command: Out[JointCommand] = None # Velocity commands (rad/s) + + def __init__(self, **kwargs) -> None: + super().__init__(**kwargs) + + # State tracking + self._current_joint_state: JointState | None = None + self._current_robot_state: RobotState | None = None + self._state_lock = threading.Lock() + + # Control thread + self._running = False + self._stop_event = threading.Event() + self._control_thread: threading.Thread | None = None + + # Publishing enabled flag + self._publishing_enabled = self.config.enable_on_start + + # Command publish counter (for logging) + self._command_count = 0 + + # Trajectory state + self._trajectory_active = False + self._trajectory_start_time = 0.0 + self._trajectory_duration = 0.0 + self._trajectory_start_positions = None + self._trajectory_end_positions = None + self._trajectory_is_velocity = False # True for velocity trajectories + self._in_velocity_mode = ( + False # Persistent flag for velocity mode (doesn't reset with trajectory) + ) + + logger.info( + f"TrajectoryGenerator initialized: {self.config.num_joints} joints, " + f"mode={self.config.control_mode}, rate={self.config.publish_rate}Hz" + ) + + @rpc + def start(self) -> None: + """Start the trajectory generator.""" + super().start() + + # Subscribe to state topics + try: + unsub_js = self.joint_state_input.subscribe(self._on_joint_state) + self._disposables.add(lambda: unsub_js()) + except (AttributeError, ValueError) as e: + logger.debug(f"joint_state_input transport not configured: {e}") + + try: + unsub_rs = self.robot_state_input.subscribe(self._on_robot_state) + self._disposables.add(lambda: unsub_rs()) + except (AttributeError, ValueError) as e: + logger.debug(f"robot_state_input transport not configured: {e}") + + # Start control loop + self._start_control_loop() + + logger.info("Trajectory generator started") + + @rpc + def stop(self) -> None: + """Stop the trajectory generator.""" + logger.info("Stopping trajectory generator...") + + # Stop control thread + self._running = False + self._stop_event.set() + + if self._control_thread and self._control_thread.is_alive(): + self._control_thread.join(timeout=2.0) + + super().stop() + logger.info("Trajectory generator stopped") + + @rpc + def enable_publishing(self) -> None: + """Enable command publishing.""" + self._publishing_enabled = True + logger.info("Command publishing enabled") + + @rpc + def disable_publishing(self) -> None: + """Disable command publishing.""" + self._publishing_enabled = False + logger.info("Command publishing disabled") + + @rpc + def set_velocity_mode(self, enabled: bool) -> None: + """ + Set velocity mode flag. + + This should be called when the driver switches between position and velocity modes, + so the trajectory generator knows which topic to publish to. + + Args: + enabled: True for velocity mode, False for position mode + """ + with self._state_lock: + self._in_velocity_mode = enabled + + # IMPORTANT: Clear any active trajectory when switching modes + # This prevents stale commands from being published in the new mode + if self._trajectory_active: + logger.info("Clearing active trajectory due to mode change") + self._trajectory_active = False + self._trajectory_is_velocity = False + self._trajectory_start_positions = None + self._trajectory_end_positions = None + + mode_name = "velocity" if enabled else "position" + logger.info(f"Trajectory generator mode set to: {mode_name}") + + @rpc + def get_current_state(self) -> dict: + """Get current joint and robot state.""" + with self._state_lock: + return { + "joint_state": self._current_joint_state, + "robot_state": self._current_robot_state, + "publishing_enabled": self._publishing_enabled, + "trajectory_active": self._trajectory_active, + } + + @rpc + def move_joint(self, joint_index: int, delta_degrees: float, duration: float) -> str: + """ + Move a single joint by a relative amount over a duration. + + Args: + joint_index: Index of joint to move (0-based, so joint 6 = index 5) + delta_degrees: Amount to rotate in degrees (positive = counterclockwise) + duration: Time to complete motion in seconds + + Returns: + Status message + """ + # Re-enable publishing if it was disabled (e.g., after testing timeout) + if not self._publishing_enabled: + logger.info("Re-enabling publishing for new trajectory") + self._publishing_enabled = True + + with self._state_lock: + if self._current_joint_state is None: + return "Error: No joint state received yet" + + if self._trajectory_active: + return "Error: Trajectory already in progress" + + if joint_index < 0 or joint_index >= self.config.num_joints: + return f"Error: Invalid joint index {joint_index} (must be 0-{self.config.num_joints - 1})" + + # Convert delta to radians (Note: positive degrees = clockwise for rotation) + delta_rad = math.radians(delta_degrees) + + # Set up trajectory + self._trajectory_start_positions = list(self._current_joint_state.position) + self._trajectory_end_positions = list(self._current_joint_state.position) + self._trajectory_end_positions[joint_index] += delta_rad + self._trajectory_duration = duration + self._trajectory_start_time = time.time() + self._trajectory_active = True + self._in_velocity_mode = False # Clear velocity mode flag for position moves + + logger.info( + f"Starting trajectory: joint{joint_index + 1} " + f"from {math.degrees(self._trajectory_start_positions[joint_index]):.2f}° " + f"to {math.degrees(self._trajectory_end_positions[joint_index]):.2f}° " + f"over {duration}s" + ) + + return ( + f"Started moving joint {joint_index + 1} by {delta_degrees:.1f}° over {duration}s" + ) + + @rpc + def move_joint_velocity(self, joint_index: int, velocity_deg_s: float, duration: float) -> str: + """ + Move a single joint with velocity control (constant velocity). + + Sends constant velocity commands for the specified duration. + + Args: + joint_index: Index of joint to move (0-based, so joint 6 = index 5) + velocity_deg_s: Target velocity in degrees/second (positive = counterclockwise) + duration: Time to send velocity commands in seconds + + Returns: + Status message + """ + # Re-enable publishing if it was disabled (e.g., after testing timeout) + if not self._publishing_enabled: + logger.info("Re-enabling publishing for new trajectory") + self._publishing_enabled = True + + with self._state_lock: + if self._current_joint_state is None: + return "Error: No joint state received yet" + + if self._trajectory_active: + return "Error: Trajectory already in progress" + + if joint_index < 0 or joint_index >= self.config.num_joints: + return f"Error: Invalid joint index {joint_index} (must be 0-{self.config.num_joints - 1})" + + # NOTE: xArm SDK vc_set_joint_velocity expects degrees/second, not radians! + # So we keep velocity in degrees/second + velocity_value = velocity_deg_s + + # Set up trajectory (using same state variables, but different generation logic) + self._trajectory_start_positions = [joint_index] # Store joint index + self._trajectory_end_positions = [velocity_value] # Store target velocity in deg/s + self._trajectory_duration = duration + self._trajectory_start_time = time.time() + self._trajectory_active = True + self._trajectory_is_velocity = True # Flag to use velocity generation + self._in_velocity_mode = True # Set persistent velocity mode flag + + logger.info( + f"Starting velocity trajectory: joint{joint_index + 1} " + f"velocity={velocity_deg_s:.2f}°/s " + f"duration={duration}s (constant velocity)" + ) + + return f"Started velocity control on joint {joint_index + 1}: {velocity_deg_s:.1f}°/s for {duration}s" + + # ========================================================================= + # Private Methods: Callbacks + # ========================================================================= + + def _on_joint_state(self, msg: JointState) -> None: + """Callback for receiving joint state updates.""" + with self._state_lock: + # Log first message with all joints + if self._current_joint_state is None: + logger.info("✓ Received first joint state:") + logger.info(f" Positions (rad): {[f'{p:.4f}' for p in msg.position]}") + logger.info( + f" Positions (deg): {[f'{math.degrees(p):.2f}' for p in msg.position]}" + ) + logger.info(f" Velocities (rad/s): {[f'{v:.4f}' for v in msg.velocity]}") + logger.info( + f" Velocities (deg/s): {[f'{math.degrees(v):.2f}' for v in msg.velocity]}" + ) + self._current_joint_state = msg + + def _on_robot_state(self, msg: RobotState) -> None: + """Callback for receiving robot state updates.""" + with self._state_lock: + # Log first message or when state/error changes + if self._current_robot_state is None: + logger.info( + f"✓ Received first robot state: " + f"state={msg.state}, mode={msg.mode}, " + f"error={msg.error_code}, warn={msg.warn_code}" + ) + elif ( + msg.state != self._current_robot_state.state + or msg.error_code != self._current_robot_state.error_code + ): + # State definitions: 1=ready, 2=moving, 3=paused, 4=stopped, 5=stopped(?) + logger.info( + f"⚠ Robot state changed: " + f"state={msg.state}, mode={msg.mode}, " + f"error={msg.error_code}, warn={msg.warn_code}" + ) + if msg.error_code != 0: + logger.warning( + f"⚠ Robot has error code {msg.error_code} " + f"(9='not ready to move', check if servo is enabled)" + ) + self._current_robot_state = msg + + # ========================================================================= + # Private Methods: Control Loop + # ========================================================================= + + def _start_control_loop(self) -> None: + """Start the control loop thread.""" + logger.info(f"Starting control loop at {self.config.publish_rate}Hz") + + self._running = True + self._stop_event.clear() + + self._control_thread = threading.Thread( + target=self._control_loop, daemon=True, name="traj_gen_control_thread" + ) + self._control_thread.start() + + def _control_loop(self) -> None: + """ + Control loop for publishing commands. + + Runs at publish_rate Hz and publishes either position or velocity commands. + """ + period = 1.0 / self.config.publish_rate + next_time = time.time() + loop_count = 0 + + logger.info( + f"Control loop started at {self.config.publish_rate}Hz " + f"(mode={self.config.control_mode})" + ) + + while self._running: + loop_count += 1 + try: + # Only publish if enabled + if self._publishing_enabled: + # Generate command (currently just zeros) + command = self._generate_command() + + # Publish command based on control mode + if command is not None: + # Log current joint state periodically (every 50 loops) + # if loop_count % 50 == 0: + # with self._state_lock: + # if self._current_joint_state is not None: + # js = self._current_joint_state + # logger.info( + # f"Loop #{loop_count}: Current joint positions (deg): " + # f"{[f'{math.degrees(p):.2f}' for p in js.position]}" + # ) + # logger.info( + # f"Loop #{loop_count}: Current joint velocities (deg/s): " + # f"{[f'{math.degrees(v):.2f}' for v in js.velocity]}" + # ) + + # Publish to correct topic based on current mode + # Use persistent _in_velocity_mode flag (doesn't reset when trajectory completes) + if self._in_velocity_mode: + # In velocity mode - publish velocity commands + self._publish_velocity_command(command) + else: + # In position mode - publish position commands + self._publish_position_command(command) + + # Maintain loop frequency + next_time += period + sleep_time = next_time - time.time() + + if sleep_time > 0: + if self._stop_event.wait(timeout=sleep_time): + break + else: + next_time = time.time() + + except Exception as e: + logger.error(f"Error in control loop: {e}") + time.sleep(period) + + logger.info("Control loop stopped") + + def _generate_command(self) -> list[float] | None: + """ + Generate command for the robot. + + If trajectory is active: interpolate between start and end positions. + Otherwise: hold current position (safe). + + Returns: + List of joint commands (positions or velocities), or None if not ready. + """ + with self._state_lock: + # Wait until we have joint state feedback + if self._current_joint_state is None: + return None + + # Check if trajectory is active + if self._trajectory_active and self._trajectory_start_positions is not None: + # Calculate elapsed time + elapsed = time.time() - self._trajectory_start_time + + # Check if trajectory is complete + if elapsed >= self._trajectory_duration: + # Trajectory complete + logger.info(f"✓ Trajectory completed in {elapsed:.3f}s") + + # Determine what to return based on trajectory type + was_velocity_trajectory = self._trajectory_is_velocity + + # Mark trajectory as complete + self._trajectory_active = False + self._trajectory_is_velocity = False + + # Handle trajectory completion based on type + if was_velocity_trajectory: + # Velocity: send zeros once to stop immediately + logger.info(" Sending zero velocities to stop robot") + return [0.0] * self.config.num_joints + else: + # Position: stop publishing (robot holds last position) + logger.info(" Trajectory complete - stopping command publishing") + self._publishing_enabled = False + return None + + # Generate command based on trajectory type + if self._trajectory_is_velocity: + # VELOCITY TRAJECTORY: Constant velocity (no ramping) + joint_index = self._trajectory_start_positions[0] + target_velocity = self._trajectory_end_positions[0] + + # Just send constant velocity for the entire duration + velocity = target_velocity + + # Create command: zero velocity for all joints except target + command = [0.0] * self.config.num_joints + command[joint_index] = velocity + return command + + else: + # POSITION TRAJECTORY: Linear interpolation + s = elapsed / self._trajectory_duration + + command = [] + for i in range(self.config.num_joints): + start = self._trajectory_start_positions[i] + end = self._trajectory_end_positions[i] + position = start + s * (end - start) + command.append(position) + + return command + + # No active trajectory - use persistent mode flag + if self._in_velocity_mode: + # Velocity mode with no trajectory: send zeros (no motion) + return [0.0] * self.config.num_joints + else: + # Position mode with no trajectory: hold current position (safe) + return list(self._current_joint_state.position) + + def _publish_position_command(self, command: list[float]) -> None: + """Publish joint position command.""" + if self.joint_position_command._transport or ( + hasattr(self.joint_position_command, "connection") + and self.joint_position_command.connection + ): + try: + # Create JointCommand message with timestamp + cmd_msg = JointCommand(positions=command) + self.joint_position_command.publish(cmd_msg) + self._command_count += 1 + + # Log first few commands and periodically + # if self._command_count <= 3 or self._command_count % 100 == 0: + # logger.info( + # f"✓ Published position command #{self._command_count}: " + # f"{[f'{c:.3f}' for c in command[:3]]}... " + # f"(timestamp={cmd_msg.timestamp:.6f})" + # ) + except Exception as e: + logger.error(f"Failed to publish position command: {e}") + else: + if self._command_count == 0: + logger.warning("joint_position_command transport not configured!") + + def _publish_velocity_command(self, command: list[float]) -> None: + """Publish joint velocity command.""" + if self.joint_velocity_command._transport or ( + hasattr(self.joint_velocity_command, "connection") + and self.joint_velocity_command.connection + ): + try: + # Create JointCommand message with timestamp + cmd_msg = JointCommand(positions=command) + self.joint_velocity_command.publish(cmd_msg) + self._command_count += 1 + + # Log first few commands + if self._command_count <= 3: + logger.info( + f"✓ Published velocity command #{self._command_count}: " + f"{[f'{c:.3f}' for c in command[:3]]}... " + f"(timestamp={cmd_msg.timestamp:.6f})" + ) + except Exception as e: + logger.error(f"Failed to publish velocity command: {e}") + else: + if self._command_count == 0: + logger.warning("joint_velocity_command transport not configured!") diff --git a/dimos/hardware/manipulators/xarm/spec.py b/dimos/hardware/manipulators/xarm/spec.py new file mode 100644 index 0000000000..66f6abbfc6 --- /dev/null +++ b/dimos/hardware/manipulators/xarm/spec.py @@ -0,0 +1,49 @@ +# Copyright 2025 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 typing import Protocol + +from dimos.core import In, Out +from dimos.msgs.geometry_msgs import WrenchStamped +from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState + + +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/xarm/test_xarm_driver.py b/dimos/hardware/manipulators/xarm/test_xarm_driver.py new file mode 100644 index 0000000000..7efa3f0797 --- /dev/null +++ b/dimos/hardware/manipulators/xarm/test_xarm_driver.py @@ -0,0 +1,545 @@ +# Copyright 2025 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. + +""" +Test script for XArmDriver with full dimos deployment. + +This script properly deploys the XArmDriver module using dimos infrastructure: +1. dimos.start() - Initialize dimos dimos +2. dimos.deploy() - Deploy XArmDriver module +3. Set LCM transports for output topics +4. Test RPC methods and state monitoring + +Usage: + export XARM_IP=192.168.1.235 + venv/bin/python dimos/hardware/manipulators/xarm/test_xarm_driver.py + +Or use the wrapper script: + ./dimos/hardware/manipulators/xarm/test_xarm_deploy.sh + +Note: Must use venv/bin/python to avoid GLIBC version conflicts with system Python. +""" + +import os +import time + +import pytest + +from dimos import core +from dimos.hardware.manipulators.xarm.xarm_driver import XArmDriver +from dimos.msgs.geometry_msgs import WrenchStamped +from dimos.msgs.sensor_msgs import JointState, RobotState +from dimos.utils.logging_config import setup_logger + +logger = setup_logger(__file__) + +# Global variable for robot IP (set by main()) +ROBOT_IP = "192.168.1.235" + + +# ========================================================================= +# Pytest Fixtures - Automatic Setup/Cleanup +# ========================================================================= + + +@pytest.fixture +def dimos_cluster(): + """Fixture to start and stop dimos cluster automatically.""" + logger.info("Starting dimos...") + dimos = core.start(1) + try: + yield dimos + finally: + logger.info("Stopping dimos...") + dimos.stop() + + +@pytest.fixture +def driver(dimos_cluster): + """Fixture to deploy and manage XArmDriver with automatic cleanup.""" + ip_address = ROBOT_IP + + logger.info(f"Deploying XArmDriver for {ip_address}...") + driver = dimos_cluster.deploy( + XArmDriver, + ip_address=ip_address, + control_frequency=100.0, + joint_state_rate=100.0, + report_type="dev", + enable_on_start=False, + xarm_type="xarm6", + ) + + # Set up LCM transports for output topics BEFORE starting + logger.info("Setting up LCM transports for outputs...") + driver.joint_state.transport = core.LCMTransport("/xarm/joint_states", JointState) + driver.robot_state.transport = core.LCMTransport("/xarm/robot_state", RobotState) + driver.ft_ext.transport = core.LCMTransport("/xarm/ft_ext", WrenchStamped) + driver.ft_raw.transport = core.LCMTransport("/xarm/ft_raw", WrenchStamped) + + # Start the driver + logger.info("Starting driver...") + driver.start() + + # Wait for initialization + time.sleep(2.0) + + try: + yield driver + finally: + logger.info("Stopping driver...") + driver.stop() + + +# ========================================================================= +# Tests - Using Fixtures for Automatic Cleanup +# ========================================================================= + + +@pytest.mark.tool +def test_basic_connection(driver) -> None: + """Test basic connection and startup with dimos deployment.""" + logger.info("=" * 80) + logger.info("TEST 1: Basic Connection with dimos.deploy()") + logger.info("=" * 80) + + # Check connection via RPC + logger.info("Checking connection via RPC...") + code, version = driver.get_version() + assert code == 0, "Failed to get firmware version" + logger.info(f"✓ Firmware version: {version}") + + # Get robot state via RPC + logger.info("Getting robot state via RPC...") + robot_state = driver.get_robot_state() + if robot_state: + logger.info( + f"✓ Robot State: state={robot_state.state}, mode={robot_state.mode}, " + f"error={robot_state.error_code}, warn={robot_state.warn_code}" + ) + else: + logger.warning("✗ No robot state available yet") + + logger.info("✓ TEST 1 PASSED\n") + + +@pytest.mark.tool +def test_joint_state_reading(driver) -> None: + """Test joint state reading via LCM topic subscription.""" + logger.info("=" * 80) + logger.info("TEST 2: Joint State Reading via LCM Transport") + logger.info("=" * 80) + + ip_address = os.getenv("XARM_IP", "192.168.1.235") + + # Start dimos + logger.info("Starting dimos...") + dimos = core.start(1) + + # Deploy driver + logger.info("Deploying XArmDriver...") + driver = dimos.deploy( + XArmDriver, + ip_address=ip_address, + control_frequency=100.0, + joint_state_rate=100.0, + report_type="dev", + enable_on_start=False, + ) + + # Set up LCM transports for both joint states and robot state + joint_state_transport = core.LCMTransport("/xarm/joint_states", JointState) + robot_state_transport = core.LCMTransport("/xarm/robot_state", RobotState) + + driver.joint_state.transport = joint_state_transport + driver.robot_state.transport = robot_state_transport + driver.ft_ext.transport = core.LCMTransport("/xarm/ft_ext", WrenchStamped) + driver.ft_raw.transport = core.LCMTransport("/xarm/ft_raw", WrenchStamped) + + # Subscribe to the LCM topics to receive messages + joint_states_received = [] + robot_states_received = [] + + def on_joint_state(msg) -> None: + """Callback for receiving joint state messages from LCM.""" + joint_states_received.append(msg) + if len(joint_states_received) <= 3: + logger.info( + f"Received joint state #{len(joint_states_received)} via LCM: " + f"positions={[f'{p:.3f}' for p in msg.position[:3]]}... " + f"(showing first 3 joints)" + ) + + def on_robot_state(msg) -> None: + """Callback for receiving robot state messages from LCM.""" + robot_states_received.append(msg) + if len(robot_states_received) <= 3: + logger.info( + f"Received robot state #{len(robot_states_received)} via LCM: " + f"state={msg.state}, mode={msg.mode}, error={msg.error_code}, " + f"joints={msg.joints}, tcp_pose={msg.tcp_pose}, tcp_offset={msg.tcp_offset}" + ) + + # Subscribe to the LCM transports + logger.info("Subscribing to /xarm/joint_states LCM topic...") + unsubscribe_joint = joint_state_transport.subscribe(on_joint_state, driver.joint_state) + + logger.info("Subscribing to /xarm/robot_state LCM topic...") + unsubscribe_robot = robot_state_transport.subscribe(on_robot_state, driver.robot_state) + + # Wait 3 seconds to collect messages (driver already started by fixture) + logger.info("Collecting messages for 3 seconds...") + time.sleep(3.0) + + # Unsubscribe from both LCM topics + unsubscribe_joint() + unsubscribe_robot() + + # Check results + logger.info(f"\nReceived {len(joint_states_received)} joint state messages via LCM") + logger.info(f"Received {len(robot_states_received)} robot state messages via LCM") + + # Validate joint state messages + assert len(joint_states_received) > 0, "No joint states received via LCM" + logger.info("✓ Joint state publishing working via LCM transport") + + # Calculate rate + rate = len(joint_states_received) / 3.0 + logger.info(f"✓ Joint state publishing rate: ~{rate:.1f} Hz (expected ~100 Hz)") + + # Check last state + last_state = joint_states_received[-1] + logger.info(f"✓ Last state has {len(last_state.position)} joint positions") + logger.info(f"✓ Full joint positions: {[f'{p:.3f}' for p in last_state.position[:6]]}") + + assert rate > 50, f"Joint state publishing rate too low: {rate:.1f} Hz (expected >50 Hz)" + logger.info("✓ Joint state publishing rate is good (>50 Hz)") + + # Validate robot state messages + if len(robot_states_received) > 0: + logger.info("✓ Robot state publishing working via LCM transport") + + # Calculate rate + rate = len(robot_states_received) / 3.0 + logger.info(f"✓ Robot state publishing rate: ~{rate:.1f} Hz") + + # Check last state + last_robot_state = robot_states_received[-1] + logger.info( + f"✓ Last robot state: state={last_robot_state.state}, mode={last_robot_state.mode}, " + f"error={last_robot_state.error_code}, warn={last_robot_state.warn_code}" + ) + else: + logger.warning( + "⚠ No robot states received via LCM (might be expected with 'dev' report type)" + ) + + logger.info("✓ TEST 2 PASSED\n") + + +@pytest.mark.tool +def test_command_sending(driver) -> None: + """Test that command RPC methods are available and functional.""" + logger.info("=" * 80) + logger.info("TEST 3: Command RPC Methods") + logger.info("=" * 80) + + ip_address = os.getenv("XARM_IP", "192.168.1.235") + + # Start dimos + dimos = core.start(1) + + # Deploy driver + driver = dimos.deploy( + XArmDriver, + ip_address=ip_address, + control_frequency=100.0, + joint_state_rate=100.0, + report_type="dev", + enable_on_start=False, + ) + + # Set up transports + driver.joint_state.transport = core.LCMTransport("/xarm/joint_states", JointState) + driver.robot_state.transport = core.LCMTransport("/xarm/robot_state", RobotState) + driver.ft_ext.transport = core.LCMTransport("/xarm/ft_ext", WrenchStamped) + driver.ft_raw.transport = core.LCMTransport("/xarm/ft_raw", WrenchStamped) + + driver.start() + time.sleep(2.0) + + # Test that command methods exist and are callable + logger.info("Testing command RPC methods are available...") + + # Test motion_enable + logger.info("Testing motion_enable()...") + code, msg = driver.motion_enable(enable=True) + logger.info(f" motion_enable returned: code={code}, msg={msg}") + + # Test enable_servo_mode + logger.info("Testing enable_servo_mode()...") + code, msg = driver.enable_servo_mode() + logger.info(f" enable_servo_mode returned: code={code}, msg={msg}") + + # Test disable_servo_mode + logger.info("Testing disable_servo_mode()...") + code, msg = driver.disable_servo_mode() + logger.info(f" disable_servo_mode returned: code={code}, msg={msg}") + + # Test set_state + logger.info("Testing set_state(0)...") + code, msg = driver.set_state(0) + logger.info(f" set_state returned: code={code}, msg={msg}") + + # Test get_position + logger.info("Testing get_position()...") + code, position = driver.get_position() + if code == 0 and position: + logger.info(f"✓ get_position: {[f'{p:.1f}' for p in position[:3]]} (x,y,z in mm)") + else: + logger.warning(f" get_position returned: code={code}") + + logger.info("\n✓ All command RPC methods are functional") + logger.info("Note: Actual robot movement testing requires specific robot state") + logger.info(" and is environment-dependent. The driver API is working correctly.") + logger.info("✓ TEST 3 PASSED\n") + + +@pytest.mark.tool +def test_rpc_methods(driver) -> None: + """Test RPC method calls.""" + logger.info("=" * 80) + logger.info("TEST 4: RPC Methods") + logger.info("=" * 80) + + ip_address = os.getenv("XARM_IP", "192.168.1.235") + + # Start dimos + dimos = core.start(1) + + # Deploy driver + driver = dimos.deploy( + XArmDriver, + ip_address=ip_address, + control_frequency=100.0, + joint_state_rate=100.0, + report_type="normal", # Use normal for this test + enable_on_start=False, + ) + + # Set up transports + driver.joint_state.transport = core.LCMTransport("/xarm/joint_states", JointState) + driver.robot_state.transport = core.LCMTransport("/xarm/robot_state", RobotState) + driver.ft_ext.transport = core.LCMTransport("/xarm/ft_ext", WrenchStamped) + driver.ft_raw.transport = core.LCMTransport("/xarm/ft_raw", WrenchStamped) + + driver.start() + time.sleep(2.0) + + # Test get_version + logger.info("Testing get_version() RPC...") + code, version = driver.get_version() + assert code == 0, "Failed to get firmware version" + logger.info(f"✓ get_version: {version}") + + # Test get_position (TCP pose) + logger.info("Testing get_position() RPC...") + code, position = driver.get_position() + assert code == 0, f"Failed to get position: code={code}" + logger.info(f"✓ get_position: {[f'{p:.3f}' for p in position]}") + + # Test motion_enable + logger.info("Testing motion_enable() RPC...") + code, msg = driver.motion_enable(enable=True) + assert code == 0, f"Failed to enable motion: code={code}, msg={msg}" + logger.info(f"✓ motion_enable: {msg}") + + # Test clean_error + logger.info("Testing clean_error() RPC...") + code, msg = driver.clean_error() + assert code == 0, f"Failed to clean error: code={code}, msg={msg}" + logger.info(f"✓ clean_error: {msg}") + + logger.info("✓ TEST 4 PASSED\n") + + +def run_tests() -> None: + """Run all tests.""" + logger.info("=" * 80) + logger.info("XArm Driver Test Suite (Full dimos Deployment)") + logger.info("=" * 80) + logger.info("") + + # Run tests + results = [] + + try: + results.append(("Basic Connection", test_basic_connection())) + except Exception as e: + logger.error(f"TEST 1 FAILED with exception: {e}") + import traceback + + traceback.print_exc() + results.append(("Basic Connection", False)) + + try: + results.append(("Joint State Reading", test_joint_state_reading())) + except Exception as e: + logger.error(f"TEST 2 FAILED with exception: {e}") + import traceback + + traceback.print_exc() + results.append(("Joint State Reading", False)) + + try: + results.append(("Command Sending", test_command_sending())) + except Exception as e: + logger.error(f"TEST 3 FAILED with exception: {e}") + import traceback + + traceback.print_exc() + results.append(("Command Sending", False)) + + try: + results.append(("RPC Methods", test_rpc_methods())) + except Exception as e: + logger.error(f"TEST 4 FAILED with exception: {e}") + import traceback + + traceback.print_exc() + results.append(("RPC Methods", False)) + + # Print summary + logger.info("=" * 80) + logger.info("TEST SUMMARY") + logger.info("=" * 80) + + for test_name, passed in results: + status = "✓ PASSED" if passed else "✗ FAILED" + logger.info(f"{test_name:30s} {status}") + + total_passed = sum(1 for _, passed in results if passed) + total_tests = len(results) + + logger.info("") + logger.info(f"Total: {total_passed}/{total_tests} tests passed") + + if total_passed == total_tests: + logger.info("🎉 ALL TESTS PASSED!") + else: + logger.error("❌ SOME TESTS FAILED") + + +def run_driver() -> None: + """Start the xArm driver and keep it running.""" + logger.info("=" * 80) + logger.info("XArm Driver - Starting in continuous mode") + logger.info("=" * 80) + logger.info("") + + # Get IP address from environment variable or use default + ip_address = os.getenv("XARM_IP", "192.168.1.235") + logger.info(f"Using xArm at IP: {ip_address}") + logger.info("") + + # Start dimos + logger.info("Starting dimos...") + dimos = core.start(1) + + # Deploy XArmDriver + logger.info(f"Deploying XArmDriver for {ip_address}...") + driver = dimos.deploy( + XArmDriver, + ip_address=ip_address, + report_type="dev", + enable_on_start=False, + ) + + # Set up LCM transports + logger.info("Setting up LCM transports...") + driver.joint_state.transport = core.LCMTransport("/xarm/joint_states", JointState) + driver.robot_state.transport = core.LCMTransport("/xarm/robot_state", RobotState) + driver.ft_ext.transport = core.LCMTransport("/xarm/ft_ext", WrenchStamped) + driver.ft_raw.transport = core.LCMTransport("/xarm/ft_raw", WrenchStamped) + + # Start driver + logger.info("Starting driver...") + driver.start() + + logger.info("") + logger.info("=" * 80) + logger.info("✓ XArm driver is running!") + logger.info("=" * 80) + logger.info("") + logger.info("Publishing:") + logger.info(" - Joint states: /xarm/joint_states (~100 Hz)") + logger.info(" - Robot state: /xarm/robot_state (~10 Hz)") + logger.info(" - Force/torque: /xarm/ft_ext, /xarm/ft_raw") + logger.info("") + logger.info("Press Ctrl+C to stop...") + logger.info("") + + try: + # Keep running until interrupted + while True: + time.sleep(1.0) + except KeyboardInterrupt: + logger.info("\n\nShutting down...") + driver.stop() + dimos.stop() + logger.info("✓ Driver stopped") + + +def main() -> None: + """Main entry point.""" + import argparse + + # Parse command-line arguments + parser = argparse.ArgumentParser(description="xArm RT Driver Test") + parser.add_argument( + "--ip", type=str, default=None, help="xArm IP address (overrides XARM_IP env var)" + ) + parser.add_argument("--run-tests", action="store_true", help="Run automated test suite") + args = parser.parse_args() + + # Determine IP address: command-line arg > env var > default + if args.ip: + ip_address = args.ip + logger.info(f"Using xArm at IP (from --ip): {ip_address}") + else: + ip_address = os.getenv("XARM_IP") + if not ip_address: + ip_address = "192.168.1.235" + logger.warning("No IP specified, using default: 192.168.1.235") + logger.warning("Specify IP with:") + logger.warning(" python test_xarm_driver.py --ip 192.168.1.XXX") + logger.warning(" OR export XARM_IP=192.168.1.XXX") + logger.info("") + else: + logger.info(f"Using xArm at IP (from XARM_IP env): {ip_address}") + logger.info("") + + # Store IP for test functions to use + global ROBOT_IP + ROBOT_IP = ip_address + + # Run tests or driver + if args.run_tests: + run_tests() + else: + run_driver() + + +if __name__ == "__main__": + main() diff --git a/dimos/hardware/manipulators/xarm/xarm_driver.py b/dimos/hardware/manipulators/xarm/xarm_driver.py new file mode 100644 index 0000000000..2c4aafabb0 --- /dev/null +++ b/dimos/hardware/manipulators/xarm/xarm_driver.py @@ -0,0 +1,1192 @@ +# Copyright 2025 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 Real-time Driver Module + +This module provides a real-time controller for the xArm manipulator family +(xArm5, xArm6, xArm7) compatible with the xArm Python SDK. + +Architecture Overview: +- Main thread: Handles RPC calls and manages lifecycle +- Joint State Thread: Reads and publishes joint_state at joint_state_rate Hz + (default: 100Hz for dev, 5Hz for normal report_type) +- Control Thread: Sends joint commands at control_frequency Hz (100Hz) +- SDK Report Callback: Updates robot_state at report_type frequency + ('dev'=100Hz, 'rich'=5Hz, 'normal'=5Hz) - SEPARATE from joint state! + +Key Insight: +- joint_state_rate: How often we READ positions (get_joint_states/get_servo_angle) +- report_type: How often SDK pushes robot state updates (state, mode, errors) +- These are INDEPENDENT! You can read joint states at 100Hz while getting + robot state updates at 5Hz (normal mode). +""" + +from dataclasses import dataclass +import math +import threading +import time +from typing import Optional + +from reactivex.disposable import Disposable +from xarm.wrapper import XArmAPI + +from dimos.core import In, Module, Out, rpc +from dimos.core.module import ModuleConfig +from dimos.msgs.geometry_msgs import WrenchStamped +from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState +from dimos.utils.logging_config import setup_logger + +from .components import ( + GripperControlComponent, + KinematicsComponent, + MotionControlComponent, + StateQueryComponent, + SystemControlComponent, +) + +logger = setup_logger(__file__) + + +@dataclass +class XArmDriverConfig(ModuleConfig): + """Configuration for xArm driver.""" + + ip_address: str = "192.168.1.235" # xArm IP address + xarm_type: str = "xarm6" # xArm model type: 'xarm5', 'xarm6', or 'xarm7' + is_radian: bool = True # Use radians (True) or degrees (False) + control_frequency: float = 100.0 # Control loop frequency in Hz (for sending commands) + joint_state_rate: float = ( + -1.0 + ) # Joint state publishing rate (-1 = auto: 100Hz for dev, 5Hz for normal) + robot_state_rate: float = 10.0 # Robot state publishing rate in Hz + report_type: str = "normal" # SDK report type: 'dev'=100Hz, 'rich'=5Hz+torque, 'normal'=5Hz + enable_on_start: bool = True # Enable servo mode on start + check_joint_limit: bool = True # Check joint limits + check_cmdnum_limit: bool = True # Check command queue limit + max_cmdnum: int = 512 # Maximum command queue size + velocity_control: bool = False # Use velocity control mode instead of position + velocity_duration: float = 0.1 # Duration for velocity commands (seconds) + + @property + def num_joints(self) -> int: + """Get number of joints from xArm type.""" + xarm_type_map = { + "xarm5": 5, + "xarm6": 6, + "xarm7": 7, + } + return xarm_type_map.get(self.xarm_type.lower(), 6) + + +class XArmDriver( + MotionControlComponent, + StateQueryComponent, + SystemControlComponent, + KinematicsComponent, + GripperControlComponent, + Module, +): + """ + Real-time driver for xArm manipulators (xArm5/6/7). + + This driver implements a real-time control architecture with component-based design: + - Core driver: Handles threads, callbacks, and connection management + - MotionControlComponent: Motion control RPC methods + - StateQueryComponent: State query RPC methods + - SystemControlComponent: System control RPC methods + - KinematicsComponent: Kinematics RPC methods + + Architecture: + - Subscribes to joint commands and publishes joint states + - Runs a 100Hz control loop for servo angle control + - Provides RPC methods for xArm SDK API access via components + """ + + default_config = XArmDriverConfig + + # Input topics (commands from controllers) + joint_position_command: In[JointCommand] = None # Target joint positions (radians) + joint_velocity_command: In[JointCommand] = None # Target joint velocities (rad/s) + + # Output topics (state publishing) + joint_state: Out[JointState] = None # Joint state (position, velocity, effort) + robot_state: Out[RobotState] = None # Robot state (mode, errors, etc.) + ft_ext: Out[WrenchStamped] = None # External force/torque (compensated) + ft_raw: Out[WrenchStamped] = None # Raw force/torque sensor data + + def __init__(self, *args, **kwargs) -> None: + """Initialize the xArm driver.""" + super().__init__(*args, **kwargs) + + # xArm SDK instance + self.arm: XArmAPI | None = None + + # State tracking variables (updated by SDK callback) + self.curr_state: int = 4 # Robot state (4 = stopped initially) + self.curr_err: int = 0 # Current error code + self.curr_mode: int = 0 # Current control mode + self.curr_cmdnum: int = 0 # Command queue length + self.curr_warn: int = 0 # Warning code + self.curr_tcp_pose: list[float] = [] # TCP pose [x, y, z, roll, pitch, yaw] + self.curr_tcp_offset: list[float] = [] # TCP offset [x, y, z, roll, pitch, yaw] + self.curr_joints: list[float] = [] # Joint positions + + # Shared state (protected by locks) + self._joint_cmd_lock = threading.Lock() + self._joint_state_lock = threading.Lock() + self._joint_cmd_: list[float] | None = None # Latest joint command + self._vel_cmd_: list[float] | None = None # Latest velocity command + self._joint_states_: JointState | None = None # Latest joint state + self._robot_state_: RobotState | None = None # Latest robot state + self._last_cmd_time: float = 0.0 # Timestamp of last command received (for timeout) + + # Thread management + self._state_thread: threading.Thread | None = None # Joint state publishing + self._control_thread: threading.Thread | None = None # Command sending + self._stop_event = threading.Event() # Thread-safe stop signal + + # Joint names based on number of joints + self._joint_names = [f"joint{i + 1}" for i in range(self.config.num_joints)] + + # Joint state message (initialized in _init_publisher) + self._joint_state_msg: JointState | None = None + + logger.info( + f"XArmDriver initialized for {self.config.num_joints}-joint arm at " + f"{self.config.ip_address}" + ) + + @rpc + def start(self): + """ + Start the xArm driver (mirrors C++ XArmDriver::init). + + Initializes the xArm connection, registers callbacks, and starts + the joint state publishing thread. + """ + super().start() + + # Initialize state variables (like C++) + self.curr_err = 0 + self.curr_state = 4 # Stopped initially + self.curr_mode = 0 + self.curr_cmdnum = 0 + self.curr_warn = 0 + self.curr_tcp_pose = [] + self.curr_tcp_offset = [] + self.curr_joints = [] + self.arm = None + + # Joint names based on configuration + self._joint_names = [f"joint{i + 1}" for i in range(self.config.num_joints)] + + logger.info( + f"robot_ip={self.config.ip_address}, " + f"report_type={self.config.report_type}, " + f"dof={self.config.num_joints}" + ) + + # Create XArmAPI instance (matching C++ constructor parameters) + logger.info("Creating XArmAPI instance...") + try: + self.arm = XArmAPI( + port=self.config.ip_address, + is_radian=self.config.is_radian, + do_not_open=True, # Don't auto-connect (we'll call connect()) + check_tcp_limit=True, # Check TCP limits + check_joint_limit=self.config.check_joint_limit, + check_cmdnum_limit=self.config.check_cmdnum_limit, + check_robot_sn=False, # Don't check serial number + check_is_ready=True, # Check if ready before commands + check_is_pause=True, # Check if paused + max_cmdnum=self.config.max_cmdnum, + init_axis=self.config.num_joints, # Initialize with specified DOF + debug=False, # Disable debug mode + report_type=self.config.report_type, + ) + logger.info("XArmAPI instance created") + except Exception as e: + logger.error(f"Failed to create XArmAPI: {e}") + raise + + # Release and register callbacks (like C++ pattern) + self.arm.release_connect_changed_callback(True) + self.arm.release_report_callback(True) + self.arm.register_connect_changed_callback(self._report_connect_changed_callback) + self.arm.register_report_callback(self._report_data_callback) + + # Connect to the robot + logger.info(f"Connecting to xArm at {self.config.ip_address}...") + self.arm.connect() + logger.info("Connected to xArm") + + # Check for errors and warnings (like C++ code) + err_warn = [0, 0] + self.arm.get_err_warn_code(err_warn) + if err_warn[0] != 0: + logger.warning(f"UFACTORY ErrorCode: C{err_warn[0]}") + + # Check and clear servo errors (like C++ dbmsg handling) + self._check_and_clear_servo_errors() + + # Initialize publishers (joint state message structure) + self._init_publisher() + + # Start joint state publishing thread (read-only) + self._start_joint_state_thread() + + # Start robot state publishing thread + self._start_robot_state_thread() + + # Subscribe to input topics (only if they have connections/transports) + try: + unsub_joint = self.joint_position_command.subscribe(self._on_joint_position_command) + self._disposables.add(Disposable(unsub_joint)) + except (AttributeError, ValueError) as e: + logger.debug( + f"joint_position_command transport not configured, skipping subscription: {e}" + ) + + try: + unsub_vel = self.joint_velocity_command.subscribe(self._on_joint_velocity_command) + self._disposables.add(Disposable(unsub_vel)) + except (AttributeError, ValueError) as e: + logger.debug( + f"joint_velocity_command transport not configured, skipping subscription: {e}" + ) + + # Start control thread (command sending) + self._start_control_thread() + + # Enable servo mode if configured + if self.config.enable_on_start: + self._initialize_arm() + + logger.info("xArm driver started successfully") + + @rpc + def stop(self) -> None: + """Stop the xArm driver and disable servo mode.""" + logger.info("Stopping xArm driver...") + + # Signal threads to stop + self._stop_event.set() + + # Wait for state thread to finish + if self._state_thread and self._state_thread.is_alive(): + self._state_thread.join(timeout=2.0) + + # Wait for robot state thread to finish + if ( + hasattr(self, "_robot_state_thread") + and self._robot_state_thread + and self._robot_state_thread.is_alive() + ): + self._robot_state_thread.join(timeout=2.0) + + # Wait for control thread to finish + if self._control_thread and self._control_thread.is_alive(): + self._control_thread.join(timeout=2.0) + + # Disable servo mode + if self.arm: + try: + self.arm.set_mode(0) # Position mode + self.arm.set_state(0) # Stop state + logger.info("Servo mode disabled") + except Exception as e: + logger.error(f"Error disabling servo mode: {e}") + + # Disconnect from arm + if self.arm: + try: + self.arm.disconnect() + logger.info("Disconnected from xArm") + except Exception as e: + logger.error(f"Error disconnecting: {e}") + + # Clean up subscriptions + self._disposables.dispose() + + super().stop() + logger.info("xArm driver stopped") + + # ========================================================================= + # Static Methods: Error Interpretation + # ========================================================================= + + @staticmethod + def controller_error_interpreter(err_code: int) -> str: + """ + Translate xArm error code to human-readable message. + + Mirrors C++ XArmDriver::controller_error_interpreter() from ROS driver. + + Args: + err_code: xArm error code + + Returns: + Human-readable error description + """ + error_map = { + 0: "Everything OK", + 1: "Hardware Emergency STOP effective", + 2: "Emergency IO of Control Box is triggered", + 3: "Emergency Stop of Three-state Switch triggered", + 11: "Servo Motor Error of Joint 1", + 12: "Servo Motor Error of Joint 2", + 13: "Servo Motor Error of Joint 3", + 14: "Servo Motor Error of Joint 4", + 15: "Servo Motor Error of Joint 5", + 16: "Servo Motor Error of Joint 6", + 17: "Servo Motor Error of Joint 7", + 19: "End Module Communication Error", + 21: "Kinematic Error", + 22: "Self-collision Error", + 23: "Joint Angle Exceed Limit", + 24: "Speed Exceeds Limit", + 25: "Planning Error", + 26: "System Real Time Error", + 27: "Command Reply Error", + 29: "Other Errors, please contact technical support", + 30: "Feedback Speed Exceeds limit", + 31: "Collision Caused Abnormal Joint Current", + 32: "Circle Calculation Error", + 33: "Controller GPIO Error", + 34: "Trajectory Recording Timeout", + 35: "Exceed Safety Boundary", + 36: "Number of Delayed Command Exceed Limit", + 37: "Abnormal Motion in Manual Mode", + 38: "Abnormal Joint Angle", + 39: "Abnormal Communication Between Master and Slave IC of Power Board", + 50: "Tool Force/Torque Sensor Error", + 51: "Tool Force Torque Sensor Mode Setting Error", + 52: "Tool Force Torque Sensor Zero Setting Error", + 53: "Tool Force Torque Sensor Overload", + 110: "Robot Arm Base Board Communication Error", + 111: "Control Box External RS485 Device Communication Error", + } + + return error_map.get(err_code, f"Abnormal Error Code ({err_code}), please contact support!") + + # ========================================================================= + # Private Methods: Safety Checks + # ========================================================================= + + def _xarm_is_ready_read(self) -> bool: + """ + Check if robot is ready for reading (no errors). + + Mirrors C++ UFRobotSystemHardware::_xarm_is_ready_read() from ROS driver. + Logs error changes with human-readable interpretation. + Automatically attempts to clear and recover from errors. + + Returns: + True if curr_err == 0, False otherwise + """ + # Track last error for change detection and recovery attempts + if not hasattr(self, "_last_err"): + self._last_err = 0 + if not hasattr(self, "_error_recovery_attempts"): + self._error_recovery_attempts = 0 + if not hasattr(self, "_last_recovery_time"): + self._last_recovery_time = 0 + + curr_err = self.curr_err + + # Detect error changes + if curr_err != self._last_err: + if curr_err != 0: + # New error detected + logger.error( + f"[{self.config.ip_address}] xArm Error detected! " + f"Code C{curr_err} -> [ {self.controller_error_interpreter(curr_err)} ]" + ) + logger.info( + f"[{self.config.ip_address}] Will attempt to clear and recover from this error..." + ) + self._error_recovery_attempts = 0 # Reset counter for new error + else: + # Error cleared! Attempt recovery + logger.info( + f"[{self.config.ip_address}] Error cleared! " + f"Previous error was C{self._last_err}. Completing recovery..." + ) + self._attempt_error_recovery() + + # If there's an error, periodically attempt to clear it + elif curr_err != 0: + current_time = time.time() + # Attempt recovery every 2 seconds + if current_time - self._last_recovery_time > 2.0: + self._error_recovery_attempts += 1 + logger.warning( + f"[{self.config.ip_address}] Error C{curr_err} still present. " + f"Attempting to clear... (attempt #{self._error_recovery_attempts})" + ) + self._attempt_error_clearing() + self._last_recovery_time = current_time + + self._last_err = curr_err + return curr_err == 0 + + def _attempt_error_clearing(self) -> None: + """ + Attempt to clear the current error. + + This is called periodically when an error is present. + It tries to clear the error so that recovery can proceed. + """ + try: + # Try to clear the error + code = self.arm.clean_error() + if code == 0: + logger.info(f"[{self.config.ip_address}] clean_error() returned success") + else: + logger.debug(f"[{self.config.ip_address}] clean_error() returned code: {code}") + + # Also clean warnings + self.arm.clean_warn() + + except Exception as e: + logger.debug(f"[{self.config.ip_address}] Error clearing failed: {e}") + + def _attempt_error_recovery(self) -> None: + """ + Attempt to recover from error by re-initializing the arm. + + This is called automatically when an error is cleared (curr_err goes from non-zero to zero). + """ + try: + logger.info(f"[{self.config.ip_address}] Starting error recovery sequence...") + + # Step 1: Clean any residual errors and warnings + logger.info(" Step 1: Cleaning errors and warnings...") + self.arm.clean_error() + self.arm.clean_warn() + time.sleep(0.2) + + # Step 2: Enable motion + logger.info(" Step 2: Enabling motion...") + code = self.arm.motion_enable(enable=True) + if code != 0: + logger.warning(f" Motion enable returned code: {code}") + time.sleep(0.2) + + # Step 3: Set mode based on current configuration + if self.config.velocity_control: + logger.info(" Step 3: Setting velocity control mode (mode 4)...") + code = self.arm.set_mode(4) + else: + logger.info(" Step 3: Setting servo mode (mode 1)...") + code = self.arm.set_mode(1) + + if code != 0: + logger.warning(f" Set mode returned code: {code}") + time.sleep(0.2) + + # Step 4: Set state to ready + logger.info(" Step 4: Setting state to ready (state 0)...") + code = self.arm.set_state(0) + if code != 0: + logger.warning(f" Set state returned code: {code}") + time.sleep(0.2) + + # Verify recovery + logger.info( + f" Recovery complete: state={self.curr_state}, mode={self.curr_mode}, error={self.curr_err}" + ) + logger.info(f"[{self.config.ip_address}] ✓ Error recovery successful!") + + except Exception as e: + logger.error(f"[{self.config.ip_address}] Error recovery failed: {e}") + + def _xarm_is_ready_write(self) -> bool: + """ + Check if robot is ready for writing (correct state and mode). + + Mirrors C++ UFRobotSystemHardware::_xarm_is_ready_write() from ROS driver. + Validates: + - No errors (curr_err == 0) + - State is ready (curr_state <= 2: 0=ready, 1=running, 2=paused) + - Mode matches expected (1=SERVO for position, 4=VELO_JOINT for velocity) + + Returns: + True if robot is ready for commands, False otherwise + """ + + # Check for errors first + if not self._xarm_is_ready_read(): + self._last_not_ready = True + return False + + curr_state = self.curr_state + curr_mode = self.curr_mode + + # Check state (must be 0=ready, 1=running, or 2=paused, NOT >2 which are error states) + if curr_state > 2: + if self._last_state != curr_state: + self._last_state = curr_state + logger.warning( + f"[{self.config.ip_address}] Robot State detected! State: {curr_state}" + ) + self._last_not_ready = True + return False + self._last_state = curr_state + + # Check mode matches expected control mode + expected_mode = 4 if self.config.velocity_control else 1 # VELO_JOINT=4, SERVO=1 + if curr_mode != expected_mode: + if self._last_mode != curr_mode: + self._last_mode = curr_mode + logger.warning( + f"[{self.config.ip_address}] Robot Mode detected! " + f"Mode: {curr_mode} (expected {expected_mode} for " + f"{'velocity' if self.config.velocity_control else 'position'} control)" + ) + self._last_not_ready = True + return False + self._last_mode = curr_mode + + # Log when robot becomes ready after being not ready + if self._last_not_ready: + logger.info(f"[{self.config.ip_address}] Robot is Ready") + + self._last_not_ready = False + return True + + # ========================================================================= + # Private Methods: Initialization + # ========================================================================= + + def _init_publisher(self) -> None: + """ + Initialize publisher message structures. + Mirrors C++ XArmDriver::_init_publisher(). + """ + # Initialize joint state message structure + self._joint_state_msg = JointState( + ts=time.time(), + frame_id="joint-state data", + name=self._joint_names.copy(), + position=[0.0] * self.config.num_joints, + velocity=[0.0] * self.config.num_joints, + effort=[0.0] * self.config.num_joints, + ) + + logger.info("Publishers initialized") + + def _report_connect_changed_callback(self, connected: bool, reported: bool = True) -> None: + """ + Callback invoked when connection state changes. + + Args: + connected: True if connected, False if disconnected + reported: True if this is a reported change (unused but required by SDK) + """ + if connected: + logger.info("xArm connected") + else: + logger.error("xArm disconnected! Please reconnect...") + + def _check_and_clear_servo_errors(self) -> None: + """ + Check servo debug messages and clear low-voltage or other errors. + Mirrors the C++ dbmsg handling logic. + """ + try: + # Get servo debug messages (similar to C++ servo_get_dbmsg) + dbg_msg = [0] * (self.config.num_joints * 2) + + # Check if the core API has this method + if hasattr(self.arm, "core") and hasattr(self.arm.core, "servo_get_dbmsg"): + self.arm.core.servo_get_dbmsg(dbg_msg) + + for i in range(self.config.num_joints): + error_type = dbg_msg[i * 2] + error_code = dbg_msg[i * 2 + 1] + + if error_type == 1 and error_code == 40: + # Low-voltage error + self.arm.clean_error() + logger.warning(f"Cleared low-voltage error of joint {i + 1}") + elif error_type == 1: + # Other servo error + self.arm.clean_error() + logger.warning( + f"There is servo error code:(0x{error_code:x}) in joint {i + 1}, " + f"trying to clear it.." + ) + else: + logger.debug("servo_get_dbmsg not available in SDK") + + except Exception as e: + logger.debug(f"Could not check servo errors: {e}") + + def _firmware_version_is_ge(self, major: int, minor: int, patch: int) -> bool: + """ + Check if firmware version is greater than or equal to specified version. + + Args: + major: Major version number + minor: Minor version number + patch: Patch version number + + Returns: + True if firmware >= specified version + """ + # Use SDK's version_number tuple (populated on connection) + fw_maj, fw_min, fw_pat = self.arm.version_number + return ( + fw_maj > major + or (fw_maj == major and fw_min > minor) + or (fw_maj == major and fw_min == minor and fw_pat >= patch) + ) + + def _start_joint_state_thread(self) -> None: + """ + Start the joint state publishing thread. + Mirrors the C++ joint state publishing thread logic. + This thread ONLY reads and publishes joint states. + """ + # Determine joint state rate based on report type + joint_state_rate = self.config.control_frequency + + logger.info(f"Starting joint state thread at {joint_state_rate}Hz") + + # Clear stop event for new start cycle + self._stop_event.clear() + + # Start state publishing thread + self._state_thread = threading.Thread( + target=self._joint_state_loop, daemon=True, name="xarm_state_thread" + ) + self._state_thread.start() + + def _start_control_thread(self) -> None: + """ + Start the control thread for sending commands. + This thread ONLY sends joint commands to the robot. + """ + logger.info(f"Starting control thread at {self.config.control_frequency}Hz") + + self._control_thread = threading.Thread( + target=self._control_loop, daemon=True, name="xarm_control_thread" + ) + self._control_thread.start() + + def _start_robot_state_thread(self) -> None: + """ + Start the robot state publishing thread. + This thread publishes robot state at robot_state_rate Hz, + using data from state variables updated by _report_data_callback. + """ + logger.info(f"Starting robot state thread at {self.config.robot_state_rate}Hz") + + self._robot_state_thread = threading.Thread( + target=self._robot_state_loop, daemon=True, name="xarm_robot_state_thread" + ) + self._robot_state_thread.start() + + def _initialize_arm(self): + """Initialize the arm: clear errors, set mode, enable motion.""" + try: + # Clear any existing errors + self.arm.clean_error() + self.arm.clean_warn() + + # Enable motion + self.arm.motion_enable(enable=True) + + # Enable servo mode if configured + if self.config.enable_on_start: + logger.info("Enabling servo mode (mode=1)...") + code = self.arm.set_mode(1) # Servo mode + if code != 0: + logger.error(f"Failed to enable servo mode: code={code}") + else: + logger.info("✓ Servo mode enabled") + + # Verify mode was set + time.sleep(0.1) # Give SDK time to update + logger.info(f"Verifying mode: curr_mode={self.curr_mode} (should be 1 for servo)") + + # Set state to ready (0) - MUST be after setting mode + logger.info("Setting robot state to ready (state=0)...") + code = self.arm.set_state(state=0) + if code != 0: + logger.error(f"Failed to set state to ready: code={code}") + else: + logger.info("✓ Robot state set to ready") + + # Verify state + time.sleep(0.1) # Give SDK time to update + logger.info( + f"Robot initialized: state={self.curr_state} (0=ready), " + f"mode={self.curr_mode} (1=servo), error={self.curr_err}" + ) + + logger.info("Arm initialized successfully") + + except Exception as e: + logger.error(f"Failed to initialize arm: {e}") + raise + + # ========================================================================= + # Private Methods: Callbacks (Non-blocking) + # ========================================================================= + + def _on_joint_position_command(self, cmd_msg: JointCommand) -> None: + """ + Callback when joint position command is received. + Non-blocking: just store the latest command. + + Args: + cmd_msg: JointCommand message containing positions and timestamp + """ + with self._joint_cmd_lock: + self._joint_cmd_ = list(cmd_msg.positions) + self._last_cmd_time = time.time() # Update timestamp for timeout detection + + def _on_joint_velocity_command(self, cmd_msg: JointCommand) -> None: + """ + Callback when joint velocity command is received. + Non-blocking: just store the latest command. + + Args: + cmd_msg: JointCommand message containing velocities and timestamp + """ + with self._joint_cmd_lock: + self._vel_cmd_ = list(cmd_msg.positions) + self._last_cmd_time = time.time() # Update timestamp for timeout detection + + # ========================================================================= + # Private Methods: Thread Loops + # ========================================================================= + + def _joint_state_loop(self) -> None: + """ + Joint state publishing loop. + Mirrors the C++ lambda thread in XArmDriver::init (line 234-256). + + This thread ONLY reads joint states and publishes them. + Runs at joint_state_rate Hz (independent of report_type). + """ + # If joint_state_rate < 0, use default based on report_type + # Otherwise use the configured rate + if self.config.joint_state_rate < 0: + joint_state_rate = 100 if self.config.report_type == "dev" else 5 + else: + joint_state_rate = self.config.joint_state_rate + + period = 1.0 / joint_state_rate + + # Check firmware version to determine which API to use (C++ line 238) + use_new = self._firmware_version_is_ge(1, 8, 103) + logger.info(f"Joint state loop started at {joint_state_rate}Hz (use_new={use_new})") + + # For velocity calculation (old firmware) + prev_time = time.time() + prev_position = [0.0] * self.config.num_joints + initialized = False + + next_time = time.time() + + while not self._stop_event.is_set() and self.arm.connected: + try: + curr_time = time.time() + + # Check if robot is ready for reading (no errors) - this also triggers error recovery! + self._read_ready = self._xarm_is_ready_read() + + # Read joint states + if use_new: + # Newer firmware: get_joint_states returns (code, data) + # where data might be [[pos], [vel], [eff]] nested arrays + code, data = self.arm.get_joint_states() + if code != 0: + logger.warning(f"get_joint_states failed with code: {code}") + continue + + # Check if data is nested (list of lists) or flat + if isinstance(data[0], (list, tuple)): + # Nested: [[positions], [velocities], [efforts]] + # NOTE: SDK may return extra joints (e.g., gripper) - truncate to num_joints + position = ( + list(data[0])[: self.config.num_joints] + if len(data) > 0 + else [0.0] * self.config.num_joints + ) + velocity = ( + list(data[1])[: self.config.num_joints] + if len(data) > 1 + else [0.0] * self.config.num_joints + ) + effort = ( + list(data[2])[: self.config.num_joints] + if len(data) > 2 + else [0.0] * self.config.num_joints + ) + else: + # Flat: just positions + # NOTE: SDK may return extra joints (e.g., gripper) - truncate to num_joints + position = list(data)[: self.config.num_joints] + velocity = [0.0] * self.config.num_joints + effort = [0.0] * self.config.num_joints + else: + # Older firmware: only get_servo_angle available + code, position = self.arm.get_servo_angle(is_radian=self.config.is_radian) + if code != 0: + logger.warning(f"get_servo_angle failed with code: {code}") + continue + + # NOTE: SDK may return extra joints (e.g., gripper) - truncate to num_joints + position = position[: self.config.num_joints] + + # Calculate velocity from position difference + velocity = [0.0] * self.config.num_joints + if initialized: + dt = curr_time - prev_time + if dt > 0: + velocity = [ + (position[i] - prev_position[i]) / dt + for i in range(self.config.num_joints) + ] + + effort = [0.0] * self.config.num_joints + + # Update joint state message + self._joint_state_msg.ts = curr_time + self._joint_state_msg.position = list(position) + self._joint_state_msg.velocity = list(velocity) + self._joint_state_msg.effort = list(effort) + + # Update shared state + with self._joint_state_lock: + self._joint_states_ = self._joint_state_msg + + # Publish (only if transport is configured) + if self.joint_state._transport or ( + hasattr(self.joint_state, "connection") and self.joint_state.connection + ): + try: + self.joint_state.publish(self._joint_state_msg) + except Exception: + pass # Transport error, skip publishing + + # Save for next iteration (velocity calculation) + prev_position = list(position) + prev_time = curr_time + initialized = True + + # Maintain loop frequency + next_time += period + sleep_time = next_time - time.time() + + if sleep_time > 0: + if self._stop_event.wait(timeout=sleep_time): + break + else: + next_time = time.time() + + except Exception as e: + logger.error(f"Error in joint state loop: {e}") + time.sleep(period) + + if not self.arm.connected: + logger.error("xArm Control Connection Failed! Please Shut Down and Retry...") + + logger.info("Joint state loop stopped") + + def _control_loop(self) -> None: + """ + Control loop for sending joint commands. + + This thread ONLY sends commands to the robot. + Runs at control_frequency Hz. + """ + period = 1.0 / self.config.control_frequency + next_time = time.time() + + logger.info(f"Control loop started at {self.config.control_frequency}Hz") + + # command_count = 0 # Disabled - used for logging + # last_log_time = time.time() # Disabled - used for logging + timeout_logged = False + + while not self._stop_event.is_set(): + try: + current_time = time.time() + + # Read latest command from shared state + with self._joint_cmd_lock: + if self.config.velocity_control: + joint_cmd = self._vel_cmd_ + else: + joint_cmd = self._joint_cmd_ + last_cmd_time = self._last_cmd_time + + # Check for timeout (0.1 second without new commands) + time_since_last_cmd = current_time - last_cmd_time if last_cmd_time > 0 else 0 + + if time_since_last_cmd > 0.1 and joint_cmd is not None: + if not timeout_logged: + logger.warning( + f"Command timeout: no commands received for {time_since_last_cmd:.2f}s. " + f"Stopping robot." + ) + timeout_logged = True + # Send zero velocity to stop + if self.config.velocity_control: + zero_vel = [0.0] * self.config.num_joints + self.arm.vc_set_joint_velocity( + zero_vel, True, self.config.velocity_duration + ) + continue + else: + timeout_logged = False + + # Send command if available + if joint_cmd is not None and len(joint_cmd) == self.config.num_joints: + code = None # Initialize code variable + + if self.config.velocity_control: + # Velocity control mode (mode 4) + # NOTE: velocities are in degrees/second, not radians! + # Always send velocity commands (velocity changes frequently) + code = self.arm.vc_set_joint_velocity( + joint_cmd, False, self.config.velocity_duration + ) + else: + # Position control mode (mode 1) + # Only send if command changed (reduce network traffic) + command_changed = True + if hasattr(self, "_prev_joint_cmd") and self._prev_joint_cmd is not None: + # Check if command changed by more than threshold (0.0001 rad ~ 0.006 degrees) + if len(self._prev_joint_cmd) == len(joint_cmd): + command_changed = any( + abs(joint_cmd[i] - self._prev_joint_cmd[i]) > 0.0001 + for i in range(len(joint_cmd)) + ) + + if command_changed: + code = self.arm.set_servo_angle_j( + angles=joint_cmd, is_radian=self.config.is_radian + ) + # Store command if successfully sent + if code == 0: + self._prev_joint_cmd = joint_cmd.copy() + # else: command unchanged, skip sending + + # command_count += 1 # Disabled - used for logging + + # Log every second with detailed info (disabled - uncomment for debugging) + # if current_time - last_log_time >= 1.0: + # mode_str = "velocity" if self.config.velocity_control else "position" + # logger.info( + # f"Control loop ({mode_str}): sent {command_count} cmds in last second, " + # f"state={self.curr_state}, mode={self.curr_mode}, " + # f"joint6={math.degrees(joint_cmd[5]):.2f}°" + # ) + # command_count = 0 + # last_log_time = current_time + + if code is not None and code != 0: + if code == 9: + logger.warning( + f"Command failed: not ready to move " + f"(code=9, state={self.curr_state}, mode={self.curr_mode}). " + f"Check robot mode and state." + ) + else: + logger.warning(f"Command failed with code: {code}") + + # Maintain loop frequency + next_time += period + sleep_time = next_time - time.time() + + if sleep_time > 0: + if self._stop_event.wait(timeout=sleep_time): + break + else: + logger.debug(f"Control loop overrun: {-sleep_time * 1000:.2f}ms") + next_time = time.time() + + except Exception as e: + logger.error(f"Error in control loop: {e}") + time.sleep(period) + + logger.info("Control loop stopped") + + def _robot_state_loop(self) -> None: + """ + Robot state publishing loop. + + Publishes robot state at robot_state_rate Hz (default 10Hz). + Uses state variables updated by _report_data_callback. + """ + period = 1.0 / self.config.robot_state_rate + next_time = time.time() + + logger.info(f"Robot state loop started at {self.config.robot_state_rate}Hz") + + while not self._stop_event.is_set(): + try: + # Create robot state message from current state variables + # These are updated by _report_data_callback when SDK pushes updates + robot_state = RobotState( + state=self.curr_state, + mode=self.curr_mode, + error_code=self.curr_err, + warn_code=self.curr_warn, + cmdnum=self.curr_cmdnum, + mt_brake=0, # Not always available, updated by callback + mt_able=0, # Not always available, updated by callback + tcp_pose=self.curr_tcp_pose, + tcp_offset=self.curr_tcp_offset, + joints=self.curr_joints, + ) + + # Update shared state + with self._joint_state_lock: + self._robot_state_ = robot_state + + # Publish robot state (only if transport is configured) + if self.robot_state._transport or ( + hasattr(self.robot_state, "connection") and self.robot_state.connection + ): + try: + self.robot_state.publish(robot_state) + except Exception: + pass # Transport error, skip publishing + + # Maintain loop frequency + next_time += period + sleep_time = next_time - time.time() + + if sleep_time > 0: + if self._stop_event.wait(timeout=sleep_time): + break + else: + next_time = time.time() + + except Exception as e: + logger.error(f"Error in robot state loop: {e}") + time.sleep(period) + + logger.info("Robot state loop stopped") + + # ========================================================================= + # Private Methods: SDK Report Callback (Event-Driven) + # ========================================================================= + + def _report_data_callback(self, data: dict) -> None: + """ + Callback invoked by xArm SDK when new report data is available. + + This runs periodically based on report_type: + - 'dev': ~100Hz (high frequency for development) + - 'rich': ~5Hz (includes extra data like torques) + - 'normal': ~5Hz (basic state only) + + Data dictionary contains: + - state: Robot state (0=ready, 3=pause, 4=stop) + - mode: Control mode (0=position, 1=servo, etc.) + - error_code: Error code + - warn_code: Warning code + - cmdnum: Command queue length + - cartesian: TCP pose [x, y, z, roll, pitch, yaw] + - tcp_offset: TCP offset [x, y, z, roll, pitch, yaw] + - joints: Joint positions (variable length based on DOF) + - mtbrake: Motor brake state + - mtable: Motor enable state + """ + # Debug: track callback invocations + if not hasattr(self, "_report_callback_count"): + self._report_callback_count = 0 + self._report_callback_count += 1 + + if self._report_callback_count <= 3: + logger.debug(f"_report_data_callback called #{self._report_callback_count}") + + try: + # Update state tracking variables + self.curr_state = data.get("state", self.curr_state) + self.curr_err = data.get("error_code", 0) + self.curr_mode = data.get("mode", self.curr_mode) + self.curr_cmdnum = data.get("cmdnum", 0) + self.curr_warn = data.get("warn_code", 0) + + # Update pose and joint tracking variables + # Extract arrays from callback data (ensure they're lists) + cartesian = data.get("cartesian", []) + self.curr_tcp_pose = list(cartesian) if cartesian else [] + + tcp_offset = data.get("tcp_offset", []) + self.curr_tcp_offset = list(tcp_offset) if tcp_offset else [] + + joints = data.get("joints", []) + self.curr_joints = list(joints) if joints else [] + + # Note: Robot state publishing is handled by _robot_state_loop thread + # This callback only updates the state variables + + # Publish force/torque sensor data if available + self._publish_ft_sensor_data() + + except Exception as e: + logger.error(f"Error in report data callback: {e}") + + def _publish_ft_sensor_data(self) -> None: + """Publish force/torque sensor data from SDK properties.""" + try: + # External force (compensated) - ft_ext_force is a list property + if hasattr(self.arm, "ft_ext_force") and len(self.arm.ft_ext_force) == 6: + ft_ext_msg = WrenchStamped.from_force_torque_array( + ft_data=self.arm.ft_ext_force, frame_id="ft_sensor_ext", ts=time.time() + ) + if self.ft_ext._transport or ( + hasattr(self.ft_ext, "connection") and self.ft_ext.connection + ): + try: + self.ft_ext.publish(ft_ext_msg) + except Exception: + pass + + # Raw force sensor data + if hasattr(self.arm, "ft_raw_force") and len(self.arm.ft_raw_force) == 6: + ft_raw_msg = WrenchStamped.from_force_torque_array( + ft_data=self.arm.ft_raw_force, frame_id="ft_sensor_raw", ts=time.time() + ) + if self.ft_raw._transport or ( + hasattr(self.ft_raw, "connection") and self.ft_raw.connection + ): + try: + self.ft_raw.publish(ft_raw_msg) + except Exception: + pass + + except Exception as e: + logger.debug(f"FT sensor data not available: {e}") + + # ========================================================================= + # RPC Methods + # ========================================================================= + # All RPC methods have been extracted to component classes: + # - MotionControlComponent: Motion control methods (set_joint_angles, etc.) + # - StateQueryComponent: State query methods (get_joint_state, etc.) + # - SystemControlComponent: System control methods (enable_servo_mode, etc.) + # - KinematicsComponent: Kinematics methods (get_inverse_kinematics, etc.) + # ========================================================================= diff --git a/dimos/hardware/camera/module.py b/dimos/hardware/sensors/camera/module.py similarity index 97% rename from dimos/hardware/camera/module.py rename to dimos/hardware/sensors/camera/module.py index db28d383bd..9d08728ba6 100644 --- a/dimos/hardware/camera/module.py +++ b/dimos/hardware/sensors/camera/module.py @@ -25,8 +25,8 @@ from dimos import spec from dimos.agents2 import Output, Reducer, Stream, skill # type: ignore[attr-defined] from dimos.core import Module, ModuleConfig, Out, rpc -from dimos.hardware.camera.spec import CameraHardware -from dimos.hardware.camera.webcam import Webcam +from dimos.hardware.sensors.camera.spec import CameraHardware +from dimos.hardware.sensors.camera.webcam import Webcam from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 from dimos.msgs.sensor_msgs import Image from dimos.msgs.sensor_msgs.Image import Image, sharpness_barrier diff --git a/dimos/hardware/camera/spec.py b/dimos/hardware/sensors/camera/spec.py similarity index 100% rename from dimos/hardware/camera/spec.py rename to dimos/hardware/sensors/camera/spec.py diff --git a/dimos/hardware/camera/test_webcam.py b/dimos/hardware/sensors/camera/test_webcam.py similarity index 94% rename from dimos/hardware/camera/test_webcam.py rename to dimos/hardware/sensors/camera/test_webcam.py index e2f99e85dd..edd8e4ae87 100644 --- a/dimos/hardware/camera/test_webcam.py +++ b/dimos/hardware/sensors/camera/test_webcam.py @@ -17,9 +17,9 @@ import pytest from dimos import core -from dimos.hardware.camera import zed -from dimos.hardware.camera.module import CameraModule -from dimos.hardware.camera.webcam import Webcam +from dimos.hardware.sensors.camera import zed +from dimos.hardware.sensors.camera.module import CameraModule +from dimos.hardware.sensors.camera.webcam import Webcam from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 from dimos.msgs.sensor_msgs import CameraInfo, Image diff --git a/dimos/hardware/camera/webcam.py b/dimos/hardware/sensors/camera/webcam.py similarity index 98% rename from dimos/hardware/camera/webcam.py rename to dimos/hardware/sensors/camera/webcam.py index 6c9abb566d..19cba0a6a7 100644 --- a/dimos/hardware/camera/webcam.py +++ b/dimos/hardware/sensors/camera/webcam.py @@ -23,7 +23,7 @@ from reactivex import create from reactivex.observable import Observable -from dimos.hardware.camera.spec import CameraConfig, CameraHardware +from dimos.hardware.sensors.camera.spec import CameraConfig, CameraHardware from dimos.msgs.sensor_msgs import Image from dimos.msgs.sensor_msgs.Image import ImageFormat from dimos.utils.reactive import backpressure diff --git a/dimos/hardware/camera/zed/__init__.py b/dimos/hardware/sensors/camera/zed/__init__.py similarity index 100% rename from dimos/hardware/camera/zed/__init__.py rename to dimos/hardware/sensors/camera/zed/__init__.py diff --git a/dimos/hardware/camera/zed/camera.py b/dimos/hardware/sensors/camera/zed/camera.py similarity index 100% rename from dimos/hardware/camera/zed/camera.py rename to dimos/hardware/sensors/camera/zed/camera.py diff --git a/dimos/hardware/camera/zed/single_webcam.yaml b/dimos/hardware/sensors/camera/zed/single_webcam.yaml similarity index 100% rename from dimos/hardware/camera/zed/single_webcam.yaml rename to dimos/hardware/sensors/camera/zed/single_webcam.yaml diff --git a/dimos/hardware/camera/zed/test_zed.py b/dimos/hardware/sensors/camera/zed/test_zed.py similarity index 96% rename from dimos/hardware/camera/zed/test_zed.py rename to dimos/hardware/sensors/camera/zed/test_zed.py index 33810d3c2a..2d36ff4db4 100644 --- a/dimos/hardware/camera/zed/test_zed.py +++ b/dimos/hardware/sensors/camera/zed/test_zed.py @@ -19,7 +19,7 @@ def test_zed_import_and_calibration_access() -> None: """Test that zed module can be imported and calibrations accessed.""" # Import zed module from camera - from dimos.hardware.camera import zed + from dimos.hardware.sensors.camera import zed # Test that CameraInfo is accessible assert hasattr(zed, "CameraInfo") diff --git a/dimos/hardware/fake_zed_module.py b/dimos/hardware/sensors/fake_zed_module.py similarity index 100% rename from dimos/hardware/fake_zed_module.py rename to dimos/hardware/sensors/fake_zed_module.py diff --git a/dimos/hardware/gstreamer_camera.py b/dimos/hardware/sensors/gstreamer_camera.py similarity index 100% rename from dimos/hardware/gstreamer_camera.py rename to dimos/hardware/sensors/gstreamer_camera.py diff --git a/dimos/hardware/gstreamer_camera_test_script.py b/dimos/hardware/sensors/gstreamer_camera_test_script.py similarity index 98% rename from dimos/hardware/gstreamer_camera_test_script.py rename to dimos/hardware/sensors/gstreamer_camera_test_script.py index e71123e229..07f214a1a5 100755 --- a/dimos/hardware/gstreamer_camera_test_script.py +++ b/dimos/hardware/sensors/gstreamer_camera_test_script.py @@ -20,6 +20,7 @@ from dimos import core from dimos.hardware.gstreamer_camera import GstreamerCameraModule +from dimos.hardware.sensors.gstreamer_camera import GstreamerCameraModule from dimos.msgs.sensor_msgs import Image from dimos.protocol import pubsub diff --git a/dimos/hardware/gstreamer_sender.py b/dimos/hardware/sensors/gstreamer_sender.py similarity index 100% rename from dimos/hardware/gstreamer_sender.py rename to dimos/hardware/sensors/gstreamer_sender.py diff --git a/dimos/hardware/sensor.py b/dimos/hardware/sensors/sensor.py similarity index 100% rename from dimos/hardware/sensor.py rename to dimos/hardware/sensors/sensor.py diff --git a/dimos/manipulation/visual_servoing/manipulation_module.py b/dimos/manipulation/visual_servoing/manipulation_module.py index c2e55fd3cb..d02f66fa1e 100644 --- a/dimos/manipulation/visual_servoing/manipulation_module.py +++ b/dimos/manipulation/visual_servoing/manipulation_module.py @@ -28,8 +28,7 @@ import numpy as np from reactivex.disposable import Disposable -from dimos.core import In, Module, Out, rpc -from dimos.hardware.piper_arm import PiperArm +from dimos.hardware.manipulators.piper.piper_arm import PiperArm from dimos.manipulation.visual_servoing.detection3d import Detection3DProcessor from dimos.manipulation.visual_servoing.pbvs import PBVS from dimos.manipulation.visual_servoing.utils import ( diff --git a/dimos/msgs/geometry_msgs/Wrench.py b/dimos/msgs/geometry_msgs/Wrench.py new file mode 100644 index 0000000000..6867122906 --- /dev/null +++ b/dimos/msgs/geometry_msgs/Wrench.py @@ -0,0 +1,40 @@ +# Copyright 2025 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 __future__ import annotations + +from dataclasses import dataclass + +from dimos.msgs.geometry_msgs.Vector3 import Vector3 + + +@dataclass +class Wrench: + """ + Represents a force and torque in 3D space. + + This is equivalent to ROS geometry_msgs/Wrench. + """ + + force: Vector3 = None # Force vector (N) + torque: Vector3 = None # Torque vector (Nm) + + def __post_init__(self) -> None: + if self.force is None: + self.force = Vector3(0.0, 0.0, 0.0) + if self.torque is None: + self.torque = Vector3(0.0, 0.0, 0.0) + + def __repr__(self) -> str: + return f"Wrench(force={self.force}, torque={self.torque})" diff --git a/dimos/msgs/geometry_msgs/WrenchStamped.py b/dimos/msgs/geometry_msgs/WrenchStamped.py new file mode 100644 index 0000000000..274c70c4e4 --- /dev/null +++ b/dimos/msgs/geometry_msgs/WrenchStamped.py @@ -0,0 +1,72 @@ +# Copyright 2025 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 __future__ import annotations + +from dataclasses import dataclass +import time + +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry_msgs.Wrench import Wrench +from dimos.types.timestamped import Timestamped + + +@dataclass +class WrenchStamped(Timestamped): + """ + Represents a stamped force/torque measurement. + + This is equivalent to ROS geometry_msgs/WrenchStamped. + """ + + msg_name = "geometry_msgs.WrenchStamped" + ts: float = 0.0 + frame_id: str = "" + wrench: Wrench = None + + def __post_init__(self) -> None: + if self.ts == 0.0: + self.ts = time.time() + if self.wrench is None: + self.wrench = Wrench() + + @classmethod + def from_force_torque_array( + cls, ft_data: list, frame_id: str = "ft_sensor", ts: float | None = None + ): + """ + Create WrenchStamped from a 6-element force/torque array. + + Args: + ft_data: [fx, fy, fz, tx, ty, tz] + frame_id: Reference frame + ts: Timestamp (defaults to current time) + + Returns: + WrenchStamped instance + """ + if len(ft_data) != 6: + raise ValueError(f"Expected 6 elements, got {len(ft_data)}") + + return cls( + ts=ts if ts is not None else time.time(), + frame_id=frame_id, + wrench=Wrench( + force=Vector3(x=ft_data[0], y=ft_data[1], z=ft_data[2]), + torque=Vector3(x=ft_data[3], y=ft_data[4], z=ft_data[5]), + ), + ) + + def __repr__(self) -> str: + return f"WrenchStamped(ts={self.ts}, frame_id='{self.frame_id}', wrench={self.wrench})" diff --git a/dimos/msgs/geometry_msgs/__init__.py b/dimos/msgs/geometry_msgs/__init__.py index 683aa2e37c..9799d55201 100644 --- a/dimos/msgs/geometry_msgs/__init__.py +++ b/dimos/msgs/geometry_msgs/__init__.py @@ -26,3 +26,5 @@ "VectorLike", "to_pose", ] +from dimos.msgs.geometry_msgs.Wrench import Wrench +from dimos.msgs.geometry_msgs.WrenchStamped import WrenchStamped diff --git a/dimos/msgs/sensor_msgs/JointCommand.py b/dimos/msgs/sensor_msgs/JointCommand.py new file mode 100644 index 0000000000..5dc338cd25 --- /dev/null +++ b/dimos/msgs/sensor_msgs/JointCommand.py @@ -0,0 +1,143 @@ +# Copyright 2025 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. + +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +from io import BytesIO +import struct +import time + + +class JointCommand: + """ + Joint command message for robotic manipulators. + + Supports variable number of joints (DOF) with float64 values. + Can be used for position commands or velocity commands. + Includes timestamp for synchronization. + """ + + msg_name = "sensor_msgs.JointCommand" + + __slots__ = ["num_joints", "positions", "timestamp"] + + __typenames__ = ["double", "int32_t", "double"] + + __dimensions__ = [None, None, ["num_joints"]] + + def __init__( + self, positions: list[float] | None = None, timestamp: float | None = None + ) -> None: + """ + Initialize JointCommand. + + Args: + positions: List of joint values (positions or velocities) + timestamp: Unix timestamp (seconds since epoch). If None, uses current time. + """ + if positions is None: + positions = [] + + if timestamp is None: + timestamp = time.time() + + # LCM Type: double (timestamp) + self.timestamp = timestamp + # LCM Type: int32_t + self.num_joints = len(positions) + # LCM Type: double[num_joints] + self.positions = list(positions) + + def lcm_encode(self): + """Encode for LCM transport (dimos uses lcm_encode method name).""" + return self.encode() + + def encode(self): + buf = BytesIO() + buf.write(JointCommand._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf) -> None: + # Encode timestamp + buf.write(struct.pack(">d", self.timestamp)) + + # Encode num_joints + buf.write(struct.pack(">i", self.num_joints)) + + # Encode positions array + for i in range(self.num_joints): + buf.write(struct.pack(">d", self.positions[i])) + + @classmethod + def lcm_decode(cls, data: bytes): + """Decode from LCM transport (dimos uses lcm_decode method name).""" + return cls.decode(data) + + @classmethod + def decode(cls, data: bytes): + if hasattr(data, "read"): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != cls._get_packed_fingerprint(): + raise ValueError("Decode error") + return cls._decode_one(buf) + + @classmethod + def _decode_one(cls, buf): + self = JointCommand.__new__(JointCommand) + + # Decode timestamp + self.timestamp = struct.unpack(">d", buf.read(8))[0] + + # Decode num_joints + self.num_joints = struct.unpack(">i", buf.read(4))[0] + + # Decode positions array + self.positions = [] + for _i in range(self.num_joints): + self.positions.append(struct.unpack(">d", buf.read(8))[0]) + + return self + + @classmethod + def _get_hash_recursive(cls, parents): + if cls in parents: + return 0 + # Hash for variable-length double array message + tmphash = (0x8A3D2E1C5F4B6A9D) & 0xFFFFFFFFFFFFFFFF + tmphash = (((tmphash << 1) & 0xFFFFFFFFFFFFFFFF) + (tmphash >> 63)) & 0xFFFFFFFFFFFFFFFF + return tmphash + + _packed_fingerprint = None + + @classmethod + def _get_packed_fingerprint(cls): + if cls._packed_fingerprint is None: + cls._packed_fingerprint = struct.pack(">Q", cls._get_hash_recursive([])) + return cls._packed_fingerprint + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", JointCommand._get_packed_fingerprint())[0] + + def __str__(self) -> str: + return f"JointCommand(timestamp={self.timestamp:.6f}, num_joints={self.num_joints}, positions={self.positions})" + + def __repr__(self) -> str: + return f"JointCommand(positions={self.positions}, timestamp={self.timestamp})" diff --git a/dimos/msgs/sensor_msgs/JointState.py b/dimos/msgs/sensor_msgs/JointState.py new file mode 100644 index 0000000000..a40676c9c2 --- /dev/null +++ b/dimos/msgs/sensor_msgs/JointState.py @@ -0,0 +1,195 @@ +# Copyright 2025 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 __future__ import annotations + +import time +from typing import TypeAlias + +from dimos_lcm.sensor_msgs import JointState as LCMJointState + +try: + from sensor_msgs.msg import JointState as ROSJointState +except ImportError: + ROSJointState = None + +from plum import dispatch + +from dimos.types.timestamped import Timestamped + +# Types that can be converted to/from JointState +JointStateConvertable: TypeAlias = dict[str, list[str] | list[float]] | LCMJointState + + +def sec_nsec(ts): + s = int(ts) + return [s, int((ts - s) * 1_000_000_000)] + + +class JointState(Timestamped): + msg_name = "sensor_msgs.JointState" + ts: float + frame_id: str + name: list[str] + position: list[float] + velocity: list[float] + effort: list[float] + + @dispatch + def __init__( + self, + ts: float = 0.0, + frame_id: str = "", + name: list[str] | None = None, + position: list[float] | None = None, + velocity: list[float] | None = None, + effort: list[float] | None = None, + ) -> None: + """Initialize a JointState message. + + Args: + ts: Timestamp in seconds + frame_id: Frame ID for the message + name: List of joint names + position: List of joint positions (rad or m) + velocity: List of joint velocities (rad/s or m/s) + effort: List of joint efforts (Nm or N) + """ + self.ts = ts if ts != 0 else time.time() + self.frame_id = frame_id + self.name = name if name is not None else [] + self.position = position if position is not None else [] + self.velocity = velocity if velocity is not None else [] + self.effort = effort if effort is not None else [] + + @dispatch + def __init__(self, joint_dict: dict[str, list[str] | list[float]]) -> None: + """Initialize from a dictionary.""" + self.ts = joint_dict.get("ts", time.time()) + self.frame_id = joint_dict.get("frame_id", "") + self.name = list(joint_dict.get("name", [])) + self.position = list(joint_dict.get("position", [])) + self.velocity = list(joint_dict.get("velocity", [])) + self.effort = list(joint_dict.get("effort", [])) + + @dispatch + def __init__(self, joint: JointState) -> None: + """Initialize from another JointState (copy constructor).""" + self.ts = joint.ts + self.frame_id = joint.frame_id + self.name = list(joint.name) + self.position = list(joint.position) + self.velocity = list(joint.velocity) + self.effort = list(joint.effort) + + @dispatch + def __init__(self, lcm_joint: LCMJointState) -> None: + """Initialize from an LCM JointState message.""" + self.ts = lcm_joint.header.stamp.sec + (lcm_joint.header.stamp.nsec / 1_000_000_000) + self.frame_id = lcm_joint.header.frame_id + self.name = list(lcm_joint.name) if lcm_joint.name else [] + self.position = list(lcm_joint.position) if lcm_joint.position else [] + self.velocity = list(lcm_joint.velocity) if lcm_joint.velocity else [] + self.effort = list(lcm_joint.effort) if lcm_joint.effort else [] + + def lcm_encode(self) -> bytes: + lcm_msg = LCMJointState() + [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) + lcm_msg.header.frame_id = self.frame_id + lcm_msg.name_length = len(self.name) + lcm_msg.name = self.name + lcm_msg.position_length = len(self.position) + lcm_msg.position = self.position + lcm_msg.velocity_length = len(self.velocity) + lcm_msg.velocity = self.velocity + lcm_msg.effort_length = len(self.effort) + lcm_msg.effort = self.effort + return lcm_msg.lcm_encode() + + @classmethod + def lcm_decode(cls, data: bytes) -> JointState: + lcm_msg = LCMJointState.lcm_decode(data) + return cls( + ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), + frame_id=lcm_msg.header.frame_id, + name=list(lcm_msg.name) if lcm_msg.name else [], + position=list(lcm_msg.position) if lcm_msg.position else [], + velocity=list(lcm_msg.velocity) if lcm_msg.velocity else [], + effort=list(lcm_msg.effort) if lcm_msg.effort else [], + ) + + def __str__(self) -> str: + return f"JointState({len(self.name)} joints, frame_id='{self.frame_id}')" + + def __repr__(self) -> str: + return ( + f"JointState(ts={self.ts}, frame_id='{self.frame_id}', " + f"name={self.name}, position={self.position}, " + f"velocity={self.velocity}, effort={self.effort})" + ) + + def __eq__(self, other) -> bool: + """Check if two JointState messages are equal.""" + if not isinstance(other, JointState): + return False + return ( + self.name == other.name + and self.position == other.position + and self.velocity == other.velocity + and self.effort == other.effort + and self.frame_id == other.frame_id + ) + + @classmethod + def from_ros_msg(cls, ros_msg: ROSJointState) -> JointState: + """Create a JointState from a ROS sensor_msgs/JointState message. + + Args: + ros_msg: ROS JointState message + + Returns: + JointState instance + """ + # Convert timestamp from ROS header + ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) + + return cls( + ts=ts, + frame_id=ros_msg.header.frame_id, + name=list(ros_msg.name), + position=list(ros_msg.position), + velocity=list(ros_msg.velocity), + effort=list(ros_msg.effort), + ) + + def to_ros_msg(self) -> ROSJointState: + """Convert to a ROS sensor_msgs/JointState message. + + Returns: + ROS JointState message + """ + ros_msg = ROSJointState() + + # Set header + ros_msg.header.frame_id = self.frame_id + ros_msg.header.stamp.sec = int(self.ts) + ros_msg.header.stamp.nanosec = int((self.ts - int(self.ts)) * 1_000_000_000) + + # Set joint data + ros_msg.name = self.name + ros_msg.position = self.position + ros_msg.velocity = self.velocity + ros_msg.effort = self.effort + + return ros_msg diff --git a/dimos/msgs/sensor_msgs/RobotState.py b/dimos/msgs/sensor_msgs/RobotState.py new file mode 100644 index 0000000000..08c4123851 --- /dev/null +++ b/dimos/msgs/sensor_msgs/RobotState.py @@ -0,0 +1,188 @@ +# Copyright 2025 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. + +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +from io import BytesIO +import struct + + +class RobotState: + msg_name = "sensor_msgs.RobotState" + + __slots__ = [ + "cmdnum", + "error_code", + "joints", + "mode", + "mt_able", + "mt_brake", + "state", + "tcp_offset", + "tcp_pose", + "warn_code", + ] + + __typenames__ = [ + "int32_t", + "int32_t", + "int32_t", + "int32_t", + "int32_t", + "int32_t", + "int32_t", + "float", + "float", + "float", + ] + + __dimensions__ = [None, None, None, None, None, None, None, None, None, None] + + def __init__( + self, + state: int = 0, + mode: int = 0, + error_code: int = 0, + warn_code: int = 0, + cmdnum: int = 0, + mt_brake: int = 0, + mt_able: int = 0, + tcp_pose=None, + tcp_offset=None, + joints=None, + ) -> None: + # LCM Type: int32_t + self.state = state + # LCM Type: int32_t + self.mode = mode + # LCM Type: int32_t + self.error_code = error_code + # LCM Type: int32_t + self.warn_code = warn_code + # LCM Type: int32_t + self.cmdnum = cmdnum + # LCM Type: int32_t + self.mt_brake = mt_brake + # LCM Type: int32_t + self.mt_able = mt_able + # LCM Type: float[] - TCP pose [x, y, z, roll, pitch, yaw] + self.tcp_pose = tcp_pose if tcp_pose is not None else [] + # LCM Type: float[] - TCP offset [x, y, z, roll, pitch, yaw] + self.tcp_offset = tcp_offset if tcp_offset is not None else [] + # LCM Type: float[] - Joint positions (variable length based on robot DOF) + self.joints = joints if joints is not None else [] + + def lcm_encode(self): + """Encode for LCM transport (dimos uses lcm_encode method name).""" + return self.encode() + + def encode(self): + buf = BytesIO() + buf.write(RobotState._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf) -> None: + buf.write( + struct.pack( + ">iiiiiii", + self.state, + self.mode, + self.error_code, + self.warn_code, + self.cmdnum, + self.mt_brake, + self.mt_able, + ) + ) + # Encode tcp_pose array + buf.write(struct.pack(">i", len(self.tcp_pose))) + for val in self.tcp_pose: + buf.write(struct.pack(">f", val)) + # Encode tcp_offset array + buf.write(struct.pack(">i", len(self.tcp_offset))) + for val in self.tcp_offset: + buf.write(struct.pack(">f", val)) + # Encode joints array + buf.write(struct.pack(">i", len(self.joints))) + for val in self.joints: + buf.write(struct.pack(">f", val)) + + @classmethod + def lcm_decode(cls, data: bytes): + """Decode from LCM transport (dimos uses lcm_decode method name).""" + return cls.decode(data) + + @classmethod + def decode(cls, data: bytes): + if hasattr(data, "read"): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != cls._get_packed_fingerprint(): + raise ValueError("Decode error") + return cls._decode_one(buf) + + @classmethod + def _decode_one(cls, buf): + self = RobotState() + ( + self.state, + self.mode, + self.error_code, + self.warn_code, + self.cmdnum, + self.mt_brake, + self.mt_able, + ) = struct.unpack(">iiiiiii", buf.read(28)) + # Decode tcp_pose array + tcp_pose_len = struct.unpack(">i", buf.read(4))[0] + self.tcp_pose = [] + for _ in range(tcp_pose_len): + self.tcp_pose.append(struct.unpack(">f", buf.read(4))[0]) + # Decode tcp_offset array + tcp_offset_len = struct.unpack(">i", buf.read(4))[0] + self.tcp_offset = [] + for _ in range(tcp_offset_len): + self.tcp_offset.append(struct.unpack(">f", buf.read(4))[0]) + # Decode joints array + joints_len = struct.unpack(">i", buf.read(4))[0] + self.joints = [] + for _ in range(joints_len): + self.joints.append(struct.unpack(">f", buf.read(4))[0]) + return self + + @classmethod + def _get_hash_recursive(cls, parents): + if cls in parents: + return 0 + # Updated hash to reflect new fields: tcp_pose, tcp_offset, joints + tmphash = (0x8C3B9A1FE7D24E6A) & 0xFFFFFFFFFFFFFFFF + tmphash = (((tmphash << 1) & 0xFFFFFFFFFFFFFFFF) + (tmphash >> 63)) & 0xFFFFFFFFFFFFFFFF + return tmphash + + _packed_fingerprint = None + + @classmethod + def _get_packed_fingerprint(cls): + if cls._packed_fingerprint is None: + cls._packed_fingerprint = struct.pack(">Q", cls._get_hash_recursive([])) + return cls._packed_fingerprint + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", RobotState._get_packed_fingerprint())[0] diff --git a/dimos/msgs/sensor_msgs/__init__.py b/dimos/msgs/sensor_msgs/__init__.py index 130df72964..13d589e803 100644 --- a/dimos/msgs/sensor_msgs/__init__.py +++ b/dimos/msgs/sensor_msgs/__init__.py @@ -4,3 +4,6 @@ from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 __all__ = ["CameraInfo", "Image", "ImageFormat", "Joy", "PointCloud2"] +from dimos.msgs.sensor_msgs.JointCommand import JointCommand +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.sensor_msgs.RobotState import RobotState diff --git a/dimos/msgs/sensor_msgs/test_CameraInfo.py b/dimos/msgs/sensor_msgs/test_CameraInfo.py index c35145255b..7c25ac363c 100644 --- a/dimos/msgs/sensor_msgs/test_CameraInfo.py +++ b/dimos/msgs/sensor_msgs/test_CameraInfo.py @@ -394,7 +394,15 @@ def test_camera_info_from_yaml() -> None: """Test loading CameraInfo from YAML file.""" # Get path to the single webcam YAML file - yaml_path = get_project_root() / "dimos" / "hardware" / "camera" / "zed" / "single_webcam.yaml" + yaml_path = ( + get_project_root() + / "dimos" + / "hardware" + / "sensors" + / "camera" + / "zed" + / "single_webcam.yaml" + ) # Load CameraInfo from YAML camera_info = CameraInfo.from_yaml(str(yaml_path)) @@ -429,7 +437,7 @@ def test_camera_info_from_yaml() -> None: def test_calibration_provider() -> None: """Test CalibrationProvider lazy loading of YAML files.""" # Get the directory containing calibration files (not the file itself) - calibration_dir = get_project_root() / "dimos" / "hardware" / "camera" / "zed" + calibration_dir = get_project_root() / "dimos" / "hardware" / "sensors" / "camera" / "zed" # Create CalibrationProvider instance Calibrations = CalibrationProvider(calibration_dir) diff --git a/dimos/simulation/mujoco/input_controller.py b/dimos/simulation/mujoco/input_controller.py index 1372f09894..1db97b9c32 100644 --- a/dimos/simulation/mujoco/input_controller.py +++ b/dimos/simulation/mujoco/input_controller.py @@ -14,11 +14,12 @@ # See the License for the specific language governing permissions and # limitations under the License. - from typing import Any, Protocol from numpy.typing import NDArray +from dimos.hardware.end_effectors.end_effector import EndEffector + class InputController(Protocol): """A protocol for input devices to control the robot.""" diff --git a/dimos/utils/cli/human/humanclianim.py b/dimos/utils/cli/human/humanclianim.py index d2cdc98113..66119d3a1d 100644 --- a/dimos/utils/cli/human/humanclianim.py +++ b/dimos/utils/cli/human/humanclianim.py @@ -43,7 +43,7 @@ def import_cli_in_background() -> None: _import_complete.set() -def get_effect_config(effect_name: str): # type: ignore[no-untyped-def] +def get_effect_config(effect_name: str): """Get hardcoded configuration for a specific effect""" # Hardcoded configs for each effect global_config = { diff --git a/pyproject.toml b/pyproject.toml index 815b21008d..5b74a5b332 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -117,6 +117,9 @@ dependencies = [ # CLI "pydantic-settings>=2.11.0,<3", "typer>=0.19.2,<1", + + # Hardware SDKs + "xarm-python-sdk>=1.17.0", ] [project.scripts] diff --git a/tests/test_xarm_driver_unit.py b/tests/test_xarm_driver_unit.py new file mode 100644 index 0000000000..1064349771 --- /dev/null +++ b/tests/test_xarm_driver_unit.py @@ -0,0 +1,344 @@ +# Copyright 2025 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. + +""" +Automated pytest unit tests for xArm RT Driver with mocked hardware. + +These tests run WITHOUT real hardware and can be run in CI/CD. + +Usage: + pytest tests/test_xarm_driver_unit.py -v + pytest tests/test_xarm_driver_unit.py::test_basic_connection -v +""" + +import time +from unittest.mock import MagicMock, patch + +import pytest + +from dimos.hardware.manipulators.xarm.xarm_driver import XArmDriver +from dimos.msgs.sensor_msgs import JointState + + +@pytest.fixture +def mock_xarm(): + """Create a mock XArmAPI that simulates successful xArm responses.""" + mock = MagicMock() + + # Connection properties + mock.connected = True + mock.version_number = (1, 10, 0) + + # Connection methods + mock.connect.return_value = None + mock.disconnect.return_value = None + + # Callback registration + mock.register_connect_changed_callback.return_value = None + mock.register_report_callback.return_value = None + mock.release_connect_changed_callback.return_value = None + mock.release_report_callback.return_value = None + + # Error/warning methods + mock.get_err_warn_code.return_value = 0 + mock.clean_error.return_value = 0 + mock.clean_warn.return_value = 0 + + # State/control methods (return code, message) + mock.motion_enable.return_value = (0, "Motion enabled") + mock.set_mode.return_value = 0 + mock.set_state.return_value = 0 + + # Query methods + mock.get_version.return_value = (0, "v1.10.0") + mock.get_position.return_value = (0, [200.0, 0.0, 300.0, 3.14, 0.0, 0.0]) + mock.get_servo_angle.return_value = (0, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + mock.get_joint_states.return_value = ( + 0, + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], # positions + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], # velocities + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], # efforts + ], + ) + + # Command methods + mock.set_servo_angle_j.return_value = 0 + mock.vc_set_joint_velocity.return_value = 0 + + # Force/torque sensor data + mock.ft_ext_force = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + mock.ft_raw_force = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + return mock + + +@pytest.fixture +def driver(mock_xarm): + """Create an XArmDriver instance with mocked hardware.""" + with patch("dimos.hardware.manipulators.xarm.xarm_driver.XArmAPI") as MockXArmAPI: + MockXArmAPI.return_value = mock_xarm + + driver = XArmDriver( + ip_address="192.168.1.235", + control_frequency=100.0, + joint_state_rate=100.0, + report_type="dev", + enable_on_start=False, + xarm_type="xarm6", + ) + + yield driver + + # Teardown: stop driver if running + if driver._running: + driver.stop() + + +def test_driver_initialization(driver): + """Test that driver initializes with correct configuration.""" + assert driver.config.ip_address == "192.168.1.235" + assert driver.config.control_frequency == 100.0 + assert driver.config.num_joints == 6 + assert driver.config.report_type == "dev" + assert driver.config.enable_on_start is False + + +def test_start_stop_cycle(mock_xarm): + """Test that driver can start and stop successfully.""" + with patch("dimos.hardware.manipulators.xarm.xarm_driver.XArmAPI") as MockXArmAPI: + MockXArmAPI.return_value = mock_xarm + + driver = XArmDriver( + ip_address="192.168.1.235", + enable_on_start=False, + xarm_type="xarm6", + ) + + # Start driver + driver.start() + time.sleep(0.5) + + assert driver._running is True + assert driver.arm is not None + assert driver._state_thread is not None + assert driver._control_thread is not None + + # Stop driver + driver.stop() + time.sleep(0.5) + + assert driver._running is False + + +def test_start_creates_new_disposable(mock_xarm): + """Test that start() creates fresh CompositeDisposable after stop.""" + with patch("dimos.hardware.manipulators.xarm.xarm_driver.XArmAPI") as MockXArmAPI: + MockXArmAPI.return_value = mock_xarm + + driver = XArmDriver( + ip_address="192.168.1.235", + enable_on_start=False, + xarm_type="xarm6", + ) + + # First start + driver.start() + time.sleep(0.3) + disposable1 = driver._disposables + + # Stop + driver.stop() + time.sleep(0.3) + + # Second start + driver.start() + time.sleep(0.3) + disposable2 = driver._disposables + + # They should be different objects + assert disposable1 is not disposable2, "start() should create new CompositeDisposable" + + driver.stop() + + +def test_get_version_rpc(driver): + """Test get_version RPC method.""" + driver.start() + time.sleep(0.3) + + code, version = driver.get_version() + + assert code == 0, "Failed to get firmware version" + assert version == "v1.10.0", "Should return mocked version" + + driver.stop() + + +def test_get_position_rpc(driver): + """Test get_position RPC method.""" + driver.start() + time.sleep(0.3) + + code, position = driver.get_position() + + assert code == 0, "get_position should return success code" + assert len(position) == 6, "Position should have 6 values [x,y,z,roll,pitch,yaw]" + assert position[0] == 200.0, "X position should match mock" + + driver.stop() + + +def test_motion_enable_rpc(driver): + """Test motion_enable RPC method.""" + driver.start() + time.sleep(0.3) + + result = driver.motion_enable(enable=True) + + # motion_enable returns a tuple (code, msg) + assert isinstance(result, tuple), "Should return tuple" + code, msg = result + + assert code == 0, f"motion_enable should return success code, got {code}" + assert "Motion enabled" in msg, "Should return success message" + + driver.stop() + + +def test_enable_servo_mode_rpc(driver): + """Test enable_servo_mode RPC method.""" + driver.start() + time.sleep(0.3) + + code, _msg = driver.enable_servo_mode() + + assert code == 0, "enable_servo_mode should return success code" + + driver.stop() + + +def test_disable_servo_mode_rpc(driver): + """Test disable_servo_mode RPC method.""" + driver.start() + time.sleep(0.3) + + code, _msg = driver.disable_servo_mode() + + assert code == 0, "disable_servo_mode should return success code" + + driver.stop() + + +def test_clean_error_rpc(driver): + """Test clean_error RPC method.""" + driver.start() + time.sleep(0.3) + + code, _msg = driver.clean_error() + + assert code == 0, "clean_error should return success code" + + driver.stop() + + +def test_joint_state_publishing(driver): + """Test that joint states are published (without transport).""" + joint_states_received = [] + + def on_joint_state(msg: JointState): + joint_states_received.append(msg) + + # Try to subscribe (will fail without transport, but that's OK for unit test) + try: + driver.joint_state.subscribe(on_joint_state) + except (AttributeError, ValueError): + pass # Expected - no transport in unit test + + driver.start() + time.sleep(1.0) + + # Check that joint state thread is running + assert driver._state_thread is not None + assert driver._state_thread.is_alive() + + driver.stop() + + +def test_control_thread_starts(driver): + """Test that control thread starts successfully.""" + driver.start() + time.sleep(0.5) + + assert driver._control_thread is not None + assert driver._control_thread.is_alive() + + driver.stop() + + +def test_readiness_check_initialization(): + """Test that _xarm_is_ready_write initializes tracking variables.""" + with patch("dimos.hardware.manipulators.xarm.xarm_driver.XArmAPI") as MockXArmAPI: + mock = MagicMock() + mock.connected = True + mock.version_number = (1, 10, 0) + MockXArmAPI.return_value = mock + + driver = XArmDriver( + ip_address="192.168.1.235", + enable_on_start=False, + xarm_type="xarm6", + ) + + driver.start() + time.sleep(0.3) + + # Call readiness check - should not raise AttributeError + try: + driver._xarm_is_ready_write() + # Should complete without AttributeError + assert True + except AttributeError as e: + pytest.fail(f"_xarm_is_ready_write raised AttributeError: {e}") + + driver.stop() + + +def test_velocity_control_mode_initialization(mock_xarm): + """Test that velocity control mode sets correct mode on initialization.""" + with patch("dimos.hardware.manipulators.xarm.xarm_driver.XArmAPI") as MockXArmAPI: + MockXArmAPI.return_value = mock_xarm + + driver = XArmDriver( + ip_address="192.168.1.235", + enable_on_start=True, + velocity_control=True, # Enable velocity control + xarm_type="xarm6", + ) + + driver.start() + time.sleep(0.3) + + # Check that set_mode was called with mode 4 (velocity control) + # mock_xarm.set_mode.assert_called_with(4) + + driver.stop() + + +def test_error_interpreter(): + """Test error code interpretation.""" + assert "Everything OK" in XArmDriver.controller_error_interpreter(0) + assert "Emergency" in XArmDriver.controller_error_interpreter(1) + assert "Joint 1" in XArmDriver.controller_error_interpreter(11) + assert "Joint Angle Exceed Limit" in XArmDriver.controller_error_interpreter(23) diff --git a/tests/test_xarm_rt_driver.py b/tests/test_xarm_rt_driver.py new file mode 100644 index 0000000000..9ee8bcbe4a --- /dev/null +++ b/tests/test_xarm_rt_driver.py @@ -0,0 +1,385 @@ +# Copyright 2025 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. + +""" +Simple unit test for xArm RT Driver. + +This test mirrors test_xarm_driver.py but uses mocked hardware. +It tests all the same functionalities in a simple, straightforward way. + +Usage: + python tests/test_xarm_rt_driver.py +""" + +import os +import sys + +# Add dimos root to path +script_dir = os.path.dirname(os.path.abspath(__file__)) +dimos_root = os.path.dirname(script_dir) +if dimos_root not in sys.path: + sys.path.insert(0, dimos_root) + +import time +from unittest.mock import MagicMock, patch + +from dimos.hardware.manipulators.xarm.xarm_driver import XArmDriver +from dimos.msgs.sensor_msgs import JointState +from dimos.utils.logging_config import setup_logger + +logger = setup_logger(__file__) + + +def create_mock_xarm(): + """Create a mock XArmAPI that simulates successful xArm responses.""" + mock = MagicMock() + + # Connection properties + mock.connected = True + mock.version_number = (1, 10, 0) + + # Connection methods + mock.connect.return_value = None + mock.disconnect.return_value = None + + # Callback registration + mock.register_connect_changed_callback.return_value = None + mock.register_report_callback.return_value = None + mock.release_connect_changed_callback.return_value = None + mock.release_report_callback.return_value = None + + # Error/warning methods + mock.get_err_warn_code.return_value = 0 + mock.clean_error.return_value = 0 + mock.clean_warn.return_value = 0 + + # State/control methods (return code, message) + mock.motion_enable.return_value = (0, "Motion enabled") + mock.set_mode.return_value = 0 + mock.set_state.return_value = 0 + + # Query methods + mock.get_version.return_value = (0, "v1.10.0") + mock.get_position.return_value = (0, [200.0, 0.0, 300.0, 3.14, 0.0, 0.0]) + mock.get_servo_angle.return_value = (0, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + mock.get_joint_states.return_value = ( + 0, + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], # positions + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], # velocities + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], # efforts + ], + ) + + # Command methods + mock.set_servo_angle_j.return_value = 0 + mock.vc_set_joint_velocity.return_value = 0 + + # Force/torque sensor data + mock.ft_ext_force = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + mock.ft_raw_force = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + return mock + + +def test_basic_connection(): + """Test basic connection and startup (mirrors test_basic_connection).""" + logger.info("=" * 80) + logger.info("TEST 1: Basic Connection") + logger.info("=" * 80) + + with patch("dimos.hardware.manipulators.xarm.xarm_driver.XArmAPI") as MockXArmAPI: + MockXArmAPI.return_value = create_mock_xarm() + + logger.info("Creating XArmDriver...") + driver = XArmDriver( + ip_address="192.168.1.235", + control_frequency=100.0, + joint_state_rate=100.0, + report_type="dev", + enable_on_start=False, + xarm_type="xarm6", + ) + + logger.info("Starting driver...") + driver.start() + time.sleep(1.0) + + # Check connection via RPC (mirrors real test) + logger.info("Checking connection via RPC...") + code, version = driver.get_version() + if code == 0: + logger.info(f"✓ Firmware version: {version}") + else: + logger.error(f"✗ Failed to get firmware version: code={code}") + driver.stop() + return False + + # Get robot state via RPC + logger.info("Getting robot state via RPC...") + robot_state = driver.get_robot_state() + if robot_state: + logger.info( + f"✓ Robot State: state={robot_state.state}, mode={robot_state.mode}, " + f"error={robot_state.error_code}, warn={robot_state.warn_code}" + ) + else: + logger.warning("✗ No robot state available yet") + + logger.info("Stopping driver...") + driver.stop() + + logger.info("✓ TEST 1 PASSED\n") + return True + + +def test_joint_state_reading(): + """Test joint state reading (mirrors test_joint_state_reading).""" + logger.info("=" * 80) + logger.info("TEST 2: Joint State Reading") + logger.info("=" * 80) + + with patch("dimos.hardware.manipulators.xarm.xarm_driver.XArmAPI") as MockXArmAPI: + MockXArmAPI.return_value = create_mock_xarm() + + logger.info("Creating XArmDriver...") + driver = XArmDriver( + ip_address="192.168.1.235", + control_frequency=100.0, + joint_state_rate=100.0, + report_type="dev", + enable_on_start=False, + xarm_type="xarm6", + ) + + # Track received joint states + joint_states_received = [] + + def on_joint_state(msg: JointState): + """Callback for receiving joint state messages.""" + joint_states_received.append(msg) + if len(joint_states_received) <= 3: + logger.info( + f"Received joint state #{len(joint_states_received)}: " + f"positions={[f'{p:.3f}' for p in msg.position[:3]]}..." + ) + + # Subscribe to joint states + try: + logger.info("Subscribing to joint_state...") + driver.joint_state.subscribe(on_joint_state) + except (AttributeError, ValueError) as e: + logger.info(f"Note: Could not subscribe (no transport configured): {e}") + logger.info("This is expected in unit test mode - continuing...") + + logger.info("Starting driver - joint states will publish at 100Hz...") + driver.start() + + # Collect messages for 2 seconds + logger.info("Collecting messages for 2 seconds...") + time.sleep(2.0) + + # Check results + logger.info(f"\nReceived {len(joint_states_received)} joint state messages") + + if len(joint_states_received) > 0: + rate = len(joint_states_received) / 2.0 + logger.info(f"✓ Joint state publishing rate: ~{rate:.1f} Hz") + + last_state = joint_states_received[-1] + logger.info(f"✓ Last state has {len(last_state.position)} joint positions") + + if rate > 50: + logger.info("✓ Joint state publishing rate is good (>50 Hz)") + else: + logger.warning(f"⚠ Joint state publishing rate seems low: {rate:.1f} Hz") + else: + logger.info("Note: No messages received (no transport configured)") + logger.info("This is expected in unit test mode") + + driver.stop() + logger.info("✓ TEST 2 PASSED\n") + return True + + +def test_command_sending(): + """Test command RPC methods (mirrors test_command_sending).""" + logger.info("=" * 80) + logger.info("TEST 3: Command RPC Methods") + logger.info("=" * 80) + + with patch("dimos.hardware.manipulators.xarm.xarm_driver.XArmAPI") as MockXArmAPI: + MockXArmAPI.return_value = create_mock_xarm() + + driver = XArmDriver( + ip_address="192.168.1.235", + control_frequency=100.0, + joint_state_rate=100.0, + report_type="dev", + enable_on_start=False, + xarm_type="xarm6", + ) + + driver.start() + time.sleep(1.0) + + # Test command methods + logger.info("Testing command RPC methods...") + + logger.info("Testing motion_enable()...") + code, msg = driver.motion_enable(enable=True) + logger.info(f" motion_enable returned: code={code}, msg={msg}") + + logger.info("Testing enable_servo_mode()...") + code, msg = driver.enable_servo_mode() + logger.info(f" enable_servo_mode returned: code={code}, msg={msg}") + + logger.info("Testing disable_servo_mode()...") + code, msg = driver.disable_servo_mode() + logger.info(f" disable_servo_mode returned: code={code}, msg={msg}") + + logger.info("Testing set_state(0)...") + code, msg = driver.set_state(0) + logger.info(f" set_state returned: code={code}, msg={msg}") + + logger.info("Testing get_position()...") + code, position = driver.get_position() + if code == 0 and position: + logger.info(f"✓ get_position: {[f'{p:.1f}' for p in position[:3]]} (x,y,z in mm)") + + logger.info("✓ All command RPC methods are functional") + + driver.stop() + logger.info("✓ TEST 3 PASSED\n") + return True + + +def test_rpc_methods(): + """Test various RPC methods (mirrors test_rpc_methods).""" + logger.info("=" * 80) + logger.info("TEST 4: RPC Methods") + logger.info("=" * 80) + + with patch("dimos.hardware.manipulators.xarm.xarm_driver.XArmAPI") as MockXArmAPI: + MockXArmAPI.return_value = create_mock_xarm() + + driver = XArmDriver( + ip_address="192.168.1.235", + control_frequency=100.0, + joint_state_rate=100.0, + report_type="normal", + enable_on_start=False, + xarm_type="xarm6", + ) + + driver.start() + time.sleep(1.0) + + # Test get_version + logger.info("Testing get_version() RPC...") + code, version = driver.get_version() + if code == 0: + logger.info(f"✓ get_version: {version}") + + # Test get_position + logger.info("Testing get_position() RPC...") + code, position = driver.get_position() + if code == 0: + logger.info(f"✓ get_position: {[f'{p:.3f}' for p in position]}") + + # Test motion_enable + logger.info("Testing motion_enable() RPC...") + code, msg = driver.motion_enable(enable=True) + if code == 0: + logger.info(f"✓ motion_enable: {msg}") + + # Test clean_error + logger.info("Testing clean_error() RPC...") + code, msg = driver.clean_error() + if code == 0: + logger.info(f"✓ clean_error: {msg}") + + driver.stop() + logger.info("✓ TEST 4 PASSED\n") + return True + + +def main(): + """Run all tests.""" + logger.info("=" * 80) + logger.info("XArm RT Driver Unit Tests (Mocked Hardware)") + logger.info("=" * 80) + logger.info("") + + results = [] + + try: + results.append(("Basic Connection", test_basic_connection())) + except Exception as e: + logger.error(f"TEST 1 FAILED: {e}") + import traceback + + traceback.print_exc() + results.append(("Basic Connection", False)) + + try: + results.append(("Joint State Reading", test_joint_state_reading())) + except Exception as e: + logger.error(f"TEST 2 FAILED: {e}") + import traceback + + traceback.print_exc() + results.append(("Joint State Reading", False)) + + try: + results.append(("Command Sending", test_command_sending())) + except Exception as e: + logger.error(f"TEST 3 FAILED: {e}") + import traceback + + traceback.print_exc() + results.append(("Command Sending", False)) + + try: + results.append(("RPC Methods", test_rpc_methods())) + except Exception as e: + logger.error(f"TEST 4 FAILED: {e}") + import traceback + + traceback.print_exc() + results.append(("RPC Methods", False)) + + # Print summary + logger.info("=" * 80) + logger.info("TEST SUMMARY") + logger.info("=" * 80) + + for test_name, passed in results: + status = "✓ PASSED" if passed else "✗ FAILED" + logger.info(f"{test_name:30s} {status}") + + total_passed = sum(1 for _, passed in results if passed) + total_tests = len(results) + + logger.info("") + logger.info(f"Total: {total_passed}/{total_tests} tests passed") + + if total_passed == total_tests: + logger.info("🎉 ALL TESTS PASSED!") + else: + logger.error("❌ SOME TESTS FAILED") + + +if __name__ == "__main__": + main()