Add torque limit (#48)

* torque limit draft

* compare

* fix idiocy

* make more common

actuator/bindings/src/lib.rs

@@ -345,6 +345,12 @@ impl PyRobstrideMotorsSupervisor {
             .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
     }
 
+    fn set_torque_limit(&self, motor_id: u8, torque_limit: f32) -> PyResult<f32> {
+        self.inner
+            .set_torque_limit(motor_id, torque_limit)
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
+    }
+
     fn add_motor_to_zero(&self, motor_id: u8) -> PyResult<()> {
         self.inner
             .add_motor_to_zero(motor_id)

actuator/robstride/src/motor.rs

@@ -33,6 +33,19 @@ impl Default for MotorControlParams {
     }
 }
 
+#[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,
@@ -343,6 +356,33 @@ 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_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(())
+    }
+
     pub fn send_reset(&mut self, id: u8) -> Result<CanPack, std::io::Error> {
         let pack = CanPack {
             ex_id: ExId {

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<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>>,
@@ -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<f32, std::io::Error> {
+        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<f32, std::io::Error> {
         let mut target_params = self.target_params.write().unwrap();
         if let Some(params) = target_params.get_mut(&motor_id) {

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)