From 3f01ee15daa93e5b443dda01151973494c89441c Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Thu, 22 Jan 2026 11:44:31 -0500 Subject: [PATCH 01/27] fixed reading case temperature from actuator --- opensourceleg/actuators/base.py | 23 +- opensourceleg/actuators/tmotor.py | 1935 ++++++++++++++++++----------- 2 files changed, 1225 insertions(+), 733 deletions(-) diff --git a/opensourceleg/actuators/base.py b/opensourceleg/actuators/base.py index 01f740f6..f67f8469 100644 --- a/opensourceleg/actuators/base.py +++ b/opensourceleg/actuators/base.py @@ -43,10 +43,13 @@ class MOTOR_CONSTANTS: """ MOTOR_COUNT_PER_REV: float - NM_PER_AMP: float # Motor torque constant (Nm/A) + NM_PER_AMP: float + NM_PER_RAD_TO_K: float + NM_S_PER_RAD_TO_B: float MAX_CASE_TEMPERATURE: float # Hard limit for case/housing temperature MAX_WINDING_TEMPERATURE: float # Hard limit for winding temperature + # 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) @@ -68,6 +71,8 @@ def __post_init__(self) -> None: >>> MOTOR_CONSTANTS( ... MOTOR_COUNT_PER_REV=-2048, ... NM_PER_AMP=0.02, + ... NM_PER_RAD_TO_K=0.001, + ... NM_S_PER_RAD_TO_B=0.0001, ... MAX_CASE_TEMPERATURE=80.0, ... MAX_WINDING_TEMPERATURE=120.0 ... ) @@ -1005,22 +1010,6 @@ def MOTOR_CONSTANTS(self) -> MOTOR_CONSTANTS: """ return self._MOTOR_CONSTANTS - @MOTOR_CONSTANTS.setter - def MOTOR_CONSTANTS(self, value: MOTOR_CONSTANTS) -> None: - """ - Set the motor constants configuration. - - Args: - value (MOTOR_CONSTANTS): The new motor constants to set. - - Examples: - >>> new_constants = MOTOR_CONSTANTS(2048, 0.03, 0.001, 0.0001, 85.0, 125.0) - >>> actuator.MOTOR_CONSTANTS = new_constants - >>> actuator.MOTOR_CONSTANTS.MAX_CASE_TEMPERATURE - 85.0 - """ - self._MOTOR_CONSTANTS = value - @property def mode(self) -> CONTROL_MODES: """ diff --git a/opensourceleg/actuators/tmotor.py b/opensourceleg/actuators/tmotor.py index 7a6b99cf..5b69c06f 100644 --- a/opensourceleg/actuators/tmotor.py +++ b/opensourceleg/actuators/tmotor.py @@ -1,716 +1,1219 @@ -import time -import warnings -from math import isfinite -from typing import Callable, Optional - -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, - CONTROL_MODES, - MOTOR_CONSTANTS, - ActuatorBase, - ControlModeConfig, -) -from opensourceleg.actuators.decorators import ( - check_actuator_connection, - check_actuator_open, - check_actuator_stream, -) -from opensourceleg.logging import LOGGER -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, -) - - -def _tmotor_impedance_mode_exit(tmotor_actuator: "TMotorMITCANActuator") -> None: - tmotor_actuator.stop_motor() - - -def _tmotor_current_mode_exit(tmotor_actuator: "TMotorMITCANActuator") -> None: - tmotor_actuator.stop_motor() - - -def _tmotor_velocity_mode_exit(tmotor_actuator: "TMotorMITCANActuator") -> None: - tmotor_actuator.stop_motor() - - -TMOTOR_CONTROL_MODE_CONFIGS = CONTROL_MODE_CONFIGS( - IMPEDANCE=ControlModeConfig( - entry_callback=lambda _: None, - exit_callback=_tmotor_impedance_mode_exit, - has_gains=False, - max_gains=None, - ), - CURRENT=ControlModeConfig( - entry_callback=lambda _: None, - exit_callback=_tmotor_current_mode_exit, - has_gains=False, - max_gains=None, - ), - VELOCITY=ControlModeConfig( - entry_callback=lambda _: None, - exit_callback=_tmotor_velocity_mode_exit, - has_gains=False, - max_gains=None, - ), -) - - -# the user-facing class that manages the motor. -class TMotorMITCANActuator(ActuatorBase, TMotorManager_mit_can): - """ - 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. - """ - - def __init__( - self, - tag: str = "TMotorActuator", - motor_type: str = "AK80-9", - motor_ID: int = 41, - gear_ratio: float = 1.0, - frequency: int = 500, - offline: bool = False, - max_mosfett_temp: float = 50, - ): - """ - 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. - - 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=tag, - gear_ratio=gear_ratio, - motor_constants=TMOTOR_ACTUATOR_CONSTANTS, - frequency=frequency, - offline=offline, - ) - 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 - 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._thermal_model: ThermalModel = 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) - - @property - def _CONTROL_MODE_CONFIGS(self) -> CONTROL_MODE_CONFIGS: - return TMOTOR_CONTROL_MODE_CONFIGS - - @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()) - - 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 - - @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) - 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"]) - 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, - ) - else: - raise RuntimeError("UNDEFINED STATE for device " + self.device_info_string()) - self._last_command_time = time.time() - - # 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 - - @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 - - @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( - self, - K: float = 0.08922, - B: float = 0.0038070, - ) -> None: - """ - Uses plain impedance mode, will send 0.0 for current command in addition to position request. - - 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']}" - ) - - 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']}" - ) - - self._command.kp = K - self._command.kd = B - self._command.velocity = 0.0 - - 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. - - 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 - - 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. - - Args: - kd: The gain for the speed controller. Control law will be (v_des - v_actual)*kd = iq - """ - self._command.kd = kd - - def set_position_gains(self) -> None: - # Not implemented - pass - - # 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. - - Args: - value: The desired output position in rads - - 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!" - ) - - self._command.position = value - - 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"]) - - # 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"]) - - def set_motor_position(self, value: float) -> None: - """ - Wrapper for set_output_angle that accounts for gear ratio to control motor-side angle - - Args: - value: The desired motor position in rad. - """ - self.set_output_position(value / (MIT_Params[self.type]["GEAR_RATIO"])) - - def set_motor_velocity(self, value: float) -> None: - """ - Wrapper for set_output_velocity that accounts for gear ratio to control motor-side velocity - - Args: - value: The desired motor velocity in rad/s. - """ - self.set_output_velocity(value / (MIT_Params[self.type]["GEAR_RATIO"])) - - def set_motor_voltage(self, value: float) -> float: - # Not implemented - pass - - @property - def motor_position(self) -> float: - """ - Wrapper for get_output_angle that accounts for gear ratio to get motor-side angle - - Returns: - The most recently updated motor-side angle in rad. - """ - return float(self._motor_state.position * MIT_Params[self.type]["GEAR_RATIO"]) - - @property - def motor_velocity(self) -> float: - """ - Wrapper for get_output_velocity that accounts for gear ratio to get motor-side velocity - - 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 motor_acceleration(self) -> float: - """ - Wrapper for get_output_acceleration that accounts for gear ratio to get motor-side acceleration - - 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_torque(self) -> float: - """ - Wrapper for get_output_torque that accounts for gear ratio to get motor-side torque - - Returns: - The most recently updated motor-side torque in Nm. - """ - return float(self.output_torque * MIT_Params[self.type]["GEAR_RATIO"]) - - # Pretty stuff - def __str__(self) -> str: - """Prints the motor's device info and current""" - 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" - ) - - # 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( - "Actual: ", - round(dev.output_position, 3), - "Desired: ", - dev._command.position, - ) - - del loop +import time +import traceback +import warnings +from dataclasses import dataclass +from enum import Enum +from typing import Any, Optional, cast, Callable + +import can +import numpy as np +import subprocess + + +from opensourceleg.actuators.base import ( + CONTROL_MODE_CONFIGS, + CONTROL_MODES, + MOTOR_CONSTANTS, + ActuatorBase, + ControlModeConfig, +) +from opensourceleg.actuators.decorators import ( + check_actuator_connection, + check_actuator_open, + check_actuator_stream, +) +from opensourceleg.logging import LOGGER +from opensourceleg.math import ThermalModel +from opensourceleg.utilities import SoftRealtimeLoop + +# 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": -3200, # -3200 deg + "P_max": 3200, # 3200 deg + "V_min": -100000, # ERPM + "V_max": 100000, # ERPM + "Curr_min": -60000, # -60A + "Curr_max": 60000, # 60A + "Kt_actual": 0.206, # Nm/A + "GEAR_RATIO": 9.0, + "NUM_POLE_PAIRS": 21, + }, + "AK80-9": { + "P_min": -3200, # -3200 deg + "P_max": 3200, # 3200 deg + "V_min": -32000, # ERPM + "V_max": 32000, # ERPM + "Curr_min": -60000, # -60A + "Curr_max": 60000, # 60A + "Kt_actual": 0.095, # 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.095, # Placeholder to satisfy validation + # 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, # Temperature parameters are also set in the driver via R-Link + MAX_WINDING_TEMPERATURE=110.0, # Temperature parameters are also set in the driver via R-Link + # Soft limits set 10°C below hard limits for safety margin + WINDING_SOFT_LIMIT=100.0, + CASE_SOFT_LIMIT=70.0, +) + + +@dataclass +class ServoMotorState: + """Motor state data structure""" + + position: float = 0.0 # degrees + velocity: float = 0.0 # ERPM + current: float = 0.0 # milliamps + 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=[]) + self._listeners = [] # Track active 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 __enter__(self) -> None: + """Context manager enter""" + return self + + def __exit__(self, exc_type, exc_val, exc_tb) -> None: + """ Context manager exit""" + self.close() + + def __del__(self) -> None: + self.close() + + def close(self) -> None: + """ + Close CAN service + """ + try: + LOGGER.info("Closing CAN Manager...") + + # Stop and remove all listeners first + if hasattr(self, "_listeners"): + for listener in self._listeners: + try: + self.notifier.remove_listener(listener) + except Exception as e: + LOGGER.warning(f"Error removing listener: {e}") + self._listeners.clear() + + # Stop the notifier + if hasattr(self, "notifier"): + self.notifier.stop() + # Give notifier time to stop cleanly + time.sleep(0.05) + + # Shutdown the bus + if hasattr(self, "bus"): + self.bus.shutdown() + + LOGGER.info("CAN Manager closed successfully") + + except Exception as e: + LOGGER.warning(f"Error closing CAN manager: {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, motor_id: int, current: float) -> None: + """ + Send current control command + Args: + motor_id (int): CAN motor ID + current (float): motor current in mA + Returns: + None + """ + buffer = self._pack_int32(int(current)) + message_id = (CAN_PACKET_ID["SET_CURRENT"] << 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 + Args: + motor_id (int): CAN motor ID + velocity (float): motor velocity in erpm + Returns: + None + """ + buffer = self._pack_int32(int(velocity)) + message_id = (CAN_PACKET_ID["SET_RPM"] << 8) | motor_id + self.send_message(message_id, buffer, len(buffer)) + + def set_position(self, motor_id: int, position: float) -> None: + """ + Send position control command + Args: + motor_id (int): CAN motor ID + position (float): motor position in degrees + Returns: + None + """ + # 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) | motor_id + self.send_message(message_id, buffer, len(buffer)) + + def set_origin(self, motor_id: int, mode: int = 1) -> None: + """ + Set motor origin + Args: + motor_id (int): CAN motor ID + mode (int): 0 represents setting a temporary origin (erased upon power loss) + 1 represents setting a permanent origin (parameters are automatically saved) + Returns: + None + """ + buffer = [mode] + message_id = (CAN_PACKET_ID["SET_ORIGIN_HERE"] << 8) | motor_id + self.send_message(message_id, buffer, len(buffer)) + + def set_control_mode(self, motor_id: int, mode: int) -> None: + """ + Send control mode switch command + Args: + motor_id (int): CAN motor ID + mode (int): represents control mode (e.g., position, current, velocity, idle) + Returns: + None + """ + buffer = [mode] + message_id = (7 << 8) | motor_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 * 10) # current (mA) + motor_temp = float(int.from_bytes(data[6:7], byteorder='big', signed=True)) # 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) + self._listeners.append(listener) + LOGGER.debug(f"Added listener for motor ID {motor.motor_id}") + + + def remove_motor_listener(self, motor: "TMotorServoActuator") -> None: + """Remove motor listener""" + for listener in self._listeners[:]: # Create a copy to iterate + if listener.motor == motor: + try: + self.notifier.remove_listener(listener) + self._listeners.remove(listener) + LOGGER.debug(f"Removed listener for motor ID {motor.motor_id}") + except Exception as e: + LOGGER.warning(f"Error removing listener for motor {motor.motor_id}: {e}") + + + def enable_debug(self, enable: bool = True) -> None: + """Enable/disable debug mode""" + self.debug = enable + if enable: + LOGGER.info("CAN debug mode enabled") + + +class MotorListener(can.Listener): + """CAN message listener""" + + def __init__(self, canman: CANManagerServo, motor: "TMotorServoActuator") -> None: + self.canman = canman + self.motor = 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)) + + + +# 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=_servo_current_mode_entry, + exit_callback=_servo_current_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, + 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, + ), +) + + +class TMotorServoActuator(ActuatorBase): + """ + 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 = "TMotorServoActuator", + motor_type: str = "AK80-9", + motor_id: int = 104, + gear_ratio: float = 1.0, + frequency: int = 1000, + offline: bool = False, + current_mode: str = "driver", + **kwargs: Any, + ) -> None: + """ + Initialize TMotor servo actuator + + Args: + 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 + 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 gear ratio matches tmotor model + if TMOTOR_MODELS[motor_type]["GEAR_RATIO"] != gear_ratio: + raise ValueError(f"Gear ratio {gear_ratio} does not match {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__( + self, + tag=tag, + gear_ratio=gear_ratio, + motor_constants=TMOTOR_SERVO_CONSTANTS, + frequency=frequency, + offline=offline, + ) + + # Override motor constants to ensure setter is called + self.MOTOR_CONSTANTS = TMOTOR_SERVO_CONSTANTS + + # Motor configuration + self.motor_type = motor_type + self.motor_id = motor_id + 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: Optional[float] = None + + # Thermal management + self._thermal_model = ThermalModel( + motor_constants = self._MOTOR_CONSTANTS, + actuator_tag = self.tag, + ) + + LOGGER.info(f"Initialized TMotor servo: {self.motor_type} ID:{self.motor_id}") + + def __str__(self) -> str: + """State string""" + return f"{self.motor_type} ID:{self.motor_id}" + + @property + def _CONTROL_MODE_CONFIGS(self) -> 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) -> None: + """ + Starts the actuator by powering on the device via CAN, setting control mode + to IDLE, and checking error status. + + Returns: + None + """ + LOGGER.info(f"Starting {self.motor_type} ID:{self.motor_id}") + + 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._is_open = True + self._is_streaming = True + + @check_actuator_stream + @check_actuator_open + def stop(self) -> None: + """Stop motor and clean up CAN communication""" + LOGGER.info(f"Stopping {self.motor_type} ID:{self.motor_id}") + + if not self.is_offline and self._canman: + # Set motor to idle mode first + try: + self._canman.set_current(self.motor_id, 0.0) + time.sleep(0.01) + self._canman.set_control_mode(self.motor_id, ServoControlMode.IDLE.value) + time.sleep(0.05) # Give motor time to process + except Exception as e: + LOGGER.warning(f"Error setting idle mode during stop: {e}") + + # Send power off command + try: + self._canman.power_off(self.motor_id) + time.sleep(0.05) # Give motor time to power off + except Exception as e: + LOGGER.warning(f"Error powering off motor: {e}") + + # Remove this motor's listener + self._canman.remove_motor_listener(self) + + # Close CAN manager (will only actually close if this is the last motor) + self._canman.close() + + self._is_open = False + self._is_streaming = False + + def update(self) -> None: + """ + Updates the actuator's data by reading new values and updating the thermal model. + It raises exceptions if thermal limits are exceeded. + + Returns: + None + """ + # 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 + + # Temperature check + self._thermal_scale = self._thermal_model.update( + dt = 1 / self.frequency, + motor_current = self.motor_current, + case_temperature = self.case_temperature, + ) + + # Communication timeout check + now = time.time() + 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 and handle errors. + + Returns: + None + """ + # 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: + LOGGER.warning(f"Motor error {servo_state.error}: {error_msg}") + else: + self._error_code = None + self._error_message = None + + self._motor_state_async = servo_state + + @property + 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 + + def home(self) -> None: + """ + This method homes the actuator and the corresponding joint by moving it to the zero position. + The zero position is defined as the position where the joint is fully extended. + Returns: + None + """ + self._is_homed = False + raise NotImplementedError("Homing not implemented.") + + def set_origin(self) -> None: + """ + Set encoder origin - automatically applies a "zero position" offset. + """ + if not self.is_offline and self._canman: + self._canman.set_origin(self.motor_id, 1) + time.sleep(0.1) + LOGGER.info(f"Set origin {self.device_info_string()}") + + @property + def MOTOR_CONSTANTS(self) -> MOTOR_CONSTANTS: + """ + Get the motor constants configuration. + Redefines the property from the ABC so we can set a setter. + + Returns: + MOTOR_CONSTANTS: The motor constants. + + Examples: + >>> constants = actuator.MOTOR_CONSTANTS + >>> constants.MAX_CASE_TEMPERATURE + 80.0 + """ + return self._MOTOR_CONSTANTS + + @MOTOR_CONSTANTS.setter + def MOTOR_CONSTANTS(self, value: MOTOR_CONSTANTS) -> None: + """ + Setter for MOTOR_CONSTANTS property. + Updates the motor constants and recalculates derived conversion factors. + + Args: + value (MOTOR_CONSTANTS): New motor constants to set. + """ + + if not isinstance(value, MOTOR_CONSTANTS): + raise TypeError(f"Expected MOTOR_CONSTANTS, got {type(value)}") + self._MOTOR_CONSTANTS = value + self._update_derived_constants() + + # ============ Control Interface ============ + + 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") + + def set_motor_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 / self._current_scale + + # Get current limits from motor parameters + motor_params = cast(dict[str, Any], self._motor_params) + max_current = motor_params["Curr_max"] + min_current = motor_params["Curr_min"] + + # 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_current(self.motor_id, clamped_driver_current) + self._last_command_time = time.time() + + def set_motor_position(self, value: float) -> None: + """ + Set motor position (radians) with clamping to motor limits + Args: + value (float): desired motor position + Return: + None + """ + position_deg = radians_to_degrees(value) + + 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"] + min_position = motor_params["P_min"] + + # Clamp position to safe limits + clamped_position = np.clip(position_deg, min_position, max_position) + + # 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}]°)" + ) + + self._canman.set_position(self.motor_id, clamped_position) + self._last_command_time = time.time() + + def set_motor_torque(self, value: float) -> None: + """ + Sets the motor 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 + """ + # 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 * 1000 + self.set_motor_current(current) # Send current command mA + + def set_output_torque(self, value: float) -> None: + """ + Set the output 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 + """ + # Output torque to motor torque: T_motor = T_output / gear_ratio + motor_torque = value / self.gear_ratio + self.set_motor_torque(motor_torque) + + 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.") + + 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: + """Set output velocity (rad/s)""" + motor_velocity = value * self.gear_ratio + self.set_motor_velocity(motor_velocity) + + # ============ Unsupported PID Functions - TMotor Servo Mode Handles All Control Loops Internally ============ + + 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 + + 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_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 + + 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 + + # ============ State Properties ============ + + @property + def motor_position(self) -> float: + """Motor position (radians) - not supported""" + raise NotImplementedError( + "Motor position reading is not available. " + "The position returned in the motor state corresponds to the output position." + ) + + @property + def output_position(self) -> float: + """Output position (radians)""" + return degrees_to_radians(self._motor_state.position) + + @property + def motor_velocity(self) -> float: + """Motor velocity (rad/s)""" + return erpm_to_rad_per_sec(self._motor_state.velocity, self.num_pole_pairs) + + @property + def output_velocity(self) -> float: + """Output velocity (rad/s)""" + return self.motor_velocity / self.gear_ratio + + @property + 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"] + + @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 in mA + + Returns: + float: Motor current in mA + """ + return self._motor_state.current * self._current_scale + + @property + def motor_torque(self) -> float: + """ + Torque at the motor in Nm. + This is calculated using motor current and k_t. + """ + 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 / 1000 + + @property + def output_torque(self) -> float: + """ + Torque at the joint output in Nm. + This is calculated using motor current, k_t, and the gear ratio. + """ + return self.motor_torque * self.gear_ratio + + @property + def case_temperature(self) -> float: + """ + Case temperature in degrees Celsius. This is read during actuator update. + + Returns: + float: Case temperature in degrees Celsius. + """ + return 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. + + Returns: + float: Winding temperature in degrees Celsius. + """ + return float(self._thermal_model.winding_temperature) + + @property + def thermal_scaling_factor(self) -> float: + """ + Scale factor to use in torque control, in [0,1]. + If you scale the torque command by this factor, the motor temperature will never + exceed max allowable temperature. For a proof, see paper referenced in thermal model. + + Returns: + float: Thermal scaling factor. + """ + return self._thermal_scale + + @property + def is_streaming(self) -> bool: + return self._is_streaming + + @is_streaming.setter + def is_streaming(self, value: bool) -> None: + self._is_streaming = value + + @property + def is_open(self) -> bool: + return self._is_open + + @is_open.setter + def is_open(self, value: bool) -> None: + self._is_open = value + + +if __name__ == "__main__": + 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.__str__()}") + + # Set origin and set current control mode + actuator.set_origin() + 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}mA | " 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( + 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() + + print("Current loop control example completed!") From ae4236819184d385aaaa79cf0c432b5493298558 Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Thu, 22 Jan 2026 12:56:01 -0500 Subject: [PATCH 02/27] reverted to most recent base.py version --- opensourceleg/actuators/base.py | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/opensourceleg/actuators/base.py b/opensourceleg/actuators/base.py index f67f8469..895e8cb4 100644 --- a/opensourceleg/actuators/base.py +++ b/opensourceleg/actuators/base.py @@ -43,13 +43,10 @@ class MOTOR_CONSTANTS: """ MOTOR_COUNT_PER_REV: float - NM_PER_AMP: float - NM_PER_RAD_TO_K: float - NM_S_PER_RAD_TO_B: float + NM_PER_AMP: float # Motor torque constant (Nm/A) MAX_CASE_TEMPERATURE: float # Hard limit for case/housing temperature MAX_WINDING_TEMPERATURE: float # Hard limit for winding temperature - # 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) @@ -71,8 +68,6 @@ def __post_init__(self) -> None: >>> MOTOR_CONSTANTS( ... MOTOR_COUNT_PER_REV=-2048, ... NM_PER_AMP=0.02, - ... NM_PER_RAD_TO_K=0.001, - ... NM_S_PER_RAD_TO_B=0.0001, ... MAX_CASE_TEMPERATURE=80.0, ... MAX_WINDING_TEMPERATURE=120.0 ... ) @@ -1010,6 +1005,22 @@ def MOTOR_CONSTANTS(self) -> MOTOR_CONSTANTS: """ return self._MOTOR_CONSTANTS + @MOTOR_CONSTANTS.setter + def MOTOR_CONSTANTS(self, value: MOTOR_CONSTANTS) -> None: + """ + Set the motor constants configuration. + + Args: + value (MOTOR_CONSTANTS): The new motor constants to set. + + Examples: + >>> new_constants = MOTOR_CONSTANTS(2048, 0.03, 0.001, 0.0001, 85.0, 125.0) + >>> actuator.MOTOR_CONSTANTS = new_constants + >>> actuator.MOTOR_CONSTANTS.MAX_CASE_TEMPERATURE + 85.0 + """ + self._MOTOR_CONSTANTS = value + @property def mode(self) -> CONTROL_MODES: """ @@ -1187,4 +1198,4 @@ def is_streaming(self) -> bool: >>> actuator.is_streaming True """ - return self._is_streaming + return self._is_streaming \ No newline at end of file From e87465c76168bd6fb61744a71434222ca4021dcb Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Thu, 22 Jan 2026 13:00:29 -0500 Subject: [PATCH 03/27] removed un-used params from MOTOR_CONSTANTS --- opensourceleg/actuators/tmotor.py | 25 ++----------------------- 1 file changed, 2 insertions(+), 23 deletions(-) diff --git a/opensourceleg/actuators/tmotor.py b/opensourceleg/actuators/tmotor.py index 5b69c06f..0d1ded77 100644 --- a/opensourceleg/actuators/tmotor.py +++ b/opensourceleg/actuators/tmotor.py @@ -90,8 +90,6 @@ TMOTOR_SERVO_CONSTANTS = MOTOR_CONSTANTS( MOTOR_COUNT_PER_REV=65536, # Encoder counts per revolution (16-bit encoder) NM_PER_AMP=0.095, # Placeholder to satisfy validation - # 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, # Temperature parameters are also set in the driver via R-Link MAX_WINDING_TEMPERATURE=110.0, # Temperature parameters are also set in the driver via R-Link # Soft limits set 10°C below hard limits for safety margin @@ -99,7 +97,6 @@ CASE_SOFT_LIMIT=70.0, ) - @dataclass class ServoMotorState: """Motor state data structure""" @@ -559,7 +556,7 @@ def __init__( f"Invalid current_mode: {current_mode}. Must be 'driver', 'amplitude-invariant', or 'power-invariant'" ) - super().__init__( + ActuatorBase.__init__( self, tag=tag, gear_ratio=gear_ratio, @@ -847,8 +844,7 @@ def MOTOR_CONSTANTS(self, value: MOTOR_CONSTANTS) -> None: if not isinstance(value, MOTOR_CONSTANTS): raise TypeError(f"Expected MOTOR_CONSTANTS, got {type(value)}") self._MOTOR_CONSTANTS = value - self._update_derived_constants() - + # ============ Control Interface ============ def set_motor_voltage(self, value: float) -> None: @@ -1113,23 +1109,6 @@ def thermal_scaling_factor(self) -> float: """ return self._thermal_scale - @property - def is_streaming(self) -> bool: - return self._is_streaming - - @is_streaming.setter - def is_streaming(self, value: bool) -> None: - self._is_streaming = value - - @property - def is_open(self) -> bool: - return self._is_open - - @is_open.setter - def is_open(self, value: bool) -> None: - self._is_open = value - - if __name__ == "__main__": print("TMotor Current Loop Control Example") From 76708369005a83ba116bf36b6225adbca79bcb5c Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Thu, 22 Jan 2026 13:47:57 -0500 Subject: [PATCH 04/27] reformatted setting control mode to take follow base ControlModeConfig format --- opensourceleg/actuators/tmotor.py | 88 ++++++++++--------------------- 1 file changed, 28 insertions(+), 60 deletions(-) diff --git a/opensourceleg/actuators/tmotor.py b/opensourceleg/actuators/tmotor.py index 0d1ded77..8c84bb82 100644 --- a/opensourceleg/actuators/tmotor.py +++ b/opensourceleg/actuators/tmotor.py @@ -100,7 +100,6 @@ @dataclass class ServoMotorState: """Motor state data structure""" - position: float = 0.0 # degrees velocity: float = 0.0 # ERPM current: float = 0.0 # milliamps @@ -396,68 +395,51 @@ def _wait_for_mode_switch(actuator: "TMotorServoActuator", timeout: float = 0.2) # 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 ============ -# Control mode configuration def _servo_position_mode_entry(actuator: "TMotorServoActuator") -> None: - actuator._servo_mode = ServoControlMode.POSITION - # Send actual mode switch command to motor + LOGGER.debug(msg=f"[{actuator.__str__()}] Entering Position control mode.") + mode_id = 4 if not actuator.is_offline and actuator._canman: - actuator._canman.set_control_mode(actuator.motor_id, ServoControlMode.POSITION.value) + actuator._canman.set_control_mode(actuator.motor_id, mode_id) _wait_for_mode_switch(actuator) - def _servo_position_mode_exit(actuator: "TMotorServoActuator") -> None: - pass # servo mode handles automatically - + LOGGER.debug(msg=f"[{actuator.__str__()}] Exiting Position control mode.") def _servo_current_mode_entry(actuator: "TMotorServoActuator") -> None: - actuator._servo_mode = ServoControlMode.CURRENT - # Send actual mode switch command to motor + LOGGER.debug(msg=f"[{actuator.__str__()}] Entering Current control mode.") + mode_id = 1 if not actuator.is_offline and actuator._canman: - actuator._canman.set_control_mode(actuator.motor_id, ServoControlMode.CURRENT.value) + actuator._canman.set_control_mode(actuator.motor_id, mode_id) _wait_for_mode_switch(actuator) - def _servo_current_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: - actuator._servo_mode = ServoControlMode.VELOCITY - # Send actual mode switch command to motor + LOGGER.debug(msg=f"[{actuator.__str__()}] Entering Velocity control mode.") + mode_id = 3 if not actuator.is_offline and actuator._canman: - actuator._canman.set_control_mode(actuator.motor_id, ServoControlMode.VELOCITY.value) + actuator._canman.set_control_mode(actuator.motor_id, mode_id) _wait_for_mode_switch(actuator) - def _servo_velocity_mode_exit(actuator: "TMotorServoActuator") -> None: + LOGGER.debug(msg=f"[{actuator.__str__()}] Exiting Velocity control mode.") 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 + LOGGER.debug(msg=f"[{actuator.__str__()}] Entering Idle control mode.") + mode_id = 7 if not actuator.is_offline and actuator._canman: - actuator._canman.set_control_mode(actuator.motor_id, ServoControlMode.IDLE.value) + actuator._canman.set_control_mode(actuator.motor_id, mode_id) _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." - ) + LOGGER.debug(msg=f"[{actuator.__str__()}] Exiting Idle control mode.") TMOTOR_SERVO_CONTROL_MODE_CONFIGS = CONTROL_MODE_CONFIGS( @@ -479,28 +461,18 @@ def _impedance_not_supported(actuator: "TMotorServoActuator") -> None: 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, - ), + IMPEDANCE=None, # IMPEDANCE mode not supported + VOLTAGE=None, # VOLTAGE mode not supported ) + class TMotorServoActuator(ActuatorBase): """ TMotor servo mode actuator for AK series motors. @@ -603,7 +575,6 @@ def __init__( # State management self._motor_state = ServoMotorState() self._motor_state_async = ServoMotorState() - self._servo_mode = ServoControlMode.IDLE # Error handling self._error_code: Optional[int] = None @@ -630,9 +601,6 @@ def __str__(self) -> str: def _CONTROL_MODE_CONFIGS(self) -> 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) -> None: """ @@ -642,7 +610,7 @@ def start(self) -> None: Returns: None """ - LOGGER.info(f"Starting {self.motor_type} ID:{self.motor_id}") + LOGGER.info(f"Starting {self.__str__()}") if not self.is_offline and self._canman: self._canman.power_on(self.motor_id) @@ -674,7 +642,7 @@ def start(self) -> None: time.sleep(0.1) # Minimum wait time # Set initial control mode to IDLE - self._canman.set_control_mode(self.motor_id, ServoControlMode.IDLE.value) + self.set_control_mode(CONTROL_MODES.IDLE) # Poll for mode switch confirmation (max 200ms timeout) mode_start_time = time.time() @@ -698,7 +666,7 @@ def start(self) -> None: @check_actuator_open def stop(self) -> None: """Stop motor and clean up CAN communication""" - LOGGER.info(f"Stopping {self.motor_type} ID:{self.motor_id}") + LOGGER.info(f"Stopping {self.__str__()}") if not self.is_offline and self._canman: # Set motor to idle mode first @@ -761,7 +729,7 @@ def update(self) -> None: def _update_state_async(self, servo_state: ServoMotorState) -> None: """ - Asynchronously update state and handle errors. + Asynchronously update state and interprets errors. Returns: None @@ -803,17 +771,17 @@ def home(self) -> None: Returns: None """ - self._is_homed = False raise NotImplementedError("Homing not implemented.") def set_origin(self) -> None: """ - Set encoder origin - automatically applies a "zero position" offset. + Set encoder origin. + This function automatically applies a "zero position" offset. """ if not self.is_offline and self._canman: self._canman.set_origin(self.motor_id, 1) time.sleep(0.1) - LOGGER.info(f"Set origin {self.device_info_string()}") + LOGGER.info(f"Set origin {self.__str__()}") @property def MOTOR_CONSTANTS(self) -> MOTOR_CONSTANTS: @@ -844,7 +812,7 @@ def MOTOR_CONSTANTS(self, value: MOTOR_CONSTANTS) -> None: if not isinstance(value, MOTOR_CONSTANTS): raise TypeError(f"Expected MOTOR_CONSTANTS, got {type(value)}") self._MOTOR_CONSTANTS = value - + # ============ Control Interface ============ def set_motor_voltage(self, value: float) -> None: From e51cf7cccd3b68222da3451c40887c5587da5580 Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Thu, 22 Jan 2026 14:14:29 -0500 Subject: [PATCH 05/27] removed unnecessary impedance functions --- opensourceleg/actuators/base.py | 1 + opensourceleg/actuators/tmotor.py | 61 ++++++------------------------- 2 files changed, 12 insertions(+), 50 deletions(-) diff --git a/opensourceleg/actuators/base.py b/opensourceleg/actuators/base.py index 895e8cb4..f42f7024 100644 --- a/opensourceleg/actuators/base.py +++ b/opensourceleg/actuators/base.py @@ -408,6 +408,7 @@ class ActuatorBase(OfflineMixin, ABC): "set_output_torque": {CONTROL_MODES.CURRENT, CONTROL_MODES.TORQUE}, "set_current_gains": {CONTROL_MODES.CURRENT, CONTROL_MODES.TORQUE}, "set_position_gains": {CONTROL_MODES.POSITION}, + "set_impedance_gains": {CONTROL_MODES.IMPEDANCE}, } # Offline mode configuration for OfflineMixin diff --git a/opensourceleg/actuators/tmotor.py b/opensourceleg/actuators/tmotor.py index 8c84bb82..e75013d6 100644 --- a/opensourceleg/actuators/tmotor.py +++ b/opensourceleg/actuators/tmotor.py @@ -107,15 +107,6 @@ class ServoMotorState: 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""" @@ -829,9 +820,8 @@ def set_motor_current(self, value: float) -> None: driver_current = value / self._current_scale # Get current limits from motor parameters - motor_params = cast(dict[str, Any], self._motor_params) - max_current = motor_params["Curr_max"] - min_current = motor_params["Curr_min"] + max_current = self._motor_params["Curr_max"] + min_current = self._motor_params["Curr_min"] # Clamp current to safe limits clamped_driver_current = np.clip(driver_current, min_current, max_current) @@ -859,9 +849,8 @@ def set_motor_position(self, value: float) -> None: 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"] - min_position = motor_params["P_min"] + max_position = self._motor_params["P_max"] + min_position = self._motor_params["P_min"] # Clamp position to safe limits clamped_position = np.clip(position_deg, min_position, max_position) @@ -906,13 +895,12 @@ def set_output_torque(self, value: float) -> None: 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 + max_velocity = self._motor_params["V_max"] # Already in ERPM + min_velocity = self._motor_params["V_min"] # Already in ERPM # Clamp velocity to safe limits clamped_velocity = np.clip(velocity_erpm, min_velocity, max_velocity) @@ -927,22 +915,6 @@ def set_motor_velocity(self, value: float) -> None: 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.") - - 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: """Set output velocity (rad/s)""" motor_velocity = value * self.gear_ratio @@ -955,27 +927,18 @@ def set_current_gains(self, kp: float, ki: float, kd: float, ff: float) -> None: LOGGER.debug( "TMotor servo mode handles current control internally. " "External current PID gains are not used." ) - # Motor handles current control internally, no action needed 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_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 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 + LOGGER.debug( + "TMotor servo mode handles control internally. " "Impedance gains are not used." + ) # ============ State Properties ============ @@ -1005,8 +968,7 @@ def output_velocity(self) -> float: @property 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"] + return self._motor_params["NUM_POLE_PAIRS"] @property def motor_voltage(self) -> float: @@ -1032,8 +994,7 @@ def motor_torque(self) -> float: Torque at the motor in Nm. This is calculated using motor current and k_t. """ - motor_params = cast(dict[str, Any], self._motor_params) - kt_user = cast(float, motor_params["Kt_actual"]) * self._kt_scale + kt_user = cast(float, self._motor_params["Kt_actual"]) * self._kt_scale return self.motor_current * kt_user / 1000 @property From 034c900c4365fc6468572f56a2988c0e6edd5292 Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Thu, 22 Jan 2026 14:31:08 -0500 Subject: [PATCH 06/27] added docstrings for CAN classes --- opensourceleg/actuators/tmotor.py | 71 +++++++++++++++++++++++-------- 1 file changed, 53 insertions(+), 18 deletions(-) diff --git a/opensourceleg/actuators/tmotor.py b/opensourceleg/actuators/tmotor.py index e75013d6..ed4e9e41 100644 --- a/opensourceleg/actuators/tmotor.py +++ b/opensourceleg/actuators/tmotor.py @@ -108,7 +108,32 @@ class ServoMotorState: class CANManagerServo: - """TMotor servo mode CAN communication manager""" + """ + Manages CAN bus communication for T-Motor actuators operating in Servo Mode. + + This class implements the Singleton pattern to ensure a single point of control + for the CAN interface. It handles the initialization of the socketcan bus, dispatches + incoming messages to registered motor listeners, and provides + utilities for packing and sending control commands (current, velocity, and position). + + Requirements: + The CAN interface ('can0') must be configured in the OS before initializing + this class. Typical configuration commands for Linux systems: + + $ sudo /sbin/ip link set can0 down + $ sudo /sbin/ip link set can0 up type can bitrate 1000000 + $ sudo ifconfig can0 txqueuelen 1000 + + Attributes: + debug (bool): If true, logs detailed information about sent and received messages. + bus (can.interface.Bus): The underlying python-can bus interface + notifier (can.Notifier): Manages message listeners for asynchronous updates + + Examples: + with CANManagerServo() as manager: + manager.power_on(motor_id=1) + manager.set_position(motor_id=1, position=90.0) + """ _instance: Optional["CANManagerServo"] = None debug: bool = False @@ -126,12 +151,6 @@ def __init__(self) -> None: 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=[]) self._listeners = [] # Track active listeners @@ -139,11 +158,11 @@ def __init__(self) -> None: 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") + LOGGER.error(f"CAN bus initialization failed: {e}" + "Please ensure CAN interface is configured. Run:" + "sudo /sbin/ip link set can0 down" + "sudo /sbin/ip link set can0 up type can bitrate 1000000" + "sudo ifconfig can0 txqueuelen 1000") raise RuntimeError("CAN bus initialization failed. Please configure CAN interface first.") from e def __enter__(self) -> None: @@ -154,9 +173,6 @@ def __exit__(self, exc_type, exc_val, exc_tb) -> None: """ Context manager exit""" self.close() - def __del__(self) -> None: - self.close() - def close(self) -> None: """ Close CAN service @@ -335,14 +351,34 @@ def enable_debug(self, enable: bool = True) -> None: class MotorListener(can.Listener): - """CAN message listener""" + """ + CAN message listener. + + This class monitors the CAN bus for messages addressed to a specific motor ID. + When a relevant message is received, it parses the data using the CAN manager + and updates the state of the associated motor instance asynchronously. + + Args: + canman (CANManagerServo): The CAN manager instance responsible for parsing + raw message data into servo states. + motor (TMotorServoActuator): The specific motor instance that this + listener monitors and updates. + """ def __init__(self, canman: CANManagerServo, motor: "TMotorServoActuator") -> None: self.canman = canman self.motor = motor def on_message_received(self, msg: can.Message) -> None: - """Handle received CAN message""" + """ + Callback triggered when a CAN message is received. + + Checks if the message's arbitration ID matches the assigned motor's ID. + If it matches, the message data is parsed and the motor's state is updated. + + Args: + msg (can.Message): The received CAN message + """ data = bytes(msg.data) motor_id = msg.arbitration_id & 0x00000FF if motor_id == self.motor.motor_id: @@ -585,7 +621,6 @@ def __init__( LOGGER.info(f"Initialized TMotor servo: {self.motor_type} ID:{self.motor_id}") def __str__(self) -> str: - """State string""" return f"{self.motor_type} ID:{self.motor_id}" @property From 1304dcc6af1b9a51a4dbd4c2e3733ede23821c69 Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Thu, 22 Jan 2026 15:56:37 -0500 Subject: [PATCH 07/27] initial commit of tutorial scripts --- .../actuators/tmotor/position_control.py | 87 +++++++++++++++++ .../actuators/tmotor/reading_sensor_data.py | 51 ++++++++++ tutorials/actuators/tmotor/torque_control.py | 96 +++++++++++++++++++ .../actuators/tmotor/velocity_control.py | 81 ++++++++++++++++ 4 files changed, 315 insertions(+) create mode 100644 tutorials/actuators/tmotor/position_control.py create mode 100644 tutorials/actuators/tmotor/reading_sensor_data.py create mode 100644 tutorials/actuators/tmotor/torque_control.py create mode 100644 tutorials/actuators/tmotor/velocity_control.py diff --git a/tutorials/actuators/tmotor/position_control.py b/tutorials/actuators/tmotor/position_control.py new file mode 100644 index 00000000..05deb86a --- /dev/null +++ b/tutorials/actuators/tmotor/position_control.py @@ -0,0 +1,87 @@ +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 + +STEP_AMPLITUDE = 0.3 # Radians +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, + gear_ratio=9.0, + offline=False, + ) + + clock = SoftRealtimeLoop(dt=DT) + + with motor: + + motor.update() + + print("Setting motor origin...") + motor.set_origin() + + # Set to position control mode (PID parameters are built-in and + # can only be modified via R-link) + motor.set_control_mode(mode=CONTROL_MODES.POSITION) + + print(f"Current motor position {motor.output_position:.3f}") + + initial_position = motor.output_position + command_position = initial_position + + position_logger.track_function(lambda: motor.output_position, "Motor Position") + position_logger.track_function(lambda: command_position, "Command Position") + position_logger.track_function(lambda: time.monotonic(), "Time") + + print(f"Starting staircase step response (Step: {STEP_AMPLITUDE})...") + + for t in clock: + # Calculate how many steps have passed + step_count = int(t) + + # Continuously increment position based on step count + command_position = initial_position - (step_count * STEP_AMPLITUDE) + + # Send command + motor.set_output_position(value=command_position) + motor.update() + + # Log data + position_logger.info( + f"Time: {t:.3f}s | " + f"Step: {step_count} | " + f"Cmd: {command_position:.3f} | " + f"Meas: {motor.output_position:.3f}" + ) + 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_output_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..3c3a7346 --- /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, + gear_ratio=9.0, + offline=False, + ) + + # Track sensor data + sensor_logger.track_function(lambda: motor.output_position, "Output Position") + sensor_logger.track_function(lambda: motor.output_velocity, "Output Velocity") + sensor_logger.track_function(lambda: motor.motor_current, "Motor Current") + sensor_logger.track_function(lambda: motor.output_torque, "Output Torque") + sensor_logger.track_function(lambda: motor.case_temperature, "Case Temperature") + sensor_logger.track_function(lambda: motor.winding_temperature, "Winding Temperature") + + with motor: + print("Reading sensor data...") + + for t in clock: + motor.update() + + sensor_logger.info( + f"Time: {t:.3f}; " + f"Position: {motor.output_position:.3f} rad; " + f"Velocity: {motor.output_velocity:.2f} rad/s; " + f"Current: {motor.motor_current:.2f} A; " + f"Torque: {motor.output_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..7c7da503 --- /dev/null +++ b/tutorials/actuators/tmotor/torque_control.py @@ -0,0 +1,96 @@ +import time + +import numpy as np + +import sys +sys.path.append('/home/kwalte/opensourceleg') + +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()}") + + # 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) + + motor.update() + + # Motor torque constant for AK80-9 + MAX_OUTPUT_TORQUE = 2.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_output_torque, "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.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) + command_output_torque = 0.0 + + # 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.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_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..3b4112d2 --- /dev/null +++ b/tutorials/actuators/tmotor/velocity_control.py @@ -0,0 +1,81 @@ +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()}") + + # Set the encoder origin first (optional) + print("Setting encoder origin...") + motor.set_origin() + + # Set to velocity control mode (PID parameters are built-in) + motor.set_control_mode(mode=CONTROL_MODES.VELOCITY) + + motor.update() + + # Track velocity data + velocity_logger.track_function(lambda: motor.output_velocity, "Output Velocity") + 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.output_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_output_velocity(value=command_velocity) + motor.update() + + velocity_logger.info( + f"Time: {t:.3f}; " + f"Command Velocity: {command_velocity:.3f} rad/s; " + f"Output Velocity: {motor.output_velocity:.3f} rad/s; " + f"Position: {motor.output_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_output_velocity(value=0.0) + motor.update() + time.sleep(1.0) + + +if __name__ == "__main__": + velocity_control() From 299dea81d0767c2a1b22c47735fd49e2c757a300 Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Thu, 22 Jan 2026 16:22:10 -0500 Subject: [PATCH 08/27] fixed position control, tested in positive and negative commanded position --- opensourceleg/actuators/tmotor.py | 21 +++++++++++-------- .../actuators/tmotor/position_control.py | 14 ++++++------- 2 files changed, 19 insertions(+), 16 deletions(-) diff --git a/opensourceleg/actuators/tmotor.py b/opensourceleg/actuators/tmotor.py index ed4e9e41..90684374 100644 --- a/opensourceleg/actuators/tmotor.py +++ b/opensourceleg/actuators/tmotor.py @@ -58,6 +58,7 @@ "SET_POS": 4, "SET_ORIGIN_HERE": 5, "SET_POS_SPD": 6, + "SET_MODE": 7, } # TMotor model specifications @@ -263,10 +264,7 @@ def set_position(self, motor_id: int, position: float) -> None: Returns: None """ - # 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 + buffer = self._pack_int32(int(position * 10000.0)) message_id = (CAN_PACKET_ID["SET_POS"] << 8) | motor_id self.send_message(message_id, buffer, len(buffer)) @@ -294,7 +292,7 @@ def set_control_mode(self, motor_id: int, mode: int) -> None: None """ buffer = [mode] - message_id = (7 << 8) | motor_id # Assume packet_id=7 for mode switching + message_id = (CAN_PACKET_ID["SET_MODE"] << 8) | motor_id self.send_message(message_id, buffer, len(buffer)) @staticmethod @@ -699,7 +697,7 @@ def stop(self) -> None: try: self._canman.set_current(self.motor_id, 0.0) time.sleep(0.01) - self._canman.set_control_mode(self.motor_id, ServoControlMode.IDLE.value) + self.set_control_mode(CONTROL_MODES.IDLE) time.sleep(0.05) # Give motor time to process except Exception as e: LOGGER.warning(f"Error setting idle mode during stop: {e}") @@ -749,7 +747,7 @@ def update(self) -> 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) + warnings.warn(f"No data from motor: {self.__str__()}", RuntimeWarning, stacklevel=2) self._last_update_time = now @@ -806,7 +804,7 @@ def set_origin(self) -> None: """ if not self.is_offline and self._canman: self._canman.set_origin(self.motor_id, 1) - time.sleep(0.1) + time.sleep(0.3) LOGGER.info(f"Set origin {self.__str__()}") @property @@ -873,6 +871,12 @@ def set_motor_current(self, value: float) -> None: self._last_command_time = time.time() def set_motor_position(self, value: float) -> None: + raise NotImplementedError( + "Setting motor position not supported" + "Recommended to use 'set_output_position' command instead." + ) + + def set_output_position(self, value: float) -> None: """ Set motor position (radians) with clamping to motor limits Args: @@ -1113,7 +1117,6 @@ def thermal_scaling_factor(self) -> float: 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" diff --git a/tutorials/actuators/tmotor/position_control.py b/tutorials/actuators/tmotor/position_control.py index 05deb86a..9ec235fe 100644 --- a/tutorials/actuators/tmotor/position_control.py +++ b/tutorials/actuators/tmotor/position_control.py @@ -34,13 +34,13 @@ def position_control(): print("Setting motor origin...") motor.set_origin() + print(f"Current motor position {motor.output_position:.3f}") + # Set to position control mode (PID parameters are built-in and # can only be modified via R-link) motor.set_control_mode(mode=CONTROL_MODES.POSITION) - print(f"Current motor position {motor.output_position:.3f}") - initial_position = motor.output_position command_position = initial_position @@ -55,7 +55,7 @@ def position_control(): step_count = int(t) # Continuously increment position based on step count - command_position = initial_position - (step_count * STEP_AMPLITUDE) + command_position = initial_position + (step_count * STEP_AMPLITUDE) # Send command motor.set_output_position(value=command_position) @@ -77,10 +77,10 @@ def position_control(): print("Position control complete") # Return to home position - # print("Returning to home...") - # motor.set_output_position(value=initial_position) - # time.sleep(1.0) - # motor.update() + print("Returning to home...") + motor.set_output_position(value=initial_position) + time.sleep(1.0) + motor.update() if __name__ == "__main__": From 66f824f0270dd957ace23c68349e6b094ee0373c Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Thu, 22 Jan 2026 16:27:51 -0500 Subject: [PATCH 09/27] tested torque and velocity control tutorials --- tutorials/actuators/tmotor/torque_control.py | 19 +++++++------------ .../actuators/tmotor/velocity_control.py | 9 ++++----- 2 files changed, 11 insertions(+), 17 deletions(-) diff --git a/tutorials/actuators/tmotor/torque_control.py b/tutorials/actuators/tmotor/torque_control.py index 7c7da503..d48df59b 100644 --- a/tutorials/actuators/tmotor/torque_control.py +++ b/tutorials/actuators/tmotor/torque_control.py @@ -1,10 +1,6 @@ import time - import numpy as np -import sys -sys.path.append('/home/kwalte/opensourceleg') - from opensourceleg.actuators.base import CONTROL_MODES from opensourceleg.actuators.tmotor import TMotorServoActuator from opensourceleg.logging.logger import Logger @@ -25,13 +21,15 @@ def torque_control(): 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: - print(f"Connected to TMotor: {motor.device_info_string()}") + + motor.update() # Set the encoder origin first (optional) print("Setting encoder origin...") @@ -40,8 +38,6 @@ def torque_control(): # 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) @@ -52,20 +48,19 @@ def torque_control(): 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.time(), "Time") + torque_logger.track_function(lambda: time.monotonic(), "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) - command_output_torque = 0.0 + # Sinusoidal output torque command (0.2 Hz, amplitude 2.0 Nm at output) + command_output_torque = 2.0 * 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) + # Set output torque motor.set_output_torque(command_output_torque) motor.update() diff --git a/tutorials/actuators/tmotor/velocity_control.py b/tutorials/actuators/tmotor/velocity_control.py index 3b4112d2..c1f4077f 100644 --- a/tutorials/actuators/tmotor/velocity_control.py +++ b/tutorials/actuators/tmotor/velocity_control.py @@ -1,5 +1,4 @@ import time - import numpy as np from opensourceleg.actuators.base import CONTROL_MODES @@ -22,13 +21,15 @@ def velocity_control(): 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: - print(f"Connected to TMotor: {motor.device_info_string()}") + + motor.update() # Set the encoder origin first (optional) print("Setting encoder origin...") @@ -37,14 +38,12 @@ def velocity_control(): # Set to velocity control mode (PID parameters are built-in) motor.set_control_mode(mode=CONTROL_MODES.VELOCITY) - motor.update() - # Track velocity data velocity_logger.track_function(lambda: motor.output_velocity, "Output Velocity") 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.output_position, "Motor Position") - velocity_logger.track_function(lambda: time.time(), "Time") + velocity_logger.track_function(lambda: time.monotonic(), "Time") print("Starting velocity control...") From af58c0b106674c18f3798b17c594c7a8a002d850 Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Thu, 22 Jan 2026 16:32:21 -0500 Subject: [PATCH 10/27] added script for controlling 2 tmotors simultaneously --- .../torque_control_multiple_actuators.py | 122 ++++++++++++++++++ 1 file changed, 122 insertions(+) create mode 100644 tutorials/actuators/tmotor/torque_control_multiple_actuators.py diff --git a/tutorials/actuators/tmotor/torque_control_multiple_actuators.py b/tutorials/actuators/tmotor/torque_control_multiple_actuators.py new file mode 100644 index 00000000..393e79bf --- /dev/null +++ b/tutorials/actuators/tmotor/torque_control_multiple_actuators.py @@ -0,0 +1,122 @@ +import time +import matplotlib.pyplot as plt +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_1 = 104 # First motor CAN ID +MOTOR_ID_2 = 105 # Second motor CAN ID + + +def torque_control(): + torque_logger = Logger( + log_path="./logs", + file_name="tmotor_dual_torque_control", + ) + + # Initialize TMotor actuators + motor1 = TMotorServoActuator( + motor_type="AK80-9", + motor_id=MOTOR_ID_1, + gear_ratio=9.0, + offline=False, + ) + + motor2 = TMotorServoActuator( + motor_type="AK80-9", + motor_id=MOTOR_ID_2, + gear_ratio=9.0, + offline=False, + ) + + clock = SoftRealtimeLoop(dt=DT) + + with motor1, motor2: + print(f"Connected to Motor 1: {motor1.__str__()}") + print(f"Connected to Motor 2: {motor2.__str__()}") + + motor1.update() + motor2.update() + + # Home both motors + print("Setting origin...") + motor1.set_origin() + motor2.set_origin() + + # Set to current control mode for torque control + motor1.set_control_mode(mode=CONTROL_MODES.CURRENT) + motor2.set_control_mode(mode=CONTROL_MODES.CURRENT) + + # Safety limits + MAX_OUTPUT_TORQUE = 2.0 # Maximum output torque in Nm + + # Initialize command torque variables + command_output_torque_1 = 0.0 + command_output_torque_2 = 0.0 + + # Track data for motor 1 + torque_logger.track_function(lambda: motor1.motor_torque, "Motor1 Motor Torque") + torque_logger.track_function(lambda: motor1.output_torque, "Motor1 Output Torque") + torque_logger.track_function(lambda: command_output_torque_1, "Motor1 Command Output Torque") + torque_logger.track_function(lambda: motor1.motor_current, "Motor1 Motor Current") + torque_logger.track_function(lambda: motor1.output_position, "Motor1 Motor Position") + torque_logger.track_function(lambda: motor1.output_velocity, "Motor1 Motor Velocity") + + # Track data for motor 2 + torque_logger.track_function(lambda: motor2.motor_torque, "Motor2 Motor Torque") + torque_logger.track_function(lambda: motor2.output_torque, "Motor2 Output Torque") + torque_logger.track_function(lambda: command_output_torque_2, "Motor2 Command Output Torque") + torque_logger.track_function(lambda: motor2.motor_current, "Motor2 Motor Current") + torque_logger.track_function(lambda: motor2.output_position, "Motor2 Motor Position") + torque_logger.track_function(lambda: motor2.output_velocity, "Motor2 Motor Velocity") + + torque_logger.track_function(lambda: time.time(), "Time") + + print("Starting dual motor torque control...") + + # Control loop + for t in clock: + # Sinusoidal output torque command (0.2 Hz, amplitude 0.5 Nm) + # Motor 1 and Motor 2 can have different commands if needed + command_output_torque_1 = 0.5 * np.sin(2 * np.pi * 0.2 * t) + command_output_torque_2 = 0.5 * np.sin(2 * np.pi * 0.2 * t) # Same command, modify as needed + + # Limit output torque for safety + command_output_torque_1 = np.clip(command_output_torque_1, -MAX_OUTPUT_TORQUE, MAX_OUTPUT_TORQUE) + command_output_torque_2 = np.clip(command_output_torque_2, -MAX_OUTPUT_TORQUE, MAX_OUTPUT_TORQUE) + + # Set output torque for both motors + motor1.set_output_torque(command_output_torque_1) + motor2.set_output_torque(command_output_torque_2) + + motor1.update() + motor2.update() + + torque_logger.info( + f"Time: {t:.3f}; " + f"M1 Cmd: {command_output_torque_1:.3f} Nm; M1 Out: {motor1.output_torque:.3f} Nm; " + f"M2 Cmd: {command_output_torque_2:.3f} Nm; M2 Out: {motor2.output_torque:.3f} Nm" + ) + torque_logger.update() + + # Run for 10 seconds + if t > 10.0: + break + + print("Torque control complete") + + # Stop both motors + print("Stopping motors...") + motor1.set_output_torque(0.0) + motor2.set_output_torque(0.0) + motor1.update() + motor2.update() + time.sleep(1.0) + +if __name__ == "__main__": + torque_control() From b3dc6efea12927feac15703a2434e8ffd2a5570b Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Thu, 22 Jan 2026 16:36:02 -0500 Subject: [PATCH 11/27] cleaned up --- opensourceleg/actuators/tmotor.py | 94 ++----------------------------- 1 file changed, 5 insertions(+), 89 deletions(-) diff --git a/opensourceleg/actuators/tmotor.py b/opensourceleg/actuators/tmotor.py index 90684374..7cf2f7dd 100644 --- a/opensourceleg/actuators/tmotor.py +++ b/opensourceleg/actuators/tmotor.py @@ -841,7 +841,7 @@ def MOTOR_CONSTANTS(self, value: MOTOR_CONSTANTS) -> None: 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") + raise NotImplementedError("Voltage control not supported in servo mode") def set_motor_current(self, value: float) -> None: """ @@ -963,19 +963,19 @@ def set_output_velocity(self, value: float) -> None: 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( + raise NotImplementedError( "TMotor servo mode handles current control internally. " "External current PID gains are not used." ) 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( + raise NotImplementedError( "TMotor servo mode handles position control internally. " "External position PID gains are not used." ) def _set_impedance_gains(self, k: float, b: float) -> None: """Internal method for impedance gains - not supported in TMotor servo mode""" - LOGGER.debug( + raise NotImplementedError( "TMotor servo mode handles control internally. " "Impedance gains are not used." ) @@ -1078,88 +1078,4 @@ def thermal_scaling_factor(self) -> float: return self._thermal_scale if __name__ == "__main__": - 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.__str__()}") - - # Set origin and set current control mode - actuator.set_origin() - 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) - - # 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}mA | " 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( - 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() - - print("Current loop control example completed!") + pass \ No newline at end of file From 743f5fb2a2b5f4ea35b06d074cd050ddf6916442 Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Thu, 22 Jan 2026 17:06:32 -0500 Subject: [PATCH 12/27] clean up --- opensourceleg/actuators/tmotor.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/opensourceleg/actuators/tmotor.py b/opensourceleg/actuators/tmotor.py index 7cf2f7dd..c239bf1c 100644 --- a/opensourceleg/actuators/tmotor.py +++ b/opensourceleg/actuators/tmotor.py @@ -93,8 +93,7 @@ NM_PER_AMP=0.095, # Placeholder to satisfy validation MAX_CASE_TEMPERATURE=80.0, # Temperature parameters are also set in the driver via R-Link MAX_WINDING_TEMPERATURE=110.0, # Temperature parameters are also set in the driver via R-Link - # Soft limits set 10°C below hard limits for safety margin - WINDING_SOFT_LIMIT=100.0, + WINDING_SOFT_LIMIT=100.0, # Soft limits set 10°C below hard limits for safety margin CASE_SOFT_LIMIT=70.0, ) @@ -524,6 +523,7 @@ def __init__( frequency: int = 1000, offline: bool = False, current_mode: str = "driver", + thermal_current_scale: float = 1.0, **kwargs: Any, ) -> None: """ @@ -540,6 +540,7 @@ def __init__( - 'driver': use driver's native current (default) - 'amplitude-invariant': true phase current convention - 'power-invariant': power-invariant current convention + thermal_current_scale: compensates for discrepancy between firmware k_t and effective k_t """ # Validate motor type if motor_type not in TMOTOR_MODELS: @@ -570,6 +571,7 @@ def __init__( self.motor_id = motor_id self._motor_params = TMOTOR_MODELS[motor_type] self.current_mode = current_mode + self._thermal_current_scale = thermal_current_scale # Current conversion factors # Physical: I_drv (line current) = K * I_user @@ -736,7 +738,7 @@ def update(self) -> None: # Temperature check self._thermal_scale = self._thermal_model.update( dt = 1 / self.frequency, - motor_current = self.motor_current, + motor_current = self.motor_current*self._thermal_current_scale, case_temperature = self.case_temperature, ) From 61bbf13b293b2b246dd6045c9307d3d03dcf8b57 Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Mon, 26 Jan 2026 11:47:26 -0500 Subject: [PATCH 13/27] added angular rate to Kalman filter --- opensourceleg/sensors/imu.py | 2 +- opensourceleg/utilities/filters.py | 130 ++++++++++-------- .../kalman_filter_imu_global_angles.py | 12 +- 3 files changed, 75 insertions(+), 69 deletions(-) diff --git a/opensourceleg/sensors/imu.py b/opensourceleg/sensors/imu.py index f6c8ae30..7124a1e3 100644 --- a/opensourceleg/sensors/imu.py +++ b/opensourceleg/sensors/imu.py @@ -1537,7 +1537,7 @@ def gyro(self, most_recent: bool = True) -> np.ndarray: Returns latest gyroscope data [gx, gy, gz] (rad/s) """ sensor_id = self.SENSOR_DICT.get("Gyro", self.SENSOR_ID_GYR) - return self._get_sensor_data(sensor_id, most_recent) * np.pi / 180.0 + return self._get_sensor_data(sensor_id, most_recent) @property def accel(self, most_recent: bool = True) -> np.ndarray: diff --git a/opensourceleg/utilities/filters.py b/opensourceleg/utilities/filters.py index 7c093bfc..8622c9b3 100644 --- a/opensourceleg/utilities/filters.py +++ b/opensourceleg/utilities/filters.py @@ -1,59 +1,68 @@ import math import time - import numpy as np - class KalmanFilter2D: """ - 2D Kalman filter to estimate global roll and pitch angles - from IMU gyroscope (rad/s) and acceleration (m/s^2) signals. + 2D Kalman filter to estimate global roll and pitch angles and angular rates + from IMU gyroscope (rad/s) and acceleration (m/s^2) signals. Roll, pitch, and yaw are defined about the x,y,z axes, respectively. - The state tracks 2D angles and gyro bias: - x = [roll, pitch, roll_bias, pitch_bias] + The state tracks 2D angles, angular rates, and gyro bias: + x = [roll, pitch, roll_rate, pitch_rate, roll_bias, pitch_bias] The process model assumes linear kinematics: - x_{t} = x_{t-1} + (gyro_{t} - bias{t-1})*dt + roll_{t} = roll_{t-1} + roll_rate_{t-1} * dt + roll_rate_{t} = roll_rate_{t-1} (assumes constant velocity) + bias_{t} = bias_{t-1} (random walk) - Important note: When using the BHI260AP IMU, the acceleration - filter input must be the "gravity" IMU output. + Measurement model: + z = [accel_roll, accel_pitch, gyro_x, gyro_y] - This implementation was tested with the BHI260AP IMU against - the LordMicrostrain GX5 IMU, with maximum error <0.02 rad. + Important note: when using the BHI260AP IMU, the acceleration filter input + must be the "gravity" IMU output. Author: Katharine Walters - + Date: - 01/13/2026 + 01/26/2026 """ def __init__( - self, tag: str = "KalmanFilter2D", Q_bias: float = 1e-13, Q_angle: float = 1e-4, R_var: float = 3e-6 + self, + tag: str = "KalmanFilter2D", + Q_bias: float = 1e-13, + Q_angle: float = 1e-4, + Q_rate: float = 1e-2, + R_accel: float = 3e-6, + R_gyro: float = 1e-3 ) -> None: """ Initialize 2D Kalman filter Params: tag: identifier of KalmanFilter2D instance - Q_bias: variance in gyroscope drift rate - Q_angle: uncertainty in gyroscope angle prediction - R_var: accelerometer angle measurement noise + Q_bias: Variance in gyro drift rate + Q_angle: Variance in gyroscope angle prediction + Q_rate: Variance in angular acceleration + R_accel: Accelerometer angle measurement noise + R_gyro: Gyroscope measurement noise """ - # Initialize state - self.x = np.zeros(4) # [roll, pitch, bias_roll, bias_pitch] + # Initialize state: [roll, pitch, roll_rate, pitch_rate, b_roll, b_pitch] + self.x = np.zeros(6) self.yaw = 0.0 # Initialize prediction covariance - self.P = 0.1 * np.eye(4) + self.P = 0.1 * np.eye(6) - # Process noise covariance - self.Q = np.diag([Q_angle, Q_angle, Q_bias, Q_bias]) + # Process noise covariance (6x6) + self.Q = np.diag([Q_angle, Q_angle, Q_rate, Q_rate, Q_bias, Q_bias]) - # Measurement noise covaraince (accel noise) - self.R = R_var * np.eye(2) + # Measurement noise covariance (4x4) + # z vector order: [roll_a, pitch_a, gyro_x, gyro_y] + self.R = np.diag([R_accel, R_accel, R_gyro, R_gyro]) # Initialize previous time self.prev_time = time.monotonic() @@ -62,29 +71,20 @@ def _normalize_angle(self, a: float) -> float: """Normalize angle to [-pi, pi].""" return (a + math.pi) % (2 * math.pi) - math.pi - def _predict(self, gx: float, gy: float, gz: float, dt: float) -> None: + def _predict(self, dt: float) -> None: """ - Prediction step - - Params: - gx: x-axis gyroscope measurement (rad/s) - gy: y-axis gyroscope measurement (rad/s) - gz: z-axis gyroscope measurement (rad/s) - dt: discrete time step (seconds) + Prediction step based on internal physics model. + We assume constant angular velocity between steps. """ - # State-transition matrix - F = np.array([[1, 0, -dt, 0], [0, 1, 0, -dt], [0, 0, 1, 0], [0, 0, 0, 1]]) - - # Control input matrix - B = np.array([[dt, 0], [0, dt], [0, 0], [0, 0]]) - u = np.array([gx, gy]) + # State-transition matrix + F = np.eye(6) + F[0, 2] = dt # roll += roll_rate * dt + F[1, 3] = dt # pitch += pitch_rate * dt - # Prediction x_new = x + (gyro - bias)*dt - # x = F*x + B*u - self.x = F @ self.x + B @ u + # Prediction + self.x = F @ self.x self.x[0] = self._normalize_angle(self.x[0]) self.x[1] = self._normalize_angle(self.x[1]) - self.yaw += gz * dt # Dead reckoning yaw (simple integration, no fusion) # Covariance prediction self.P = F @ self.P @ F.T + self.Q * dt @@ -94,26 +94,25 @@ def update(self, ax: float, ay: float, az: float, gx: float, gy: float, gz: floa Update global angle estimations Params: - ax: x-axis gravity measurement (m/s^2) - ay: y-axis gravity measurement (m/s^2) - az: z-axis gravity measurement (m/s^2) - gx: x-axis gyroscope measurement (rad/s) - gy: y-axis gyroscope measurement (rad/s) - gz: z-axis gyroscope measurement (rad/s) - - Returns: (roll, pitch, yaw) + ax, ay, az: Gravity measurement (m/s^2) + gx, gy, gz: Gyroscope measurement (rad/s) + + Returns: (roll, pitch, roll_rate, pitch_rate, yaw) """ - # Calculate change in time since last update + # Calculate dt curr_time = time.monotonic() dt = curr_time - self.prev_time self.prev_time = curr_time # Guard against invalid dt if dt <= 0: - return self.x[0].copy(), self.x[1].copy(), self.yaw + return self.x[0], self.x[1], self.x[2], self.x[3], self.yaw - # Predict - self._predict(gx, gy, gz, dt) + # Prediction: assumes constant angular velocity + self._predict(dt) + + # Dead reckoning yaw (simple integration, no fusion) + self.yaw += gz * dt # Accelerometer global angle calculation roll_a = math.atan2(ay, az) @@ -125,11 +124,19 @@ def update(self, ax: float, ay: float, az: float, gx: float, gy: float, gz: floa roll_a -= np.pi elif roll_a < 0: roll_a += np.pi - - z = np.array([roll_a, pitch_a]) + + z = np.array([roll_a, pitch_a, gx, gy]) # Measurement model - H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) + # roll_meas = roll + # pitch_meas = pitch + # gx = roll_rate + bias_roll + # gy = pitch_rate + bias_pitch + H = np.array([ + [1,0,0,0,0,0], + [0,1,0,0,0,0], + [0,0,1,0,1,0], + [0,0,0,1,0,1]]) # Innovation step S = H @ self.P @ H.T + self.R @@ -139,9 +146,10 @@ def update(self, ax: float, ay: float, az: float, gx: float, gy: float, gz: floa y[0] = self._normalize_angle(y[0]) y[1] = self._normalize_angle(y[1]) - # Update + # Update self.x = self.x + K @ y - I_KH = np.eye(4) - K @ H - self.P = I_KH @ self.P @ I_KH.T + K @ self.R @ K.T # Joseph form for numeric stability + I_KH = np.eye(6) - K @ H + self.P = I_KH @ self.P @ I_KH.T + K @ self.R @ K.T - return self.x[0].copy(), self.x[1].copy(), self.yaw + # Return roll, pitch, roll_rate, pitch_rate, yaw + return self.x[0], self.x[1], self.x[2], self.x[3], self.yaw \ No newline at end of file diff --git a/tutorials/utilities/kalman_filter_imu_global_angles.py b/tutorials/utilities/kalman_filter_imu_global_angles.py index 4f082117..bcdbeebd 100644 --- a/tutorials/utilities/kalman_filter_imu_global_angles.py +++ b/tutorials/utilities/kalman_filter_imu_global_angles.py @@ -17,17 +17,15 @@ imu_logger.track_function(lambda: roll, "Roll") imu_logger.track_function(lambda: pitch, "Pitch") + imu_logger.track_function(lambda: roll_rate, "Roll Rate") + imu_logger.track_function(lambda: pitch_rate, "Pitch Rate") imu_logger.track_function(lambda: yaw, "Yaw") # Define axis transformation (modify as needed for your IMU mounting) imu_transform = AxisTransform(roll="z", pitch="y", yaw="-x") # Kalman filter - kalman_filter = KalmanFilter2D( - Q_angle=1e-4, - Q_bias=1e-13, - R_var=3e-6, - ) + kalman_filter = KalmanFilter2D() # Enable sensors imu.enable_gyroscope() @@ -47,9 +45,9 @@ gx_t, gy_t, gz_t = imu_transform.transform_gyro(gyro_x, gyro_y, gyro_z) # Update filter - roll, pitch, yaw = kalman_filter.update(ax_t, ay_t, az_t, gx_t, gy_t, gz_t) + roll, pitch, roll_rate, pitch_rate, yaw = kalman_filter.update(ax_t, ay_t, az_t, gx_t, gy_t, gz_t) - imu_logger.info(f"Time: {t:.4f}; Roll: {roll:+7.3f}, Pitch: {pitch:+7.3f}, Yaw: {yaw:+7.3f}") + imu_logger.info(f"Time: {t:.4f}; Roll: {roll:+7.3f}, Pitch: {pitch:+7.3f}, Roll Rate: {roll_rate:+7.3f}, Pitch Rate: {pitch_rate:+7.3f}, Yaw: {yaw:+7.3f}") imu_logger.update() # Stop IMU From 1fde3e0289740cbfaf5d1e138eab3487f2fa599e Mon Sep 17 00:00:00 2001 From: Katharine-Walters <111811694+Katharine-Walters@users.noreply.github.com> Date: Wed, 4 Feb 2026 20:36:25 +0000 Subject: [PATCH 14/27] fixed quality --- opensourceleg/actuators/base.py | 2 +- opensourceleg/actuators/tmotor.py | 143 +++++++++--------- opensourceleg/utilities/filters.py | 6 +- .../actuators/tmotor/position_control.py | 11 +- tutorials/actuators/tmotor/torque_control.py | 4 +- .../torque_control_multiple_actuators.py | 3 +- .../actuators/tmotor/velocity_control.py | 2 +- uv.lock | 2 +- 8 files changed, 87 insertions(+), 86 deletions(-) diff --git a/opensourceleg/actuators/base.py b/opensourceleg/actuators/base.py index 186cb249..2371939f 100644 --- a/opensourceleg/actuators/base.py +++ b/opensourceleg/actuators/base.py @@ -1203,4 +1203,4 @@ def is_streaming(self) -> bool: >>> actuator.is_streaming True """ - return self._is_streaming \ No newline at end of file + return self._is_streaming diff --git a/opensourceleg/actuators/tmotor.py b/opensourceleg/actuators/tmotor.py index ead204b5..f8c2d6a6 100644 --- a/opensourceleg/actuators/tmotor.py +++ b/opensourceleg/actuators/tmotor.py @@ -1,14 +1,10 @@ import time -import traceback import warnings from dataclasses import dataclass -from enum import Enum -from typing import Any, Optional, cast, Callable +from typing import Any, Optional, cast import can import numpy as np -import subprocess - from opensourceleg.actuators.base import ( CONTROL_MODE_CONFIGS, @@ -24,7 +20,6 @@ ) from opensourceleg.logging import LOGGER from opensourceleg.math import ThermalModel -from opensourceleg.utilities import SoftRealtimeLoop # TMotor servo mode error codes TMOTOR_ERROR_CODES: dict[int, str] = { @@ -68,8 +63,8 @@ "P_max": 3200, # 3200 deg "V_min": -100000, # ERPM "V_max": 100000, # ERPM - "Curr_min": -60000, # -60A - "Curr_max": 60000, # 60A + "Curr_min": -60000, # -60A + "Curr_max": 60000, # 60A "Kt_actual": 0.206, # Nm/A "GEAR_RATIO": 9.0, "NUM_POLE_PAIRS": 21, @@ -79,8 +74,8 @@ "P_max": 3200, # 3200 deg "V_min": -32000, # ERPM "V_max": 32000, # ERPM - "Curr_min": -60000, # -60A - "Curr_max": 60000, # 60A + "Curr_min": -60000, # -60A + "Curr_max": 60000, # 60A "Kt_actual": 0.095, # Nm/A "GEAR_RATIO": 9.0, "NUM_POLE_PAIRS": 21, @@ -91,15 +86,17 @@ TMOTOR_SERVO_CONSTANTS = MOTOR_CONSTANTS( MOTOR_COUNT_PER_REV=65536, # Encoder counts per revolution (16-bit encoder) NM_PER_AMP=0.095, # Placeholder to satisfy validation - MAX_CASE_TEMPERATURE=80.0, # Temperature parameters are also set in the driver via R-Link - MAX_WINDING_TEMPERATURE=110.0, # Temperature parameters are also set in the driver via R-Link + MAX_CASE_TEMPERATURE=80.0, # Temperature parameters are also set in the driver via R-Link + MAX_WINDING_TEMPERATURE=110.0, # Temperature parameters are also set in the driver via R-Link WINDING_SOFT_LIMIT=100.0, # Soft limits set 10°C below hard limits for safety margin CASE_SOFT_LIMIT=70.0, ) + @dataclass class ServoMotorState: """Motor state data structure""" + position: float = 0.0 # degrees velocity: float = 0.0 # ERPM current: float = 0.0 # milliamps @@ -124,7 +121,7 @@ class CANManagerServo: $ sudo /sbin/ip link set can0 up type can bitrate 1000000 $ sudo ifconfig can0 txqueuelen 1000 - Attributes: + Attributes: debug (bool): If true, logs detailed information about sent and received messages. bus (can.interface.Bus): The underlying python-can bus interface notifier (can.Notifier): Manages message listeners for asynchronous updates @@ -158,11 +155,13 @@ def __init__(self) -> None: self._initialized = True except Exception as e: - LOGGER.error(f"CAN bus initialization failed: {e}" - "Please ensure CAN interface is configured. Run:" - "sudo /sbin/ip link set can0 down" - "sudo /sbin/ip link set can0 up type can bitrate 1000000" - "sudo ifconfig can0 txqueuelen 1000") + LOGGER.error( + f"CAN bus initialization failed: {e}" + "Please ensure CAN interface is configured. Run:" + "sudo /sbin/ip link set can0 down" + "sudo /sbin/ip link set can0 up type can bitrate 1000000" + "sudo ifconfig can0 txqueuelen 1000" + ) raise RuntimeError("CAN bus initialization failed. Please configure CAN interface first.") from e def __enter__(self) -> None: @@ -170,7 +169,7 @@ def __enter__(self) -> None: return self def __exit__(self, exc_type, exc_val, exc_tb) -> None: - """ Context manager exit""" + """Context manager exit""" self.close() def close(self) -> None: @@ -179,7 +178,7 @@ def close(self) -> None: """ try: LOGGER.info("Closing CAN Manager...") - + # Stop and remove all listeners first if hasattr(self, "_listeners"): for listener in self._listeners: @@ -188,7 +187,7 @@ def close(self) -> None: except Exception as e: LOGGER.warning(f"Error removing listener: {e}") self._listeners.clear() - + # Stop the notifier if hasattr(self, "notifier"): self.notifier.stop() @@ -197,14 +196,13 @@ def close(self) -> None: # Shutdown the bus if hasattr(self, "bus"): - self.bus.shutdown() - + self.bus.shutdown() + LOGGER.info("CAN Manager closed successfully") except Exception as e: LOGGER.warning(f"Error closing CAN manager: {e}") - def send_message(self, motor_id: int, data: list, data_len: int) -> None: """Send CAN message""" if data_len > 8: @@ -232,9 +230,9 @@ def set_current(self, motor_id: int, current: float) -> None: """ Send current control command Args: - motor_id (int): CAN motor ID + motor_id (int): CAN motor ID current (float): motor current in mA - Returns: + Returns: None """ buffer = self._pack_int32(int(current)) @@ -263,7 +261,7 @@ def set_position(self, motor_id: int, position: float) -> None: Returns: None """ - buffer = self._pack_int32(int(position * 10000.0)) + buffer = self._pack_int32(int(position * 10000.0)) message_id = (CAN_PACKET_ID["SET_POS"] << 8) | motor_id self.send_message(message_id, buffer, len(buffer)) @@ -291,7 +289,7 @@ def set_control_mode(self, motor_id: int, mode: int) -> None: None """ buffer = [mode] - message_id = (CAN_PACKET_ID["SET_MODE"] << 8) | motor_id + message_id = (CAN_PACKET_ID["SET_MODE"] << 8) | motor_id self.send_message(message_id, buffer, len(buffer)) @staticmethod @@ -315,7 +313,7 @@ def parse_servo_message(self, data: bytes) -> ServoMotorState: motor_pos = float(pos_int * 0.1) # position (degrees) motor_spd = float(spd_int * 10.0) # velocity (ERPM) motor_cur = float(cur_int * 10) # current (mA) - motor_temp = float(int.from_bytes(data[6:7], byteorder='big', signed=True)) # temperature (celsius) + motor_temp = float(int.from_bytes(data[6:7], byteorder="big", signed=True)) # temperature (celsius) motor_error = int(data[7]) # error code return ServoMotorState(motor_pos, motor_spd, motor_cur, motor_temp, motor_error) @@ -327,7 +325,6 @@ def add_motor_listener(self, motor: "TMotorServoActuator") -> None: self._listeners.append(listener) LOGGER.debug(f"Added listener for motor ID {motor.motor_id}") - def remove_motor_listener(self, motor: "TMotorServoActuator") -> None: """Remove motor listener""" for listener in self._listeners[:]: # Create a copy to iterate @@ -339,7 +336,6 @@ def remove_motor_listener(self, motor: "TMotorServoActuator") -> None: except Exception as e: LOGGER.warning(f"Error removing listener for motor {motor.motor_id}: {e}") - def enable_debug(self, enable: bool = True) -> None: """Enable/disable debug mode""" self.debug = enable @@ -358,7 +354,7 @@ class MotorListener(can.Listener): Args: canman (CANManagerServo): The CAN manager instance responsible for parsing raw message data into servo states. - motor (TMotorServoActuator): The specific motor instance that this + motor (TMotorServoActuator): The specific motor instance that this listener monitors and updates. """ @@ -369,8 +365,8 @@ def __init__(self, canman: CANManagerServo, motor: "TMotorServoActuator") -> Non def on_message_received(self, msg: can.Message) -> None: """ Callback triggered when a CAN message is received. - - Checks if the message's arbitration ID matches the assigned motor's ID. + + Checks if the message's arbitration ID matches the assigned motor's ID. If it matches, the message data is parsed and the motor's state is updated. Args: @@ -382,7 +378,6 @@ def on_message_received(self, msg: can.Message) -> None: self.motor._update_state_async(self.canman.parse_servo_message(data)) - # Simplified unit conversion functions def degrees_to_radians(degrees: float) -> float: """Convert degrees to radians""" @@ -419,8 +414,10 @@ def _wait_for_mode_switch(actuator: "TMotorServoActuator", timeout: float = 0.2) # 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: LOGGER.debug(msg=f"[{actuator.__str__()}] Entering Position control mode.") mode_id = 4 @@ -428,9 +425,11 @@ def _servo_position_mode_entry(actuator: "TMotorServoActuator") -> None: actuator._canman.set_control_mode(actuator.motor_id, mode_id) _wait_for_mode_switch(actuator) + def _servo_position_mode_exit(actuator: "TMotorServoActuator") -> None: LOGGER.debug(msg=f"[{actuator.__str__()}] Exiting Position control mode.") + def _servo_current_mode_entry(actuator: "TMotorServoActuator") -> None: LOGGER.debug(msg=f"[{actuator.__str__()}] Entering Current control mode.") mode_id = 1 @@ -438,11 +437,13 @@ def _servo_current_mode_entry(actuator: "TMotorServoActuator") -> None: actuator._canman.set_control_mode(actuator.motor_id, mode_id) _wait_for_mode_switch(actuator) + def _servo_current_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 @@ -450,11 +451,13 @@ def _servo_velocity_mode_entry(actuator: "TMotorServoActuator") -> None: actuator._canman.set_control_mode(actuator.motor_id, mode_id) _wait_for_mode_switch(actuator) + def _servo_velocity_mode_exit(actuator: "TMotorServoActuator") -> None: LOGGER.debug(msg=f"[{actuator.__str__()}] Exiting Velocity control mode.") 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: LOGGER.debug(msg=f"[{actuator.__str__()}] Entering Idle control mode.") mode_id = 7 @@ -462,6 +465,7 @@ def _servo_idle_mode_entry(actuator: "TMotorServoActuator") -> None: actuator._canman.set_control_mode(actuator.motor_id, mode_id) _wait_for_mode_switch(actuator) + def _servo_idle_mode_exit(actuator: "TMotorServoActuator") -> None: LOGGER.debug(msg=f"[{actuator.__str__()}] Exiting Idle control mode.") @@ -491,12 +495,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 + IMPEDANCE=None, # IMPEDANCE mode not supported + VOLTAGE=None, # VOLTAGE mode not supported ) - class TMotorServoActuator(ActuatorBase): """ TMotor servo mode actuator for AK series motors. @@ -523,7 +526,7 @@ def __init__( frequency: int = 1000, offline: bool = False, current_mode: str = "driver", - thermal_current_scale: float = 1.0, + thermal_current_scale: float = 1.0, **kwargs: Any, ) -> None: """ @@ -545,7 +548,7 @@ def __init__( # Validate motor type if motor_type not in TMOTOR_MODELS: raise ValueError(f"Unsupported motor type: {motor_type}") - # Validate gear ratio matches tmotor model + # Validate gear ratio matches tmotor model if TMOTOR_MODELS[motor_type]["GEAR_RATIO"] != gear_ratio: raise ValueError(f"Gear ratio {gear_ratio} does not match {motor_type}") # Validate current mode @@ -562,7 +565,7 @@ def __init__( frequency=frequency, offline=offline, ) - + # Override motor constants to ensure setter is called self.MOTOR_CONSTANTS = TMOTOR_SERVO_CONSTANTS @@ -614,8 +617,8 @@ def __init__( # Thermal management self._thermal_model = ThermalModel( - motor_constants = self._MOTOR_CONSTANTS, - actuator_tag = self.tag, + motor_constants=self._MOTOR_CONSTANTS, + actuator_tag=self.tag, ) LOGGER.info(f"Initialized TMotor servo: {self.motor_type} ID:{self.motor_id}") @@ -703,17 +706,17 @@ def stop(self) -> None: time.sleep(0.05) # Give motor time to process except Exception as e: LOGGER.warning(f"Error setting idle mode during stop: {e}") - + # Send power off command try: self._canman.power_off(self.motor_id) time.sleep(0.05) # Give motor time to power off except Exception as e: LOGGER.warning(f"Error powering off motor: {e}") - + # Remove this motor's listener self._canman.remove_motor_listener(self) - + # Close CAN manager (will only actually close if this is the last motor) self._canman.close() @@ -733,13 +736,13 @@ def update(self) -> None: 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 + self._motor_state.error = self._motor_state_async.error # Temperature check self._thermal_scale = self._thermal_model.update( - dt = 1 / self.frequency, - motor_current = self.motor_current*self._thermal_current_scale, - case_temperature = self.case_temperature, + dt=1 / self.frequency, + motor_current=self.motor_current * self._thermal_current_scale, + case_temperature=self.case_temperature, ) # Communication timeout check @@ -756,7 +759,7 @@ def update(self) -> None: def _update_state_async(self, servo_state: ServoMotorState) -> None: """ Asynchronously update state and interprets errors. - + Returns: None """ @@ -794,7 +797,7 @@ def home(self) -> None: """ This method homes the actuator and the corresponding joint by moving it to the zero position. The zero position is defined as the position where the joint is fully extended. - Returns: + Returns: None """ raise NotImplementedError("Homing not implemented.") @@ -848,7 +851,7 @@ def set_motor_voltage(self, value: float) -> None: def set_motor_current(self, value: float) -> None: """ Set motor current with clamping to motor limits - Args: + Args: value (float): desired motor current in mA """ if not self.is_offline and self._canman: @@ -856,7 +859,7 @@ def set_motor_current(self, value: float) -> None: # Get current limits from motor parameters max_current = self._motor_params["Curr_max"] - min_current = self._motor_params["Curr_min"] + min_current = self._motor_params["Curr_min"] # Clamp current to safe limits clamped_driver_current = np.clip(driver_current, min_current, max_current) @@ -866,7 +869,7 @@ def set_motor_current(self, value: float) -> None: 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)" + f"(limits: [{min_current * self._current_scale:.1f}, {max_current * self._current_scale:.1f}]mA)" ) self._canman.set_current(self.motor_id, clamped_driver_current) @@ -874,24 +877,23 @@ def set_motor_current(self, value: float) -> None: def set_motor_position(self, value: float) -> None: raise NotImplementedError( - "Setting motor position not supported" - "Recommended to use 'set_output_position' command instead." + "Setting motor position not supported" "Recommended to use 'set_output_position' command instead." ) def set_output_position(self, value: float) -> None: """ Set motor position (radians) with clamping to motor limits - Args: - value (float): desired motor position - Return: + Args: + value (float): desired motor position + Return: None """ position_deg = radians_to_degrees(value) if not self.is_offline and self._canman: # Get position limits from motor parameters - max_position = self._motor_params["P_max"] - min_position = self._motor_params["P_min"] + max_position = self._motor_params["P_max"] + min_position = self._motor_params["P_min"] # Clamp position to safe limits clamped_position = np.clip(position_deg, min_position, max_position) @@ -908,18 +910,18 @@ def set_output_position(self, value: float) -> None: def set_motor_torque(self, value: float) -> None: """ - Sets the motor torque in Nm. + Sets the motor 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: + value (float): The torque to set in Nm. + Returns: None """ # 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 * 1000 - self.set_motor_current(current) # Send current command mA + self.set_motor_current(current) # Send current command mA def set_output_torque(self, value: float) -> None: """ @@ -977,9 +979,7 @@ def set_position_gains(self, kp: float, ki: float, kd: float, ff: float) -> None def _set_impedance_gains(self, k: float, b: float) -> None: """Internal method for impedance gains - not supported in TMotor servo mode""" - raise NotImplementedError( - "TMotor servo mode handles control internally. " "Impedance gains are not used." - ) + raise NotImplementedError("TMotor servo mode handles control internally. " "Impedance gains are not used.") # ============ State Properties ============ @@ -1027,7 +1027,7 @@ def motor_current(self) -> float: Returns: float: Motor current in mA """ - return self._motor_state.current * self._current_scale + return self._motor_state.current * self._current_scale @property def motor_torque(self) -> float: @@ -1036,7 +1036,7 @@ def motor_torque(self) -> float: This is calculated using motor current and k_t. """ kt_user = cast(float, self._motor_params["Kt_actual"]) * self._kt_scale - return self.motor_current * kt_user / 1000 + return self.motor_current * kt_user / 1000 @property def output_torque(self) -> float: @@ -1079,5 +1079,6 @@ def thermal_scaling_factor(self) -> float: """ return self._thermal_scale + if __name__ == "__main__": pass diff --git a/opensourceleg/utilities/filters.py b/opensourceleg/utilities/filters.py index 4ebfa082..dc3bf8d7 100644 --- a/opensourceleg/utilities/filters.py +++ b/opensourceleg/utilities/filters.py @@ -1,7 +1,9 @@ import math import time + import numpy as np + class KalmanFilter2D: """ 2D Kalman filter to estimate global roll and pitch angles and angular rates @@ -25,7 +27,7 @@ class KalmanFilter2D: Author: Katharine Walters - + Date: 01/26/2026 """ @@ -142,7 +144,7 @@ def update(self, ax: float, ay: float, az: float, gx: float, gy: float, gz: floa y[0] = self._normalize_angle(y[0]) y[1] = self._normalize_angle(y[1]) - # Update + # Update self.x = self.x + K @ y I_KH = np.eye(6) - K @ H self.P = I_KH @ self.P @ I_KH.T + K @ self.R @ K.T diff --git a/tutorials/actuators/tmotor/position_control.py b/tutorials/actuators/tmotor/position_control.py index 9ec235fe..61a7e857 100644 --- a/tutorials/actuators/tmotor/position_control.py +++ b/tutorials/actuators/tmotor/position_control.py @@ -1,12 +1,11 @@ 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 -STEP_AMPLITUDE = 0.3 # Radians +STEP_AMPLITUDE = 0.3 # Radians FREQUENCY = 200 DT = 1 / FREQUENCY MOTOR_ID = 104 # Change this to match your motor's CAN ID @@ -29,15 +28,13 @@ def position_control(): clock = SoftRealtimeLoop(dt=DT) with motor: - motor.update() print("Setting motor origin...") motor.set_origin() print(f"Current motor position {motor.output_position:.3f}") - - # Set to position control mode (PID parameters are built-in and + # Set to position control mode (PID parameters are built-in and # can only be modified via R-link) motor.set_control_mode(mode=CONTROL_MODES.POSITION) @@ -49,11 +46,11 @@ def position_control(): position_logger.track_function(lambda: time.monotonic(), "Time") print(f"Starting staircase step response (Step: {STEP_AMPLITUDE})...") - + for t in clock: # Calculate how many steps have passed step_count = int(t) - + # Continuously increment position based on step count command_position = initial_position + (step_count * STEP_AMPLITUDE) diff --git a/tutorials/actuators/tmotor/torque_control.py b/tutorials/actuators/tmotor/torque_control.py index d48df59b..eccdd91e 100644 --- a/tutorials/actuators/tmotor/torque_control.py +++ b/tutorials/actuators/tmotor/torque_control.py @@ -1,4 +1,5 @@ import time + import numpy as np from opensourceleg.actuators.base import CONTROL_MODES @@ -28,7 +29,6 @@ def torque_control(): clock = SoftRealtimeLoop(dt=DT) with motor: - motor.update() # Set the encoder origin first (optional) @@ -77,7 +77,7 @@ def torque_control(): # Run for 10 seconds if t > 10.0: break - + print("Torque control complete") # Stop the motor diff --git a/tutorials/actuators/tmotor/torque_control_multiple_actuators.py b/tutorials/actuators/tmotor/torque_control_multiple_actuators.py index 393e79bf..7339a601 100644 --- a/tutorials/actuators/tmotor/torque_control_multiple_actuators.py +++ b/tutorials/actuators/tmotor/torque_control_multiple_actuators.py @@ -1,5 +1,5 @@ import time -import matplotlib.pyplot as plt + import numpy as np from opensourceleg.actuators.base import CONTROL_MODES @@ -118,5 +118,6 @@ def torque_control(): motor2.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 index c1f4077f..4fa2dc47 100644 --- a/tutorials/actuators/tmotor/velocity_control.py +++ b/tutorials/actuators/tmotor/velocity_control.py @@ -1,4 +1,5 @@ import time + import numpy as np from opensourceleg.actuators.base import CONTROL_MODES @@ -28,7 +29,6 @@ def velocity_control(): clock = SoftRealtimeLoop(dt=DT) with motor: - motor.update() # Set the encoder origin first (optional) diff --git a/uv.lock b/uv.lock index 2def35b4..718fe31d 100644 --- a/uv.lock +++ b/uv.lock @@ -914,7 +914,7 @@ wheels = [ [[package]] name = "opensourceleg" -version = "3.4.2" +version = "3.5.0" source = { editable = "." } dependencies = [ { name = "numpy" }, From f535bf8067583a06ebe48a70c4bc496e40996713 Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Thu, 5 Feb 2026 11:50:44 -0500 Subject: [PATCH 15/27] fixed current scale issue, removed NotImplementedErrors so tmotor is hot swappable with dephy --- opensourceleg/actuators/tmotor.py | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/opensourceleg/actuators/tmotor.py b/opensourceleg/actuators/tmotor.py index ead204b5..824f90a0 100644 --- a/opensourceleg/actuators/tmotor.py +++ b/opensourceleg/actuators/tmotor.py @@ -81,7 +81,7 @@ "V_max": 32000, # ERPM "Curr_min": -60000, # -60A "Curr_max": 60000, # 60A - "Kt_actual": 0.095, # Nm/A + "Kt_actual": 0.0952, # Nm/A "GEAR_RATIO": 9.0, "NUM_POLE_PAIRS": 21, }, @@ -579,12 +579,12 @@ def __init__( # 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": + if self.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": + elif self.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) @@ -852,7 +852,7 @@ def set_motor_current(self, value: float) -> None: value (float): desired motor current in mA """ if not self.is_offline and self._canman: - driver_current = value / self._current_scale + driver_current = value # Get current limits from motor parameters max_current = self._motor_params["Curr_max"] @@ -963,21 +963,21 @@ def set_output_velocity(self, value: float) -> None: # ============ Unsupported PID Functions - TMotor Servo Mode Handles All Control Loops Internally ============ - def set_current_gains(self, kp: float, ki: float, kd: float, ff: float) -> None: + def set_current_gains(self, kp: float = 0.0, ki: float = 0.0, kd: float = 0.0, ff: float = 0.0) -> None: """TMotor servo mode does not support external current PID gains - motor handles current control internally""" - raise NotImplementedError( + LOGGER.warning( "TMotor servo mode handles current control internally. " "External current PID gains are not used." ) - def set_position_gains(self, kp: float, ki: float, kd: float, ff: float) -> None: + def set_position_gains(self, kp: float = 0.0, ki: float = 0.0, kd: float = 0.0, ff: float = 0.0) -> None: """TMotor servo mode does not support external position PID gains - motor handles position control internally""" - raise NotImplementedError( + LOGGER.warning( "TMotor servo mode handles position control internally. " "External position PID gains are not used." ) - def _set_impedance_gains(self, k: float, b: float) -> None: + def _set_impedance_gains(self, k: float = 0.0, b: float = 0.0) -> None: """Internal method for impedance gains - not supported in TMotor servo mode""" - raise NotImplementedError( + LOGGER.warning( "TMotor servo mode handles control internally. " "Impedance gains are not used." ) @@ -986,10 +986,11 @@ def _set_impedance_gains(self, k: float, b: float) -> None: @property def motor_position(self) -> float: """Motor position (radians) - not supported""" - raise NotImplementedError( + LOGGER.warning( "Motor position reading is not available. " "The position returned in the motor state corresponds to the output position." ) + return 0.0 @property def output_position(self) -> float: @@ -1014,10 +1015,11 @@ def num_pole_pairs(self) -> int: @property def motor_voltage(self) -> float: """Motor voltage - not available in servo mode""" - raise NotImplementedError( + LOGGER.warning( "Motor voltage reading is not available in TMotor servo mode. " "The motor does not provide voltage feedback through the CAN protocol." ) + return 0.0 @property def motor_current(self) -> float: From 7f8fbe668dd969e92fb847c8bff112593dc9bdfb Mon Sep 17 00:00:00 2001 From: Katharine-Walters <111811694+Katharine-Walters@users.noreply.github.com> Date: Thu, 5 Feb 2026 11:55:26 -0500 Subject: [PATCH 16/27] removed whitespace --- opensourceleg/actuators/tmotor.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/opensourceleg/actuators/tmotor.py b/opensourceleg/actuators/tmotor.py index 1ec7de3d..442d1304 100644 --- a/opensourceleg/actuators/tmotor.py +++ b/opensourceleg/actuators/tmotor.py @@ -74,8 +74,8 @@ "P_max": 3200, # 3200 deg "V_min": -32000, # ERPM "V_max": 32000, # ERPM - "Curr_min": -60000, # -60A - "Curr_max": 60000, # 60A + "Curr_min": -60000, # -60A + "Curr_max": 60000, # 60A "Kt_actual": 0.0952, # Nm/A "GEAR_RATIO": 9.0, "NUM_POLE_PAIRS": 21, @@ -1086,3 +1086,4 @@ def thermal_scaling_factor(self) -> float: if __name__ == "__main__": pass + From 302d1d86be7d7b8e92ca9da87d3a04d84a2eb616 Mon Sep 17 00:00:00 2001 From: Katharine-Walters <111811694+Katharine-Walters@users.noreply.github.com> Date: Thu, 5 Feb 2026 17:01:29 +0000 Subject: [PATCH 17/27] fixed quality issues --- opensourceleg/actuators/tmotor.py | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/opensourceleg/actuators/tmotor.py b/opensourceleg/actuators/tmotor.py index 442d1304..c91972d2 100644 --- a/opensourceleg/actuators/tmotor.py +++ b/opensourceleg/actuators/tmotor.py @@ -855,7 +855,7 @@ def set_motor_current(self, value: float) -> None: value (float): desired motor current in mA """ if not self.is_offline and self._canman: - driver_current = value + driver_current = value # Get current limits from motor parameters max_current = self._motor_params["Curr_max"] @@ -979,9 +979,7 @@ def set_position_gains(self, kp: float = 0.0, ki: float = 0.0, kd: float = 0.0, def _set_impedance_gains(self, k: float = 0.0, b: float = 0.0) -> None: """Internal method for impedance gains - not supported in TMotor servo mode""" - LOGGER.warning( - "TMotor servo mode handles control internally. " "Impedance gains are not used." - ) + LOGGER.warning("TMotor servo mode handles control internally. " "Impedance gains are not used.") # ============ State Properties ============ @@ -1086,4 +1084,3 @@ def thermal_scaling_factor(self) -> float: if __name__ == "__main__": pass - From abee1529e9f8db3f9a8e62f361d5f183c39f40d0 Mon Sep 17 00:00:00 2001 From: Katharine-Walters <111811694+Katharine-Walters@users.noreply.github.com> Date: Mon, 9 Feb 2026 20:09:49 +0000 Subject: [PATCH 18/27] replaced time.time() with time.monotonic() --- opensourceleg/actuators/tmotor.py | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/opensourceleg/actuators/tmotor.py b/opensourceleg/actuators/tmotor.py index c91972d2..6ae5027b 100644 --- a/opensourceleg/actuators/tmotor.py +++ b/opensourceleg/actuators/tmotor.py @@ -401,13 +401,13 @@ def rad_per_sec_to_erpm(rad_per_sec: float, num_pole_pairs: int) -> float: def _wait_for_mode_switch(actuator: "TMotorServoActuator", timeout: float = 0.2) -> None: """Wait for control mode switch confirmation with timeout""" - start_time = time.time() + start_time = time.monotonic() poll_interval = 0.01 # 10ms polling - while time.time() - start_time < timeout: + while time.monotonic() - 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") + LOGGER.debug(f"Mode switch confirmed after {time.monotonic() - start_time:.3f} seconds") return time.sleep(poll_interval) @@ -611,7 +611,7 @@ def __init__( self._error_message: Optional[str] = None # Time management - self._start_time = time.time() + self._start_time = time.monotonic() self._last_update_time = self._start_time self._last_command_time: Optional[float] = None @@ -645,12 +645,12 @@ def start(self) -> None: self._canman.power_on(self.motor_id) # Poll for motor readiness (max 1 second timeout) - start_time = time.time() + start_time = time.monotonic() timeout = 1.0 poll_interval = 0.01 # 10ms polling interval motor_ready = False - while time.time() - start_time < timeout: + while time.monotonic() - 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) @@ -661,7 +661,7 @@ def start(self) -> None: or self._motor_state_async.temperature > 0.0 ): motor_ready = True - LOGGER.debug(f"Motor ready after {time.time() - start_time:.3f} seconds") + LOGGER.debug(f"Motor ready after {time.monotonic() - start_time:.3f} seconds") break time.sleep(poll_interval) @@ -674,13 +674,13 @@ def start(self) -> None: self.set_control_mode(CONTROL_MODES.IDLE) # Poll for mode switch confirmation (max 200ms timeout) - mode_start_time = time.time() + mode_start_time = time.monotonic() mode_timeout = 0.2 - while time.time() - mode_start_time < mode_timeout: + while time.monotonic() - 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") + LOGGER.debug(f"Mode switch confirmed after {time.monotonic() - mode_start_time:.3f} seconds") break time.sleep(poll_interval) @@ -746,7 +746,7 @@ def update(self) -> None: ) # Communication timeout check - now = time.time() + now = time.monotonic() if ( self._last_command_time is not None and (now - self._last_command_time) < 0.25 @@ -873,7 +873,7 @@ def set_motor_current(self, value: float) -> None: ) self._canman.set_current(self.motor_id, clamped_driver_current) - self._last_command_time = time.time() + self._last_command_time = time.monotonic() def set_motor_position(self, value: float) -> None: raise NotImplementedError( @@ -906,7 +906,7 @@ def set_output_position(self, value: float) -> None: ) self._canman.set_position(self.motor_id, clamped_position) - self._last_command_time = time.time() + self._last_command_time = time.monotonic() def set_motor_torque(self, value: float) -> None: """ @@ -956,7 +956,7 @@ def set_motor_velocity(self, value: float) -> None: ) self._canman.set_velocity(self.motor_id, clamped_velocity) - self._last_command_time = time.time() + self._last_command_time = time.monotonic() def set_output_velocity(self, value: float) -> None: """Set output velocity (rad/s)""" From cd12fe0653041ceeff874db86788961d6bab2066 Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Tue, 10 Feb 2026 11:55:09 -0500 Subject: [PATCH 19/27] added handling for unavailable sensor data in fifo read --- opensourceleg/sensors/imu.py | 37 +++++++++++++++++++++++++++--------- 1 file changed, 28 insertions(+), 9 deletions(-) diff --git a/opensourceleg/sensors/imu.py b/opensourceleg/sensors/imu.py index 7124a1e3..477e2e6b 100644 --- a/opensourceleg/sensors/imu.py +++ b/opensourceleg/sensors/imu.py @@ -594,7 +594,7 @@ def start(self) -> None: try: self._adafruit_imu = self.adafruit_bno055.BNO055_I2C(i2c, address=self._address) except ValueError: - print("BNO055 IMU Not Found on i2c bus! Check wiring!") + LOGGER.error("BNO055 IMU Not Found on i2c bus! Check wiring!") self.configure_IMU_settings() self._is_streaming = True @@ -801,6 +801,7 @@ class BHI260AP(IMUBase): "Gravity": SENSOR_ID_GRAVITY, "LinAccel": SENSOR_ID_LIN_ACC, } + _SENSOR_ID_TO_NAME: ClassVar[dict[int, str]] = {v: k for k, v in SENSOR_DICT.items()} def __init__( self, @@ -849,7 +850,11 @@ def __init__( self._is_streaming = False self._enabled_sensors: dict[int, float] = {} - self._sensor_data: list[dict[str, Union[int, float]]] = [] + self._sensor_data: dict[int, dict[Union[int, float]]] = {} + + # Tracker for stale data + self._stale_data_tracker: dict[int, int] = {} + self._stale_threshold = 2 def start(self) -> None: """ @@ -884,7 +889,7 @@ def stop(self) -> None: try: self._disable_sensor(sensor_id) except Exception as e: - print(f"Warning: Could not disable sensor {sensor_id}: {e}") + LOGGER.error(f"Warning: Could not disable sensor {sensor_id}: {e}") self._spi.close() self._is_streaming = False LOGGER.info("IMU stopped successfully.") @@ -1061,10 +1066,11 @@ def _enable_sensor( # Modify list of enabled sensors self._enabled_sensors[sensor_id] = scale + self._stale_data_tracker[sensor_id] = 0 # Initialize stale data tracker # Add 0 data to sensor data list - sample = {"sensor_id": sensor_id, "timestamp": 0.0, "x": 0.0, "y": 0.0, "z": 0.0} - self._sensor_data.append(sample) + sample = [{"timestamp": 0.0, "x": 0.0, "y": 0.0, "z": 0.0}] + self._sensor_data[sensor_id] = sample # TODO: Implement this functionality def _change_sensor_dynamic_range(self, sensor_id: int, dynamic_range: int) -> None: @@ -1163,14 +1169,27 @@ def enable_gravity(self, rate_hz: int = -1, dynamic_range: int = 4096) -> None: def update(self) -> None: """ Read data in buffer and save to class - """ + """ + # Read and parse FIFO raw_data = self._read_fifo() parsed_data = self._parse_fifo(raw_data) if not parsed_data and not self._enabled_sensors: - print("BHI260AP Warning: no sensors enabled.") + LOGGER.warning("BHI260AP: No sensors enabled.") - self._sensor_data = parsed_data + for sensor_id in self._enabled_sensors: + data_samples = [{"x": s["x"], "y": s["y"], "z": s["z"]} for s in parsed_data if s.get("sensor_id")==sensor_id] + if not data_samples: + self._stale_data_tracker[sensor_id] = self._stale_data_tracker[sensor_id] + 1 + else: + self._sensor_data[sensor_id] = data_samples + self._stale_data_tracker[sensor_id] = 0 + + # Check for stale data + for sensor_id, stale_count in self._stale_data_tracker.items(): + if stale_count > self._stale_threshold: + sensor_name = self._SENSOR_ID_TO_NAME.get(sensor_id) + LOGGER.warning(f"{sensor_name} data is stale! No new data for {stale_count} updates.") def _read_fifo(self, address: int = REG_CHAN2_NONWAKEUP_FIFO) -> bytes: """ @@ -1296,7 +1315,7 @@ def _get_sensor_data(self, sensor_id: int, most_recent: bool) -> np.ndarray: return np.array([]) # collect (x,y,z) tuples for samples matching sensor_id - data_samples = [(s["x"], s["y"], s["z"]) for s in self._sensor_data if s.get("sensor_id") == sensor_id] + data_samples = [(s["x"], s["y"], s["z"]) for s in self._sensor_data[sensor_id]] if not data_samples: return np.array([]) From 66228d8cab3c1fb5598c24701074b0b2299eea20 Mon Sep 17 00:00:00 2001 From: Katharine-Walters <111811694+Katharine-Walters@users.noreply.github.com> Date: Tue, 10 Feb 2026 17:10:27 +0000 Subject: [PATCH 20/27] made check --- opensourceleg/sensors/imu.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/opensourceleg/sensors/imu.py b/opensourceleg/sensors/imu.py index 477e2e6b..7a47774d 100644 --- a/opensourceleg/sensors/imu.py +++ b/opensourceleg/sensors/imu.py @@ -1066,7 +1066,7 @@ def _enable_sensor( # Modify list of enabled sensors self._enabled_sensors[sensor_id] = scale - self._stale_data_tracker[sensor_id] = 0 # Initialize stale data tracker + self._stale_data_tracker[sensor_id] = 0 # Initialize stale data tracker # Add 0 data to sensor data list sample = [{"timestamp": 0.0, "x": 0.0, "y": 0.0, "z": 0.0}] @@ -1169,7 +1169,7 @@ def enable_gravity(self, rate_hz: int = -1, dynamic_range: int = 4096) -> None: def update(self) -> None: """ Read data in buffer and save to class - """ + """ # Read and parse FIFO raw_data = self._read_fifo() parsed_data = self._parse_fifo(raw_data) @@ -1178,13 +1178,15 @@ def update(self) -> None: LOGGER.warning("BHI260AP: No sensors enabled.") for sensor_id in self._enabled_sensors: - data_samples = [{"x": s["x"], "y": s["y"], "z": s["z"]} for s in parsed_data if s.get("sensor_id")==sensor_id] + data_samples = [ + {"x": s["x"], "y": s["y"], "z": s["z"]} for s in parsed_data if s.get("sensor_id") == sensor_id + ] if not data_samples: self._stale_data_tracker[sensor_id] = self._stale_data_tracker[sensor_id] + 1 else: self._sensor_data[sensor_id] = data_samples self._stale_data_tracker[sensor_id] = 0 - + # Check for stale data for sensor_id, stale_count in self._stale_data_tracker.items(): if stale_count > self._stale_threshold: From 4583d426f526e86b7314d4b2c4585899df9e7623 Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Tue, 10 Feb 2026 13:04:57 -0500 Subject: [PATCH 21/27] fixing type and test issues --- opensourceleg/sensors/imu.py | 4 ++-- tests/test_sensors/test_bhi260ap_imu.py | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/opensourceleg/sensors/imu.py b/opensourceleg/sensors/imu.py index 7a47774d..317856c5 100644 --- a/opensourceleg/sensors/imu.py +++ b/opensourceleg/sensors/imu.py @@ -850,7 +850,7 @@ def __init__( self._is_streaming = False self._enabled_sensors: dict[int, float] = {} - self._sensor_data: dict[int, dict[Union[int, float]]] = {} + self._sensor_data: dict[int, list[dict[Union[int, float]]]] = {} # Tracker for stale data self._stale_data_tracker: dict[int, int] = {} @@ -1458,7 +1458,7 @@ def is_streaming(self) -> bool: return self._is_streaming @property - def data(self) -> list[dict]: + def data(self) -> dict: """Get the latest parsed sensor data packets.""" return self._sensor_data diff --git a/tests/test_sensors/test_bhi260ap_imu.py b/tests/test_sensors/test_bhi260ap_imu.py index cf09d96c..e53ede60 100644 --- a/tests/test_sensors/test_bhi260ap_imu.py +++ b/tests/test_sensors/test_bhi260ap_imu.py @@ -15,6 +15,7 @@ def __init__(self): self.bits_per_word = None self._is_open = False self._data = {} + self._stale_data_tracker = {} def open(self, bus: int, cs: int): self.bus = bus From 3fe3b7256d4b98dae705bf19ed59405a57dcd6e6 Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Tue, 10 Feb 2026 13:09:15 -0500 Subject: [PATCH 22/27] fixing type and test issues take 2 --- opensourceleg/sensors/imu.py | 2 +- tests/test_sensors/test_bhi260ap_imu.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/opensourceleg/sensors/imu.py b/opensourceleg/sensors/imu.py index 317856c5..64db4f6c 100644 --- a/opensourceleg/sensors/imu.py +++ b/opensourceleg/sensors/imu.py @@ -850,7 +850,7 @@ def __init__( self._is_streaming = False self._enabled_sensors: dict[int, float] = {} - self._sensor_data: dict[int, list[dict[Union[int, float]]]] = {} + self._sensor_data: dict[int, list[dict[str, Union[int, float]]]] = {} # Tracker for stale data self._stale_data_tracker: dict[int, int] = {} diff --git a/tests/test_sensors/test_bhi260ap_imu.py b/tests/test_sensors/test_bhi260ap_imu.py index e53ede60..760b031f 100644 --- a/tests/test_sensors/test_bhi260ap_imu.py +++ b/tests/test_sensors/test_bhi260ap_imu.py @@ -15,7 +15,6 @@ def __init__(self): self.bits_per_word = None self._is_open = False self._data = {} - self._stale_data_tracker = {} def open(self, bus: int, cs: int): self.bus = bus @@ -76,7 +75,8 @@ def __init__( self._firmware_path = firmware_path self._is_streaming = False self._enabled_sensors = {} - self._sensor_data = [] + self._sensor_data = {} + self._stale_data_tracker = {} # Use mock SPI instead of real spidev self._spi = MockSPI() From 9a61e05a256d0a235333fc892e12e396ff08a085 Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Tue, 10 Feb 2026 13:16:08 -0500 Subject: [PATCH 23/27] fix test and print issues --- tests/test_sensors/test_bhi260ap_imu.py | 4 ++-- tutorials/utilities/kalman_filter_imu_global_angles.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/tests/test_sensors/test_bhi260ap_imu.py b/tests/test_sensors/test_bhi260ap_imu.py index 760b031f..a4688113 100644 --- a/tests/test_sensors/test_bhi260ap_imu.py +++ b/tests/test_sensors/test_bhi260ap_imu.py @@ -315,12 +315,12 @@ def test_acc_z(sample_imu_with_data: MockBHI260AP): def test_gyro_property(sample_imu_with_data: MockBHI260AP): gyro = sample_imu_with_data.gyro - assert isinstance(gyro, np.ndarray) + assert isinstance(gyro, (list, dict)) def test_accel_property(sample_imu_with_data: MockBHI260AP): accel = sample_imu_with_data.accel - assert isinstance(accel, np.ndarray) + assert isinstance(accel, (list, dict)) # Test FIFO parsing diff --git a/tutorials/utilities/kalman_filter_imu_global_angles.py b/tutorials/utilities/kalman_filter_imu_global_angles.py index 4e17f80d..946f1245 100644 --- a/tutorials/utilities/kalman_filter_imu_global_angles.py +++ b/tutorials/utilities/kalman_filter_imu_global_angles.py @@ -51,7 +51,7 @@ imu_logger.info( f"Time: {t:.4f}; Roll: {roll:+7.3f}, Pitch: {pitch:+7.3f}, " - "Roll Rate: {roll_rate:+7.3f}, Pitch Rate: {pitch_rate:+7.3f}, Yaw: {yaw:+7.3f}" + f"Roll Rate: {roll_rate:+7.3f}, Pitch Rate: {pitch_rate:+7.3f}, Yaw: {yaw:+7.3f}" ) imu_logger.update() From 56193dfa15889ad17f67ce5efadfee7cc8d49e8b Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Tue, 10 Feb 2026 13:21:23 -0500 Subject: [PATCH 24/27] still fixing the test cases --- tests/test_sensors/test_bhi260ap_imu.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/tests/test_sensors/test_bhi260ap_imu.py b/tests/test_sensors/test_bhi260ap_imu.py index a4688113..f52916f9 100644 --- a/tests/test_sensors/test_bhi260ap_imu.py +++ b/tests/test_sensors/test_bhi260ap_imu.py @@ -141,10 +141,11 @@ def sample_imu_with_data(): imu.enable_accelerometer(rate_hz=200, dynamic_range=4096) # Simulate sensor data - imu._sensor_data = [ - {"sensor_id": BHI260AP.SENSOR_ID_GYR, "timestamp": 0.0, "x": 0.1, "y": 0.2, "z": 0.3}, - {"sensor_id": BHI260AP.SENSOR_ID_ACC, "timestamp": 0.0, "x": 1.0, "y": 2.0, "z": 3.0}, - ] + imu._sensor_data = { + BHI260AP.SENSOR_ID_GYR: [{"timestamp": 0.0, "x": 0.1, "y": 0.2, "z": 0.3}], + BHI260AP.SENSOR_ID_ACC: [{"sensor_id": , "timestamp": 0.0, "x": 1.0, "y": 2.0, "z": 3.0}], + } + return imu @@ -157,7 +158,7 @@ def test_init_default(sample_imu: MockBHI260AP): sample_imu._data_rate == 200, sample_imu._is_streaming is False, isinstance(sample_imu._enabled_sensors, dict), - isinstance(sample_imu._sensor_data, list), + isinstance(sample_imu._sensor_data, dict), ]) From 89b5a5a50611e71a00eb6f9174cab5c3d651d50d Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Tue, 10 Feb 2026 13:22:39 -0500 Subject: [PATCH 25/27] that was silly --- tests/test_sensors/test_bhi260ap_imu.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_sensors/test_bhi260ap_imu.py b/tests/test_sensors/test_bhi260ap_imu.py index f52916f9..b2e6d5c1 100644 --- a/tests/test_sensors/test_bhi260ap_imu.py +++ b/tests/test_sensors/test_bhi260ap_imu.py @@ -143,9 +143,9 @@ def sample_imu_with_data(): # Simulate sensor data imu._sensor_data = { BHI260AP.SENSOR_ID_GYR: [{"timestamp": 0.0, "x": 0.1, "y": 0.2, "z": 0.3}], - BHI260AP.SENSOR_ID_ACC: [{"sensor_id": , "timestamp": 0.0, "x": 1.0, "y": 2.0, "z": 3.0}], + BHI260AP.SENSOR_ID_ACC: [{"timestamp": 0.0, "x": 1.0, "y": 2.0, "z": 3.0}], } - + return imu From bdf4942a321e2615651b8cbfe9d4cec34e69184b Mon Sep 17 00:00:00 2001 From: Katharine Walters Date: Tue, 10 Feb 2026 13:25:21 -0500 Subject: [PATCH 26/27] fixed type issues --- tests/test_sensors/test_bhi260ap_imu.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tests/test_sensors/test_bhi260ap_imu.py b/tests/test_sensors/test_bhi260ap_imu.py index b2e6d5c1..89eba7af 100644 --- a/tests/test_sensors/test_bhi260ap_imu.py +++ b/tests/test_sensors/test_bhi260ap_imu.py @@ -280,7 +280,7 @@ def test_is_streaming(sample_imu: MockBHI260AP): def test_data_property(sample_imu_with_data: MockBHI260AP): data = sample_imu_with_data.data - assert isinstance(data, list) + assert isinstance(data, dict) assert len(data) == 2 @@ -316,12 +316,12 @@ def test_acc_z(sample_imu_with_data: MockBHI260AP): def test_gyro_property(sample_imu_with_data: MockBHI260AP): gyro = sample_imu_with_data.gyro - assert isinstance(gyro, (list, dict)) + assert isinstance(gyro, np.ndarray) def test_accel_property(sample_imu_with_data: MockBHI260AP): accel = sample_imu_with_data.accel - assert isinstance(accel, (list, dict)) + assert isinstance(accel, np.ndarray) # Test FIFO parsing From 8e64137c359cf3e18007ae6793e6ce6a7ca398ad Mon Sep 17 00:00:00 2001 From: Katharine-Walters <111811694+Katharine-Walters@users.noreply.github.com> Date: Tue, 10 Feb 2026 18:28:13 +0000 Subject: [PATCH 27/27] fixed quality --- tests/test_sensors/test_bhi260ap_imu.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_sensors/test_bhi260ap_imu.py b/tests/test_sensors/test_bhi260ap_imu.py index 89eba7af..f49f7bb1 100644 --- a/tests/test_sensors/test_bhi260ap_imu.py +++ b/tests/test_sensors/test_bhi260ap_imu.py @@ -142,7 +142,7 @@ def sample_imu_with_data(): # Simulate sensor data imu._sensor_data = { - BHI260AP.SENSOR_ID_GYR: [{"timestamp": 0.0, "x": 0.1, "y": 0.2, "z": 0.3}], + BHI260AP.SENSOR_ID_GYR: [{"timestamp": 0.0, "x": 0.1, "y": 0.2, "z": 0.3}], BHI260AP.SENSOR_ID_ACC: [{"timestamp": 0.0, "x": 1.0, "y": 2.0, "z": 3.0}], }