feat: Add TMotor servo mode actuator with native CAN protocol support #468
Conversation
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Implements complete TMotor servo mode control for AK series actuators (AK80-9, AK10-9) using native CAN communication protocol. ## Architecture ### Core Classes: - `TMotorServoActuator`: Main actuator class extending ActuatorBase - Manages motor state, control modes, and command interface - Implements thermal protection via ThermalModel - Tracks communication timeouts and error states - `CANManagerServo`: Singleton CAN communication manager - Configures CAN0 interface: 1MHz bitrate, txqueuelen 1000 - Handles message sending/receiving via python-can library - Power on/off commands: 0xFC (on), 0xFD (off) - `MotorListener`: CAN message listener (can.Listener) - Asynchronous message reception via can.Notifier - Filters messages by motor ID (arbitration_id & 0x00000FF) - Triggers async state updates in actuator ### Data Structures: - `ServoMotorState`: Motor state container - position (degrees), velocity (ERPM), current (amps) - temperature (celsius), error code, acceleration (rad/s) - `ServoControlMode`: Enum for control modes - POSITION (4), VELOCITY (3), CURRENT (1), IDLE (7) ## CAN Protocol Implementation ### Message IDs (Extended CAN): - SET_DUTY: 0x00 (not used) - SET_CURRENT: 0x01 (current control) - SET_CURRENT_BRAKE: 0x02 (not used) - SET_RPM: 0x03 (velocity control) - SET_POS: 0x04 (position control) - SET_ORIGIN_HERE: 0x05 (homing) - SET_POS_SPD: 0x06 (not used) - Control mode switch: 0x07 ### Message Format: - TX (4 bytes, big-endian): int32 command value - Position: degrees * 10 (0.1 resolution, configurable to *1000000) - Velocity: ERPM value directly - Current: milliamps (current * 1000) - RX (8 bytes, big-endian parsing): - Bytes[0-1]: Position (int16, *0.1 degrees) - Bytes[2-3]: Velocity (int16, *10 ERPM) - Bytes[4-5]: Current (int16, *0.01 amps) - Byte[6]: Temperature (uint8, celsius) - Byte[7]: Error code (uint8) ## Control Features ### Implemented Methods: - `set_motor_torque(Nm)`: Converts via Kt to current command - `set_output_torque(Nm)`: Applies gear ratio compensation - `set_motor_current(A)`: Direct current command - `set_motor_position(rad)`: Converts to degrees, sends position command - `set_motor_velocity(rad/s)`: Converts to ERPM via pole pairs - `set_output_velocity(rad/s)`: Applies gear ratio - `home()`: Sets origin via CAN_PACKET_SET_ORIGIN_HERE ### State Properties: - motor_position/output_position (radians) - motor_velocity/output_velocity (rad/s) - motor_current (amps) - motor_torque/output_torque (Nm) - case_temperature/winding_temperature (C) - error_info: Optional[tuple[error_code, error_message]] ## Safety Features - Thermal protection: 80C case, 110C winding limits (configurable) - Communication timeout: 100ms threshold warning - Auto-stop on critical errors (overcurrent, overtemp) - Acceleration calculation for sudden change detection - Control mode verification via test command - Detailed status reporting via `get_detailed_status()` ## Utility Functions - `degrees_to_radians()` / `radians_to_degrees()` - `erpm_to_rad_per_sec()` / `rad_per_sec_to_erpm()` - Accounts for motor pole pairs in conversion - `_pack_int32()`: Big-endian int32 packing for CAN TX ## Control Mode Management Each mode has entry/exit callbacks: - POSITION: Switch to mode 4, no exit action - VELOCITY: Switch to mode 3, zero velocity on exit - CURRENT: Switch to mode 1, zero current on exit - IDLE: Switch to mode 7, no exit action - All modes wait 100ms after switching for stabilization ## Example Code (__main__) Demonstrates current control mode with 15Nm torque command: 1. Initialize motor (AK80-9, ID:1, offline mode for testing) 2. Home motor and activate current control mode 3. Command 15Nm output torque 4. Run 10Hz monitoring loop for 5 seconds displaying: - Commanded vs actual torque (motor and output) - Motor and output angles (rad and degrees) - Motor and output velocities (rad/s) - Current, voltage, temperature - Error status 5. Safe shutdown with zero torque 6. Display final state summary Note: Position resolution currently set to 0.1 for simplicity. To enable high-precision mode (0.000001), modify line 323.
senthurayyappan
requested changes
Aug 19, 2025
opensourceleg/logging/logger.py
Outdated
| self._file_path = os.path.join(self._log_path, f"{file_name}.log") | ||
| self._csv_path = os.path.join(self._log_path, f"{file_name}.csv") | ||
| # Build paths using forward slash to satisfy cross-platform tests | ||
| self._file_path = f"{self._log_path}/{file_name}.log" |
There was a problem hiding this comment.
os.path.join is platform agnostic and more robust to linux/win path changes!
There was a problem hiding this comment.
self._file_path = posixpath.join(self._log_path, f"{file_name}.log")
self._csv_path = posixpath.join(self._log_path, f"{file_name}.csv")I used posixpath to solve this, since "os.path.join" can't pass make test
opensourceleg/logging/logger.py
Outdated
| @@ -618,7 +619,8 @@ def _generate_file_paths(self) -> None: | |||
|
|
|||
| base_name = self._user_file_name if self._user_file_name else f"{script_name}_{timestamp}" | |||
|
|
|||
| file_path = os.path.join(self._log_path, base_name) | |||
| # Build paths using forward slash to satisfy cross-platform tests | |||
There was a problem hiding this comment.
file_path = posixpath.join(self._log_path, base_name)
self._file_path = file_path + ".log"
self._csv_path = file_path + ".csv"Same
| @@ -357,7 +358,7 @@ def _next_original_phase(self) -> float: | |||
|
|
|||
| # Busy wait until the time we should be running at | |||
| while time.monotonic() < self.loop_deadline and not self.killer.kill_now: | |||
| if os.name == "posix" and signal.sigtimedwait(self.killer.signals, 0): | |||
| if os.name == "posix" and hasattr(signal, "sigtimedwait") and signal.sigtimedwait(self.killer.signals, 0): | |||
There was a problem hiding this comment.
can we please split this conditional statement into two? with the hasattr check in the upper level?
There was a problem hiding this comment.
if not hasattr(signal, "sigtimedwait"):
continue
if os.name == "posix" and signal.sigtimedwait(self.killer.signals, 0):
opensourceleg/actuators/tmotor.py
Outdated
|
|
||
| # TMotor servo mode constants | ||
| TMOTOR_SERVO_CONSTANTS = MOTOR_CONSTANTS( | ||
| MOTOR_COUNT_PER_REV=3600, # 360 degrees * 10 |
There was a problem hiding this comment.
Can we please validate this number for MOTOR_COUNT_PER_REV?
There was a problem hiding this comment.
It should be 2^16 = 65536 (16-bit encoder)
opensourceleg/actuators/tmotor.py
Outdated
| LOGGER.info("Initializing CAN Manager for TMotor Servo Mode") | ||
| try: | ||
| # Configure CAN interface | ||
| os.system("sudo /sbin/ip link set can0 down") # noqa: S605, S607 |
There was a problem hiding this comment.
Can we avoid running sudo commands here? These instructions can be moved to documentation
There was a problem hiding this comment.
All of them have been removed. And I created a doc to instruct users to do this on their end.
at docs/tutorials/actuators/tmotor_servo_setup.md
opensourceleg/actuators/tmotor.py
Outdated
| def set_motor_velocity(self, value: float) -> None: | ||
| """Set motor velocity (rad/s)""" | ||
| motor_params = cast(dict[str, Any], self._motor_params) | ||
| velocity_erpm = rad_per_sec_to_erpm(value, motor_params["NUM_POLE_PAIRS"]) |
There was a problem hiding this comment.
use this class' MOTOR_CONSTANTS attribute here
opensourceleg/actuators/tmotor.py
Outdated
| Wrapper for get_output_acceleration that accounts for gear ratio to get motor-side acceleration | ||
| def motor_voltage(self) -> float: | ||
| """Motor voltage (estimated)""" | ||
| return 24.0 # Cannot get directly in servo mode |
There was a problem hiding this comment.
raise not implementer error or some warning rather than return 24
opensourceleg/actuators/tmotor.py
Outdated
| self._canman.notifier.remove_listener(Listener) | ||
| return success | ||
| def verify_control_mode(self) -> bool: | ||
| """Verify current control mode matches expected mode""" |
There was a problem hiding this comment.
can we please remove this?
opensourceleg/actuators/tmotor.py
Outdated
| return False | ||
| return True | ||
|
|
||
| def get_detailed_status(self) -> dict[str, Any]: |
opensourceleg/actuators/tmotor.py
Outdated
Comment on lines
751
to
752
|
|
||
| with actuator as motor: |
|
Hey @DasuanerA, can you please address the comments soon so that we can merge this PR and start planning our tests? |
…tion - Fix cross-platform path handling using os.path.join() in logger.py - Split hasattr conditional check into nested structure in softrealtimeloop.py - Correct MOTOR_COUNT_PER_REV from 3600 to 65536 for 16-bit encoder - Remove sudo commands from code and add CAN setup documentation - Refactor SERVO_PARAMS into separate semantic constants (TMOTOR_ERROR_CODES, CAN_PACKET_ID, TMOTOR_MODELS) - Remove unused verify_control_mode() and get_detailed_status() methods - Add comprehensive TMotor servo setup guide in docs/tutorials/actuators/
- Fix SIM102 linting error in softrealtimeloop.py to pass make check by combining nested if statements with 'and' operator - Replace os.path.join with posixpath.join in logger.py to pass make test, as os.path.join produces Windows backslashes that fail test assertions expecting forward slashes
- Replace fixed time delays with status-based polling for motor startup and mode switches - Add safety clamping for current, velocity, and position commands using np.clip() - Remove acceleration calculation and field from ServoMotorState - Fix motor_voltage property to raise NotImplementedError instead of returning hardcoded value - Add num_pole_pairs property for cleaner parameter access - Simplify example code by removing redundant cast operations - Add warning logs when motor commands exceed safe limits - Fix example code to avoid calling unsupported motor_voltage property - Resolve linting issues including trailing whitespace and code formatting
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Implements complete TMotor servo mode control for AK series actuators (AK80-9, AK10-9) using native CAN communication protocol.
Architecture
Core Classes:
TMotorServoActuator: Main actuator class extending ActuatorBaseCANManagerServo: Singleton CAN communication managerMotorListener: CAN message listener (can.Listener)Data Structures:
ServoMotorState: Motor state containerServoControlMode: Enum for control modesCAN Protocol Implementation
Message IDs (Extended CAN):
Message Format:
TX (4 bytes, big-endian): int32 command value
RX (8 bytes, big-endian parsing):
Control Features
Implemented Methods:
set_motor_torque(Nm): Converts via Kt to current commandset_output_torque(Nm): Applies gear ratio compensationset_motor_current(A): Direct current commandset_motor_position(rad): Converts to degrees, sends position commandset_motor_velocity(rad/s): Converts to ERPM via pole pairsset_output_velocity(rad/s): Applies gear ratiohome(): Sets origin via CAN_PACKET_SET_ORIGIN_HEREState Properties:
Safety Features
get_detailed_status()Utility Functions
degrees_to_radians()/radians_to_degrees()erpm_to_rad_per_sec()/rad_per_sec_to_erpm()_pack_int32(): Big-endian int32 packing for CAN TXControl Mode Management
Each mode has entry/exit callbacks:
Example Code (main)
Demonstrates current control mode with 15Nm torque command:
Note: Position resolution currently set to 0.1 for simplicity. To enable high-precision mode (0.000001), modify line 323.