diff --git a/Cargo.toml b/Cargo.toml index e3e54956..c1529f44 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -25,3 +25,6 @@ rand_distr = "0.4.3" rerun = "0.18.0" ndarray = "0.15.6" thiserror = "1.0.63" +serde = { version = "1.0.209", features = ["derive"] } +serde_yaml = "0.9.34" +lazy_static = "1.5.0" diff --git a/README.md b/README.md index e72c5975..6b541151 100644 --- a/README.md +++ b/README.md @@ -93,12 +93,23 @@ I would like to thank [tokei](https://github.com/XAMPPRocky/tokei) for providing My goal is to keep the project minimal and easy to understand by keeping the code line count below 1500. The markdown lines are used for generating the documentation. ```markdown -======================================================== - Language Files Lines Code Comments Blanks -======================================================== - Rust 1 1270 1233 12 25 - |- Markdown 1 291 0 291 0 - (Total) 1561 1233 303 25 +=============================================================================== +Language Files Lines Code Comments Blanks +=============================================================================== +SVG 1 1 1 0 0 +TOML 1 30 29 0 1 +YAML 1 93 86 0 7 +------------------------------------------------------------------------------- +Markdown 4 274 0 210 64 +|- Markdown 1 17 0 17 0 +(Total) 291 0 227 64 +------------------------------------------------------------------------------- +Rust 3 1542 1491 13 38 +|- Markdown 2 301 0 301 0 +(Total) 1843 1491 314 38 +=============================================================================== +Total 10 1940 1607 223 110 +=============================================================================== ``` ## Star History diff --git a/config/quad.yaml b/config/quad.yaml new file mode 100644 index 00000000..12dc105e --- /dev/null +++ b/config/quad.yaml @@ -0,0 +1,93 @@ +simulation: + control_frequency: 200.0 + simulation_frequency: 1000.0 + log_frequency: 20.0 + duration: 70.0 # in seconds + +quadrotor: + mass: 1.3 + gravity: 9.81 + drag_coefficient: 0.000 + inertia_matrix: [3.04e-3, 0.0, 0.0, 0.0, 4.55e-3, 0.0, 0.0, 0.0, 2.82e-3] + +controller: + pos_gains: + kp: [7.1, 7.1, 11.9] + kd: [2.4, 2.4, 6.7] + ki: [0.0, 0.0, 0.0] + att_gains: + kp: [1.5, 1.5, 1.0] + kd: [0.13, 0.13, 0.1] + ki: [0.0, 0.0, 0.0] + pos_max_int: [10.0, 10.0, 10.0] + att_max_int: [0.5, 0.5, 0.5] + +imu: + accel_noise_std: 0.02 + gyro_noise_std: 0.01 + bias_instability: 0.0001 + +maze: + lower_bounds: [-3.0, -2.0, 0.0] + upper_bounds: [3.0, 2.0, 2.0] + num_obstacles: 20 + +camera: + resolution: [128, 96] + fov: 90.0 + near: 0.1 + far: 5.0 + +mesh: + division: 7 + spacing: 0.5 + +planner_schedule: + steps: + - step: 1000 + planner_type: MinimumJerkLine + params: + end_position: [0.0, 0.0, 1.0] + end_yaw: 0.0 + duration: 2.5 + - step: 5000 + planner_type: Lissajous + params: + center: [0.5, 0.5, 1.0] + amplitude: [0.5, 0.5, 0.2] + frequency: [1.0, 2.0, 3.0] + phase: [0.0, 1.5707963267948966, 0.0] # [0.0, PI/2, 0.0] + duration: 20.0 + end_yaw: 6.283185307179586 # 2*PI + ramp_time: 5.0 + - step: 27000 + planner_type: Circle + params: + center: [0.5, 0.5, 1.0] + radius: 0.5 + angular_velocity: 1.0 + duration: 8.0 + ramp_time: 2.0 + - step: 37000 + planner_type: ObstacleAvoidance + params: + target_position: [1.5, 1.0, 0.5] + duration: 10.0 + end_yaw: 0.0 + k_att: 0.03 + k_rep: 0.02 + d0: 0.5 + - step: 45000 + planner_type: MinimumSnapWaypoint + params: + waypoints: + - [1.0, 1.0, 1.5] + - [-1.0, 1.0, 1.75] + - [0.0, -1.0, 1.0] + - [0.0, 0.0, 0.5] + yaws: [1.5707963267948966, 3.141592653589793, -1.5707963267948966, 0.0] # [PI/2, PI, -PI/2, 0.0] + segment_times: [5.0, 5.0, 5.0, 5.0] + - step: 65000 + planner_type: Landing + params: + duration: 5.0 diff --git a/src/config.rs b/src/config.rs new file mode 100644 index 00000000..90a40e71 --- /dev/null +++ b/src/config.rs @@ -0,0 +1,93 @@ +use serde::Deserialize; +use serde_yaml::Value; +use std::fs::File; +use std::io::Read; + +#[derive(Deserialize)] +pub struct Config { + pub simulation: SimulationConfig, + pub quadrotor: QuadrotorConfig, + pub controller: ControllerConfig, + pub imu: ImuConfig, + pub maze: MazeConfig, + pub camera: CameraConfig, + pub mesh: MeshConfig, + pub planner_schedule: PlannerScheduleConfig, +} + +#[derive(Deserialize)] +pub struct PlannerScheduleConfig { + pub steps: Vec, +} + +#[derive(Deserialize)] +pub struct PlannerStep { + pub step: usize, + pub planner_type: String, + pub params: Value, +} +#[derive(Deserialize)] +pub struct SimulationConfig { + pub control_frequency: f32, + pub simulation_frequency: f32, + pub log_frequency: f32, + pub duration: f32, +} + +#[derive(Deserialize)] +pub struct QuadrotorConfig { + pub mass: f32, + pub gravity: f32, + pub drag_coefficient: f32, + pub inertia_matrix: [f32; 9], +} + +#[derive(Deserialize)] +pub struct ControllerConfig { + pub pos_gains: PIDGains, + pub att_gains: PIDGains, + pub pos_max_int: [f32; 3], + pub att_max_int: [f32; 3], +} + +#[derive(Deserialize)] +pub struct PIDGains { + pub kp: [f32; 3], + pub ki: [f32; 3], + pub kd: [f32; 3], +} + +#[derive(Deserialize, Default)] +pub struct ImuConfig { + pub accel_noise_std: f32, + pub gyro_noise_std: f32, + pub bias_instability: f32, +} + +#[derive(Deserialize)] +pub struct MazeConfig { + pub upper_bounds: [f32; 3], + pub lower_bounds: [f32; 3], + pub num_obstacles: usize, +} + +#[derive(Deserialize)] +pub struct CameraConfig { + pub resolution: (usize, usize), + pub fov: f32, + pub near: f32, + pub far: f32, +} + +#[derive(Deserialize)] +pub struct MeshConfig { + pub division: usize, + pub spacing: f32, +} +impl Config { + pub fn from_yaml(filename: &str) -> Result> { + let mut contents = String::new(); + File::open(filename)?.read_to_string(&mut contents)?; + Ok(serde_yaml::from_str(&contents)?) + } +} diff --git a/src/lib.rs b/src/lib.rs new file mode 100644 index 00000000..10209825 --- /dev/null +++ b/src/lib.rs @@ -0,0 +1,1599 @@ +//! # Quadrotor Simulation +//! This crate provides a comprehensive simulation environment for quadrotor drones. +//! It includes models for quadrotor dynamics, IMU simulation, various trajectory planners, +//! and a PID controller for position and attitude control. +//! ## Features +//! - Realistic quadrotor dynamics simulation +//! - IMU sensor simulation with configurable noise parameters +//! - Multiple trajectory planners including hover, minimum jerk, Lissajous curves, and circular paths +//! - PID controller for position and attitude control +//! - Integration with the `rerun` crate for visualization +#[macro_use] +extern crate lazy_static; +use nalgebra::{Matrix3, Rotation3, SMatrix, UnitQuaternion, Vector3}; +use rand_distr::{Distribution, Normal}; +use std::collections::HashMap; +use std::f32::consts::PI; +#[derive(thiserror::Error, Debug)] +pub enum SimulationError { + #[error("Rerun error: {0}")] + RerunError(#[from] rerun::RecordingStreamError), + #[error("Nalgebra error: {0}")] + NalgebraError(String), + #[error("Normal error: {0}")] + NormalError(#[from] rand_distr::NormalError), + #[error("Other error: {0}")] + OtherError(String), +} +type PlannerCreationFn = + fn(&PlannerStepConfig, &Quadrotor, f32, &Vec) -> Result; +lazy_static! { + static ref PLANNER_CREATORS: HashMap = { + vec![ + ("MinimumJerkLine", create_planner as PlannerCreationFn), + ("Lissajous", create_planner as PlannerCreationFn), + ("Circle", create_planner as PlannerCreationFn), + ("ObstacleAvoidance", create_planner as PlannerCreationFn), + ("MinimumSnapWaypoint", create_planner as PlannerCreationFn), + ("Landing", create_planner as PlannerCreationFn), + ] + .into_iter() + .map(|(k, v)| (k.to_string(), v)) + .collect() + }; +} +/// Represents a quadrotor with its physical properties and state +pub struct Quadrotor { + /// Current position of the quadrotor in 3D space + pub position: Vector3, + /// Current velocity of the quadrotor + pub velocity: Vector3, + /// Current orientation of the quadrotor + pub orientation: UnitQuaternion, + /// Current angular velocity of the quadrotor + pub angular_velocity: Vector3, + /// Mass of the quadrotor in kg + pub mass: f32, + /// Gravitational acceleration in m/s^2 + pub gravity: f32, + /// Simulation time step in seconds + pub time_step: f32, + /// Drag coefficient + pub drag_coefficient: f32, + /// Inertia matrix of the quadrotor + pub inertia_matrix: Matrix3, + /// Inverse of the inertia matrix + pub inertia_matrix_inv: Matrix3, +} + +impl Quadrotor { + /// Creates a new Quadrotor with default parameters + /// * Arguments + /// * `time_step` - The simulation time step in seconds + pub fn new( + time_step: f32, + _mass: f32, + _gravity: f32, + _drag_coefficients: f32, + _inertia_matrix: [f32; 9], + ) -> Result { + let inertia_matrix = Matrix3::from_row_slice(&_inertia_matrix); + let inertia_matrix_inv = + inertia_matrix + .try_inverse() + .ok_or(SimulationError::NalgebraError( + "Failed to invert inertia matrix".to_string(), + ))?; + Ok(Self { + position: Vector3::zeros(), + velocity: Vector3::zeros(), + orientation: UnitQuaternion::identity(), + angular_velocity: Vector3::zeros(), + mass: 1.3, + gravity: 9.81, + time_step, + drag_coefficient: 0.000, + inertia_matrix, + inertia_matrix_inv, + }) + } + /// Updates the quadrotor's dynamics with control inputs + /// # Arguments + /// * `control_thrust` - The total thrust force applied to the quadrotor + /// * `control_torque` - The 3D torque vector applied to the quadrotor + pub fn update_dynamics_with_controls( + &mut self, + control_thrust: f32, + control_torque: &Vector3, + ) { + let gravity_force = Vector3::new(0.0, 0.0, -self.mass * self.gravity); + let drag_force = -self.drag_coefficient * self.velocity.norm() * self.velocity; + let thrust_world = self.orientation * Vector3::new(0.0, 0.0, control_thrust); + let acceleration = (thrust_world + gravity_force + drag_force) / self.mass; + self.velocity += acceleration * self.time_step; + self.position += self.velocity * self.time_step; + let inertia_angular_velocity = self.inertia_matrix * self.angular_velocity; + let gyroscopic_torque = self.angular_velocity.cross(&inertia_angular_velocity); + let angular_acceleration = self.inertia_matrix_inv * (control_torque - gyroscopic_torque); + self.angular_velocity += angular_acceleration * self.time_step; + self.orientation *= + UnitQuaternion::from_scaled_axis(self.angular_velocity * self.time_step); + } + /// Simulates IMU readings + /// # Returns + /// A tuple containing the actual acceleration and angular velocity + pub fn read_imu(&self) -> Result<(Vector3, Vector3), SimulationError> { + let gravity_world = Vector3::new(0.0, 0.0, self.gravity); + let true_acceleration = + self.orientation.inverse() * (self.velocity / self.time_step - gravity_world); + Ok((true_acceleration, self.angular_velocity)) + } +} +/// Represents an Inertial Measurement Unit (IMU) with bias and noise characteristics +pub struct Imu { + /// Accelerometer bias + pub accel_bias: Vector3, + /// Gyroscope bias + pub gyro_bias: Vector3, + /// Standard deviation of accelerometer noise + pub accel_noise_std: f32, + /// Standard deviation of gyroscope noise + pub gyro_noise_std: f32, + /// Bias instability coefficient + pub bias_instability: f32, +} + +impl Imu { + /// Creates a new IMU with default parameters + pub fn new(_accel_noise_std: f32, _gyro_noise_std: f32, _bias_instability: f32) -> Self { + Self { + accel_bias: Vector3::zeros(), + gyro_bias: Vector3::zeros(), + accel_noise_std: _accel_noise_std, + gyro_noise_std: _gyro_noise_std, + bias_instability: _bias_instability, + } + } + /// Updates the IMU biases over time + /// # Arguments + /// * `dt` - Time step for the update + pub fn update(&mut self, dt: f32) -> Result<(), SimulationError> { + let bias_drift = Normal::new(0.0, self.bias_instability * dt.sqrt())?; + let drift_vector = + || Vector3::from_iterator((0..3).map(|_| bias_drift.sample(&mut rand::thread_rng()))); + self.accel_bias += drift_vector(); + self.gyro_bias += drift_vector(); + Ok(()) + } + /// Simulates IMU readings with added bias and noise + /// # Arguments + /// * `true_acceleration` - The true acceleration vector + /// * `true_angular_velocity` - The true angular velocity vector + /// # Returns + /// A tuple containing the measured acceleration and angular velocity + pub fn read( + &self, + true_acceleration: Vector3, + true_angular_velocity: Vector3, + ) -> Result<(Vector3, Vector3), SimulationError> { + let mut rng = rand::thread_rng(); + let accel_noise = Normal::new(0.0, self.accel_noise_std)?; + let gyro_noise = Normal::new(0.0, self.gyro_noise_std)?; + let measured_acceleration = true_acceleration + + self.accel_bias + + Vector3::new( + accel_noise.sample(&mut rng), + accel_noise.sample(&mut rng), + accel_noise.sample(&mut rng), + ); + let measured_angular_velocity = true_angular_velocity + + self.gyro_bias + + Vector3::new( + gyro_noise.sample(&mut rng), + gyro_noise.sample(&mut rng), + gyro_noise.sample(&mut rng), + ); + Ok((measured_acceleration, measured_angular_velocity)) + } +} +/// PID controller for quadrotor position and attitude control +pub struct PIDController { + /// PID gain for position control including proportional, derivative, and integral gains + kpid_pos: [Vector3; 3], + /// PID gain for attitude control including proportional, derivative, and integral gains + kpid_att: [Vector3; 3], + /// Accumulated integral error for position + integral_pos_error: Vector3, + /// Accumulated integral error for attitude + integral_att_error: Vector3, + /// Maximum allowed integral error for position + max_integral_pos: Vector3, + /// Maximum allowed integral error for attitude + max_integral_att: Vector3, +} + +impl PIDController { + /// Creates a new PIDController with default gains + /// gains are in the order of proportional, derivative, and integral + pub fn new( + _kpid_pos: [[f32; 3]; 3], + _kpid_att: [[f32; 3]; 3], + _max_integral_pos: [f32; 3], + _max_integral_att: [f32; 3], + ) -> Self { + Self { + kpid_pos: _kpid_pos.map(Vector3::from), + kpid_att: _kpid_att.map(Vector3::from), + integral_pos_error: Vector3::zeros(), + integral_att_error: Vector3::zeros(), + max_integral_pos: Vector3::from(_max_integral_pos), + max_integral_att: Vector3::from(_max_integral_att), + } + } + /// Computes attitude control torques + /// # Arguments + /// * `desired_orientation` - The desired orientation quaternion + /// * `current_orientation` - The current orientation quaternion + /// * `current_angular_velocity` - The current angular velocity + /// * `dt` - Time step + /// # Returns + /// The computed control torque vector + pub fn compute_attitude_control( + &mut self, + desired_orientation: &UnitQuaternion, + current_orientation: &UnitQuaternion, + current_angular_velocity: &Vector3, + dt: f32, + ) -> Vector3 { + let error_orientation = current_orientation.inverse() * desired_orientation; + let (roll_error, pitch_error, yaw_error) = error_orientation.euler_angles(); + let error_angles = Vector3::new(roll_error, pitch_error, yaw_error); + self.integral_att_error += error_angles * dt; + self.integral_att_error = self + .integral_att_error + .zip_map(&self.max_integral_att, |int, max| int.clamp(-max, max)); + let error_angular_velocity = -current_angular_velocity; // TODO: Add desired angular velocity + self.kpid_att[0].component_mul(&error_angles) + + self.kpid_att[1].component_mul(&error_angular_velocity) + + self.kpid_att[2].component_mul(&self.integral_att_error) + } + /// Computes position control thrust and desired orientation + /// # Arguments + /// * `desired_position` - The desired position + /// * `desired_velocity` - The desired velocity + /// * `desired_yaw` - The desired yaw angle + /// * `current_position` - The current position + /// * `current_velocity` - The current velocity + /// * `dt` - Time step + /// * `mass` - Mass of the quadrotor + /// * `gravity` - Gravitational acceleration + /// # Returns + /// A tuple containing the computed thrust and desired orientation quaternion + pub fn compute_position_control( + &mut self, + desired_position: &Vector3, + desired_velocity: &Vector3, + desired_yaw: f32, + current_position: &Vector3, + current_velocity: &Vector3, + dt: f32, + mass: f32, + gravity: f32, + ) -> (f32, UnitQuaternion) { + let error_position = desired_position - current_position; + let error_velocity = desired_velocity - current_velocity; + self.integral_pos_error += error_position * dt; + self.integral_pos_error = self + .integral_pos_error + .zip_map(&self.max_integral_pos, |int, max| int.clamp(-max, max)); + let acceleration = self.kpid_pos[0].component_mul(&error_position) + + self.kpid_pos[1].component_mul(&error_velocity) + + self.kpid_pos[2].component_mul(&self.integral_pos_error); + let gravity_compensation = Vector3::new(0.0, 0.0, gravity); + let total_acceleration = acceleration + gravity_compensation; + let thrust = mass * total_acceleration.norm(); + let desired_orientation = if total_acceleration.norm() > 1e-6 { + let z_body = total_acceleration.normalize(); + 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(); + let x_body = y_body.cross(&z_body); + UnitQuaternion::from_rotation_matrix(&Rotation3::from_matrix_unchecked( + Matrix3::from_columns(&[x_body, y_body, z_body]), + )) + } else { + UnitQuaternion::from_euler_angles(0.0, 0.0, desired_yaw) + }; + (thrust, desired_orientation) + } +} +/// Enum representing different types of trajectory planners +pub enum PlannerType { + Hover(HoverPlanner), + MinimumJerkLine(MinimumJerkLinePlanner), + Lissajous(LissajousPlanner), + Circle(CirclePlanner), + Landing(LandingPlanner), + ObstacleAvoidance(ObstacleAvoidancePlanner), + MinimumSnapWaypoint(MinimumSnapWaypointPlanner), +} +impl PlannerType { + /// Plans the trajectory based on the current planner type + /// # Arguments + /// * `current_position` - The current position of the quadrotor + /// * `current_velocity` - The current velocity of the quadrotor + /// * `time` - The current simulation time + /// # Returns + /// A tuple containing the desired position, velocity, and yaw angle + pub fn plan( + &self, + current_position: Vector3, + current_velocity: Vector3, + time: f32, + ) -> (Vector3, Vector3, f32) { + match self { + PlannerType::Hover(p) => p.plan(current_position, current_velocity, time), + PlannerType::MinimumJerkLine(p) => p.plan(current_position, current_velocity, time), + PlannerType::Lissajous(p) => p.plan(current_position, current_velocity, time), + PlannerType::Circle(p) => p.plan(current_position, current_velocity, time), + PlannerType::Landing(p) => p.plan(current_position, current_velocity, time), + PlannerType::ObstacleAvoidance(p) => p.plan(current_position, current_velocity, time), + PlannerType::MinimumSnapWaypoint(p) => p.plan(current_position, current_velocity, time), + } + } + /// Checks if the current trajectory is finished + /// # Arguments + /// * `current_position` - The current position of the quadrotor + /// * `time` - The current simulation time + /// # Returns + /// `true` if the trajectory is finished, `false` otherwise + pub fn is_finished( + &self, + current_position: Vector3, + time: f32, + ) -> Result { + match self { + PlannerType::Hover(p) => Ok(p.is_finished(current_position, time)), + PlannerType::MinimumJerkLine(p) => Ok(p.is_finished(current_position, time)), + PlannerType::Lissajous(p) => Ok(p.is_finished(current_position, time)), + PlannerType::Circle(p) => Ok(p.is_finished(current_position, time)), + PlannerType::Landing(p) => Ok(p.is_finished(current_position, time)), + PlannerType::ObstacleAvoidance(p) => Ok(p.is_finished(current_position, time)), + PlannerType::MinimumSnapWaypoint(p) => p.is_finished(current_position, time), + } + } +} +/// Trait defining the interface for trajectory planners +trait Planner { + /// Plans the trajectory based on the current state and time + /// # Arguments + /// * `current_position` - The current position of the quadrotor + /// * `current_velocity` - The current velocity of the quadrotor + /// * `time` - The current simulation time + /// # Returns + fn plan( + &self, + current_position: Vector3, + current_velocity: Vector3, + time: f32, + ) -> (Vector3, Vector3, f32); + + /// Checks if the current trajectory is finished + /// # Arguments + /// * `current_position` - The current position of the quadrotor + /// * `time` - The current simulation time + /// # Returns + /// `true` if the trajectory is finished, `false` otherwise + fn is_finished(&self, current_position: Vector3, time: f32) -> bool; +} +/// Planner for hovering at a fixed position +pub struct HoverPlanner { + /// Target position for hovering + target_position: Vector3, + /// Target yaw angle for hovering + target_yaw: f32, +} + +impl Planner for HoverPlanner { + fn plan( + &self, + _current_position: Vector3, + _current_velocity: Vector3, + _time: f32, + ) -> (Vector3, Vector3, f32) { + (self.target_position, Vector3::zeros(), self.target_yaw) + } + + fn is_finished(&self, _current_position: Vector3, _time: f32) -> bool { + true // Hover planner is always "finished" as it's the default state + } +} +/// Planner for minimum jerk trajectories along a straight line +pub struct MinimumJerkLinePlanner { + /// Starting position of the trajectory + start_position: Vector3, + /// Ending position of the trajectory + end_position: Vector3, + /// Starting yaw angle + start_yaw: f32, + /// Ending yaw angle + end_yaw: f32, + /// Start time of the trajectory + start_time: f32, + /// Duration of the trajectory + duration: f32, +} + +impl Planner for MinimumJerkLinePlanner { + fn plan( + &self, + _current_position: Vector3, + _current_velocity: Vector3, + 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 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; + let yaw = self.start_yaw + (self.end_yaw - self.start_yaw) * s; + (position, velocity, yaw) + } + + fn is_finished(&self, _current_position: Vector3, _time: f32) -> bool { + (_current_position - self.end_position).norm() < 0.01 + && _time >= self.start_time + self.duration + } +} +/// Planner for Lissajous curve trajectories +pub struct LissajousPlanner { + /// Starting position of the trajectory + start_position: Vector3, + /// Center of the Lissajous curve + center: Vector3, + /// Amplitude of the Lissajous curve + amplitude: Vector3, + /// Frequency of the Lissajous curve + frequency: Vector3, + /// Phase of the Lissajous curve + phase: Vector3, + /// Start time of the trajectory + start_time: f32, + /// Duration of the trajectory + duration: f32, + /// Starting yaw angle + start_yaw: f32, + /// Ending yaw angle + end_yaw: f32, + /// Ramp-up time for smooth transitions + ramp_time: f32, +} + +impl Planner for LissajousPlanner { + fn plan( + &self, + _current_position: Vector3, + _current_velocity: Vector3, + time: f32, + ) -> (Vector3, Vector3, f32) { + let t = ((time - self.start_time) / self.duration).clamp(0.0, 1.0); + let smooth_start = if t < self.ramp_time / self.duration { + let t_ramp = t / (self.ramp_time / self.duration); + t_ramp * t_ramp * (3.0 - 2.0 * t_ramp) + } else { + 1.0 + }; + let velocity_ramp = if t < self.ramp_time / self.duration { + smooth_start + } else if t > 1.0 - self.ramp_time / self.duration { + let t_down = (1.0 - t) / (self.ramp_time / self.duration); + t_down * t_down * (3.0 - 2.0 * t_down) + } else { + 1.0 + }; + let ang_pos = self.frequency * t * 2.0 * PI + self.phase; + let lissajous = self.amplitude.component_mul(&ang_pos.map(f32::sin)); + let position = + self.start_position + smooth_start * ((self.center + lissajous) - self.start_position); + let mut velocity = Vector3::new( + self.amplitude.x * self.frequency.x * 2.0 * PI * ang_pos.x.cos(), + self.amplitude.y * self.frequency.y * 2.0 * PI * ang_pos.y.cos(), + self.amplitude.z * self.frequency.z * 2.0 * PI * ang_pos.z.cos(), + ) * velocity_ramp + / self.duration; + if t < self.ramp_time / self.duration { + let transition_velocity = (self.center - self.start_position) + * (2.0 * t / self.ramp_time - 2.0 * t * t / (self.ramp_time * self.ramp_time)) + / self.duration; + velocity += transition_velocity; + } + let yaw = self.start_yaw + (self.end_yaw - self.start_yaw) * t; + (position, velocity, yaw) + } + + fn is_finished(&self, _current_position: Vector3, time: f32) -> bool { + time >= self.start_time + self.duration + } +} +/// Planner for circular trajectories +pub struct CirclePlanner { + /// Center of the circular trajectory + center: Vector3, + /// Radius of the circular trajectory + radius: f32, + /// Angular velocity of the circular motion + angular_velocity: f32, + /// Starting position of the trajectory + start_position: Vector3, + /// Start time of the trajectory + start_time: f32, + /// Duration of the trajectory + duration: f32, + /// Starting yaw angle + start_yaw: f32, + /// Ending yaw angle + end_yaw: f32, + /// Ramp-up time for smooth transitions + ramp_time: f32, +} + +impl Planner for CirclePlanner { + fn plan( + &self, + _current_position: Vector3, + _current_velocity: Vector3, + time: f32, + ) -> (Vector3, Vector3, f32) { + let t = (time - self.start_time) / self.duration; + let t = t.clamp(0.0, 1.0); + let smooth_start = if t < self.ramp_time / self.duration { + let t_ramp = t / (self.ramp_time / self.duration); + t_ramp * t_ramp * (3.0 - 2.0 * t_ramp) + } else { + 1.0 + }; + let velocity_ramp = if t < self.ramp_time / self.duration { + smooth_start + } else if t > 1.0 - self.ramp_time / self.duration { + let t_down = (1.0 - t) / (self.ramp_time / self.duration); + t_down * t_down * (3.0 - 2.0 * t_down) + } else { + 1.0 + }; + let angle = self.angular_velocity * t * self.duration; + let circle_offset = Vector3::new(self.radius * angle.cos(), self.radius * angle.sin(), 0.0); + let position = self.start_position + + smooth_start * ((self.center + circle_offset) - self.start_position); + let tangential_velocity = Vector3::new( + -self.radius * self.angular_velocity * angle.sin(), + self.radius * self.angular_velocity * angle.cos(), + 0.0, + ); + let velocity = tangential_velocity * velocity_ramp; + let yaw = self.start_yaw + (self.end_yaw - self.start_yaw) * t; + (position, velocity, yaw) + } + + fn is_finished(&self, _current_position: Vector3, time: f32) -> bool { + time >= self.start_time + self.duration + } +} + +/// Planner for landing maneuvers +pub struct LandingPlanner { + /// Starting position of the landing maneuver + start_position: Vector3, + /// Start time of the landing maneuver + start_time: f32, + /// Duration of the landing maneuver + duration: f32, + /// Starting yaw angle + start_yaw: f32, +} + +impl Planner for LandingPlanner { + fn plan( + &self, + _current_position: Vector3, + _current_velocity: Vector3, + time: f32, + ) -> (Vector3, Vector3, f32) { + let t = ((time - self.start_time) / self.duration).clamp(0.0, 1.0); + let target_z = self.start_position.z * (1.0 - t); + let target_position = Vector3::new(self.start_position.x, self.start_position.y, target_z); + let target_velocity = Vector3::new(0.0, 0.0, -self.start_position.z / self.duration); + (target_position, target_velocity, self.start_yaw) + } + + fn is_finished(&self, current_position: Vector3, time: f32) -> bool { + current_position.z < 0.05 || time >= self.start_time + self.duration + } +} +/// Manages different trajectory planners and switches between them +pub struct PlannerManager { + current_planner: PlannerType, +} + +impl PlannerManager { + /// Creates a new PlannerManager with an initial hover planner + /// # Arguments + /// * `initial_position` - The initial position for hovering + /// * `initial_yaw` - The initial yaw angle for hovering + /// # Returns + /// A new PlannerManager instance + pub fn new(initial_position: Vector3, initial_yaw: f32) -> Self { + let hover_planner = HoverPlanner { + target_position: initial_position, + target_yaw: initial_yaw, + }; + Self { + current_planner: PlannerType::Hover(hover_planner), + } + } + /// Sets a new planner + /// # Arguments + /// * `new_planner` - The new planner to be set + pub fn set_planner(&mut self, new_planner: PlannerType) { + self.current_planner = new_planner; + } + /// Updates the current planner and returns the desired position, velocity, and yaw + /// # Arguments + /// * `current_position` - The current position of the quadrotor + /// * `current_orientation` - The current orientation of the quadrotor + /// * `current_velocity` - The current velocity of the quadrotor + /// * `time` - The current simulation time + /// # Returns + /// A tuple containing the desired position, velocity, and yaw angle + pub fn update( + &mut self, + current_position: Vector3, + current_orientation: UnitQuaternion, + current_velocity: Vector3, + time: f32, + obstacles: &Vec, + ) -> Result<(Vector3, Vector3, f32), SimulationError> { + if self.current_planner.is_finished(current_position, time)? { + self.current_planner = PlannerType::Hover(HoverPlanner { + target_position: current_position, + target_yaw: current_orientation.euler_angles().2, + }); + } + // Update obstacles for ObstacleAvoidancePlanner if needed + if let PlannerType::ObstacleAvoidance(ref mut planner) = self.current_planner { + planner.obstacles = obstacles.clone(); + } + Ok(self + .current_planner + .plan(current_position, current_velocity, time)) + } +} +/// Obstacle avoidance planner that uses a potential field approach to avoid obstacles +/// The planner calculates a repulsive force for each obstacle and an attractive force towards the goal +/// The resulting force is then used to calculate the desired position and velocity +pub struct ObstacleAvoidancePlanner { + /// Target position of the planner + target_position: Vector3, + /// Start time of the planner + start_time: f32, + /// Duration of the planner + duration: f32, + /// Starting yaw angle + start_yaw: f32, + /// Ending yaw angle + end_yaw: f32, + /// List of obstacles + obstacles: Vec, + /// Attractive force gain + k_att: f32, + /// Repulsive force gain + k_rep: f32, + /// Influence distance of obstacles + d0: f32, +} + +impl Planner for ObstacleAvoidancePlanner { + fn plan( + &self, + current_position: Vector3, + _current_velocity: Vector3, + time: f32, + ) -> (Vector3, Vector3, f32) { + let t = ((time - self.start_time) / self.duration).clamp(0.0, 1.0); + // Attractive force towards the goal + let f_att = self.k_att * (self.target_position - current_position); + // Repulsive force from obstacles + let mut f_rep = Vector3::zeros(); + for obstacle in &self.obstacles { + let diff = current_position - obstacle.position; + let distance = diff.norm(); + if distance < self.d0 { + f_rep += self.k_rep + * (1.0 / distance - 1.0 / self.d0) + * (1.0 / distance.powi(2)) + * diff.normalize(); + } + } + let f_total = f_att + f_rep; + let max_speed: f32 = 1.0; + let desired_velocity = f_total.normalize() * max_speed.min(f_total.norm()); + let desired_position = current_position + desired_velocity * self.duration * (1.0 - t); + let desired_yaw = self.start_yaw + (self.end_yaw - self.start_yaw) * t; + (desired_position, desired_velocity, desired_yaw) + } + + fn is_finished(&self, current_position: Vector3, time: f32) -> bool { + (current_position - self.target_position).norm() < 0.1 + && time >= self.start_time + self.duration + } +} + +/// Waypoint planner that generates a minimum snap trajectory between waypoints +/// # Arguments +/// * `waypoints` - A list of waypoints to follow +/// * `yaws` - A list of yaw angles at each waypoint +/// * `segment_times` - A list of times to reach each waypoint +/// * `start_time` - The start time of the trajectory +pub struct MinimumSnapWaypointPlanner { + waypoints: Vec>, + yaws: Vec, + times: Vec, + coefficients: Vec>>, + yaw_coefficients: Vec>, + start_time: f32, +} + +impl MinimumSnapWaypointPlanner { + fn new( + waypoints: Vec>, + yaws: Vec, + segment_times: Vec, + start_time: f32, + ) -> Result { + if waypoints.len() < 2 { + return Err(SimulationError::OtherError( + "At least two waypoints are required".to_string(), + )); + } + if waypoints.len() != segment_times.len() + 1 || waypoints.len() != yaws.len() { + return Err(SimulationError::OtherError("Number of segment times must be one less than number of waypoints, and yaws must match waypoints".to_string())); + } + let mut planner = Self { + waypoints, + yaws, + times: segment_times, + coefficients: Vec::new(), + yaw_coefficients: Vec::new(), + start_time, + }; + planner.compute_minimum_snap_trajectories()?; + planner.compute_minimum_acceleration_yaw_trajectories()?; + Ok(planner) + } + /// Compute the coefficients for the minimum snap trajectory, calculated for each segment between waypoints + fn compute_minimum_snap_trajectories(&mut self) -> Result<(), SimulationError> { + let n = self.waypoints.len() - 1; + for i in 0..n { + let duration = self.times[i]; + let (start, end) = (self.waypoints[i], self.waypoints[i + 1]); + let mut a = SMatrix::::zeros(); + let mut b = SMatrix::::zeros(); + a.fixed_view_mut::<4, 4>(0, 0).fill_with_identity(); + b.fixed_view_mut::<1, 3>(0, 0).copy_from(&start.transpose()); + b.fixed_view_mut::<1, 3>(4, 0).copy_from(&end.transpose()); + // End point constraints + for j in 0..8 { + a[(4, j)] = duration.powi(j as i32); + if j > 0 { + a[(5, j)] = j as f32 * duration.powi(j as i32 - 1); + } + if j > 1 { + a[(6, j)] = j as f32 * (j - 1) as f32 * duration.powi(j as i32 - 2); + } + if j > 2 { + a[(7, j)] = + j as f32 * (j - 1) as f32 * (j - 2) as f32 * duration.powi(j as i32 - 3); + } + } + let coeffs = a.lu().solve(&b).ok_or_else(|| { + SimulationError::NalgebraError( + "Failed to solve for coefficients in MinimumSnapWaypointPlanner".to_string(), + ) + })?; + self.coefficients.push( + (0..8) + .map(|j| Vector3::new(coeffs[(j, 0)], coeffs[(j, 1)], coeffs[(j, 2)])) + .collect(), + ); + } + Ok(()) + } + /// Compute the coefficients for yaw trajectories + /// The yaw trajectory is a cubic polynomial and interpolated between waypoints + fn compute_minimum_acceleration_yaw_trajectories(&mut self) -> Result<(), SimulationError> { + let n = self.yaws.len() - 1; // Number of segments + for i in 0..n { + let duration = self.times[i]; + let start_yaw = self.yaws[i]; + let end_yaw = self.yaws[i + 1]; + let mut a = SMatrix::::zeros(); + let mut b = SMatrix::::zeros(); + // Start point constraints + a[(0, 0)] = 1.0; + a[(1, 1)] = 1.0; + b[0] = start_yaw; + // End point constraints + for j in 0..4 { + a[(2, j)] = duration.powi(j as i32); + if j > 0 { + a[(3, j)] = j as f32 * duration.powi(j as i32 - 1); + } + } + b[2] = end_yaw; + let yaw_coeffs = a.lu().solve(&b).ok_or(SimulationError::NalgebraError( + "Failed to solve for yaw coefficients in MinimumSnapWaypointPlanner".to_string(), + ))?; + self.yaw_coefficients.push(yaw_coeffs.as_slice().to_vec()); + } + Ok(()) + } + /// Evaluate the trajectory at a given time, returns the position, velocity, yaw, and yaw rate at the given time + /// # Arguments + /// * `t` - The time to evaluate the trajectory at + /// * `coeffs` - The coefficients for the position trajectory + /// * `yaw_coeffs` - The coefficients for the yaw trajectory + /// # Returns + /// * `position` - The position at the given time (meters) + /// * `velocity` - The velocity at the given time (meters / second) + /// * `yaw` - The yaw at the given time (radians) + /// * `yaw_rate` - The yaw rate at the given time (radians / second) + fn evaluate_polynomial( + &self, + t: f32, + coeffs: &[Vector3], + yaw_coeffs: &[f32], + ) -> (Vector3, Vector3, f32, f32) { + let mut position = Vector3::zeros(); + let mut velocity = Vector3::zeros(); + let mut yaw = 0.0; + let mut yaw_rate = 0.0; + for (i, coeff) in coeffs.iter().enumerate() { + let ti = t.powi(i as i32); + position += coeff * ti; + if i > 0 { + velocity += coeff * (i as f32) * t.powi(i as i32 - 1); + } + } + for (i, &coeff) in yaw_coeffs.iter().enumerate() { + let ti = t.powi(i as i32); + yaw += coeff * ti; + if i > 0 { + yaw_rate += coeff * (i as f32) * t.powi(i as i32 - 1); + } + } + (position, velocity, yaw, yaw_rate) + } + fn plan( + &self, + _current_position: Vector3, + _current_velocity: Vector3, + time: f32, + ) -> (Vector3, Vector3, f32) { + let relative_time = time - self.start_time; + // Find the current segment + let mut segment_start_time = 0.0; + let mut current_segment = 0; + for (i, &segment_duration) in self.times.iter().enumerate() { + if relative_time < segment_start_time + segment_duration { + current_segment = i; + break; + } + segment_start_time += segment_duration; + } + // Evaluate the polynomial for the current segment + let segment_time = relative_time - segment_start_time; + let (position, velocity, yaw, _yaw_rate) = self.evaluate_polynomial( + segment_time, + &self.coefficients[current_segment], + &self.yaw_coefficients[current_segment], + ); + (position, velocity, yaw) + } + + fn is_finished( + &self, + current_position: Vector3, + time: f32, + ) -> Result { + let last_waypoint = self.waypoints.last().ok_or(SimulationError::OtherError( + "No waypoints available".to_string(), + ))?; + Ok(time >= self.start_time + self.times.iter().sum::() + && (current_position - last_waypoint).norm() < 0.1) + } +} + +#[derive(Clone)] +pub struct PlannerStepConfig { + pub step: usize, + pub planner_type: String, + pub params: serde_yaml::Value, +} + +#[derive(Clone)] +pub struct PlannerConfig { + pub steps: Vec, +} +/// Updates the planner based on the current simulation step and configuration +/// # Arguments +/// * `planner_manager` - The PlannerManager instance to update +/// * `step` - The current simulation step +/// * `time` - The current simulation time +/// * `quad` - The Quadrotor instance +/// * `obstacles` - The current obstacles in the simulation +/// * `planner_config` - The planner configuration +pub fn update_planner( + planner_manager: &mut PlannerManager, + step: usize, + time: f32, + quad: &Quadrotor, + obstacles: &Vec, + planner_config: &PlannerConfig, +) -> Result<(), SimulationError> { + if let Some(planner_step) = planner_config.steps.iter().find(|s| s.step == step) { + println!("[INFO] Step: {}, {}", step, planner_step.planner_type); + if let Some(creator_fn) = PLANNER_CREATORS.get(&planner_step.planner_type) { + planner_manager.set_planner(creator_fn(planner_step, quad, time, obstacles)?); + } else { + println!("[WARNING] Unknown type: {}", planner_step.planner_type); + } + } + Ok(()) +} +/// Creates a planner based on the configuration +/// # Arguments +/// * `step` - The configuration for the planner step +/// * `quad` - The Quadrotor instance +/// * `time` - The current simulation time +/// * `obstacles` - The current obstacles in the simulation +/// # Returns +/// * `PlannerType` - The created planner +fn create_planner( + step: &PlannerStepConfig, + quad: &Quadrotor, + time: f32, + obstacles: &Vec, +) -> Result { + let params = &step.params; + match step.planner_type.as_str() { + "MinimumJerkLine" => { + let end_position = parse_vector3(params, "end_position")?; + let end_yaw = parse_f32(params, "end_yaw")?; + let duration = parse_f32(params, "duration")?; + Ok(PlannerType::MinimumJerkLine(MinimumJerkLinePlanner { + start_position: quad.position, + end_position, + start_yaw: quad.orientation.euler_angles().2, + end_yaw, + start_time: time, + duration, + })) + } + "Lissajous" => { + let center = parse_vector3(params, "center")?; + let amplitude = parse_vector3(params, "amplitude")?; + let frequency = parse_vector3(params, "frequency")?; + let phase = parse_vector3(params, "phase")?; + let duration = parse_f32(params, "duration")?; + let end_yaw = parse_f32(params, "end_yaw")?; + let ramp_time = parse_f32(params, "ramp_time")?; + Ok(PlannerType::Lissajous(LissajousPlanner { + start_position: quad.position, + center, + amplitude, + frequency, + phase, + start_time: time, + duration, + start_yaw: quad.orientation.euler_angles().2, + end_yaw, + ramp_time, + })) + } + "Circle" => { + let center = parse_vector3(params, "center")?; + let radius = parse_f32(params, "radius")?; + let angular_velocity = parse_f32(params, "angular_velocity")?; + let duration = parse_f32(params, "duration")?; + let ramp_time = parse_f32(params, "ramp_time")?; + Ok(PlannerType::Circle(CirclePlanner { + center, + radius, + angular_velocity, + start_position: quad.position, + start_time: time, + duration, + start_yaw: quad.orientation.euler_angles().2, + end_yaw: quad.orientation.euler_angles().2, + ramp_time, + })) + } + "ObstacleAvoidance" => { + let target_position = parse_vector3(params, "target_position")?; + let duration = parse_f32(params, "duration")?; + let end_yaw = parse_f32(params, "end_yaw")?; + let k_att = parse_f32(params, "k_att")?; + let k_rep = parse_f32(params, "k_rep")?; + let d0 = parse_f32(params, "d0")?; + Ok(PlannerType::ObstacleAvoidance(ObstacleAvoidancePlanner { + target_position, + start_time: time, + duration, + start_yaw: quad.orientation.euler_angles().2, + end_yaw, + obstacles: obstacles.clone(), + k_att, + k_rep, + d0, + })) + } + "MinimumSnapWaypoint" => { + let mut waypoints = vec![quad.position]; + waypoints.extend( + params["waypoints"] + .as_sequence() + .ok_or_else(|| SimulationError::OtherError("Invalid waypoints".to_string()))? + .iter() + .map(|w| { + w.as_sequence() + .and_then(|coords| { + Some(Vector3::new( + coords.get(0)?.as_f64()? as f32, + coords.get(1)?.as_f64()? as f32, + coords.get(2)?.as_f64()? as f32, + )) + }) + .ok_or_else(|| { + SimulationError::OtherError("Invalid waypoint".to_string()) + }) + }) + .collect::, _>>()?, + ); + let mut yaws = vec![quad.orientation.euler_angles().2]; + yaws.extend( + params["yaws"] + .as_sequence() + .ok_or_else(|| SimulationError::OtherError("Invalid yaws".to_string()))? + .iter() + .map(|y| { + y.as_f64() + .map(|v| v as f32) + .ok_or_else(|| SimulationError::OtherError("Invalid yaw".to_string())) + }) + .collect::, _>>()?, + ); + let segment_times = params["segment_times"] + .as_sequence() + .ok_or_else(|| SimulationError::OtherError("Invalid segment_times".to_string()))? + .iter() + .map(|t| { + t.as_f64().map(|v| v as f32).ok_or_else(|| { + SimulationError::OtherError("Invalid segment time".to_string()) + }) + }) + .collect::, _>>()?; + MinimumSnapWaypointPlanner::new(waypoints, yaws, segment_times, time) + .map(PlannerType::MinimumSnapWaypoint) + } + "Landing" => { + let duration = parse_f32(params, "duration")?; + Ok(PlannerType::Landing(LandingPlanner { + start_position: quad.position, + start_time: time, + duration, + start_yaw: quad.orientation.euler_angles().2, + })) + } + _ => Err(SimulationError::OtherError(format!( + "Unknown planner type: {}", + step.planner_type + ))), + } +} +// Helper function to parse Vector3 from YAML +fn parse_vector3(value: &serde_yaml::Value, key: &str) -> Result, SimulationError> { + value[key] + .as_sequence() + .and_then(|seq| { + if seq.len() == 3 { + Some(Vector3::new( + seq[0].as_f64()? as f32, + seq[1].as_f64()? as f32, + seq[2].as_f64()? as f32, + )) + } else { + None + } + }) + .ok_or_else(|| SimulationError::OtherError(format!("Invalid {} vector", key))) +} + +// Helper function to parse f32 from YAML +fn parse_f32(value: &serde_yaml::Value, key: &str) -> Result { + value[key] + .as_f64() + .map(|v| v as f32) + .ok_or_else(|| SimulationError::OtherError(format!("Invalid {}", key))) +} +/// Represents an obstacle in the simulation +/// # Fields +/// * `position` - The position of the obstacle +/// * `velocity` - The velocity of the obstacle +/// * `radius` - The radius of the obstacle +#[derive(Clone)] +pub struct Obstacle { + pub position: Vector3, + pub velocity: Vector3, + pub radius: f32, +} + +impl Obstacle { + fn new(position: Vector3, velocity: Vector3, radius: f32) -> Self { + Self { + position, + velocity, + radius, + } + } +} +/// Represents a maze in the simulation +/// # Fields +/// * `lower_bounds` - The lower bounds of the maze +/// * `upper_bounds` - The upper bounds of the maze +/// * `obstacles` - The obstacles in the maze +pub struct Maze { + pub lower_bounds: Vector3, + pub upper_bounds: Vector3, + pub obstacles: Vec, +} +impl Maze { + /// Creates a new maze with the given bounds and number of obstacles + /// # Arguments + /// * `lower_bounds` - The lower bounds of the maze + /// * `upper_bounds` - The upper bounds of the maze + /// * `num_obstacles` - The number of obstacles in the maze + pub fn new( + lower_bounds: Vector3, + upper_bounds: Vector3, + num_obstacles: usize, + ) -> Self { + let mut maze = Maze { + lower_bounds, + upper_bounds, + obstacles: Vec::new(), + }; + maze.generate_obstacles(num_obstacles); + maze + } + /// Generates the obstacles in the maze + /// # Arguments + /// * `num_obstacles` - The number of obstacles to generate + pub fn generate_obstacles(&mut self, num_obstacles: usize) { + let mut rng = rand::thread_rng(); + self.obstacles = (0..num_obstacles) + .map(|_| { + let position = Vector3::new( + rand::Rng::gen_range(&mut rng, self.lower_bounds.x..self.upper_bounds.x), + rand::Rng::gen_range(&mut rng, self.lower_bounds.y..self.upper_bounds.y), + rand::Rng::gen_range(&mut rng, self.lower_bounds.z..self.upper_bounds.z), + ); + let velocity = Vector3::new( + rand::Rng::gen_range(&mut rng, -0.2..0.2), + rand::Rng::gen_range(&mut rng, -0.2..0.2), + rand::Rng::gen_range(&mut rng, -0.1..0.1), + ); + let radius = rand::Rng::gen_range(&mut rng, 0.05..0.1); + Obstacle::new(position, velocity, radius) + }) + .collect(); + } + /// Updates the obstacles in the maze, if an obstacle hits a boundary, it bounces off + /// # Arguments + /// * `dt` - The time step + pub fn update_obstacles(&mut self, dt: f32) { + self.obstacles.iter_mut().for_each(|obstacle| { + obstacle.position += obstacle.velocity * dt; + for i in 0..3 { + if obstacle.position[i] - obstacle.radius < self.lower_bounds[i] + || obstacle.position[i] + obstacle.radius > self.upper_bounds[i] + { + obstacle.velocity[i] *= -1.0; + } + } + }); + } +} +/// Represents a camera in the simulation which is used to render the depth of the scene +/// # Fields +/// * `resolution` - The resolution of the camera +/// * `fov` - The field of view of the camera +/// * `near` - The near clipping plane of the camera +/// * `far` - The far clipping plane of the camera +/// * `tan_half_fov` - The tangent of half the field of view +/// * `aspect_ratio` - The aspect ratio of the camera +pub struct Camera { + pub resolution: (usize, usize), + pub fov: f32, + pub near: f32, + pub far: f32, + pub aspect_ratio: f32, + pub ray_directions: Vec>, +} + +impl Camera { + /// Creates a new camera with the given resolution, field of view, near and far clipping planes + /// # Arguments + /// * `resolution` - The resolution of the camera + /// * `fov` - The field of view of the camera + /// * `near` - The near clipping plane of the camera + /// * `far` - The far clipping plane of the camera + pub fn new(resolution: (usize, usize), fov: f32, near: f32, far: f32) -> Self { + let (width, height) = resolution; + let (aspect_ratio, tan_half_fov) = (width as f32 / height as f32, (fov / 2.0).tan()); + let mut ray_directions = Vec::with_capacity(width * height); + for y in 0..height { + for x in 0..width { + let x_ndc = (2.0 * x as f32 / width as f32 - 1.0) * aspect_ratio * tan_half_fov; + let y_ndc = (1.0 - 2.0 * y as f32 / height as f32) * tan_half_fov; + ray_directions.push(Vector3::new(1.0, x_ndc, y_ndc).normalize()); + } + } + Self { + resolution, + fov, + near, + far, + aspect_ratio, + ray_directions, + } + } + /// Renders the depth of the scene from the perspective of the quadrotor + /// # Arguments + /// * `quad_position` - The position of the quadrotor + /// * `quad_orientation` - The orientation of the quadrotor + /// * `maze` - The maze in the scene + /// * `depth_buffer` - The depth buffer to store the depth values + pub fn render_depth( + &self, + quad_position: &Vector3, + quad_orientation: &UnitQuaternion, + maze: &Maze, + depth_buffer: &mut Vec, + ) -> Result<(), SimulationError> { + let (width, height) = self.resolution; + let total_pixels = width * height; + depth_buffer.clear(); + if depth_buffer.capacity() < total_pixels { + depth_buffer.reserve(total_pixels - depth_buffer.capacity()); + } + 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 { + let depth = self.ray_cast( + quad_position, + &rotation_world_to_camera, + &(rotation_camera_to_world * self.ray_directions[i]), + maze, + )?; + depth_buffer.push(depth); + } + Ok(()) + } + /// Casts a ray from the camera origin in the given direction + /// # Arguments + /// * `origin` - The origin of the ray + /// * `rotation_world_to_camera` - The rotation matrix from world to camera coordinates + /// * `direction` - The direction of the ray + /// * `maze` - The maze in the scene + /// # Returns + /// The distance to the closest obstacle hit by the ray + pub fn ray_cast( + &self, + origin: &Vector3, + rotation_world_to_camera: &Matrix3, + direction: &Vector3, + maze: &Maze, + ) -> Result { + let mut closest_hit = self.far; + // Inline tube intersection + for axis in 0..3 { + if direction[axis].abs() > f32::EPSILON { + for &bound in &[maze.lower_bounds[axis], maze.upper_bounds[axis]] { + let t = (bound - origin[axis]) / direction[axis]; + if t > self.near && t < closest_hit { + let intersection_point = origin + direction * t; + if (0..3).all(|i| { + i == axis + || (intersection_point[i] >= maze.lower_bounds[i] + && intersection_point[i] <= maze.upper_bounds[i]) + }) { + closest_hit = t; + } + } + } + } + } + // Early exit if we've hit a wall closer than any possible obstacle + if closest_hit <= self.near { + return Ok(std::f32::INFINITY); + } + // Inline sphere intersection + for obstacle in &maze.obstacles { + let oc = origin - &obstacle.position; + let b = oc.dot(direction); + let c = oc.dot(&oc) - obstacle.radius * obstacle.radius; + let discriminant = b * b - c; + if discriminant >= 0.0 { + let t = -b - discriminant.sqrt(); + if t > self.near && t < closest_hit { + closest_hit = t; + } + } + } + if closest_hit < self.far { + Ok((rotation_world_to_camera * direction * closest_hit).x) + } else { + Ok(std::f32::INFINITY) + } + } +} +/// Logs simulation data to the rerun recording stream +/// # Arguments +/// * `rec` - The rerun::RecordingStream instance +/// * `quad` - The Quadrotor instance +/// * `desired_position` - The desired position vector +/// * `measured_accel` - The measured acceleration vector +/// * `measured_gyro` - The measured angular velocity vector +pub fn log_data( + rec: &rerun::RecordingStream, + quad: &Quadrotor, + desired_position: &Vector3, + desired_velocity: &Vector3, + measured_accel: &Vector3, + measured_gyro: &Vector3, +) -> Result<(), SimulationError> { + rec.log( + "world/quad/desired_position", + &rerun::Points3D::new([(desired_position.x, desired_position.y, desired_position.z)]) + .with_radii([0.1]) + .with_colors([rerun::Color::from_rgb(255, 255, 255)]), + )?; + rec.log( + "world/quad/base_link", + &rerun::Transform3D::from_translation_rotation( + rerun::Vec3D::new(quad.position.x, quad.position.y, quad.position.z), + rerun::Quaternion::from_xyzw([ + quad.orientation.i, + quad.orientation.j, + quad.orientation.k, + quad.orientation.w, + ]), + ) + .with_axis_length(0.7), + )?; + let (quad_roll, quad_pitch, quad_yaw) = quad.orientation.euler_angles(); + let quad_euler_angles: Vector3 = Vector3::new(quad_roll, quad_pitch, quad_yaw); + for (pre, vec) in [ + ("position", &quad.position), + ("velocity", &quad.velocity), + ("accel", measured_accel), + ("orientation", &quad_euler_angles), + ("gyro", measured_gyro), + ("desired_position", desired_position), + ("desired_velocity", desired_velocity), + ] { + for (i, a) in ["x", "y", "z"].iter().enumerate() { + rec.log(format!("{}/{}", pre, a), &rerun::Scalar::new(vec[i] as f64))?; + } + } + Ok(()) +} +/// Log the maze tube to the rerun recording stream +/// # Arguments +/// * `rec` - The rerun::RecordingStream instance +/// * `maze` - The maze instance +pub fn log_maze_tube(rec: &rerun::RecordingStream, maze: &Maze) -> Result<(), SimulationError> { + let (lower_bounds, upper_bounds) = (maze.lower_bounds, maze.upper_bounds); + let center_position = rerun::external::glam::Vec3::new( + (lower_bounds.x + upper_bounds.x) / 2.0, + (lower_bounds.y + upper_bounds.y) / 2.0, + (lower_bounds.z + upper_bounds.z) / 2.0, + ); + let half_sizes = rerun::external::glam::Vec3::new( + (upper_bounds.x - lower_bounds.x) / 2.0, + (upper_bounds.y - lower_bounds.y) / 2.0, + (upper_bounds.z - lower_bounds.z) / 2.0, + ); + rec.log( + "world/maze/tube", + &rerun::Boxes3D::from_centers_and_half_sizes([center_position], [half_sizes]) + .with_colors([rerun::Color::from_rgb(128, 128, 255)]), + )?; + Ok(()) +} +/// Log the maze obstacles to the rerun recording stream +/// # Arguments +/// * `rec` - The rerun::RecordingStream instance +/// * `maze` - The maze instance +pub fn log_maze_obstacles( + rec: &rerun::RecordingStream, + maze: &Maze, +) -> Result<(), SimulationError> { + let (positions, radii): (Vec<_>, Vec<_>) = maze + .obstacles + .iter() + .map(|obstacle| { + ( + ( + obstacle.position.x, + obstacle.position.y, + obstacle.position.z, + ), + obstacle.radius, + ) + }) + .unzip(); + rec.log( + "world/maze/obstacles", + &rerun::Points3D::new(positions) + .with_radii(radii) + .with_colors([rerun::Color::from_rgb(255, 128, 128)]), + )?; + Ok(()) +} +/// A struct to hold trajectory data +/// # Fields +/// * `points` - A vector of 3D points +/// * `last_logged_point` - The last point that was logged +/// * `min_distance_threadhold` - The minimum distance between points to log +pub struct Trajectory { + points: Vec>, + last_logged_point: Vector3, + min_distance_threadhold: f32, +} + +impl Trajectory { + pub fn new(initial_point: Vector3) -> Self { + Self { + points: vec![initial_point], + last_logged_point: initial_point, + min_distance_threadhold: 0.05, + } + } + /// Add a point to the trajectory if it is further than the minimum distance threshold + /// # Arguments + /// * `point` - The point to add + /// # Returns + /// * `true` if the point was added, `false` otherwise + pub fn add_point(&mut self, point: Vector3) -> bool { + if (point - self.last_logged_point).norm() > self.min_distance_threadhold { + self.points.push(point); + self.last_logged_point = point; + true + } else { + false + } + } +} +/// log trajectory data to the rerun recording stream +/// # Arguments +/// * `rec` - The rerun::RecordingStream instance +/// * `trajectory` - The Trajectory instance +pub fn log_trajectory( + rec: &rerun::RecordingStream, + trajectory: &Trajectory, +) -> Result<(), SimulationError> { + let path = trajectory + .points + .iter() + .map(|p| (p.x, p.y, p.z)) + .collect::>(); + rec.log( + "world/quad/path", + &rerun::LineStrips3D::new([path]).with_colors([rerun::Color::from_rgb(0, 255, 255)]), + )?; + Ok(()) +} +/// log mesh data to the rerun recording stream +/// # Arguments +/// * `rec` - The rerun::RecordingStream instance +/// * `division` - The number of divisions in the mesh +/// * `spacing` - The spacing between divisions +pub fn log_mesh( + rec: &rerun::RecordingStream, + division: usize, + spacing: f32, +) -> Result<(), SimulationError> { + let grid_size: usize = division + 1; + let half_grid_size: f32 = (division as f32 * spacing) / 2.0; + let points: Vec = (0..grid_size) + .flat_map(|i| { + (0..grid_size).map(move |j| { + rerun::external::glam::Vec3::new( + j as f32 * spacing - half_grid_size, + i as f32 * spacing - half_grid_size, + 0.0, + ) + }) + }) + .collect(); + let horizontal_lines: Vec> = (0..grid_size) + .map(|i| points[i * grid_size..(i + 1) * grid_size].to_vec()) + .collect(); + let vertical_lines: Vec> = (0..grid_size) + .map(|j| (0..grid_size).map(|i| points[i * grid_size + j]).collect()) + .collect(); + let line_strips: Vec> = + horizontal_lines.into_iter().chain(vertical_lines).collect(); + rec.log( + "world/mesh", + &rerun::LineStrips3D::new(line_strips) + .with_colors([rerun::Color::from_rgb(255, 255, 255)]) + .with_radii([0.02]), + )?; + Ok(()) +} +/// log depth image data to the rerun recording stream +/// # Arguments +/// * `rec` - The rerun::RecordingStream instance +/// * `depth_image` - The depth image data +/// * `width` - The width of the depth image +/// * `height` - The height of the depth image +/// * `min_depth` - The minimum depth value +/// * `max_depth` - The maximum depth value +pub fn log_depth_image( + rec: &rerun::RecordingStream, + depth_image: &Vec, + width: usize, + height: usize, + min_depth: f32, + max_depth: f32, +) -> Result<(), SimulationError> { + let mut image = ndarray::Array::zeros((height, width, 3)); + let depth_range = max_depth - min_depth; + image + .axis_iter_mut(ndarray::Axis(0)) + .enumerate() + .for_each(|(y, mut row)| { + for (x, mut pixel) in row.axis_iter_mut(ndarray::Axis(0)).enumerate() { + let depth = depth_image[y * width + x]; + let color = if depth.is_finite() { + let normalized_depth = ((depth - min_depth) / depth_range).clamp(0.0, 1.0); + color_map_fn(normalized_depth * 255.0) + } else { + (0, 0, 0) + }; + (pixel[0], pixel[1], pixel[2]) = color; + } + }); + let rerun_image = rerun::Image::from_color_model_and_tensor(rerun::ColorModel::RGB, image) + .map_err(|e| SimulationError::OtherError(format!("Failed to create rerun image: {}", e)))?; + rec.log("world/quad/cam/depth", &rerun_image)?; + Ok(()) +} +/// turbo color map function +/// # Arguments +/// * `gray` - The gray value in the range [0, 255] +/// # Returns +/// * The RGB color value in the range [0, 255] +pub fn color_map_fn(gray: f32) -> (u8, u8, u8) { + let x = gray / 255.0; + let r = (34.61 + + x * (1172.33 - x * (10793.56 - x * (33300.12 - x * (38394.49 - x * 14825.05))))) + .clamp(0.0, 255.0) as u8; + let g = (23.31 + x * (557.33 + x * (1225.33 - x * (3574.96 - x * (1073.77 + x * 707.56))))) + .clamp(0.0, 255.0) as u8; + let b = (27.2 + x * (3211.1 - x * (15327.97 - x * (27814.0 - x * (22569.18 - x * 6838.66))))) + .clamp(0.0, 255.0) as u8; + (r, g, b) +} diff --git a/src/main.rs b/src/main.rs index d09fd45f..e7e3d32d 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,1501 +1,84 @@ -//! # Quadrotor Simulation -//! This crate provides a comprehensive simulation environment for quadrotor drones. -//! It includes models for quadrotor dynamics, IMU simulation, various trajectory planners, -//! and a PID controller for position and attitude control. -//! ## Features -//! - Realistic quadrotor dynamics simulation -//! - IMU sensor simulation with configurable noise parameters -//! - Multiple trajectory planners including hover, minimum jerk, Lissajous curves, and circular paths -//! - PID controller for position and attitude control -//! - Integration with the `rerun` crate for visualization -use nalgebra::{Matrix3, Rotation3, SMatrix, UnitQuaternion, Vector3}; -use rand_distr::{Distribution, Normal}; -use std::f32::consts::{FRAC_PI_2, PI}; -#[derive(thiserror::Error, Debug)] -enum SimulationError { - #[error("Rerun error: {0}")] - RerunError(#[from] rerun::RecordingStreamError), - #[error("Nalgebra error: {0}")] - NalgebraError(String), - #[error("Normal error: {0}")] - NormalError(#[from] rand_distr::NormalError), - #[error("Other error: {0}")] - OtherError(String), -} -/// Represents a quadrotor with its physical properties and state -struct Quadrotor { - /// Current position of the quadrotor in 3D space - position: Vector3, - /// Current velocity of the quadrotor - velocity: Vector3, - /// Current orientation of the quadrotor - orientation: UnitQuaternion, - /// Current angular velocity of the quadrotor - angular_velocity: Vector3, - /// Mass of the quadrotor in kg - mass: f32, - /// Gravitational acceleration in m/s^2 - gravity: f32, - /// Simulation time step in seconds - time_step: f32, - /// Drag coefficient - drag_coefficient: f32, - /// Inertia matrix of the quadrotor - inertia_matrix: Matrix3, - /// Inverse of the inertia matrix - inertia_matrix_inv: Matrix3, -} - -impl Quadrotor { - /// Creates a new Quadrotor with default parameters - /// * Arguments - /// * `time_step` - The simulation time step in seconds - pub fn new(time_step: f32) -> Result { - let inertia_matrix = Matrix3::new(3.04e-3, 0.0, 0.0, 0.0, 4.55e-3, 0.0, 0.0, 0.0, 2.82e-3); - let inertia_matrix_inv = - inertia_matrix - .try_inverse() - .ok_or(SimulationError::NalgebraError( - "Failed to invert inertia matrix".to_string(), - ))?; - Ok(Self { - position: Vector3::zeros(), - velocity: Vector3::zeros(), - orientation: UnitQuaternion::identity(), - angular_velocity: Vector3::zeros(), - mass: 1.3, - gravity: 9.81, - time_step, - drag_coefficient: 0.000, - inertia_matrix, - inertia_matrix_inv, - }) - } - /// Updates the quadrotor's dynamics with control inputs - /// # Arguments - /// * `control_thrust` - The total thrust force applied to the quadrotor - /// * `control_torque` - The 3D torque vector applied to the quadrotor - pub fn update_dynamics_with_controls( - &mut self, - control_thrust: f32, - control_torque: &Vector3, - ) { - let gravity_force = Vector3::new(0.0, 0.0, -self.mass * self.gravity); - let drag_force = -self.drag_coefficient * self.velocity.norm() * self.velocity; - let thrust_world = self.orientation * Vector3::new(0.0, 0.0, control_thrust); - let acceleration = (thrust_world + gravity_force + drag_force) / self.mass; - self.velocity += acceleration * self.time_step; - self.position += self.velocity * self.time_step; - let inertia_angular_velocity = self.inertia_matrix * self.angular_velocity; - let gyroscopic_torque = self.angular_velocity.cross(&inertia_angular_velocity); - let angular_acceleration = self.inertia_matrix_inv * (control_torque - gyroscopic_torque); - self.angular_velocity += angular_acceleration * self.time_step; - self.orientation *= - UnitQuaternion::from_scaled_axis(self.angular_velocity * self.time_step); - } - /// Simulates IMU readings with added noise - /// # Returns - /// A tuple containing the measured acceleration and angular velocity - pub fn read_imu(&self) -> Result<(Vector3, Vector3), SimulationError> { - let (accel_noise, gyro_noise) = (Normal::new(0.0, 0.02)?, Normal::new(0.0, 0.01)?); - let mut rng = rand::thread_rng(); - let gravity_world = Vector3::new(0.0, 0.0, self.gravity); - let specific_force = - self.orientation.inverse() * (self.velocity / self.time_step - gravity_world); - let measured_acceleration = specific_force - + Vector3::new( - accel_noise.sample(&mut rng), - accel_noise.sample(&mut rng), - accel_noise.sample(&mut rng), - ); - let measured_angular_velocity = self.angular_velocity - + Vector3::new( - gyro_noise.sample(&mut rng), - gyro_noise.sample(&mut rng), - gyro_noise.sample(&mut rng), - ); - Ok((measured_acceleration, measured_angular_velocity)) - } -} -/// Represents an Inertial Measurement Unit (IMU) with bias and noise characteristics -struct Imu { - /// Accelerometer bias - accel_bias: Vector3, - /// Gyroscope bias - gyro_bias: Vector3, - /// Standard deviation of accelerometer noise - accel_noise_std: f32, - /// Standard deviation of gyroscope noise - gyro_noise_std: f32, - /// Bias instability coefficient - bias_instability: f32, -} - -impl Imu { - /// Creates a new IMU with default parameters - pub fn new() -> Self { - Self { - accel_bias: Vector3::zeros(), - gyro_bias: Vector3::zeros(), - accel_noise_std: 0.02, - gyro_noise_std: 0.01, - bias_instability: 0.0001, - } - } - /// Updates the IMU biases over time - /// # Arguments - /// * `dt` - Time step for the update - pub fn update(&mut self, dt: f32) -> Result<(), SimulationError> { - let bias_drift = Normal::new(0.0, self.bias_instability * dt.sqrt())?; - let drift_vector = - || Vector3::from_iterator((0..3).map(|_| bias_drift.sample(&mut rand::thread_rng()))); - self.accel_bias += drift_vector(); - self.gyro_bias += drift_vector(); - Ok(()) - } - /// Simulates IMU readings with added bias and noise - /// # Arguments - /// * `true_acceleration` - The true acceleration vector - /// * `true_angular_velocity` - The true angular velocity vector - /// # Returns - /// A tuple containing the measured acceleration and angular velocity - pub fn read( - &self, - true_acceleration: Vector3, - true_angular_velocity: Vector3, - ) -> Result<(Vector3, Vector3), SimulationError> { - let mut rng = rand::thread_rng(); - let accel_noise = Normal::new(0.0, self.accel_noise_std)?; - let gyro_noise = Normal::new(0.0, self.gyro_noise_std)?; - let measured_acceleration = true_acceleration - + self.accel_bias - + Vector3::new( - accel_noise.sample(&mut rng), - accel_noise.sample(&mut rng), - accel_noise.sample(&mut rng), - ); - let measured_angular_velocity = true_angular_velocity - + self.gyro_bias - + Vector3::new( - gyro_noise.sample(&mut rng), - gyro_noise.sample(&mut rng), - gyro_noise.sample(&mut rng), - ); - Ok((measured_acceleration, measured_angular_velocity)) - } -} -/// PID controller for quadrotor position and attitude control -struct PIDController { - /// PID gain for position control including proportional, derivative, and integral gains - kpid_pos: [Vector3; 3], - /// PID gain for attitude control including proportional, derivative, and integral gains - kpid_att: [Vector3; 3], - /// Accumulated integral error for position - integral_pos_error: Vector3, - /// Accumulated integral error for attitude - integral_att_error: Vector3, - /// Maximum allowed integral error for position - max_integral_pos: Vector3, - /// Maximum allowed integral error for attitude - max_integral_att: Vector3, -} - -impl PIDController { - /// Creates a new PIDController with default gains - fn new() -> Self { - Self { - kpid_pos: [ - Vector3::new(7.1, 7.1, 11.9), // Proportional gain for position control - Vector3::new(2.4, 2.4, 6.7), // Derivative gain for position control - Vector3::zeros(), // Integral gain for position control - ], - kpid_att: [ - Vector3::new(1.5, 1.5, 1.0), // Proportional gain for attitude control - Vector3::new(0.13, 0.13, 0.1), // Derivative gain for attitude control - Vector3::zeros(), // Integral gain for attitude control - ], - integral_pos_error: Vector3::zeros(), - integral_att_error: Vector3::zeros(), - max_integral_pos: Vector3::new(10.0, 10.0, 10.0), - max_integral_att: Vector3::new(1.0, 1.0, 1.0), - } - } - /// Computes attitude control torques - /// # Arguments - /// * `desired_orientation` - The desired orientation quaternion - /// * `current_orientation` - The current orientation quaternion - /// * `current_angular_velocity` - The current angular velocity - /// * `dt` - Time step - /// # Returns - /// The computed control torque vector - fn compute_attitude_control( - &mut self, - desired_orientation: &UnitQuaternion, - current_orientation: &UnitQuaternion, - current_angular_velocity: &Vector3, - dt: f32, - ) -> Vector3 { - let error_orientation = current_orientation.inverse() * desired_orientation; - let (roll_error, pitch_error, yaw_error) = error_orientation.euler_angles(); - let error_angles = Vector3::new(roll_error, pitch_error, yaw_error); - self.integral_att_error += error_angles * dt; - self.integral_att_error = self - .integral_att_error - .zip_map(&self.max_integral_att, |int, max| int.clamp(-max, max)); - let error_angular_velocity = -current_angular_velocity; // TODO: Add desired angular velocity - self.kpid_att[0].component_mul(&error_angles) - + self.kpid_att[1].component_mul(&error_angular_velocity) - + self.kpid_att[2].component_mul(&self.integral_att_error) - } - /// Computes position control thrust and desired orientation - /// # Arguments - /// * `desired_position` - The desired position - /// * `desired_velocity` - The desired velocity - /// * `desired_yaw` - The desired yaw angle - /// * `current_position` - The current position - /// * `current_velocity` - The current velocity - /// * `dt` - Time step - /// * `mass` - Mass of the quadrotor - /// * `gravity` - Gravitational acceleration - /// # Returns - /// A tuple containing the computed thrust and desired orientation quaternion - fn compute_position_control( - &mut self, - desired_position: &Vector3, - desired_velocity: &Vector3, - desired_yaw: f32, - current_position: &Vector3, - current_velocity: &Vector3, - dt: f32, - mass: f32, - gravity: f32, - ) -> (f32, UnitQuaternion) { - let error_position = desired_position - current_position; - let error_velocity = desired_velocity - current_velocity; - self.integral_pos_error += error_position * dt; - self.integral_pos_error = self - .integral_pos_error - .zip_map(&self.max_integral_pos, |int, max| int.clamp(-max, max)); - let acceleration = self.kpid_pos[0].component_mul(&error_position) - + self.kpid_pos[1].component_mul(&error_velocity) - + self.kpid_pos[2].component_mul(&self.integral_pos_error); - let gravity_compensation = Vector3::new(0.0, 0.0, gravity); - let total_acceleration = acceleration + gravity_compensation; - let thrust = mass * total_acceleration.norm(); - let desired_orientation = if total_acceleration.norm() > 1e-6 { - let z_body = total_acceleration.normalize(); - 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(); - let x_body = y_body.cross(&z_body); - UnitQuaternion::from_rotation_matrix(&Rotation3::from_matrix_unchecked( - Matrix3::from_columns(&[x_body, y_body, z_body]), - )) - } else { - UnitQuaternion::from_euler_angles(0.0, 0.0, desired_yaw) - }; - (thrust, desired_orientation) - } -} -/// Enum representing different types of trajectory planners -enum PlannerType { - Hover(HoverPlanner), - MinimumJerkLine(MinimumJerkLinePlanner), - Lissajous(LissajousPlanner), - Circle(CirclePlanner), - Landing(LandingPlanner), - ObstacleAvoidance(ObstacleAvoidancePlanner), - MinimumSnapWaypoint(MinimumSnapWaypointPlanner), -} -impl PlannerType { - /// Plans the trajectory based on the current planner type - /// # Arguments - /// * `current_position` - The current position of the quadrotor - /// * `current_velocity` - The current velocity of the quadrotor - /// * `time` - The current simulation time - /// # Returns - /// A tuple containing the desired position, velocity, and yaw angle - fn plan( - &self, - current_position: Vector3, - current_velocity: Vector3, - time: f32, - ) -> (Vector3, Vector3, f32) { - match self { - PlannerType::Hover(p) => p.plan(current_position, current_velocity, time), - PlannerType::MinimumJerkLine(p) => p.plan(current_position, current_velocity, time), - PlannerType::Lissajous(p) => p.plan(current_position, current_velocity, time), - PlannerType::Circle(p) => p.plan(current_position, current_velocity, time), - PlannerType::Landing(p) => p.plan(current_position, current_velocity, time), - PlannerType::ObstacleAvoidance(p) => p.plan(current_position, current_velocity, time), - PlannerType::MinimumSnapWaypoint(p) => p.plan(current_position, current_velocity, time), - } - } - /// Checks if the current trajectory is finished - /// # Arguments - /// * `current_position` - The current position of the quadrotor - /// * `time` - The current simulation time - /// # Returns - /// `true` if the trajectory is finished, `false` otherwise - fn is_finished( - &self, - current_position: Vector3, - time: f32, - ) -> Result { - match self { - PlannerType::Hover(p) => Ok(p.is_finished(current_position, time)), - PlannerType::MinimumJerkLine(p) => Ok(p.is_finished(current_position, time)), - PlannerType::Lissajous(p) => Ok(p.is_finished(current_position, time)), - PlannerType::Circle(p) => Ok(p.is_finished(current_position, time)), - PlannerType::Landing(p) => Ok(p.is_finished(current_position, time)), - PlannerType::ObstacleAvoidance(p) => Ok(p.is_finished(current_position, time)), - PlannerType::MinimumSnapWaypoint(p) => p.is_finished(current_position, time), - } - } -} -/// Trait defining the interface for trajectory planners -trait Planner { - /// Plans the trajectory based on the current state and time - /// # Arguments - /// * `current_position` - The current position of the quadrotor - /// * `current_velocity` - The current velocity of the quadrotor - /// * `time` - The current simulation time - /// # Returns - /// A tuple containing the desired position, velocity, and yaw angle - fn plan( - &self, - current_position: Vector3, - current_velocity: Vector3, - time: f32, - ) -> (Vector3, Vector3, f32); - - /// Checks if the current trajectory is finished - /// # Arguments - /// * `current_position` - The current position of the quadrotor - /// * `time` - The current simulation time - /// # Returns - /// `true` if the trajectory is finished, `false` otherwise - fn is_finished(&self, current_position: Vector3, time: f32) -> bool; -} -/// Planner for hovering at a fixed position -struct HoverPlanner { - /// Target position for hovering - target_position: Vector3, - /// Target yaw angle for hovering - target_yaw: f32, -} - -impl Planner for HoverPlanner { - fn plan( - &self, - _current_position: Vector3, - _current_velocity: Vector3, - _time: f32, - ) -> (Vector3, Vector3, f32) { - (self.target_position, Vector3::zeros(), self.target_yaw) - } - - fn is_finished(&self, _current_position: Vector3, _time: f32) -> bool { - true // Hover planner is always "finished" as it's the default state - } -} -/// Planner for minimum jerk trajectories along a straight line -struct MinimumJerkLinePlanner { - /// Starting position of the trajectory - start_position: Vector3, - /// Ending position of the trajectory - end_position: Vector3, - /// Starting yaw angle - start_yaw: f32, - /// Ending yaw angle - end_yaw: f32, - /// Start time of the trajectory - start_time: f32, - /// Duration of the trajectory - duration: f32, -} - -impl Planner for MinimumJerkLinePlanner { - fn plan( - &self, - _current_position: Vector3, - _current_velocity: Vector3, - 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 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; - let yaw = self.start_yaw + (self.end_yaw - self.start_yaw) * s; - (position, velocity, yaw) - } - - fn is_finished(&self, _current_position: Vector3, _time: f32) -> bool { - (_current_position - self.end_position).norm() < 0.01 - && _time >= self.start_time + self.duration - } -} -/// Planner for Lissajous curve trajectories -struct LissajousPlanner { - /// Starting position of the trajectory - start_position: Vector3, - /// Center of the Lissajous curve - center: Vector3, - /// Amplitude of the Lissajous curve - amplitude: Vector3, - /// Frequency of the Lissajous curve - frequency: Vector3, - /// Phase of the Lissajous curve - phase: Vector3, - /// Start time of the trajectory - start_time: f32, - /// Duration of the trajectory - duration: f32, - /// Starting yaw angle - start_yaw: f32, - /// Ending yaw angle - end_yaw: f32, - /// Ramp-up time for smooth transitions - ramp_time: f32, -} - -impl Planner for LissajousPlanner { - fn plan( - &self, - _current_position: Vector3, - _current_velocity: Vector3, - time: f32, - ) -> (Vector3, Vector3, f32) { - let t = ((time - self.start_time) / self.duration).clamp(0.0, 1.0); - let smooth_start = if t < self.ramp_time / self.duration { - let t_ramp = t / (self.ramp_time / self.duration); - t_ramp * t_ramp * (3.0 - 2.0 * t_ramp) - } else { - 1.0 - }; - let velocity_ramp = if t < self.ramp_time / self.duration { - smooth_start - } else if t > 1.0 - self.ramp_time / self.duration { - let t_down = (1.0 - t) / (self.ramp_time / self.duration); - t_down * t_down * (3.0 - 2.0 * t_down) - } else { - 1.0 - }; - let ang_pos = self.frequency * t * 2.0 * PI + self.phase; - let lissajous = self.amplitude.component_mul(&ang_pos.map(f32::sin)); - let position = - self.start_position + smooth_start * ((self.center + lissajous) - self.start_position); - let mut velocity = Vector3::new( - self.amplitude.x * self.frequency.x * 2.0 * PI * ang_pos.x.cos(), - self.amplitude.y * self.frequency.y * 2.0 * PI * ang_pos.y.cos(), - self.amplitude.z * self.frequency.z * 2.0 * PI * ang_pos.z.cos(), - ) * velocity_ramp - / self.duration; - if t < self.ramp_time / self.duration { - let transition_velocity = (self.center - self.start_position) - * (2.0 * t / self.ramp_time - 2.0 * t * t / (self.ramp_time * self.ramp_time)) - / self.duration; - velocity += transition_velocity; - } - let yaw = self.start_yaw + (self.end_yaw - self.start_yaw) * t; - (position, velocity, yaw) - } - - fn is_finished(&self, _current_position: Vector3, time: f32) -> bool { - time >= self.start_time + self.duration - } -} -/// Planner for circular trajectories -struct CirclePlanner { - /// Center of the circular trajectory - center: Vector3, - /// Radius of the circular trajectory - radius: f32, - /// Angular velocity of the circular motion - angular_velocity: f32, - /// Starting position of the trajectory - start_position: Vector3, - /// Start time of the trajectory - start_time: f32, - /// Duration of the trajectory - duration: f32, - /// Starting yaw angle - start_yaw: f32, - /// Ending yaw angle - end_yaw: f32, - /// Ramp-up time for smooth transitions - ramp_time: f32, -} - -impl Planner for CirclePlanner { - fn plan( - &self, - _current_position: Vector3, - _current_velocity: Vector3, - time: f32, - ) -> (Vector3, Vector3, f32) { - let t = (time - self.start_time) / self.duration; - let t = t.clamp(0.0, 1.0); - let smooth_start = if t < self.ramp_time / self.duration { - let t_ramp = t / (self.ramp_time / self.duration); - t_ramp * t_ramp * (3.0 - 2.0 * t_ramp) - } else { - 1.0 - }; - let velocity_ramp = if t < self.ramp_time / self.duration { - smooth_start - } else if t > 1.0 - self.ramp_time / self.duration { - let t_down = (1.0 - t) / (self.ramp_time / self.duration); - t_down * t_down * (3.0 - 2.0 * t_down) - } else { - 1.0 - }; - let angle = self.angular_velocity * t * self.duration; - let circle_offset = Vector3::new(self.radius * angle.cos(), self.radius * angle.sin(), 0.0); - let position = self.start_position - + smooth_start * ((self.center + circle_offset) - self.start_position); - let tangential_velocity = Vector3::new( - -self.radius * self.angular_velocity * angle.sin(), - self.radius * self.angular_velocity * angle.cos(), - 0.0, - ); - let velocity = tangential_velocity * velocity_ramp; - let yaw = self.start_yaw + (self.end_yaw - self.start_yaw) * t; - (position, velocity, yaw) - } - - fn is_finished(&self, _current_position: Vector3, time: f32) -> bool { - time >= self.start_time + self.duration - } -} - -/// Planner for landing maneuvers -struct LandingPlanner { - /// Starting position of the landing maneuver - start_position: Vector3, - /// Start time of the landing maneuver - start_time: f32, - /// Duration of the landing maneuver - duration: f32, - /// Starting yaw angle - start_yaw: f32, -} - -impl Planner for LandingPlanner { - fn plan( - &self, - _current_position: Vector3, - _current_velocity: Vector3, - time: f32, - ) -> (Vector3, Vector3, f32) { - let t = ((time - self.start_time) / self.duration).clamp(0.0, 1.0); - let target_z = self.start_position.z * (1.0 - t); - let target_position = Vector3::new(self.start_position.x, self.start_position.y, target_z); - let target_velocity = Vector3::new(0.0, 0.0, -self.start_position.z / self.duration); - (target_position, target_velocity, self.start_yaw) - } - - fn is_finished(&self, current_position: Vector3, time: f32) -> bool { - current_position.z < 0.05 || time >= self.start_time + self.duration - } -} -/// Manages different trajectory planners and switches between them -struct PlannerManager { - current_planner: PlannerType, -} - -impl PlannerManager { - /// Creates a new PlannerManager with an initial hover planner - /// # Arguments - /// * `initial_position` - The initial position for hovering - /// * `initial_yaw` - The initial yaw angle for hovering - /// # Returns - /// A new PlannerManager instance - fn new(initial_position: Vector3, initial_yaw: f32) -> Self { - let hover_planner = HoverPlanner { - target_position: initial_position, - target_yaw: initial_yaw, - }; - Self { - current_planner: PlannerType::Hover(hover_planner), - } - } - /// Sets a new planner - /// # Arguments - /// * `new_planner` - The new planner to be set - fn set_planner(&mut self, new_planner: PlannerType) { - self.current_planner = new_planner; - } - /// Updates the current planner and returns the desired position, velocity, and yaw - /// # Arguments - /// * `current_position` - The current position of the quadrotor - /// * `current_orientation` - The current orientation of the quadrotor - /// * `current_velocity` - The current velocity of the quadrotor - /// * `time` - The current simulation time - /// # Returns - /// A tuple containing the desired position, velocity, and yaw angle - fn update( - &mut self, - current_position: Vector3, - current_orientation: UnitQuaternion, - current_velocity: Vector3, - time: f32, - obstacles: &Vec, - ) -> Result<(Vector3, Vector3, f32), SimulationError> { - if self.current_planner.is_finished(current_position, time)? { - self.current_planner = PlannerType::Hover(HoverPlanner { - target_position: current_position, - target_yaw: current_orientation.euler_angles().2, - }); - } - // Update obstacles for ObstacleAvoidancePlanner if needed - if let PlannerType::ObstacleAvoidance(ref mut planner) = self.current_planner { - planner.obstacles = obstacles.clone(); - } - Ok(self - .current_planner - .plan(current_position, current_velocity, time)) - } -} -/// Obstacle avoidance planner that uses a potential field approach to avoid obstacles -/// The planner calculates a repulsive force for each obstacle and an attractive force towards the goal -/// The resulting force is then used to calculate the desired position and velocity -struct ObstacleAvoidancePlanner { - /// Target position of the planner - target_position: Vector3, - /// Start time of the planner - start_time: f32, - /// Duration of the planner - duration: f32, - /// Starting yaw angle - start_yaw: f32, - /// Ending yaw angle - end_yaw: f32, - /// List of obstacles - obstacles: Vec, - /// Attractive force gain - k_att: f32, - /// Repulsive force gain - k_rep: f32, - /// Influence distance of obstacles - d0: f32, -} - -impl Planner for ObstacleAvoidancePlanner { - fn plan( - &self, - current_position: Vector3, - _current_velocity: Vector3, - time: f32, - ) -> (Vector3, Vector3, f32) { - let t = ((time - self.start_time) / self.duration).clamp(0.0, 1.0); - // Attractive force towards the goal - let f_att = self.k_att * (self.target_position - current_position); - // Repulsive force from obstacles - let mut f_rep = Vector3::zeros(); - for obstacle in &self.obstacles { - let diff = current_position - obstacle.position; - let distance = diff.norm(); - if distance < self.d0 { - f_rep += self.k_rep - * (1.0 / distance - 1.0 / self.d0) - * (1.0 / distance.powi(2)) - * diff.normalize(); - } - } - let f_total = f_att + f_rep; - let max_speed: f32 = 1.0; - let desired_velocity = f_total.normalize() * max_speed.min(f_total.norm()); - let desired_position = current_position + desired_velocity * self.duration * (1.0 - t); - let desired_yaw = self.start_yaw + (self.end_yaw - self.start_yaw) * t; - (desired_position, desired_velocity, desired_yaw) - } +use nalgebra::Vector3; +use std::env; - fn is_finished(&self, current_position: Vector3, time: f32) -> bool { - (current_position - self.target_position).norm() < 0.1 - && time >= self.start_time + self.duration - } -} - -/// Waypoint planner that generates a minimum snap trajectory between waypoints -/// # Arguments -/// * `waypoints` - A list of waypoints to follow -/// * `yaws` - A list of yaw angles at each waypoint -/// * `segment_times` - A list of times to reach each waypoint -/// * `start_time` - The start time of the trajectory -struct MinimumSnapWaypointPlanner { - waypoints: Vec>, - yaws: Vec, - times: Vec, - coefficients: Vec>>, - yaw_coefficients: Vec>, - start_time: f32, -} - -impl MinimumSnapWaypointPlanner { - fn new( - waypoints: Vec>, - yaws: Vec, - segment_times: Vec, - start_time: f32, - ) -> Result { - if waypoints.len() < 2 { - return Err(SimulationError::OtherError( - "At least two waypoints are required".to_string(), - )); - } - if waypoints.len() != segment_times.len() + 1 || waypoints.len() != yaws.len() { - return Err(SimulationError::OtherError("Number of segment times must be one less than number of waypoints, and yaws must match waypoints".to_string())); - } - let mut planner = Self { - waypoints, - yaws, - times: segment_times, - coefficients: Vec::new(), - yaw_coefficients: Vec::new(), - start_time, - }; - planner.compute_minimum_snap_trajectories()?; - planner.compute_minimum_acceleration_yaw_trajectories()?; - Ok(planner) - } - /// Compute the coefficients for the minimum snap trajectory, calculated for each segment between waypoints - fn compute_minimum_snap_trajectories(&mut self) -> Result<(), SimulationError> { - let n = self.waypoints.len() - 1; - for i in 0..n { - let duration = self.times[i]; - let (start, end) = (self.waypoints[i], self.waypoints[i + 1]); - let mut a = SMatrix::::zeros(); - let mut b_x = SMatrix::::zeros(); - let mut b_y = SMatrix::::zeros(); - let mut b_z = SMatrix::::zeros(); - // Start point constraints - (a[(0, 0)], a[(1, 1)], a[(2, 2)], a[(3, 3)]) = (1.0, 1.0, 2.0, 6.0); - (b_x[0], b_x[4], b_y[0], b_y[4], b_z[0], b_z[4]) = - (start.x, end.x, start.y, end.y, start.z, end.z); - // End point constraints - for j in 0..8 { - a[(4, j)] = duration.powi(j as i32); - if j > 0 { - a[(5, j)] = j as f32 * duration.powi(j as i32 - 1); - } - if j > 1 { - a[(6, j)] = j as f32 * (j - 1) as f32 * duration.powi(j as i32 - 2); - } - if j > 2 { - a[(7, j)] = - j as f32 * (j - 1) as f32 * (j - 2) as f32 * duration.powi(j as i32 - 3); - } - } - let x_coeffs = a.lu().solve(&b_x).ok_or(SimulationError::NalgebraError( - "Failed to solve for x coefficients in MinimumSnapWaypointPlanner".to_string(), - ))?; - let y_coeffs = a.lu().solve(&b_y).ok_or(SimulationError::NalgebraError( - "Failed to solve for y coefficients in MinimumSnapWaypointPlanner".to_string(), - ))?; - let z_coeffs = a.lu().solve(&b_z).ok_or(SimulationError::NalgebraError( - "Failed to solve for z coefficients in MinimumSnapWaypointPlanner".to_string(), - ))?; - self.coefficients.push(vec![ - Vector3::new(x_coeffs[0], y_coeffs[0], z_coeffs[0]), - Vector3::new(x_coeffs[1], y_coeffs[1], z_coeffs[1]), - Vector3::new(x_coeffs[2], y_coeffs[2], z_coeffs[2]), - Vector3::new(x_coeffs[3], y_coeffs[3], z_coeffs[3]), - Vector3::new(x_coeffs[4], y_coeffs[4], z_coeffs[4]), - Vector3::new(x_coeffs[5], y_coeffs[5], z_coeffs[5]), - Vector3::new(x_coeffs[6], y_coeffs[6], z_coeffs[6]), - Vector3::new(x_coeffs[7], y_coeffs[7], z_coeffs[7]), - ]); - } - Ok(()) - } - /// Compute the coefficients for yaw trajectories - /// The yaw trajectory is a cubic polynomial and interpolated between waypoints - fn compute_minimum_acceleration_yaw_trajectories(&mut self) -> Result<(), SimulationError> { - let n = self.yaws.len() - 1; // Number of segments - for i in 0..n { - let duration = self.times[i]; - let start_yaw = self.yaws[i]; - let end_yaw = self.yaws[i + 1]; - let mut a = SMatrix::::zeros(); - let mut b = SMatrix::::zeros(); - // Start point constraints - a[(0, 0)] = 1.0; - a[(1, 1)] = 1.0; - b[0] = start_yaw; - // End point constraints - for j in 0..4 { - a[(2, j)] = duration.powi(j as i32); - if j > 0 { - a[(3, j)] = j as f32 * duration.powi(j as i32 - 1); - } - } - b[2] = end_yaw; - let yaw_coeffs = a.lu().solve(&b).ok_or(SimulationError::NalgebraError( - "Failed to solve for yaw coefficients in MinimumSnapWaypointPlanner".to_string(), - ))?; - self.yaw_coefficients.push(yaw_coeffs.as_slice().to_vec()); - } - Ok(()) - } - /// Evaluate the trajectory at a given time, returns the position, velocity, yaw, and yaw rate at the given time - /// # Arguments - /// * `t` - The time to evaluate the trajectory at - /// * `coeffs` - The coefficients for the position trajectory - /// * `yaw_coeffs` - The coefficients for the yaw trajectory - /// # Returns - /// * `position` - The position at the given time (meters) - /// * `velocity` - The velocity at the given time (meters / second) - /// * `yaw` - The yaw at the given time (radians) - /// * `yaw_rate` - The yaw rate at the given time (radians / second) - fn evaluate_polynomial( - &self, - t: f32, - coeffs: &[Vector3], - yaw_coeffs: &[f32], - ) -> (Vector3, Vector3, f32, f32) { - let mut position = Vector3::zeros(); - let mut velocity = Vector3::zeros(); - let mut yaw = 0.0; - let mut yaw_rate = 0.0; - for (i, coeff) in coeffs.iter().enumerate() { - let ti = t.powi(i as i32); - position += coeff * ti; - if i > 0 { - velocity += coeff * (i as f32) * t.powi(i as i32 - 1); - } - } - for (i, &coeff) in yaw_coeffs.iter().enumerate() { - let ti = t.powi(i as i32); - yaw += coeff * ti; - if i > 0 { - yaw_rate += coeff * (i as f32) * t.powi(i as i32 - 1); - } - } - (position, velocity, yaw, yaw_rate) - } - fn plan( - &self, - _current_position: Vector3, - _current_velocity: Vector3, - time: f32, - ) -> (Vector3, Vector3, f32) { - let relative_time = time - self.start_time; - // Find the current segment - let mut segment_start_time = 0.0; - let mut current_segment = 0; - for (i, &segment_duration) in self.times.iter().enumerate() { - if relative_time < segment_start_time + segment_duration { - current_segment = i; - break; - } - segment_start_time += segment_duration; - } - // Evaluate the polynomial for the current segment - let segment_time = relative_time - segment_start_time; - let (position, velocity, yaw, _yaw_rate) = self.evaluate_polynomial( - segment_time, - &self.coefficients[current_segment], - &self.yaw_coefficients[current_segment], - ); - (position, velocity, yaw) - } - - fn is_finished( - &self, - current_position: Vector3, - time: f32, - ) -> Result { - let last_waypoint = self.waypoints.last().ok_or(SimulationError::OtherError( - "No waypoints available".to_string(), - ))?; - Ok(time >= self.start_time + self.times.iter().sum::() - && (current_position - last_waypoint).norm() < 0.1) - } -} -/// Updates the planner based on the current simulation step -/// # Arguments -/// * `planner_manager` - The PlannerManager instance to update -/// * `step` - The current simulation step -/// * `time` - The current simulation time -/// * `quad` - The Quadrotor instance -fn update_planner( - planner_manager: &mut PlannerManager, - step: usize, - time: f32, - quad: &Quadrotor, - obstacles: &Vec, -) { - let step = (step as f32 * 100.0 * quad.time_step) as i32; - let current_yaw = quad.orientation.euler_angles().2; - match step { - 100 => planner_manager.set_planner(PlannerType::MinimumJerkLine(MinimumJerkLinePlanner { - start_position: quad.position, - end_position: Vector3::new(0.0, 0.0, 1.0), - start_yaw: current_yaw, - end_yaw: 0.0, - start_time: time, - duration: 2.5, - })), - 500 => planner_manager.set_planner(PlannerType::Lissajous(LissajousPlanner { - start_position: quad.position, - center: Vector3::new(0.5, 0.5, 1.0), - amplitude: Vector3::new(0.5, 0.5, 0.2), - frequency: Vector3::new(1.0, 2.0, 3.0), - phase: Vector3::new(0.0, FRAC_PI_2, 0.0), - start_time: time, - duration: 20.0, - start_yaw: current_yaw, - end_yaw: current_yaw + 2.0 * PI, - ramp_time: 5.0, - })), - 2700 => planner_manager.set_planner(PlannerType::Circle(CirclePlanner { - center: Vector3::new(0.5, 0.5, 1.0), - radius: 0.5, - angular_velocity: 1.0, - start_position: quad.position, - start_time: time, - duration: 8.0, - start_yaw: current_yaw, - end_yaw: current_yaw, - ramp_time: 2.0, - })), - 3700 => { - planner_manager.set_planner(PlannerType::ObstacleAvoidance(ObstacleAvoidancePlanner { - target_position: Vector3::new(1.5, 1.0, 0.5), - start_time: time, - duration: 10.0, - start_yaw: current_yaw, - end_yaw: 0.0, - obstacles: obstacles.clone(), - k_att: 0.03, - k_rep: 0.02, - d0: 0.5, - })) - } - 4500 => { - let waypoints = vec![ - quad.position, - Vector3::new(1.0, 1.0, 1.5), - Vector3::new(-1.0, 1.0, 1.75), - Vector3::new(0.0, -1.0, 1.0), - Vector3::new(0.0, 0.0, 0.5), - ]; - let yaws = vec![current_yaw, FRAC_PI_2, PI, -FRAC_PI_2, 0.0]; - let segment_times = vec![5.0, 5.0, 5.0, 5.0]; - match MinimumSnapWaypointPlanner::new(waypoints, yaws, segment_times, time) { - Ok(planner) => { - planner_manager.set_planner(PlannerType::MinimumSnapWaypoint(planner)) - } - Err(e) => println!("Error creating MinimumSnapWaypointPlanner: {}", e), - } - } - 6500 => planner_manager.set_planner(PlannerType::Landing(LandingPlanner { - start_position: quad.position, - start_time: time, - duration: 5.0, - start_yaw: current_yaw, - })), - _ => {} - } -} -/// Represents an obstacle in the simulation -/// # Fields -/// * `position` - The position of the obstacle -/// * `velocity` - The velocity of the obstacle -/// * `radius` - The radius of the obstacle -#[derive(Clone)] -struct Obstacle { - position: Vector3, - velocity: Vector3, - radius: f32, -} - -impl Obstacle { - fn new(position: Vector3, velocity: Vector3, radius: f32) -> Self { - Self { - position, - velocity, - radius, - } - } -} -/// Represents a maze in the simulation -/// # Fields -/// * `lower_bounds` - The lower bounds of the maze -/// * `upper_bounds` - The upper bounds of the maze -/// * `obstacles` - The obstacles in the maze -struct Maze { - lower_bounds: Vector3, - upper_bounds: Vector3, - obstacles: Vec, -} -impl Maze { - /// Creates a new maze with the given bounds and number of obstacles - /// # Arguments - /// * `lower_bounds` - The lower bounds of the maze - /// * `upper_bounds` - The upper bounds of the maze - /// * `num_obstacles` - The number of obstacles in the maze - fn new(lower_bounds: Vector3, upper_bounds: Vector3, num_obstacles: usize) -> Self { - let mut maze = Maze { - lower_bounds, - upper_bounds, - obstacles: Vec::new(), - }; - maze.generate_obstacles(num_obstacles); - maze - } - /// Generates the obstacles in the maze - /// # Arguments - /// * `num_obstacles` - The number of obstacles to generate - fn generate_obstacles(&mut self, num_obstacles: usize) { - let mut rng = rand::thread_rng(); - self.obstacles = (0..num_obstacles) - .map(|_| { - let position = Vector3::new( - rand::Rng::gen_range(&mut rng, self.lower_bounds.x..self.upper_bounds.x), - rand::Rng::gen_range(&mut rng, self.lower_bounds.y..self.upper_bounds.y), - rand::Rng::gen_range(&mut rng, self.lower_bounds.z..self.upper_bounds.z), - ); - let velocity = Vector3::new( - rand::Rng::gen_range(&mut rng, -0.2..0.2), - rand::Rng::gen_range(&mut rng, -0.2..0.2), - rand::Rng::gen_range(&mut rng, -0.1..0.1), - ); - let radius = rand::Rng::gen_range(&mut rng, 0.05..0.1); - Obstacle::new(position, velocity, radius) - }) - .collect(); - } - /// Updates the obstacles in the maze, if an obstacle hits a boundary, it bounces off - /// # Arguments - /// * `dt` - The time step - fn update_obstacles(&mut self, dt: f32) { - self.obstacles.iter_mut().for_each(|obstacle| { - obstacle.position += obstacle.velocity * dt; - for i in 0..3 { - if obstacle.position[i] - obstacle.radius < self.lower_bounds[i] - || obstacle.position[i] + obstacle.radius > self.upper_bounds[i] - { - obstacle.velocity[i] *= -1.0; - } - } - }); - } -} -/// Represents a camera in the simulation which is used to render the depth of the scene -/// # Fields -/// * `resolution` - The resolution of the camera -/// * `fov` - The field of view of the camera -/// * `near` - The near clipping plane of the camera -/// * `far` - The far clipping plane of the camera -/// * `tan_half_fov` - The tangent of half the field of view -/// * `aspect_ratio` - The aspect ratio of the camera -#[allow(dead_code)] -struct Camera { - resolution: (usize, usize), - fov: f32, - near: f32, - far: f32, - aspect_ratio: f32, - ray_directions: Vec>, -} - -impl Camera { - /// Creates a new camera with the given resolution, field of view, near and far clipping planes - /// # Arguments - /// * `resolution` - The resolution of the camera - /// * `fov` - The field of view of the camera - /// * `near` - The near clipping plane of the camera - /// * `far` - The far clipping plane of the camera - fn new(resolution: (usize, usize), fov: f32, near: f32, far: f32) -> Self { - let (width, height) = resolution; - let (aspect_ratio, tan_half_fov) = (width as f32 / height as f32, (fov / 2.0).tan()); - let mut ray_directions = Vec::with_capacity(width * height); - for y in 0..height { - for x in 0..width { - let x_ndc = (2.0 * x as f32 / width as f32 - 1.0) * aspect_ratio * tan_half_fov; - let y_ndc = (1.0 - 2.0 * y as f32 / height as f32) * tan_half_fov; - ray_directions.push(Vector3::new(1.0, x_ndc, y_ndc).normalize()); - } - } - Self { - resolution, - fov, - near, - far, - aspect_ratio, - ray_directions, - } - } - /// Renders the depth of the scene from the perspective of the quadrotor - /// # Arguments - /// * `quad_position` - The position of the quadrotor - /// * `quad_orientation` - The orientation of the quadrotor - /// * `maze` - The maze in the scene - /// * `depth_buffer` - The depth buffer to store the depth values - fn render_depth( - &self, - quad_position: &Vector3, - quad_orientation: &UnitQuaternion, - maze: &Maze, - depth_buffer: &mut Vec, - ) -> Result<(), SimulationError> { - let (width, height) = self.resolution; - let total_pixels = width * height; - depth_buffer.clear(); - if depth_buffer.capacity() < total_pixels { - depth_buffer.reserve(total_pixels - depth_buffer.capacity()); - } - 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 { - let depth = self.ray_cast( - quad_position, - &rotation_world_to_camera, - &(rotation_camera_to_world * self.ray_directions[i]), - maze, - )?; - depth_buffer.push(depth); - } - Ok(()) - } - /// Casts a ray from the camera origin in the given direction - /// # Arguments - /// * `origin` - The origin of the ray - /// * `rotation_world_to_camera` - The rotation matrix from world to camera coordinates - /// * `direction` - The direction of the ray - /// * `maze` - The maze in the scene - /// # Returns - /// The distance to the closest obstacle hit by the ray - fn ray_cast( - &self, - origin: &Vector3, - rotation_world_to_camera: &Matrix3, - direction: &Vector3, - maze: &Maze, - ) -> Result { - let mut closest_hit = self.far; - // Inline tube intersection - for axis in 0..3 { - if direction[axis].abs() > f32::EPSILON { - for &bound in &[maze.lower_bounds[axis], maze.upper_bounds[axis]] { - let t = (bound - origin[axis]) / direction[axis]; - if t > self.near && t < closest_hit { - let intersection_point = origin + direction * t; - if (0..3).all(|i| { - i == axis - || (intersection_point[i] >= maze.lower_bounds[i] - && intersection_point[i] <= maze.upper_bounds[i]) - }) { - closest_hit = t; - } - } - } - } - } - // Early exit if we've hit a wall closer than any possible obstacle - if closest_hit <= self.near { - return Ok(std::f32::INFINITY); - } - // Inline sphere intersection - for obstacle in &maze.obstacles { - let oc = origin - &obstacle.position; - let b = oc.dot(direction); - let c = oc.dot(&oc) - obstacle.radius * obstacle.radius; - let discriminant = b * b - c; - if discriminant >= 0.0 { - let t = -b - discriminant.sqrt(); - if t > self.near && t < closest_hit { - closest_hit = t; - } - } - } - if closest_hit < self.far { - let closest_pt = rotation_world_to_camera * direction * closest_hit; - Ok(closest_pt.x) - } else { - Ok(std::f32::INFINITY) - } - } -} -/// Logs simulation data to the rerun recording stream -/// # Arguments -/// * `rec` - The rerun::RecordingStream instance -/// * `quad` - The Quadrotor instance -/// * `desired_position` - The desired position vector -/// * `measured_accel` - The measured acceleration vector -/// * `measured_gyro` - The measured angular velocity vector -fn log_data( - rec: &rerun::RecordingStream, - quad: &Quadrotor, - desired_position: &Vector3, - desired_velocity: &Vector3, - measured_accel: &Vector3, - measured_gyro: &Vector3, -) -> Result<(), SimulationError> { - rec.log( - "world/quad/desired_position", - &rerun::Points3D::new([(desired_position.x, desired_position.y, desired_position.z)]) - .with_radii([0.1]) - .with_colors([rerun::Color::from_rgb(255, 255, 255)]), - )?; - rec.log( - "world/quad/base_link", - &rerun::Transform3D::from_translation_rotation( - rerun::Vec3D::new(quad.position.x, quad.position.y, quad.position.z), - rerun::Quaternion::from_xyzw([ - quad.orientation.i, - quad.orientation.j, - quad.orientation.k, - quad.orientation.w, - ]), - ) - .with_axis_length(0.7), +mod config; +use config::Config; +use peng_quad::*; +/// Main function to run the quadrotor simulation +fn main() -> Result<(), SimulationError> { + let args: Vec = env::args().collect(); + if args.len() != 2 { + return Err(SimulationError::OtherError(format!( + "Usage: {} ", + args[0] + ))); + } + let config = Config::from_yaml(&args[1]).expect("Failed to load configuration"); + let mut quad = Quadrotor::new( + 1.0 / config.simulation.simulation_frequency, + config.quadrotor.mass, + config.quadrotor.gravity, + config.quadrotor.drag_coefficient, + config.quadrotor.inertia_matrix, )?; - let (quad_roll, quad_pitch, quad_yaw) = quad.orientation.euler_angles(); - let quad_euler_angles: Vector3 = Vector3::new(quad_roll, quad_pitch, quad_yaw); - for (prefix, vector) in [ - ("position", &quad.position), - ("velocity", &quad.velocity), - ("accel", measured_accel), - ("orientation", &quad_euler_angles), - ("gyro", measured_gyro), - ("desired_position", desired_position), - ("desired_velocity", desired_velocity), - ] { - for (i, axis) in ["x", "y", "z"].iter().enumerate() { - let name = format!("{}/{}", prefix, axis); - rec.log(name, &rerun::Scalar::new(vector[i] as f64))?; - } - } - Ok(()) -} -/// Log the maze tube to the rerun recording stream -/// # Arguments -/// * `rec` - The rerun::RecordingStream instance -/// * `maze` - The maze instance -fn log_maze_tube(rec: &rerun::RecordingStream, maze: &Maze) -> Result<(), SimulationError> { - let (lower_bounds, upper_bounds) = (maze.lower_bounds, maze.upper_bounds); - let center_position = rerun::external::glam::Vec3::new( - (lower_bounds.x + upper_bounds.x) / 2.0, - (lower_bounds.y + upper_bounds.y) / 2.0, - (lower_bounds.z + upper_bounds.z) / 2.0, + let _pos_gains = config.controller.pos_gains; + let _att_gains = config.controller.att_gains; + let mut controller = PIDController::new( + [_pos_gains.kp, _pos_gains.kd, _pos_gains.ki], + [_att_gains.kp, _att_gains.kd, _att_gains.ki], + config.controller.pos_max_int, + config.controller.att_max_int, ); - let half_sizes = rerun::external::glam::Vec3::new( - (upper_bounds.x - lower_bounds.x) / 2.0, - (upper_bounds.y - lower_bounds.y) / 2.0, - (upper_bounds.z - lower_bounds.z) / 2.0, + let mut imu = Imu::new( + config.imu.accel_noise_std, + config.imu.gyro_noise_std, + config.imu.bias_instability, ); - rec.log( - "world/maze/tube", - &rerun::Boxes3D::from_centers_and_half_sizes([center_position], [half_sizes]) - .with_colors([rerun::Color::from_rgb(128, 128, 255)]), - )?; - Ok(()) -} -/// Log the maze obstacles to the rerun recording stream -/// # Arguments -/// * `rec` - The rerun::RecordingStream instance -/// * `maze` - The maze instance -fn log_maze_obstacles(rec: &rerun::RecordingStream, maze: &Maze) -> Result<(), SimulationError> { - let (positions, radii): (Vec<_>, Vec<_>) = maze - .obstacles - .iter() - .map(|obstacle| { - ( - ( - obstacle.position.x, - obstacle.position.y, - obstacle.position.z, - ), - obstacle.radius, - ) - }) - .unzip(); - rec.log( - "world/maze/obstacles", - &rerun::Points3D::new(positions) - .with_radii(radii) - .with_colors([rerun::Color::from_rgb(255, 128, 128)]), - )?; - Ok(()) -} -/// A struct to hold trajectory data -/// # Fields -/// * `points` - A vector of 3D points -/// * `last_logged_point` - The last point that was logged -/// * `min_distance_threadhold` - The minimum distance between points to log -struct Trajectory { - points: Vec>, - last_logged_point: Vector3, - min_distance_threadhold: f32, -} - -impl Trajectory { - fn new(initial_point: Vector3) -> Self { - Self { - points: vec![initial_point], - last_logged_point: initial_point, - min_distance_threadhold: 0.05, - } - } - /// Add a point to the trajectory if it is further than the minimum distance threshold - /// # Arguments - /// * `point` - The point to add - /// # Returns - /// * `true` if the point was added, `false` otherwise - fn add_point(&mut self, point: Vector3) -> bool { - if (point - self.last_logged_point).norm() > self.min_distance_threadhold { - self.points.push(point); - self.last_logged_point = point; - true - } else { - false - } - } -} -/// log trajectory data to the rerun recording stream -/// # Arguments -/// * `rec` - The rerun::RecordingStream instance -/// * `trajectory` - The Trajectory instance -fn log_trajectory( - rec: &rerun::RecordingStream, - trajectory: &Trajectory, -) -> Result<(), SimulationError> { - let path = trajectory - .points - .iter() - .map(|p| (p.x, p.y, p.z)) - .collect::>(); - rec.log( - "world/quad/path", - &rerun::LineStrips3D::new([path]).with_colors([rerun::Color::from_rgb(0, 255, 255)]), - )?; - Ok(()) -} -/// log mesh data to the rerun recording stream -/// # Arguments -/// * `rec` - The rerun::RecordingStream instance -/// * `division` - The number of divisions in the mesh -/// * `spacing` - The spacing between divisions -fn log_mesh( - rec: &rerun::RecordingStream, - division: usize, - spacing: f32, -) -> Result<(), SimulationError> { - let grid_size: usize = division + 1; - let half_grid_size: f32 = (division as f32 * spacing) / 2.0; - let points: Vec = (0..grid_size) - .flat_map(|i| { - (0..grid_size).map(move |j| { - rerun::external::glam::Vec3::new( - j as f32 * spacing - half_grid_size, - i as f32 * spacing - half_grid_size, - 0.0, - ) - }) - }) - .collect(); - let horizontal_lines: Vec> = (0..grid_size) - .map(|i| points[i * grid_size..(i + 1) * grid_size].to_vec()) - .collect(); - let vertical_lines: Vec> = (0..grid_size) - .map(|j| (0..grid_size).map(|i| points[i * grid_size + j]).collect()) - .collect(); - let line_strips: Vec> = - horizontal_lines.into_iter().chain(vertical_lines).collect(); - rec.log( - "world/mesh", - &rerun::LineStrips3D::new(line_strips) - .with_colors([rerun::Color::from_rgb(255, 255, 255)]) - .with_radii([0.02]), - )?; - Ok(()) -} -/// log depth image data to the rerun recording stream -/// # Arguments -/// * `rec` - The rerun::RecordingStream instance -/// * `depth_image` - The depth image data -/// * `width` - The width of the depth image -/// * `height` - The height of the depth image -/// * `min_depth` - The minimum depth value -/// * `max_depth` - The maximum depth value -fn log_depth_image( - rec: &rerun::RecordingStream, - depth_image: &Vec, - width: usize, - height: usize, - min_depth: f32, - max_depth: f32, -) -> Result<(), SimulationError> { - let mut image = ndarray::Array::zeros((height, width, 3)); - let depth_range = max_depth - min_depth; - image - .axis_iter_mut(ndarray::Axis(0)) - .enumerate() - .for_each(|(y, mut row)| { - for (x, mut pixel) in row.axis_iter_mut(ndarray::Axis(0)).enumerate() { - let depth = depth_image[y * width + x]; - let color = if depth.is_finite() { - let normalized_depth = ((depth - min_depth) / depth_range).clamp(0.0, 1.0); - color_map_fn(normalized_depth * 255.0) - } else { - (0, 0, 0) - }; - (pixel[0], pixel[1], pixel[2]) = color; - } - }); - let rerun_image = rerun::Image::from_color_model_and_tensor(rerun::ColorModel::RGB, image) - .map_err(|e| SimulationError::OtherError(format!("Failed to create rerun image: {}", e)))?; - rec.log("world/quad/cam/depth", &rerun_image)?; - Ok(()) -} -/// turbo color map function -/// # Arguments -/// * `gray` - The gray value in the range [0, 255] -/// # Returns -/// * The RGB color value in the range [0, 255] -fn color_map_fn(gray: f32) -> (u8, u8, u8) { - let x = gray / 255.0; - let r = (34.61 - + x * (1172.33 - x * (10793.56 - x * (33300.12 - x * (38394.49 - x * 14825.05))))) - .clamp(0.0, 255.0) as u8; - let g = (23.31 + x * (557.33 + x * (1225.33 - x * (3574.96 - x * (1073.77 + x * 707.56))))) - .clamp(0.0, 255.0) as u8; - let b = (27.2 + x * (3211.1 - x * (15327.97 - x * (27814.0 - x * (22569.18 - x * 6838.66))))) - .clamp(0.0, 255.0) as u8; - (r, g, b) -} - -/// Main function to run the quadrotor simulation -fn main() -> Result<(), SimulationError> { - let (control_frequency, simulation_frequency, log_frequency) = (200.0, 1000.0, 20.0); - let mut quad = Quadrotor::new(1.0 / simulation_frequency)?; - let mut controller = PIDController::new(); - let mut imu = Imu::new(); - println!("Please start a rerun-cli in another terminal.\n1. cargo install rerun-cli.\n2. rerun\nWaiting for connection to rerun..."); + println!("Please start a rerun-cli in another terminal.\n1. cargo install rerun-cli.\n2. rerun\n\n[INFO] Waiting for connection to rerun..."); let rec = rerun::RecordingStreamBuilder::new("Peng").connect()?; - let (upper_bounds, lower_bounds) = (Vector3::new(3.0, 2.0, 2.0), Vector3::new(-3.0, -2.0, 0.0)); - let mut maze = Maze::new(lower_bounds, upper_bounds, 20); - let camera = Camera::new((128, 96), 90.0_f32.to_radians(), 0.1, 5.0); + let upper_bounds = Vector3::from(config.maze.upper_bounds); + let lower_bounds = Vector3::from(config.maze.lower_bounds); + let mut maze = Maze::new(lower_bounds, upper_bounds, config.maze.num_obstacles); + let camera = Camera::new( + config.camera.resolution, + config.camera.fov.to_radians(), + config.camera.near, + config.camera.far, + ); let mut planner_manager = PlannerManager::new(Vector3::zeros(), 0.0); let mut trajectory = Trajectory::new(Vector3::new(0.0, 0.0, 0.0)); let mut depth_buffer: Vec = vec![0.0; camera.resolution.0 * camera.resolution.1]; rec.set_time_seconds("timestamp", 0); - log_mesh(&rec, 7, 0.5)?; + log_mesh(&rec, config.mesh.division, config.mesh.spacing)?; log_maze_tube(&rec, &maze)?; log_maze_obstacles(&rec, &maze)?; let mut previous_thrust = 0.0; let mut previous_torque = Vector3::zeros(); let mut i = 0; + let planner_config = PlannerConfig { + steps: config + .planner_schedule + .steps + .iter() + .map(|step| PlannerStepConfig { + step: step.step, + planner_type: step.planner_type.clone(), + params: step.params.clone(), + }) + .collect(), + }; loop { let time = quad.time_step * i as f32; rec.set_time_seconds("timestamp", time); maze.update_obstacles(quad.time_step); - update_planner(&mut planner_manager, i, time, &quad, &maze.obstacles); + update_planner( + &mut planner_manager, + i, + time, + &quad, + &maze.obstacles, + &planner_config, + )?; let (desired_position, desired_velocity, desired_yaw) = planner_manager.update( quad.position, quad.orientation, @@ -1519,7 +102,10 @@ fn main() -> Result<(), SimulationError> { &quad.angular_velocity, quad.time_step, ); - if i % (simulation_frequency as usize / control_frequency as usize) == 0 { + if i % (config.simulation.simulation_frequency as usize + / config.simulation.control_frequency as usize) + == 0 + { quad.update_dynamics_with_controls(thrust, &torque); previous_thrust = thrust; previous_torque = torque; @@ -1529,7 +115,10 @@ 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 % (simulation_frequency as usize / log_frequency as usize) == 0 { + if i % (config.simulation.simulation_frequency as usize + / config.simulation.log_frequency as usize) + == 0 + { if trajectory.add_point(quad.position) { log_trajectory(&rec, &trajectory)?; } @@ -1553,7 +142,8 @@ fn main() -> Result<(), SimulationError> { log_maze_obstacles(&rec, &maze)?; } i += 1; - if (i as f32 * 100.0 * quad.time_step) as i32 >= 7000 { + if time >= config.simulation.duration { + println!("[INFO] Simulation complete"); break; } }