torque limit draft

actuator/bindings/src/lib.rs

@@ -230,13 +230,14 @@ struct PyRobstrideMotorsSupervisor {
 #[pymethods]
 impl PyRobstrideMotorsSupervisor {
     #[new]
-    #[pyo3(signature = (port_name, motor_infos, verbose = false, target_update_rate = 50.0, zero_on_init = false))]
+    #[pyo3(signature = (port_name, motor_infos, verbose = false, target_update_rate = 50.0, zero_on_init = false, safe_mode = false))]
     fn new(
         port_name: String,
         motor_infos: HashMap<u8, String>,
         verbose: bool,
         target_update_rate: f64,
         zero_on_init: bool,
+        safe_mode: bool,
     ) -> PyResult<Self> {
         let motor_infos = motor_infos
             .into_iter()
@@ -252,6 +253,7 @@ impl PyRobstrideMotorsSupervisor {
             verbose,
             target_update_rate,
             zero_on_init,
+            safe_mode,
         )
         .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
 

actuator/robstride/src/bin/multisupervisor.rs

@@ -16,6 +16,8 @@ struct Args {
     max_update_rate: f64,
     #[arg(long, help = "Zero on init", default_value_t = false)]
     zero_on_init: bool,
+    #[arg(long, help = "Safe mode", default_value_t = false)]
+    safe_mode: bool,
 }
 
 fn sinusoid(
@@ -131,6 +133,7 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
         args.verbose,
         args.max_update_rate,
         args.zero_on_init,
+        args.safe_mode,
     )?;
 
     println!("Motor Controller Test CLI");

actuator/robstride/src/bin/supervisor.rs

@@ -16,6 +16,8 @@ struct Args {
     max_update_rate: f64,
     #[arg(long, help = "Zero on init", default_value_t = false)]
     zero_on_init: bool,
+    #[arg(long, help = "Safe mode", default_value_t = false)]
+    safe_mode: bool,
 }
 
 fn sinusoid(
@@ -93,6 +95,7 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
         args.verbose,
         args.max_update_rate,
         args.zero_on_init,
+        args.safe_mode,
     )?;
 
     println!("Motor Controller Test CLI");

actuator/robstride/src/lib.rs

@@ -439,6 +439,11 @@ pub fn motor_type_from_str(s: &str) -> Result<MotorType, std::io::Error> {
     }
 }
 
+#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
+pub struct MotorSdoParams {
+    pub torque_limit: f32,
+}
+
 #[derive(Debug, Clone, Copy, Serialize, Deserialize)]
 pub struct MotorControlParams {
     pub position: f32,
@@ -682,6 +687,31 @@ impl Motors {
         Ok(())
     }
 
+    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(())
+    }
+
     fn read_u16_param(&mut self, motor_id: u8, index: u16) -> Result<u16, std::io::Error> {
         let mut pack = CanPack {
             ex_id: ExId {
@@ -924,6 +954,7 @@ pub struct MotorsSupervisor {
     running: Arc<RwLock<bool>>,
     latest_feedback: Arc<RwLock<HashMap<u8, MotorFeedback>>>,
     motors_to_zero: Arc<Mutex<HashSet<u8>>>,
+    motors_to_set_sdo: Arc<Mutex<HashMap<u8, MotorSdoParams>>>,
     paused: Arc<RwLock<bool>>,
     restart: Arc<Mutex<bool>>,
     total_commands: Arc<RwLock<u64>>,
@@ -975,13 +1006,14 @@ impl MotorsSupervisor {
         let motor_ids: Vec<u8> = motor_infos.keys().cloned().collect();
         let total_commands = 0;
         let failed_commands = motor_ids.iter().map(|&id| (id, 0)).collect();
-g
+
         let controller = MotorsSupervisor {
             motors: Arc::new(Mutex::new(motors)),
             target_params: Arc::new(RwLock::new(target_params)),
             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)),
@@ -1011,6 +1043,7 @@ g
         let actual_update_rate = Arc::clone(&self.actual_update_rate);
         let serial = Arc::clone(&self.serial);
         let safe_mode = Arc::clone(&self.safe_mode);
+        let motors_to_set_sdo = Arc::clone(&self.motors_to_set_sdo);
 
         thread::spawn(move || {
             let mut motors = motors.lock().unwrap();
@@ -1070,6 +1103,17 @@ g
                     }
                 }
 
+                {
+                    // Send SDO commands to motors.
+                    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();
+                        }
+                        motors_to_set_sdo.clear();
+                    }
+                }
+
                 {
                     // Send PD commands to motors.
                     let target_params = target_params.read().unwrap();
@@ -1286,6 +1330,11 @@ g
             })
     }
 
+    pub fn set_torque_limit(&self, motor_id: u8, torque_limit: f32) -> Result<(), std::io::Error> {
+        let mut motors = self.motors.lock().unwrap();
+        motors.set_torque_limit(motor_id, torque_limit)
+    }
+
     pub fn add_motor_to_zero(&self, motor_id: u8) -> Result<(), std::io::Error> {
         // We need to set the motor parameters to zero to avoid the motor
         // rapidly changing to the new target after it is zeroed.