From c5315dbb7bc7907ec4605cd4d9ced45edc2c0e3d Mon Sep 17 00:00:00 2001
From: Wesley Maa <wesley.maa@gmail.com>
Date: Sat, 21 Dec 2024 17:36:15 -0800
Subject: [PATCH] moves but dies

---
 actuator/bindings/src/lib.rs | 22 +++++++++++-----------
 examples/supervisor.py       | 23 +++++++++++++++++++++--
 2 files changed, 32 insertions(+), 13 deletions(-)

diff --git a/actuator/bindings/src/lib.rs b/actuator/bindings/src/lib.rs
index cd3727f..dd4fcff 100644
--- a/actuator/bindings/src/lib.rs
+++ b/actuator/bindings/src/lib.rs
@@ -55,12 +55,12 @@ struct PyRobstrideActuatorCommand {
 #[pymethods]
 impl PyRobstrideActuatorCommand {
     #[new]
-    fn new(actuator_id: u32) -> Self {
+    fn new(actuator_id: u32, position: Option<f64>, velocity: Option<f64>, torque: Option<f64>) -> Self {
         Self {
             actuator_id,
-            position: None,
-            velocity: None,
-            torque: None,
+            position,
+            velocity,
+            torque,
         }
     }
 }
@@ -89,15 +89,15 @@ struct PyRobstrideConfigureRequest {
 #[pymethods]
 impl PyRobstrideConfigureRequest {
     #[new]
-    fn new(actuator_id: u32) -> Self {
+    fn new(actuator_id: u32, kp: Option<f64>, kd: Option<f64>, max_torque: Option<f64>, torque_enabled: Option<bool>, zero_position: Option<bool>, new_actuator_id: Option<u32>) -> Self {
         Self {
             actuator_id,
-            kp: None,
-            kd: None,
-            max_torque: None,
-            torque_enabled: None,
-            zero_position: None,
-            new_actuator_id: None,
+            kp,
+            kd,
+            max_torque,
+            torque_enabled,
+            zero_position,
+            new_actuator_id,
         }
     }
 }
diff --git a/examples/supervisor.py b/examples/supervisor.py
index df5c9ec..802fa9e 100644
--- a/examples/supervisor.py
+++ b/examples/supervisor.py
@@ -3,8 +3,9 @@
 import argparse
 import logging
 import time
+import math
 
-from actuator import RobstrideActuatorConfig, RobstrideSupervisor, RobstrideActuatorCommand
+from actuator import RobstrideActuatorConfig, RobstrideSupervisor, RobstrideActuatorCommand, RobstrideConfigureRequest
 
 def setup_logging() -> None:
     """Set up logging configuration."""
@@ -29,6 +30,11 @@ def main() -> None:
     parser.add_argument("--motor-type", type=int, default=2)
     parser.add_argument("--polling-interval", type=float, default=0.001)
     parser.add_argument("--verbose", action="store_true")
+    
+    parser.add_argument("--test-sinusoidal", action="store_true", help="Enable sinusoidal motion testing")
+    parser.add_argument("--sin-period", type=float, default=20.0, help="Period of sinusoidal motion in seconds")
+    parser.add_argument("--sin-amplitude", type=float, default=10.0, help="Amplitude of sinusoidal motion in degrees")
+    
     args = parser.parse_args()
 
     if args.verbose:
@@ -48,6 +54,12 @@ def main() -> None:
         logger.error(f"Failed to initialize supervisor: {e}")
         return
 
+    start_time = time.time()
+
+    if args.test_sinusoidal:
+        logger.info(f"Configuring motor {args.motor_id} for sinusoidal motion")
+        supervisor.configure_actuator(RobstrideConfigureRequest(args.motor_id, kp=5.0, kd=1.0, max_torque=5.0, torque_enabled=True))
+
     try:
         while True:
             states = supervisor.get_actuators_state([args.motor_id])
@@ -60,9 +72,16 @@ def main() -> None:
                         logger.info(f"  Velocity: {state.velocity:.2f}°/s")
                         logger.info(f"  Torque: {state.torque:.2f}")
                         logger.info(f"  Temperature: {state.temperature:.2f}°C")
+
+            if args.test_sinusoidal:
+                elapsed_time = time.time() - start_time
+                target_position = args.sin_amplitude * math.sin(2 * math.pi * elapsed_time / args.sin_period)
+                cmd = RobstrideActuatorCommand(args.motor_id, position=target_position_rad)
+                supervisor.command_actuators([cmd])
+                logger.info(f"  Target Position: {target_position:.2f}°")
             else:
                 logger.warning("No motor states received")
-            time.sleep(1)
+            time.sleep(0.1)
     except KeyboardInterrupt:
         logger.info("\nShutting down gracefully...")