diff --git a/docs/tutorials/actuators/tmotor_servo_setup.md b/docs/tutorials/actuators/tmotor_servo_setup.md new file mode 100644 index 00000000..c3ef41df --- /dev/null +++ b/docs/tutorials/actuators/tmotor_servo_setup.md @@ -0,0 +1,207 @@ +# TMotor Servo Mode Setup Guide + +## Prerequisites + +Before using TMotor actuators in servo mode, you need to configure the CAN interface on your system. + +## CAN Interface Configuration + +### Initial Setup + +Run the following commands to configure the CAN interface for TMotor servo mode: + +```bash +# Bring down the CAN interface +sudo /sbin/ip link set can0 down + +# Configure CAN interface with 1MHz bitrate +sudo /sbin/ip link set can0 up type can bitrate 1000000 +``` + +### Verification + +To verify that the CAN interface is properly configured: + +```bash +# Check CAN interface status +ip link show can0 + +# Monitor CAN traffic (optional) +candump can0 +``` + +## Motor Configuration + +### Supported Motors + +- **AK80-9**: + - Torque constant (Kt): 0.095 Nm/A (driver-compatible value for Delta winding) + - Gear ratio: 9:1 + - Pole pairs: 21 + +- **AK10-9**: + - Torque constant (Kt): 0.206 Nm/A + - Gear ratio: 9:1 + - Pole pairs: 21 + +### Understanding Torque Constant (Kt) for Delta-Wound Motors + +#### Background + +The TMotor actuators use **Delta-wound motors** with CubeMars FOC drivers. There is a discrepancy between different torque constant (Kt) values: + +- **0.095 Nm/A** - Driver-compatible Kt (what you should use in your code for AK80-9) +- **0.14 Nm/A** - CubeMars specification (power-invariant transform) +- **0.163 Nm/A** - Physical Kt from Back-EMF testing (amplitude-invariant transform) + +#### The Root Cause: Delta vs Wye Windings + +The CubeMars FOC driver defines current as follows: + +> "The definition of CubeMars' current: Amplitude of the line currents, which using the amplitude-invariant Clarke-Park transform is equivalent to the q-axis current." + +**This definition only holds true for Wye-wound motors**, where line current equals phase current. However, these motors use **Delta winding**, where: + +``` +Line Current (I_l) = √3 × Phase Current (I_φ) +``` + +The standard amplitude-invariant Clarke-Park transform is built on phase current (I_φ), but CubeMars sets their I_q equal to the line current amplitude. This causes the I_q value in their FOC driver to be inflated by a factor of √3. + +#### Mathematical Analysis + +For a Delta winding motor: + +1. **Physical torque equation** (from Back-EMF testing): + ``` + τ = Kt^φ × I^φ + where Kt^φ = 0.163 Nm/A (true physical constant) + ``` + +2. **Driver torque equation** (what CubeMars uses): + ``` + τ = Kt^drv × I_q^drv + where I_q^drv = I_l = √3 × I^φ + ``` + +3. **Derivation of the relationship**: + ``` + Kt^drv × (√3 × I^φ) = Kt^φ × I^φ + Kt^φ = √3 × Kt^drv + √3 × 0.095 ≈ 0.165 Nm/A ≈ 0.163 Nm/A ✓ + ``` + +This calculation matches the Back-EMF measurement. + +#### Why Use 0.095 Nm/A? + +**Use Kt = 0.095 Nm/A** when working with CubeMars drivers and Delta-wound motors because: + +1. It matches the driver's current definition (line current amplitude) +2. The FOC driver settings cannot be easily modified +3. It provides the correct torque calculation when combined with the driver's current interpretation + +#### Practical Implications + +Due to the CubeMars FOC driver settings: + +- The driver's I_q value is √3 times larger than the true phase current +- The maximum current limit is also √3 times higher than the physical phase current +- The motor can be commanded with current values that are √3 times higher than would be expected from the physical Kt (0.163 Nm/A) + +#### Summary of Three Kt Values + +| Kt Value | Transform Type | Usage | +|----------|----------------|-------| +| 0.163 Nm/A | Amplitude-invariant (I_q = phase current) | Physical/theoretical analysis | +| 0.14 Nm/A | Power-invariant (I_q^pwr = √(3/2) × I^φ) | CubeMars specification | +| 0.095 Nm/A | Driver-compatible (I_q = line current) | **Use this in your code** | + +The three values are related by: +``` +Kt^φ = √3 × Kt^drv ≈ 1.732 × 0.095 ≈ 0.165 Nm/A +Kt^pwr = √2 × Kt^drv ≈ 1.414 × 0.095 ≈ 0.134 Nm/A +``` + +### Control Modes + +The TMotor servo mode supports the following control modes: + +1. **Position Control** (Mode 4): Control motor position in degrees +2. **Velocity Control** (Mode 3): Control motor velocity in ERPM +3. **Current Control** (Mode 1): Control motor current in Amps +4. **Idle Mode** (Mode 7): Motor idle state + +## Usage Example + +```python +from opensourceleg.actuators.tmotor import TMotorServoActuator + +# Initialize motor +motor = TMotorServoActuator( + motor_id=104, #the default CAN ID is 104 + gear_ratio=9.0, + motor_type="AK80-9" +) + +# Start motor +motor.start() + +# Home the motor +motor.home() + +# Set control mode +motor.set_control_mode(mode=CONTROL_MODES.POSITION) + +# Command position +motor.set_motor_position(position_rad=1.57) # 90 degrees + +# Stop motor +motor.stop() +``` + +## Troubleshooting + +### CAN Bus Initialization Failed + +If you encounter a "CAN bus initialization failed" error: + +1. Ensure the CAN interface is properly configured (see CAN Interface Configuration above) +2. Check that you have the necessary permissions (may require sudo) +3. Verify the CAN hardware is connected +4. Check for conflicting CAN bus processes + +### Permission Denied + +If you get permission errors when configuring the CAN interface: + +```bash +# Add your user to the dialout group +sudo usermod -a -G dialout $USER + +# Logout and login again for changes to take effect +``` + +### CAN Interface Not Found + +If `can0` is not found: + +1. Check if CAN drivers are loaded: + ```bash + lsmod | grep can + ``` + +2. Load CAN drivers if needed: + ```bash + sudo modprobe can + sudo modprobe can_raw + sudo modprobe vcan # For virtual CAN (testing) + ``` + +## Notes + +- The CAN interface configuration must be done before initializing the TMotorServoActuator +- The interface configuration is not persistent across reboots +- For production use, consider adding the configuration to system startup scripts +- Always ensure proper grounding and shielding for CAN communication +- Unlike some motors, in this type the current reading only becomes negative when the velocity and torque directions are opposite. diff --git a/opensourceleg/actuators/base.py b/opensourceleg/actuators/base.py index 01f740f6..77ac762a 100644 --- a/opensourceleg/actuators/base.py +++ b/opensourceleg/actuators/base.py @@ -47,6 +47,10 @@ class MOTOR_CONSTANTS: MAX_CASE_TEMPERATURE: float # Hard limit for case/housing temperature MAX_WINDING_TEMPERATURE: float # Hard limit for winding temperature + # TMotor Servo Mode specific parameters (Servo Mode does not support PID) + NM_PER_RAD_TO_K: float = 0.001 # Default value for non-servo mode actuators + NM_S_PER_RAD_TO_B: float = 0.0001 # Default value for non-servo mode actuators + # Thermal model parameters from research paper (with defaults from Jake Schuchmann's tests) WINDING_THERMAL_CAPACITANCE: float = 0.20 * 81.46202695970649 # Cw (J/°C) CASE_THERMAL_CAPACITANCE: float = 512.249065845453 # Ch (J/°C) diff --git a/opensourceleg/actuators/tmotor.py b/opensourceleg/actuators/tmotor.py index 7a6b99cf..6425c427 100644 --- a/opensourceleg/actuators/tmotor.py +++ b/opensourceleg/actuators/tmotor.py @@ -1,17 +1,12 @@ import time +import traceback import warnings -from math import isfinite -from typing import Callable, Optional +from dataclasses import dataclass +from enum import Enum +from typing import Any, Optional, cast import can import numpy as np -from TMotorCANControl.mit_can import ( - CAN_Manager, - MIT_command, - MIT_Params, - TMotorManager_mit_can, - motor_state, -) from opensourceleg.actuators.base import ( CONTROL_MODE_CONFIGS, @@ -29,688 +24,962 @@ from opensourceleg.math import ThermalModel from opensourceleg.utilities import SoftRealtimeLoop -TMOTOR_ACTUATOR_CONSTANTS = MOTOR_CONSTANTS( - MOTOR_COUNT_PER_REV=16384, - NM_PER_AMP=0.1133, - MAX_CASE_TEMPERATURE=80, - MAX_WINDING_TEMPERATURE=110, +# TMotor servo mode error codes +TMOTOR_ERROR_CODES: dict[int, str] = { + 0: "No Error", + 1: "Over voltage fault", + 2: "Under voltage fault", + 3: "DRV fault", + 4: "Absolute over current fault", + 5: "Over temp FET fault", + 6: "Over temp motor fault", + 7: "Gate driver over voltage fault", + 8: "Gate driver under voltage fault", + 9: "MCU under voltage fault", + 10: "Booting from watchdog reset fault", + 11: "Encoder SPI fault", + 12: "Encoder sincos below min amplitude fault", + 13: "Encoder sincos above max amplitude fault", + 14: "Flash corruption fault", + 15: "High offset current sensor 1 fault", + 16: "High offset current sensor 2 fault", + 17: "High offset current sensor 3 fault", + 18: "Unbalanced currents fault", +} + +# TMotor CAN packet IDs +CAN_PACKET_ID = { + "SET_DUTY": 0, + "SET_CURRENT": 1, + "SET_CURRENT_BRAKE": 2, + "SET_RPM": 3, + "SET_POS": 4, + "SET_ORIGIN_HERE": 5, + "SET_POS_SPD": 6, +} + +# TMotor model specifications +TMOTOR_MODELS: dict[str, dict[str, Any]] = { + "AK10-9": { + "P_min": -32000, # -3200 deg + "P_max": 32000, # 3200 deg + "V_min": -100000, # ERPM + "V_max": 100000, # ERPM + "Curr_min": -60000, # -60A (protocol units) + "Curr_max": 60000, # 60A (protocol units) + "Kt_actual": 0.206, # Nm/A + "GEAR_RATIO": 9.0, + "NUM_POLE_PAIRS": 21, + }, + "AK80-9": { + "P_min": -32000, # -3200 deg + "P_max": 32000, # 3200 deg + "V_min": -32000, # ERPM + "V_max": 32000, # ERPM + "Curr_min": -60000, # -60A (protocol units) + "Curr_max": 60000, # 60A (protocol units) + "Kt_actual": 0.115, # Nm/A + "GEAR_RATIO": 9.0, + "NUM_POLE_PAIRS": 21, + }, +} + +# TMotor servo mode constants (for ActuatorBase compatibility) +TMOTOR_SERVO_CONSTANTS = MOTOR_CONSTANTS( + MOTOR_COUNT_PER_REV=65536, # Encoder counts per revolution (16-bit encoder) + NM_PER_AMP=0.115, # AK80-9 default + NM_PER_RAD_TO_K=1e-9, # small positive placeholder to satisfy validation + NM_S_PER_RAD_TO_B=1e-9, # small positive placeholder to satisfy validation + MAX_CASE_TEMPERATURE=80.0, + MAX_WINDING_TEMPERATURE=110.0, + # Soft limits set 10°C below hard limits for safety margin + WINDING_SOFT_LIMIT=100.0, + CASE_SOFT_LIMIT=70.0, ) -def _tmotor_impedance_mode_exit(tmotor_actuator: "TMotorMITCANActuator") -> None: - tmotor_actuator.stop_motor() +@dataclass +class ServoMotorState: + """Motor state data structure""" + + position: float = 0.0 # degrees + velocity: float = 0.0 # ERPM + current: float = 0.0 # amps + temperature: float = 0.0 # celsius + error: int = 0 + + +class ServoControlMode(Enum): + """TMotor servo control modes""" + + POSITION = 4 + VELOCITY = 3 + CURRENT = 1 + IDLE = 7 + + +class CANManagerServo: + """TMotor servo mode CAN communication manager""" + + _instance: Optional["CANManagerServo"] = None + debug: bool = False + _initialized: bool = False + + def __new__(cls) -> "CANManagerServo": + if not cls._instance: + cls._instance = super().__new__(cls) + cls._instance._initialized = False + return cls._instance + + def __init__(self) -> None: + if hasattr(self, "_initialized") and self._initialized: + return + + LOGGER.info("Initializing CAN Manager for TMotor Servo Mode") + try: + # NOTE: CAN interface must be configured before running this code. + # Please run the following commands before using TMotor servo mode: + # sudo /sbin/ip link set can0 down + # sudo /sbin/ip link set can0 up type can bitrate 1000000 + # sudo ifconfig can0 txqueuelen 1000 + + self.bus = can.interface.Bus(channel="can0", bustype="socketcan") + self.notifier = can.Notifier(bus=self.bus, listeners=[]) + + LOGGER.info(f"CAN bus connected: {self.bus}") + self._initialized = True + + except Exception as e: + LOGGER.error(f"CAN bus initialization failed: {e}") + LOGGER.error("Please ensure CAN interface is configured. Run:") + LOGGER.error("sudo /sbin/ip link set can0 down") + LOGGER.error("sudo /sbin/ip link set can0 up type can bitrate 1000000") + LOGGER.error("sudo ifconfig can0 txqueuelen 1000") + raise RuntimeError("CAN bus initialization failed. Please configure CAN interface first.") from e + + def __del__(self) -> None: + try: + if hasattr(self, "bus"): + self.bus.shutdown() + except Exception as e: + LOGGER.warning(f"Error shutting down CAN bus: {e}") + + def send_message(self, motor_id: int, data: list, data_len: int) -> None: + """Send CAN message""" + if data_len > 8: + raise ValueError(f"Data too long in message for motor {motor_id}") + + if self.debug: + LOGGER.info(f'ID: {hex(motor_id)} Data: [{", ".join(hex(d) for d in data)}]') + + message = can.Message(arbitration_id=motor_id, data=data, is_extended_id=True) + + try: + self.bus.send(message) + except can.CanError as e: + LOGGER.error(f"Failed to send CAN message: {e}") + + def power_on(self, motor_id: int) -> None: + """Send power on command""" + self.send_message(motor_id, [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC], 8) + + def power_off(self, motor_id: int) -> None: + """Send power off command""" + self.send_message(motor_id, [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD], 8) + + def set_current(self, controller_id: int, current: float) -> None: + """Send current control command""" + current_protocol = int(current * 1000.0) # Convert to protocol units + buffer = self._pack_int32(current_protocol) + message_id = (CAN_PACKET_ID["SET_CURRENT"] << 8) | controller_id + self.send_message(message_id, buffer, len(buffer)) + + def set_velocity(self, controller_id: int, velocity: float) -> None: + """Send velocity control command""" + buffer = self._pack_int32(int(velocity)) + message_id = (CAN_PACKET_ID["SET_RPM"] << 8) | controller_id + self.send_message(message_id, buffer, len(buffer)) + + def set_position(self, controller_id: int, position: float) -> None: + """Send position control command""" + # NOTE(IMPORTANT): TMotor spec uses 1,000,000 scale (0.000001° resolution) per documentation + # Current implementation uses 10 scale (0.1° resolution) for simplicity + # To enable high-precision position control, change to: int(position * 1000000.0) + buffer = self._pack_int32(int(position * 10.0)) # 0.1 degree resolution + message_id = (CAN_PACKET_ID["SET_POS"] << 8) | controller_id + self.send_message(message_id, buffer, len(buffer)) + + def set_origin(self, controller_id: int, mode: int = 1) -> None: + """Set motor origin""" + buffer = [mode] + message_id = (CAN_PACKET_ID["SET_ORIGIN_HERE"] << 8) | controller_id + self.send_message(message_id, buffer, len(buffer)) + + def set_control_mode(self, controller_id: int, mode: int) -> None: + """Send control mode switch command""" + buffer = [mode] + message_id = (7 << 8) | controller_id # Assume packet_id=7 for mode switching + self.send_message(message_id, buffer, len(buffer)) + + @staticmethod + def _pack_int32(number: int) -> list: + """Pack 32-bit integer to byte list (big-endian, MSB first)""" + if number < 0: + number = (1 << 32) + number + # TMotor expects big-endian (MSB first) per documentation + return [(number >> 24) & 0xFF, (number >> 16) & 0xFF, (number >> 8) & 0xFF, number & 0xFF] + + def parse_servo_message(self, data: bytes) -> ServoMotorState: + """Parse servo message to motor state""" + if len(data) < 8: + raise ValueError(f"Invalid message length: {len(data)}") + + # Fix endian issue: TMotor packs as big-endian (Data[n]<<8 | Data[n+1]) + pos_int = int.from_bytes(data[0:2], byteorder="big", signed=True) + spd_int = int.from_bytes(data[2:4], byteorder="big", signed=True) + cur_int = int.from_bytes(data[4:6], byteorder="big", signed=True) + + motor_pos = float(pos_int * 0.1) # position (degrees) + motor_spd = float(spd_int * 10.0) # velocity (ERPM) + motor_cur = float(cur_int * 0.01) # current (amps) + motor_temp = float(data[6]) # temperature (celsius) + motor_error = int(data[7]) # error code + + return ServoMotorState(motor_pos, motor_spd, motor_cur, motor_temp, motor_error) + + def add_motor_listener(self, motor: "TMotorServoActuator") -> None: + """Add motor listener""" + listener = MotorListener(self, motor) + self.notifier.add_listener(listener) + + def enable_debug(self, enable: bool = True) -> None: + """Enable/disable debug mode""" + self.debug = enable + if enable: + LOGGER.info("CAN debug mode enabled") -def _tmotor_current_mode_exit(tmotor_actuator: "TMotorMITCANActuator") -> None: - tmotor_actuator.stop_motor() +class MotorListener(can.Listener): + """CAN message listener""" + def __init__(self, canman: CANManagerServo, motor: "TMotorServoActuator") -> None: + self.canman = canman + self.motor = motor -def _tmotor_velocity_mode_exit(tmotor_actuator: "TMotorMITCANActuator") -> None: - tmotor_actuator.stop_motor() + def on_message_received(self, msg: can.Message) -> None: + """Handle received CAN message""" + data = bytes(msg.data) + motor_id = msg.arbitration_id & 0x00000FF + if motor_id == self.motor.motor_id: + self.motor._update_state_async(self.canman.parse_servo_message(data)) -TMOTOR_CONTROL_MODE_CONFIGS = CONTROL_MODE_CONFIGS( - IMPEDANCE=ControlModeConfig( - entry_callback=lambda _: None, - exit_callback=_tmotor_impedance_mode_exit, - has_gains=False, +# Simplified unit conversion functions +def degrees_to_radians(degrees: float) -> float: + """Convert degrees to radians""" + return degrees * np.pi / 180.0 + + +def radians_to_degrees(radians: float) -> float: + """Convert radians to degrees""" + return radians * 180.0 / np.pi + + +def erpm_to_rad_per_sec(erpm: float, num_pole_pairs: int) -> float: + """Convert ERPM to rad/s""" + return erpm * 2 * np.pi / (60 * num_pole_pairs) + + +def rad_per_sec_to_erpm(rad_per_sec: float, num_pole_pairs: int) -> float: + """Convert rad/s to ERPM""" + return rad_per_sec * 60 * num_pole_pairs / (2 * np.pi) + + +def _wait_for_mode_switch(actuator: "TMotorServoActuator", timeout: float = 0.2) -> None: + """Wait for control mode switch confirmation with timeout""" + start_time = time.time() + poll_interval = 0.01 # 10ms polling + + while time.time() - start_time < timeout: + actuator.update() # Read status + if actuator._motor_state.error == 0: + LOGGER.debug(f"Mode switch confirmed after {time.time() - start_time:.3f} seconds") + return + time.sleep(poll_interval) + + # If we reach here, mode switch may not be confirmed but we continue + LOGGER.warning(f"Mode switch confirmation timeout after {timeout} seconds") + + +# Control mode configuration +def _servo_position_mode_entry(actuator: "TMotorServoActuator") -> None: + actuator._servo_mode = ServoControlMode.POSITION + # Send actual mode switch command to motor + if not actuator.is_offline and actuator._canman: + actuator._canman.set_control_mode(actuator.motor_id, ServoControlMode.POSITION.value) + _wait_for_mode_switch(actuator) + + +def _servo_position_mode_exit(actuator: "TMotorServoActuator") -> None: + pass # servo mode handles automatically + + +def _servo_current_mode_entry(actuator: "TMotorServoActuator") -> None: + actuator._servo_mode = ServoControlMode.CURRENT + # Send actual mode switch command to motor + if not actuator.is_offline and actuator._canman: + actuator._canman.set_control_mode(actuator.motor_id, ServoControlMode.CURRENT.value) + _wait_for_mode_switch(actuator) + + +def _servo_current_mode_exit(actuator: "TMotorServoActuator") -> None: + 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: + actuator._servo_mode = ServoControlMode.VELOCITY + # Send actual mode switch command to motor + if not actuator.is_offline and actuator._canman: + actuator._canman.set_control_mode(actuator.motor_id, ServoControlMode.VELOCITY.value) + _wait_for_mode_switch(actuator) + + +def _servo_velocity_mode_exit(actuator: "TMotorServoActuator") -> None: + if not actuator.is_offline and actuator._canman: + actuator._canman.set_velocity(actuator.motor_id, 0.0) + + +def _servo_idle_mode_entry(actuator: "TMotorServoActuator") -> None: + actuator._servo_mode = ServoControlMode.IDLE + # Send actual mode switch command to motor + if not actuator.is_offline and actuator._canman: + actuator._canman.set_control_mode(actuator.motor_id, ServoControlMode.IDLE.value) + _wait_for_mode_switch(actuator) + + +def _servo_idle_mode_exit(actuator: "TMotorServoActuator") -> None: + pass + + +def _impedance_not_supported(actuator: "TMotorServoActuator") -> None: + """Log that impedance control is not supported""" + LOGGER.error( + "TMotor servo mode does not support impedance control. " + "Use position, velocity, or current control modes instead." + ) + raise NotImplementedError( + "TMotor servo mode does not support impedance control. " + "Use position, velocity, or current control modes instead." + ) + + +TMOTOR_SERVO_CONTROL_MODE_CONFIGS = CONTROL_MODE_CONFIGS( + POSITION=ControlModeConfig( + entry_callback=_servo_position_mode_entry, + exit_callback=_servo_position_mode_exit, + has_gains=False, # servo mode handles internally max_gains=None, ), CURRENT=ControlModeConfig( - entry_callback=lambda _: None, - exit_callback=_tmotor_current_mode_exit, - has_gains=False, + entry_callback=_servo_current_mode_entry, + exit_callback=_servo_current_mode_exit, + has_gains=False, # servo mode handles internally max_gains=None, ), VELOCITY=ControlModeConfig( - entry_callback=lambda _: None, - exit_callback=_tmotor_velocity_mode_exit, + entry_callback=_servo_velocity_mode_entry, + exit_callback=_servo_velocity_mode_exit, + has_gains=False, # servo mode handles internally + max_gains=None, + ), + VOLTAGE=ControlModeConfig( + entry_callback=lambda actuator: setattr(actuator, "_servo_mode", ServoControlMode.IDLE), + exit_callback=lambda actuator: None, + has_gains=False, + max_gains=None, + ), + IDLE=ControlModeConfig( + entry_callback=_servo_idle_mode_entry, + exit_callback=_servo_idle_mode_exit, + has_gains=False, + max_gains=None, + ), + # Explicitly define IMPEDANCE as not supported + IMPEDANCE=ControlModeConfig( + entry_callback=_impedance_not_supported, + exit_callback=lambda actuator: None, has_gains=False, max_gains=None, ), ) -# the user-facing class that manages the motor. -class TMotorMITCANActuator(ActuatorBase, TMotorManager_mit_can): +class TMotorServoActuator(ActuatorBase): """ - The user-facing class that manages the motor. This class should be - used in the context of a with as block, in order to safely enter/exit - control of the motor. + 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 + sudo /sbin/ip link set can0 up type can bitrate 1000000 + + For detailed setup instructions, see docs/tutorials/actuators/tmotor_servo_setup.md + + Example: + >>> with TMotorServoActuator(motor_type="AK80-9", motor_id=1) as motor: + ... motor.set_control_mode(CONTROL_MODES.CURRENT) + ... motor.set_output_torque(2.5) + ... motor.update() """ def __init__( self, - tag: str = "TMotorActuator", + tag: str = "TMotorServoActuator", motor_type: str = "AK80-9", - motor_ID: int = 41, + motor_id: int = 104, gear_ratio: float = 1.0, - frequency: int = 500, + frequency: int = 1000, offline: bool = False, - max_mosfett_temp: float = 50, - ): + max_temperature: float = 80.0, + current_mode: str = "driver", + **kwargs: Any, + ) -> None: """ - Sets up the motor manager. Note the device will not be powered on by this method! You must - call __enter__, mostly commonly by using a with block, before attempting to control the motor. + Initialize TMotor servo actuator Args: - tag: A string tag to identify the motor - motor_type: The type of motor to control. Must be a key in MIT_Params. - motor_ID: The ID of the motor to control. - gear_ratio: The gear ratio of the motor. Default is 1.0. - frequency: The frequency at which to send commands to the motor. Default is 500. - offline: Whether to run the motor in offline mode. Default is False. - max_mosfett_temp: The maximum temperature of the mosfet in degrees C. Default is 50. - """ - ActuatorBase.__init__( - self, + tag: actuator identifier + motor_type: motor model (AK80-9, AK10-9) + motor_id: CAN ID + gear_ratio: gear ratio + frequency: control frequency Hz + offline: offline mode + max_temperature: maximum temperature + current_mode: current convention ('driver', 'amplitude-invariant', 'power-invariant') + - 'driver': use driver's native current (default) + - 'amplitude-invariant': true phase current convention + - 'power-invariant': power-invariant current convention + """ + # Validate motor type + if motor_type not in TMOTOR_MODELS: + raise ValueError(f"Unsupported motor type: {motor_type}") + + # Validate current mode + if current_mode not in ["driver", "amplitude-invariant", "power-invariant"]: + raise ValueError( + f"Invalid current_mode: {current_mode}. Must be 'driver', 'amplitude-invariant', or 'power-invariant'" + ) + + super().__init__( tag=tag, gear_ratio=gear_ratio, - motor_constants=TMOTOR_ACTUATOR_CONSTANTS, + motor_constants=TMOTOR_SERVO_CONSTANTS, frequency=frequency, offline=offline, + **kwargs, ) - TMotorManager_mit_can.__init__( - self, - motor_type=motor_type, - motor_ID=motor_ID, - max_mosfett_temp=max_mosfett_temp, - ) - self.type = motor_type - self.ID = motor_ID - # self.csv_file_name = CSV_file - print("Initializing device: " + self.device_info_string()) - - self._motor_state = motor_state(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - self._motor_state_async = motor_state(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - self._command = MIT_command(0.0, 0.0, 0.0, 0.0, 0.0) - # self._control_state = _TMotorManState.IDLE - self._times_past_position_limit = 0 - self._times_past_current_limit = 0 - self._times_past_velocity_limit = 0 - self._angle_threshold = ( - MIT_Params[self.type]["P_max"] - 2.0 - ) # radians, only really matters if the motor's going super fast - self._current_threshold = ( - self.TMotor_current_to_qaxis_current(MIT_Params[self.type]["T_max"]) - 3.0 - ) # A, only really matters if the current changes quick - self._velocity_threshold = ( - MIT_Params[self.type]["V_max"] - 2.0 - ) # radians, only really matters if the motor's going super fast - self._old_pos = None - self._old_curr = 0.0 - self._old_vel = 0.0 - self._old_current_zone = 0 - self.max_temp = max_mosfett_temp # max temp in deg C, can update later - - self._entered = False + + # Motor configuration + self.motor_type = motor_type + self.motor_id = motor_id + self.max_temperature = max_temperature + self._motor_params = TMOTOR_MODELS[motor_type] + self.current_mode = current_mode + + # Current conversion factors + # Physical: I_drv (line current) = K * I_user + # Code: driver_current = user_current / self._current_scale + # So: I_drv = I_user / scale, which means scale = I_user / I_drv = 1 / K + # Kt relationship: τ = Kt_drv * I_drv = Kt_user * I_user + # So: Kt_user = Kt_drv * (I_drv / I_user) = Kt_drv * K + if current_mode == "amplitude-invariant": + # I_drv (line) = √3 * I_phase, so scale = I_phase / I_drv = 1/√3 + # Kt_amp = Kt_drv * √3 + self._current_scale = 1.0 / np.sqrt(3.0) + self._kt_scale = np.sqrt(3.0) + elif current_mode == "power-invariant": + # I_drv (line) = √2 * I_pwr, so scale = I_pwr / I_drv = 1/√2 + # Kt_pwr = Kt_drv * √2 + self._current_scale = 1.0 / np.sqrt(2.0) + self._kt_scale = np.sqrt(2.0) + else: # driver + self._current_scale = 1.0 + self._kt_scale = 1.0 + + # CAN communication + self._canman: Optional[CANManagerServo] = None + if not self.is_offline: + self._canman = CANManagerServo() + self._canman.add_motor_listener(self) + + # State management + self._motor_state = ServoMotorState() + self._motor_state_async = ServoMotorState() + self._servo_mode = ServoControlMode.IDLE + + # Error handling + self._error_code: Optional[int] = None + self._error_message: Optional[str] = None + + # Time management self._start_time = time.time() self._last_update_time = self._start_time - self._last_command_time = None - self._updated = False - self.SF = 1.0 + self._last_command_time: Optional[float] = None - self._thermal_model: ThermalModel = ThermalModel( - motor_constants=self.MOTOR_CONSTANTS, + # Thermal management + self._thermal_model = ThermalModel( + motor_constants=self._MOTOR_CONSTANTS, actuator_tag=self.tag, ) - self._thermal_scale: float = 1.0 - self._canman = CAN_Manager() - self._canman.add_motor(self) + LOGGER.info(f"Initialized TMotor servo: {self.motor_type} ID:{self.motor_id} (current_mode: {current_mode})") @property def _CONTROL_MODE_CONFIGS(self) -> CONTROL_MODE_CONFIGS: - return TMOTOR_CONTROL_MODE_CONFIGS + return TMOTOR_SERVO_CONTROL_MODE_CONFIGS + + def device_info_string(self) -> str: + return f"{self.motor_type} ID:{self.motor_id}" @check_actuator_connection - def start(self): - """ - Used to safely power the motor on and begin the log file (if specified). - """ - print("Turning on control for device: " + self.device_info_string()) + def start(self) -> None: + """Start motor""" + LOGGER.info(f"Starting {self.device_info_string()}") + + if not self.is_offline and self._canman: + self._canman.power_on(self.motor_id) + + # Poll for motor readiness (max 1 second timeout) + start_time = time.time() + timeout = 1.0 + poll_interval = 0.01 # 10ms polling interval + motor_ready = False + + while time.time() - start_time < timeout: + # The motor should start sending status messages after power on + # Check if we've received valid data by checking if position/velocity/current are non-zero + # or if the motor_state has been updated (timestamp check could be added) + if ( + self._motor_state_async.position != 0.0 + or self._motor_state_async.velocity != 0.0 + or self._motor_state_async.current != 0.0 + or self._motor_state_async.temperature > 0.0 + ): + motor_ready = True + LOGGER.debug(f"Motor ready after {time.time() - start_time:.3f} seconds") + break + time.sleep(poll_interval) + + if not motor_ready: + # Fall back to minimum wait time if no status received + LOGGER.warning("No status received from motor, using fallback delay") + time.sleep(0.1) # Minimum wait time + + # Set initial control mode to IDLE + self._canman.set_control_mode(self.motor_id, ServoControlMode.IDLE.value) + + # Poll for mode switch confirmation (max 200ms timeout) + mode_start_time = time.time() + mode_timeout = 0.2 + + while time.time() - mode_start_time < mode_timeout: + self.update() # Read status + if self._motor_state.error == 0: + LOGGER.debug(f"Mode switch confirmed after {time.time() - mode_start_time:.3f} seconds") + break + time.sleep(poll_interval) + + # Final status check + if self._motor_state.error != 0: + raise RuntimeError(f"Motor startup failed with error: {self._motor_state.error}") - self.power_on() - self._send_command() - self._entered = True - self.is_streaming = True - if not self.check_can_connection(): - raise RuntimeError("Device not connected: " + str(self.device_info_string())) - return self + self._is_open = True + self._is_streaming = True @check_actuator_stream @check_actuator_open - def stop(self): - """ - Used to safely power the motor off and close the log file (if specified). - """ - print("Turning off control for device: " + self.device_info_string()) - self.power_off() - - def home( - self, - homing_voltage: int = 2000, - homing_frequency: Optional[int] = None, - homing_direction: int = -1, - output_position_offset: float = 0.0, - current_threshold: int = 5000, - velocity_threshold: float = 0.001, - callback: Optional[Callable[[], None]] = None, - ): - """ - Home the actuator and corresponding joint by moving it to the zero position. - The zero position is defined as the position where the joint is fully extended. - - Args: - homing_voltage (int): Voltage in mV to use for homing. - Default is 2000 mV. - homing_frequency (Optional[int]): Frequency in Hz to use for homing. - Default is the actuator's frequency. - homing_direction (int): Direction to move the actuator during homing. - Default is -1. - output_position_offset (float): Offset in radians to add to the output position. - Default is 0.0. - current_threshold (int): Current threshold in mA to stop homing the joint or actuator. - Default is 5000 mA. - velocity_threshold (float): Velocity threshold in rad/s to stop homing the joint or actuator. - Default is 0.001 rad/s. - callback (Optional[Callable[[], None]]): Optional callback function to be called when homing completes. - The function should take no arguments and return None. - """ - # TODO: implement homing - LOGGER.info(msg=f"[{self.__repr__()}] Homing not implemented.") - pass - - def update(self): # noqa: C901 - """ - This method is called by the user to synchronize the current state used by the controller - with the most recent message recieved, as well as to send the current command. - """ - - # check that the motor is safely turned on - if not self._entered: - raise RuntimeError( - "Tried to update motor state before safely powering on for device: " + self.device_info_string() - ) - - if self.case_temperature > self.max_temp: - raise RuntimeError(f"Temperature greater than {self.max_temp}C for device: {self.device_info_string()}") - - # check that the motor data is recent - # print(self._command_sent) + def stop(self) -> None: + """Stop motor""" + LOGGER.info(f"Stopping {self.device_info_string()}") + + if not self.is_offline and self._canman: + self._canman.power_off(self.motor_id) + + self._is_open = False + self._is_streaming = False + + def update(self) -> None: + """Update motor state""" + # Temperature check + if self.case_temperature > self.max_temperature: + raise RuntimeError(f"Temperature {self.case_temperature}°C exceeds limit") + + # Update state + self._motor_state.position = self._motor_state_async.position + self._motor_state.velocity = self._motor_state_async.velocity + self._motor_state.current = self._motor_state_async.current + self._motor_state.temperature = self._motor_state_async.temperature + self._motor_state.error = self._motor_state_async.error + + # Communication timeout check now = time.time() - if (now - self._last_command_time) < 0.25 and ((now - self._last_update_time) > 0.1): - warnings.warn( - "State update requested but no data from motor. Delay longer after zeroing, \ - decrease frequency, or check connection. " - + self.device_info_string(), - RuntimeWarning, - stacklevel=2, - ) - else: - self._command_sent = False - - # artificially extending the range of the position, current, and velocity that we track - P_max = MIT_Params[self.type]["P_max"] + 0.01 - I_max = self.TMotor_current_to_qaxis_current(MIT_Params[self.type]["T_max"]) + 1.0 - V_max = MIT_Params[self.type]["V_max"] + 0.01 - - if self._old_pos is None: - self._old_pos = self._motor_state_async.position - old_pos = self._old_pos - old_curr = self._old_curr - old_vel = self._old_vel - - new_pos = self._motor_state_async.position - new_curr = self._motor_state_async.current - new_vel = self._motor_state_async.velocity - - thresh_pos = self._angle_threshold - thresh_curr = self._current_threshold - thresh_vel = self._velocity_threshold - - curr_command = self._command.current - - actual_current = new_curr - - # The TMotor will wrap around to -max at the limits for all values it returns!! Account for this - if (thresh_pos <= new_pos and new_pos <= P_max) and (-P_max <= old_pos and old_pos <= -thresh_pos): - self._times_past_position_limit -= 1 - elif (thresh_pos <= old_pos and old_pos <= P_max) and (-P_max <= new_pos and new_pos <= -thresh_pos): - self._times_past_position_limit += 1 - - # current is basically the same as position, but if you instantly - # command a switch it can actually change fast enough - # to throw this off, so that is accounted for too. We just put a hard limit on the current - # to solve current jitter problems. - if (thresh_curr <= new_curr and new_curr <= I_max) and (-I_max <= old_curr and old_curr <= -thresh_curr): - # self._old_current_zone = -1 - # if (thresh_curr <= curr_command and curr_command <= I_max): - # self._times_past_current_limit -= 1 - if curr_command > 0: - actual_current = self.TMotor_current_to_qaxis_current(MIT_Params[self.type]["T_max"]) - elif curr_command < 0: - actual_current = -self.TMotor_current_to_qaxis_current(MIT_Params[self.type]["T_max"]) - else: - actual_current = -self.TMotor_current_to_qaxis_current(MIT_Params[self.type]["T_max"]) - new_curr = actual_current - elif (thresh_curr <= old_curr and old_curr <= I_max) and (-I_max <= new_curr and new_curr <= -thresh_curr): - # self._old_current_zone = 1 - # if not (-I_max <= curr_command and curr_command <= -thresh_curr): - # self._times_past_current_limit += 1 - if curr_command > 0: - actual_current = self.TMotor_current_to_qaxis_current(MIT_Params[self.type]["T_max"]) - elif curr_command < 0: - actual_current = -self.TMotor_current_to_qaxis_current(MIT_Params[self.type]["T_max"]) + if ( + self._last_command_time is not None + and (now - self._last_command_time) < 0.25 + and (now - self._last_update_time) > 0.1 + ): + warnings.warn(f"No data from motor: {self.device_info_string()}", RuntimeWarning, stacklevel=2) + + self._last_update_time = now + + def _update_state_async(self, servo_state: ServoMotorState) -> None: + """Asynchronously update state""" + # More detailed error handling + if servo_state.error != 0: + error_codes = TMOTOR_ERROR_CODES + error_msg = error_codes.get(servo_state.error, f"Unknown error code: {servo_state.error}") + self._error_code = servo_state.error + self._error_message = error_msg + + # Take different actions based on error type + if servo_state.error in [1, 2]: # Voltage errors + LOGGER.error(f"Voltage error {servo_state.error}: {error_msg}") + elif servo_state.error in [4, 5, 6]: # Overcurrent or overtemperature + LOGGER.critical(f"Critical error {servo_state.error}: {error_msg}") + # Auto-stop motor + if self._canman: + self._canman.set_current(self.motor_id, 0.0) else: - actual_current = self.TMotor_current_to_qaxis_current(MIT_Params[self.type]["T_max"]) - new_curr = actual_current - - # velocity should work the same as position - if (thresh_vel <= new_vel and new_vel <= V_max) and (-V_max <= old_vel and old_vel <= -thresh_vel): - self._times_past_velocity_limit -= 1 - elif (thresh_vel <= old_vel and old_vel <= V_max) and (-V_max <= new_vel and new_vel <= -thresh_vel): - self._times_past_velocity_limit += 1 - - # update expanded state variables - self._old_pos = new_pos - self._old_curr = new_curr - self._old_vel = new_vel - - self._motor_state.set_state_obj(self._motor_state_async) - self._motor_state.position += self._times_past_position_limit * 2 * MIT_Params[self.type]["P_max"] - self._motor_state.current = actual_current - self._motor_state.velocity += self._times_past_velocity_limit * 2 * MIT_Params[self.type]["V_max"] - - # send current motor command - self._send_command() - self._updated = False - - # sends a command to the motor depending on whats controlm mode the motor is in - def _send_command(self): - """ - Sends a command to the motor depending on whats controlm mode the motor is in. This method - is called by update(), and should only be called on its own if you don't want to update the motor state info. - - Notably, the current is converted to amps from the reported 'torque' value, which is i*Kt. - This allows control based on actual q-axis current, rather than estimated torque, which - doesn't account for friction losses. - """ - if self.mode == CONTROL_MODES.IMPEDANCE: - self._canman.MIT_controller( - self.ID, - self.type, - self._command.position, - self._command.velocity, - self._command.kp, - self._command.kd, - 0.0, - ) - elif self.mode == CONTROL_MODES.CURRENT: - self._canman.MIT_controller( - self.ID, - self.type, - 0.0, - 0.0, - 0.0, - 0.0, - self.qaxis_current_to_TMotor_current(self._command.current), - ) - elif self.mode == CONTROL_MODES.IDLE: - self._canman.MIT_controller(self.ID, self.type, 0.0, 0.0, 0.0, 0.0, 0.0) - elif self.mode == CONTROL_MODES.VELOCITY: - self._canman.MIT_controller( - self.ID, - self.type, - 0.0, - self._command.velocity, - 0.0, - self._command.kd, - 0.0, - ) + LOGGER.warning(f"Motor error {servo_state.error}: {error_msg}") else: - raise RuntimeError("UNDEFINED STATE for device " + self.device_info_string()) - self._last_command_time = time.time() + self._error_code = None + self._error_message = None - # getters for motor state - @property - def case_temperature(self) -> float: - """ - Returns: - The most recently updated motor temperature in degrees C. - """ - return float(self._motor_state.temperature) - - @property - def winding_temperature(self) -> float: - """ - ESTIMATED temperature of the windings in celsius. - This is calculated based on the thermal model using motor current. - """ - if self._data is not None: - return float(self._thermal_model.winding_temperature) - else: - return 0.0 + self._motor_state_async = servo_state @property - def motor_current(self) -> float: - """ - Returns: - The most recently updated qaxis current in amps - """ - return float(self._motor_state.current) - - @property - def motor_voltage(self) -> float: - # Not implemented - return 0.0 + def error_info(self) -> Optional[tuple[int, str]]: + """Get error information""" + if self._error_code is not None and self._error_message is not None: + return (self._error_code, self._error_message) + return None - @property - def output_position(self) -> float: - """ - Returns: - The most recently updated output angle in radians - """ - return float(self._motor_state.position) - - @property - def output_velocity(self) -> float: - """ - Returns: - The most recently updated output velocity in radians per second - """ - return float(self._motor_state.velocity) - - @property - def output_acceleration(self) -> float: - """ - Returns: - The most recently updated output acceleration in radians per second per second - """ - return float(self._motor_state.acceleration) - - @property - def output_torque(self) -> float: - """ - Returns: - the most recently updated output torque in Nm - """ - return float(self.motor_current * MIT_Params[self.type]["Kt_actual"] * MIT_Params[self.type]["GEAR_RATIO"]) - - # uses plain impedance mode, will send 0.0 for current command. - def _set_impedance_gains( + def home( self, - K: float = 0.08922, - B: float = 0.0038070, + homing_voltage: int = 0, + homing_frequency: Optional[int] = None, + homing_direction: int = 0, + output_position_offset: float = 0.0, + current_threshold: int = 0, + velocity_threshold: float = 0.0, ) -> None: - """ - Uses plain impedance mode, will send 0.0 for current command in addition to position request. + """Home motor""" + if not self.is_offline and self._canman: + self._canman.set_origin(self.motor_id, 1) + time.sleep(0.1) + self._is_homed = True + LOGGER.info(f"Homed {self.device_info_string()}") - Args: - K: The stiffness in Nm/rad - B: The damping in Nm/(rad/s) - """ - if not (isfinite(K) and MIT_Params[self.type]["Kp_min"] <= K and MIT_Params[self.type]["Kp_max"] >= K): - raise ValueError( - f"K must be finite and between \ - {MIT_Params[self.type]['Kp_min']} and {MIT_Params[self.type]['Kp_max']}" - ) + # ============ Control Interface ============ - if not (isfinite(B) and MIT_Params[self.type]["Kd_min"] <= B and MIT_Params[self.type]["Kd_max"] >= B): - raise ValueError( - f"B must be finite and between \ - {MIT_Params[self.type]['Kd_min']} and {MIT_Params[self.type]['Kd_max']}" - ) + def set_motor_voltage(self, value: float) -> None: + """Set motor voltage (not directly supported in servo mode)""" + LOGGER.warning("Voltage control not supported in servo mode") - self._command.kp = K - self._command.kd = B - self._command.velocity = 0.0 + def set_motor_current(self, value: float) -> None: + """Set motor current with clamping to motor limits""" + if not self.is_offline and self._canman: + # Convert from desired current convention to driver current + driver_current = value / self._current_scale + + # Get current limits from motor parameters (these are in driver convention) + motor_params = cast(dict[str, Any], self._motor_params) + max_current = motor_params["Curr_max"] / 1000.0 # Convert from protocol units to amps + min_current = motor_params["Curr_min"] / 1000.0 # Convert from protocol units to amps + + # Clamp current to safe limits + clamped_driver_current = np.clip(driver_current, min_current, max_current) + + # Log warning if clamping occurred (show in user's convention) + if driver_current != clamped_driver_current: + clamped_user_current = clamped_driver_current * self._current_scale + LOGGER.warning( + f"Current command {value:.2f}A clamped to {clamped_user_current:.2f}A " + f"(limits: [{min_current * self._current_scale:.1f}, {max_current * self._current_scale:.1f}]A)" + ) + + self._canman.set_current(self.motor_id, clamped_driver_current) + self._last_command_time = time.time() - def set_current_gains( - self, - kp: float = 40, - ki: float = 400, - ff: float = 128, - spoof: bool = False, - ) -> None: - """ - Uses plain current mode, will send 0.0 for position gains in addition to requested current. + def set_motor_position(self, value: float) -> None: + """Set motor position (radians) with clamping to motor limits""" + position_deg = radians_to_degrees(value) - Args: - kp: A dummy argument for backward compatibility with the dephy library. - ki: A dummy argument for backward compatibility with the dephy library. - ff: A dummy argument for backward compatibility with the dephy library. - spoof: A dummy argument for backward compatibility with the dephy library. - """ - pass + if not self.is_offline and self._canman: + # Get position limits from motor parameters + motor_params = cast(dict[str, Any], self._motor_params) + max_position = motor_params["P_max"] / 10.0 # Convert from protocol units to degrees + min_position = motor_params["P_min"] / 10.0 # Convert from protocol units to degrees - def set_velocity_gains( - self, - kd: float = 1.0, - ) -> None: - """ - Uses plain speed mode, will send 0.0 for position gain and for feed forward current. + # Clamp position to safe limits + clamped_position = np.clip(position_deg, min_position, max_position) - Args: - kd: The gain for the speed controller. Control law will be (v_des - v_actual)*kd = iq - """ - self._command.kd = kd + # Log warning if clamping occurred + if position_deg != clamped_position: + LOGGER.warning( + f"Position command {position_deg:.1f}° clamped to {clamped_position:.1f}° " + f"(limits: [{min_position:.0f}, {max_position:.0f}]°)" + ) - def set_position_gains(self) -> None: - # Not implemented - pass + self._canman.set_position(self.motor_id, clamped_position) + self._last_command_time = time.time() - # used for either impedance or MIT mode to set output angle - def set_output_position(self, value: float) -> None: - """ - Used for either impedance or full state feedback mode to set output angle command. - Note, this does not send a command, it updates the TMotorManager's saved command, - which will be sent when update() is called. + def set_motor_torque(self, value: float) -> None: + """Set motor torque (Nm) - core functionality as requested by user""" + # Torque to current: T = I * Kt + # Kt_user = Kt_drv * kt_scale + kt_user = self._motor_params["Kt_actual"] * self._kt_scale + current = value / kt_user + self.set_motor_current(current) - Args: - value: The desired output position in rads + def set_output_torque(self, value: float) -> None: + """Set output torque (Nm) - core functionality as requested by user""" + # Output torque to motor torque: T_motor = T_output * gear_ratio + motor_torque = value * self.gear_ratio + self.set_motor_torque(motor_torque) - Raises: - RuntimeError: If the position command is outside the range of the motor. - """ - if np.abs(value) >= MIT_Params[self.type]["P_max"]: - raise RuntimeError( - "Cannot control using impedance mode for angles with magnitude greater than " - + str(MIT_Params[self.type]["P_max"]) - + "rad!" - ) + def set_motor_velocity(self, value: float) -> None: + """Set motor velocity (rad/s) with clamping to motor limits""" + motor_params = cast(dict[str, Any], self._motor_params) + velocity_erpm = rad_per_sec_to_erpm(value, self.num_pole_pairs) + + if not self.is_offline and self._canman: + # Get velocity limits from motor parameters + max_velocity = motor_params["V_max"] # Already in ERPM + min_velocity = motor_params["V_min"] # Already in ERPM + + # Clamp velocity to safe limits + clamped_velocity = np.clip(velocity_erpm, min_velocity, max_velocity) + + # Log warning if clamping occurred + if velocity_erpm != clamped_velocity: + LOGGER.warning( + f"Velocity command {velocity_erpm:.0f} ERPM clamped to {clamped_velocity:.0f} ERPM " + f"(limits: [{min_velocity}, {max_velocity}] ERPM)" + ) + + self._canman.set_velocity(self.motor_id, clamped_velocity) + self._last_command_time = time.time() + + def set_motor_impedance(self, position: float, velocity: float, kp: float, kd: float, torque_ff: float) -> None: + """TMotor servo mode does not support impedance control""" + LOGGER.error( + "TMotor servo mode does not support impedance control. " + "Use position, velocity, or current control modes instead." + ) + raise NotImplementedError("TMotor servo mode does not support impedance control.") - self._command.position = value + def set_output_impedance(self, position: float, velocity: float, kp: float, kd: float, torque_ff: float) -> None: + """TMotor servo mode does not support impedance control""" + LOGGER.error( + "TMotor servo mode does not support impedance control. " + "Use position, velocity, or current control modes instead." + ) + raise NotImplementedError("TMotor servo mode does not support impedance control.") def set_output_velocity(self, value: float) -> None: - """ - Used for either speed or full state feedback mode to set output velocity command. - Note, this does not send a command, it updates the TMotorManager's saved command, - which will be sent when update() is called. - - Args: - value: The desired output speed in rad/s - - Raises: - RuntimeError: If the velocity command is outside the range of the motor. - """ - if np.abs(value) >= MIT_Params[self.type]["V_max"]: - raise RuntimeError( - "Cannot control using speed mode for angles with magnitude greater than " - + str(MIT_Params[self.type]["V_max"]) - + "rad/s!" - ) - - self._command.velocity = value - - # used for either current MIT mode to set current - - def set_motor_current(self, value: float) -> None: - """ - Used for either current or full state feedback mode to set current command. - Note, this does not send a command, it updates the TMotorManager's saved command, - which will be sent when update() is called. - - Args: - value: the desired current in amps. - """ - self._command.current = value - - # used for either current or MIT Mode to set current, based on desired torque - def set_output_torque(self, value: float) -> None: - """ - Used for either current or MIT Mode to set current, based on desired torque. - If a more complicated torque model is available for the motor, that will be used. - Otherwise it will just use the motor's torque constant. - - Args: - value: The desired output torque in Nm. - """ - self.set_motor_current(value / MIT_Params[self.type]["Kt_actual"] / MIT_Params[self.type]["GEAR_RATIO"]) + """Set output velocity (rad/s)""" + motor_velocity = value * self.gear_ratio + self.set_motor_velocity(motor_velocity) - # motor-side functions to account for the gear ratio - def set_motor_torque(self, value: float) -> None: - """ - Version of set_output_torque that accounts for gear ratio to control motor-side torque - - Args: - value: The desired motor torque in Nm. - """ - self.set_output_torque(value * MIT_Params[self.type]["Kt_actual"]) + # ============ Unsupported PID Functions - TMotor Servo Mode Handles All Control Loops Internally ============ - def set_motor_position(self, value: float) -> None: - """ - Wrapper for set_output_angle that accounts for gear ratio to control motor-side angle + def set_current_gains(self, kp: float, ki: float, kd: float, ff: float) -> None: + """TMotor servo mode does not support external current PID gains - motor handles current control internally""" + LOGGER.debug( + "TMotor servo mode handles current control internally. " "External current PID gains are not used." + ) + # Motor handles current control internally, no action needed - Args: - value: The desired motor position in rad. - """ - self.set_output_position(value / (MIT_Params[self.type]["GEAR_RATIO"])) + def set_position_gains(self, kp: float, ki: float, kd: float, ff: float) -> None: + """TMotor servo mode does not support external position PID gains - motor handles position control internally""" + LOGGER.debug( + "TMotor servo mode handles position control internally. " "External position PID gains are not used." + ) + # Motor handles position control internally, no action needed - def set_motor_velocity(self, value: float) -> None: - """ - Wrapper for set_output_velocity that accounts for gear ratio to control motor-side velocity + def set_impedance_gains(self, kp: float, ki: float, kd: float, k: float, b: float, ff: float) -> None: + """TMotor servo mode does not support impedance control""" + LOGGER.debug( + "TMotor servo mode does not support impedance control. " + "Use position, velocity, or current control modes instead." + ) + # Impedance control not supported in servo mode, no action needed - Args: - value: The desired motor velocity in rad/s. - """ - self.set_output_velocity(value / (MIT_Params[self.type]["GEAR_RATIO"])) + def _set_impedance_gains(self, k: float, b: float) -> None: + """Internal method for impedance gains - not supported in TMotor servo mode""" + LOGGER.debug("TMotor servo mode handles control internally. " "Impedance gains are not used.") + # Motor handles control internally, no action needed - def set_motor_voltage(self, value: float) -> float: - # Not implemented - pass + # ============ State Properties ============ @property def motor_position(self) -> float: - """ - Wrapper for get_output_angle that accounts for gear ratio to get motor-side angle + """Motor position (radians)""" + return degrees_to_radians(self._motor_state.position) - Returns: - The most recently updated motor-side angle in rad. - """ - return float(self._motor_state.position * MIT_Params[self.type]["GEAR_RATIO"]) + @property + def output_position(self) -> float: + """Output position (radians)""" + return self.motor_position / self.gear_ratio @property def motor_velocity(self) -> float: - """ - Wrapper for get_output_velocity that accounts for gear ratio to get motor-side velocity + """Motor velocity (rad/s)""" + return erpm_to_rad_per_sec(self._motor_state.velocity, self.num_pole_pairs) - Returns: - The most recently updated motor-side velocity in rad/s. - """ - return float(self._motor_state.velocity * MIT_Params[self.type]["GEAR_RATIO"]) + @property + def output_velocity(self) -> float: + """Output velocity (rad/s)""" + return self.motor_velocity / self.gear_ratio @property - def motor_acceleration(self) -> float: - """ - Wrapper for get_output_acceleration that accounts for gear ratio to get motor-side acceleration + def num_pole_pairs(self) -> int: + """Number of motor pole pairs""" + motor_params = cast(dict[str, Any], self._motor_params) + return motor_params["NUM_POLE_PAIRS"] - Returns: - The most recently updated motor-side acceleration in rad/s/s. - """ - return float(self._motor_state.acceleration * MIT_Params[self.type]["GEAR_RATIO"]) + @property + def motor_voltage(self) -> float: + """Motor voltage - not available in servo mode""" + raise NotImplementedError( + "Motor voltage reading is not available in TMotor servo mode. " + "The motor does not provide voltage feedback through the CAN protocol." + ) + + @property + def motor_current(self) -> float: + """Motor current (A) - converted to selected current convention""" + return self._motor_state.current * self._current_scale @property def motor_torque(self) -> float: - """ - Wrapper for get_output_torque that accounts for gear ratio to get motor-side torque + """Motor torque (Nm)""" + motor_params = cast(dict[str, Any], self._motor_params) + kt_user = cast(float, motor_params["Kt_actual"]) * self._kt_scale + return self.motor_current * kt_user - Returns: - The most recently updated motor-side torque in Nm. - """ - return float(self.output_torque * MIT_Params[self.type]["GEAR_RATIO"]) + @property + def output_torque(self) -> float: + """Output torque (Nm)""" + return self.motor_torque / self.gear_ratio + + @property + def case_temperature(self) -> float: + """Case temperature (°C)""" + return self._motor_state.temperature + + @property + def winding_temperature(self) -> float: + """Winding temperature (°C)""" + return cast(float, getattr(self._thermal_model, "T_w", self.case_temperature)) - # Pretty stuff def __str__(self) -> str: - """Prints the motor's device info and current""" + """State string""" return ( - self.device_info_string() - + " | Position: " - + f"{round(self.output_angle, 3): 1f}" - + " rad | Velocity: " - + f"{round(self.output_velocity, 3): 1f}" - + " rad/s | current: " - + f"{round(self.motor_current, 3): 1f}" - + " A | torque: " - + f"{round(self.output_torque, 3): 1f}" - + " Nm" + f"{self.device_info_string()} | " + f"Pos: {self.output_position:.3f}rad | " + f"Vel: {self.output_velocity:.3f}rad/s | " + f"Torque: {self.output_torque:.3f}Nm | " + f"Current: {self.motor_current:.3f}A | " + f"Temp: {self.case_temperature:.1f}°C" ) - # Checks the motor connection by sending a 10 commands and making sure the motor responds. - def check_can_connection(self) -> bool: - """ - Checks the motor's connection by attempting to send 10 startup messages. - If it gets 10 replies, then the connection is confirmed. - - Returns: - True if a connection is established and False otherwise. - - Raises: - RuntimeError: If the motor control has not been entered. - """ - if not self._entered: - raise RuntimeError( - "Tried to check_can_connection before entering motor control! \ - Enter control using the __enter__ method, or instantiating the TMotorManager in a with block." - ) - Listener = can.BufferedReader() - self._canman.notifier.add_listener(Listener) - for _i in range(10): - self.power_on() - time.sleep(0.001) - success = True - self._is_open = True - time.sleep(0.1) - for _i in range(10): - if Listener.get_message(timeout=0.1) is None: - success = False - self._is_open = False - self._canman.notifier.remove_listener(Listener) - return success - if __name__ == "__main__": - with TMotorMITCANActuator(motor_type="AK80-9", motor_ID=41) as dev: - dev.set_zero_position() # has a delay! - time.sleep(1.5) - dev.set_control_mode(CONTROL_MODES.IMPEDANCE) - dev._set_impedance_gains(K=10, B=0.5) - - print("Starting position step demo. Press ctrl+C to quit.") - - loop = SoftRealtimeLoop(dt=0.01, report=True, fade=0) - for t in loop: - dev.update() - if t < 1.0: - dev.set_motor_position(0.0) - else: - dev.set_motor_position(10) - + print("TMotor Current Loop Control Example") + + try: + actuator = TMotorServoActuator(motor_type="AK80-9", motor_id=1, offline=True) + + with actuator: + print(f"Motor initialized: {actuator.device_info_string()}") + + # Home and set current control mode + actuator.home() + actuator.set_control_mode(CONTROL_MODES.CURRENT) + print("Current loop mode activated") + + # Send 15Nm torque command + target_torque = 15.0 # Nm + actuator.set_output_torque(target_torque) + print(f"Sending {target_torque}Nm torque command to motor") + print() + + # Create real-time reading loop + loop = SoftRealtimeLoop(dt=0.1, report=False, fade=0) # 10Hz, slower for observation + print("📊 Reading motor parameters...") + + for t in loop: + if t > 5.0: # Run for 5 seconds + break + + # Update motor state + actuator.update() + + # Read motor parameters + motor_angle = actuator.motor_position # motor angle (rad) + output_angle = actuator.output_position # output angle (rad) + motor_velocity = actuator.motor_velocity # motor velocity (rad/s) + output_velocity = actuator.output_velocity # output velocity (rad/s) + motor_current = actuator.motor_current # motor current (A) + motor_torque = actuator.motor_torque # motor torque (Nm) + output_torque = actuator.output_torque # output torque (Nm) + temperature = actuator.case_temperature # temperature (°C) + # motor_voltage = actuator.motor_voltage # voltage (V) - Not available in servo mode + + # Check for errors + error_status = "OK" + if actuator.error_info: + error_code, error_msg = actuator.error_info + error_status = f"Error{error_code}: {error_msg}" + + # Display complete status - every 0.5 seconds + if int(t * 2) % 1 == 0: + print(f"Time: {t:4.1f}s") + print( + f" Torque: Cmd={target_torque:6.2f}Nm | " + f"Motor={motor_torque:6.2f}Nm | Output={output_torque:6.2f}Nm" + ) + print( + f" Angle: Motor={motor_angle:8.4f}rad ({np.degrees(motor_angle):7.2f}°) | " + f"Output={output_angle:8.4f}rad ({np.degrees(output_angle):7.2f}°)" + ) + print(f" Speed: Motor={motor_velocity:8.4f}rad/s | Output={output_velocity:8.4f}rad/s") + print(f" Current: {motor_current:6.2f}A | " f"Temp: {temperature:4.1f}°C") + print(f" Status: {error_status}") + print("-" * 80) + + # Safe stop + actuator.set_output_torque(0.0) + actuator.update() + print("Motor safely stopped") + print() + + # Display final state + print(" Final Motor State:") print( - "Actual: ", - round(dev.output_position, 3), - "Desired: ", - dev._command.position, + f" Final Position: {np.degrees(actuator.output_position):.2f}° " + f"({actuator.output_position:.4f} rad)" ) + print(f" Final Torque: {actuator.output_torque:.2f} Nm") + print(f" Final Current: {actuator.motor_current:.2f} A") + print(f" Final Temperature: {actuator.case_temperature:.1f}°C") + + except Exception as e: + print(f"Error: {e}") + import traceback + + traceback.print_exc() - del loop + print("Current loop control example completed!") diff --git a/opensourceleg/common/__init__.py b/opensourceleg/common/__init__.py deleted file mode 100644 index ea4c0ec8..00000000 --- a/opensourceleg/common/__init__.py +++ /dev/null @@ -1,7 +0,0 @@ -""" -Common utilities and mixins for opensourceleg. -""" - -from .offline import OfflineMixin - -__all__ = ["OfflineMixin"] diff --git a/opensourceleg/utilities/softrealtimeloop.py b/opensourceleg/utilities/softrealtimeloop.py index a27e5f9d..cfcb7f89 100644 --- a/opensourceleg/utilities/softrealtimeloop.py +++ b/opensourceleg/utilities/softrealtimeloop.py @@ -357,6 +357,9 @@ def _next_original_phase(self) -> float: # Busy wait until the time we should be running at while time.monotonic() < self.loop_deadline and not self.killer.kill_now: + if not hasattr(signal, "sigtimedwait"): + continue + if os.name == "posix" and signal.sigtimedwait(self.killer.signals, 0): self.stop() @@ -402,7 +405,7 @@ def _next_consistent_dt(self) -> float: # Busy wait to compensate for sleep durations precision time_to_busy_wait = time.monotonic() + PRECISION_OF_SLEEP while time.monotonic() < time_to_busy_wait and not self.killer.kill_now: - if os.name == "posix" and signal.sigtimedwait(self.killer.signals, 0): + if os.name == "posix" and hasattr(signal, "sigtimedwait") and signal.sigtimedwait(self.killer.signals, 0): self.stop() raise StopIteration diff --git a/tutorials/actuators/tmotor/position_control.py b/tutorials/actuators/tmotor/position_control.py new file mode 100644 index 00000000..ab1037ae --- /dev/null +++ b/tutorials/actuators/tmotor/position_control.py @@ -0,0 +1,82 @@ +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 + +TIME_TO_STEP = 2.0 +FREQUENCY = 200 +DT = 1 / FREQUENCY +MOTOR_ID = 104 # Change this to match your motor's CAN ID + + +def position_control(): + position_logger = Logger( + log_path="./logs", + file_name="tmotor_position_control", + ) + + # Initialize TMotor actuator + motor = TMotorServoActuator( + motor_type="AK80-9", # Change to your motor model + motor_id=MOTOR_ID, + offline=False, + ) + + clock = SoftRealtimeLoop(dt=DT) + + with motor: + print(f"Connected to TMotor: {motor.device_info_string()}") + + # Home the motor first + print("Homing motor...") + motor.home() + + # Set to position control mode (PID parameters are built-in) + motor.set_control_mode(mode=CONTROL_MODES.POSITION) + + motor.update() + initial_position = motor.motor_position + command_position = initial_position + + position_logger.track_function(lambda: motor.motor_position, "Motor Position") + position_logger.track_function(lambda: command_position, "Command Position") + position_logger.track_function(lambda: time.time(), "Time") + + print("Starting position control...") + for t in clock: + if t > TIME_TO_STEP: + # Step to a new position + command_position = initial_position + (np.pi / 8) # 22.5 degrees + motor.set_motor_position(value=command_position) + else: + motor.set_motor_position(value=command_position) + + motor.update() + + position_logger.info( + f"Time: {t:.3f}; " + f"Command Position: {command_position:.3f} rad; " + f"Motor Position: {motor.motor_position:.3f} rad; " + f"Error: {command_position - motor.motor_position:.4f} rad" + ) + position_logger.update() + + # Run for 5 seconds total + if t > 5.0: + break + + print("Position control complete") + + # Return to home position + print("Returning to home...") + motor.set_motor_position(value=initial_position) + time.sleep(1.0) + motor.update() + + +if __name__ == "__main__": + position_control() diff --git a/tutorials/actuators/tmotor/reading_sensor_data.py b/tutorials/actuators/tmotor/reading_sensor_data.py new file mode 100644 index 00000000..3b4398ab --- /dev/null +++ b/tutorials/actuators/tmotor/reading_sensor_data.py @@ -0,0 +1,51 @@ +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 + +if __name__ == "__main__": + sensor_logger = Logger( + log_path="./logs", + file_name="tmotor_sensor_data", + ) + clock = SoftRealtimeLoop(dt=DT) + + # Initialize TMotor actuator + motor = TMotorServoActuator( + motor_type="AK80-9", # Change to your motor model + motor_id=MOTOR_ID, + offline=False, + ) + + # Track sensor data + sensor_logger.track_function(lambda: motor.motor_position, "Motor Position") + sensor_logger.track_function(lambda: motor.motor_velocity, "Motor Velocity") + sensor_logger.track_function(lambda: motor.motor_current, "Motor Current") + sensor_logger.track_function(lambda: motor.motor_torque, "Motor Torque") + sensor_logger.track_function(lambda: motor.case_temperature, "Case Temperature") + sensor_logger.track_function(lambda: motor.winding_temperature, "Winding Temperature") + + with motor: + print(f"Connected to TMotor: {motor.device_info_string()}") + print("Reading sensor data...") + + for t in clock: + motor.update() + + sensor_logger.info( + f"Time: {t:.3f}; " + f"Position: {motor.motor_position:.3f} rad; " + f"Velocity: {motor.motor_velocity:.2f} rad/s; " + f"Current: {motor.motor_current:.2f} A; " + f"Torque: {motor.motor_torque:.3f} Nm" + ) + sensor_logger.update() + + # Stop after 5 seconds + if t > 5.0: + break + + print("Sensor data logging complete") diff --git a/tutorials/actuators/tmotor/torque_control.py b/tutorials/actuators/tmotor/torque_control.py new file mode 100644 index 00000000..d2e0af24 --- /dev/null +++ b/tutorials/actuators/tmotor/torque_control.py @@ -0,0 +1,95 @@ +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, + offline=False, + ) + + clock = SoftRealtimeLoop(dt=DT) + + with motor: + print(f"Connected to TMotor: {motor.device_info_string()}") + + # Home the motor first + print("Homing motor...") + motor.home() + + # Set to current control mode for torque control + motor.set_control_mode(mode=CONTROL_MODES.CURRENT) + + motor.update() + + # Motor torque constant for AK80-9 + MAX_OUTPUT_TORQUE = 2.0 # Maximum output torque in Nm (safety limit) + + # Initialize command torque variable before tracking (needed for lambda) + command_output_torque = 0.0 + + # 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_output_torque, "Command Output Torque") + torque_logger.track_function(lambda: motor.motor_current, "Motor Current") + torque_logger.track_function(lambda: motor.motor_position, "Motor Position") + torque_logger.track_function(lambda: motor.motor_velocity, "Motor Velocity") + torque_logger.track_function(lambda: time.time(), "Time") + + print("Starting torque control...") + + # Create a torque profile + for t in clock: + # Sinusoidal output torque command (0.2 Hz, amplitude 0.5 Nm at output) + command_output_torque = 0.5 * np.sin(2 * np.pi * 0.2 * t) + + # Limit output torque for safety + command_output_torque = np.clip(command_output_torque, -MAX_OUTPUT_TORQUE, MAX_OUTPUT_TORQUE) + + # Set output torque (automatically handles gear ratio conversion) + motor.set_output_torque(command_output_torque) + motor.update() + + torque_logger.info( + f"Time: {t:.3f}; " + f"Command Output Torque: {command_output_torque:.3f} Nm; " + f"Output Torque: {motor.output_torque:.3f} Nm; " + f"Motor Torque: {motor.motor_torque:.3f} Nm; " + f"Current: {motor.motor_current:.2f} A; " + f"Velocity: {motor.motor_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_torque(0.0) + motor.update() + time.sleep(1.0) + + +if __name__ == "__main__": + torque_control() diff --git a/tutorials/actuators/tmotor/velocity_control.py b/tutorials/actuators/tmotor/velocity_control.py new file mode 100644 index 00000000..976bc78d --- /dev/null +++ b/tutorials/actuators/tmotor/velocity_control.py @@ -0,0 +1,83 @@ +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 velocity_control(): + velocity_logger = Logger( + log_path="./logs", + file_name="tmotor_velocity_control", + ) + + # Initialize TMotor actuator + motor = TMotorServoActuator( + motor_type="AK80-9", # Change to your motor model + motor_id=MOTOR_ID, + offline=False, + ) + + clock = SoftRealtimeLoop(dt=DT) + + with motor: + print(f"Connected to TMotor: {motor.device_info_string()}") + + # Home the motor first + print("Homing motor...") + motor.home() + + # Set to velocity control mode (PID parameters are built-in) + motor.set_control_mode(mode=CONTROL_MODES.VELOCITY) + + motor.update() + + # Initialize command velocity variable before tracking (needed for lambda) + command_velocity = 0.0 + + # Track velocity data + velocity_logger.track_function(lambda: motor.motor_velocity, "Motor Velocity") + velocity_logger.track_function(lambda: command_velocity, "Command Velocity") + velocity_logger.track_function(lambda: motor.motor_position, "Motor Position") + velocity_logger.track_function(lambda: time.time(), "Time") + + print("Starting velocity control...") + + # Create a sinusoidal velocity profile + for t in clock: + # Sinusoidal velocity command (0.5 Hz, amplitude 0.5 rad/s) + command_velocity = 0.5 * np.sin(2 * np.pi * 0.5 * t) + + motor.set_motor_velocity(value=command_velocity) + motor.update() + + velocity_logger.info( + f"Time: {t:.3f}; " + f"Command Velocity: {command_velocity:.3f} rad/s; " + f"Motor Velocity: {motor.motor_velocity:.3f} rad/s; " + f"Position: {motor.motor_position:.3f} rad" + ) + velocity_logger.update() + + # Run for 10 seconds to see multiple cycles + if t > 10.0: + break + + print("Velocity control complete") + + # Stop the motor + print("Stopping motor...") + motor.set_motor_velocity(value=0.0) + motor.update() + time.sleep(1.0) + + +if __name__ == "__main__": + velocity_control() diff --git a/uv.lock b/uv.lock index 72157c29..06a66d0b 100644 --- a/uv.lock +++ b/uv.lock @@ -917,7 +917,7 @@ wheels = [ [[package]] name = "opensourceleg" -version = "3.4.1" +version = "3.4.2" source = { editable = "." } dependencies = [ { name = "numpy" },