From 3e67e69efe7bfdf3ba6792b2fd8374c1a182b0df Mon Sep 17 00:00:00 2001 From: Yang Zhou Date: Sat, 7 Sep 2024 21:56:05 -0400 Subject: [PATCH] implement use_multithreading_depth_rendering, optimize rng fix docs --- Cargo.toml | 7 +++- README.md | 4 +- config/quad.yaml | 3 +- src/config.rs | 2 + src/lib.rs | 105 ++++++++++++++++++++++++++++++++--------------- src/main.rs | 25 ++++++----- 6 files changed, 96 insertions(+), 50 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index ba9a1832..a419bba2 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,7 @@ [package] +exclude = ["assets/", "CONTRIBUTING.md", "CODE_OF_CONDUCT.md", "SECURITY.md"] name = "peng_quad" -version = "0.5.0" +version = "0.5.1" edition = "2021" rust-version = "1.76" authors = ["Yang Zhou "] @@ -27,7 +28,7 @@ codegen-units = 1 # Compile the entire crate as one unit. lto = "thin" # Do a second optimization pass over the entire program, including dependencies. [dependencies] nalgebra = "0.33.0" -rand = "0.8.5" +rand = { version = "0.8.5", features = ["rand_chacha"] } rand_distr = "0.4.3" rerun = "0.18.2" thiserror = "1.0.63" @@ -35,3 +36,5 @@ serde = { version = "1.0.209", features = ["derive"] } serde_yaml = "0.9.34" env_logger = "0.11.5" log = "0.4.22" +rayon = "1.10.0" +rand_chacha = "0.3.1" diff --git a/README.md b/README.md index 2fade3cf..4e6a427d 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# [![Peng](assets/Peng.svg)](https://github.com/makeecat/Peng) +# [![Peng](https://raw.githubusercontent.com/makeecat/Peng/main/assets/Peng.svg)](https://github.com/makeecat/Peng) [![License](https://img.shields.io/badge/license-MIT%2FApache-blue.svg)](https://github.com/makeecat/Peng#license) [![Crates.io](https://img.shields.io/crates/v/peng_quad.svg)](https://crates.io/crates/peng_quad) @@ -11,7 +11,7 @@ ## What is Peng Peng is a minimal quadrotor autonomy framework in Rust. It includes a simulator, controller, and planner, providing a basic framework for simulating quadrotor dynamics and control. -![demo](assets/Peng_demo.gif) +![demo](https://raw.githubusercontent.com/makeecat/Peng/main/assets/Peng_demo.gif) ## Getting Started diff --git a/config/quad.yaml b/config/quad.yaml index e1c4bfc3..56b41ae5 100644 --- a/config/quad.yaml +++ b/config/quad.yaml @@ -1,5 +1,6 @@ use_rerun: true # Enable visualization using rerun.io -render_depth: true # Enable render depth if use_rerun is true +render_depth: true # Enable rendering depth +use_multithreading_depth_rendering: true # Enable multithreading for depth rendering for large resolution (above 32x24) simulation: control_frequency: 200 # Frequency of control loop execution (Hz) diff --git a/src/config.rs b/src/config.rs index b40d2a09..7cf43b7e 100644 --- a/src/config.rs +++ b/src/config.rs @@ -21,6 +21,8 @@ pub struct Config { pub use_rerun: bool, /// Render depth pub render_depth: bool, + /// MultiThreading depth rendering + pub use_multithreading_depth_rendering: bool, } #[derive(serde::Deserialize)] diff --git a/src/lib.rs b/src/lib.rs index e4f593d8..f3aa975a 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -16,8 +16,11 @@ //! let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977]; //! let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix); //! ``` +use rand::SeedableRng; +use rayon::prelude::*; pub mod config; use nalgebra::{Matrix3, Rotation3, SMatrix, UnitQuaternion, Vector3}; +use rand_chacha::ChaCha8Rng; use rand_distr::{Distribution, Normal}; use std::f32::consts::PI; #[derive(thiserror::Error, Debug)] @@ -204,6 +207,16 @@ pub struct Imu { pub accel_bias_std: f32, /// Standard deviation of gyroscope bias drift pub gyro_bias_std: f32, + /// Accelerometer noise distribution + accel_noise: Normal, + /// Gyroscope noise distribution + gyro_noise: Normal, + /// Accelerometer bias drift distribution + accel_bias_drift: Normal, + /// Gyroscope bias drift distribution + gyro_bias_drift: Normal, + /// Random number generator + rng: ChaCha8Rng, } /// Implements the IMU impl Imu { @@ -226,15 +239,20 @@ impl Imu { gyro_noise_std: f32, accel_bias_std: f32, gyro_bias_std: f32, - ) -> Self { - Self { + ) -> Result { + Ok(Self { accel_bias: Vector3::zeros(), gyro_bias: Vector3::zeros(), accel_noise_std, gyro_noise_std, accel_bias_std, gyro_bias_std, - } + accel_noise: Normal::new(0.0, accel_noise_std)?, + gyro_noise: Normal::new(0.0, gyro_noise_std)?, + accel_bias_drift: Normal::new(0.0, accel_bias_std)?, + gyro_bias_drift: Normal::new(0.0, gyro_bias_std)?, + rng: ChaCha8Rng::from_entropy(), + }) } /// Updates the IMU biases over time /// # Arguments @@ -245,18 +263,15 @@ impl Imu { /// ``` /// use peng_quad::Imu; /// - /// let mut imu = Imu::new(0.01, 0.01, 0.01, 0.01); + /// let mut imu = Imu::new(0.01, 0.01, 0.01, 0.01).unwrap(); /// imu.update(0.01).unwrap(); /// ``` pub fn update(&mut self, dt: f32) -> Result<(), SimulationError> { - let accel_drift = Normal::new(0.0, self.accel_bias_std * dt.sqrt())?; - let gyro_drift = Normal::new(0.0, self.gyro_bias_std * dt.sqrt())?; - let accel_drift_vector = - || Vector3::from_iterator((0..3).map(|_| accel_drift.sample(&mut rand::thread_rng()))); - let gyro_drift_vector = - || Vector3::from_iterator((0..3).map(|_| gyro_drift.sample(&mut rand::thread_rng()))); - self.accel_bias += accel_drift_vector(); - self.gyro_bias += gyro_drift_vector(); + let dt_sqrt = fast_sqrt(dt); + let accel_drift = self.accel_bias_drift.sample(&mut self.rng) * dt_sqrt; + let gyro_drift = self.gyro_bias_drift.sample(&mut self.rng) * dt_sqrt; + self.accel_bias += Vector3::from_iterator((0..3).map(|_| accel_drift)); + self.gyro_bias += Vector3::from_iterator((0..3).map(|_| gyro_drift)); Ok(()) } /// Simulates IMU readings with added bias and noise @@ -272,24 +287,22 @@ impl Imu { /// use nalgebra::Vector3; /// use peng_quad::Imu; /// - /// let imu = Imu::new(0.01, 0.01, 0.01, 0.01); + /// let mut imu = Imu::new(0.01, 0.01, 0.01, 0.01).unwrap(); /// let true_acceleration = Vector3::new(0.0, 0.0, 9.81); /// let true_angular_velocity = Vector3::new(0.0, 0.0, 0.0); /// let (measured_acceleration, measured_ang_velocity) = imu.read(true_acceleration, true_angular_velocity).unwrap(); /// ``` pub fn read( - &self, + &mut self, true_acceleration: Vector3, true_angular_velocity: Vector3, ) -> Result<(Vector3, Vector3), SimulationError> { - let accel_noise = Normal::new(0.0, self.accel_noise_std)?; - let gyro_noise = Normal::new(0.0, self.gyro_noise_std)?; let accel_noise_sample = - || Vector3::from_iterator((0..3).map(|_| accel_noise.sample(&mut rand::thread_rng()))); + Vector3::from_iterator((0..3).map(|_| self.accel_noise.sample(&mut self.rng))); let gyro_noise_sample = - || Vector3::from_iterator((0..3).map(|_| gyro_noise.sample(&mut rand::thread_rng()))); - let measured_acceleration = true_acceleration + self.accel_bias + accel_noise_sample(); - let measured_ang_velocity = true_angular_velocity + self.gyro_bias + gyro_noise_sample(); + Vector3::from_iterator((0..3).map(|_| self.gyro_noise.sample(&mut self.rng))); + let measured_acceleration = true_acceleration + self.accel_bias + accel_noise_sample; + let measured_ang_velocity = true_angular_velocity + self.gyro_bias + gyro_noise_sample; Ok((measured_acceleration, measured_ang_velocity)) } } @@ -1860,7 +1873,7 @@ impl Maze { /// maze.generate_obstacles(5); /// ``` pub fn generate_obstacles(&mut self, num_obstacles: usize) { - let mut rng = rand::thread_rng(); + let mut rng = ChaCha8Rng::from_entropy(); self.obstacles = (0..num_obstacles) .map(|_| { let position = Vector3::new( @@ -1974,7 +1987,8 @@ impl Camera { /// let quad_orientation = UnitQuaternion::identity(); /// let mut maze = Maze::new([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 5, [0.1, 0.1, 0.1], [0.1, 0.5]); /// let mut depth_buffer = vec![0.0; 800 * 600]; - /// camera.render_depth(&quad_position, &quad_orientation, &maze, &mut depth_buffer); + /// let use_multithreading = true; + /// camera.render_depth(&quad_position, &quad_orientation, &maze, &mut depth_buffer, use_multithreading); /// ``` pub fn render_depth( &self, @@ -1982,21 +1996,35 @@ impl Camera { quad_orientation: &UnitQuaternion, maze: &Maze, depth_buffer: &mut Vec, + use_multi_threading: bool, ) -> Result<(), SimulationError> { let (width, height) = self.resolution; let total_pixels = width * height; - depth_buffer.clear(); - depth_buffer.reserve((total_pixels - depth_buffer.capacity()).max(0)); let rotation_camera_to_world = quad_orientation.to_rotation_matrix().matrix() * Matrix3::new(1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0); let rotation_world_to_camera = rotation_camera_to_world.transpose(); - for i in 0..total_pixels { - depth_buffer.push(self.ray_cast( - quad_position, - &rotation_world_to_camera, - &(rotation_camera_to_world * self.ray_directions[i]), - maze, - )?); + if use_multi_threading { + depth_buffer.reserve((total_pixels - depth_buffer.capacity()).max(0)); + depth_buffer + .par_iter_mut() + .enumerate() + .try_for_each(|(i, depth)| { + let direction = rotation_camera_to_world * self.ray_directions[i]; + *depth = + self.ray_cast(quad_position, &rotation_world_to_camera, &direction, maze)?; + Ok::<(), SimulationError>(()) + })?; + } else { + depth_buffer.clear(); + depth_buffer.reserve((total_pixels - depth_buffer.capacity()).max(0)); + for i in 0..total_pixels { + depth_buffer.push(self.ray_cast( + quad_position, + &rotation_world_to_camera, + &(rotation_camera_to_world * self.ray_directions[i]), + maze, + )?); + } } Ok(()) } @@ -2058,7 +2086,8 @@ impl Camera { let c = oc.dot(&oc) - obstacle.radius * obstacle.radius; let discriminant = b * b - c; if discriminant >= 0.0 { - let t = -b - discriminant.sqrt(); + // let t = -b - discriminant.sqrt(); + let t = -b - fast_sqrt(discriminant); if t > self.near && t < closest_hit { closest_hit = t; } @@ -2411,3 +2440,15 @@ pub fn color_map_fn(gray: f32) -> (u8, u8, u8) { .clamp(0.0, 255.0) as u8; (r, g, b) } + +/// Fast square root function +/// # Arguments +/// * `x` - The input value +/// # Returns +/// * The square root of the input value +#[inline(always)] +fn fast_sqrt(x: f32) -> f32 { + let i = x.to_bits(); + let i = 0x1fbd1df5 + (i >> 1); + f32::from_bits(i) +} diff --git a/src/main.rs b/src/main.rs index ea76392b..dbd63da8 100644 --- a/src/main.rs +++ b/src/main.rs @@ -34,7 +34,7 @@ fn main() -> Result<(), SimulationError> { config.imu.gyro_noise_std, config.imu.accel_bias_std, config.imu.gyro_bias_std, - ); + )?; let mut maze = Maze::new( config.maze.lower_bounds, config.maze.upper_bounds, @@ -112,9 +112,7 @@ fn main() -> Result<(), SimulationError> { &quad.angular_velocity, quad.time_step, ); - if i % (config.simulation.simulation_frequency / config.simulation.control_frequency) - == 0 - { + if i % (config.simulation.simulation_frequency / config.simulation.control_frequency) == 0 { quad.update_dynamics_with_controls(thrust, &torque); previous_thrust = thrust; previous_torque = torque; @@ -124,9 +122,16 @@ fn main() -> Result<(), SimulationError> { imu.update(quad.time_step)?; let (true_accel, true_gyro) = quad.read_imu()?; let (_measured_accel, _measured_gyro) = imu.read(true_accel, true_gyro)?; - if i % (config.simulation.simulation_frequency / config.simulation.log_frequency) - == 0 - { + if i % (config.simulation.simulation_frequency / config.simulation.log_frequency) == 0 { + if config.render_depth { + camera.render_depth( + &quad.position, + &quad.orientation, + &maze, + &mut depth_buffer, + config.use_multithreading_depth_rendering, + )?; + } if let Some(rec) = &rec { rec.set_time_seconds("timestamp", time); if trajectory.add_point(quad.position) { @@ -141,12 +146,6 @@ fn main() -> Result<(), SimulationError> { &_measured_gyro, )?; if config.render_depth { - camera.render_depth( - &quad.position, - &quad.orientation, - &maze, - &mut depth_buffer, - )?; log_depth_image( &rec, &depth_buffer,