From 323812a556b70130b7869098573a94890d376e34 Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Tue, 17 Feb 2026 10:02:08 -0500 Subject: [PATCH 1/5] fixed spi read to avoid occasional fifo overflow error --- opensourceleg/sensors/imu.py | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/opensourceleg/sensors/imu.py b/opensourceleg/sensors/imu.py index 64db4f6c..8087f3c0 100644 --- a/opensourceleg/sensors/imu.py +++ b/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(" 0: - fifo_data = self._read_register(address, transfer_len) - return bytes(fifo_data) - return b"" + + if transfer_len <= 0: + return b"" + + chunk_size = 4000 + fifo_data = [] + 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 """ From d3a7eaba544289fd187f2bcee463c88438d387f5 Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Tue, 17 Feb 2026 11:16:25 -0500 Subject: [PATCH 2/5] changed spi read chunk size --- opensourceleg/sensors/imu.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opensourceleg/sensors/imu.py b/opensourceleg/sensors/imu.py index 8087f3c0..73208acb 100644 --- a/opensourceleg/sensors/imu.py +++ b/opensourceleg/sensors/imu.py @@ -1207,7 +1207,7 @@ def _read_fifo(self, address: int = REG_CHAN2_NONWAKEUP_FIFO) -> bytes: if transfer_len <= 0: return b"" - chunk_size = 4000 + chunk_size = 4094 fifo_data = [] bytes_read = 0 From d40759d91159a435f114f886e29a3ab2bd230947 Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Wed, 18 Feb 2026 11:43:16 -0500 Subject: [PATCH 3/5] defined fifo_data type --- opensourceleg/sensors/imu.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opensourceleg/sensors/imu.py b/opensourceleg/sensors/imu.py index 73208acb..06ae1a99 100644 --- a/opensourceleg/sensors/imu.py +++ b/opensourceleg/sensors/imu.py @@ -1208,7 +1208,7 @@ def _read_fifo(self, address: int = REG_CHAN2_NONWAKEUP_FIFO) -> bytes: return b"" chunk_size = 4094 - fifo_data = [] + fifo_data: list[int] = [] bytes_read = 0 while bytes_read < transfer_len: From 7a019180cff72d7cd63358afff964df432cd6e47 Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Wed, 25 Feb 2026 14:10:50 -0500 Subject: [PATCH 4/5] added current brake control mode for tmotor --- opensourceleg/actuators/base.py | 3 + opensourceleg/actuators/tmotor.py | 92 ++++++++++++++++++- .../actuators/tmotor/torque_brake_control.py | 91 ++++++++++++++++++ 3 files changed, 181 insertions(+), 5 deletions(-) create mode 100644 tutorials/actuators/tmotor/torque_brake_control.py diff --git a/opensourceleg/actuators/base.py b/opensourceleg/actuators/base.py index 2371939f..f8bd7d12 100644 --- a/opensourceleg/actuators/base.py +++ b/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] = [ diff --git a/opensourceleg/actuators/tmotor.py b/opensourceleg/actuators/tmotor.py index 6ae5027b..b1b26606 100644 --- a/opensourceleg/actuators/tmotor.py +++ b/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, @@ -494,15 +527,12 @@ def _servo_idle_mode_exit(actuator: "TMotorServoActuator") -> None: exit_callback=_servo_idle_mode_exit, 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) diff --git a/tutorials/actuators/tmotor/torque_brake_control.py b/tutorials/actuators/tmotor/torque_brake_control.py new file mode 100644 index 00000000..79c187de --- /dev/null +++ b/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() From ff9d01df02508481114a84b771bc4fd0ce6947c2 Mon Sep 17 00:00:00 2001 From: Katharine-Walters <111811694+Katharine-Walters@users.noreply.github.com> Date: Wed, 25 Feb 2026 19:23:32 +0000 Subject: [PATCH 5/5] fixed quality --- opensourceleg/actuators/tmotor.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/opensourceleg/actuators/tmotor.py b/opensourceleg/actuators/tmotor.py index b1b26606..9269b79f 100644 --- a/opensourceleg/actuators/tmotor.py +++ b/opensourceleg/actuators/tmotor.py @@ -527,12 +527,12 @@ def _servo_idle_mode_exit(actuator: "TMotorServoActuator") -> None: exit_callback=_servo_idle_mode_exit, has_gains=False, max_gains=None, - ) + ), ) 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