functions to set the minimum and target update rates (#23)

* functions to set the minimum and target update rates

* update python bindings

actuator/rust/bindings/bindings.pyi

@@ -53,7 +53,7 @@ class PyRobstrideMotors:
 
 
 class PyRobstrideMotorsSupervisor:
-    def __new__(cls,port_name,motor_infos,verbose = ...): ...
+    def __new__(cls,port_name,motor_infos,verbose = ...,min_update_rate = ...,target_update_rate = ...): ...
     def set_position(self, motor_id:int, position:float) -> None:
         ...
 
@@ -69,9 +69,6 @@ class PyRobstrideMotorsSupervisor:
     def set_torque(self, motor_id:int, torque:float) -> None:
         ...
 
-    def set_sleep_duration(self, sleep_duration:float) -> None:
-        ...
-
     def add_motor_to_zero(self, motor_id:int) -> None:
         ...
 
@@ -99,4 +96,13 @@ class PyRobstrideMotorsSupervisor:
     def reset_command_counters(self) -> None:
         ...
 
+    def set_min_update_rate(self, rate:float) -> None:
+        ...
+
+    def set_target_update_rate(self, rate:float) -> None:
+        ...
+
+    def get_actual_update_rate(self) -> float:
+        ...
+
 

actuator/rust/bindings/src/lib.rs

@@ -8,7 +8,6 @@ use robstride::{
     MotorsSupervisor as RobstrideMotorsSupervisor,
 };
 use std::collections::HashMap;
-use std::time::Duration;
 
 #[gen_stub_pyclass]
 #[pyclass]
@@ -227,8 +226,14 @@ struct PyRobstrideMotorsSupervisor {
 #[pymethods]
 impl PyRobstrideMotorsSupervisor {
     #[new]
-    #[pyo3(signature = (port_name, motor_infos, verbose = false))]
-    fn new(port_name: String, motor_infos: HashMap<u8, String>, verbose: bool) -> PyResult<Self> {
+    #[pyo3(signature = (port_name, motor_infos, verbose = false, min_update_rate = 10.0, target_update_rate = 50.0))]
+    fn new(
+        port_name: String,
+        motor_infos: HashMap<u8, String>,
+        verbose: bool,
+        min_update_rate: f64,
+        target_update_rate: f64,
+    ) -> PyResult<Self> {
         let motor_infos = motor_infos
             .into_iter()
             .map(|(id, type_str)| {
@@ -237,8 +242,14 @@ impl PyRobstrideMotorsSupervisor {
             })
             .collect::<PyResult<HashMap<u8, RobstrideMotorType>>>()?;
 
-        let controller = RobstrideMotorsSupervisor::new(&port_name, &motor_infos, verbose)
-            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        let controller = RobstrideMotorsSupervisor::new(
+            &port_name,
+            &motor_infos,
+            verbose,
+            min_update_rate,
+            target_update_rate,
+        )
+        .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
 
         Ok(PyRobstrideMotorsSupervisor { inner: controller })
     }
@@ -268,12 +279,6 @@ impl PyRobstrideMotorsSupervisor {
         Ok(())
     }
 
-    fn set_sleep_duration(&self, sleep_duration: f32) -> PyResult<()> {
-        self.inner
-            .set_sleep_duration(Duration::from_millis(sleep_duration as u64));
-        Ok(())
-    }
-
     fn add_motor_to_zero(&self, motor_id: u8) -> PyResult<()> {
         self.inner.add_motor_to_zero(motor_id);
         Ok(())
@@ -331,6 +336,20 @@ impl PyRobstrideMotorsSupervisor {
         self.inner.reset_command_counters();
         Ok(())
     }
+
+    fn set_min_update_rate(&self, rate: f64) -> PyResult<()> {
+        self.inner.set_min_update_rate(rate);
+        Ok(())
+    }
+
+    fn set_target_update_rate(&self, rate: f64) -> PyResult<()> {
+        self.inner.set_target_update_rate(rate);
+        Ok(())
+    }
+
+    fn get_actual_update_rate(&self) -> PyResult<f64> {
+        Ok(self.inner.get_actual_update_rate())
+    }
 }
 
 #[pymodule]

actuator/rust/robstride/src/bin/supervisor.rs

@@ -8,6 +8,10 @@ use std::io::{self, Write};
 struct Args {
     #[arg(short, long, help = "Enable verbose output")]
     verbose: bool,
+    #[arg(short, long, help = "Minimum update rate (Hz)", default_value_t = 10.0)]
+    min_update_rate: f64,
+    #[arg(short, long, help = "Target update rate (Hz)", default_value_t = 50.0)]
+    target_update_rate: f64,
 }
 
 fn main() -> Result<(), Box<dyn std::error::Error>> {
@@ -42,7 +46,13 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
     };
     let motor_type = motor_type_from_str(motor_type_input.as_str())?;
     let motor_infos: HashMap<u8, MotorType> = HashMap::from([(test_id, motor_type)]);
-    let controller = MotorsSupervisor::new(&port_name, &motor_infos, args.verbose)?;
+    let controller = MotorsSupervisor::new(
+        &port_name,
+        &motor_infos,
+        args.verbose,
+        args.min_update_rate,
+        args.target_update_rate,
+    )?;
 
     println!("Motor Controller Test CLI");
     println!("Available commands:");

actuator/rust/robstride/src/lib.rs

@@ -898,18 +898,22 @@ pub struct MotorsSupervisor {
     running: Arc<Mutex<bool>>,
     latest_feedback: Arc<Mutex<HashMap<u8, MotorFeedback>>>,
     motors_to_zero: Arc<Mutex<HashSet<u8>>>,
-    sleep_duration: Arc<Mutex<Duration>>,
     paused: Arc<Mutex<bool>>,
     restart: Arc<Mutex<bool>>,
     total_commands: Arc<Mutex<u64>>,
     failed_commands: Arc<Mutex<u64>>,
+    min_update_rate: Arc<Mutex<f64>>,
+    target_update_rate: Arc<Mutex<f64>>,
+    actual_update_rate: Arc<Mutex<f64>>,
 }
 
 impl MotorsSupervisor {
     pub fn new(
         port_name: &str,
         motor_infos: &HashMap<u8, MotorType>,
         verbose: bool,
+        min_update_rate: f64,
+        target_update_rate: f64,
     ) -> Result<Self, Box<dyn std::error::Error>> {
         // Initialize Motors
         let motors = Motors::new(port_name, motor_infos, verbose)?;
@@ -953,11 +957,13 @@ impl MotorsSupervisor {
             running,
             latest_feedback: Arc::new(Mutex::new(HashMap::new())),
             motors_to_zero,
-            sleep_duration: Arc::new(Mutex::new(Duration::from_micros(10))),
             paused,
             restart,
             total_commands: Arc::new(Mutex::new(0)),
             failed_commands: Arc::new(Mutex::new(0)),
+            min_update_rate: Arc::new(Mutex::new(min_update_rate)),
+            target_update_rate: Arc::new(Mutex::new(target_update_rate)),
+            actual_update_rate: Arc::new(Mutex::new(0.0)),
         };
 
         controller.start_control_thread();
@@ -971,34 +977,56 @@ impl MotorsSupervisor {
         let running = Arc::clone(&self.running);
         let latest_feedback = Arc::clone(&self.latest_feedback);
         let motors_to_zero = Arc::clone(&self.motors_to_zero);
-        let sleep_duration = Arc::clone(&self.sleep_duration);
         let paused = Arc::clone(&self.paused);
         let restart = Arc::clone(&self.restart);
         let total_commands = Arc::clone(&self.total_commands);
         let failed_commands = Arc::clone(&self.failed_commands);
+        let min_update_rate = Arc::clone(&self.min_update_rate);
+        let target_update_rate = Arc::clone(&self.target_update_rate);
+        let actual_update_rate = Arc::clone(&self.actual_update_rate);
 
         thread::spawn(move || {
             let mut motors = motors.lock().unwrap();
 
             let _ = motors.send_reset();
             let _ = motors.send_start();
-            let _ = motors.send_can_timeout(100); // If motor doesn't receive a command for 100ms, it will stop.
 
-            while *running.lock().unwrap() {
-                // If paused, just wait 100ms without sending any commands.
-                if *paused.lock().unwrap() {
-                    std::thread::sleep(Duration::from_millis(100));
-                    continue;
+            // Set CAN timeout based on minimum update rate
+            let can_timeout = (1000.0 / *min_update_rate.lock().unwrap()) as u32;
+            let _ = motors.send_can_timeout(can_timeout);
+
+            let mut last_update_time = std::time::Instant::now();
+
+            loop {
+                {
+                    // If not running, break the loop.
+                    if !*running.lock().unwrap() {
+                        break;
+                    }
+                }
+
+                {
+                    // If paused, just wait a short time without sending any commands.
+                    if *paused.lock().unwrap() {
+                        std::thread::sleep(Duration::from_millis(10));
+                        continue;
+                    }
                 }
 
-                if *restart.lock().unwrap() {
-                    *restart.lock().unwrap() = false;
-                    let _ = motors.send_reset();
-                    let _ = motors.send_start();
+                {
+                    // If restart is requested, reset and restart the motors.
+                    let mut restart = restart.lock().unwrap();
+                    if *restart {
+                        *restart = false;
+                        let _ = motors.send_reset();
+                        let _ = motors.send_start();
+                    }
                 }
 
-                // Read latest feedback from motors.
+                let loop_start_time = std::time::Instant::now();
+
                 {
+                    // Read latest feedback from motors.
                     let latest_feedback_from_motors = motors.get_latest_feedback();
                     let mut latest_feedback = latest_feedback.lock().unwrap();
                     *latest_feedback = latest_feedback_from_motors.clone();
@@ -1034,8 +1062,18 @@ impl MotorsSupervisor {
                     *total_commands.lock().unwrap() += 1;
                 }
 
-                {
-                    std::thread::sleep(sleep_duration.lock().unwrap().clone());
+                // Calculate actual update rate
+                let elapsed = loop_start_time.duration_since(last_update_time);
+                last_update_time = loop_start_time;
+                let current_rate = 1.0 / elapsed.as_secs_f64();
+                *actual_update_rate.lock().unwrap() = current_rate;
+
+                // Sleep to maintain target update rate
+                let target_duration =
+                    Duration::from_secs_f64(1.0 / *target_update_rate.lock().unwrap());
+                let elapsed = loop_start_time.elapsed();
+                if elapsed < target_duration {
+                    std::thread::sleep(target_duration - elapsed);
                 }
             }
 
@@ -1109,11 +1147,6 @@ impl MotorsSupervisor {
         }
     }
 
-    pub fn set_sleep_duration(&self, sleep_duration: Duration) {
-        let mut sleep_duration_to_set = self.sleep_duration.lock().unwrap();
-        *sleep_duration_to_set = sleep_duration;
-    }
-
     pub fn add_motor_to_zero(&self, motor_id: u8) {
         // We need to set the motor parameters to zero to avoid the motor
         // rapidly changing to the new target after it is zeroed.
@@ -1146,6 +1179,23 @@ impl MotorsSupervisor {
         }
         std::thread::sleep(Duration::from_millis(200));
     }
+
+    pub fn set_min_update_rate(&self, rate: f64) {
+        let mut min_rate = self.min_update_rate.lock().unwrap();
+        *min_rate = rate;
+        let can_timeout = (1000.0 / rate) as u32;
+        let mut motors = self.motors.lock().unwrap();
+        let _ = motors.send_can_timeout(can_timeout);
+    }
+
+    pub fn set_target_update_rate(&self, rate: f64) {
+        let mut target_rate = self.target_update_rate.lock().unwrap();
+        *target_rate = rate;
+    }
+
+    pub fn get_actual_update_rate(&self) -> f64 {
+        *self.actual_update_rate.lock().unwrap()
+    }
 }
 
 impl Drop for MotorsSupervisor {