Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Optimization of Efficiency #6

Merged
merged 5 commits into from
Nov 1, 2024
Merged

Optimization of Efficiency #6

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
1684830
optimize plan for MinimumJerkLinePlanner
makeecat Nov 1, 2024
6e0de68
optimize maze rng declaration
makeecat Nov 1, 2024
9e7c643
enlarge x of maze to fix depth rendering issue when the robot is outs…
makeecat Nov 1, 2024
23cfd3f
use chunk for render_depth multithreading
makeecat Nov 1, 2024
b185eef
optimize cache for compute_position_control
makeecat Nov 1, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions config/quad.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
56 changes: 39 additions & 17 deletions src/lib.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -915,7 +916,10 @@ impl Planner for MinimumJerkLinePlanner {
time: f32,
) -> (Vector3<f32>, Vector3<f32>, 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;
Expand Down Expand Up @@ -1975,13 +1979,16 @@ 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],
/// upper_bounds: [1.0, 1.0, 1.0],
/// 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 {
Expand All @@ -1995,6 +2002,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 {
Expand Down Expand Up @@ -2023,6 +2032,7 @@ impl Maze {
obstacles: Vec::new(),
obstacles_velocity_bounds,
obstacles_radius_bounds,
rng: ChaCha8Rng::from_entropy(),
};
maze.generate_obstacles(num_obstacles);
maze
Expand All @@ -2037,22 +2047,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();
Expand Down Expand Up @@ -2181,15 +2190,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 {
Expand Down