From 652c1237feaef4579d6dbb83a632885ccd135558 Mon Sep 17 00:00:00 2001
From: Benjamin Bolte <ben@bolte.cc>
Date: Fri, 18 Oct 2024 09:18:03 +0000
Subject: [PATCH] zero on init

---
 actuator/rust/bindings/src/lib.rs                  | 13 ++++++++++---
 actuator/rust/robstride/src/bin/multisupervisor.rs | 11 +++++++++--
 actuator/rust/robstride/src/bin/supervisor.rs      | 11 +++++++++--
 actuator/rust/robstride/src/lib.rs                 |  3 ++-
 4 files changed, 30 insertions(+), 8 deletions(-)

diff --git a/actuator/rust/bindings/src/lib.rs b/actuator/rust/bindings/src/lib.rs
index a30f9df..698dc66 100644
--- a/actuator/rust/bindings/src/lib.rs
+++ b/actuator/rust/bindings/src/lib.rs
@@ -193,12 +193,13 @@ struct PyRobstrideMotorsSupervisor {
 #[pymethods]
 impl PyRobstrideMotorsSupervisor {
     #[new]
-    #[pyo3(signature = (port_name, motor_infos, verbose = false, target_update_rate = 50.0))]
+    #[pyo3(signature = (port_name, motor_infos, verbose = false, target_update_rate = 50.0, zero_on_init = false))]
     fn new(
         port_name: String,
         motor_infos: HashMap<u8, String>,
         verbose: bool,
         target_update_rate: f64,
+        zero_on_init: bool,
     ) -> PyResult<Self> {
         let motor_infos = motor_infos
             .into_iter()
@@ -209,8 +210,14 @@ impl PyRobstrideMotorsSupervisor {
             .collect::<PyResult<HashMap<u8, RobstrideMotorType>>>()?;
 
         let controller =
-            RobstrideMotorsSupervisor::new(&port_name, &motor_infos, verbose, target_update_rate)
-                .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+            RobstrideMotorsSupervisor::new(
+                &port_name,
+                &motor_infos,
+                verbose,
+                target_update_rate,
+                zero_on_init,
+            )
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
 
         Ok(PyRobstrideMotorsSupervisor { inner: controller })
     }
diff --git a/actuator/rust/robstride/src/bin/multisupervisor.rs b/actuator/rust/robstride/src/bin/multisupervisor.rs
index 5cce8f7..8a9fd2d 100644
--- a/actuator/rust/robstride/src/bin/multisupervisor.rs
+++ b/actuator/rust/robstride/src/bin/multisupervisor.rs
@@ -14,6 +14,8 @@ struct Args {
     min_update_rate: f64,
     #[arg(long, help = "Maximum update rate (Hz)", default_value_t = 1000.0)]
     max_update_rate: f64,
+    #[arg(long, help = "Zero on init", default_value_t = false)]
+    zero_on_init: bool,
 }
 
 fn sinusoid(
@@ -123,8 +125,13 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
         })
         .collect();
 
-    let controller =
-        MotorsSupervisor::new(&port_name, &motor_types, args.verbose, args.max_update_rate)?;
+    let controller = MotorsSupervisor::new(
+        &port_name,
+        &motor_types,
+        args.verbose,
+        args.max_update_rate,
+        args.zero_on_init,
+    )?;
 
     println!("Motor Controller Test CLI");
     println!("Available commands:");
diff --git a/actuator/rust/robstride/src/bin/supervisor.rs b/actuator/rust/robstride/src/bin/supervisor.rs
index 2aa6d94..96686ea 100644
--- a/actuator/rust/robstride/src/bin/supervisor.rs
+++ b/actuator/rust/robstride/src/bin/supervisor.rs
@@ -14,6 +14,8 @@ struct Args {
     min_update_rate: f64,
     #[arg(long, help = "Maximum update rate (Hz)", default_value_t = 1000.0)]
     max_update_rate: f64,
+    #[arg(long, help = "Zero on init", default_value_t = false)]
+    zero_on_init: bool,
 }
 
 fn sinusoid(
@@ -85,8 +87,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, args.max_update_rate)?;
+    let controller = MotorsSupervisor::new(
+        &port_name,
+        &motor_infos,
+        args.verbose,
+        args.max_update_rate,
+        args.zero_on_init,
+    )?;
 
     println!("Motor Controller Test CLI");
     println!("Available commands:");
diff --git a/actuator/rust/robstride/src/lib.rs b/actuator/rust/robstride/src/lib.rs
index b97b291..3309db3 100644
--- a/actuator/rust/robstride/src/lib.rs
+++ b/actuator/rust/robstride/src/lib.rs
@@ -899,6 +899,7 @@ impl MotorsSupervisor {
         motor_infos: &HashMap<u8, MotorType>,
         verbose: bool,
         max_update_rate: f64,
+        zero_on_init: bool,
     ) -> Result<Self, Box<dyn std::error::Error>> {
         // Initialize Motors
         let motors = Motors::new(port_name, motor_infos, verbose)?;
@@ -925,7 +926,7 @@ impl MotorsSupervisor {
         let zero_on_init_motors = motors
             .motor_configs
             .iter()
-            .filter(|(_, &config)| config.zero_on_init)
+            .filter(|(_, &config)| config.zero_on_init || zero_on_init)
             .map(|(&id, _)| id)
             .collect::<HashSet<u8>>();