Merge pull request #348 from pollen-robotics/347-log-dynamixel-hardware-error

Monitor hardware errors.

Author: apirrone

pyproject.toml

@@ -12,7 +12,7 @@ requires-python = ">=3.10"
 dependencies = [
     "numpy>=2.2.5,<2.3.0",
     "scipy==1.15.3",
-    "reachy_mini_motor_controller>=1.0.0",    
+    "reachy_mini_motor_controller>=1.2.0",    
     "eclipse-zenoh>=1.4.0",
     "opencv-python<=5.0",
     "cv2_enumerate_cameras>=1.2.1",

src/reachy_mini/daemon/backend/robot/backend.py

@@ -30,6 +30,7 @@ def __init__(
         log_level: str = "INFO",
         check_collision: bool = False,
         kinematics_engine: str = "AnalyticalKinematics",
+        hardware_error_check_frequency: float = 1.0,
     ):
         """Initialize the RobotBackend.
 
@@ -38,6 +39,7 @@ def __init__(
             log_level (str): The logging level for the backend. Default is "INFO".
             check_collision (bool): If True, enable collision checking. Default is False.
             kinematics_engine (str): Kinematics engine to use. Defaults to "AnalyticalKinematics".
+            hardware_error_check_frequency (float): Frequency in seconds to check for hardware errors. Default is 1.0.
 
         Tries to connect to the Reachy Mini motor controller and initializes the control loop.
 
@@ -82,6 +84,8 @@ def __init__(
         self.target_antenna_joint_current = None  # Placeholder for antenna joint torque
         self.target_head_joint_current = None  # Placeholder for head joint torque
 
+        self.hardware_error_check_frequency = hardware_error_check_frequency  # seconds
+
     def run(self) -> None:
         """Run the control loop for the robot backend.
 
@@ -96,6 +100,8 @@ def run(self) -> None:
         self.retries = 5
         self.stats_record_t0 = time.time()
 
+        self.last_hardware_error_check_time = time.time()
+
         next_call_event = Event()
 
         # Compute the forward kinematics to get the initial head pose
@@ -240,6 +246,18 @@ def _update(self) -> None:
                 self._stats["nb_error"] = 0
                 self.stats_record_t0 = time.time()
 
+            if (
+                time.time() - self.last_hardware_error_check_time
+                > self.hardware_error_check_frequency
+            ):
+                hardware_errors = self.read_hardware_errors()
+                if hardware_errors:
+                    for motor_name, errors in hardware_errors.items():
+                        self.logger.error(
+                            f"Motor '{motor_name}' hardware errors: {errors}"
+                        )
+                self.last_hardware_error_check_time = time.time()
+
     def close(self) -> None:
         """Close the motor controller connection."""
         if self.c is not None:
@@ -478,6 +496,33 @@ def _infer_control_mode(self) -> MotorControlMode:
         else:
             raise ValueError(f"Unknown motor control mode: {mode}")
 
+    def read_hardware_errors(self) -> dict[str, list[str]]:
+        """Read hardware errors from the motor controller."""
+        if self.c is None:
+            return {}
+
+        def decode_hardware_error_byte(err_byte: int) -> list[str]:
+            # https://emanual.robotis.com/docs/en/dxl/x/xl330-m288/#hardware-error-status
+            bits_to_error = {
+                0: "Input Voltage Error",
+                2: "Overheating Error",
+                4: "Electrical Shock Error",
+                5: "Overload Error",
+            }
+            err_bits = [i for i in range(8) if (err_byte & (1 << i)) != 0]
+            return [bits_to_error[b] for b in err_bits if b in bits_to_error]
+
+        errors = {}
+        for name, id in self.c.get_motor_name_id().items():
+            # https://emanual.robotis.com/docs/en/dxl/x/xl330-m288/#hardware-error-status
+            err_byte = self.c.async_read_raw_bytes(id, 70, 1)
+            assert len(err_byte) == 1
+            err = decode_hardware_error_byte(err_byte[0])
+            if err:
+                errors[name] = err
+
+        return errors
+
 
 @dataclass
 class RobotBackendStatus: