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..9269b79f 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, @@ -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) diff --git a/opensourceleg/sensors/imu.py b/opensourceleg/sensors/imu.py index 64db4f6c..06ae1a99 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 = 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 """ 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()