update zero command (#14)

kscalelabs · Oct 10, 2024 · 6afc131 · 6afc131
1 parent f66e724
commit 6afc131
Show file tree

Hide file tree

Showing 6 changed files with 69 additions and 46 deletions.
diff --git a/actuator/__init__.py b/actuator/__init__.py
@@ -1,3 +1,3 @@
-__version__ = "0.0.8"
+__version__ = "0.0.9"
 
 from .rust.bindings import PyRobstrideMotorFeedback as RobstrideMotorFeedback, PyRobstrideMotors as RobstrideMotors
diff --git a/actuator/rust/bindings/bindings.pyi b/actuator/rust/bindings/bindings.pyi
@@ -19,7 +19,7 @@ class PyRobstrideMotors:
     def send_get_mode(self) -> dict[int, str]:
         ...
 
-    def send_set_zero(self) -> dict[int, PyRobstrideMotorFeedback]:
+    def send_set_zero(self, motor_ids:typing.Optional[typing.Sequence[int]]) -> dict[int, PyRobstrideMotorFeedback]:
         ...
 
     def send_reset(self) -> dict[int, PyRobstrideMotorFeedback]:

diff --git a/actuator/rust/bindings/src/lib.rs b/actuator/rust/bindings/src/lib.rs
@@ -51,9 +51,12 @@ impl PyRobstrideMotors {
             .collect()
     }
 
-    fn send_set_zero(&mut self) -> PyResult<HashMap<u8, PyRobstrideMotorFeedback>> {
+    fn send_set_zero(
+        &mut self,
+        motor_ids: Option<Vec<u8>>,
+    ) -> PyResult<HashMap<u8, PyRobstrideMotorFeedback>> {
         self.inner
-            .send_set_zero()
+            .send_set_zero(motor_ids.as_deref())
             .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?
             .into_iter()
             .map(|(k, v)| Ok((k, v.into())))

diff --git a/actuator/rust/robstride/src/lib.rs b/actuator/rust/robstride/src/lib.rs
@@ -398,10 +398,22 @@ impl Motors {
         self.read_all_pending_responses()
     }
 
-    pub fn send_set_zero(&mut self) -> Result<HashMap<u8, MotorFeedback>, std::io::Error> {
-        let motor_ids = self.motor_configs.keys().cloned().collect::<Vec<u8>>();
+    pub fn send_set_zero(
+        &mut self,
+        motor_ids: Option<&[u8]>,
+    ) -> Result<HashMap<u8, MotorFeedback>, std::io::Error> {
+        let ids_to_zero = motor_ids
+            .map(|ids| ids.to_vec())
+            .unwrap_or_else(|| self.motor_configs.keys().cloned().collect());
+
+        for &id in &ids_to_zero {
+            if !self.motor_configs.contains_key(&id) {
+                return Err(std::io::Error::new(
+                    std::io::ErrorKind::InvalidInput,
+                    format!("Invalid motor ID: {}", id),
+                ));
+            }
 
-        for id in motor_ids {
             let pack = CanPack {
                 ex_id: ExId {
                     id,
@@ -416,7 +428,7 @@ impl Motors {
             self.send_command(&pack)?;
         }
 
-        // After setting the mode for all motors, sleep for a short time.
+        // After setting the zero for specified motors, sleep for a short time.
         std::thread::sleep(self.sleep_time);
 
         self.read_all_pending_responses()

diff --git a/actuator/rust/robstride/src/main.rs b/actuator/rust/robstride/src/main.rs
@@ -11,7 +11,6 @@ const MAX_TORQUE: f32 = 1.0;
 
 fn run_motion_test(motors: &mut Motors) -> Result<(), Box<dyn Error>> {
     motors.send_reset()?;
-    // motors.send_set_zero()?;
     motors.send_start()?;
 
     let start_time = Instant::now();

diff --git a/examples/sinusoidal_movement.py b/examples/sinusoidal_movement.py
@@ -14,54 +14,59 @@ def run_motion_test(
     motors: RobstrideMotors,
     period: float = 1.0,
     motor_id: int = 1,
-    max_torque: float = 1.0,
+    max_torque: float = 10.0,
     amplitude: float = math.pi / 2.0,
     run_time_s: float = 10.0,
+    kp: float = 1.0,
+    kd: float = 0.01,
 ) -> None:
     motors.send_reset()
-    motors.send_set_zero()
+    motors.send_set_zero([motor_id])
     motors.send_start()
 
     start_time = time.time()
     command_count = 0
 
-    # PD controller parameters
-    kp_04 = 0.5
-    kd_04 = 0.1
-
-    while time.time() - start_time < run_time_s:
-        elapsed_time = time.time() - start_time
-        desired_position = amplitude * math.cos(elapsed_time * math.pi * 2.0 / period + math.pi / 2.0)
-
-        try:
-            feedback = motors.get_latest_feedback_for(motor_id)
-            current_position = feedback.position
-            current_velocity = feedback.velocity
-            torque = max(
-                min(kp_04 * (desired_position - current_position) - kd_04 * current_velocity, max_torque), -max_torque
+    try:
+        while time.time() - start_time < run_time_s:
+            dt = time.time() - start_time
+
+            # Reference velocity is the derivative of the reference position.
+            p_ref = amplitude * math.cos(dt * math.pi * 2.0 / period + math.pi / 2.0)
+            v_ref = -amplitude * math.pi * 2.0 / period * math.sin(dt * math.pi * 2.0 / period + math.pi / 2.0)
+            # v_ref = 0.0
+
+            try:
+                feedback = motors.get_latest_feedback_for(motor_id)
+                p_cur = feedback.position
+                v_cur = feedback.velocity
+                torque = max(min(kp * (p_ref - p_cur) + kd * (v_ref - v_cur), max_torque), -max_torque)
+
+                motors.send_torque_controls({motor_id: torque})
+
+            except RuntimeError:
+                logger.exception("Runtime error while getting latest feedback")
+                motors.send_torque_controls({motor_id: 0.0})
+
+            command_count += 1
+            logger.info(
+                "Motor %d Commands: %d, Frequency: %.2f Hz, Desired position: %.2f Feedback: %s",
+                motor_id,
+                command_count,
+                command_count / dt,
+                p_ref,
+                feedback,
             )
 
-            motors.send_torque_controls({motor_id: torque})
-
-        except RuntimeError:
-            logger.exception("Runtime error while getting latest feedback")
-            motors.send_torque_controls({motor_id: 0.0})
+    except KeyboardInterrupt:
+        pass
 
-        command_count += 1
-        logger.info(
-            "Motor %d Commands: %d, Frequency: %.2f Hz, Desired position: %.2f Feedback: %s",
-            motor_id,
-            command_count,
-            command_count / elapsed_time,
-            desired_position,
-            feedback,
-        )
-
-    motors.send_torque_controls({motor_id: 0.0})
-    motors.send_reset()
+    finally:
+        motors.send_torque_controls({motor_id: 0.0})
+        motors.send_reset()
 
-    elapsed_time = time.time() - start_time
-    logger.info(f"Done. Average control frequency: {command_count / elapsed_time:.2f} Hz")
+    dt = time.time() - start_time
+    logger.info(f"Done. Average control frequency: {command_count / dt:.2f} Hz")
 
 
 def main() -> None:
@@ -70,10 +75,12 @@ def main() -> None:
     parser = argparse.ArgumentParser()
     parser.add_argument("--port-name", type=str, default="/dev/ttyUSB0")
     parser.add_argument("--motor-id", type=int, default=1)
-    parser.add_argument("--max-torque", type=float, default=1.0)
-    parser.add_argument("--amplitude", type=float, default=math.pi / 2.0)
+    parser.add_argument("--max-torque", type=float, default=10.0)
+    parser.add_argument("--amplitude", type=float, default=math.pi)
     parser.add_argument("--period", type=float, default=1.0)
     parser.add_argument("--run-time-s", type=float, default=10.0)
+    parser.add_argument("--kp", type=float, default=1.0)
+    parser.add_argument("--kd", type=float, default=0.01)
     args = parser.parse_args()
 
     motors = RobstrideMotors(
@@ -88,6 +95,8 @@ def main() -> None:
         max_torque=args.max_torque,
         amplitude=args.amplitude,
         run_time_s=args.run_time_s,
+        kp=args.kp,
+        kd=args.kd,
     )