From c594e50e663a29119d3d3c814eed1cd92298f771 Mon Sep 17 00:00:00 2001 From: Benjamin Bolte Date: Fri, 11 Oct 2024 13:20:40 -0700 Subject: [PATCH 1/3] better api --- actuator/__init__.py | 2 +- actuator/rust/bindings/src/lib.rs | 29 +++- actuator/rust/robstride/src/bin/motors.rs | 139 ---------------- actuator/rust/robstride/src/bin/supervisor.rs | 55 +++++-- actuator/rust/robstride/src/lib.rs | 154 ++++++------------ 5 files changed, 116 insertions(+), 263 deletions(-) delete mode 100644 actuator/rust/robstride/src/bin/motors.rs diff --git a/actuator/__init__.py b/actuator/__init__.py index 3cd3fc2..5407618 100644 --- a/actuator/__init__.py +++ b/actuator/__init__.py @@ -1,4 +1,4 @@ -__version__ = "0.0.11" +__version__ = "0.0.12" from .rust.bindings import ( PyRobstrideMotorFeedback as RobstrideMotorFeedback, diff --git a/actuator/rust/bindings/src/lib.rs b/actuator/rust/bindings/src/lib.rs index 5755446..ca23d84 100644 --- a/actuator/rust/bindings/src/lib.rs +++ b/actuator/rust/bindings/src/lib.rs @@ -72,12 +72,12 @@ impl PyRobstrideMotors { .collect() } - fn send_torque_controls( + fn send_motor_controls( &mut self, - torque_sets: HashMap, + motor_controls: HashMap, ) -> PyResult> { self.inner - .send_torque_controls(&torque_sets) + .send_motor_controls(&motor_controls) .map_err(|e| PyErr::new::(e.to_string()))? .into_iter() .map(|(k, v)| Ok((k, v.into()))) @@ -172,13 +172,28 @@ impl PyRobstrideMotorsSupervisor { Ok(PyRobstrideMotorsSupervisor { inner: controller }) } - fn set_target_position(&self, motor_id: u8, position: f32) -> PyResult<()> { - self.inner.set_target_position(motor_id, position); + fn set_p(&self, motor_id: u8, p_set: f32) -> PyResult<()> { + self.inner.set_p(motor_id, p_set); Ok(()) } - fn set_kp_kd(&self, motor_id: u8, kp: f32, kd: f32) -> PyResult<()> { - self.inner.set_kp_kd(motor_id, kp, kd); + fn set_v(&self, motor_id: u8, v_set: f32) -> PyResult<()> { + self.inner.set_v(motor_id, v_set); + Ok(()) + } + + fn set_kp(&self, motor_id: u8, kp_set: f32) -> PyResult<()> { + self.inner.set_kp(motor_id, kp_set); + Ok(()) + } + + fn set_kd(&self, motor_id: u8, kd_set: f32) -> PyResult<()> { + self.inner.set_kd(motor_id, kd_set); + Ok(()) + } + + fn set_t(&self, motor_id: u8, t_set: f32) -> PyResult<()> { + self.inner.set_t(motor_id, t_set); Ok(()) } diff --git a/actuator/rust/robstride/src/bin/motors.rs b/actuator/rust/robstride/src/bin/motors.rs deleted file mode 100644 index 21087a9..0000000 --- a/actuator/rust/robstride/src/bin/motors.rs +++ /dev/null @@ -1,139 +0,0 @@ -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 RUN_TIME: f32 = 3.0; -const MAX_TORQUE: f32 = 1.0; - -fn run_motion_test(motors: &mut Motors, test_id: u8) -> Result<(), Box> { - motors.send_reset()?; - motors.send_start()?; - - let start_time = Instant::now(); - let mut command_count = 0; - - // PD controller parameters - let kp_04 = 0.5; - let kd_04 = 0.1; - - // Define period and amplitude - let period = RUN_TIME; - let amplitude = PI / 1.0; - - while start_time.elapsed().as_secs_f32() < RUN_TIME { - // Track a sinusoidal trajectory over time. - 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 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)]))?; - - command_count += 1; - println!( - "Motor {} Commands: {}, Frequency: {:.2} Hz, Desired position: {:.2} Feedback: {:?}", - test_id, - command_count, - command_count as f32 / elapsed_time, - desired_position, - feedback - ); - } - - motors.send_torque_controls(&HashMap::from([(test_id, 0.0)]))?; - motors.send_reset()?; - - let elapsed_time = start_time.elapsed().as_secs_f32(); - println!( - "Done. Average control frequency: {:.2} Hz", - (command_count as f32 / elapsed_time) - ); - - Ok(()) -} - -fn print_current_mode(motors: &mut Motors) { - let modes = motors.send_get_mode(); - println!("Current mode: {:?}", modes.unwrap()); -} - -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(&port_name, &HashMap::from([(test_id, motor_type)]))?; - - let mut last_command: i32 = -1; - - loop { - println!("\nChoose an option:"); - println!("1. Print current mode"); - println!("2. Run motion test"); - println!("3. Exit"); - print!("Enter your choice (1-3), or press Enter to repeat the last command: "); - io::stdout().flush()?; - - let mut choice = String::new(); - io::stdin().read_line(&mut choice)?; - - let choice = choice.trim(); - let command = if choice.is_empty() { - last_command - } else { - choice.parse::().unwrap_or(-1) - }; - - match command { - 1 => { - print_current_mode(&mut motors); - last_command = 1; - } - 2 => { - run_motion_test(&mut motors, test_id)?; - last_command = 2; - } - 3 => break, - -1 if choice.is_empty() => { - println!("No previous command to repeat."); - } - _ => println!("Invalid choice. Please try again."), - } - } - - println!("Exiting program."); - Ok(()) -} diff --git a/actuator/rust/robstride/src/bin/supervisor.rs b/actuator/rust/robstride/src/bin/supervisor.rs index 085af72..96f58e2 100644 --- a/actuator/rust/robstride/src/bin/supervisor.rs +++ b/actuator/rust/robstride/src/bin/supervisor.rs @@ -36,11 +36,14 @@ fn main() -> Result<(), Box> { println!("Motor Controller Test CLI"); println!("Available commands:"); - println!(" set_position / s "); - println!(" set_kp_kd / k "); + println!(" p "); + println!(" v "); + println!(" t "); + println!(" kp "); + println!(" kd "); println!(" zero / z"); println!(" get_feedback / g"); - println!(" pause / p"); + println!(" pause / w"); println!(" quit / q"); loop { @@ -56,24 +59,48 @@ fn main() -> Result<(), Box> { } match parts[0] { - "set_position" | "s" => { + "p" => { if parts.len() != 2 { - println!("Usage: set_position "); + println!("Usage: p "); continue; } let position: f32 = parts[1].parse()?; - controller.set_target_position(test_id, position); + controller.set_p(test_id, position); println!("Set target position to {}", position); } - "set_kp_kd" | "k" => { - if parts.len() != 3 { - println!("Usage: set_kp_kd "); + "v" => { + if parts.len() != 2 { + println!("Usage: v "); + continue; + } + let velocity: f32 = parts[1].parse()?; + controller.set_v(test_id, velocity); + } + "t" => { + if parts.len() != 2 { + println!("Usage: t "); + continue; + } + let torque: f32 = parts[1].parse()?; + controller.set_t(test_id, torque); + } + "kp" => { + if parts.len() != 2 { + println!("Usage: kp "); 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); + controller.set_kp(test_id, kp); + println!("Set KP for motor {} to {}", test_id, kp); + } + "kd" => { + if parts.len() != 2 { + println!("Usage: kd "); + continue; + } + let kd: f32 = parts[1].parse()?; + controller.set_kd(test_id, kd); + println!("Set KD for motor {} to {}", test_id, kd); } "zero" | "z" => { controller.add_motor_to_zero(test_id); @@ -85,7 +112,7 @@ fn main() -> Result<(), Box> { println!("Motor {}: {:?}", id, fb); } } - "pause" | "p" => { + "pause" | "w" => { controller.toggle_pause(); println!("Toggled pause state"); } @@ -95,7 +122,7 @@ fn main() -> Result<(), Box> { break; } _ => { - println!("Unknown command. Available commands: set_position, set_kp_kd, get_feedback, quit"); + println!("Unknown command"); } } } diff --git a/actuator/rust/robstride/src/lib.rs b/actuator/rust/robstride/src/lib.rs index 3f0da7e..fe2502e 100644 --- a/actuator/rust/robstride/src/lib.rs +++ b/actuator/rust/robstride/src/lib.rs @@ -777,23 +777,12 @@ impl Motors { } } - fn send_pd_control( + pub fn send_motor_controls( &mut self, - motor_id: u8, - pos_set: f32, - vel_set: f32, - kp_set: f32, - kd_set: f32, - ) -> Result<(), std::io::Error> { - self.send_motor_control(motor_id, pos_set, vel_set, kp_set, kd_set, 0.0) - } - - pub fn send_pd_controls( - &mut self, - pd_sets: &HashMap, + p_v_kp_kd_t_map: &HashMap, ) -> Result, Box> { // Check if all provided motor IDs are valid - for &motor_id in pd_sets.keys() { + for &motor_id in p_v_kp_kd_t_map.keys() { if !self.motor_configs.contains_key(&motor_id) { return Err(Box::new(std::io::Error::new( std::io::ErrorKind::InvalidInput, @@ -803,33 +792,8 @@ impl Motors { } // Send PD commands for each motor - for (&motor_id, &(pos_set, vel_set, kp, kd)) in pd_sets { - self.send_pd_control(motor_id, pos_set, vel_set, kp, kd)?; - } - self.read_all_pending_responses().map_err(|e| e.into()) - } - - fn send_torque_control(&mut self, motor_id: u8, torque_set: f32) -> Result<(), std::io::Error> { - self.send_motor_control(motor_id, 0.0, 0.0, 0.0, 0.0, torque_set) - } - - pub fn send_torque_controls( - &mut self, - torque_sets: &HashMap, - ) -> Result, Box> { - // Check if all provided motor IDs are valid - for &motor_id in torque_sets.keys() { - if !self.motor_configs.contains_key(&motor_id) { - return Err(Box::new(std::io::Error::new( - std::io::ErrorKind::InvalidInput, - format!("Invalid motor ID: {}", motor_id), - ))); - } - } - - // Send torque commands for each motor - for (&motor_id, &torque_set) in torque_sets { - self.send_torque_control(motor_id, torque_set)?; + for (&motor_id, &(pos_set, vel_set, kp_set, kd_set, t_set)) in p_v_kp_kd_t_map { + self.send_motor_control(motor_id, pos_set, vel_set, kp_set, kd_set, t_set)?; } self.read_all_pending_responses().map_err(|e| e.into()) } @@ -880,8 +844,7 @@ impl Motors { pub struct MotorsSupervisor { motors: Arc>, - target_positions: Arc>>, - kp_kd_values: Arc>>, + target_p_v_kp_kd_t: Arc>>, running: Arc>, latest_feedback: Arc>>, motors_to_zero: Arc>>, @@ -898,11 +861,11 @@ impl MotorsSupervisor { let motors = Motors::new(port_name, motor_infos)?; // Get default KP/KD values for all motors. - let kp_kd_values = motors + let target_p_v_kp_kd_t = motors .motor_configs .iter() - .map(|(id, config)| (*id, (config.kp_default, config.kd_default))) - .collect::>(); + .map(|(id, config)| (*id, (0.0, 0.0, config.kp_default, config.kd_default, 0.0))) + .collect::>(); // Find motors that need to be zeroed on initialization. let zero_on_init_motors = motors @@ -914,15 +877,13 @@ impl MotorsSupervisor { let motors = Arc::new(Mutex::new(motors)); let motors_to_zero = Arc::new(Mutex::new(zero_on_init_motors)); - let target_positions = Arc::new(Mutex::new(HashMap::new())); - let kp_kd_values = Arc::new(Mutex::new(kp_kd_values)); + let target_p_v_kp_kd_t = Arc::new(Mutex::new(target_p_v_kp_kd_t)); let running = Arc::new(Mutex::new(true)); let paused = Arc::new(Mutex::new(false)); let controller = MotorsSupervisor { motors, - target_positions, - kp_kd_values, + target_p_v_kp_kd_t, running, latest_feedback: Arc::new(Mutex::new(HashMap::new())), motors_to_zero, @@ -937,8 +898,7 @@ impl MotorsSupervisor { 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 target_p_v_kp_kd_t = Arc::clone(&self.target_p_v_kp_kd_t); let running = Arc::clone(&self.running); let latest_feedback = Arc::clone(&self.latest_feedback); let motors_to_zero = Arc::clone(&self.motors_to_zero); @@ -973,51 +933,16 @@ impl MotorsSupervisor { 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 torque_commands = HashMap::from_iter( + motor_ids.iter().map(|id| (*id, (0.0, 0.0, 0.0, 0.0, 0.0))), + ); + let _ = motors.send_motor_controls(&torque_commands); } // Send PD commands to motors. { - /* - This code is equivalent, where we run closed-loop torque - control, but on the host device rather than on the motor. - - const TWO_PI: f32 = 2.0 * std::f32::consts::PI; - - 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); - } - */ - - let target_positions = target_positions.lock().unwrap(); - let kp_kd_values = kp_kd_values.lock().unwrap(); - - let pd_sets = HashMap::from_iter(target_positions.iter().map(|(id, pos)| { - (*id, (*pos, 0.0, kp_kd_values[id].0, kp_kd_values[id].1)) - })); - if !pd_sets.is_empty() { - let _ = motors.send_pd_controls(&pd_sets); - } + let target_p_v_kp_kd_t = target_p_v_kp_kd_t.lock().unwrap(); + let _ = motors.send_motor_controls(&target_p_v_kp_kd_t); } { @@ -1031,21 +956,46 @@ impl MotorsSupervisor { .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 zero_torque_sets: HashMap = + HashMap::from_iter(motor_ids.iter().map(|id| (*id, (0.0, 0.0, 0.0, 0.0, 0.0)))); + let _ = motors.send_motor_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_p_v_kp_kd_t(&self, motor_id: u8, p: f32, v: f32, kp: f32, kd: f32, t: f32) { + let mut target_p_v_kp_kd_t = self.target_p_v_kp_kd_t.lock().unwrap(); + target_p_v_kp_kd_t.insert(motor_id, (p, v, kp, kd, t)); + } + + pub fn set_p(&self, motor_id: u8, p_set: f32) { + let mut target_p_v_kp_kd_t = self.target_p_v_kp_kd_t.lock().unwrap(); + let (p, _, _, _, _) = target_p_v_kp_kd_t.get_mut(&motor_id).unwrap(); + *p = p_set; + } + + pub fn set_v(&self, motor_id: u8, v_set: f32) { + let mut target_p_v_kp_kd_t = self.target_p_v_kp_kd_t.lock().unwrap(); + let (_, v, _, _, _) = target_p_v_kp_kd_t.get_mut(&motor_id).unwrap(); + *v = v_set; + } + + pub fn set_kp(&self, motor_id: u8, kp_set: f32) { + let mut target_p_v_kp_kd_t = self.target_p_v_kp_kd_t.lock().unwrap(); + let (_, _, kp, _, _) = target_p_v_kp_kd_t.get_mut(&motor_id).unwrap(); + *kp = kp_set; + } + + pub fn set_kd(&self, motor_id: u8, kd_set: f32) { + let mut target_p_v_kp_kd_t = self.target_p_v_kp_kd_t.lock().unwrap(); + let (_, _, _, kd, _) = target_p_v_kp_kd_t.get_mut(&motor_id).unwrap(); + *kd = kd_set; } - 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_t(&self, motor_id: u8, t_set: f32) { + let mut target_p_v_kp_kd_t = self.target_p_v_kp_kd_t.lock().unwrap(); + let (_, _, _, _, t) = target_p_v_kp_kd_t.get_mut(&motor_id).unwrap(); + *t = t_set; } pub fn set_sleep_duration(&self, sleep_duration: Duration) { From f5ef5225e48b4b70ce0f8714fb31e838a2d6410c Mon Sep 17 00:00:00 2001 From: Benjamin Bolte Date: Fri, 11 Oct 2024 14:00:36 -0700 Subject: [PATCH 2/3] more improvements --- actuator/rust/bindings/bindings.pyi | 32 ++++- actuator/rust/bindings/src/lib.rs | 112 +++++++++++++-- actuator/rust/robstride/src/bin/supervisor.rs | 6 +- actuator/rust/robstride/src/lib.rs | 136 +++++++++++------- examples/sinusoidal_movement.py | 106 -------------- examples/supervisor.py | 26 +++- 6 files changed, 239 insertions(+), 179 deletions(-) delete mode 100644 examples/sinusoidal_movement.py diff --git a/actuator/rust/bindings/bindings.pyi b/actuator/rust/bindings/bindings.pyi index 8715682..9c2b632 100644 --- a/actuator/rust/bindings/bindings.pyi +++ b/actuator/rust/bindings/bindings.pyi @@ -3,6 +3,17 @@ import typing +class PyRobstrideMotorControlParams: + position: float + velocity: float + kp: float + kd: float + torque: float + def __new__(cls,position:float, velocity:float, kp:float, kd:float, torque:float): ... + def __repr__(self) -> str: + ... + + class PyRobstrideMotorFeedback: can_id: int position: float @@ -28,7 +39,7 @@ class PyRobstrideMotors: def send_start(self) -> dict[int, PyRobstrideMotorFeedback]: ... - def send_torque_controls(self, torque_sets:typing.Mapping[int, float]) -> dict[int, PyRobstrideMotorFeedback]: + def send_motor_controls(self, motor_controls:typing.Mapping[int, PyRobstrideMotorControlParams]) -> dict[int, PyRobstrideMotorFeedback]: ... def get_latest_feedback(self) -> dict[int, PyRobstrideMotorFeedback]: @@ -43,10 +54,19 @@ 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_position(self, motor_id:int, position:float) -> None: + ... + + def set_velocity(self, motor_id:int, velocity:float) -> None: ... - def set_kp_kd(self, motor_id:int, kp:float, kd:float) -> None: + def set_kp(self, motor_id:int, kp:float) -> None: + ... + + def set_kd(self, motor_id:int, kd:float) -> None: + ... + + def set_torque(self, motor_id:int, torque:float) -> None: ... def set_sleep_duration(self, sleep_duration:float) -> None: @@ -58,10 +78,16 @@ class PyRobstrideMotorsSupervisor: def get_latest_feedback(self) -> dict[int, PyRobstrideMotorFeedback]: ... + def toggle_pause(self) -> None: + ... + def stop(self) -> None: ... def __repr__(self) -> str: ... + def set_params(self, motor_id:int, params:PyRobstrideMotorControlParams) -> None: + ... + diff --git a/actuator/rust/bindings/src/lib.rs b/actuator/rust/bindings/src/lib.rs index ca23d84..487cb6f 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, MotorFeedback as RobstrideMotorFeedback, + 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, }; @@ -74,8 +75,13 @@ impl PyRobstrideMotors { fn send_motor_controls( &mut self, - motor_controls: HashMap, + motor_controls: HashMap, ) -> PyResult> { + let motor_controls: HashMap = motor_controls + .into_iter() + .map(|(k, v)| (k, v.into())) + .collect(); + self.inner .send_motor_controls(&motor_controls) .map_err(|e| PyErr::new::(e.to_string()))? @@ -147,6 +153,68 @@ impl From for PyRobstrideMotorFeedback { } } +#[gen_stub_pyclass] +#[pyclass] +#[derive(FromPyObject)] +struct PyRobstrideMotorControlParams { + #[pyo3(get, set)] + position: f32, + #[pyo3(get, set)] + velocity: f32, + #[pyo3(get, set)] + kp: f32, + #[pyo3(get, set)] + kd: f32, + #[pyo3(get, set)] + torque: f32, +} + +#[gen_stub_pymethods] +#[pymethods] +impl PyRobstrideMotorControlParams { + #[new] + fn new(position: f32, velocity: f32, kp: f32, kd: f32, torque: f32) -> Self { + PyRobstrideMotorControlParams { + position, + velocity, + kp, + kd, + torque, + } + } + + fn __repr__(&self) -> PyResult { + Ok(format!( + "PyRobstrideMotorControlParams(position={:.2}, velocity={:.2}, kp={:.2}, kd={:.2}, torque={:.2})", + self.position, self.velocity, self.kp, self.kd, self.torque + )) + } +} + +impl From for RobstrideMotorControlParams { + fn from(params: PyRobstrideMotorControlParams) -> Self { + RobstrideMotorControlParams { + position: params.position, + velocity: params.velocity, + kp: params.kp, + kd: params.kd, + torque: params.torque, + } + } +} + +impl From for PyRobstrideMotorControlParams { + fn from(params: RobstrideMotorControlParams) -> Self { + PyRobstrideMotorControlParams { + position: params.position, + velocity: params.velocity, + kp: params.kp, + kd: params.kd, + torque: params.torque, + } + } +} + #[gen_stub_pyclass] #[pyclass] struct PyRobstrideMotorsSupervisor { @@ -172,28 +240,28 @@ impl PyRobstrideMotorsSupervisor { Ok(PyRobstrideMotorsSupervisor { inner: controller }) } - fn set_p(&self, motor_id: u8, p_set: f32) -> PyResult<()> { - self.inner.set_p(motor_id, p_set); + fn set_position(&self, motor_id: u8, position: f32) -> PyResult<()> { + self.inner.set_position(motor_id, position); Ok(()) } - fn set_v(&self, motor_id: u8, v_set: f32) -> PyResult<()> { - self.inner.set_v(motor_id, v_set); + fn set_velocity(&self, motor_id: u8, velocity: f32) -> PyResult<()> { + self.inner.set_velocity(motor_id, velocity); Ok(()) } - fn set_kp(&self, motor_id: u8, kp_set: f32) -> PyResult<()> { - self.inner.set_kp(motor_id, kp_set); + fn set_kp(&self, motor_id: u8, kp: f32) -> PyResult<()> { + self.inner.set_kp(motor_id, kp); Ok(()) } - fn set_kd(&self, motor_id: u8, kd_set: f32) -> PyResult<()> { - self.inner.set_kd(motor_id, kd_set); + fn set_kd(&self, motor_id: u8, kd: f32) -> PyResult<()> { + self.inner.set_kd(motor_id, kd); Ok(()) } - fn set_t(&self, motor_id: u8, t_set: f32) -> PyResult<()> { - self.inner.set_t(motor_id, t_set); + fn set_torque(&self, motor_id: u8, torque: f32) -> PyResult<()> { + self.inner.set_torque(motor_id, torque); Ok(()) } @@ -216,6 +284,11 @@ impl PyRobstrideMotorsSupervisor { .collect() } + fn toggle_pause(&self) -> PyResult<()> { + self.inner.toggle_pause(); + Ok(()) + } + fn stop(&self) -> PyResult<()> { self.inner.stop(); Ok(()) @@ -228,6 +301,20 @@ impl PyRobstrideMotorsSupervisor { motor_count )) } + + fn set_params(&self, motor_id: u8, params: &PyRobstrideMotorControlParams) -> PyResult<()> { + self.inner.set_params( + motor_id, + RobstrideMotorControlParams { + position: params.position, + velocity: params.velocity, + kp: params.kp, + kd: params.kd, + torque: params.torque, + }, + ); + Ok(()) + } } #[pymodule] @@ -235,6 +322,7 @@ fn bindings(m: &Bound) -> PyResult<()> { m.add_class::()?; m.add_class::()?; m.add_class::()?; + m.add_class::()?; Ok(()) } diff --git a/actuator/rust/robstride/src/bin/supervisor.rs b/actuator/rust/robstride/src/bin/supervisor.rs index 96f58e2..3157f63 100644 --- a/actuator/rust/robstride/src/bin/supervisor.rs +++ b/actuator/rust/robstride/src/bin/supervisor.rs @@ -65,7 +65,7 @@ fn main() -> Result<(), Box> { continue; } let position: f32 = parts[1].parse()?; - controller.set_p(test_id, position); + controller.set_position(test_id, position); println!("Set target position to {}", position); } "v" => { @@ -74,7 +74,7 @@ fn main() -> Result<(), Box> { continue; } let velocity: f32 = parts[1].parse()?; - controller.set_v(test_id, velocity); + controller.set_velocity(test_id, velocity); } "t" => { if parts.len() != 2 { @@ -82,7 +82,7 @@ fn main() -> Result<(), Box> { continue; } let torque: f32 = parts[1].parse()?; - controller.set_t(test_id, torque); + controller.set_torque(test_id, torque); } "kp" => { if parts.len() != 2 { diff --git a/actuator/rust/robstride/src/lib.rs b/actuator/rust/robstride/src/lib.rs index fe2502e..80911f4 100644 --- a/actuator/rust/robstride/src/lib.rs +++ b/actuator/rust/robstride/src/lib.rs @@ -359,6 +359,27 @@ pub fn motor_type_from_str(s: &str) -> Result { } } +#[derive(Debug, Clone, Copy)] +pub struct MotorControlParams { + pub position: f32, + pub velocity: f32, + pub kp: f32, + pub kd: f32, + pub torque: f32, +} + +impl Default for MotorControlParams { + fn default() -> Self { + MotorControlParams { + position: 0.0, + velocity: 0.0, + kp: 0.0, + kd: 0.0, + torque: 0.0, + } + } +} + pub struct Motors { port: Box, motor_configs: HashMap, @@ -736,11 +757,7 @@ impl Motors { fn send_motor_control( &mut self, id: u8, - pos_set: f32, - vel_set: f32, - kp_set: f32, - kd_set: f32, - torque_set: f32, + params: &MotorControlParams, ) -> Result<(), std::io::Error> { self.send_set_mode(RunMode::MitMode)?; @@ -756,11 +773,11 @@ impl Motors { data: vec![0; 8], }; - let pos_int_set = float_to_uint(pos_set, config.p_min, config.p_max, 16); - let vel_int_set = float_to_uint(vel_set, config.v_min, config.v_max, 16); - let kp_int_set = float_to_uint(kp_set, config.kp_min, config.kp_max, 16); - let kd_int_set = float_to_uint(kd_set, config.kd_min, config.kd_max, 16); - let torque_int_set = float_to_uint(torque_set, config.t_min, config.t_max, 16); + let pos_int_set = float_to_uint(params.position, config.p_min, config.p_max, 16); + let vel_int_set = float_to_uint(params.velocity, config.v_min, config.v_max, 16); + let kp_int_set = float_to_uint(params.kp, config.kp_min, config.kp_max, 16); + let kd_int_set = float_to_uint(params.kd, config.kd_min, config.kd_max, 16); + let torque_int_set = float_to_uint(params.torque, config.t_min, config.t_max, 16); pack.ex_id.data = torque_int_set; pack.data[0..2].copy_from_slice(&pos_int_set.to_be_bytes()); @@ -779,10 +796,10 @@ impl Motors { pub fn send_motor_controls( &mut self, - p_v_kp_kd_t_map: &HashMap, + params_map: &HashMap, ) -> Result, Box> { // Check if all provided motor IDs are valid - for &motor_id in p_v_kp_kd_t_map.keys() { + for &motor_id in params_map.keys() { if !self.motor_configs.contains_key(&motor_id) { return Err(Box::new(std::io::Error::new( std::io::ErrorKind::InvalidInput, @@ -792,8 +809,8 @@ impl Motors { } // Send PD commands for each motor - for (&motor_id, &(pos_set, vel_set, kp_set, kd_set, t_set)) in p_v_kp_kd_t_map { - self.send_motor_control(motor_id, pos_set, vel_set, kp_set, kd_set, t_set)?; + for (&motor_id, params) in params_map { + self.send_motor_control(motor_id, params)?; } self.read_all_pending_responses().map_err(|e| e.into()) } @@ -844,7 +861,7 @@ impl Motors { pub struct MotorsSupervisor { motors: Arc>, - target_p_v_kp_kd_t: Arc>>, + target_params: Arc>>, running: Arc>, latest_feedback: Arc>>, motors_to_zero: Arc>>, @@ -861,11 +878,22 @@ impl MotorsSupervisor { let motors = Motors::new(port_name, motor_infos)?; // Get default KP/KD values for all motors. - let target_p_v_kp_kd_t = motors + let target_params = motors .motor_configs .iter() - .map(|(id, config)| (*id, (0.0, 0.0, config.kp_default, config.kd_default, 0.0))) - .collect::>(); + .map(|(id, config)| { + ( + *id, + MotorControlParams { + position: 0.0, + velocity: 0.0, + kp: config.kp_default, + kd: config.kd_default, + torque: 0.0, + }, + ) + }) + .collect::>(); // Find motors that need to be zeroed on initialization. let zero_on_init_motors = motors @@ -877,13 +905,13 @@ impl MotorsSupervisor { let motors = Arc::new(Mutex::new(motors)); let motors_to_zero = Arc::new(Mutex::new(zero_on_init_motors)); - let target_p_v_kp_kd_t = Arc::new(Mutex::new(target_p_v_kp_kd_t)); + let target_params = Arc::new(Mutex::new(target_params)); let running = Arc::new(Mutex::new(true)); let paused = Arc::new(Mutex::new(false)); let controller = MotorsSupervisor { motors, - target_p_v_kp_kd_t, + target_params, running, latest_feedback: Arc::new(Mutex::new(HashMap::new())), motors_to_zero, @@ -898,7 +926,7 @@ impl MotorsSupervisor { fn start_control_thread(&self) { let motors = Arc::clone(&self.motors); - let target_p_v_kp_kd_t = Arc::clone(&self.target_p_v_kp_kd_t); + let target_params = Arc::clone(&self.target_params); let running = Arc::clone(&self.running); let latest_feedback = Arc::clone(&self.latest_feedback); let motors_to_zero = Arc::clone(&self.motors_to_zero); @@ -934,15 +962,17 @@ impl MotorsSupervisor { motor_ids_to_zero.clear(); } let torque_commands = HashMap::from_iter( - motor_ids.iter().map(|id| (*id, (0.0, 0.0, 0.0, 0.0, 0.0))), + motor_ids + .iter() + .map(|id| (*id, MotorControlParams::default())), ); let _ = motors.send_motor_controls(&torque_commands); } // Send PD commands to motors. { - let target_p_v_kp_kd_t = target_p_v_kp_kd_t.lock().unwrap(); - let _ = motors.send_motor_controls(&target_p_v_kp_kd_t); + let target_params = target_params.lock().unwrap(); + let _ = motors.send_motor_controls(&target_params); } { @@ -956,46 +986,54 @@ impl MotorsSupervisor { .cloned() .collect::>(); - let zero_torque_sets: HashMap = - HashMap::from_iter(motor_ids.iter().map(|id| (*id, (0.0, 0.0, 0.0, 0.0, 0.0)))); + let zero_torque_sets: HashMap = HashMap::from_iter( + motor_ids + .iter() + .map(|id| (*id, MotorControlParams::default())), + ); let _ = motors.send_motor_controls(&zero_torque_sets); let _ = motors.send_reset(); }); } - pub fn set_p_v_kp_kd_t(&self, motor_id: u8, p: f32, v: f32, kp: f32, kd: f32, t: f32) { - let mut target_p_v_kp_kd_t = self.target_p_v_kp_kd_t.lock().unwrap(); - target_p_v_kp_kd_t.insert(motor_id, (p, v, kp, kd, t)); + pub fn set_params(&self, motor_id: u8, params: MotorControlParams) { + let mut target_params = self.target_params.lock().unwrap(); + target_params.insert(motor_id, params); } - pub fn set_p(&self, motor_id: u8, p_set: f32) { - let mut target_p_v_kp_kd_t = self.target_p_v_kp_kd_t.lock().unwrap(); - let (p, _, _, _, _) = target_p_v_kp_kd_t.get_mut(&motor_id).unwrap(); - *p = p_set; + pub fn set_position(&self, motor_id: u8, position: f32) { + let mut target_params = self.target_params.lock().unwrap(); + if let Some(params) = target_params.get_mut(&motor_id) { + params.position = position; + } } - pub fn set_v(&self, motor_id: u8, v_set: f32) { - let mut target_p_v_kp_kd_t = self.target_p_v_kp_kd_t.lock().unwrap(); - let (_, v, _, _, _) = target_p_v_kp_kd_t.get_mut(&motor_id).unwrap(); - *v = v_set; + pub fn set_velocity(&self, motor_id: u8, velocity: f32) { + let mut target_params = self.target_params.lock().unwrap(); + if let Some(params) = target_params.get_mut(&motor_id) { + params.velocity = velocity; + } } - pub fn set_kp(&self, motor_id: u8, kp_set: f32) { - let mut target_p_v_kp_kd_t = self.target_p_v_kp_kd_t.lock().unwrap(); - let (_, _, kp, _, _) = target_p_v_kp_kd_t.get_mut(&motor_id).unwrap(); - *kp = kp_set; + pub fn set_kp(&self, motor_id: u8, kp: f32) { + let mut target_params = self.target_params.lock().unwrap(); + if let Some(params) = target_params.get_mut(&motor_id) { + params.kp = kp; + } } - pub fn set_kd(&self, motor_id: u8, kd_set: f32) { - let mut target_p_v_kp_kd_t = self.target_p_v_kp_kd_t.lock().unwrap(); - let (_, _, _, kd, _) = target_p_v_kp_kd_t.get_mut(&motor_id).unwrap(); - *kd = kd_set; + pub fn set_kd(&self, motor_id: u8, kd: f32) { + let mut target_params = self.target_params.lock().unwrap(); + if let Some(params) = target_params.get_mut(&motor_id) { + params.kd = kd; + } } - pub fn set_t(&self, motor_id: u8, t_set: f32) { - let mut target_p_v_kp_kd_t = self.target_p_v_kp_kd_t.lock().unwrap(); - let (_, _, _, _, t) = target_p_v_kp_kd_t.get_mut(&motor_id).unwrap(); - *t = t_set; + pub fn set_torque(&self, motor_id: u8, torque: f32) { + let mut target_params = self.target_params.lock().unwrap(); + if let Some(params) = target_params.get_mut(&motor_id) { + params.torque = torque; + } } pub fn set_sleep_duration(&self, sleep_duration: Duration) { diff --git a/examples/sinusoidal_movement.py b/examples/sinusoidal_movement.py deleted file mode 100644 index 15378af..0000000 --- a/examples/sinusoidal_movement.py +++ /dev/null @@ -1,106 +0,0 @@ -"""Example of moving a motor in a sinusoidal pattern.""" - -import argparse -import logging -import math -import time - -from actuator import RobstrideMotors - -logger = logging.getLogger(__name__) - - -def run_motion_test( - motors: RobstrideMotors, - period: float = 1.0, - motor_id: int = 1, - max_torque: float = 10.0, - amplitude: float = math.pi / 2.0, - run_time_s: float = 10.0, - kp: float = 1.0, - kd: float = 0.01, -) -> None: - motors.send_reset() - motors.send_set_zero([motor_id]) - motors.send_start() - - start_time = time.time() - command_count = 0 - - try: - while time.time() - start_time < run_time_s: - dt = time.time() - start_time - - # Reference velocity is the derivative of the reference position. - p_ref = amplitude * math.cos(dt * math.pi * 2.0 / period + math.pi / 2.0) - v_ref = -amplitude * math.pi * 2.0 / period * math.sin(dt * math.pi * 2.0 / period + math.pi / 2.0) - # v_ref = 0.0 - - try: - feedback = motors.get_latest_feedback_for(motor_id) - p_cur = feedback.position - v_cur = feedback.velocity - torque = max(min(kp * (p_ref - p_cur) + kd * (v_ref - v_cur), max_torque), -max_torque) - - motors.send_torque_controls({motor_id: torque}) - - except RuntimeError: - logger.exception("Runtime error while getting latest feedback") - motors.send_torque_controls({motor_id: 0.0}) - - command_count += 1 - logger.info( - "Motor %d Commands: %d, Frequency: %.2f Hz, Desired position: %.2f Feedback: %s", - motor_id, - command_count, - command_count / dt, - p_ref, - feedback, - ) - - except KeyboardInterrupt: - pass - - finally: - motors.send_torque_controls({motor_id: 0.0}) - motors.send_reset() - - dt = time.time() - start_time - logger.info(f"Done. Average control frequency: {command_count / dt:.2f} Hz") - - -def main() -> None: - logging.basicConfig(level=logging.INFO) - - 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) - parser.add_argument("--run-time-s", type=float, default=10.0) - parser.add_argument("--kp", type=float, default=1.0) - parser.add_argument("--kd", type=float, default=0.01) - args = parser.parse_args() - - motors = RobstrideMotors( - port_name=args.port_name, - motor_infos={args.motor_id: args.motor_type}, - ) - - run_motion_test( - motors, - period=args.period, - motor_id=args.motor_id, - max_torque=args.max_torque, - amplitude=args.amplitude, - run_time_s=args.run_time_s, - kp=args.kp, - kd=args.kd, - ) - - -if __name__ == "__main__": - # python -m examples.sinusoidal_movement - main() diff --git a/examples/supervisor.py b/examples/supervisor.py index c3b0367..3607012 100644 --- a/examples/supervisor.py +++ b/examples/supervisor.py @@ -12,19 +12,33 @@ def main() -> None: 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("--sleep", type=float, default=0.0) + parser.add_argument("--period", type=float, default=10.0) + parser.add_argument("--amplitude", type=float, default=1.0) + parser.add_argument("--verbose", action="store_true") args = parser.parse_args() + amplitude = args.amplitude + period = args.period + supervisor = RobstrideMotorsSupervisor(args.port_name, {args.motor_id: args.motor_type}) - supervisor.add_motor_to_zero(args.motor_id) + + supervisor.set_kp(args.motor_id, 10.0) + supervisor.set_kd(args.motor_id, 1.0) + 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) + elapsed_time = time.time() - start_time + target_position = amplitude * math.sin(elapsed_time * 2 * math.pi / period) + target_velocity = amplitude * 2 * math.pi / period * math.cos(elapsed_time * 2 * math.pi / period) + supervisor.set_position(args.motor_id, target_position) + supervisor.set_velocity(args.motor_id, target_velocity) + time.sleep(args.sleep) + if args.verbose: + feedback = supervisor.get_latest_feedback() + print(feedback) except KeyboardInterrupt: supervisor.stop() From 3b07772176f0cf230423601234664eeab977c171 Mon Sep 17 00:00:00 2001 From: Benjamin Bolte Date: Fri, 11 Oct 2024 14:02:45 -0700 Subject: [PATCH 3/3] update project description --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 7cda94f..7cff5ea 100644 --- a/setup.py +++ b/setup.py @@ -34,7 +34,7 @@ setup( name="actuator", version=version, - description="The actuator project", + description="Python interface for controlling various robotic actuators", author="Benjamin Bolte", url="https://github.com/kscalelabs/actuator", rust_extensions=[