Verify Python Bindings

actuator/__init__.py

@@ -1,8 +1,11 @@
 """Defines the top-level API for the actuator package."""
 
 from .bindings import (
-    PyRobstrideActuator as RobstrideActuator,
+    PyRobstrideActuatorCommand as RobstrideActuatorCommand,
     PyRobstrideActuatorConfig as RobstrideActuatorConfig,
+    PyRobstrideActuatorState as RobstrideActuatorState,
+    PyRobstrideConfigureRequest as RobstrideConfigureRequest,
+    PyRobstrideSupervisor as RobstrideSupervisor,
     get_version,
 )
 

actuator/bindings/src/lib.rs

@@ -19,6 +19,12 @@ impl From<eyre::Report> for ErrReportWrapper {
     }
 }
 
+impl From<PyErr> for ErrReportWrapper {
+    fn from(err: PyErr) -> Self {
+        ErrReportWrapper(err.into())
+    }
+}
+
 impl From<ErrReportWrapper> for PyErr {
     fn from(err: ErrReportWrapper) -> PyErr {
         PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(err.0.to_string())
@@ -49,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,
         }
     }
 }
@@ -83,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,
         }
     }
 }
@@ -141,14 +147,14 @@ impl PyRobstrideActuatorConfig {
 
 #[gen_stub_pyclass]
 #[pyclass]
-struct PyRobstrideActuator {
+struct PyRobstrideSupervisor {
     supervisor: Arc<Mutex<Supervisor>>,
     rt: Runtime,
 }
 
 #[gen_stub_pymethods]
 #[pymethods]
-impl PyRobstrideActuator {
+impl PyRobstrideSupervisor {
     #[new]
     fn new(
         ports: Vec<String>,
@@ -163,10 +169,17 @@ impl PyRobstrideActuator {
         let rt = Runtime::new().map_err(|e| ErrReportWrapper(e.into()))?;
 
         let supervisor = rt.block_on(async {
-            let mut supervisor =
-                Supervisor::new(Duration::from_secs(1)).map_err(|e| ErrReportWrapper(e))?;
+            let mut supervisor = Supervisor::new(Duration::from_secs(1))
+                .map_err(|e| ErrReportWrapper(e))?;
+            let mut found_motors = vec![false; actuators_config.len()];
 
+            // Add transports
             for port in &ports {
+                Python::with_gil(|py| {
+                    py.run_bound(&format!("print('Adding transport for port: {}')", port), None, None)?;
+                    Ok::<_, PyErr>(())
+                })?;
+
                 if port.starts_with("/dev/tty") {
                     let serial = CH341Transport::new(port.clone())
                         .await
@@ -188,23 +201,67 @@ impl PyRobstrideActuator {
                 }
             }
 
-            // Scan for motors
+            // Start supervisor runner
+            let mut supervisor_runner = supervisor.clone_controller();
+            Python::with_gil(|py| {
+                py.run_bound("print('Starting supervisor runner...')", None, None)?;
+                Ok::<_, PyErr>(())
+            })?;
+
+            tokio::spawn(async move {
+                if let Err(e) = supervisor_runner.run(Duration::from_secs_f64(polling_interval)).await {
+                    let _ = Python::with_gil(|py| {
+                        py.run_bound(&format!("print('ERROR: Supervisor task failed: {}')", e), None, None)
+                    });
+                }
+            });
+
+            // Scan for motors on each port
             for port in &ports {
+                Python::with_gil(|py| {
+                    py.run_bound(&format!("print('Scanning for motors on port: {}')", port), None, None)?;
+                    Ok::<_, PyErr>(())
+                })?;
+
                 let discovered_ids = supervisor
                     .scan_bus(0xFD, port, &actuators_config)
                     .await
                     .map_err(|e| ErrReportWrapper(e))?;
-                for (motor_id, _) in &actuators_config {
-                    if !discovered_ids.contains(motor_id) {
-                        tracing::warn!("Configured motor not found - ID: {}", motor_id);
+
+                Python::with_gil(|py| {
+                    py.run_bound(&format!("print('Discovered IDs on {}: {:?}')", port, discovered_ids), None, None)?;
+                    Ok::<_, PyErr>(())
+                })?;
+
+                for (idx, (motor_id, _)) in actuators_config.iter().enumerate() {
+                    if discovered_ids.contains(motor_id) {
+                        found_motors[idx] = true;
                     }
                 }
             }
 
+            // Log warnings for missing motors
+            for (idx, (motor_id, config)) in actuators_config.iter().enumerate() {
+                if !found_motors[idx] {
+                    Python::with_gil(|py| {
+                        py.run_bound(&format!("print('WARNING: Configured motor not found - ID: {}, Type: {:?}')", motor_id, config.actuator_type), None, None)?;
+                        Ok::<_, PyErr>(())
+                    })?;
+                } else {
+                    Python::with_gil(|py| {
+                        py.run_bound(&format!(
+                            "print('Found configured motor - ID: {}, Type: {:?}')",
+                            motor_id, config.actuator_type
+                        ), None, None)?;
+                        Ok::<_, PyErr>(())
+                    })?;
+                }
+            }
+
             Ok(supervisor)
         })?;
 
-        Ok(PyRobstrideActuator {
+        Ok(PyRobstrideSupervisor {
             supervisor: Arc::new(Mutex::new(supervisor)),
             rt,
         })
@@ -326,9 +383,9 @@ impl From<PyRobstrideActuatorConfig> for robstride::ActuatorConfiguration {
 }
 
 #[pymodule]
-fn robstride_bindings(m: &Bound<PyModule>) -> PyResult<()> {
+fn bindings(m: &Bound<PyModule>) -> PyResult<()> {
     m.add_function(wrap_pyfunction!(get_version, m)?)?;
-    m.add_class::<PyRobstrideActuator>()?;
+    m.add_class::<PyRobstrideSupervisor>()?;
     m.add_class::<PyRobstrideActuatorCommand>()?;
     m.add_class::<PyRobstrideConfigureRequest>()?;
     m.add_class::<PyRobstrideActuatorState>()?;

actuator/robstride/src/main.rs

@@ -56,7 +56,7 @@ async fn main() -> eyre::Result<()> {
     supervisor
         .add_actuator(
             Box::new(RobStride02::new(
-                1,
+                14,
                 0xFE,
                 supervisor.get_transport_tx("stub").await?,
             )),

examples/supervisor.py

@@ -1,36 +1,90 @@
 """Example of moving a motor using the supervisor."""
 
 import argparse
+import logging
 import time
+import math
 
-from actuator import RobstrideActuator, RobstrideActuatorConfig
+from actuator import RobstrideActuatorConfig, RobstrideSupervisor, RobstrideActuatorCommand, RobstrideConfigureRequest
 
+def setup_logging() -> None:
+    """Set up logging configuration."""
+    logging.basicConfig(
+        level=logging.INFO,
+        format='%(asctime)s - %(levelname)s - %(message)s',
+        datefmt='%Y-%m-%d %H:%M:%S'
+    )
 
 def main() -> None:
+    setup_logging()
+    logger = logging.getLogger(__name__)
+
     parser = argparse.ArgumentParser()
-    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="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)
+    parser.add_argument(
+        "--port-names",
+        type=str,
+        default="can0,can1",
+        help="Comma-separated list of port names"
+    )
+    parser.add_argument("--motor-id", type=int, default=23)
+    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()
 
-    _amplitude = args.amplitude
-    _period = args.period
+    if args.verbose:
+        logging.getLogger().setLevel(logging.DEBUG)
 
-    supervisor = RobstrideActuator(
-        ports=[args.port_name],
-        py_actuators_config=[(args.motor_id, RobstrideActuatorConfig(args.motor_type))],
-        polling_interval=args.sleep,
-    )
+    ports = [port.strip() for port in args.port_names.split(",")]
+    logger.info(f"Initializing supervisor with ports: {ports}")
+
+    try:
+        supervisor = RobstrideSupervisor(
+            ports=ports,
+            py_actuators_config=[(args.motor_id, RobstrideActuatorConfig(args.motor_type))],
+            polling_interval=args.polling_interval,
+        )
+        logger.info("Supervisor initialized successfully")
+    except Exception as e:
+        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])
+            if states:
+                for state in states:
+                    status = "online" if state.online else "offline"
+                    logger.info(f"Motor {state.actuator_id}: {status}")
+                    if state.online:
+                        logger.info(f"  Position: {state.position:.2f}°")
+                        logger.info(f"  Velocity: {state.velocity:.2f}°/s")
+                        logger.info(f"  Torque: {state.torque:.2f}")
+                        logger.info(f"  Temperature: {state.temperature:.2f}°C")
 
-    while True:
-        print(supervisor.get_actuators_state([args.motor_id]))
-        time.sleep(1)
+            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(0.1)
+    except KeyboardInterrupt:
+        logger.info("\nShutting down gracefully...")
 
 
 if __name__ == "__main__":
-    # python -m examples.supervisor
     main()