From 1684830469f1e160e02a281c6a50d0cab10b0a49 Mon Sep 17 00:00:00 2001 From: Yang Zhou Date: Fri, 1 Nov 2024 11:09:21 -0400 Subject: [PATCH 1/5] optimize plan for MinimumJerkLinePlanner --- src/lib.rs | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index b6cfc47b..48907289 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -915,7 +915,10 @@ impl Planner for MinimumJerkLinePlanner { time: f32, ) -> (Vector3, Vector3, f32) { let t = ((time - self.start_time) / self.duration).clamp(0.0, 1.0); - let s = 10.0 * t.powi(3) - 15.0 * t.powi(4) + 6.0 * t.powi(5); + let t2 = t * t; + let t3 = t2 * t; + let t4 = t3 * t; + let s = 10.0 * t2 - 15.0 * t3 + 6.0 * t4; let s_dot = (30.0 * t.powi(2) - 60.0 * t.powi(3) + 30.0 * t.powi(4)) / self.duration; let position = self.start_position + (self.end_position - self.start_position) * s; let velocity = (self.end_position - self.start_position) * s_dot; From 6e0de68601b39124a421097cf0ad599e98e7bd24 Mon Sep 17 00:00:00 2001 From: Yang Zhou Date: Fri, 1 Nov 2024 11:19:14 -0400 Subject: [PATCH 2/5] optimize maze rng declaration --- src/lib.rs | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 48907289..563d33de 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1978,6 +1978,8 @@ impl Obstacle { /// # Example /// ``` /// use peng_quad::{Maze, Obstacle}; +/// use rand_chacha::ChaCha8Rng; +/// use rand::SeedableRng; /// use nalgebra::Vector3; /// let maze = Maze { /// lower_bounds: [0.0, 0.0, 0.0], @@ -1985,6 +1987,7 @@ impl Obstacle { /// obstacles: vec![Obstacle::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 1.0)], /// obstacles_velocity_bounds: [0.0, 0.0, 0.0], /// obstacles_radius_bounds: [0.0, 0.0], +/// rng: ChaCha8Rng::from_entropy(), /// }; /// ``` pub struct Maze { @@ -1998,6 +2001,8 @@ pub struct Maze { pub obstacles_velocity_bounds: [f32; 3], /// The bounds of the obstacles' radius pub obstacles_radius_bounds: [f32; 2], + /// Rng for generating random numbers + pub rng: ChaCha8Rng, } /// Implementation of the maze impl Maze { @@ -2026,6 +2031,7 @@ impl Maze { obstacles: Vec::new(), obstacles_velocity_bounds, obstacles_radius_bounds, + rng: ChaCha8Rng::from_entropy(), }; maze.generate_obstacles(num_obstacles); maze @@ -2040,22 +2046,21 @@ impl Maze { /// maze.generate_obstacles(5); /// ``` pub fn generate_obstacles(&mut self, num_obstacles: usize) { - let mut rng = ChaCha8Rng::from_entropy(); self.obstacles = (0..num_obstacles) .map(|_| { let position = Vector3::new( - rand::Rng::gen_range(&mut rng, self.lower_bounds[0]..self.upper_bounds[0]), - rand::Rng::gen_range(&mut rng, self.lower_bounds[1]..self.upper_bounds[1]), - rand::Rng::gen_range(&mut rng, self.lower_bounds[2]..self.upper_bounds[2]), + rand::Rng::gen_range(&mut self.rng, self.lower_bounds[0]..self.upper_bounds[0]), + rand::Rng::gen_range(&mut self.rng, self.lower_bounds[1]..self.upper_bounds[1]), + rand::Rng::gen_range(&mut self.rng, self.lower_bounds[2]..self.upper_bounds[2]), ); let v_bounds = self.obstacles_velocity_bounds; let r_bounds = self.obstacles_radius_bounds; let velocity = Vector3::new( - rand::Rng::gen_range(&mut rng, -v_bounds[0]..v_bounds[0]), - rand::Rng::gen_range(&mut rng, -v_bounds[1]..v_bounds[1]), - rand::Rng::gen_range(&mut rng, -v_bounds[2]..v_bounds[2]), + rand::Rng::gen_range(&mut self.rng, -v_bounds[0]..v_bounds[0]), + rand::Rng::gen_range(&mut self.rng, -v_bounds[1]..v_bounds[1]), + rand::Rng::gen_range(&mut self.rng, -v_bounds[2]..v_bounds[2]), ); - let radius = rand::Rng::gen_range(&mut rng, r_bounds[0]..r_bounds[1]); + let radius = rand::Rng::gen_range(&mut self.rng, r_bounds[0]..r_bounds[1]); Obstacle::new(position, velocity, radius) }) .collect(); From 9e7c643953fdd5caa48a8efb0ce833bbd180effe Mon Sep 17 00:00:00 2001 From: Yang Zhou Date: Fri, 1 Nov 2024 12:05:41 -0400 Subject: [PATCH 3/5] enlarge x of maze to fix depth rendering issue when the robot is outside of box --- config/quad.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/config/quad.yaml b/config/quad.yaml index 461dbd7b..827379b9 100644 --- a/config/quad.yaml +++ b/config/quad.yaml @@ -36,8 +36,8 @@ imu: gyro_bias_std: 0.0001 # Standard deviation of gyroscope bias instability (rad/s) maze: - lower_bounds: [-3.0, -2.0, 0.0] # Lower bounds of the maze [x, y, z] (m) - upper_bounds: [3.0, 2.0, 2.0] # Upper bounds of the maze [x, y, z] (m) + lower_bounds: [-4.0, -2.0, 0.0] # Lower bounds of the maze [x, y, z] (m) + upper_bounds: [4.0, 2.0, 2.0] # Upper bounds of the maze [x, y, z] (m) num_obstacles: 20 # Number of obstacles in the maze obstacles_velocity_bounds: [0.2, 0.2, 0.1] # Maximum velocity of obstacles [x, y, z] (m/s) obstacles_radius_bounds: [0.05, 0.1] # Range of obstacle radii [min, max] (m) From 23cfd3f6d10ab52999e44ee6d7adccce266e400a Mon Sep 17 00:00:00 2001 From: Yang Zhou Date: Fri, 1 Nov 2024 12:17:45 -0400 Subject: [PATCH 4/5] use chunk for render_depth multithreading --- src/lib.rs | 23 ++++++++++++++++++----- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 563d33de..46c71ade 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -2189,15 +2189,28 @@ impl Camera { 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(); + + const CHUNK_SIZE: usize = 64; if use_multi_threading { depth_buffer.reserve((total_pixels - depth_buffer.capacity()).max(0)); depth_buffer - .par_iter_mut() + .par_chunks_mut(CHUNK_SIZE) .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)?; + .try_for_each(|(chunk_idx, chunk)| { + let start_idx = chunk_idx * CHUNK_SIZE; + for (i, depth) in chunk.iter_mut().enumerate() { + let ray_idx = start_idx + i; + if ray_idx >= total_pixels { + break; + } + let direction = rotation_camera_to_world * self.ray_directions[ray_idx]; + *depth = self.ray_cast( + quad_position, + &rotation_world_to_camera, + &direction, + maze, + )?; + } Ok::<(), SimulationError>(()) })?; } else { From b185eefb3773d53e681f0b2f62bf9229e7c3ae10 Mon Sep 17 00:00:00 2001 From: Yang Zhou Date: Fri, 1 Nov 2024 12:22:25 -0400 Subject: [PATCH 5/5] optimize cache for compute_position_control --- src/lib.rs | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 46c71ade..febf717e 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -634,9 +634,10 @@ impl PIDController { + self.kpid_pos[2].component_mul(&self.integral_pos_error); let gravity_compensation = Vector3::new(0.0, 0.0, self.gravity); let total_acceleration = acceleration + gravity_compensation; - let thrust = self.mass * total_acceleration.norm(); - let desired_orientation = if total_acceleration.norm() > 1e-6 { - let z_body = total_acceleration.normalize(); + let total_acc_norm = total_acceleration.norm(); + let thrust = self.mass * total_acc_norm; + let desired_orientation = if total_acc_norm > 1e-6 { + let z_body = total_acceleration / total_acc_norm; let yaw_rotation = UnitQuaternion::from_euler_angles(0.0, 0.0, desired_yaw); let x_body_horizontal = yaw_rotation * Vector3::new(1.0, 0.0, 0.0); let y_body = z_body.cross(&x_body_horizontal).normalize();