Tmotor current brake

opensourceleg/actuators/base.py

@@ -134,6 +134,7 @@ class CONTROL_MODES(Enum):
     IMPEDANCE = 3
     VELOCITY = 4
     TORQUE = 5
+    CURRENT_BRAKE = 6
 
 
 # TODO: This can be ordered and requires validation
@@ -200,6 +201,7 @@ class CONTROL_MODE_CONFIGS(NamedTuple):
         IMPEDANCE (Optional[ControlModeConfig]): Configuration for IMPEDANCE mode.
         VELOCITY (Optional[ControlModeConfig]): Configuration for VELOCITY mode.
         TORQUE (Optional[ControlModeConfig]): Configuration for TORQUE mode.
+        CURRENT_BRAKE (Optional[ControlModeConfig]): Configuration for CURRENT_BRAKE mode.
 
     Examples:
         >>> idle_config = ControlModeConfig(
@@ -218,6 +220,7 @@ class CONTROL_MODE_CONFIGS(NamedTuple):
     IMPEDANCE: Optional[ControlModeConfig] = None
     VELOCITY: Optional[ControlModeConfig] = None
     TORQUE: Optional[ControlModeConfig] = None
+    CURRENT_BRAKE: Optional[ControlModeConfig] = None
 
 
 CONTROL_MODE_METHODS: list[str] = [

opensourceleg/actuators/tmotor.py

@@ -239,6 +239,19 @@ def set_current(self, motor_id: int, current: float) -> None:
         message_id = (CAN_PACKET_ID["SET_CURRENT"] << 8) | motor_id
         self.send_message(message_id, buffer, len(buffer))
 
+    def set_brake_current(self, motor_id: int, current: float) -> None:
+        """
+        Send current brake control command
+        Args:
+            motor_id (int): CAN motor ID
+            current (float): motor braking current in mA
+        Returns:
+            None
+        """
+        buffer = self._pack_int32(int(current))
+        message_id = (CAN_PACKET_ID["SET_CURRENT_BRAKE"] << 8) | motor_id
+        self.send_message(message_id, buffer, len(buffer))
+
     def set_velocity(self, motor_id: int, velocity: float) -> None:
         """
         Send velocity control command
@@ -444,6 +457,20 @@ def _servo_current_mode_exit(actuator: "TMotorServoActuator") -> None:
         actuator._canman.set_current(actuator.motor_id, 0.0)
 
 
+def _servo_current_brake_mode_entry(actuator: "TMotorServoActuator") -> None:
+    LOGGER.debug(msg=f"[{actuator.__str__()}] Entering Current control mode.")
+    mode_id = 2
+    if not actuator.is_offline and actuator._canman:
+        actuator._canman.set_control_mode(actuator.motor_id, mode_id)
+        _wait_for_mode_switch(actuator)
+
+
+def _servo_current_brake_mode_exit(actuator: "TMotorServoActuator") -> None:
+    LOGGER.debug(msg=f"[{actuator.__str__()}] Exiting Current control mode.")
+    if not actuator.is_offline and actuator._canman:
+        actuator._canman.set_current(actuator.motor_id, 0.0)
+
+
 def _servo_velocity_mode_entry(actuator: "TMotorServoActuator") -> None:
     LOGGER.debug(msg=f"[{actuator.__str__()}] Entering Velocity control mode.")
     mode_id = 3
@@ -483,6 +510,12 @@ def _servo_idle_mode_exit(actuator: "TMotorServoActuator") -> None:
         has_gains=False,  # servo mode handles internally
         max_gains=None,
     ),
+    CURRENT_BRAKE=ControlModeConfig(
+        entry_callback=_servo_current_brake_mode_entry,
+        exit_callback=_servo_current_brake_mode_exit,
+        has_gains=False,  # servo mode handles internally
+        max_gains=None,
+    ),
     VELOCITY=ControlModeConfig(
         entry_callback=_servo_velocity_mode_entry,
         exit_callback=_servo_velocity_mode_exit,
@@ -495,14 +528,11 @@ def _servo_idle_mode_exit(actuator: "TMotorServoActuator") -> None:
         has_gains=False,
         max_gains=None,
     ),
-    IMPEDANCE=None,  # IMPEDANCE mode not supported
-    VOLTAGE=None,  # VOLTAGE mode not supported
 )
 
 
 class TMotorServoActuator(ActuatorBase):
-    """
-    TMotor servo mode actuator for AK series motors.
+    """TMotor servo mode actuator for AK series motors.
 
     Important: Before using this actuator, the CAN interface must be configured:
         sudo /sbin/ip link set can0 down
@@ -875,6 +905,33 @@ def set_motor_current(self, value: float) -> None:
             self._canman.set_current(self.motor_id, clamped_driver_current)
             self._last_command_time = time.monotonic()
 
+    def set_motor_brake_current(self, value: float) -> None:
+        """
+        Set motor current with clamping to motor limits
+        Args:
+            value (float): desired motor current in mA
+        """
+        if not self.is_offline and self._canman:
+            driver_current = value
+
+            # Get current limits from motor parameters
+            max_current = self._motor_params["Curr_max"]
+            min_current = 0.0
+
+            # Clamp current to safe limits
+            clamped_driver_current = np.clip(driver_current, min_current, max_current)
+
+            # Log warning if clamping occurred
+            if driver_current != clamped_driver_current:
+                clamped_user_current = clamped_driver_current * self._current_scale
+                LOGGER.warning(
+                    f"Current command {value}mA clamped to {clamped_user_current}mA "
+                    f"(limits: [{min_current * self._current_scale:.1f}, {max_current * self._current_scale:.1f}]mA)"
+                )
+
+            self._canman.set_brake_current(self.motor_id, clamped_driver_current)
+            self._last_command_time = time.monotonic()
+
     def set_motor_position(self, value: float) -> None:
         raise NotImplementedError(
             "Setting motor position not supported" "Recommended to use 'set_output_position' command instead."
@@ -923,6 +980,19 @@ def set_motor_torque(self, value: float) -> None:
         current = value / kt_user * 1000
         self.set_motor_current(current)  # Send current command mA
 
+    def set_motor_brake_torque(self, value: float) -> None:
+        """
+        Sets the motor braking torque in Nm.
+        This is the torque that is applied to the motor rotor, not the joint or output.
+        Args:
+            value (float): The torque to set in Nm.
+        Returns:
+            None
+        """
+        kt_user = self._motor_params["Kt_actual"] * self._kt_scale
+        current = value / kt_user * 1000
+        self.set_motor_brake_current(current)  # Send current command mA
+
     def set_output_torque(self, value: float) -> None:
         """
         Set the output torque of the joint.
@@ -936,6 +1006,18 @@ def set_output_torque(self, value: float) -> None:
         motor_torque = value / self.gear_ratio
         self.set_motor_torque(motor_torque)
 
+    def set_output_brake_torque(self, value: float) -> None:
+        """
+        Set the output braking torque of the joint.
+        This is the torque that is applied to the joint, not the motor.
+        Args:
+            value (float): torque in Nm
+        Returns:
+            None
+        """
+        motor_torque = value / self.gear_ratio
+        self.set_motor_brake_torque(motor_torque)
+
     def set_motor_velocity(self, value: float) -> None:
         """Set motor velocity (rad/s) with clamping to motor limits"""
         velocity_erpm = rad_per_sec_to_erpm(value, self.num_pole_pairs)

opensourceleg/sensors/imu.py

@@ -1203,10 +1203,23 @@ def _read_fifo(self, address: int = REG_CHAN2_NONWAKEUP_FIFO) -> bytes:
         """
         length_bytes = self._read_register(address, 2)
         transfer_len = struct.unpack("<H", bytes(length_bytes))[0]
-        if transfer_len > 0:
-            fifo_data = self._read_register(address, transfer_len)
-            return bytes(fifo_data)
-        return b""
+
+        if transfer_len <= 0:
+            return b""
+
+        chunk_size = 4094
+        fifo_data: list[int] = []
+        bytes_read = 0
+
+        while bytes_read < transfer_len:
+            remaining_bytes = transfer_len - bytes_read
+            current_chunk_size = min(remaining_bytes, chunk_size)
+
+            chunk = self._read_register(address, current_chunk_size)
+            fifo_data.extend(chunk)
+            bytes_read += current_chunk_size
+
+        return bytes(fifo_data)
 
     def _parse_fifo(self, fifo_data: bytes) -> list[dict]:  # noqa: C901
         """

tutorials/actuators/tmotor/torque_brake_control.py

@@ -0,0 +1,91 @@
+import time
+
+import numpy as np
+
+from opensourceleg.actuators.base import CONTROL_MODES
+from opensourceleg.actuators.tmotor import TMotorServoActuator
+from opensourceleg.logging.logger import Logger
+from opensourceleg.utilities import SoftRealtimeLoop
+
+FREQUENCY = 200
+DT = 1 / FREQUENCY
+MOTOR_ID = 104  # Change this to match your motor's CAN ID
+
+
+def torque_control():
+    torque_logger = Logger(
+        log_path="./logs",
+        file_name="tmotor_torque_control",
+    )
+
+    # Initialize TMotor actuator
+    motor = TMotorServoActuator(
+        motor_type="AK80-9",  # Change to your motor model
+        motor_id=MOTOR_ID,
+        gear_ratio=9.0,
+        offline=False,
+    )
+
+    clock = SoftRealtimeLoop(dt=DT)
+
+    with motor:
+        motor.update()
+
+        # Set the encoder origin first (optional)
+        print("Setting encoder origin...")
+        motor.set_origin()
+
+        # Set to current control mode for torque control
+        motor.set_control_mode(mode=CONTROL_MODES.CURRENT_BRAKE)
+
+        # Motor torque constant for AK80-9
+        MAX_OUTPUT_TORQUE = 10.0  # Maximum output torque in Nm (safety limit)
+
+        # Track torque data
+        torque_logger.track_function(lambda: motor.motor_torque, "Motor Torque")
+        torque_logger.track_function(lambda: motor.output_torque, "Output Torque")
+        torque_logger.track_function(lambda: command_torque_brake, "Command Output Torque")
+        torque_logger.track_function(lambda: motor.motor_current, "Motor Current")
+        torque_logger.track_function(lambda: motor.output_position, "Motor Position")
+        torque_logger.track_function(lambda: motor.output_velocity, "Motor Velocity")
+        torque_logger.track_function(lambda: time.monotonic(), "Time")
+
+        print("Starting torque control...")
+
+        # Create a torque profile
+        for t in clock:
+            # Current brake command
+            command_torque_brake = 5.0
+
+            # Limit output torque for safety
+            command_torque_brake = np.clip(command_torque_brake, 0, MAX_OUTPUT_TORQUE)
+
+            # Set output torque
+            motor.set_output_brake_torque(command_torque_brake)
+            motor.update()
+
+            torque_logger.info(
+                f"Time: {t:.3f}; "
+                f"Command Output Torque: {command_torque_brake:.3f} Nm; "
+                f"Output Torque: {motor.output_torque:.3f} Nm; "
+                f"Motor Torque: {motor.motor_torque:.3f} Nm; "
+                f"Current: {motor.motor_current:.2f} mA; "
+                f"Velocity: {motor.output_velocity:.2f} rad/s"
+            )
+            torque_logger.update()
+
+            # Run for 10 seconds
+            if t > 10.0:
+                break
+
+        print("Torque control complete")
+
+        # Stop the motor
+        print("Stopping motor...")
+        motor.set_output_brake_torque(0.0)
+        motor.update()
+        time.sleep(1.0)
+
+
+if __name__ == "__main__":
+    torque_control()