Improved the Error Handling and Logging for Slate Base

* Handle slate base error. (Segmentation Fault)

* Added exception handling for Slate.

* Add Error State Logging

lerobot/common/robot_devices/motors/trossen_arm_driver.py

@@ -99,7 +99,7 @@ class TrossenArmDriver:
         self.TIME_SCALING_FACTOR = 3.0
 
         # Minimum time to move for the arm (This is a tuning parameter)
-        self.MIN_TIME_TO_MOVE = 6.0 / self.fps
+        self.MIN_TIME_TO_MOVE = 3.0 / self.fps
 
     def connect(self):
         print(f"Connecting to {self.model} arm at {self.ip}...")

lerobot/common/robot_devices/robots/trossen_ai_mobile.py

@@ -1,4 +1,5 @@
 import time
+from typing import Tuple
 from dataclasses import replace
 
 import numpy as np
@@ -17,9 +18,9 @@ from lerobot.common.robot_devices.robots.utils import get_arm_id
 from lerobot.common.robot_devices.utils import (
     RobotDeviceAlreadyConnectedError,
     RobotDeviceNotConnectedError,
+    SlateBaseSystemState,
 )
 
-
 class TrossenAIMobile():
 
     def __init__(self, config: TrossenAIMobileRobotConfig | None = None, **kwargs):
@@ -109,9 +110,26 @@ class TrossenAIMobile():
             raise RobotDeviceAlreadyConnectedError(
                 "TrossenAIMobile is already connected. Do not run `robot.connect()` twice."
             )
-        self.base.init_base()
+        success, result = self.base.init_base()
+        if not success:
+            raise RobotDeviceNotConnectedError(
+                f"{result}.\nMake sure the robot is powered on and connected to the computer.\n{self.check_base_state()[0]}"
+            )
+        try:
+            if self.check_base_state()[1]==SlateBaseSystemState.SYS_ESTOP:
+                raise RuntimeError(
+                    f"Robot is in emergency stop state. Please release the emergency stop button and try again"
+                )
+        except Exception as e:
+            raise RuntimeError(
+                f"Failed to check base state.\n{e}."
+            )
 
-        self.base.enable_motor_torque(self.enable_motor_torque)
+        success, result = self.base.enable_motor_torque(self.enable_motor_torque)
+        if not success:
+            raise RuntimeError(
+                f"Failed to enable motor torque.\n{result}.\n{self.check_base_state()[0]}"
+            )
 
         if not self.leader_arms and not self.follower_arms and not self.cameras:
             raise ValueError(
@@ -152,8 +170,27 @@ class TrossenAIMobile():
         self.is_connected = True
 
 
+    def check_base_state(self) -> Tuple[str, SlateBaseSystemState]:
+        success = self.base.update_state()
+        if not success:
+            return "Failed to get base state. Make sure the robot is powered on and connected to the computer."
+            
+        self.base.read(self.slate_base_data)
+        state_code = self.slate_base_data.system_state
+        try:
+            state_name = SlateBaseSystemState(state_code).name
+        except ValueError:
+            state_name = f"UNKNOWN_STATE (code: {state_code})"
+
+        return state_name, state_code
+
+
     def get_base_state(self) -> dict:
-        self.base.update_state()
+        success = self.base.update_state()
+        if not success:
+            raise RobotDeviceNotConnectedError(
+                "Failed to get base state. Make sure the robot is powered on and connected to the computer."
+            )
         self.base.read(self.slate_base_data)
         return {
             "odom_x": self.slate_base_data.odom_x,
@@ -328,7 +365,11 @@ class TrossenAIMobile():
 
         linear_vel, angular_vel = action.tolist()[-2:]
         before_write_t = time.perf_counter()
-        self.base.set_cmd_vel(linear_vel, angular_vel)
+        success = self.base.set_cmd_vel(linear_vel, angular_vel)
+        if not success:
+            raise RobotDeviceNotConnectedError(
+                "Failed to send action to base slate. Make sure the robot is powered on and connected to the computer."
+            )
         self.logs["write_base_dt_s"] = time.perf_counter() - before_write_t
 
         action_sent.append(action[-2:])
@@ -340,7 +381,11 @@ class TrossenAIMobile():
             raise RobotDeviceNotConnectedError(
                 "TrossenAIMobile is not connected. You need to run `robot.connect()` before disconnecting."
             )
-        self.base.enable_motor_torque(False)
+        success, result = self.base.enable_motor_torque(False)
+        if not success:
+            raise RuntimeError(
+                f"Failed to disable motor torque.\n{result}."
+            )
 
         for name in self.follower_arms:
             self.follower_arms[name].disconnect()

lerobot/common/robot_devices/utils.py

@@ -12,9 +12,28 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from enum import IntEnum
 import platform
 import time
 
+class SlateBaseSystemState(IntEnum):
+    SYS_INIT = 0x00
+    SYS_NORMAL = 0x01
+    SYS_REMOTE = 0x02
+    SYS_ESTOP = 0x03
+    SYS_CALIB = 0x04
+    SYS_TEST = 0x05
+    SYS_CHARGING = 0x06
+
+    SYS_ERR = 0x10
+    SYS_ERR_ID = 0x11
+    SYS_ERR_COM = 0x12
+    SYS_ERR_ENC = 0x13
+    SYS_ERR_COLLISION = 0x14
+    SYS_ERR_LOW_VOLTAGE = 0x15
+    SYS_ERR_OVER_VOLTAGE = 0x16
+    SYS_ERR_OVER_CURRENT = 0x17
+    SYS_ERR_OVER_TEMP = 0x18
 
 def busy_wait(seconds):
     if platform.system() == "Darwin":