From c85a644b3616822fcb24b249e2dc506902ba7d37 Mon Sep 17 00:00:00 2001 From: WT-MM Date: Mon, 11 Nov 2024 11:50:12 -0800 Subject: [PATCH 1/4] torque limit draft --- actuator/bindings/src/lib.rs | 6 ++++++ actuator/robstride/src/motor.rs | 30 ++++++++++++++++++++++++++++ actuator/robstride/src/supervisor.rs | 23 ++++++++++++++++++++- 3 files changed, 58 insertions(+), 1 deletion(-) diff --git a/actuator/bindings/src/lib.rs b/actuator/bindings/src/lib.rs index 07014e9..0c328f3 100644 --- a/actuator/bindings/src/lib.rs +++ b/actuator/bindings/src/lib.rs @@ -345,6 +345,12 @@ impl PyRobstrideMotorsSupervisor { .map_err(|e| PyErr::new::(e.to_string())) } + fn set_torque_limit(&self, motor_id: u8, torque_limit: f32) -> PyResult { + self.inner + .set_torque_limit(motor_id, torque_limit) + .map_err(|e| PyErr::new::(e.to_string())) + } + fn add_motor_to_zero(&self, motor_id: u8) -> PyResult<()> { self.inner .add_motor_to_zero(motor_id) diff --git a/actuator/robstride/src/motor.rs b/actuator/robstride/src/motor.rs index 34f5c93..55bf28f 100644 --- a/actuator/robstride/src/motor.rs +++ b/actuator/robstride/src/motor.rs @@ -33,6 +33,11 @@ impl Default for MotorControlParams { } } +#[derive(Debug, Default, Clone, Serialize, Deserialize)] +pub struct MotorSdoParams { + pub torque_limit: f32, +} + #[derive(Debug, Default, Clone, Serialize, Deserialize)] pub struct MotorFeedback { pub can_id: u8, @@ -343,6 +348,31 @@ impl Motors { Ok(()) } + pub fn set_torque_limit(&mut self, motor_id: u8, torque_limit: f32) -> Result<(), std::io::Error> { + let config = *self.motor_configs.get(&motor_id).ok_or(std::io::Error::new( + std::io::ErrorKind::NotFound, + "Motor not found", + ))?; + + let mut pack = CanPack { + ex_id: ExId { + id: motor_id, + data: CAN_ID_DEBUG_UI as u16, + mode: CanComMode::SdoWrite, + res: 0, + }, + len: 8, + data: vec![0; 8], + }; + let index: u16 = 0x700b; + pack.data[..2].copy_from_slice(&index.to_le_bytes()); + + let torque_limit_int = float_to_uint(torque_limit, config.t_min, config.t_max, 16); + pack.data[4..8].copy_from_slice(&torque_limit_int.to_le_bytes()); + self.send_command(&pack, true)?; + Ok(()) + } + pub fn send_reset(&mut self, id: u8) -> Result { let pack = CanPack { ex_id: ExId { diff --git a/actuator/robstride/src/supervisor.rs b/actuator/robstride/src/supervisor.rs index b567b65..403b22a 100644 --- a/actuator/robstride/src/supervisor.rs +++ b/actuator/robstride/src/supervisor.rs @@ -3,7 +3,7 @@ use std::sync::{Arc, Mutex, RwLock}; use std::thread; use std::time::Duration; -use crate::motor::{MotorControlParams, MotorFeedback, Motors}; +use crate::motor::{MotorControlParams, MotorFeedback, MotorSdoParams, Motors}; use crate::types::{MotorType, RunMode}; use log::{error, info}; @@ -13,6 +13,7 @@ pub struct MotorsSupervisor { running: Arc>, latest_feedback: Arc>>, motors_to_zero: Arc>>, + motors_to_set_sdo: Arc>>, paused: Arc>, restart: Arc>, total_commands: Arc>, @@ -69,6 +70,7 @@ impl MotorsSupervisor { running: Arc::new(RwLock::new(true)), latest_feedback: Arc::new(RwLock::new(HashMap::new())), motors_to_zero: Arc::new(Mutex::new(zero_on_init_motors)), + motors_to_set_sdo: Arc::new(Mutex::new(HashMap::new())), paused: Arc::new(RwLock::new(false)), restart: Arc::new(Mutex::new(false)), total_commands: Arc::new(RwLock::new(total_commands)), @@ -91,6 +93,7 @@ impl MotorsSupervisor { let motors_to_zero = Arc::clone(&self.motors_to_zero); let paused = Arc::clone(&self.paused); let restart = Arc::clone(&self.restart); + let motors_to_set_sdo = Arc::clone(&self.motors_to_set_sdo); let total_commands = Arc::clone(&self.total_commands); let failed_commands = Arc::clone(&self.failed_commands); let max_update_rate = Arc::clone(&self.max_update_rate); @@ -155,6 +158,18 @@ impl MotorsSupervisor { } } + { + // Send updated sdo parameters to motors that need them. + let mut motors_to_set_sdo = motors_to_set_sdo.lock().unwrap(); + if !motors_to_set_sdo.is_empty() { + for (motor_id, params) in motors_to_set_sdo.iter_mut() { + motors.set_torque_limit(*motor_id, params.torque_limit).unwrap(); + // Any other sdo parameters can be updated here. + } + motors_to_set_sdo.clear(); + } + } + { let params_copy = { let target_params = target_params.read().unwrap(); @@ -374,6 +389,12 @@ impl MotorsSupervisor { }) } + pub fn set_torque_limit(&self, motor_id: u8, torque_limit: f32) -> Result { + let mut motors_to_set_sdo = self.motors_to_set_sdo.lock().unwrap(); + motors_to_set_sdo.insert(motor_id, MotorSdoParams { torque_limit }); + Ok(torque_limit) + } + pub fn set_torque(&self, motor_id: u8, torque: f32) -> Result { let mut target_params = self.target_params.write().unwrap(); if let Some(params) = target_params.get_mut(&motor_id) { From 94757565938a4844730d90e96f7a71bbdb5d381b Mon Sep 17 00:00:00 2001 From: WT-MM Date: Tue, 12 Nov 2024 07:31:55 +0800 Subject: [PATCH 2/4] compare --- actuator/robstride/src/config.rs | 10 +++++----- actuator/robstride/src/motor.rs | 18 ++++++++++++++---- actuator/robstride/src/supervisor.rs | 22 +++++++++++----------- 3 files changed, 30 insertions(+), 20 deletions(-) diff --git a/actuator/robstride/src/config.rs b/actuator/robstride/src/config.rs index 2bd7188..baea106 100644 --- a/actuator/robstride/src/config.rs +++ b/actuator/robstride/src/config.rs @@ -34,7 +34,7 @@ lazy_static! { kp_max: 500.0, kd_min: 0.0, kd_max: 5.0, - t_min: -14.0, + t_min: 0.0, t_max: 14.0, zero_on_init: false, can_timeout_command: 0x200b, // Unchecked @@ -52,7 +52,7 @@ lazy_static! { kp_max: 500.0, kd_min: 0.0, kd_max: 5.0, - t_min: -12.0, + t_min: 0.0, t_max: 12.0, zero_on_init: true, // Single encoder motor. can_timeout_command: 0x200c, @@ -70,7 +70,7 @@ lazy_static! { kp_max: 500.0, kd_min: 0.0, kd_max: 5.0, - t_min: -12.0, + t_min: 0.0, t_max: 12.0, zero_on_init: false, can_timeout_command: 0x200b, // Unchecked @@ -88,7 +88,7 @@ lazy_static! { kp_max: 5000.0, kd_min: 0.0, kd_max: 100.0, - t_min: -60.0, + t_min: 0.0, t_max: 60.0, zero_on_init: false, can_timeout_command: 0x200b, @@ -106,7 +106,7 @@ lazy_static! { kp_max: 5000.0, kd_min: 0.0, kd_max: 100.0, - t_min: -120.0, + t_min: 0.0, t_max: 120.0, zero_on_init: false, can_timeout_command: 0x200b, diff --git a/actuator/robstride/src/motor.rs b/actuator/robstride/src/motor.rs index 55bf28f..df8b77c 100644 --- a/actuator/robstride/src/motor.rs +++ b/actuator/robstride/src/motor.rs @@ -33,11 +33,19 @@ impl Default for MotorControlParams { } } -#[derive(Debug, Default, Clone, Serialize, Deserialize)] +#[derive(Debug, Clone, Serialize, Deserialize)] pub struct MotorSdoParams { pub torque_limit: f32, } +impl Default for MotorSdoParams { + fn default() -> Self { + MotorSdoParams { + torque_limit: 0.0, + } + } +} + #[derive(Debug, Default, Clone, Serialize, Deserialize)] pub struct MotorFeedback { pub can_id: u8, @@ -364,11 +372,13 @@ impl Motors { len: 8, data: vec![0; 8], }; - let index: u16 = 0x700b; + + let index: u16 = 0x700B; pack.data[..2].copy_from_slice(&index.to_le_bytes()); - let torque_limit_int = float_to_uint(torque_limit, config.t_min, config.t_max, 16); - pack.data[4..8].copy_from_slice(&torque_limit_int.to_le_bytes()); + let torque_limit_safe = torque_limit.clamp(config.t_min, config.t_max); + pack.data[4..8].copy_from_slice(&torque_limit_safe.to_le_bytes()); + self.send_command(&pack, true)?; Ok(()) } diff --git a/actuator/robstride/src/supervisor.rs b/actuator/robstride/src/supervisor.rs index 403b22a..1cec2b0 100644 --- a/actuator/robstride/src/supervisor.rs +++ b/actuator/robstride/src/supervisor.rs @@ -158,17 +158,17 @@ impl MotorsSupervisor { } } - { - // Send updated sdo parameters to motors that need them. - let mut motors_to_set_sdo = motors_to_set_sdo.lock().unwrap(); - if !motors_to_set_sdo.is_empty() { - for (motor_id, params) in motors_to_set_sdo.iter_mut() { - motors.set_torque_limit(*motor_id, params.torque_limit).unwrap(); - // Any other sdo parameters can be updated here. - } - motors_to_set_sdo.clear(); - } - } + // { + // // Send updated sdo parameters to motors that need them. + // let mut motors_to_set_sdo = motors_to_set_sdo.lock().unwrap(); + // if !motors_to_set_sdo.is_empty() { + // for (motor_id, params) in motors_to_set_sdo.iter_mut() { + // motors.set_torque_limit(*motor_id, params.torque_limit).unwrap(); + // // Any other sdo parameters can be updated here. + // } + // motors_to_set_sdo.clear(); + // } + // } { let params_copy = { From 5fa8c053d61e8c2e5ae225d9599acefdee361524 Mon Sep 17 00:00:00 2001 From: WT-MM Date: Tue, 12 Nov 2024 07:40:52 +0800 Subject: [PATCH 3/4] fix idiocy --- actuator/robstride/src/config.rs | 10 +++++----- actuator/robstride/src/supervisor.rs | 22 +++++++++++----------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/actuator/robstride/src/config.rs b/actuator/robstride/src/config.rs index baea106..2bd7188 100644 --- a/actuator/robstride/src/config.rs +++ b/actuator/robstride/src/config.rs @@ -34,7 +34,7 @@ lazy_static! { kp_max: 500.0, kd_min: 0.0, kd_max: 5.0, - t_min: 0.0, + t_min: -14.0, t_max: 14.0, zero_on_init: false, can_timeout_command: 0x200b, // Unchecked @@ -52,7 +52,7 @@ lazy_static! { kp_max: 500.0, kd_min: 0.0, kd_max: 5.0, - t_min: 0.0, + t_min: -12.0, t_max: 12.0, zero_on_init: true, // Single encoder motor. can_timeout_command: 0x200c, @@ -70,7 +70,7 @@ lazy_static! { kp_max: 500.0, kd_min: 0.0, kd_max: 5.0, - t_min: 0.0, + t_min: -12.0, t_max: 12.0, zero_on_init: false, can_timeout_command: 0x200b, // Unchecked @@ -88,7 +88,7 @@ lazy_static! { kp_max: 5000.0, kd_min: 0.0, kd_max: 100.0, - t_min: 0.0, + t_min: -60.0, t_max: 60.0, zero_on_init: false, can_timeout_command: 0x200b, @@ -106,7 +106,7 @@ lazy_static! { kp_max: 5000.0, kd_min: 0.0, kd_max: 100.0, - t_min: 0.0, + t_min: -120.0, t_max: 120.0, zero_on_init: false, can_timeout_command: 0x200b, diff --git a/actuator/robstride/src/supervisor.rs b/actuator/robstride/src/supervisor.rs index 1cec2b0..403b22a 100644 --- a/actuator/robstride/src/supervisor.rs +++ b/actuator/robstride/src/supervisor.rs @@ -158,17 +158,17 @@ impl MotorsSupervisor { } } - // { - // // Send updated sdo parameters to motors that need them. - // let mut motors_to_set_sdo = motors_to_set_sdo.lock().unwrap(); - // if !motors_to_set_sdo.is_empty() { - // for (motor_id, params) in motors_to_set_sdo.iter_mut() { - // motors.set_torque_limit(*motor_id, params.torque_limit).unwrap(); - // // Any other sdo parameters can be updated here. - // } - // motors_to_set_sdo.clear(); - // } - // } + { + // Send updated sdo parameters to motors that need them. + let mut motors_to_set_sdo = motors_to_set_sdo.lock().unwrap(); + if !motors_to_set_sdo.is_empty() { + for (motor_id, params) in motors_to_set_sdo.iter_mut() { + motors.set_torque_limit(*motor_id, params.torque_limit).unwrap(); + // Any other sdo parameters can be updated here. + } + motors_to_set_sdo.clear(); + } + } { let params_copy = { From 65cca225b66e39d984ccfd6dae43e20b3d5f25ae Mon Sep 17 00:00:00 2001 From: WT-MM Date: Tue, 12 Nov 2024 07:41:26 +0800 Subject: [PATCH 4/4] make more common --- examples/supervisor.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/supervisor.py b/examples/supervisor.py index 3607012..a696d38 100644 --- a/examples/supervisor.py +++ b/examples/supervisor.py @@ -9,9 +9,9 @@ def main() -> None: parser = argparse.ArgumentParser() - parser.add_argument("--port-name", type=str, default="/dev/ttyUSB0") + parser.add_argument("--port-name", type=str, default="/dev/ttyCH341USB0") parser.add_argument("--motor-id", type=int, default=1) - parser.add_argument("--motor-type", type=str, default="01") + parser.add_argument("--motor-type", type=str, default="04") 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)