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..dd4fcff 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()) @@ -49,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, } } } @@ -83,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, } } } @@ -141,14 +147,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, @@ -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 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::()?; m.add_class::()?; 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 72d1526..802fa9e 100644 --- a/examples/supervisor.py +++ b/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()