From 18b35797c170e3c02ae4ce68c805f6c1fee12ae5 Mon Sep 17 00:00:00 2001 From: Ben Bolte Date: Thu, 10 Oct 2024 13:50:58 -0400 Subject: [PATCH] supervisor running as background thread in rust (#15) * supervisor running as background thread in rust * Bump version to 0.0.10 --- actuator/__init__.py | 8 +- actuator/rust/bindings/bindings.pyi | 24 ++ actuator/rust/bindings/src/lib.rs | 90 +++++-- .../robstride/src/{main.rs => bin/motors.rs} | 49 +++- actuator/rust/robstride/src/bin/supervisor.rs | 99 ++++++++ actuator/rust/robstride/src/lib.rs | 240 +++++++++++++++++- examples/sinusoidal_movement.py | 3 +- examples/supervisor.py | 37 +++ 8 files changed, 517 insertions(+), 33 deletions(-) rename actuator/rust/robstride/src/{main.rs => bin/motors.rs} (65%) create mode 100644 actuator/rust/robstride/src/bin/supervisor.rs create mode 100644 examples/supervisor.py diff --git a/actuator/__init__.py b/actuator/__init__.py index 490763b..d26db5b 100644 --- a/actuator/__init__.py +++ b/actuator/__init__.py @@ -1,3 +1,7 @@ -__version__ = "0.0.9" +__version__ = "0.0.10" -from .rust.bindings import PyRobstrideMotorFeedback as RobstrideMotorFeedback, PyRobstrideMotors as RobstrideMotors +from .rust.bindings import ( + PyRobstrideMotorFeedback as RobstrideMotorFeedback, + PyRobstrideMotors as RobstrideMotors, + PyRobstrideMotorsSupervisor as RobstrideMotorsSupervisor, +) diff --git a/actuator/rust/bindings/bindings.pyi b/actuator/rust/bindings/bindings.pyi index 90d1e4f..8715682 100644 --- a/actuator/rust/bindings/bindings.pyi +++ b/actuator/rust/bindings/bindings.pyi @@ -41,3 +41,27 @@ class PyRobstrideMotors: ... +class PyRobstrideMotorsSupervisor: + def __new__(cls,port_name:str, motor_infos:typing.Mapping[int, str]): ... + def set_target_position(self, motor_id:int, position:float) -> None: + ... + + def set_kp_kd(self, motor_id:int, kp:float, kd:float) -> None: + ... + + def set_sleep_duration(self, sleep_duration:float) -> None: + ... + + def add_motor_to_zero(self, motor_id:int) -> None: + ... + + def get_latest_feedback(self) -> dict[int, PyRobstrideMotorFeedback]: + ... + + def stop(self) -> None: + ... + + def __repr__(self) -> str: + ... + + diff --git a/actuator/rust/bindings/src/lib.rs b/actuator/rust/bindings/src/lib.rs index fc76c89..5755446 100644 --- a/actuator/rust/bindings/src/lib.rs +++ b/actuator/rust/bindings/src/lib.rs @@ -2,11 +2,12 @@ use pyo3::prelude::*; use pyo3_stub_gen::define_stub_info_gatherer; use pyo3_stub_gen::derive::{gen_stub_pyclass, gen_stub_pymethods}; use robstride::{ - MotorFeedback as RobstrideMotorFeedback, MotorType as RobstrideMotorType, - Motors as RobstrideMotors, + motor_type_from_str as robstride_motor_type_from_str, MotorFeedback as RobstrideMotorFeedback, + MotorType as RobstrideMotorType, Motors as RobstrideMotors, + MotorsSupervisor as RobstrideMotorsSupervisor, }; use std::collections::HashMap; - +use std::time::Duration; #[gen_stub_pyclass] #[pyclass] struct PyRobstrideMotors { @@ -21,22 +22,12 @@ impl PyRobstrideMotors { let motor_infos = motor_infos .into_iter() .map(|(id, type_str)| { - let motor_type = match type_str.as_str() { - "01" => RobstrideMotorType::Type01, - "03" => RobstrideMotorType::Type03, - "04" => RobstrideMotorType::Type04, - _ => { - return Err(PyErr::new::(format!( - "Invalid motor type: {}", - type_str - ))) - } - }; + let motor_type = robstride_motor_type_from_str(type_str.as_str())?; Ok((id, motor_type)) }) .collect::>>()?; - let motors = RobstrideMotors::new(&port_name, motor_infos) + let motors = RobstrideMotors::new(&port_name, &motor_infos) .map_err(|e| PyErr::new::(e.to_string()))?; Ok(PyRobstrideMotors { inner: motors }) @@ -156,10 +147,79 @@ impl From for PyRobstrideMotorFeedback { } } +#[gen_stub_pyclass] +#[pyclass] +struct PyRobstrideMotorsSupervisor { + inner: RobstrideMotorsSupervisor, +} + +#[gen_stub_pymethods] +#[pymethods] +impl PyRobstrideMotorsSupervisor { + #[new] + fn new(port_name: String, motor_infos: HashMap) -> PyResult { + let motor_infos = motor_infos + .into_iter() + .map(|(id, type_str)| { + let motor_type = robstride_motor_type_from_str(type_str.as_str())?; + Ok((id, motor_type)) + }) + .collect::>>()?; + + let controller = RobstrideMotorsSupervisor::new(&port_name, &motor_infos) + .map_err(|e| PyErr::new::(e.to_string()))?; + + Ok(PyRobstrideMotorsSupervisor { inner: controller }) + } + + fn set_target_position(&self, motor_id: u8, position: f32) -> PyResult<()> { + self.inner.set_target_position(motor_id, position); + Ok(()) + } + + fn set_kp_kd(&self, motor_id: u8, kp: f32, kd: f32) -> PyResult<()> { + self.inner.set_kp_kd(motor_id, kp, kd); + Ok(()) + } + + fn set_sleep_duration(&self, sleep_duration: f32) -> PyResult<()> { + self.inner + .set_sleep_duration(Duration::from_millis(sleep_duration as u64)); + Ok(()) + } + + fn add_motor_to_zero(&self, motor_id: u8) -> PyResult<()> { + self.inner.add_motor_to_zero(motor_id); + Ok(()) + } + + fn get_latest_feedback(&self) -> HashMap { + self.inner + .get_latest_feedback() + .into_iter() + .map(|(k, v)| (k, v.into())) + .collect() + } + + fn stop(&self) -> PyResult<()> { + self.inner.stop(); + Ok(()) + } + + fn __repr__(&self) -> PyResult { + let motor_count = self.inner.get_latest_feedback().len(); + Ok(format!( + "PyRobstrideMotorsSupervisor(motor_count={})", + motor_count + )) + } +} + #[pymodule] fn bindings(m: &Bound) -> PyResult<()> { m.add_class::()?; m.add_class::()?; + m.add_class::()?; Ok(()) } diff --git a/actuator/rust/robstride/src/main.rs b/actuator/rust/robstride/src/bin/motors.rs similarity index 65% rename from actuator/rust/robstride/src/main.rs rename to actuator/rust/robstride/src/bin/motors.rs index 08d339f..68a0edf 100644 --- a/actuator/rust/robstride/src/main.rs +++ b/actuator/rust/robstride/src/bin/motors.rs @@ -1,15 +1,14 @@ -use robstride::{MotorType, Motors}; +use robstride::{motor_type_from_str, Motors}; use std::collections::HashMap; use std::error::Error; use std::f32::consts::PI; use std::io::{self, Write}; use std::time::Instant; -const TEST_ID: u8 = 2; const RUN_TIME: f32 = 3.0; const MAX_TORQUE: f32 = 1.0; -fn run_motion_test(motors: &mut Motors) -> Result<(), Box> { +fn run_motion_test(motors: &mut Motors, test_id: u8) -> Result<(), Box> { motors.send_reset()?; motors.send_start()?; @@ -29,18 +28,18 @@ fn run_motion_test(motors: &mut Motors) -> Result<(), Box> { let elapsed_time = start_time.elapsed().as_secs_f32(); let desired_position = amplitude * (elapsed_time * PI * 2.0 / period + PI / 2.0).cos(); - let feedback = motors.get_latest_feedback_for(TEST_ID)?.clone(); + let feedback = motors.get_latest_feedback_for(test_id)?.clone(); let current_position = feedback.position; let current_velocity = feedback.velocity; let torque = (kp_04 * (desired_position - current_position) - kd_04 * current_velocity) .clamp(-MAX_TORQUE, MAX_TORQUE); - motors.send_torque_controls(&HashMap::from([(TEST_ID, torque)]))?; + motors.send_torque_controls(&HashMap::from([(test_id, torque)]))?; command_count += 1; println!( "Motor {} Commands: {}, Frequency: {:.2} Hz, Desired position: {:.2} Feedback: {:?}", - TEST_ID, + test_id, command_count, command_count as f32 / elapsed_time, desired_position, @@ -48,7 +47,7 @@ fn run_motion_test(motors: &mut Motors) -> Result<(), Box> { ); } - motors.send_torque_controls(&HashMap::from([(TEST_ID, 0.0)]))?; + motors.send_torque_controls(&HashMap::from([(test_id, 0.0)]))?; motors.send_reset()?; let elapsed_time = start_time.elapsed().as_secs_f32(); @@ -66,11 +65,37 @@ fn print_current_mode(motors: &mut Motors) { } fn main() -> Result<(), Box> { + print!("Enter the TEST_ID (u8): "); + io::stdout().flush()?; + let mut input = String::new(); + io::stdin().read_line(&mut input)?; + let test_id: u8 = input.trim().parse()?; + + print!("Enter the port name (default: /dev/ttyUSB0): "); + io::stdout().flush()?; + let mut port_input = String::new(); + io::stdin().read_line(&mut port_input)?; + let port_name = port_input.trim().to_string(); + let port_name = if port_name.is_empty() { + String::from("/dev/ttyUSB0") + } else { + port_name + }; + + print!("Enter the motor type (default: 01): "); + io::stdout().flush()?; + let mut motor_type_input = String::new(); + io::stdin().read_line(&mut motor_type_input)?; + let motor_type_input = motor_type_input.trim().to_string(); + let motor_type_input = if motor_type_input.is_empty() { + String::from("01") + } else { + motor_type_input + }; + let motor_type = motor_type_from_str(motor_type_input.as_str())?; + // Create motor instances - let mut motors = Motors::new( - "/dev/ttyUSB0", - HashMap::from([(TEST_ID, MotorType::Type01)]), - )?; + let mut motors = Motors::new(&port_name, HashMap::from([(test_id, motor_type)]))?; let mut last_command: i32 = -1; @@ -98,7 +123,7 @@ fn main() -> Result<(), Box> { last_command = 1; } 2 => { - run_motion_test(&mut motors)?; + run_motion_test(&mut motors, test_id)?; last_command = 2; } 3 => break, diff --git a/actuator/rust/robstride/src/bin/supervisor.rs b/actuator/rust/robstride/src/bin/supervisor.rs new file mode 100644 index 0000000..83a7252 --- /dev/null +++ b/actuator/rust/robstride/src/bin/supervisor.rs @@ -0,0 +1,99 @@ +use robstride::{motor_type_from_str, MotorType, MotorsSupervisor}; +use std::collections::HashMap; +use std::io::{self, Write}; + +fn main() -> Result<(), Box> { + print!("Enter the TEST_ID (u8): "); + io::stdout().flush()?; + let mut input = String::new(); + io::stdin().read_line(&mut input)?; + let test_id: u8 = input.trim().parse()?; + + print!("Enter the port name (default: /dev/ttyUSB0): "); + io::stdout().flush()?; + let mut port_input = String::new(); + io::stdin().read_line(&mut port_input)?; + let port_name = port_input.trim().to_string(); + let port_name = if port_name.is_empty() { + String::from("/dev/ttyUSB0") + } else { + port_name + }; + + print!("Enter the motor type (default: 01): "); + io::stdout().flush()?; + let mut motor_type_input = String::new(); + io::stdin().read_line(&mut motor_type_input)?; + let motor_type_input = motor_type_input.trim().to_string(); + let motor_type_input = if motor_type_input.is_empty() { + String::from("01") + } else { + motor_type_input + }; + let motor_type = motor_type_from_str(motor_type_input.as_str())?; + let motor_infos: HashMap = HashMap::from([(test_id, motor_type)]); + let controller = MotorsSupervisor::new(&port_name, &motor_infos)?; + + println!("Motor Controller Test CLI"); + println!("Available commands:"); + println!(" set_position / s "); + println!(" set_kp_kd / k "); + println!(" zero / z"); + println!(" get_feedback / g"); + println!(" quit / q"); + + loop { + print!("> "); + io::stdout().flush()?; + + let mut input = String::new(); + io::stdin().read_line(&mut input)?; + let parts: Vec<&str> = input.trim().split_whitespace().collect(); + + if parts.is_empty() { + continue; + } + + match parts[0] { + "set_position" | "s" => { + if parts.len() != 2 { + println!("Usage: set_position "); + continue; + } + let position: f32 = parts[1].parse()?; + controller.set_target_position(test_id, position); + println!("Set target position to {}", position); + } + "set_kp_kd" | "k" => { + if parts.len() != 3 { + println!("Usage: set_kp_kd "); + continue; + } + let kp: f32 = parts[1].parse()?; + let kd: f32 = parts[2].parse()?; + controller.set_kp_kd(test_id, kp, kd); + println!("Set KP/KD for motor {} to {}/{}", test_id, kp, kd); + } + "zero" | "z" => { + controller.add_motor_to_zero(test_id); + println!("Added motor {} to zero list", test_id); + } + "get_feedback" | "g" => { + let feedback = controller.get_latest_feedback(); + for (id, fb) in feedback { + println!("Motor {}: {:?}", id, fb); + } + } + "quit" | "q" => { + controller.stop(); + println!("Exiting..."); + break; + } + _ => { + println!("Unknown command. Available commands: set_position, set_kp_kd, get_feedback, quit"); + } + } + } + + Ok(()) +} diff --git a/actuator/rust/robstride/src/lib.rs b/actuator/rust/robstride/src/lib.rs index f63a030..42e7fc0 100644 --- a/actuator/rust/robstride/src/lib.rs +++ b/actuator/rust/robstride/src/lib.rs @@ -1,6 +1,8 @@ use serialport::SerialPort; -use std::collections::HashMap; +use std::collections::{HashMap, HashSet}; use std::io::{Read, Write}; +use std::sync::{Arc, Mutex}; +use std::thread; use std::time::Duration; #[macro_use] @@ -13,6 +15,8 @@ pub const CAN_ID_DEBUG_UI: u8 = 0xFD; pub const BAUDRATE: u32 = 921600; +const TWO_PI: f32 = 2.0 * std::f32::consts::PI; + pub struct MotorConfig { pub p_min: f32, pub p_max: f32, @@ -44,6 +48,22 @@ lazy_static! { t_max: 12.0, }, ); + // This is probably not correct, the Type02 is not released yet. + m.insert( + 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, + }, + ); m.insert( MotorType::Type03, MotorConfig { @@ -274,10 +294,24 @@ fn rx_unpack_feedback(port: &mut Box) -> Result Result { + match s { + "01" => Ok(MotorType::Type01), + "02" => Ok(MotorType::Type02), + "03" => Ok(MotorType::Type03), + "04" => Ok(MotorType::Type04), + _ => Err(std::io::Error::new( + std::io::ErrorKind::InvalidInput, + "Invalid motor type", + )), + } +} + pub struct Motors { port: Box, motor_configs: HashMap, @@ -290,10 +324,11 @@ pub struct Motors { impl Motors { pub fn new( port_name: &str, - motor_infos: HashMap, + motor_infos: &HashMap, ) -> Result> { let port = init_serial_port(port_name)?; let motor_configs: HashMap = motor_infos + .clone() .into_iter() .filter_map(|(id, motor_type)| { ROBSTRIDE_CONFIGS @@ -413,7 +448,27 @@ impl Motors { format!("Invalid motor ID: {}", id), )); } + } + + // Reset. + for &id in &ids_to_zero { + let pack = CanPack { + ex_id: ExId { + id, + data: CAN_ID_DEBUG_UI as u16, + mode: CanComMode::MotorReset, + res: 0, + }, + len: 8, + data: vec![0; 8], + }; + self.send_command(&pack)?; + } + std::thread::sleep(self.sleep_time); + + // Zero. + for &id in &ids_to_zero { let pack = CanPack { ex_id: ExId { id, @@ -427,8 +482,23 @@ impl Motors { self.send_command(&pack)?; } + std::thread::sleep(self.sleep_time); - // After setting the zero for specified motors, sleep for a short time. + // Start. + for &id in &ids_to_zero { + let pack = CanPack { + ex_id: ExId { + id, + data: CAN_ID_DEBUG_UI as u16, + mode: CanComMode::MotorIn, + res: 0, + }, + len: 8, + data: vec![0; 8], + }; + + self.send_command(&pack)?; + } std::thread::sleep(self.sleep_time); self.read_all_pending_responses() @@ -598,3 +668,167 @@ impl Motors { .ok_or_else(|| std::io::Error::new(std::io::ErrorKind::NotFound, "No feedback found")) } } + +fn get_default_kp_kd_values(motor_infos: &HashMap) -> HashMap { + let mut kp_kd_values = HashMap::new(); + + for (&motor_id, _) in motor_infos { + kp_kd_values.insert(motor_id, (10.0, 1.0)); + } + + kp_kd_values +} + +pub struct MotorsSupervisor { + motors: Arc>, + target_positions: Arc>>, + kp_kd_values: Arc>>, + running: Arc>, + latest_feedback: Arc>>, + motors_to_zero: Arc>>, + sleep_duration: Arc>, +} + +impl MotorsSupervisor { + pub fn new( + port_name: &str, + motor_infos: &HashMap, + ) -> Result> { + // Initialize Motors + let motors = Motors::new(port_name, motor_infos)?; + let kp_kd_values = get_default_kp_kd_values(&motor_infos); + + let motors = Arc::new(Mutex::new(motors)); + let target_positions = Arc::new(Mutex::new(HashMap::new())); + let kp_kd_values = Arc::new(Mutex::new(kp_kd_values)); + let running = Arc::new(Mutex::new(true)); + + let controller = MotorsSupervisor { + motors, + target_positions, + kp_kd_values, + running, + latest_feedback: Arc::new(Mutex::new(HashMap::new())), + motors_to_zero: Arc::new(Mutex::new(HashSet::new())), + sleep_duration: Arc::new(Mutex::new(Duration::from_micros(100))), + }; + + controller.start_control_thread(); + + Ok(controller) + } + + fn start_control_thread(&self) { + let motors = Arc::clone(&self.motors); + let target_positions = Arc::clone(&self.target_positions); + let kp_kd_values = Arc::clone(&self.kp_kd_values); + let running = Arc::clone(&self.running); + let latest_feedback = Arc::clone(&self.latest_feedback); + let motors_to_zero = Arc::clone(&self.motors_to_zero); + let sleep_duration = Arc::clone(&self.sleep_duration); + + thread::spawn(move || { + let mut motors = motors.lock().unwrap(); + let _ = motors.send_reset(); + let _ = motors.send_start(); + + while *running.lock().unwrap() { + { + let latest_feedback_from_motors = motors.get_latest_feedback(); + let mut latest_feedback = latest_feedback.lock().unwrap(); + *latest_feedback = latest_feedback_from_motors.clone(); + } + + { + let mut motor_ids_to_zero = motors_to_zero.lock().unwrap(); + let motor_ids = motor_ids_to_zero.iter().cloned().collect::>(); + if !motor_ids.is_empty() { + let _ = motors.send_set_zero(Some(&motor_ids)); + motor_ids_to_zero.clear(); + } + let torque_commands = HashMap::from_iter(motor_ids.iter().map(|id| (*id, 0.0))); + let _ = motors.send_torque_controls(&torque_commands); + } + + { + let target_positions = target_positions.lock().unwrap(); + let kp_kd_values = kp_kd_values.lock().unwrap(); + + let mut torque_commands = HashMap::new(); + for (&motor_id, &target_position) in target_positions.iter() { + if let Ok(feedback) = motors.get_latest_feedback_for(motor_id) { + if feedback.position < -TWO_PI || feedback.position > TWO_PI { + torque_commands.insert(motor_id, 0.0); + } else if let Some(&(kp, kd)) = kp_kd_values.get(&motor_id) { + let position_error = target_position - feedback.position; + let velocity_error = 0.0 - feedback.velocity; // Assuming we want to stop at the target position + + let torque = kp * position_error + kd * velocity_error; + torque_commands.insert(motor_id, torque); + } + } + } + + // println!("Torque commands: {:?}", torque_commands); + if !torque_commands.is_empty() { + let _ = motors.send_torque_controls(&torque_commands); + } + } + + { + std::thread::sleep(sleep_duration.lock().unwrap().clone()); + } + } + + let motor_ids: Vec = motors + .get_latest_feedback() + .keys() + .cloned() + .collect::>(); + + let zero_torque_sets: HashMap = + HashMap::from_iter(motor_ids.iter().map(|id| (*id, 0.0))); + let _ = motors.send_torque_controls(&zero_torque_sets); + let _ = motors.send_reset(); + }); + } + + pub fn set_target_position(&self, motor_id: u8, position: f32) { + let mut target_positions = self.target_positions.lock().unwrap(); + target_positions.insert(motor_id, position); + } + + pub fn set_kp_kd(&self, motor_id: u8, kp: f32, kd: f32) { + let mut kp_kd_values = self.kp_kd_values.lock().unwrap(); + kp_kd_values.insert(motor_id, (kp, kd)); + } + + pub fn set_sleep_duration(&self, sleep_duration: Duration) { + let mut sleep_duration_to_set = self.sleep_duration.lock().unwrap(); + *sleep_duration_to_set = sleep_duration; + } + + pub fn add_motor_to_zero(&self, motor_id: u8) { + let mut motors_to_zero = self.motors_to_zero.lock().unwrap(); + motors_to_zero.insert(motor_id); + } + + pub fn get_latest_feedback(&self) -> HashMap { + let latest_feedback = self.latest_feedback.lock().unwrap(); + latest_feedback.clone() + } + + pub fn stop(&self) { + { + let mut running = self.running.lock().unwrap(); + *running = false; + } + std::thread::sleep(Duration::from_millis(200)); + } +} + +impl Drop for MotorsSupervisor { + fn drop(&mut self) { + self.stop(); + } +} diff --git a/examples/sinusoidal_movement.py b/examples/sinusoidal_movement.py index 9be8cf2..15378af 100644 --- a/examples/sinusoidal_movement.py +++ b/examples/sinusoidal_movement.py @@ -75,6 +75,7 @@ def main() -> None: parser = argparse.ArgumentParser() parser.add_argument("--port-name", type=str, default="/dev/ttyUSB0") parser.add_argument("--motor-id", type=int, default=1) + parser.add_argument("--motor-type", type=str, default="01") parser.add_argument("--max-torque", type=float, default=10.0) parser.add_argument("--amplitude", type=float, default=math.pi) parser.add_argument("--period", type=float, default=1.0) @@ -85,7 +86,7 @@ def main() -> None: motors = RobstrideMotors( port_name=args.port_name, - motor_infos={args.motor_id: "01"}, + motor_infos={args.motor_id: args.motor_type}, ) run_motion_test( diff --git a/examples/supervisor.py b/examples/supervisor.py new file mode 100644 index 0000000..c3b0367 --- /dev/null +++ b/examples/supervisor.py @@ -0,0 +1,37 @@ +"""Example of moving a motor using the supervisor.""" + +import argparse +import math +import time + +from actuator import RobstrideMotorsSupervisor + + +def main() -> None: + parser = argparse.ArgumentParser() + parser.add_argument("--port-name", type=str, default="/dev/ttyUSB0") + parser.add_argument("--motor-id", type=int, default=1) + parser.add_argument("--motor-type", type=str, default="01") + args = parser.parse_args() + + supervisor = RobstrideMotorsSupervisor(args.port_name, {args.motor_id: args.motor_type}) + supervisor.add_motor_to_zero(args.motor_id) + start_time = time.time() + + try: + while True: + time.sleep(0.25) + target_position = 0.5 * math.sin(time.time() - start_time) + supervisor.set_target_position(args.motor_id, target_position) + # feedback = supervisor.get_latest_feedback() + # print(feedback) + + except KeyboardInterrupt: + supervisor.stop() + time.sleep(0.1) + raise + + +if __name__ == "__main__": + # python -m examples.supervisor + main()