From d52b528913520fdcd56f8d6d6d8f07e2e60f26f5 Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Sat, 21 Dec 2024 15:40:14 -0800 Subject: [PATCH 1/3] rename to supervisor --- actuator/__init__.py | 5 ++++- actuator/bindings/src/lib.rs | 8 ++++---- examples/supervisor.py | 4 ++-- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/actuator/__init__.py b/actuator/__init__.py index 4ce7b7b..38a1695 100644 --- a/actuator/__init__.py +++ b/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, ) diff --git a/actuator/bindings/src/lib.rs b/actuator/bindings/src/lib.rs index 164fa65..4c54a13 100644 --- a/actuator/bindings/src/lib.rs +++ b/actuator/bindings/src/lib.rs @@ -141,14 +141,14 @@ impl PyRobstrideActuatorConfig { #[gen_stub_pyclass] #[pyclass] -struct PyRobstrideActuator { +struct PyRobstrideSupervisor { supervisor: Arc>, rt: Runtime, } #[gen_stub_pymethods] #[pymethods] -impl PyRobstrideActuator { +impl PyRobstrideSupervisor { #[new] fn new( ports: Vec, @@ -204,7 +204,7 @@ impl PyRobstrideActuator { Ok(supervisor) })?; - Ok(PyRobstrideActuator { + Ok(PyRobstrideSupervisor { supervisor: Arc::new(Mutex::new(supervisor)), rt, }) @@ -328,7 +328,7 @@ impl From for robstride::ActuatorConfiguration { #[pymodule] fn robstride_bindings(m: &Bound) -> PyResult<()> { m.add_function(wrap_pyfunction!(get_version, m)?)?; - m.add_class::()?; + m.add_class::()?; m.add_class::()?; m.add_class::()?; m.add_class::()?; diff --git a/examples/supervisor.py b/examples/supervisor.py index 72d1526..da7efba 100644 --- a/examples/supervisor.py +++ b/examples/supervisor.py @@ -3,7 +3,7 @@ import argparse import time -from actuator import RobstrideActuator, RobstrideActuatorConfig +from actuator import RobstrideActuatorConfig, RobstrideSupervisor def main() -> None: @@ -20,7 +20,7 @@ def main() -> None: _amplitude = args.amplitude _period = args.period - supervisor = RobstrideActuator( + supervisor = RobstrideSupervisor( ports=[args.port_name], py_actuators_config=[(args.motor_id, RobstrideActuatorConfig(args.motor_type))], polling_interval=args.sleep, From 3e682be0d9ab950718e6f1478f7e1b02564c54f3 Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Sat, 21 Dec 2024 17:18:04 -0800 Subject: [PATCH 2/3] fix bindings + logging --- actuator/bindings/src/lib.rs | 71 ++++++++++++++++++++++++++++++---- actuator/robstride/src/main.rs | 2 +- examples/supervisor.py | 71 +++++++++++++++++++++++++--------- 3 files changed, 118 insertions(+), 26 deletions(-) diff --git a/actuator/bindings/src/lib.rs b/actuator/bindings/src/lib.rs index 4c54a13..cd3727f 100644 --- a/actuator/bindings/src/lib.rs +++ b/actuator/bindings/src/lib.rs @@ -19,6 +19,12 @@ impl From for ErrReportWrapper { } } +impl From for ErrReportWrapper { + fn from(err: PyErr) -> Self { + ErrReportWrapper(err.into()) + } +} + impl From for PyErr { fn from(err: ErrReportWrapper) -> PyErr { PyErr::new::(err.0.to_string()) @@ -163,10 +169,17 @@ impl PyRobstrideSupervisor { 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,19 +201,63 @@ impl PyRobstrideSupervisor { } } - // 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) })?; @@ -326,7 +383,7 @@ impl From for robstride::ActuatorConfiguration { } #[pymodule] -fn robstride_bindings(m: &Bound) -> PyResult<()> { +fn bindings(m: &Bound) -> PyResult<()> { m.add_function(wrap_pyfunction!(get_version, m)?)?; m.add_class::()?; m.add_class::()?; diff --git a/actuator/robstride/src/main.rs b/actuator/robstride/src/main.rs index b8e26bf..1e17c42 100644 --- a/actuator/robstride/src/main.rs +++ b/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?, )), diff --git a/examples/supervisor.py b/examples/supervisor.py index da7efba..df5c9ec 100644 --- a/examples/supervisor.py +++ b/examples/supervisor.py @@ -1,36 +1,71 @@ """Example of moving a motor using the supervisor.""" import argparse +import logging import time -from actuator import RobstrideActuatorConfig, RobstrideSupervisor +from actuator import RobstrideActuatorConfig, RobstrideSupervisor, RobstrideActuatorCommand +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") args = parser.parse_args() - _amplitude = args.amplitude - _period = args.period + if args.verbose: + logging.getLogger().setLevel(logging.DEBUG) - supervisor = RobstrideSupervisor( - 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 - while True: - print(supervisor.get_actuators_state([args.motor_id])) - time.sleep(1) + 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") + else: + logger.warning("No motor states received") + time.sleep(1) + except KeyboardInterrupt: + logger.info("\nShutting down gracefully...") if __name__ == "__main__": - # python -m examples.supervisor main() From c5315dbb7bc7907ec4605cd4d9ced45edc2c0e3d Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Sat, 21 Dec 2024 17:36:15 -0800 Subject: [PATCH 3/3] 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, velocity: Option, torque: Option) -> 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, kd: Option, max_torque: Option, torque_enabled: Option, zero_position: Option, new_actuator_id: Option) -> 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...")