diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 17454db..b79d1eb 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -26,6 +26,19 @@ jobs: - name: Check formatting run: cargo fmt --all --check + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y \ + libx11-dev \ + libxrandr-dev \ + libxinerama-dev \ + libxcursor-dev \ + libxi-dev \ + libxext-dev \ + libgl1-mesa-dev \ + libasound2-dev + - name: Build run: cargo check --workspace --all-targets diff --git a/utilities/Cargo.toml b/utilities/Cargo.toml index 53b8c3f..73fdb72 100644 --- a/utilities/Cargo.toml +++ b/utilities/Cargo.toml @@ -7,6 +7,7 @@ edition = "2024" serde = { workspace = true } eframe = { workspace = true } egui_plot = { workspace = true } -nexosim = "1.0.0" +nexosim = { version = "1.0.0", features = ["server"]} rand_xoshiro = { version = "0.7.0", features = ["serde"] } rand_distr = "0.5.1" +raylib = "6.0.0" \ No newline at end of file diff --git a/utilities/examples/pendulum/client.py b/utilities/examples/pendulum/client.py new file mode 100644 index 0000000..21870b2 --- /dev/null +++ b/utilities/examples/pendulum/client.py @@ -0,0 +1,70 @@ +import time +import timeit +import matplotlib.pyplot as plt + +from nexosim import Simulation +from nexosim.time import Duration + +# Plot configuration +plt.ion() +fig, ax = plt.subplots(figsize=(10, 5)) + +times = [] +positions = [] +current_time_s = 0.0 + +(line,) = ax.plot([], [], "b-", label="Position") +ax.set_title("Real-time Pendulum Position Plot") +ax.set_xlabel("Simulation Time [s]") +ax.set_ylabel("Position [rad]") +ax.grid(True) +ax.legend() + + +def step_and_pos(sim, step_duration): + global current_time_s + + # Simulation step + sim.step_until(step_duration) + + # Update current time + step_in_seconds = step_duration.nanos / 1e9 + step_duration.secs + current_time_s += step_in_seconds + + # Read position events + pos_events = sim.try_read_events("position") + + # If there are position events, take the latest one and update the plot + if pos_events: + latest_pos = pos_events[-1] + print(f"t={current_time_s:.4f}s | pos={latest_pos}") + + times.append(current_time_s) + positions.append(latest_pos) + + line.set_xdata(times) + line.set_ydata(positions) + ax.relim() + ax.autoscale_view() + fig.canvas.draw() + fig.canvas.flush_events() + + +with Simulation("0.0.0.0:41633") as sim: + sim.build() + sim.init() + + print("Initial position events:", sim.try_read_events("position")) + + duration = Duration.create(microseconds=16) + + try: + while True: + # Measure the time taken for a simulation step and adjust the duration accordingly + d = timeit.timeit(lambda: step_and_pos(sim, duration), number=1) + duration = Duration.create(nanoseconds=int(d * 1e9)) + + except KeyboardInterrupt: + plt.ioff() + plt.show() + diff --git a/utilities/examples/pendulum/client_QT.py b/utilities/examples/pendulum/client_QT.py new file mode 100644 index 0000000..d28fda3 --- /dev/null +++ b/utilities/examples/pendulum/client_QT.py @@ -0,0 +1,161 @@ +import sys +import time +import timeit +from PyQt6.QtCore import Qt, QTimer +from PyQt6.QtWidgets import ( + QApplication, + QMainWindow, + QPushButton, + QVBoxLayout, + QWidget, +) +import pyqtgraph as pg + +from nexosim import Simulation +from nexosim.time import Duration + + +class SimulationPlotter(QMainWindow): + def __init__(self, sim): + super().__init__() + self.sim = sim + self.current_time_s = 0.0 + + # Define the simulation step duration (16 ms) + self.step_dt_s = 0.016 + self.step_duration = Duration.create(microseconds=int(self.step_dt_s * 1e6)) + self.last_real_time = None + + # Array to store time and position data for plotting + self.times = [] + self.positions = [] + + # Qt window setup + self.setWindowTitle("Real-time Pendulum Position Plot") + self.resize(800, 550) + + # Main widget and layout + central_widget = QWidget() + self.setCentralWidget(central_widget) + layout = QVBoxLayout(central_widget) + + # Start/Pause button + self.is_running = True + self.btn_toggle = QPushButton("Pause Simulation (Space)") + self.btn_toggle.setStyleSheet( + "font-size: 14px; padding: 8px; font-weight: bold; background-color: #f44336; color: white;" + ) + self.btn_toggle.clicked.connect(self.toggle_simulation) + self.btn_toggle.setFocusPolicy(Qt.FocusPolicy.NoFocus) + layout.addWidget(self.btn_toggle) + + # Plot widget setup + pg.setConfigOption("background", "w") + pg.setConfigOption("foreground", "k") + + self.plot_widget = pg.PlotWidget() + layout.addWidget(self.plot_widget) + + self.plot_widget.setTitle("Real-time Pendulum Position Plot", size="14pt") + self.plot_widget.setLabel("bottom", "Simulation Time [s]") + self.plot_widget.setLabel("left", "Position [rad]") + self.plot_widget.showGrid(x=True, y=True) + + self.curve = self.plot_widget.plot(pen=pg.mkPen("b", width=2)) + + # QTimer setup for periodic simulation stepping + self.timer = QTimer() + self.timer.timeout.connect(self.step_and_pos) + # Start the timer with the defined step duration in milliseconds + self.timer.start(int(self.step_dt_s * 1000)) + + def keyPressEvent(self, event): + # Handle key press events for controlling the simulation. + # Space: Toggle simulation start/pause. + # Up Arrow: Increase setpoint by 10.0. + # Down Arrow: Decrease setpoint by 10.0. + if event.key() == Qt.Key.Key_Space: + self.toggle_simulation() + elif event.key() == Qt.Key.Key_Up: + current_setpoint = self.sim.try_read_events("setpoint") + new_setpoint = ( + current_setpoint[-1] + 10.0 + if current_setpoint + else 10.0 + ) + self.sim.process_event("setpoint", new_setpoint) + elif event.key() == Qt.Key.Key_Down: + current_setpoint = self.sim.try_read_events("setpoint") + new_setpoint = ( + current_setpoint[-1] - 10.0 + if current_setpoint + else -10.0 + ) + self.sim.process_event("setpoint", new_setpoint) + else: + super().keyPressEvent(event) + + def toggle_simulation(self): + # Toggle the simulation state between running and paused. + if self.is_running: + self.timer.stop() + self.last_real_time = None + self.btn_toggle.setText("Start Simulation (Space)") + self.btn_toggle.setStyleSheet( + "font-size: 14px; padding: 8px; font-weight: bold; background-color: #4CAF50; color: white;" + ) + self.is_running = False + else: + self.timer.start(int(self.step_dt_s * 1000)) + self.btn_toggle.setText("Pause Simulation (Space)") + self.btn_toggle.setStyleSheet( + "font-size: 14px; padding: 8px; font-weight: bold; background-color: #f44336; color: white;" + ) + self.is_running = True + + def step_and_pos(self): + now = time.perf_counter() + + # If this is the first call, initialize last_real_time and return without stepping the simulation. + if self.last_real_time is None: + self.last_real_time = now + return + + # Calculate the elapsed real time since the last simulation step + real_elapsed = now - self.last_real_time + self.last_real_time = now + + # Step the simulation by the elapsed real time. + step_duration = Duration.create(microseconds=int(real_elapsed * 1e6)) + self.sim.step_until(step_duration) + + # Update the current simulation time based on the real elapsed time. + self.current_time_s += real_elapsed + + # Read events. + pos_events = self.sim.try_read_events("position") + # If there are position events, take the latest one and update the plot. + if pos_events: + latest_pos = pos_events[-1] + self.times.append(self.current_time_s) + self.positions.append(latest_pos) + self.curve.setData(self.times, self.positions) + + +def main(): + app = QApplication(sys.argv) + + with Simulation("0.0.0.0:41633") as sim: + sim.build() + sim.init() + + print("Initial position events:", sim.try_read_events("position")) + + window = SimulationPlotter(sim) + window.show() + + sys.exit(app.exec()) + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/utilities/examples/pendulum/main.rs b/utilities/examples/pendulum/main.rs new file mode 100644 index 0000000..6ed7629 --- /dev/null +++ b/utilities/examples/pendulum/main.rs @@ -0,0 +1,437 @@ +// Running example starts a simulation server. +// Simulation is controlled by a python client so it will start after client.py or client_QT.py is also run. +// To run the example, first run the server with `cargo run --example pendulum`, then run the client with `python3 client.py` or `python3 client_QT.py` in a separate terminal. +// After that the simulation will start and all visualizations will be displayed. +// The program also sends position over UDP to localhost:8080, which can be received using udp_client.py, that can be run with `python3 udp_client.py` in third terminal. +// To setup python environment use following commands: +// `cd /utilities/examples/pendulum` +// `python3 -m venv venv` +// `source venv/bin/activate` +// `pip3 install -r requirements.txt` + +use std::error::Error; +use std::net::UdpSocket; +use std::time::Duration; + +use serde::{Deserialize, Serialize}; + +use nexosim::model::{Context, Model, schedulable}; +use nexosim::ports::{ + EventSinkReader, EventSlotReader, EventSource, Output, SinkState, UniRequestor, + event_queue_endpoint, event_slot, +}; +use nexosim::server; +use nexosim::simulation::{Mailbox, SimInit, SimulationError}; +use nexosim::time::MonotonicTime; + +use raylib::prelude::*; +use std::sync::mpsc; +use utilities::angles::{Degrees, Radians}; +use utilities::pid::PidController; + +use std::f64::consts::PI; + +/// Pendulum model +#[derive(Serialize, Deserialize)] +pub struct Pendulum { + /// Position [rad] -- output port. + pub position: Output, + /// velocity [rad/s] -- output port. + pub velocity: Output, + /// acceleration [rad/s^2] -- output port. + pub acceleration: Output, + /// Energy [J] -- output port. + pub energy: Output, + /// Gravitational acceleration [m/s^2] -- requestor port. + pub gravity: UniRequestor<(), f64>, + + /// Position [rad] -- internal state. + pos: Radians, + /// Previous velocity [rad/s] -- internal state. + prev_vel: f64, + /// Previous acceleration [rad/s^2] -- internal state. + prev_acc: f64, + /// Time of last position update -- internal state. + last_position_update: MonotonicTime, + /// Mass on the end of pendulum [kg] -- constant. + mass: f64, + /// Length of the pendulum -- constant. + length: f64, +} + +#[Model] +impl Pendulum { + /// Creates a new pendulum model. + pub fn new( + initial_position: Radians, + mass: f64, + length: f64, + gravity_requestor: UniRequestor<(), f64>, + ) -> Self { + assert!(mass > 0.0); + assert!(length > 0.0); + Self { + position: Default::default(), + velocity: Default::default(), + acceleration: Default::default(), + energy: Default::default(), + gravity: gravity_requestor, + pos: initial_position.normalize_two_pi(), + prev_vel: 0.0, + prev_acc: 0.0, + last_position_update: MonotonicTime::EPOCH, + mass, + length, + } + } + + /// Broadcasts the initial position of the pendulum. + #[nexosim(init)] + async fn init(&mut self, cx: &Context) { + self.position.send(self.pos.to_degrees().value()).await; + self.velocity.send(self.prev_vel).await; + self.acceleration.send(self.prev_acc).await; + self.last_position_update = cx.time(); + } + + /// Torque applied at the center of rotation [Nm]. + /// + /// Calculates pendulum position. Assumes 'elapsed_time' is small. + /// Uses semi-implicit Euler method for integration to conserve energy. + pub async fn torque_in(&mut self, torque: f64, cx: &Context) { + // Request gravitational acceleration value. + let g = self.gravity.send(()).await; + // Calculate time since last update. + let now = cx.time(); + let elapsed_time = now.duration_since(self.last_position_update).as_secs_f64(); + // Current velocity calculated based on acceleration during last period. + let vel = self.prev_vel + self.prev_acc * elapsed_time; + // Integration of velocity over time to get position change. + self.pos += Radians::new(vel * elapsed_time); + // Convert position to [0; 2.0*PI] range. + self.pos = self.pos.normalize_two_pi(); + // Saves velocity for next iteration. + self.prev_vel = vel; + // Saves time for next iteration. + self.last_position_update = now; + // Calculates current acceleration to use at next iteration. + let gravity_torque = self.mass * g * self.pos.sin() * self.length; + let rotational_inertia = self.mass * self.length * self.length; + let mut acceleration = (torque - gravity_torque) / rotational_inertia; + // Adds friction. + let normal_force = self.mass * g * self.pos.cos(); + let centrifugal_force = self.mass * self.prev_vel * self.prev_vel * self.length; + let bearing_radius = 0.02; + let friction_coefficient = 0.1; // for plain bearing + let friction_torque = + (normal_force + centrifugal_force) * friction_coefficient * bearing_radius; + acceleration += -self.prev_vel.signum() * friction_torque / rotational_inertia; + self.prev_acc = acceleration; + + // Sends position. + self.position.send(self.pos.to_degrees().value()).await; + // Sends velocity. + self.velocity.send(self.prev_vel).await; + // Sends acceleration. + self.acceleration.send(self.prev_acc).await; + // Calculates and sends energy. + let kinetic_energy = + 0.5 * self.mass * self.prev_vel * self.prev_vel * self.length * self.length; + let potential_energy = self.mass * g * self.length * (1.0 - self.pos.cos()); + self.energy.send(kinetic_energy + potential_energy).await; + } +} + +/// Controller utilizing PID algorithm. +#[derive(Serialize, Deserialize)] +pub struct Controller { + /// Torque applied to the pendulum -- output port. + pub torque_out: Output, + /// Setpoint for regulator [degrees] -- output port. + pub setpoint_out: Output, + + /// Position of the pendulum [rad] -- internal state. + pos: Radians, + /// Setpoint for regulator [rad] -- internal state. + setpoint: Radians, + /// Period of the control loop [s] -- constant. + period: f64, + /// Implementation of PID controller. + pid: PidController, +} + +#[Model] +impl Controller { + /// Creates a new servo controller. + pub fn new( + period: f64, + proportional_gain: f64, + integral_gain: f64, + derivative_gain: f64, + initial_setpoint: Degrees, + ) -> Self { + assert!(period > 0.0); + let pid = PidController::new(proportional_gain, integral_gain, derivative_gain); + + Self { + torque_out: Default::default(), + setpoint_out: Default::default(), + pos: Radians::new(0.0), + setpoint: initial_setpoint.to_radians(), + period, + pid, + } + } + + #[nexosim(init)] + async fn init(&mut self, cx: &Context) { + // Broadcast initial setpoint + self.setpoint_out + .send(self.setpoint.to_degrees().value()) + .await; + // Schedule logic iterations + let period = Duration::from_secs_f64(self.period); + cx.schedule_periodic_event( + period, + period, + schedulable!(Self::update_controller_logic), + (), + ) + .unwrap(); + } + + /// Sets the position. + pub async fn position_in(&mut self, position: f64) { + // Convert position to [0; 2.0*PI] range. + self.pos = Degrees::new(position).to_radians().normalize_two_pi(); + } + + /// Sets the setpoint + pub async fn setpoint_in(&mut self, angle: f64) { + self.setpoint = Degrees::new(angle).to_radians().normalize_two_pi(); + self.setpoint_out + .send(self.setpoint.to_degrees().value()) + .await; + } + + /// Sends torque value. + #[nexosim(schedulable)] + async fn update_controller_logic(&mut self, _: ()) { + // Normalized error. + let mut error = self.setpoint - self.pos; + let alt_error = -error.value().signum() * (Radians::new(2.0 * PI) - error.abs()); + if alt_error.abs() < error.abs() { + error = alt_error; + } + // Calculating control value using PID regulator. + let torque = self.pid.update(error.value(), self.period); + // Sending torque value. + self.torque_out.send(torque).await; + } +} + +/// Model of environment in which the pendulum is located. +#[derive(Serialize, Deserialize)] +pub struct Environment; + +#[Model] +impl Environment { + /// Returns the value of gravitational acceleration. + pub async fn gravity(&mut self) -> f64 { + 9.80665 + } +} + +/// Bench function for the simulation server. Takes a channel to send event readers to allow access to output ports from within Rust. +fn pendulum_bench( + viz_tx: mpsc::Sender>, +) -> impl Fn(Option) -> Result> { + move |_cfg| { + // Parameters + let period = 0.01; + let proportional_gain = 30.0; + let integral_gain = 15.0; + let derivative_gain = 10.0; + let initial_setpoint = Degrees::new(90.0); + + // Models + let environment = Environment; + let environment_mbox = Mailbox::new(); + let gravity_requestor = UniRequestor::new(Environment::gravity, &environment_mbox); + let mut pendulum = Pendulum::new(Radians::new(PI / 2.0), 1.0, 1.0, gravity_requestor); + let pendulum_mbox = Mailbox::new(); + let mut controller = Controller::new( + period, + proportional_gain, + integral_gain, + derivative_gain, + initial_setpoint, + ); + let controller_mbox = Mailbox::new(); + + // Connections + pendulum + .position + .connect(Controller::position_in, &controller_mbox); + controller + .torque_out + .connect(Pendulum::torque_in, &pendulum_mbox); + + let mut bench = SimInit::new(); + + // Connections to allow access to output ports from within Rust. + let (position_sink, position_reader) = event_slot(SinkState::Enabled); + pendulum.position.connect_sink(position_sink); + viz_tx.send(position_reader).unwrap(); + + let (position_sink, position_reader) = event_slot(SinkState::Enabled); + pendulum.position.connect_sink(position_sink); + viz_tx.send(position_reader).unwrap(); + + let (setpoint_sink, setpoint_reader) = event_slot(SinkState::Enabled); + controller.setpoint_out.connect_sink(setpoint_sink); + viz_tx.send(setpoint_reader).unwrap(); + + let (torque_sink, torque_reader) = event_slot(SinkState::Enabled); + controller.torque_out.connect_sink(torque_sink); + viz_tx.send(torque_reader).unwrap(); + + let (velocity_sink, velocity_reader) = event_slot(SinkState::Enabled); + pendulum.velocity.connect_sink(velocity_sink); + viz_tx.send(velocity_reader).unwrap(); + + let (acceleration_sink, acceleration_reader) = event_slot(SinkState::Enabled); + pendulum.acceleration.connect_sink(acceleration_sink); + viz_tx.send(acceleration_reader).unwrap(); + + let (energy_sink, energy_reader) = event_slot(SinkState::Enabled); + pendulum.energy.connect_sink(energy_sink); + viz_tx.send(energy_reader).unwrap(); + + // Connections to allow access to output ports from Python client. + let position = event_queue_endpoint(&mut bench, SinkState::Enabled, "position").unwrap(); + pendulum.position.connect_sink(position); + + let setpoint = event_queue_endpoint(&mut bench, SinkState::Enabled, "setpoint").unwrap(); + controller.setpoint_out.connect_sink(setpoint); + + // Connection to allow sending setpoint events from Python client. + let _ = EventSource::new() + .connect(Controller::setpoint_in, &controller_mbox) + .bind_endpoint(&mut bench, "setpoint"); + + // Adding models to the simulation. + let sim = bench + .add_model(pendulum, pendulum_mbox, "pendulum") + .add_model(controller, controller_mbox, "controller") + .add_model(environment, environment_mbox, "environment"); + Ok(sim) + } +} + +fn main() -> Result<(), SimulationError> { + // Channel to send event readers. + let (viz_tx, viz_rx) = mpsc::channel(); + + // Server on a background thread (it blocks). + std::thread::spawn(move || { + server::run(pendulum_bench(viz_tx), "0.0.0.0:41633".parse().unwrap()).unwrap() + }); + + // Code below is executed after the client connects to the server and the simulation is initialized. + + // Receives event readers from the server thread in order of sending. + let mut position_for_thread = viz_rx.recv().unwrap(); + let mut position = viz_rx.recv().unwrap(); + let mut setpoint = viz_rx.recv().unwrap(); + let mut torque = viz_rx.recv().unwrap(); + let mut velocity = viz_rx.recv().unwrap(); + let mut acceleration = viz_rx.recv().unwrap(); + let mut energy = viz_rx.recv().unwrap(); + + // Thread sending position over UDP. + std::thread::spawn(move || { + let socket = UdpSocket::bind("0.0.0.0:0").unwrap(); + loop { + let position = position_for_thread.read(); + if let Some(position) = position { + let bytes = position.to_be_bytes(); + let _ = socket.send_to(&bytes, "127.0.0.1:8080"); + }; + } + }); + + // Visualization using raylib with debug information. + let (mut rl, thread) = raylib::init() + .size(640 * 2, 480 * 2) + .title("Pendulum") + .build(); + let x0 = 640; + let y0 = 480; + let len = 400.0; + let mut angle = Degrees::new(0.0); + let mut setpoint_value = Degrees::new(0.0); + let mut torque_value = 0.0; + let mut velocity_value = 0.0; + let mut acceleration_value = 0.0; + let mut energy_value = 0.0; + + while !rl.window_should_close() { + angle = Degrees::new(position.try_read().unwrap_or(angle.value())); + setpoint_value = Degrees::new(setpoint.try_read().unwrap_or(setpoint_value.value())); + torque_value = torque.try_read().unwrap_or(torque_value); + velocity_value = velocity.try_read().unwrap_or(velocity_value); + acceleration_value = acceleration.try_read().unwrap_or(acceleration_value); + energy_value = energy.try_read().unwrap_or(energy_value); + let mut d = rl.begin_drawing(&thread); + d.clear_background(Color::RAYWHITE); + d.draw_text( + &format!("Angle: {:.2} degrees", angle.value()), + 10, + 10, + 20, + Color::BLACK, + ); + d.draw_text( + &format!("Setpoint: {:.2} degrees", setpoint_value.value()), + 10, + 40, + 20, + Color::BLACK, + ); + d.draw_text( + &format!("Torque: {:.2} Nm", torque_value), + 10, + 70, + 20, + Color::BLACK, + ); + d.draw_text( + &format!("Velocity: {:.2} rad/s", velocity_value), + 10, + 100, + 20, + Color::BLACK, + ); + d.draw_text( + &format!("Acceleration: {:.2} rad/s²", acceleration_value), + 10, + 130, + 20, + Color::BLACK, + ); + d.draw_text( + &format!("Energy: {:.2} J", energy_value), + 10, + 160, + 20, + Color::BLACK, + ); + let x = x0 + (angle.sin() * len).round() as i32; + let y = y0 + (angle.cos() * len).round() as i32; + d.draw_line(x0, y0, x, y, Color::BLACK); + d.draw_circle(x, y, 20.0, Color::GOLD); + } + + Ok(()) +} diff --git a/utilities/examples/pendulum/requirements.txt b/utilities/examples/pendulum/requirements.txt new file mode 100644 index 0000000..d01cf80 --- /dev/null +++ b/utilities/examples/pendulum/requirements.txt @@ -0,0 +1,23 @@ +attrs==26.1.0 +cattrs==26.1.0 +cbor2==6.1.3 +colorama==0.4.6 +contourpy==1.3.3 +cycler==0.12.1 +fonttools==4.63.0 +grpcio==1.82.1 +kiwisolver==1.5.0 +matplotlib==3.11.1 +NeXosim-py==0.2.1 +numpy==2.5.1 +packaging==26.2 +pillow==12.3.0 +protobuf==5.29.6 +pyparsing==3.3.2 +PyQt6==6.11.0 +PyQt6-Qt6==6.11.1 +PyQt6_sip==13.11.1 +pyqtgraph==0.14.0 +python-dateutil==2.9.0.post0 +six==1.17.0 +typing_extensions==4.16.0 diff --git a/utilities/examples/pendulum/udp_client.py b/utilities/examples/pendulum/udp_client.py new file mode 100644 index 0000000..ca8e5fe --- /dev/null +++ b/utilities/examples/pendulum/udp_client.py @@ -0,0 +1,12 @@ +import socket +import struct + +sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +sock.bind(("127.0.0.1", 8080)) + +while True: + data, addr = sock.recvfrom(8) + if len(data) == 8: + # '>d' means Big-Endian 64-bit float + (val,) = struct.unpack(">d", data) + print(f"Received float: {val}") \ No newline at end of file