From 5ce8fed4763f4d64710db6f27b82d82e0444626d Mon Sep 17 00:00:00 2001 From: Benjamin Bolte Date: Sun, 10 Nov 2024 18:12:28 -0800 Subject: [PATCH 1/2] expose more methods --- actuator/bindings/src/lib.rs | 27 ++++++++++++++++++ actuator/robstride/src/supervisor.rs | 41 ++++++++++++++++++++++++---- 2 files changed, 62 insertions(+), 6 deletions(-) diff --git a/actuator/bindings/src/lib.rs b/actuator/bindings/src/lib.rs index 038bcf7..52b6341 100644 --- a/actuator/bindings/src/lib.rs +++ b/actuator/bindings/src/lib.rs @@ -258,6 +258,27 @@ impl PyRobstrideMotorsSupervisor { Ok(PyRobstrideMotorsSupervisor { inner: controller }) } + fn set_all_params(&self, params: HashMap) -> PyResult<()> { + let params: HashMap = params + .into_iter() + .map(|(k, v)| (k, RobstrideMotorControlParams::from(v))) + .collect(); + self.inner.set_all_params(params); + Ok(()) + } + + fn set_params(&self, motor_id: u8, params: PyRobstrideMotorControlParams) -> PyResult<()> { + self.inner + .set_params(motor_id, RobstrideMotorControlParams::from(params)) + .map_err(|e| PyErr::new::(e.to_string())) + } + + fn set_positions(&self, positions: HashMap) -> PyResult<()> { + self.inner + .set_positions(positions) + .map_err(|e| PyErr::new::(e.to_string())) + } + fn set_position(&self, motor_id: u8, position: f32) -> PyResult { self.inner .set_position(motor_id, position) @@ -270,6 +291,12 @@ impl PyRobstrideMotorsSupervisor { .map_err(|e| PyErr::new::(e.to_string())) } + fn set_velocities(&self, velocities: HashMap) -> PyResult<()> { + self.inner + .set_velocities(velocities) + .map_err(|e| PyErr::new::(e.to_string())) + } + fn set_velocity(&self, motor_id: u8, velocity: f32) -> PyResult { self.inner .set_velocity(motor_id, velocity) diff --git a/actuator/robstride/src/supervisor.rs b/actuator/robstride/src/supervisor.rs index 28eabb3..b567b65 100644 --- a/actuator/robstride/src/supervisor.rs +++ b/actuator/robstride/src/supervisor.rs @@ -156,14 +156,17 @@ impl MotorsSupervisor { } { - // Send PD commands to motors. - let target_params = target_params.read().unwrap(); - if !target_params.is_empty() { - match motors.send_motor_controls(&target_params, *serial.read().unwrap()) { + let params_copy = { + let target_params = target_params.read().unwrap(); + target_params.clone() + }; + + if !params_copy.is_empty() { + match motors.send_motor_controls(¶ms_copy, *serial.read().unwrap()) { Ok(feedbacks) => { let mut latest_feedback = latest_feedback.write().unwrap(); let mut failed_commands = failed_commands.write().unwrap(); - for &motor_id in target_params.keys() { + for &motor_id in params_copy.keys() { if let Some(feedback) = feedbacks.get(&motor_id) { latest_feedback.insert(motor_id, feedback.clone()); } else { @@ -236,9 +239,27 @@ impl MotorsSupervisor { *self.failed_commands.write().unwrap() = HashMap::new(); } - pub fn set_params(&self, motor_id: u8, params: MotorControlParams) { + pub fn set_all_params(&self, params: HashMap) { + let mut target_params = self.target_params.write().unwrap(); + *target_params = params; + } + + pub fn set_params( + &self, + motor_id: u8, + params: MotorControlParams, + ) -> Result<(), std::io::Error> { let mut target_params = self.target_params.write().unwrap(); target_params.insert(motor_id, params); + Ok(()) + } + + pub fn set_positions(&self, positions: HashMap) -> Result<(), std::io::Error> { + let mut target_params = self.target_params.write().unwrap(); + for (motor_id, position) in positions { + target_params.get_mut(&motor_id).unwrap().position = position; + } + Ok(()) } pub fn set_position(&self, motor_id: u8, position: f32) -> Result { @@ -267,6 +288,14 @@ impl MotorsSupervisor { }) } + pub fn set_velocities(&self, velocities: HashMap) -> Result<(), std::io::Error> { + let mut target_params = self.target_params.write().unwrap(); + for (motor_id, velocity) in velocities { + target_params.get_mut(&motor_id).unwrap().velocity = velocity; + } + Ok(()) + } + pub fn set_velocity(&self, motor_id: u8, velocity: f32) -> Result { let mut target_params = self.target_params.write().unwrap(); if let Some(params) = target_params.get_mut(&motor_id) { From e4f88c10243aae6846ec3655d35bbd808e7b3713 Mon Sep 17 00:00:00 2001 From: Benjamin Bolte Date: Sun, 10 Nov 2024 18:18:37 -0800 Subject: [PATCH 2/2] remove duplicate method --- actuator/bindings/src/lib.rs | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/actuator/bindings/src/lib.rs b/actuator/bindings/src/lib.rs index 52b6341..07014e9 100644 --- a/actuator/bindings/src/lib.rs +++ b/actuator/bindings/src/lib.rs @@ -377,20 +377,6 @@ impl PyRobstrideMotorsSupervisor { )) } - 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(()) - } - #[getter] fn total_commands(&self) -> PyResult { Ok(self.inner.get_total_commands())