From 57bde99f39640885ad1ea70667db831bc388d188 Mon Sep 17 00:00:00 2001 From: WT-MM Date: Tue, 22 Oct 2024 09:54:42 -0700 Subject: [PATCH 1/2] add sim robstride --- actuator/rust/bindings/bindings.pyi | 4 + actuator/rust/bindings/src/lib.rs | 23 +++- actuator/rust/robstride/src/lib.rs | 21 +++- actuator/rust/robstride/src/port.rs | 9 +- actuator/sim/__init__.py | 0 actuator/sim/robstride.py | 186 ++++++++++++++++++++++++++++ examples/sim.py | 78 ++++++++++++ setup.py | 6 +- 8 files changed, 321 insertions(+), 6 deletions(-) create mode 100644 actuator/sim/__init__.py create mode 100644 actuator/sim/robstride.py create mode 100644 examples/sim.py diff --git a/actuator/rust/bindings/bindings.pyi b/actuator/rust/bindings/bindings.pyi index b6725c5..706504a 100644 --- a/actuator/rust/bindings/bindings.pyi +++ b/actuator/rust/bindings/bindings.pyi @@ -24,6 +24,10 @@ class PyRobstrideMotorFeedback: def __repr__(self) -> str: ... + @staticmethod + def create_feedback(can_id:int, position:float, velocity:float, torque:float, mode:str, faults:int) -> PyRobstrideMotorFeedback: + ... + class PyRobstrideMotors: def __new__(cls,port_name,motor_infos,verbose = ...): ... diff --git a/actuator/rust/bindings/src/lib.rs b/actuator/rust/bindings/src/lib.rs index 698dc66..eb4b7ad 100644 --- a/actuator/rust/bindings/src/lib.rs +++ b/actuator/rust/bindings/src/lib.rs @@ -2,7 +2,7 @@ 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_type_from_str as robstride_motor_type_from_str, motor_mode_from_str as robstride_motor_mode_from_str, MotorControlParams as RobstrideMotorControlParams, MotorFeedback as RobstrideMotorFeedback, MotorType as RobstrideMotorType, Motors as RobstrideMotors, MotorsSupervisor as RobstrideMotorsSupervisor, @@ -106,6 +106,27 @@ impl PyRobstrideMotorFeedback { self.can_id, self.position, self.velocity, self.torque, self.mode, self.faults )) } + + #[staticmethod] + fn create_feedback( + can_id: u8, + position: f32, + velocity: f32, + torque: f32, + mode: String, + faults: u16, + ) -> PyResult { + let feedback = RobstrideMotorFeedback { + can_id, + position, + velocity, + torque, + mode: robstride_motor_mode_from_str(mode.as_str())?, + faults, + }; + + Ok(feedback.into()) + } } impl From for PyRobstrideMotorFeedback { diff --git a/actuator/rust/robstride/src/lib.rs b/actuator/rust/robstride/src/lib.rs index 3309db3..58f9437 100644 --- a/actuator/rust/robstride/src/lib.rs +++ b/actuator/rust/robstride/src/lib.rs @@ -18,7 +18,13 @@ pub const CAN_ID_MOTOR_DEFAULT: u8 = 0x7F; pub const CAN_ID_BROADCAST: u8 = 0xFE; pub const CAN_ID_DEBUG_UI: u8 = 0xFD; -pub const BAUDRATE: nix::sys::termios::BaudRate = nix::sys::termios::BaudRate::B921600; +#[cfg(target_os = "linux")] +pub const BAUD_RATE: nix::sys::termios::BaudRate = nix::sys::termios::BaudRate::B921600; + +// WARNING: NOT A VALID BAUDRATE +// This is just a configuration to build on MacOS +#[cfg(target_os = "macos")] +pub const BAUD_RATE: nix::sys::termios::BaudRate = nix::sys::termios::BaudRate::B115200; pub struct MotorConfig { pub p_min: f32, @@ -386,6 +392,19 @@ pub enum MotorType { Type04, } +pub fn motor_mode_from_str(s: &str) -> Result { + match s { + "Reset" => Ok(MotorMode::Reset), + "Cali" => Ok(MotorMode::Cali), + "Motor" => Ok(MotorMode::Motor), + "Brake" => Ok(MotorMode::Brake), + _ => Err(std::io::Error::new( + std::io::ErrorKind::InvalidInput, + "Invalid motor mode", + )), + } +} + pub fn motor_type_from_str(s: &str) -> Result { match s { "01" => Ok(MotorType::Type01), diff --git a/actuator/rust/robstride/src/port.rs b/actuator/rust/robstride/src/port.rs index 0da94d7..9f2b928 100644 --- a/actuator/rust/robstride/src/port.rs +++ b/actuator/rust/robstride/src/port.rs @@ -3,7 +3,14 @@ use serialport::{SerialPort, TTYPort}; use std::os::unix::io::FromRawFd; use std::time::Duration; -const BAUD_RATE: termios::BaudRate = termios::BaudRate::B921600; +#[cfg(target_os = "linux")] +pub const BAUD_RATE: nix::sys::termios::BaudRate = nix::sys::termios::BaudRate::B921600; + +// WARNING: NOT A VALID BAUDRATE +// This is just a configuration to build on MacOS +#[cfg(target_os = "macos")] +pub const BAUD_RATE: nix::sys::termios::BaudRate = nix::sys::termios::BaudRate::B115200; + const TIMEOUT: Duration = Duration::from_millis(10); pub fn init_serial_port(device: &str) -> Result { diff --git a/actuator/sim/__init__.py b/actuator/sim/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/actuator/sim/robstride.py b/actuator/sim/robstride.py new file mode 100644 index 0000000..f76a92c --- /dev/null +++ b/actuator/sim/robstride.py @@ -0,0 +1,186 @@ +"""Simulated Robstride motors.""" +import time +from enum import Enum, auto + +from actuator import RobstrideMotorControlParams, RobstrideMotorFeedback + + +class MotorType(Enum): + Type01 = auto() + Type02 = auto() + Type03 = auto() + Type04 = auto() + + @classmethod + 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: + self.p_min = p_min + self.p_max = p_max + self.v_min = v_min + self.v_max = v_max + self.kp_min = kp_min + self.kp_max = kp_max + self.kd_min = kd_min + self.kd_max = kd_max + self.t_min = t_min + self.t_max = t_max + self.zero_on_init = zero_on_init + 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, + zero_on_init=True, + 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, + zero_on_init=False, + 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, + zero_on_init=False, + 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, + zero_on_init=False, + can_timeout_command=0x200b, + can_timeout_factor=12000.0 + ) +} + +class _MotorSim: + def __init__(self, motor_id: int) -> None: + self.motor_id = motor_id + self.position = 0.0 + self.velocity = 0.0 + self.torque = 0.0 + self.kp = 0.0 + self.kd = 0.0 + + def update_state(self, params: RobstrideMotorControlParams) -> None: + self.position = params.position + self.velocity = params.velocity + self.torque = params.torque + self.kp = params.kp + self.kd = params.kd + + def get_feedback(self) -> RobstrideMotorFeedback: + feedback = RobstrideMotorFeedback.create_feedback( + self.motor_id, + self.position, + self.velocity, + self.torque, + "Motor", + 0 + ) + return feedback + + +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() + } + self.verbose = verbose + + self.motors = {motor_id: _MotorSim(motor_id) for motor_id in motor_infos} # Use MotorSim for each motor + + def send_get_mode(self) -> dict[int, str]: + return {motor_id: "Motor" for motor_id in self.motors} + + def send_resets(self) -> None: + for motor_id in self.motors: + self.motors[motor_id] = _MotorSim(motor_id) + + 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]: + feedback: dict[int, RobstrideMotorFeedback] = {} + for motor_id, params in motor_controls.items(): + self.motors[motor_id].update_state(params) + feedback[motor_id] = self.motors[motor_id].get_feedback() + return feedback + + 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.running = True + self.motors_sim = RobstrideMotorsSim("virtual_port", motor_infos, verbose) + + 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]: + """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) + self.latest_feedback.update(feedback) + return self.latest_feedback.copy() + + def get_latest_feedback(self) -> dict[int, RobstrideMotorFeedback]: + """Get the latest feedback from the motors.""" + return self.latest_feedback.copy() + + def stop(self) -> None: + """Stop the supervisor.""" + self.running = False + self.motors_sim.send_resets() + + def is_running(self) -> bool: + """Check if the supervisor is running.""" + return self.running diff --git a/examples/sim.py b/examples/sim.py new file mode 100644 index 0000000..59c16e9 --- /dev/null +++ b/examples/sim.py @@ -0,0 +1,78 @@ +"""Example use of simulated actuators.""" +import math +import time + +from actuator import RobstrideMotorControlParams +from actuator.sim.robstride import RobstrideMotorsSim, RobstrideMotorsSupervisorSim + + +def test_robstride_motors_sim() -> None: + # Define motor information + motor_infos = { + 1: "01", + 2: "02" + } + + # Initialize the simulator + motors_sim = RobstrideMotorsSim("virtual_port", motor_infos, verbose=True) + + # Send start command + motors_sim.send_starts() + + # 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) + } + + # Send motor controls + feedback = motors_sim.send_motor_controls(motor_controls, serial=False) + print("Feedback:", feedback) + + # Send reset command + motors_sim.send_resets() + +def test_robstride_motors_supervisor_sim() -> None: + # Define motor information + motor_infos = { + 1: "01", + 2: "02" + } + + # Initialize the supervisor simulator + supervisor_sim = RobstrideMotorsSupervisorSim(motor_infos, verbose=True) + + # Set a delay for the supervisor + supervisor_sim.set_delay(0.01) + + # 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) + } + + start_time = time.time() + + try: + 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) + # Send motor controls + feedback = supervisor_sim.send_motor_controls(motor_controls) + print("Feedback:", feedback) + + time.sleep(0.1) + + except KeyboardInterrupt: + print("\nSimulation interrupted by user") + + # Stop the supervisor + supervisor_sim.stop() + +if __name__ == "__main__": + print("Testing Robstride Motors Sim") + test_robstride_motors_sim() + print("\nTesting Robstride Motors Supervisor Sim") + test_robstride_motors_supervisor_sim() diff --git a/setup.py b/setup.py index 7cff5ea..25a17e6 100644 --- a/setup.py +++ b/setup.py @@ -5,7 +5,7 @@ import glob import re -from setuptools import setup +from setuptools import find_packages, setup from setuptools_rust import Binding, RustExtension with open("README.md", "r", encoding="utf-8") as f: @@ -35,7 +35,7 @@ name="actuator", version=version, description="Python interface for controlling various robotic actuators", - author="Benjamin Bolte", + author="K-Scale Labs", url="https://github.com/kscalelabs/actuator", rust_extensions=[ RustExtension( @@ -53,5 +53,5 @@ extras_require={"dev": requirements_dev}, include_package_data=True, package_data={"actuator": package_data}, - packages=["actuator"], + packages=find_packages(include=["actuator", "actuator.*"]), ) From 447e8fe562dddc74bbfb12eaa4b03f7eba4b31ee Mon Sep 17 00:00:00 2001 From: WT-MM Date: Tue, 22 Oct 2024 09:59:48 -0700 Subject: [PATCH 2/2] 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()