From 447e8fe562dddc74bbfb12eaa4b03f7eba4b31ee Mon Sep 17 00:00:00 2001 From: WT-MM Date: Tue, 22 Oct 2024 09:59:48 -0700 Subject: [PATCH] lint --- actuator/rust/bindings/src/lib.rs | 20 ++--- actuator/sim/robstride.py | 137 ++++++++++++++++++------------ examples/sim.py | 25 +++--- 3 files changed, 105 insertions(+), 77 deletions(-) diff --git a/actuator/rust/bindings/src/lib.rs b/actuator/rust/bindings/src/lib.rs index eb4b7ad..596da0f 100644 --- a/actuator/rust/bindings/src/lib.rs +++ b/actuator/rust/bindings/src/lib.rs @@ -2,7 +2,8 @@ use pyo3::prelude::*; use pyo3_stub_gen::define_stub_info_gatherer; use pyo3_stub_gen::derive::{gen_stub_pyclass, gen_stub_pymethods}; use robstride::{ - motor_type_from_str as robstride_motor_type_from_str, motor_mode_from_str as robstride_motor_mode_from_str, + motor_mode_from_str as robstride_motor_mode_from_str, + motor_type_from_str as robstride_motor_type_from_str, MotorControlParams as RobstrideMotorControlParams, MotorFeedback as RobstrideMotorFeedback, MotorType as RobstrideMotorType, Motors as RobstrideMotors, MotorsSupervisor as RobstrideMotorsSupervisor, @@ -230,15 +231,14 @@ impl PyRobstrideMotorsSupervisor { }) .collect::>>()?; - let controller = - RobstrideMotorsSupervisor::new( - &port_name, - &motor_infos, - verbose, - target_update_rate, - zero_on_init, - ) - .map_err(|e| PyErr::new::(e.to_string()))?; + let controller = RobstrideMotorsSupervisor::new( + &port_name, + &motor_infos, + verbose, + target_update_rate, + zero_on_init, + ) + .map_err(|e| PyErr::new::(e.to_string()))?; Ok(PyRobstrideMotorsSupervisor { inner: controller }) } diff --git a/actuator/sim/robstride.py b/actuator/sim/robstride.py index f76a92c..54fa043 100644 --- a/actuator/sim/robstride.py +++ b/actuator/sim/robstride.py @@ -1,4 +1,5 @@ """Simulated Robstride motors.""" + import time from enum import Enum, auto @@ -12,27 +13,30 @@ class MotorType(Enum): Type04 = auto() @classmethod - def from_str(cls, s: str) -> 'MotorType': + def from_str(cls, s: str) -> "MotorType": try: return cls[f"Type{s}"] except KeyError: raise ValueError(f"Invalid motor type: {s}") + class MotorConfig: - def __init__(self, - p_min: float, - p_max: float, - v_min: float, - v_max: float, - kp_min: float, - kp_max: float, - kd_min: float, - kd_max: float, - t_min: float, - t_max: float, - zero_on_init: bool, - can_timeout_command: int, - can_timeout_factor: float) -> None: + def __init__( + self, + p_min: float, + p_max: float, + v_min: float, + v_max: float, + kp_min: float, + kp_max: float, + kd_min: float, + kd_max: float, + t_min: float, + t_max: float, + zero_on_init: bool, + can_timeout_command: int, + can_timeout_factor: float, + ) -> None: self.p_min = p_min self.p_max = p_max self.v_min = v_min @@ -47,49 +51,71 @@ def __init__(self, self.can_timeout_command = can_timeout_command self.can_timeout_factor = can_timeout_factor + ROBSTRIDE_CONFIGS = { MotorType.Type01: MotorConfig( - p_min=-12.5, p_max=12.5, - v_min=-44.0, v_max=44.0, - kp_min=0.0, kp_max=500.0, - kd_min=0.0, kd_max=5.0, - t_min=-12.0, t_max=12.0, + p_min=-12.5, + p_max=12.5, + v_min=-44.0, + v_max=44.0, + kp_min=0.0, + kp_max=500.0, + kd_min=0.0, + kd_max=5.0, + t_min=-12.0, + t_max=12.0, zero_on_init=True, - can_timeout_command=0x200c, - can_timeout_factor=12000.0 + can_timeout_command=0x200C, + can_timeout_factor=12000.0, ), MotorType.Type02: MotorConfig( - p_min=-12.5, p_max=12.5, - v_min=-44.0, v_max=44.0, - kp_min=0.0, kp_max=500.0, - kd_min=0.0, kd_max=5.0, - t_min=-12.0, t_max=12.0, + p_min=-12.5, + p_max=12.5, + v_min=-44.0, + v_max=44.0, + kp_min=0.0, + kp_max=500.0, + kd_min=0.0, + kd_max=5.0, + t_min=-12.0, + t_max=12.0, zero_on_init=False, - can_timeout_command=0x200b, - can_timeout_factor=12000.0 + can_timeout_command=0x200B, + can_timeout_factor=12000.0, ), MotorType.Type03: MotorConfig( - p_min=-12.5, p_max=12.5, - v_min=-44.0, v_max=44.0, - kp_min=0.0, kp_max=500.0, - kd_min=0.0, kd_max=5.0, - t_min=-12.0, t_max=12.0, + p_min=-12.5, + p_max=12.5, + v_min=-44.0, + v_max=44.0, + kp_min=0.0, + kp_max=500.0, + kd_min=0.0, + kd_max=5.0, + t_min=-12.0, + t_max=12.0, zero_on_init=False, - can_timeout_command=0x200b, - can_timeout_factor=12000.0 + can_timeout_command=0x200B, + can_timeout_factor=12000.0, ), MotorType.Type04: MotorConfig( - p_min=-12.5, p_max=12.5, - v_min=-44.0, v_max=44.0, - kp_min=0.0, kp_max=500.0, - kd_min=0.0, kd_max=5.0, - t_min=-12.0, t_max=12.0, + p_min=-12.5, + p_max=12.5, + v_min=-44.0, + v_max=44.0, + kp_min=0.0, + kp_max=500.0, + kd_min=0.0, + kd_max=5.0, + t_min=-12.0, + t_max=12.0, zero_on_init=False, - can_timeout_command=0x200b, - can_timeout_factor=12000.0 - ) + can_timeout_command=0x200B, + can_timeout_factor=12000.0, + ), } + class _MotorSim: def __init__(self, motor_id: int) -> None: self.motor_id = motor_id @@ -108,12 +134,7 @@ def update_state(self, params: RobstrideMotorControlParams) -> None: def get_feedback(self) -> RobstrideMotorFeedback: feedback = RobstrideMotorFeedback.create_feedback( - self.motor_id, - self.position, - self.velocity, - self.torque, - "Motor", - 0 + self.motor_id, self.position, self.velocity, self.torque, "Motor", 0 ) return feedback @@ -122,8 +143,7 @@ class RobstrideMotorsSim: def __init__(self, port_name: str, motor_infos: dict[int, str], verbose: bool = False) -> None: self.port_name = port_name self.motor_configs = { - id: ROBSTRIDE_CONFIGS[MotorType.from_str(motor_type)] - for id, motor_type in motor_infos.items() + id: ROBSTRIDE_CONFIGS[MotorType.from_str(motor_type)] for id, motor_type in motor_infos.items() } self.verbose = verbose @@ -140,7 +160,9 @@ def send_starts(self) -> None: if self.verbose: print("Motors started") - def send_motor_controls(self, motor_controls: dict[int, RobstrideMotorControlParams], serial: bool) -> dict[int, RobstrideMotorFeedback]: + def send_motor_controls( + self, motor_controls: dict[int, RobstrideMotorControlParams], serial: bool + ) -> dict[int, RobstrideMotorFeedback]: feedback: dict[int, RobstrideMotorFeedback] = {} for motor_id, params in motor_controls.items(): self.motors[motor_id].update_state(params) @@ -150,13 +172,16 @@ def send_motor_controls(self, motor_controls: dict[int, RobstrideMotorControlPar def __repr__(self) -> str: return f"RobstrideMotorsSim(port_name={self.port_name}, motor_configs={self.motor_configs})" + class RobstrideMotorsSupervisorSim: def __init__(self, motor_infos: dict[int, str], verbose: bool = False) -> None: self.verbose = verbose self.delay = 0.01 # Default delay in seconds self.target_params = {motor_id: RobstrideMotorControlParams(0, 0, 0, 0, 0) for motor_id in motor_infos} - self.latest_feedback = {motor_id: RobstrideMotorFeedback.create_feedback(0, 0, 0, 0, "Reset", 0) for motor_id in motor_infos} + self.latest_feedback = { + motor_id: RobstrideMotorFeedback.create_feedback(0, 0, 0, 0, "Reset", 0) for motor_id in motor_infos + } self.running = True self.motors_sim = RobstrideMotorsSim("virtual_port", motor_infos, verbose) @@ -165,7 +190,9 @@ def set_delay(self, delay: float) -> None: """Set the delay for blocking communication methods.""" self.delay = delay - def send_motor_controls(self, motor_controls: dict[int, RobstrideMotorControlParams]) -> dict[int, RobstrideMotorFeedback]: + def send_motor_controls( + self, motor_controls: dict[int, RobstrideMotorControlParams] + ) -> dict[int, RobstrideMotorFeedback]: """Simulate sending motor controls with a blocking delay.""" time.sleep(self.delay) # Simulate blocking communication feedback = self.motors_sim.send_motor_controls(motor_controls, serial=False) diff --git a/examples/sim.py b/examples/sim.py index 59c16e9..1822731 100644 --- a/examples/sim.py +++ b/examples/sim.py @@ -1,4 +1,5 @@ """Example use of simulated actuators.""" + import math import time @@ -8,10 +9,7 @@ def test_robstride_motors_sim() -> None: # Define motor information - motor_infos = { - 1: "01", - 2: "02" - } + motor_infos = {1: "01", 2: "02"} # Initialize the simulator motors_sim = RobstrideMotorsSim("virtual_port", motor_infos, verbose=True) @@ -22,7 +20,7 @@ def test_robstride_motors_sim() -> None: # Define control parameters for the motors motor_controls = { 1: RobstrideMotorControlParams(position=1.0, velocity=0.0, kp=10.0, kd=1.0, torque=0.0), - 2: RobstrideMotorControlParams(position=-1.0, velocity=0.0, kp=10.0, kd=1.0, torque=0.0) + 2: RobstrideMotorControlParams(position=-1.0, velocity=0.0, kp=10.0, kd=1.0, torque=0.0), } # Send motor controls @@ -32,12 +30,10 @@ def test_robstride_motors_sim() -> None: # Send reset command motors_sim.send_resets() + def test_robstride_motors_supervisor_sim() -> None: # Define motor information - motor_infos = { - 1: "01", - 2: "02" - } + motor_infos = {1: "01", 2: "02"} # Initialize the supervisor simulator supervisor_sim = RobstrideMotorsSupervisorSim(motor_infos, verbose=True) @@ -48,7 +44,7 @@ def test_robstride_motors_supervisor_sim() -> None: # Define control parameters for the motors motor_controls = { 1: RobstrideMotorControlParams(position=1.0, velocity=0.0, kp=10.0, kd=1.0, torque=0.0), - 2: RobstrideMotorControlParams(position=-1.0, velocity=0.0, kp=10.0, kd=1.0, torque=0.0) + 2: RobstrideMotorControlParams(position=-1.0, velocity=0.0, kp=10.0, kd=1.0, torque=0.0), } start_time = time.time() @@ -57,8 +53,12 @@ def test_robstride_motors_supervisor_sim() -> None: while supervisor_sim.is_running(): # Sinusoidal set point goal_pos = math.sin(2 * math.pi * (time.time() - start_time) / 5.0) - motor_controls[1] = RobstrideMotorControlParams(position=goal_pos, velocity=0.0, kp=10.0, kd=1.0, torque=0.0) - motor_controls[2] = RobstrideMotorControlParams(position=-goal_pos, velocity=0.0, kp=10.0, kd=1.0, torque=0.0) + motor_controls[1] = RobstrideMotorControlParams( + position=goal_pos, velocity=0.0, kp=10.0, kd=1.0, torque=0.0 + ) + motor_controls[2] = RobstrideMotorControlParams( + position=-goal_pos, velocity=0.0, kp=10.0, kd=1.0, torque=0.0 + ) # Send motor controls feedback = supervisor_sim.send_motor_controls(motor_controls) print("Feedback:", feedback) @@ -71,6 +71,7 @@ def test_robstride_motors_supervisor_sim() -> None: # Stop the supervisor supervisor_sim.stop() + if __name__ == "__main__": print("Testing Robstride Motors Sim") test_robstride_motors_sim()