Configuration module +This module contains the configuration for the simulation, quadrotor, PID controller, IMU, maze, camera, mesh, and planner schedule. +The configuration is loaded from a YAML file using the serde library. +The configuration is then used to initialize the simulation, quadrotor, PID controller, IMU, maze, camera, mesh, and planner schedule.
+pub struct CameraConfig {
+ pub resolution: (usize, usize),
+ pub fov: f32,
+ pub near: f32,
+ pub far: f32,
+ pub rotation_transform: [f32; 9],
+}Configuration for the camera
+resolution: (usize, usize)Camera resolution in pixels (width, height)
+fov: f32Camera field of view in height in degrees
+near: f32Camera near clipping plane in meters
+far: f32Camera far clipping plane in meters
+rotation_transform: [f32; 9]Camera transform matrix for depth
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct Config {Show 13 fields
+ pub simulation: SimulationConfig,
+ pub quadrotor: QuadrotorConfig,
+ pub pid_controller: PIDControllerConfig,
+ pub imu: ImuConfig,
+ pub maze: MazeConfig,
+ pub camera: CameraConfig,
+ pub mesh: MeshConfig,
+ pub planner_schedule: Vec<PlannerStep>,
+ pub use_rerun: bool,
+ pub render_depth: bool,
+ pub use_multithreading_depth_rendering: bool,
+ pub use_rk4_for_dynamics_control: bool,
+ pub use_rk4_for_dynamics_update: bool,
+}Configuration for the simulation
+simulation: SimulationConfigSimulation configuration
+quadrotor: QuadrotorConfigQuadrotor configuration
+pid_controller: PIDControllerConfigPID Controller configuration
+imu: ImuConfigIMU configuration
+maze: MazeConfigMaze configuration
+camera: CameraConfigCamera configuration
+mesh: MeshConfigMesh configuration
+planner_schedule: Vec<PlannerStep>Planner schedule configuration
+use_rerun: boolUse rerun.io for recording
+render_depth: boolRender depth
+use_multithreading_depth_rendering: boolMultiThreading depth rendering
+use_rk4_for_dynamics_control: boolUse RK4 for updating quadrotor dynamics_with_controls
+use_rk4_for_dynamics_update: boolUse RK4 for updating quadrotor dynamics without controls
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct ImuConfig {
+ pub accel_noise_std: f32,
+ pub gyro_noise_std: f32,
+ pub accel_bias_std: f32,
+ pub gyro_bias_std: f32,
+}Configuration for the IMU
+accel_noise_std: f32Accelerometer noise standard deviation
+gyro_noise_std: f32Gyroscope noise standard deviation
+accel_bias_std: f32Accelerometer bias drift standard deviation
+gyro_bias_std: f32Gyroscope bias drift standard deviation
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct MazeConfig {
+ pub upper_bounds: [f32; 3],
+ pub lower_bounds: [f32; 3],
+ pub num_obstacles: usize,
+ pub obstacles_velocity_bounds: [f32; 3],
+ pub obstacles_radius_bounds: [f32; 2],
+}Configuration for the maze
+upper_bounds: [f32; 3]Upper bounds of the maze in meters (x, y, z)
+lower_bounds: [f32; 3]Lower bounds of the maze in meters (x, y, z)
+num_obstacles: usizeNumber of obstacles in the maze
+obstacles_velocity_bounds: [f32; 3]Obstacle velocity maximum bounds in m/s in (x, y, z) directions
+obstacles_radius_bounds: [f32; 2]Obstacle radius bounds in meters (min, max)
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct MeshConfig {
+ pub division: usize,
+ pub spacing: f32,
+}Configuration for the mesh
+division: usizeDivision of the 2D mesh, the mesh will be division x division squares
+spacing: f32Spacing between the squares in meters
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct PIDControllerConfig {
+ pub pos_gains: PIDGains,
+ pub att_gains: PIDGains,
+ pub pos_max_int: [f32; 3],
+ pub att_max_int: [f32; 3],
+}Configuration for the PID controller
+pos_gains: PIDGainsPosition gains
+att_gains: PIDGainsAttitude gains
+pos_max_int: [f32; 3]Maximum integral error for position control
+att_max_int: [f32; 3]Maximum integral error for attitude control
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct PIDGains {
+ pub kp: [f32; 3],
+ pub ki: [f32; 3],
+ pub kd: [f32; 3],
+}Configuration for PID gains
+kp: [f32; 3]Proportional gains
+ki: [f32; 3]Integral gains
+kd: [f32; 3]Derivative gains
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct PlannerStep {
+ pub step: usize,
+ pub planner_type: String,
+ pub params: Value,
+}Configuration for a planner step
+step: usizeStep number that the planner should be executed (Unit: ms)
+planner_type: StringType of planner to use
+params: ValueParameters for the planner
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct QuadrotorConfig {
+ pub mass: f32,
+ pub gravity: f32,
+ pub drag_coefficient: f32,
+ pub inertia_matrix: [f32; 9],
+}Configuration for the quadrotor
+mass: f32Mass of the quadrotor in kg
+gravity: f32Gravity in m/s^2
+drag_coefficient: f32Drag coefficient in Ns^2/m^2
+inertia_matrix: [f32; 9]Inertia matrix in kg*m^2
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct SimulationConfig {
+ pub control_frequency: usize,
+ pub simulation_frequency: usize,
+ pub log_frequency: usize,
+ pub duration: f32,
+}Configuration for the simulation
+control_frequency: usizeControl frequency in Hz
+simulation_frequency: usizeSimulation frequency in Hz
+log_frequency: usizeLog frequency in Hz
+duration: f32Duration of the simulation in seconds
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub enum PlannerType {
+ Hover(HoverPlanner),
+ MinimumJerkLine(MinimumJerkLinePlanner),
+ Lissajous(LissajousPlanner),
+ Circle(CirclePlanner),
+ Landing(LandingPlanner),
+ ObstacleAvoidance(ObstacleAvoidancePlanner),
+ MinimumSnapWaypoint(MinimumSnapWaypointPlanner),
+}Enum representing different types of trajectory planners
+use peng_quad::PlannerType;
+use peng_quad::HoverPlanner;
+let hover_planner : PlannerType = PlannerType::Hover(HoverPlanner{
+ target_position: nalgebra::Vector3::new(0.0, 0.0, 1.0),
+ target_yaw: 0.0,
+});Hover planner
+Minimum jerk line planner
+Minimum jerk circle planner
+Minimum jerk circle planner
+Minimum jerk landing planner
+Obstacle avoidance planner
+Minimum snap waypoint planner
+Implementation of the planner type
+Plans the trajectory based on the current planner type
+current_position - The current position of the quadrotorcurrent_velocity - The current velocity of the quadrotortime - The current simulation timeuse nalgebra::Vector3;
+use peng_quad::PlannerType;
+use peng_quad::HoverPlanner;
+let hover_planner = HoverPlanner {
+ target_position: Vector3::new(0.0, 0.0, 1.0),
+ target_yaw: 0.0
+};
+let hover_planner_type = PlannerType::Hover(hover_planner);
+let (desired_position, desired_velocity, desired_yaw) = hover_planner_type.plan(Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 0.0);Checks if the current trajectory is finished
+current_position - The current position of the quadrotortime - The current simulation timetrue if the trajectory is finished, false otherwiseuse nalgebra::Vector3;
+use peng_quad::PlannerType;
+use peng_quad::HoverPlanner;
+use peng_quad::Planner;
+let hover_planner = HoverPlanner{
+ target_position: Vector3::new(0.0, 0.0, 1.0),
+ target_yaw: 0.0,
+};
+let is_finished = hover_planner.is_finished(Vector3::new(0.0, 0.0, 0.0), 0.0);self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub enum SimulationError {
+ RerunError(RecordingStreamError),
+ RerunSpawnError(SpawnError),
+ NalgebraError(String),
+ NormalError(NormalError),
+ OtherError(String),
+}Represents errors that can occur during simulation
+use peng_quad::SimulationError;
+let error = SimulationError::NalgebraError("Matrix inversion failed".to_string());Error related to Rerun visualization
+Error related to Rerun spawn process
+Error related to linear algebra operations
+Error related to normal distribution calculations
+Other general errors
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub fn create_planner(
+ step: &PlannerStepConfig,
+ quad: &Quadrotor,
+ time: f32,
+ obstacles: &[Obstacle],
+) -> Result<PlannerType, SimulationError>Creates a planner based on the configuration
+step - The configuration for the planner step in ms unitquad - The Quadrotor instancetime - The current simulation timeobstacles - The current obstacles in the simulationPlannerType - The created planneruse peng_quad::{PlannerType, Quadrotor, Obstacle, PlannerStepConfig, create_planner};
+use nalgebra::Vector3;
+let step = PlannerStepConfig {
+ step: 0,
+ planner_type: "MinimumJerkLine".to_string(),
+ params:
+ serde_yaml::from_str(r#"
+ end_position: [0.0, 0.0, 1.0]
+ end_yaw: 0.0
+ duration: 2.0
+ "#).unwrap(),
+};
+let time = 0.0;
+let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+let obstacles = vec![Obstacle::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 1.0)];
+let planner = create_planner(&step, &quadrotor, time, &obstacles).unwrap();
+match planner {
+ PlannerType::MinimumJerkLine(_) => log::info!("Created MinimumJerkLine planner"),
+ _ => log::info!("Created another planner"),
+}pub fn log_data(
+ rec: &RecordingStream,
+ quad: &Quadrotor,
+ desired_position: &Vector3<f32>,
+ desired_velocity: &Vector3<f32>,
+ measured_accel: &Vector3<f32>,
+ measured_gyro: &Vector3<f32>,
+) -> Result<(), SimulationError>Logs simulation data to the rerun recording stream
+rec - The rerun::RecordingStream instancequad - The Quadrotor instancedesired_position - The desired position vectormeasured_accel - The measured acceleration vectormeasured_gyro - The measured angular velocity vectoruse peng_quad::{Quadrotor, log_data};
+use nalgebra::Vector3;
+let rec = rerun::RecordingStreamBuilder::new("peng").connect().unwrap();
+let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+let quad = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+let desired_position = Vector3::new(0.0, 0.0, 0.0);
+let desired_velocity = Vector3::new(0.0, 0.0, 0.0);
+let measured_accel = Vector3::new(0.0, 0.0, 0.0);
+let measured_gyro = Vector3::new(0.0, 0.0, 0.0);
+log_data(&rec, &quad, &desired_position, &desired_velocity, &measured_accel, &measured_gyro).unwrap();pub fn log_depth_image(
+ rec: &RecordingStream,
+ depth_image: &[f32],
+ width: usize,
+ height: usize,
+ min_depth: f32,
+ max_depth: f32,
+) -> Result<(), SimulationError>log depth image data to the rerun recording stream
+rec - The rerun::RecordingStream instancedepth_image - The depth image datawidth - The width of the depth imageheight - The height of the depth imagemin_depth - The minimum depth valuemax_depth - The maximum depth valueuse peng_quad::log_depth_image;
+let rec = rerun::RecordingStreamBuilder::new("log.rerun").connect().unwrap();
+let depth_image = vec![0.0; 640 * 480];
+log_depth_image(&rec, &depth_image, 640, 480, 0.0, 1.0).unwrap();pub fn log_maze_obstacles(
+ rec: &RecordingStream,
+ maze: &Maze,
+) -> Result<(), SimulationError>Log the maze obstacles to the rerun recording stream
+rec - The rerun::RecordingStream instancemaze - The maze instanceuse peng_quad::{Maze, log_maze_obstacles};
+let rec = rerun::RecordingStreamBuilder::new("log.rerun").connect().unwrap();
+let mut maze = Maze::new([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 5, [0.1, 0.1, 0.1], [0.1, 0.5]);
+log_maze_obstacles(&rec, &maze).unwrap();pub fn log_maze_tube(
+ rec: &RecordingStream,
+ maze: &Maze,
+) -> Result<(), SimulationError>Log the maze tube to the rerun recording stream
+rec - The rerun::RecordingStream instancemaze - The maze instanceuse peng_quad::{Maze, log_maze_tube};
+use rerun::RecordingStreamBuilder;
+let rec = rerun::RecordingStreamBuilder::new("log.rerun").connect().unwrap();
+let mut maze = Maze::new([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 5, [0.1, 0.1, 0.1], [0.1, 0.5]);
+log_maze_tube(&rec, &maze).unwrap();pub fn log_mesh(
+ rec: &RecordingStream,
+ division: usize,
+ spacing: f32,
+) -> Result<(), SimulationError>log mesh data to the rerun recording stream
+rec - The rerun::RecordingStream instancedivision - The number of divisions in the meshspacing - The spacing between divisionsuse peng_quad::log_mesh;
+let rec = rerun::RecordingStreamBuilder::new("log.rerun").connect().unwrap();
+log_mesh(&rec, 10, 0.1).unwrap();pub fn log_trajectory(
+ rec: &RecordingStream,
+ trajectory: &Trajectory,
+) -> Result<(), SimulationError>log trajectory data to the rerun recording stream
+rec - The rerun::RecordingStream instancetrajectory - The Trajectory instanceuse peng_quad::{Trajectory, log_trajectory};
+use nalgebra::Vector3;
+let rec = rerun::RecordingStreamBuilder::new("log.rerun").connect().unwrap();
+let mut trajectory = Trajectory::new(nalgebra::Vector3::new(0.0, 0.0, 0.0));
+trajectory.add_point(nalgebra::Vector3::new(1.0, 0.0, 0.0));
+log_trajectory(&rec, &trajectory).unwrap();pub fn parse_f32(value: &Value, key: &str) -> Result<f32, SimulationError>Helper function to parse f32 from YAML
+value - YAML valuekey - key to parsef32 - parsed valueSimulationError - if the value is not a valid f32use peng_quad::{parse_f32, SimulationError};
+let value = serde_yaml::from_str("key: 1.0").unwrap();
+let result = parse_f32(&value, "key").unwrap();
+assert_eq!(result, 1.0);pub fn parse_vector3(
+ value: &Value,
+ key: &str,
+) -> Result<Vector3<f32>, SimulationError>Helper function to parse Vector3 from YAML
+value - YAML valuekey - key to parseVector3<f32> - parsed vectorSimulationError - if the value is not a valid vectoruse nalgebra::Vector3;
+use peng_quad::{parse_vector3, SimulationError};
+let value = serde_yaml::from_str("test: [1.0, 2.0, 3.0]").unwrap();
+assert_eq!(parse_vector3(&value, "test").unwrap(), Vector3::new(1.0, 2.0, 3.0));pub fn pinhole_depth(
+ rec: &RecordingStream,
+ width: usize,
+ height: usize,
+ fov: f32,
+ cam_position: Vector3<f32>,
+ cam_orientation: UnitQuaternion<f32>,
+ cam_transform: [f32; 9],
+ depth_image: &[f32],
+) -> Result<(), SimulationError>creates pinhole camera
+rec - The rerun::RecordingStream instancewidth - The width component of the camera resolutionheight - The height component of the camera resolutionfov - The fov of the cameracam_position - The position vector of the camera (aligns with the quad)cam_orientation - The orientation quaternion of quadcam_transform - The transform matrix between quad and camera alignmentdepth_image - The depth image datause peng_quad::pinhole_depth;
+use nalgebra::{Vector3, UnitQuaternion};
+let rec = rerun::RecordingStreamBuilder::new("log.rerun").connect().unwrap();
+let depth_image = vec![ 0.0f32 ; 640 * 480];
+let cam_position = Vector3::new(0.0,0.0,0.0);
+let cam_orientation = UnitQuaternion::identity();
+let cam_transform = [0.0, 0.0, 1.0, -1.0, 0.0, 0.0, 0.0, -1.0, 0.0];
+pinhole_depth(&rec, 128usize, 96usize, 90.0, cam_position, cam_orientation, cam_transform, &depth_image).unwrap();pub fn update_planner(
+ planner_manager: &mut PlannerManager,
+ step: usize,
+ time: f32,
+ simulation_frequency: usize,
+ quad: &Quadrotor,
+ obstacles: &[Obstacle],
+ planner_config: &[PlannerStepConfig],
+) -> Result<(), SimulationError>Updates the planner based on the current simulation step and configuration
+planner_manager - The PlannerManager instance to updatestep - The current simulation step in ms unittime - The current simulation timequad - The Quadrotor instanceobstacles - The current obstacles in the simulationplanner_config - The planner configurationuse peng_quad::{PlannerManager, Quadrotor, Obstacle, PlannerStepConfig, update_planner};
+use nalgebra::Vector3;
+let simulation_frequency = 1000;
+let initial_position = Vector3::new(0.0, 0.0, 0.0);
+let initial_yaw = 0.0;
+let mut planner_manager = PlannerManager::new(initial_position, initial_yaw);
+let step = 0;
+let time = 0.0;
+let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+let obstacles = vec![Obstacle::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 1.0)];
+let planner_config = vec![PlannerStepConfig {
+ step: 0,
+ planner_type: "MinimumJerkLine".to_string(),
+ params:
+ serde_yaml::from_str(r#"
+ end_position: [0.0, 0.0, 1.0]
+ end_yaw: 0.0
+ duration: 2.0
+ "#).unwrap(),
+}];
+update_planner(&mut planner_manager, step, time, simulation_frequency, &quadrotor, &obstacles, &planner_config).unwrap();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.
+rerun crate for visualizationuse nalgebra::Vector3;
+use peng_quad::{Quadrotor, SimulationError};
+let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix);pub struct Camera {
+ pub resolution: (usize, usize),
+ pub fov: f32,
+ pub near: f32,
+ pub far: f32,
+ pub aspect_ratio: f32,
+ pub ray_directions: Vec<Vector3<f32>>,
+}Represents a camera in the simulation which is used to render the depth of the scene
+use peng_quad::Camera;
+let camera = Camera::new((800, 600), 60.0, 0.1, 100.0);resolution: (usize, usize)The resolution of the camera
+fov: f32The field of view of the camera
+near: f32The near clipping plane of the camera
+far: f32The far clipping plane of the camera
+aspect_ratio: f32The aspect ratio of the camera
+ray_directions: Vec<Vector3<f32>>The ray directions of each pixel in the camera
+Implementation of the camera
+Creates a new camera with the given resolution, field of view, near and far clipping planes
+resolution - The resolution of the camerafov - The field of view of the cameranear - The near clipping plane of the camerafar - The far clipping plane of the camerause peng_quad::Camera;
+let camera = Camera::new((800, 600), 60.0, 0.1, 100.0);Renders the depth of the scene from the perspective of the quadrotor
+quad_position - The position of the quadrotorquad_orientation - The orientation of the quadrotormaze - The maze in the scenedepth_buffer - The depth buffer to store the depth valuesuse peng_quad::{Camera, Maze};
+use nalgebra::{Vector3, UnitQuaternion};
+let camera = Camera::new((800, 600), 60.0, 0.1, 100.0);
+let quad_position = Vector3::new(0.0, 0.0, 0.0);
+let quad_orientation = UnitQuaternion::identity();
+let mut maze = Maze::new([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 5, [0.1, 0.1, 0.1], [0.1, 0.5]);
+let mut depth_buffer = vec![0.0; 800 * 600];
+let use_multithreading = true;
+camera.render_depth(&quad_position, &quad_orientation, &maze, &mut depth_buffer, use_multithreading);Casts a ray from the camera origin in the given direction
+origin - The origin of the rayrotation_world_to_camera - The rotation matrix from world to camera coordinatesdirection - The direction of the raymaze - The maze in the sceneuse peng_quad::{Camera, Maze};
+use nalgebra::{Vector3, Matrix3};
+let camera = Camera::new((800, 600), 60.0, 0.1, 100.0);
+let origin = Vector3::new(0.0, 0.0, 0.0);
+let rotation_world_to_camera = Matrix3::identity();
+let direction = Vector3::new(1.0, 0.0, 0.0);
+let mut maze = Maze::new([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 5, [0.1, 0.1, 0.1], [0.1, 0.5]);
+let distance = camera.ray_cast(&origin, &rotation_world_to_camera, &direction, &maze);self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct CirclePlanner {
+ pub center: Vector3<f32>,
+ pub radius: f32,
+ pub angular_velocity: f32,
+ pub start_position: Vector3<f32>,
+ pub start_time: f32,
+ pub duration: f32,
+ pub start_yaw: f32,
+ pub end_yaw: f32,
+ pub ramp_time: f32,
+}Planner for circular trajectories
+use nalgebra::Vector3;
+use peng_quad::CirclePlanner;
+let circle_planner = CirclePlanner {
+ center: Vector3::new(1.0, 1.0, 1.0),
+ radius: 1.0,
+ angular_velocity: 1.0,
+ start_position: Vector3::new(0.0, 0.0, 0.0),
+ start_time: 0.0,
+ duration: 1.0,
+ start_yaw: 0.0,
+ end_yaw: 0.0,
+ ramp_time: 0.1,
+};center: Vector3<f32>Center of the circular trajectory
+radius: f32Radius of the circular trajectory
+angular_velocity: f32Angular velocity of the circular motion
+start_position: Vector3<f32>Starting position of the trajectory
+start_time: f32Start time of the trajectory
+duration: f32Duration of the trajectory
+start_yaw: f32Starting yaw angle
+end_yaw: f32Ending yaw angle
+ramp_time: f32Ramp-up time for smooth transitions
+Implementation of the Planner trait for CirclePlanner
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct HoverPlanner {
+ pub target_position: Vector3<f32>,
+ pub target_yaw: f32,
+}Planner for hovering at a fixed position
+use nalgebra::Vector3;
+use peng_quad::HoverPlanner;
+let hover_planner = HoverPlanner {
+ target_position: Vector3::new(0.0, 0.0, 0.0),
+ target_yaw: 0.0,
+};target_position: Vector3<f32>Target position for hovering
+target_yaw: f32Target yaw angle for hovering
+Implementation of the Planner trait for the HoverPlanner
self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct Imu {
+ pub accel_bias: Vector3<f32>,
+ pub gyro_bias: Vector3<f32>,
+ pub accel_noise_std: f32,
+ pub gyro_noise_std: f32,
+ pub accel_bias_std: f32,
+ pub gyro_bias_std: f32,
+ /* private fields */
+}Represents an Inertial Measurement Unit (IMU) with bias and noise characteristics
+use nalgebra::Vector3;
+use peng_quad::Imu;
+let accel_noise_std = 0.0003;
+let gyro_noise_std = 0.02;
+let accel_bias_std = 0.0001;
+let gyro_bias_std = 0.001;
+let imu = Imu::new(accel_noise_std, gyro_noise_std, accel_bias_std, gyro_bias_std);accel_bias: Vector3<f32>Accelerometer bias
+gyro_bias: Vector3<f32>Gyroscope bias
+accel_noise_std: f32Standard deviation of accelerometer noise
+gyro_noise_std: f32Standard deviation of gyroscope noise
+accel_bias_std: f32Standard deviation of accelerometer bias drift
+gyro_bias_std: f32Standard deviation of gyroscope bias drift
+Implements the IMU
+Creates a new IMU with default parameters
+accel_noise_std - Standard deviation of accelerometer noisegyro_noise_std - Standard deviation of gyroscope noiseaccel_bias_std - Standard deviation of accelerometer bias driftgyro_bias_std - Standard deviation of gyroscope bias driftuse peng_quad::Imu;
+
+let imu = Imu::new(0.01, 0.01, 0.01, 0.01);Simulates IMU readings with added bias and noise
+true_acceleration - The true acceleration vectortrue_angular_velocity - The true angular velocity vectoruse nalgebra::Vector3;
+use peng_quad::Imu;
+
+let mut imu = Imu::new(0.01, 0.01, 0.01, 0.01).unwrap();
+let true_acceleration = Vector3::new(0.0, 0.0, 9.81);
+let true_angular_velocity = Vector3::new(0.0, 0.0, 0.0);
+let (measured_acceleration, measured_ang_velocity) = imu.read(true_acceleration, true_angular_velocity).unwrap();self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct LandingPlanner {
+ pub start_position: Vector3<f32>,
+ pub start_time: f32,
+ pub duration: f32,
+ pub start_yaw: f32,
+}Planner for landing maneuvers
+use nalgebra::Vector3;
+use peng_quad::LandingPlanner;
+let landing_planner = LandingPlanner {
+ start_position: Vector3::new(0.0, 0.0, 1.0),
+ start_time: 0.0,
+ duration: 1.0,
+ start_yaw: 0.0,
+};start_position: Vector3<f32>Starting position of the landing maneuver
+start_time: f32Start time of the landing maneuver
+duration: f32Duration of the landing maneuver
+start_yaw: f32Starting yaw angle
+Implementation of the Planner trait for LandingPlanner
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct LissajousPlanner {
+ pub start_position: Vector3<f32>,
+ pub center: Vector3<f32>,
+ pub amplitude: Vector3<f32>,
+ pub frequency: Vector3<f32>,
+ pub phase: Vector3<f32>,
+ pub start_time: f32,
+ pub duration: f32,
+ pub start_yaw: f32,
+ pub end_yaw: f32,
+ pub ramp_time: f32,
+}Planner for Lissajous curve trajectories
+use nalgebra::Vector3;
+use peng_quad::LissajousPlanner;
+let lissajous_planner = LissajousPlanner {
+ start_position: Vector3::new(0.0, 0.0, 0.0),
+ center: Vector3::new(1.0, 1.0, 1.0),
+ amplitude: Vector3::new(1.0, 1.0, 1.0),
+ frequency: Vector3::new(1.0, 1.0, 1.0),
+ phase: Vector3::new(0.0, 0.0, 0.0),
+ start_time: 0.0,
+ duration: 1.0,
+ start_yaw: 0.0,
+ end_yaw: 0.0,
+ ramp_time: 0.1,
+};start_position: Vector3<f32>Starting position of the trajectory
+center: Vector3<f32>Center of the Lissajous curve
+amplitude: Vector3<f32>Amplitude of the Lissajous curve
+frequency: Vector3<f32>Frequency of the Lissajous curve
+phase: Vector3<f32>Phase of the Lissajous curve
+start_time: f32Start time of the trajectory
+duration: f32Duration of the trajectory
+start_yaw: f32Starting yaw angle
+end_yaw: f32Ending yaw angle
+ramp_time: f32Ramp-up time for smooth transitions
+Implementation of the planner trait for Lissajous curve trajectories
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct Maze {
+ pub lower_bounds: [f32; 3],
+ pub upper_bounds: [f32; 3],
+ pub obstacles: Vec<Obstacle>,
+ pub obstacles_velocity_bounds: [f32; 3],
+ pub obstacles_radius_bounds: [f32; 2],
+}Represents a maze in the simulation
+use peng_quad::{Maze, Obstacle};
+use nalgebra::Vector3;
+let maze = Maze {
+ lower_bounds: [0.0, 0.0, 0.0],
+ upper_bounds: [1.0, 1.0, 1.0],
+ obstacles: vec![Obstacle::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 1.0)],
+ obstacles_velocity_bounds: [0.0, 0.0, 0.0],
+ obstacles_radius_bounds: [0.0, 0.0],
+};lower_bounds: [f32; 3]The lower bounds of the maze in the x, y, and z directions
+upper_bounds: [f32; 3]The upper bounds of the maze in the x, y, and z directions
+obstacles: Vec<Obstacle>The obstacles in the maze
+obstacles_velocity_bounds: [f32; 3]The bounds of the obstacles’ velocity
+obstacles_radius_bounds: [f32; 2]The bounds of the obstacles’ radius
+Implementation of the maze
+Creates a new maze with the given bounds and number of obstacles
+lower_bounds - The lower bounds of the mazeupper_bounds - The upper bounds of the mazenum_obstacles - The number of obstacles in the mazeuse peng_quad::Maze;
+let maze = Maze::new([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 5, [0.1, 0.1, 0.1], [0.1, 0.5]);self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct MinimumJerkLinePlanner {
+ pub start_position: Vector3<f32>,
+ pub end_position: Vector3<f32>,
+ pub start_yaw: f32,
+ pub end_yaw: f32,
+ pub start_time: f32,
+ pub duration: f32,
+}Planner for minimum jerk trajectories along a straight line
+use nalgebra::Vector3;
+use peng_quad::MinimumJerkLinePlanner;
+let minimum_jerk_line_planner = MinimumJerkLinePlanner {
+ start_position: Vector3::new(0.0, 0.0, 0.0),
+ end_position: Vector3::new(1.0, 1.0, 1.0),
+ start_yaw: 0.0,
+ end_yaw: 0.0,
+ start_time: 0.0,
+ duration: 1.0,
+};start_position: Vector3<f32>Starting position of the trajectory
+end_position: Vector3<f32>Ending position of the trajectory
+start_yaw: f32Starting yaw angle
+end_yaw: f32Ending yaw angle
+start_time: f32Start time of the trajectory
+duration: f32Duration of the trajectory
+Implementation of the planner trait for minimum jerk line planner
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct MinimumSnapWaypointPlanner {
+ pub waypoints: Vec<Vector3<f32>>,
+ pub yaws: Vec<f32>,
+ pub times: Vec<f32>,
+ pub coefficients: Vec<Vec<Vector3<f32>>>,
+ pub yaw_coefficients: Vec<Vec<f32>>,
+ pub start_time: f32,
+}Waypoint planner that generates a minimum snap trajectory between waypoints
+use peng_quad::MinimumSnapWaypointPlanner;
+use nalgebra::Vector3;
+let planner = MinimumSnapWaypointPlanner::new(
+ vec![Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 0.0, 0.0)],
+ vec![0.0, 0.0],
+ vec![1.0],
+ 0.0,
+);waypoints: Vec<Vector3<f32>>List of waypoints
+yaws: Vec<f32>List of yaw angles
+times: Vec<f32>List of segment times to reach each waypoint
+coefficients: Vec<Vec<Vector3<f32>>>Coefficients for the x, y, and z components of the trajectory
+yaw_coefficients: Vec<Vec<f32>>Coefficients for the yaw component of the trajectory
+start_time: f32Start time of the trajectory
+Implementation of the MinimumSnapWaypointPlanner
+Generate a new minimum snap waypoint planner
+waypoints - List of waypointsyaws - List of yaw anglessegment_times - List of segment times to reach each waypointstart_time - Start time of the trajectoryuse peng_quad::MinimumSnapWaypointPlanner;
+use nalgebra::Vector3;
+let waypoints = vec![Vector3::zeros(), Vector3::new(1.0, 0.0, 0.0)];
+let yaws = vec![0.0, 0.0];
+let segment_times = vec![1.0];
+let start_time = 0.0;
+let planner = MinimumSnapWaypointPlanner::new(waypoints, yaws, segment_times, start_time);Compute the coefficients for the minimum snap trajectory, calculated for each segment between waypoints
+use peng_quad::MinimumSnapWaypointPlanner;
+use nalgebra::Vector3;
+let waypoints = vec![Vector3::zeros(), Vector3::new(1.0, 0.0, 0.0)];
+let yaws = vec![0.0, 0.0];
+let segment_times = vec![1.0];
+let start_time = 0.0;
+let mut planner = MinimumSnapWaypointPlanner::new(waypoints, yaws, segment_times, start_time).unwrap();
+planner.compute_minimum_snap_trajectories();Compute the coefficients for yaw trajectories +The yaw trajectory is a cubic polynomial and interpolated between waypoints
+use peng_quad::MinimumSnapWaypointPlanner;
+use nalgebra::Vector3;
+let waypoints = vec![Vector3::zeros(), Vector3::new(1.0, 0.0, 0.0)];
+let yaws = vec![0.0, 0.0];
+let segment_times = vec![1.0];
+let start_time = 0.0;
+let mut planner = MinimumSnapWaypointPlanner::new(waypoints, yaws, segment_times, start_time).unwrap();
+planner.compute_minimum_snap_trajectories();
+planner.compute_minimum_acceleration_yaw_trajectories();Evaluate the trajectory at a given time, returns the position, velocity, yaw, and yaw rate at the given time
+t - The time to evaluate the trajectory atcoeffs - The coefficients for the position trajectoryyaw_coeffs - The coefficients for the yaw trajectoryposition - 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)use nalgebra::Vector3;
+use peng_quad::MinimumSnapWaypointPlanner;
+let waypoints = vec![Vector3::zeros(), Vector3::new(1.0, 0.0, 0.0)];
+let yaws = vec![0.0, 0.0];
+let segment_times = vec![1.0];
+let start_time = 0.0;
+let mut planner = MinimumSnapWaypointPlanner::new(waypoints, yaws, segment_times, start_time).unwrap();
+planner.compute_minimum_snap_trajectories();
+planner.compute_minimum_acceleration_yaw_trajectories();
+let (position, velocity, yaw, yaw_rate) = planner.evaluate_polynomial(0.5, &planner.coefficients[0], &planner.yaw_coefficients[0]);Implement the Planner trait for MinimumSnapWaypointPlanner
self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct Obstacle {
+ pub position: Vector3<f32>,
+ pub velocity: Vector3<f32>,
+ pub radius: f32,
+}Represents an obstacle in the simulation
+use peng_quad::Obstacle;
+use nalgebra::Vector3;
+let obstacle = Obstacle::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 1.0);position: Vector3<f32>The position of the obstacle
+velocity: Vector3<f32>The velocity of the obstacle
+radius: f32The radius of the obstacle
+Implementation of the Obstacle
+Creates a new obstacle with the given position, velocity, and radius
+position - The position of the obstaclevelocity - The velocity of the obstacleradius - The radius of the obstacleuse peng_quad::Obstacle;
+use nalgebra::Vector3;
+let obstacle = Obstacle::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 1.0);clone_to_uninit)self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct ObstacleAvoidancePlanner {
+ pub target_position: Vector3<f32>,
+ pub start_time: f32,
+ pub duration: f32,
+ pub start_yaw: f32,
+ pub end_yaw: f32,
+ pub obstacles: Vec<Obstacle>,
+ pub k_att: f32,
+ pub k_rep: f32,
+ pub k_vortex: f32,
+ pub d0: f32,
+ pub d_target: f32,
+ pub max_speed: f32,
+}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
+use nalgebra::Vector3;
+use peng_quad::{ObstacleAvoidancePlanner, Obstacle};
+let planner = ObstacleAvoidancePlanner {
+ target_position: Vector3::new(0.0, 0.0, 1.0),
+ start_time: 0.0,
+ duration: 10.0,
+ start_yaw: 0.0,
+ end_yaw: 0.0,
+ obstacles: vec![Obstacle {
+ position: Vector3::new(1.0, 0.0, 1.0),
+ velocity: Vector3::new(0.0, 0.0, 0.0),
+ radius: 0.5,
+ }],
+ k_att: 1.0,
+ k_rep: 1.0,
+ k_vortex: 1.0,
+ d0: 1.0,
+ d_target: 1.0,
+ max_speed: 1.0,
+};target_position: Vector3<f32>Target position of the planner
+start_time: f32Start time of the planner
+duration: f32Duration of the planner
+start_yaw: f32Starting yaw angle
+end_yaw: f32Ending yaw angle
+obstacles: Vec<Obstacle>List of obstacles
+k_att: f32Attractive force gain
+k_rep: f32Repulsive force gain
+k_vortex: f32Vortex force gain
+d0: f32Influence distance of obstacles
+d_target: f32Influence distance of target
+max_speed: f32Maximum speed of the quadrotor
+Implementation of the ObstacleAvoidancePlanner
+A smooth attractive force function that transitions from linear to exponential decay +When the distance to the target is less than the target distance, the force is linear +When the distance is greater, the force decays exponentially
+distance - The distance to the targetuse peng_quad::ObstacleAvoidancePlanner;
+let planner = ObstacleAvoidancePlanner {
+ target_position: nalgebra::Vector3::new(0.0, 0.0, 1.0),
+ start_time: 0.0,
+ duration: 10.0,
+ start_yaw: 0.0,
+ end_yaw: 0.0,
+ obstacles: vec![],
+ k_att: 1.0,
+ k_rep: 1.0,
+ k_vortex: 1.0,
+ d0: 1.0,
+ d_target: 1.0,
+ max_speed: 1.0,
+};
+let distance = 1.0;
+let force = planner.smooth_attractive_force(distance);Implementation of the Planner trait for ObstacleAvoidancePlanner
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct PIDController {
+ pub kpid_pos: [Vector3<f32>; 3],
+ pub kpid_att: [Vector3<f32>; 3],
+ pub integral_pos_error: Vector3<f32>,
+ pub integral_att_error: Vector3<f32>,
+ pub max_integral_pos: Vector3<f32>,
+ pub max_integral_att: Vector3<f32>,
+ pub mass: f32,
+ pub gravity: f32,
+}PID controller for quadrotor position and attitude control
+use nalgebra::Vector3;
+use peng_quad::PIDController;
+let kpid_pos = [
+ [1.0, 1.0, 1.0],
+ [0.1, 0.1, 0.1],
+ [0.01, 0.01, 0.01],
+];
+let kpid_att = [
+ [1.0, 1.0, 1.0],
+ [0.1, 0.1, 0.1],
+ [0.01, 0.01, 0.01],
+];
+let max_integral_pos = [1.0, 1.0, 1.0];
+let max_integral_att = [1.0, 1.0, 1.0];
+let mass = 1.0;
+let gravity = 9.81;
+let pid_controller = PIDController::new(kpid_pos, kpid_att, max_integral_pos, max_integral_att, mass, gravity);kpid_pos: [Vector3<f32>; 3]PID gain for position control including proportional, derivative, and integral gains
+kpid_att: [Vector3<f32>; 3]PID gain for attitude control including proportional, derivative, and integral gains
+integral_pos_error: Vector3<f32>Accumulated integral error for position
+integral_att_error: Vector3<f32>Accumulated integral error for attitude
+max_integral_pos: Vector3<f32>Maximum allowed integral error for position
+max_integral_att: Vector3<f32>Maximum allowed integral error for attitude
+mass: f32Mass of the quadrotor
+gravity: f32Gravity constant
+Implementation of PIDController
+Creates a new PIDController with default gains +gains are in the order of proportional, derivative, and integral
+_kpid_pos - PID gains for position control_kpid_att - PID gains for attitude control_max_integral_pos - Maximum allowed integral error for position_max_integral_att - Maximum allowed integral error for attitudeuse nalgebra::Vector3;
+use peng_quad::PIDController;
+let kpid_pos = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
+let kpid_att = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
+let max_integral_pos = [1.0, 1.0, 1.0];
+let max_integral_att = [1.0, 1.0, 1.0];
+let mass = 1.0;
+let gravity = 9.81;
+let pid = PIDController::new(kpid_pos, kpid_att, max_integral_pos, max_integral_att, mass, gravity);Computes attitude control torques
+desired_orientation - The desired orientation quaternioncurrent_orientation - The current orientation quaternioncurrent_angular_velocity - The current angular velocitydt - Time stepuse nalgebra::{UnitQuaternion, Vector3};
+use peng_quad::PIDController;
+
+let kpid_pos = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
+let kpid_att = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
+let max_integral_pos = [1.0, 1.0, 1.0];
+let max_integral_att = [1.0, 1.0, 1.0];
+let mass = 1.0;
+let gravity = 9.81;
+let mut pid = PIDController::new(kpid_pos, kpid_att, max_integral_pos, max_integral_att, mass, gravity);
+let desired_orientation = UnitQuaternion::identity();
+let current_orientation = UnitQuaternion::identity();
+let current_angular_velocity = Vector3::zeros();
+let dt = 0.01;
+let control_torques = pid.compute_attitude_control(&desired_orientation, ¤t_orientation, ¤t_angular_velocity, dt);Computes position control thrust and desired orientation
+desired_position - The desired positiondesired_velocity - The desired velocitydesired_yaw - The desired yaw anglecurrent_position - The current positioncurrent_velocity - The current velocitydt - Time stepmass - Mass of the quadrotorgravity - Gravitational accelerationuse nalgebra::{UnitQuaternion, Vector3};
+use peng_quad::PIDController;
+
+let kpid_pos = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
+let kpid_att = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
+let max_integral_pos = [1.0, 1.0, 1.0];
+let max_integral_att = [1.0, 1.0, 1.0];
+let mass = 1.0;
+let gravity = 9.81;
+let mut pid = PIDController::new(kpid_pos, kpid_att, max_integral_pos, max_integral_att, mass, gravity);
+let desired_position = Vector3::new(0.0, 0.0, 1.0);
+let desired_velocity = Vector3::zeros();
+let desired_yaw = 0.0;
+let current_position = Vector3::zeros();
+let current_velocity = Vector3::zeros();
+let dt = 0.01;
+let (thrust, desired_orientation) = pid.compute_position_control(&desired_position, &desired_velocity, desired_yaw, ¤t_position, ¤t_velocity, dt);self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct PlannerManager {
+ pub current_planner: PlannerType,
+}Manages different trajectory planners and switches between them
+use nalgebra::Vector3;
+use peng_quad::PlannerManager;
+let initial_position = Vector3::new(0.0, 0.0, 1.0);
+let initial_yaw = 0.0;
+let planner_manager = PlannerManager::new(initial_position, initial_yaw);current_planner: PlannerTypeThe current planner
+Implementation of the PlannerManager
+Creates a new PlannerManager with an initial hover planner
+initial_position - The initial position for hoveringinitial_yaw - The initial yaw angle for hoveringuse nalgebra::Vector3;
+use peng_quad::PlannerManager;
+let initial_position = Vector3::new(0.0, 0.0, 1.0);
+let initial_yaw = 0.0;
+let planner_manager = PlannerManager::new(initial_position, initial_yaw);Sets a new planner
+new_planner - The new planner to be setuse nalgebra::Vector3;
+use peng_quad::{PlannerManager, CirclePlanner, PlannerType};
+let initial_position = Vector3::new(0.0, 0.0, 1.0);
+let initial_yaw = 0.0;
+let mut planner_manager = PlannerManager::new(initial_position, initial_yaw);
+let new_planner = CirclePlanner {
+ center: Vector3::new(0.0, 0.0, 1.0),
+ radius: 1.0,
+ angular_velocity: 1.0,
+ start_yaw: 0.0,
+ end_yaw: 0.0,
+ start_time: 0.0,
+ duration: 10.0,
+ ramp_time: 1.0,
+ start_position: Vector3::new(0.0, 0.0, 1.0),
+};
+planner_manager.set_planner(PlannerType::Circle(new_planner));Updates the current planner and returns the desired position, velocity, and yaw
+current_position - The current position of the quadrotorcurrent_orientation - The current orientation of the quadrotorcurrent_velocity - The current velocity of the quadrotortime - The current simulation timeuse nalgebra::{Vector3, UnitQuaternion};
+use peng_quad::{PlannerManager, SimulationError};
+let initial_position = Vector3::new(0.0, 0.0, 1.0);
+let initial_yaw = 0.0;
+let mut planner_manager = PlannerManager::new(initial_position, initial_yaw);
+let current_position = Vector3::new(0.0, 0.0, 1.0);
+let current_orientation = UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0);
+let current_velocity = Vector3::new(0.0, 0.0, 0.0);
+let obstacles = vec![];
+let time = 0.0;
+let result = planner_manager.update(current_position, current_orientation, current_velocity, time, &obstacles);
+match result {
+ Ok((target_position, target_velocity, target_yaw)) => {
+ println!("Target Position: {:?}", target_position);
+ println!("Target Velocity: {:?}", target_velocity);
+ println!("Target Yaw: {:?}", target_yaw);
+ }
+ Err(SimulationError) => {
+ log::error!("Error: Planner is not finished");
+ }
+}self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct PlannerStepConfig {
+ pub step: usize,
+ pub planner_type: String,
+ pub params: Value,
+}Represents a step in the planner schedule.
+use peng_quad::PlannerStepConfig;
+let step = PlannerStepConfig {
+ step: 0,
+ planner_type: "MinimumJerkLocalPlanner".to_string(),
+ params: serde_yaml::Value::Null,
+};step: usizeThe simulation step at which this planner should be activated (in ms unit).
+planner_type: StringThe type of planner to use for this step.
+params: ValueAdditional parameters for the planner, stored as a YAML value.
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct Quadrotor {
+ pub position: Vector3<f32>,
+ pub velocity: Vector3<f32>,
+ pub orientation: UnitQuaternion<f32>,
+ pub angular_velocity: Vector3<f32>,
+ pub mass: f32,
+ pub gravity: f32,
+ pub time_step: f32,
+ pub drag_coefficient: f32,
+ pub inertia_matrix: Matrix3<f32>,
+ pub inertia_matrix_inv: Matrix3<f32>,
+}Represents a quadrotor with its physical properties and state
+use nalgebra::Vector3;
+use peng_quad::Quadrotor;
+let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix);position: Vector3<f32>Current position of the quadrotor in 3D space
+velocity: Vector3<f32>Current velocity of the quadrotor
+orientation: UnitQuaternion<f32>Current orientation of the quadrotor
+angular_velocity: Vector3<f32>Current angular velocity of the quadrotor
+mass: f32Mass of the quadrotor in kg
+gravity: f32Gravitational acceleration in m/s^2
+time_step: f32Simulation time step in seconds
+drag_coefficient: f32Drag coefficient
+inertia_matrix: Matrix3<f32>Inertia matrix of the quadrotor
+inertia_matrix_inv: Matrix3<f32>Inverse of the inertia matrix
+Implementation of the Quadrotor struct
+Creates a new Quadrotor with default parameters
+time_step - The simulation time step in secondsmass - The mass of the quadrotor in kggravity - The gravitational acceleration in m/s^2drag_coefficient - The drag coefficientinertia_matrix - The inertia matrix of the quadrotoruse nalgebra::Vector3;
+use peng_quad::Quadrotor;
+
+let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix);Updates the quadrotor’s dynamics with control inputs
+control_thrust - The total thrust force applied to the quadrotorcontrol_torque - The 3D torque vector applied to the quadrotoruse nalgebra::Vector3;
+use peng_quad::Quadrotor;
+
+let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+let mut quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+let control_thrust = mass * gravity;
+let control_torque = Vector3::new(0.0, 0.0, 0.0);
+quadrotor.update_dynamics_with_controls_euler(control_thrust, &control_torque);Updates the quadrotor’s dynamics with control inputs using the Runge-Kutta 4th order method
+control_thrust - The total thrust force applied to the quadrotorcontrol_torque - The 3D torque vector applied to the quadrotoruse nalgebra::Vector3;
+use peng_quad::Quadrotor;
+let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+let mut quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+let control_thrust = mass * gravity;
+let control_torque = Vector3::new(0.0, 0.0, 0.0);
+quadrotor.update_dynamics_with_controls_rk4(control_thrust, &control_torque);Returns the state derivative of the quadrotor
+state - The state of the quadrotoruse nalgebra::Vector3;
+use peng_quad::Quadrotor;
+use nalgebra::UnitQuaternion;
+let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+let state = quadrotor.get_state();Sets the state of the quadrotor
+state - The state of the quadrotoruse nalgebra::Vector3;
+use peng_quad::Quadrotor;
+use nalgebra::UnitQuaternion;
+let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+let mut quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+let state = [
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+];
+quadrotor.set_state(&state);Calculates the derivative of the state of the quadrotor
+state - The current state of the quadrotorcontrol_thrust - The thrust applied to the quadrotorcontrol_torque - The torque applied to the quadrotorThe derivative of the state
+use nalgebra::Vector3;
+use peng_quad::Quadrotor;
+use nalgebra::UnitQuaternion;
+let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+let state = [
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+];
+let control_thrust = 0.0;
+let control_torque = Vector3::new(0.0, 0.0, 0.0);
+let derivative = quadrotor.state_derivative(&state, control_thrust, &control_torque);Simulates IMU readings
+use nalgebra::Vector3;
+use peng_quad::Quadrotor;
+
+let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+let (true_acceleration, true_angular_velocity) = quadrotor.read_imu().unwrap();self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct Trajectory {
+ pub points: Vec<Vector3<f32>>,
+ pub last_logged_point: Vector3<f32>,
+ pub min_distance_threadhold: f32,
+}A struct to hold trajectory data
+use peng_quad::Trajectory;
+let initial_point = nalgebra::Vector3::new(0.0, 0.0, 0.0);
+let mut trajectory = Trajectory::new(initial_point);points: Vec<Vector3<f32>>A vector of 3D points
+last_logged_point: Vector3<f32>The last point that was logged
+min_distance_threadhold: f32The minimum distance between points to log
+Implement the Trajectory struct
+Create a new Trajectory instance
+initial_point - The initial point to add to the trajectoryuse peng_quad::Trajectory;
+let initial_point = nalgebra::Vector3::new(0.0, 0.0, 0.0);
+let mut trajectory = Trajectory::new(initial_point);Add a point to the trajectory if it is further than the minimum distance threshold
+point - The point to addtrue if the point was added, false otherwiseuse peng_quad::Trajectory;
+let mut trajectory = Trajectory::new(nalgebra::Vector3::new(0.0, 0.0, 0.0));
+let point = nalgebra::Vector3::new(1.0, 0.0, 0.0);
+assert_eq!(trajectory.add_point(point), true);
+assert_eq!(trajectory.add_point(point), false);self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub trait Planner {
+ // Required methods
+ fn plan(
+ &self,
+ current_position: Vector3<f32>,
+ current_velocity: Vector3<f32>,
+ time: f32,
+ ) -> (Vector3<f32>, Vector3<f32>, f32);
+ fn is_finished(
+ &self,
+ current_position: Vector3<f32>,
+ time: f32,
+ ) -> Result<bool, SimulationError>;
+}Trait defining the interface for trajectory planners
+use nalgebra::Vector3;
+use peng_quad::{Planner, SimulationError};
+struct TestPlanner;
+impl Planner for TestPlanner {
+ fn plan(
+ &self,
+ current_position: Vector3<f32>,
+ current_velocity: Vector3<f32>,
+ time: f32,
+) -> (Vector3<f32>, Vector3<f32>, f32) {
+ (Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 0.0)
+ }
+ fn is_finished(
+ &self,
+ current_position: Vector3<f32>,
+ time: f32,
+ ) -> Result<bool, SimulationError> {
+ Ok(true)
+ }
+}Plans the trajectory based on the current state and time
+current_position - The current position of the quadrotorcurrent_velocity - The current velocity of the quadrotortime - The current simulation timeuse nalgebra::Vector3;
+use peng_quad::{Planner, SimulationError};
+struct TestPlanner;
+impl Planner for TestPlanner {
+ fn plan(
+ &self,
+ current_position: Vector3<f32>,
+ current_velocity: Vector3<f32>,
+ time: f32,
+) -> (Vector3<f32>, Vector3<f32>, f32) {
+ (Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 0.0)
+ }
+ fn is_finished(
+ &self,
+ current_position: Vector3<f32>,
+ time: f32,
+ ) -> Result<bool, SimulationError> {
+ Ok(true)
+ }
+}Checks if the current trajectory is finished
+current_position - The current position of the quadrotortime - The current simulation timetrue if the trajectory is finished, false otherwiseuse nalgebra::Vector3;
+use peng_quad::{Planner, SimulationError};
+struct TestPlanner;
+impl Planner for TestPlanner {
+ fn plan(
+ &self,
+ current_position: Vector3<f32>,
+ current_velocity: Vector3<f32>,
+ time: f32,
+) -> (Vector3<f32>, Vector3<f32>, f32) {
+ (Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 0.0)
+ }
+ fn is_finished(
+ &self,
+ current_position: Vector3<f32>,
+ time: f32,
+ ) -> Result<bool, SimulationError> {
+ Ok(true)
+ }
+}Implementation of the Planner trait for CirclePlanner
+Implementation of the Planner trait for the HoverPlanner
Implementation of the Planner trait for LandingPlanner
+Implementation of the planner trait for Lissajous curve trajectories
+Implementation of the planner trait for minimum jerk line planner
+Implement the Planner trait for MinimumSnapWaypointPlanner
Implementation of the Planner trait for ObstacleAvoidancePlanner
+U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nChecks if the current trajectory is finished\nChecks if the current trajectory is finished\nAttractive force gain\nRepulsive force gain\nVortex force gain\nPID gain for attitude control including proportional, …\nPID gain for position control including proportional, …\nThe last point that was logged\nLogs simulation data to the rerun recording stream\nlog depth image data to the rerun recording stream\nLog the maze obstacles to the rerun recording stream\nLog the maze tube to the rerun recording stream\nlog mesh data to the rerun recording stream\nlog trajectory data to the rerun recording stream\nThe lower bounds of the maze in the x, y, and z directions\nMass of the quadrotor in kg\nMass of the quadrotor\nMaximum allowed integral error for attitude\nMaximum allowed integral error for position\nMaximum speed of the quadrotor\nThe minimum distance between points to log\nThe near clipping plane of the camera\nCreates a new Quadrotor with default parameters\nCreates a new IMU with default parameters\nCreates a new PIDController with default gains gains are …\nCreates a new PlannerManager with an initial hover planner\nGenerate a new minimum snap waypoint planner\nCreates a new maze with the given bounds and number of …\nCreates a new camera with the given resolution, field of …\nCreate a new Trajectory instance\nCreates a new obstacle with the given position, velocity, …\nList of obstacles\nThe obstacles in the maze\nThe bounds of the obstacles’ radius\nThe bounds of the obstacles’ velocity\nCurrent orientation of the quadrotor\nAdditional parameters for the planner, stored as a YAML …\nHelper function to parse f32 from YAML\nHelper function to parse Vector3 from YAML\nPhase of the Lissajous curve\ncreates pinhole camera\nPlans the trajectory based on the current state and time\nPlans the trajectory based on the current planner type\nThe type of planner to use for this step.\nA vector of 3D points\nCurrent position of the quadrotor in 3D space\nThe position of the obstacle\nRadius of the circular trajectory\nThe radius of the obstacle\nRamp-up time for smooth transitions\nRamp-up time for smooth transitions\nCasts a ray from the camera origin in the given direction\nThe ray directions of each pixel in the camera\nSimulates IMU readings with added bias and noise\nSimulates IMU readings\nRenders the depth of the scene from the perspective of the …\nThe resolution of the camera\nSets a new planner\nSets the state of the quadrotor\nA smooth attractive force function that transitions from …\nStarting position of the trajectory\nStarting position of the trajectory\nStarting position of the trajectory\nStarting position of the landing maneuver\nStart time of the trajectory\nStart time of the trajectory\nStart time of the trajectory\nStart time of the landing maneuver\nStart time of the planner\nStart time of the trajectory\nStarting yaw angle\nStarting yaw angle\nStarting yaw angle\nStarting yaw angle\nStarting yaw angle\nCalculates the derivative of the state of the quadrotor\nThe simulation step at which this planner should be …\nTarget position for hovering\nTarget position of the planner\nTarget yaw angle for hovering\nSimulation time step in seconds\nList of segment times to reach each waypoint\nUpdates the IMU biases over time\nUpdates the current planner and returns the desired …\nUpdates the quadrotor’s dynamics with control inputs\nUpdates the quadrotor’s dynamics with control inputs …\nUpdates the obstacles in the maze, if an obstacle hits a …\nUpdates the planner based on the current simulation step …\nThe upper bounds of the maze in the x, y, and z directions\nCurrent velocity of the quadrotor\nThe velocity of the obstacle\nList of waypoints\nCoefficients for the yaw component of the trajectory\nList of yaw angles\nConfiguration for the camera\nConfiguration for the simulation\nConfiguration for the IMU\nConfiguration for the maze\nConfiguration for the mesh\nConfiguration for the PID controller\nConfiguration for PID gains\nConfiguration for a planner step\nConfiguration for the quadrotor\nConfiguration for the simulation\nAccelerometer bias drift standard deviation\nAccelerometer noise standard deviation\nAttitude gains\nMaximum integral error for attitude control\nCamera configuration\nControl frequency in Hz\nDivision of the 2D mesh, the mesh will be division x …\nDrag coefficient in Ns^2/m^2\nDuration of the simulation in seconds\nCamera far clipping plane in meters\nCamera field of view in height in degrees\nReturns the argument unchanged.\nReturns the argument unchanged.\nReturns the argument unchanged.\nReturns the argument unchanged.\nReturns the argument unchanged.\nReturns the argument unchanged.\nReturns the argument unchanged.\nReturns the argument unchanged.\nReturns the argument unchanged.\nReturns the argument unchanged.\nLoad configuration from a YAML file.\nGravity in m/s^2\nGyroscope bias drift standard deviation\nGyroscope noise standard deviation\nIMU configuration\nInertia matrix in kg*m^2\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nDerivative gains\nIntegral gains\nProportional gains\nLog frequency in Hz\nLower bounds of the maze in meters (x, y, z)\nMass of the quadrotor in kg\nMaze configuration\nMesh configuration\nCamera near clipping plane in meters\nNumber of obstacles in the maze\nObstacle radius bounds in meters (min, max)\nObstacle velocity maximum bounds in m/s in (x, y, z) …\nParameters for the planner\nPID Controller configuration\nPlanner schedule configuration\nType of planner to use\nPosition gains\nMaximum integral error for position control\nQuadrotor configuration\nRender depth\nCamera resolution in pixels (width, height)\nCamera transform matrix for depth\nSimulation configuration\nSimulation frequency in Hz\nSpacing between the squares in meters\nStep number that the planner should be executed (Unit: ms)\nUpper bounds of the maze in meters (x, y, z)\nMultiThreading depth rendering\nUse rerun.io for recording\nUse RK4 for updating quadrotor dynamics_with_controls\nUse RK4 for updating quadrotor dynamics without controls")
\ No newline at end of file
diff --git a/settings.html b/settings.html
new file mode 100644
index 00000000..9a427dc5
--- /dev/null
+++ b/settings.html
@@ -0,0 +1 @@
+1 +2 +3 +4 +5 +6 +7 +8 +9 +10 +11 +12 +13 +14 +15 +16 +17 +18 +19 +20 +21 +22 +23 +24 +25 +26 +27 +28 +29 +30 +31 +32 +33 +34 +35 +36 +37 +38 +39 +40 +41 +42 +43 +44 +45 +46 +47 +48 +49 +50 +51 +52 +53 +54 +55 +56 +57 +58 +59 +60 +61 +62 +63 +64 +65 +66 +67 +68 +69 +70 +71 +72 +73 +74 +75 +76 +77 +78 +79 +80 +81 +82 +83 +84 +85 +86 +87 +88 +89 +90 +91 +92 +93 +94 +95 +96 +97 +98 +99 +100 +101 +102 +103 +104 +105 +106 +107 +108 +109 +110 +111 +112 +113 +114 +115 +116 +117 +118 +119 +120 +121 +122 +123 +124 +125 +126 +127 +128 +129 +130 +131 +132 +133 +134 +135 +136 +137 +138 +139 +140 +141 +142 +143 +144 +145 +146 +147 +148 +149 +150 +151 +152 +153 +154 +155 +156 +157 +158 +159 +160 +161 +
//! Configuration module
+//! This module contains the configuration for the simulation, quadrotor, PID controller, IMU, maze, camera, mesh, and planner schedule.
+//! The configuration is loaded from a YAML file using the serde library.
+//! The configuration is then used to initialize the simulation, quadrotor, PID controller, IMU, maze, camera, mesh, and planner schedule.
+#[derive(serde::Deserialize)]
+/// Configuration for the simulation
+pub struct Config {
+ /// Simulation configuration
+ pub simulation: SimulationConfig,
+ /// Quadrotor configuration
+ pub quadrotor: QuadrotorConfig,
+ /// PID Controller configuration
+ pub pid_controller: PIDControllerConfig,
+ /// IMU configuration
+ pub imu: ImuConfig,
+ /// Maze configuration
+ pub maze: MazeConfig,
+ /// Camera configuration
+ pub camera: CameraConfig,
+ /// Mesh configuration
+ pub mesh: MeshConfig,
+ /// Planner schedule configuration
+ pub planner_schedule: Vec<PlannerStep>,
+ /// Use rerun.io for recording
+ pub use_rerun: bool,
+ /// Render depth
+ pub render_depth: bool,
+ /// MultiThreading depth rendering
+ pub use_multithreading_depth_rendering: bool,
+ /// Use RK4 for updating quadrotor dynamics_with_controls
+ pub use_rk4_for_dynamics_control: bool,
+ /// Use RK4 for updating quadrotor dynamics without controls
+ pub use_rk4_for_dynamics_update: bool,
+}
+
+#[derive(serde::Deserialize)]
+/// Configuration for a planner step
+pub struct PlannerStep {
+ /// Step number that the planner should be executed (Unit: ms)
+ pub step: usize,
+ /// Type of planner to use
+ pub planner_type: String,
+ /// Parameters for the planner
+ pub params: serde_yaml::Value,
+}
+
+#[derive(serde::Deserialize)]
+/// Configuration for the simulation
+pub struct SimulationConfig {
+ /// Control frequency in Hz
+ pub control_frequency: usize,
+ /// Simulation frequency in Hz
+ pub simulation_frequency: usize,
+ /// Log frequency in Hz
+ pub log_frequency: usize,
+ /// Duration of the simulation in seconds
+ pub duration: f32,
+}
+
+#[derive(serde::Deserialize)]
+/// Configuration for the quadrotor
+pub struct QuadrotorConfig {
+ /// Mass of the quadrotor in kg
+ pub mass: f32,
+ /// Gravity in m/s^2
+ pub gravity: f32,
+ /// Drag coefficient in Ns^2/m^2
+ pub drag_coefficient: f32,
+ /// Inertia matrix in kg*m^2
+ pub inertia_matrix: [f32; 9],
+}
+
+#[derive(serde::Deserialize)]
+/// Configuration for the PID controller
+pub struct PIDControllerConfig {
+ /// Position gains
+ pub pos_gains: PIDGains,
+ /// Attitude gains
+ pub att_gains: PIDGains,
+ /// Maximum integral error for position control
+ pub pos_max_int: [f32; 3],
+ /// Maximum integral error for attitude control
+ pub att_max_int: [f32; 3],
+}
+
+#[derive(serde::Deserialize)]
+/// Configuration for PID gains
+pub struct PIDGains {
+ /// Proportional gains
+ pub kp: [f32; 3],
+ /// Integral gains
+ pub ki: [f32; 3],
+ /// Derivative gains
+ pub kd: [f32; 3],
+}
+
+#[derive(serde::Deserialize, Default)]
+/// Configuration for the IMU
+pub struct ImuConfig {
+ /// Accelerometer noise standard deviation
+ pub accel_noise_std: f32,
+ /// Gyroscope noise standard deviation
+ pub gyro_noise_std: f32,
+ /// Accelerometer bias drift standard deviation
+ pub accel_bias_std: f32,
+ /// Gyroscope bias drift standard deviation
+ pub gyro_bias_std: f32,
+}
+
+#[derive(serde::Deserialize)]
+/// Configuration for the maze
+pub struct MazeConfig {
+ /// Upper bounds of the maze in meters (x, y, z)
+ pub upper_bounds: [f32; 3],
+ /// Lower bounds of the maze in meters (x, y, z)
+ pub lower_bounds: [f32; 3],
+ /// Number of obstacles in the maze
+ pub num_obstacles: usize,
+ /// Obstacle velocity maximum bounds in m/s in (x, y, z) directions
+ pub obstacles_velocity_bounds: [f32; 3],
+ /// Obstacle radius bounds in meters (min, max)
+ pub obstacles_radius_bounds: [f32; 2],
+}
+
+#[derive(serde::Deserialize)]
+/// Configuration for the camera
+pub struct CameraConfig {
+ /// Camera resolution in pixels (width, height)
+ pub resolution: (usize, usize),
+ /// Camera field of view in height in degrees
+ pub fov: f32,
+ /// Camera near clipping plane in meters
+ pub near: f32,
+ /// Camera far clipping plane in meters
+ pub far: f32,
+ /// Camera transform matrix for depth
+ pub rotation_transform: [f32; 9]
+}
+
+#[derive(serde::Deserialize)]
+/// Configuration for the mesh
+pub struct MeshConfig {
+ /// Division of the 2D mesh, the mesh will be division x division squares
+ pub division: usize,
+ /// Spacing between the squares in meters
+ pub spacing: f32,
+}
+/// Implementation of the Config struct
+impl Config {
+ /// Load configuration from a YAML file.
+ /// # Arguments
+ /// * `filename` - The name of the file to load.
+ /// # Returns
+ /// * The configuration object.
+ /// # Errors
+ /// * If the file cannot be read or the YAML cannot be parsed.
+ pub fn from_yaml(filename: &str) -> Result<Self, Box<dyn std::error::Error>> {
+ let contents = std::fs::read_to_string(filename)?;
+ Ok(serde_yaml::from_str(&contents)?)
+ }
+}
+1 +2 +3 +4 +5 +6 +7 +8 +9 +10 +11 +12 +13 +14 +15 +16 +17 +18 +19 +20 +21 +22 +23 +24 +25 +26 +27 +28 +29 +30 +31 +32 +33 +34 +35 +36 +37 +38 +39 +40 +41 +42 +43 +44 +45 +46 +47 +48 +49 +50 +51 +52 +53 +54 +55 +56 +57 +58 +59 +60 +61 +62 +63 +64 +65 +66 +67 +68 +69 +70 +71 +72 +73 +74 +75 +76 +77 +78 +79 +80 +81 +82 +83 +84 +85 +86 +87 +88 +89 +90 +91 +92 +93 +94 +95 +96 +97 +98 +99 +100 +101 +102 +103 +104 +105 +106 +107 +108 +109 +110 +111 +112 +113 +114 +115 +116 +117 +118 +119 +120 +121 +122 +123 +124 +125 +126 +127 +128 +129 +130 +131 +132 +133 +134 +135 +136 +137 +138 +139 +140 +141 +142 +143 +144 +145 +146 +147 +148 +149 +150 +151 +152 +153 +154 +155 +156 +157 +158 +159 +160 +161 +162 +163 +164 +165 +166 +167 +168 +169 +170 +171 +172 +173 +174 +175 +176 +177 +178 +179 +180 +181 +182 +183 +184 +185 +186 +187 +188 +189 +190 +191 +192 +193 +194 +195 +196 +197 +198 +199 +200 +201 +202 +203 +204 +205 +206 +207 +208 +209 +210 +211 +212 +213 +214 +215 +216 +217 +218 +219 +220 +221 +222 +223 +224 +225 +226 +227 +228 +229 +230 +231 +232 +233 +234 +235 +236 +237 +238 +239 +240 +241 +242 +243 +244 +245 +246 +247 +248 +249 +250 +251 +252 +253 +254 +255 +256 +257 +258 +259 +260 +261 +262 +263 +264 +265 +266 +267 +268 +269 +270 +271 +272 +273 +274 +275 +276 +277 +278 +279 +280 +281 +282 +283 +284 +285 +286 +287 +288 +289 +290 +291 +292 +293 +294 +295 +296 +297 +298 +299 +300 +301 +302 +303 +304 +305 +306 +307 +308 +309 +310 +311 +312 +313 +314 +315 +316 +317 +318 +319 +320 +321 +322 +323 +324 +325 +326 +327 +328 +329 +330 +331 +332 +333 +334 +335 +336 +337 +338 +339 +340 +341 +342 +343 +344 +345 +346 +347 +348 +349 +350 +351 +352 +353 +354 +355 +356 +357 +358 +359 +360 +361 +362 +363 +364 +365 +366 +367 +368 +369 +370 +371 +372 +373 +374 +375 +376 +377 +378 +379 +380 +381 +382 +383 +384 +385 +386 +387 +388 +389 +390 +391 +392 +393 +394 +395 +396 +397 +398 +399 +400 +401 +402 +403 +404 +405 +406 +407 +408 +409 +410 +411 +412 +413 +414 +415 +416 +417 +418 +419 +420 +421 +422 +423 +424 +425 +426 +427 +428 +429 +430 +431 +432 +433 +434 +435 +436 +437 +438 +439 +440 +441 +442 +443 +444 +445 +446 +447 +448 +449 +450 +451 +452 +453 +454 +455 +456 +457 +458 +459 +460 +461 +462 +463 +464 +465 +466 +467 +468 +469 +470 +471 +472 +473 +474 +475 +476 +477 +478 +479 +480 +481 +482 +483 +484 +485 +486 +487 +488 +489 +490 +491 +492 +493 +494 +495 +496 +497 +498 +499 +500 +501 +502 +503 +504 +505 +506 +507 +508 +509 +510 +511 +512 +513 +514 +515 +516 +517 +518 +519 +520 +521 +522 +523 +524 +525 +526 +527 +528 +529 +530 +531 +532 +533 +534 +535 +536 +537 +538 +539 +540 +541 +542 +543 +544 +545 +546 +547 +548 +549 +550 +551 +552 +553 +554 +555 +556 +557 +558 +559 +560 +561 +562 +563 +564 +565 +566 +567 +568 +569 +570 +571 +572 +573 +574 +575 +576 +577 +578 +579 +580 +581 +582 +583 +584 +585 +586 +587 +588 +589 +590 +591 +592 +593 +594 +595 +596 +597 +598 +599 +600 +601 +602 +603 +604 +605 +606 +607 +608 +609 +610 +611 +612 +613 +614 +615 +616 +617 +618 +619 +620 +621 +622 +623 +624 +625 +626 +627 +628 +629 +630 +631 +632 +633 +634 +635 +636 +637 +638 +639 +640 +641 +642 +643 +644 +645 +646 +647 +648 +649 +650 +651 +652 +653 +654 +655 +656 +657 +658 +659 +660 +661 +662 +663 +664 +665 +666 +667 +668 +669 +670 +671 +672 +673 +674 +675 +676 +677 +678 +679 +680 +681 +682 +683 +684 +685 +686 +687 +688 +689 +690 +691 +692 +693 +694 +695 +696 +697 +698 +699 +700 +701 +702 +703 +704 +705 +706 +707 +708 +709 +710 +711 +712 +713 +714 +715 +716 +717 +718 +719 +720 +721 +722 +723 +724 +725 +726 +727 +728 +729 +730 +731 +732 +733 +734 +735 +736 +737 +738 +739 +740 +741 +742 +743 +744 +745 +746 +747 +748 +749 +750 +751 +752 +753 +754 +755 +756 +757 +758 +759 +760 +761 +762 +763 +764 +765 +766 +767 +768 +769 +770 +771 +772 +773 +774 +775 +776 +777 +778 +779 +780 +781 +782 +783 +784 +785 +786 +787 +788 +789 +790 +791 +792 +793 +794 +795 +796 +797 +798 +799 +800 +801 +802 +803 +804 +805 +806 +807 +808 +809 +810 +811 +812 +813 +814 +815 +816 +817 +818 +819 +820 +821 +822 +823 +824 +825 +826 +827 +828 +829 +830 +831 +832 +833 +834 +835 +836 +837 +838 +839 +840 +841 +842 +843 +844 +845 +846 +847 +848 +849 +850 +851 +852 +853 +854 +855 +856 +857 +858 +859 +860 +861 +862 +863 +864 +865 +866 +867 +868 +869 +870 +871 +872 +873 +874 +875 +876 +877 +878 +879 +880 +881 +882 +883 +884 +885 +886 +887 +888 +889 +890 +891 +892 +893 +894 +895 +896 +897 +898 +899 +900 +901 +902 +903 +904 +905 +906 +907 +908 +909 +910 +911 +912 +913 +914 +915 +916 +917 +918 +919 +920 +921 +922 +923 +924 +925 +926 +927 +928 +929 +930 +931 +932 +933 +934 +935 +936 +937 +938 +939 +940 +941 +942 +943 +944 +945 +946 +947 +948 +949 +950 +951 +952 +953 +954 +955 +956 +957 +958 +959 +960 +961 +962 +963 +964 +965 +966 +967 +968 +969 +970 +971 +972 +973 +974 +975 +976 +977 +978 +979 +980 +981 +982 +983 +984 +985 +986 +987 +988 +989 +990 +991 +992 +993 +994 +995 +996 +997 +998 +999 +1000 +1001 +1002 +1003 +1004 +1005 +1006 +1007 +1008 +1009 +1010 +1011 +1012 +1013 +1014 +1015 +1016 +1017 +1018 +1019 +1020 +1021 +1022 +1023 +1024 +1025 +1026 +1027 +1028 +1029 +1030 +1031 +1032 +1033 +1034 +1035 +1036 +1037 +1038 +1039 +1040 +1041 +1042 +1043 +1044 +1045 +1046 +1047 +1048 +1049 +1050 +1051 +1052 +1053 +1054 +1055 +1056 +1057 +1058 +1059 +1060 +1061 +1062 +1063 +1064 +1065 +1066 +1067 +1068 +1069 +1070 +1071 +1072 +1073 +1074 +1075 +1076 +1077 +1078 +1079 +1080 +1081 +1082 +1083 +1084 +1085 +1086 +1087 +1088 +1089 +1090 +1091 +1092 +1093 +1094 +1095 +1096 +1097 +1098 +1099 +1100 +1101 +1102 +1103 +1104 +1105 +1106 +1107 +1108 +1109 +1110 +1111 +1112 +1113 +1114 +1115 +1116 +1117 +1118 +1119 +1120 +1121 +1122 +1123 +1124 +1125 +1126 +1127 +1128 +1129 +1130 +1131 +1132 +1133 +1134 +1135 +1136 +1137 +1138 +1139 +1140 +1141 +1142 +1143 +1144 +1145 +1146 +1147 +1148 +1149 +1150 +1151 +1152 +1153 +1154 +1155 +1156 +1157 +1158 +1159 +1160 +1161 +1162 +1163 +1164 +1165 +1166 +1167 +1168 +1169 +1170 +1171 +1172 +1173 +1174 +1175 +1176 +1177 +1178 +1179 +1180 +1181 +1182 +1183 +1184 +1185 +1186 +1187 +1188 +1189 +1190 +1191 +1192 +1193 +1194 +1195 +1196 +1197 +1198 +1199 +1200 +1201 +1202 +1203 +1204 +1205 +1206 +1207 +1208 +1209 +1210 +1211 +1212 +1213 +1214 +1215 +1216 +1217 +1218 +1219 +1220 +1221 +1222 +1223 +1224 +1225 +1226 +1227 +1228 +1229 +1230 +1231 +1232 +1233 +1234 +1235 +1236 +1237 +1238 +1239 +1240 +1241 +1242 +1243 +1244 +1245 +1246 +1247 +1248 +1249 +1250 +1251 +1252 +1253 +1254 +1255 +1256 +1257 +1258 +1259 +1260 +1261 +1262 +1263 +1264 +1265 +1266 +1267 +1268 +1269 +1270 +1271 +1272 +1273 +1274 +1275 +1276 +1277 +1278 +1279 +1280 +1281 +1282 +1283 +1284 +1285 +1286 +1287 +1288 +1289 +1290 +1291 +1292 +1293 +1294 +1295 +1296 +1297 +1298 +1299 +1300 +1301 +1302 +1303 +1304 +1305 +1306 +1307 +1308 +1309 +1310 +1311 +1312 +1313 +1314 +1315 +1316 +1317 +1318 +1319 +1320 +1321 +1322 +1323 +1324 +1325 +1326 +1327 +1328 +1329 +1330 +1331 +1332 +1333 +1334 +1335 +1336 +1337 +1338 +1339 +1340 +1341 +1342 +1343 +1344 +1345 +1346 +1347 +1348 +1349 +1350 +1351 +1352 +1353 +1354 +1355 +1356 +1357 +1358 +1359 +1360 +1361 +1362 +1363 +1364 +1365 +1366 +1367 +1368 +1369 +1370 +1371 +1372 +1373 +1374 +1375 +1376 +1377 +1378 +1379 +1380 +1381 +1382 +1383 +1384 +1385 +1386 +1387 +1388 +1389 +1390 +1391 +1392 +1393 +1394 +1395 +1396 +1397 +1398 +1399 +1400 +1401 +1402 +1403 +1404 +1405 +1406 +1407 +1408 +1409 +1410 +1411 +1412 +1413 +1414 +1415 +1416 +1417 +1418 +1419 +1420 +1421 +1422 +1423 +1424 +1425 +1426 +1427 +1428 +1429 +1430 +1431 +1432 +1433 +1434 +1435 +1436 +1437 +1438 +1439 +1440 +1441 +1442 +1443 +1444 +1445 +1446 +1447 +1448 +1449 +1450 +1451 +1452 +1453 +1454 +1455 +1456 +1457 +1458 +1459 +1460 +1461 +1462 +1463 +1464 +1465 +1466 +1467 +1468 +1469 +1470 +1471 +1472 +1473 +1474 +1475 +1476 +1477 +1478 +1479 +1480 +1481 +1482 +1483 +1484 +1485 +1486 +1487 +1488 +1489 +1490 +1491 +1492 +1493 +1494 +1495 +1496 +1497 +1498 +1499 +1500 +1501 +1502 +1503 +1504 +1505 +1506 +1507 +1508 +1509 +1510 +1511 +1512 +1513 +1514 +1515 +1516 +1517 +1518 +1519 +1520 +1521 +1522 +1523 +1524 +1525 +1526 +1527 +1528 +1529 +1530 +1531 +1532 +1533 +1534 +1535 +1536 +1537 +1538 +1539 +1540 +1541 +1542 +1543 +1544 +1545 +1546 +1547 +1548 +1549 +1550 +1551 +1552 +1553 +1554 +1555 +1556 +1557 +1558 +1559 +1560 +1561 +1562 +1563 +1564 +1565 +1566 +1567 +1568 +1569 +1570 +1571 +1572 +1573 +1574 +1575 +1576 +1577 +1578 +1579 +1580 +1581 +1582 +1583 +1584 +1585 +1586 +1587 +1588 +1589 +1590 +1591 +1592 +1593 +1594 +1595 +1596 +1597 +1598 +1599 +1600 +1601 +1602 +1603 +1604 +1605 +1606 +1607 +1608 +1609 +1610 +1611 +1612 +1613 +1614 +1615 +1616 +1617 +1618 +1619 +1620 +1621 +1622 +1623 +1624 +1625 +1626 +1627 +1628 +1629 +1630 +1631 +1632 +1633 +1634 +1635 +1636 +1637 +1638 +1639 +1640 +1641 +1642 +1643 +1644 +1645 +1646 +1647 +1648 +1649 +1650 +1651 +1652 +1653 +1654 +1655 +1656 +1657 +1658 +1659 +1660 +1661 +1662 +1663 +1664 +1665 +1666 +1667 +1668 +1669 +1670 +1671 +1672 +1673 +1674 +1675 +1676 +1677 +1678 +1679 +1680 +1681 +1682 +1683 +1684 +1685 +1686 +1687 +1688 +1689 +1690 +1691 +1692 +1693 +1694 +1695 +1696 +1697 +1698 +1699 +1700 +1701 +1702 +1703 +1704 +1705 +1706 +1707 +1708 +1709 +1710 +1711 +1712 +1713 +1714 +1715 +1716 +1717 +1718 +1719 +1720 +1721 +1722 +1723 +1724 +1725 +1726 +1727 +1728 +1729 +1730 +1731 +1732 +1733 +1734 +1735 +1736 +1737 +1738 +1739 +1740 +1741 +1742 +1743 +1744 +1745 +1746 +1747 +1748 +1749 +1750 +1751 +1752 +1753 +1754 +1755 +1756 +1757 +1758 +1759 +1760 +1761 +1762 +1763 +1764 +1765 +1766 +1767 +1768 +1769 +1770 +1771 +1772 +1773 +1774 +1775 +1776 +1777 +1778 +1779 +1780 +1781 +1782 +1783 +1784 +1785 +1786 +1787 +1788 +1789 +1790 +1791 +1792 +1793 +1794 +1795 +1796 +1797 +1798 +1799 +1800 +1801 +1802 +1803 +1804 +1805 +1806 +1807 +1808 +1809 +1810 +1811 +1812 +1813 +1814 +1815 +1816 +1817 +1818 +1819 +1820 +1821 +1822 +1823 +1824 +1825 +1826 +1827 +1828 +1829 +1830 +1831 +1832 +1833 +1834 +1835 +1836 +1837 +1838 +1839 +1840 +1841 +1842 +1843 +1844 +1845 +1846 +1847 +1848 +1849 +1850 +1851 +1852 +1853 +1854 +1855 +1856 +1857 +1858 +1859 +1860 +1861 +1862 +1863 +1864 +1865 +1866 +1867 +1868 +1869 +1870 +1871 +1872 +1873 +1874 +1875 +1876 +1877 +1878 +1879 +1880 +1881 +1882 +1883 +1884 +1885 +1886 +1887 +1888 +1889 +1890 +1891 +1892 +1893 +1894 +1895 +1896 +1897 +1898 +1899 +1900 +1901 +1902 +1903 +1904 +1905 +1906 +1907 +1908 +1909 +1910 +1911 +1912 +1913 +1914 +1915 +1916 +1917 +1918 +1919 +1920 +1921 +1922 +1923 +1924 +1925 +1926 +1927 +1928 +1929 +1930 +1931 +1932 +1933 +1934 +1935 +1936 +1937 +1938 +1939 +1940 +1941 +1942 +1943 +1944 +1945 +1946 +1947 +1948 +1949 +1950 +1951 +1952 +1953 +1954 +1955 +1956 +1957 +1958 +1959 +1960 +1961 +1962 +1963 +1964 +1965 +1966 +1967 +1968 +1969 +1970 +1971 +1972 +1973 +1974 +1975 +1976 +1977 +1978 +1979 +1980 +1981 +1982 +1983 +1984 +1985 +1986 +1987 +1988 +1989 +1990 +1991 +1992 +1993 +1994 +1995 +1996 +1997 +1998 +1999 +2000 +2001 +2002 +2003 +2004 +2005 +2006 +2007 +2008 +2009 +2010 +2011 +2012 +2013 +2014 +2015 +2016 +2017 +2018 +2019 +2020 +2021 +2022 +2023 +2024 +2025 +2026 +2027 +2028 +2029 +2030 +2031 +2032 +2033 +2034 +2035 +2036 +2037 +2038 +2039 +2040 +2041 +2042 +2043 +2044 +2045 +2046 +2047 +2048 +2049 +2050 +2051 +2052 +2053 +2054 +2055 +2056 +2057 +2058 +2059 +2060 +2061 +2062 +2063 +2064 +2065 +2066 +2067 +2068 +2069 +2070 +2071 +2072 +2073 +2074 +2075 +2076 +2077 +2078 +2079 +2080 +2081 +2082 +2083 +2084 +2085 +2086 +2087 +2088 +2089 +2090 +2091 +2092 +2093 +2094 +2095 +2096 +2097 +2098 +2099 +2100 +2101 +2102 +2103 +2104 +2105 +2106 +2107 +2108 +2109 +2110 +2111 +2112 +2113 +2114 +2115 +2116 +2117 +2118 +2119 +2120 +2121 +2122 +2123 +2124 +2125 +2126 +2127 +2128 +2129 +2130 +2131 +2132 +2133 +2134 +2135 +2136 +2137 +2138 +2139 +2140 +2141 +2142 +2143 +2144 +2145 +2146 +2147 +2148 +2149 +2150 +2151 +2152 +2153 +2154 +2155 +2156 +2157 +2158 +2159 +2160 +2161 +2162 +2163 +2164 +2165 +2166 +2167 +2168 +2169 +2170 +2171 +2172 +2173 +2174 +2175 +2176 +2177 +2178 +2179 +2180 +2181 +2182 +2183 +2184 +2185 +2186 +2187 +2188 +2189 +2190 +2191 +2192 +2193 +2194 +2195 +2196 +2197 +2198 +2199 +2200 +2201 +2202 +2203 +2204 +2205 +2206 +2207 +2208 +2209 +2210 +2211 +2212 +2213 +2214 +2215 +2216 +2217 +2218 +2219 +2220 +2221 +2222 +2223 +2224 +2225 +2226 +2227 +2228 +2229 +2230 +2231 +2232 +2233 +2234 +2235 +2236 +2237 +2238 +2239 +2240 +2241 +2242 +2243 +2244 +2245 +2246 +2247 +2248 +2249 +2250 +2251 +2252 +2253 +2254 +2255 +2256 +2257 +2258 +2259 +2260 +2261 +2262 +2263 +2264 +2265 +2266 +2267 +2268 +2269 +2270 +2271 +2272 +2273 +2274 +2275 +2276 +2277 +2278 +2279 +2280 +2281 +2282 +2283 +2284 +2285 +2286 +2287 +2288 +2289 +2290 +2291 +2292 +2293 +2294 +2295 +2296 +2297 +2298 +2299 +2300 +2301 +2302 +2303 +2304 +2305 +2306 +2307 +2308 +2309 +2310 +2311 +2312 +2313 +2314 +2315 +2316 +2317 +2318 +2319 +2320 +2321 +2322 +2323 +2324 +2325 +2326 +2327 +2328 +2329 +2330 +2331 +2332 +2333 +2334 +2335 +2336 +2337 +2338 +2339 +2340 +2341 +2342 +2343 +2344 +2345 +2346 +2347 +2348 +2349 +2350 +2351 +2352 +2353 +2354 +2355 +2356 +2357 +2358 +2359 +2360 +2361 +2362 +2363 +2364 +2365 +2366 +2367 +2368 +2369 +2370 +2371 +2372 +2373 +2374 +2375 +2376 +2377 +2378 +2379 +2380 +2381 +2382 +2383 +2384 +2385 +2386 +2387 +2388 +2389 +2390 +2391 +2392 +2393 +2394 +2395 +2396 +2397 +2398 +2399 +2400 +2401 +2402 +2403 +2404 +2405 +2406 +2407 +2408 +2409 +2410 +2411 +2412 +2413 +2414 +2415 +2416 +2417 +2418 +2419 +2420 +2421 +2422 +2423 +2424 +2425 +2426 +2427 +2428 +2429 +2430 +2431 +2432 +2433 +2434 +2435 +2436 +2437 +2438 +2439 +2440 +2441 +2442 +2443 +2444 +2445 +2446 +2447 +2448 +2449 +2450 +2451 +2452 +2453 +2454 +2455 +2456 +2457 +2458 +2459 +2460 +2461 +2462 +2463 +2464 +2465 +2466 +2467 +2468 +2469 +2470 +2471 +2472 +2473 +2474 +2475 +2476 +2477 +2478 +2479 +2480 +2481 +2482 +2483 +2484 +2485 +2486 +2487 +2488 +2489 +2490 +2491 +2492 +2493 +2494 +2495 +2496 +2497 +2498 +2499 +2500 +2501 +2502 +2503 +2504 +2505 +2506 +2507 +2508 +2509 +2510 +2511 +2512 +2513 +2514 +2515 +2516 +2517 +2518 +2519 +2520 +2521 +2522 +2523 +2524 +2525 +2526 +2527 +2528 +2529 +2530 +2531 +2532 +2533 +2534 +2535 +2536 +2537 +2538 +2539 +2540 +2541 +2542 +2543 +2544 +2545 +2546 +2547 +2548 +2549 +2550 +2551 +2552 +2553 +2554 +2555 +2556 +2557 +2558 +2559 +2560 +2561 +2562 +2563 +2564 +2565 +2566 +2567 +2568 +2569 +2570 +2571 +2572 +2573 +2574 +2575 +2576 +2577 +2578 +2579 +2580 +2581 +2582 +2583 +2584 +2585 +2586 +2587 +2588 +2589 +2590 +2591 +2592 +2593 +2594 +2595 +2596 +2597 +2598 +2599 +2600 +2601 +2602 +2603 +2604 +2605 +2606 +2607 +2608 +2609 +2610 +2611 +2612 +2613 +2614 +2615 +2616 +2617 +2618 +2619 +2620 +2621 +2622 +2623 +2624 +2625 +2626 +2627 +2628 +2629 +2630 +2631 +2632 +2633 +2634 +2635 +2636 +2637 +2638 +2639 +2640 +2641 +2642 +2643 +2644 +2645 +2646 +2647 +2648 +2649 +2650 +2651 +2652 +2653 +2654 +2655 +2656 +2657 +2658 +2659 +2660 +2661 +2662 +2663 +2664 +2665 +2666 +2667 +2668 +2669 +2670 +2671 +2672 +2673 +2674 +2675 +2676 +2677 +2678 +2679 +2680 +2681 +2682 +2683 +2684 +2685 +2686 +2687 +2688 +
//! # 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
+//! ## Example
+//! ```
+//! use nalgebra::Vector3;
+//! use peng_quad::{Quadrotor, SimulationError};
+//! let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+//! let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+//! let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix);
+//! ```
+use rand::SeedableRng;
+use rayon::prelude::*;
+pub mod config;
+use nalgebra::{Matrix3, Quaternion, Rotation3, SMatrix, UnitQuaternion, Vector3};
+use rand_chacha::ChaCha8Rng;
+use rand_distr::{Distribution, Normal};
+use std::f32::consts::PI;
+#[derive(thiserror::Error, Debug)]
+/// Represents errors that can occur during simulation
+/// # Example
+/// ```
+/// use peng_quad::SimulationError;
+/// let error = SimulationError::NalgebraError("Matrix inversion failed".to_string());
+/// ```
+pub enum SimulationError {
+ /// Error related to Rerun visualization
+ #[error("Rerun error: {0}")]
+ RerunError(#[from] rerun::RecordingStreamError),
+ /// Error related to Rerun spawn process
+ #[error("Rerun spawn error: {0}")]
+ RerunSpawnError(#[from] rerun::SpawnError),
+ /// Error related to linear algebra operations
+ #[error("Nalgebra error: {0}")]
+ NalgebraError(String),
+ /// Error related to normal distribution calculations
+ #[error("Normal error: {0}")]
+ NormalError(#[from] rand_distr::NormalError),
+ /// Other general errors
+ #[error("Other error: {0}")]
+ OtherError(String),
+}
+/// Represents a quadrotor with its physical properties and state
+/// # Example
+/// ```
+/// use nalgebra::Vector3;
+/// use peng_quad::Quadrotor;
+/// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+/// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+/// let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix);
+/// ```
+pub struct Quadrotor {
+ /// Current position of the quadrotor in 3D space
+ pub position: Vector3<f32>,
+ /// Current velocity of the quadrotor
+ pub velocity: Vector3<f32>,
+ /// Current orientation of the quadrotor
+ pub orientation: UnitQuaternion<f32>,
+ /// Current angular velocity of the quadrotor
+ pub angular_velocity: Vector3<f32>,
+ /// 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<f32>,
+ /// Inverse of the inertia matrix
+ pub inertia_matrix_inv: Matrix3<f32>,
+}
+/// Implementation of the Quadrotor struct
+impl Quadrotor {
+ /// Creates a new Quadrotor with default parameters
+ /// # Arguments
+ /// * `time_step` - The simulation time step in seconds
+ /// * `mass` - The mass of the quadrotor in kg
+ /// * `gravity` - The gravitational acceleration in m/s^2
+ /// * `drag_coefficient` - The drag coefficient
+ /// * `inertia_matrix` - The inertia matrix of the quadrotor
+ /// # Returns
+ /// * A new Quadrotor instance
+ /// # Errors
+ /// * Returns a SimulationError if the inertia matrix cannot be inverted
+ /// # Example
+ /// ```
+ /// use nalgebra::Vector3;
+ /// use peng_quad::Quadrotor;
+ ///
+ /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+ /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+ /// let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix);
+ /// ```
+ pub fn new(
+ time_step: f32,
+ mass: f32,
+ gravity: f32,
+ drag_coefficient: f32,
+ inertia_matrix: [f32; 9],
+ ) -> Result<Self, SimulationError> {
+ 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,
+ gravity,
+ time_step,
+ drag_coefficient,
+ 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
+ /// # Example
+ /// ```
+ /// use nalgebra::Vector3;
+ /// use peng_quad::Quadrotor;
+ ///
+ /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+ /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+ /// let mut quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+ /// let control_thrust = mass * gravity;
+ /// let control_torque = Vector3::new(0.0, 0.0, 0.0);
+ /// quadrotor.update_dynamics_with_controls_euler(control_thrust, &control_torque);
+ /// ```
+ pub fn update_dynamics_with_controls_euler(
+ &mut self,
+ control_thrust: f32,
+ control_torque: &Vector3<f32>,
+ ) {
+ 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);
+ }
+ /// Updates the quadrotor's dynamics with control inputs using the Runge-Kutta 4th order method
+ /// # Arguments
+ /// * `control_thrust` - The total thrust force applied to the quadrotor
+ /// * `control_torque` - The 3D torque vector applied to the quadrotor
+ /// # Example
+ /// ```
+ /// use nalgebra::Vector3;
+ /// use peng_quad::Quadrotor;
+ /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+ /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+ /// let mut quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+ /// let control_thrust = mass * gravity;
+ /// let control_torque = Vector3::new(0.0, 0.0, 0.0);
+ /// quadrotor.update_dynamics_with_controls_rk4(control_thrust, &control_torque);
+ /// ```
+ pub fn update_dynamics_with_controls_rk4(
+ &mut self,
+ control_thrust: f32,
+ control_torque: &Vector3<f32>,
+ ) {
+ let h = self.time_step;
+ let state = self.get_state();
+
+ let k1 = self.state_derivative(&state, control_thrust, control_torque);
+ let mut temp_state = [0.0; 13];
+ for i in 0..13 {
+ temp_state[i] = state[i] + 0.5 * h * k1[i];
+ }
+ let k2 = self.state_derivative(&temp_state, control_thrust, control_torque);
+
+ for i in 0..13 {
+ temp_state[i] = state[i] + 0.5 * h * k2[i];
+ }
+ let k3 = self.state_derivative(&temp_state, control_thrust, control_torque);
+
+ for i in 0..13 {
+ temp_state[i] = state[i] + h * k3[i];
+ }
+ let k4 = self.state_derivative(&temp_state, control_thrust, control_torque);
+
+ let mut new_state = [0.0; 13];
+ for i in 0..13 {
+ new_state[i] = state[i] + (h / 6.0) * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]);
+ }
+
+ let mut q = Quaternion::new(new_state[9], new_state[6], new_state[7], new_state[8]);
+ q = q.normalize();
+ new_state[6..10].copy_from_slice(q.coords.as_slice());
+
+ self.set_state(&new_state);
+ }
+ /// Returns the state derivative of the quadrotor
+ /// # Arguments
+ /// * `state` - The state of the quadrotor
+ /// # Example
+ /// ```
+ /// use nalgebra::Vector3;
+ /// use peng_quad::Quadrotor;
+ /// use nalgebra::UnitQuaternion;
+ /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+ /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+ /// let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+ /// let state = quadrotor.get_state();
+ /// ```
+ pub fn get_state(&self) -> [f32; 13] {
+ let mut state = [0.0; 13];
+ state[0..3].copy_from_slice(self.position.as_slice());
+ state[3..6].copy_from_slice(self.velocity.as_slice());
+ state[6..10].copy_from_slice(self.orientation.coords.as_slice());
+ state[10..13].copy_from_slice(self.angular_velocity.as_slice());
+ state
+ }
+ /// Sets the state of the quadrotor
+ /// # Arguments
+ /// * `state` - The state of the quadrotor
+ /// # Example
+ /// ```
+ /// use nalgebra::Vector3;
+ /// use peng_quad::Quadrotor;
+ /// use nalgebra::UnitQuaternion;
+ /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+ /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+ /// let mut quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+ /// let state = [
+ /// 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+ /// ];
+ /// quadrotor.set_state(&state);
+ /// ```
+ pub fn set_state(&mut self, state: &[f32; 13]) {
+ self.position = Vector3::from_column_slice(&state[0..3]);
+ self.velocity = Vector3::from_column_slice(&state[3..6]);
+ self.orientation = UnitQuaternion::from_quaternion(Quaternion::new(
+ state[9], state[6], state[7], state[8],
+ ));
+ self.angular_velocity = Vector3::from_column_slice(&state[10..13]);
+ }
+ /// Calculates the derivative of the state of the quadrotor
+ /// # Arguments
+ /// * `state` - The current state of the quadrotor
+ /// * `control_thrust` - The thrust applied to the quadrotor
+ /// * `control_torque` - The torque applied to the quadrotor
+ /// # Returns
+ /// The derivative of the state
+ /// # Example
+ /// ```
+ /// use nalgebra::Vector3;
+ /// use peng_quad::Quadrotor;
+ /// use nalgebra::UnitQuaternion;
+ /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+ /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+ /// let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+ /// let state = [
+ /// 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+ /// ];
+ /// let control_thrust = 0.0;
+ /// let control_torque = Vector3::new(0.0, 0.0, 0.0);
+ /// let derivative = quadrotor.state_derivative(&state, control_thrust, &control_torque);
+ /// ```
+ pub fn state_derivative(
+ &self,
+ state: &[f32],
+ control_thrust: f32,
+ control_torque: &Vector3<f32>,
+ ) -> [f32; 13] {
+ let velocity = Vector3::from_column_slice(&state[3..6]);
+ let orientation = UnitQuaternion::from_quaternion(Quaternion::new(
+ state[9], state[6], state[7], state[8],
+ ));
+ // Quaternion deriviative
+ let omega_quat = Quaternion::new(0.0, state[10], state[11], state[12]);
+ let q_dot = orientation.into_inner() * omega_quat * 0.5;
+
+ let angular_velocity = Vector3::from_column_slice(&state[10..13]);
+
+ let gravity_force = Vector3::new(0.0, 0.0, -self.mass * self.gravity);
+ let drag_force = -self.drag_coefficient * velocity.norm() * velocity;
+ let thrust_world = orientation * Vector3::new(0.0, 0.0, control_thrust);
+ let acceleration = (thrust_world + gravity_force + drag_force) / self.mass;
+
+ let inertia_angular_velocity = self.inertia_matrix * angular_velocity;
+ let gyroscopic_torque = angular_velocity.cross(&inertia_angular_velocity);
+ let angular_acceleration = self.inertia_matrix_inv * (control_torque - gyroscopic_torque);
+
+ let mut derivative = [0.0; 13];
+ derivative[0..3].copy_from_slice(velocity.as_slice());
+ derivative[3..6].copy_from_slice(acceleration.as_slice());
+ derivative[6..10].copy_from_slice(q_dot.coords.as_slice());
+ derivative[10..13].copy_from_slice(angular_acceleration.as_slice());
+ derivative
+ }
+ /// Simulates IMU readings
+ /// # Returns
+ /// * A tuple containing the true acceleration and angular velocity of the quadrotor
+ /// # Errors
+ /// * Returns a SimulationError if the IMU readings cannot be calculated
+ /// # Example
+ /// ```
+ /// use nalgebra::Vector3;
+ /// use peng_quad::Quadrotor;
+ ///
+ /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+ /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+ /// let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+ /// let (true_acceleration, true_angular_velocity) = quadrotor.read_imu().unwrap();
+ /// ```
+ pub fn read_imu(&self) -> Result<(Vector3<f32>, Vector3<f32>), 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
+/// # Example
+/// ```
+/// use nalgebra::Vector3;
+/// use peng_quad::Imu;
+/// let accel_noise_std = 0.0003;
+/// let gyro_noise_std = 0.02;
+/// let accel_bias_std = 0.0001;
+/// let gyro_bias_std = 0.001;
+/// let imu = Imu::new(accel_noise_std, gyro_noise_std, accel_bias_std, gyro_bias_std);
+/// ```
+pub struct Imu {
+ /// Accelerometer bias
+ pub accel_bias: Vector3<f32>,
+ /// Gyroscope bias
+ pub gyro_bias: Vector3<f32>,
+ /// Standard deviation of accelerometer noise
+ pub accel_noise_std: f32,
+ /// Standard deviation of gyroscope noise
+ pub gyro_noise_std: f32,
+ /// Standard deviation of accelerometer bias drift
+ pub accel_bias_std: f32,
+ /// Standard deviation of gyroscope bias drift
+ pub gyro_bias_std: f32,
+ /// Accelerometer noise distribution
+ accel_noise: Normal<f32>,
+ /// Gyroscope noise distribution
+ gyro_noise: Normal<f32>,
+ /// Accelerometer bias drift distribution
+ accel_bias_drift: Normal<f32>,
+ /// Gyroscope bias drift distribution
+ gyro_bias_drift: Normal<f32>,
+ /// Random number generator
+ rng: ChaCha8Rng,
+}
+/// Implements the IMU
+impl Imu {
+ /// Creates a new IMU with default parameters
+ /// # Arguments
+ /// * `accel_noise_std` - Standard deviation of accelerometer noise
+ /// * `gyro_noise_std` - Standard deviation of gyroscope noise
+ /// * `accel_bias_std` - Standard deviation of accelerometer bias drift
+ /// * `gyro_bias_std` - Standard deviation of gyroscope bias drift
+ /// # Returns
+ /// * A new Imu instance
+ /// # Example
+ /// ```
+ /// use peng_quad::Imu;
+ ///
+ /// let imu = Imu::new(0.01, 0.01, 0.01, 0.01);
+ /// ```
+ pub fn new(
+ accel_noise_std: f32,
+ gyro_noise_std: f32,
+ accel_bias_std: f32,
+ gyro_bias_std: f32,
+ ) -> Result<Self, SimulationError> {
+ Ok(Self {
+ accel_bias: Vector3::zeros(),
+ gyro_bias: Vector3::zeros(),
+ accel_noise_std,
+ gyro_noise_std,
+ accel_bias_std,
+ gyro_bias_std,
+ accel_noise: Normal::new(0.0, accel_noise_std)?,
+ gyro_noise: Normal::new(0.0, gyro_noise_std)?,
+ accel_bias_drift: Normal::new(0.0, accel_bias_std)?,
+ gyro_bias_drift: Normal::new(0.0, gyro_bias_std)?,
+ rng: ChaCha8Rng::from_entropy(),
+ })
+ }
+ /// Updates the IMU biases over time
+ /// # Arguments
+ /// * `dt` - Time step for the update
+ /// # Errors
+ /// * Returns a SimulationError if the bias drift cannot be calculated
+ /// # Example
+ /// ```
+ /// use peng_quad::Imu;
+ ///
+ /// let mut imu = Imu::new(0.01, 0.01, 0.01, 0.01).unwrap();
+ /// imu.update(0.01).unwrap();
+ /// ```
+ pub fn update(&mut self, dt: f32) -> Result<(), SimulationError> {
+ let dt_sqrt = fast_sqrt(dt);
+ let accel_drift = self.accel_bias_drift.sample(&mut self.rng) * dt_sqrt;
+ let gyro_drift = self.gyro_bias_drift.sample(&mut self.rng) * dt_sqrt;
+ self.accel_bias += Vector3::from_iterator((0..3).map(|_| accel_drift));
+ self.gyro_bias += Vector3::from_iterator((0..3).map(|_| gyro_drift));
+ Ok(())
+ }
+ /// Simulates IMU readings with added bias and noise
+ /// # 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
+ /// # Errors
+ /// * Returns a SimulationError if the IMU readings cannot be calculated
+ /// # Example
+ /// ```
+ /// use nalgebra::Vector3;
+ /// use peng_quad::Imu;
+ ///
+ /// let mut imu = Imu::new(0.01, 0.01, 0.01, 0.01).unwrap();
+ /// let true_acceleration = Vector3::new(0.0, 0.0, 9.81);
+ /// let true_angular_velocity = Vector3::new(0.0, 0.0, 0.0);
+ /// let (measured_acceleration, measured_ang_velocity) = imu.read(true_acceleration, true_angular_velocity).unwrap();
+ /// ```
+ pub fn read(
+ &mut self,
+ true_acceleration: Vector3<f32>,
+ true_angular_velocity: Vector3<f32>,
+ ) -> Result<(Vector3<f32>, Vector3<f32>), SimulationError> {
+ let accel_noise_sample =
+ Vector3::from_iterator((0..3).map(|_| self.accel_noise.sample(&mut self.rng)));
+ let gyro_noise_sample =
+ Vector3::from_iterator((0..3).map(|_| self.gyro_noise.sample(&mut self.rng)));
+ let measured_acceleration = true_acceleration + self.accel_bias + accel_noise_sample;
+ let measured_ang_velocity = true_angular_velocity + self.gyro_bias + gyro_noise_sample;
+ Ok((measured_acceleration, measured_ang_velocity))
+ }
+}
+/// PID controller for quadrotor position and attitude control
+/// # Example
+/// ```
+/// use nalgebra::Vector3;
+/// use peng_quad::PIDController;
+/// let kpid_pos = [
+/// [1.0, 1.0, 1.0],
+/// [0.1, 0.1, 0.1],
+/// [0.01, 0.01, 0.01],
+/// ];
+/// let kpid_att = [
+/// [1.0, 1.0, 1.0],
+/// [0.1, 0.1, 0.1],
+/// [0.01, 0.01, 0.01],
+/// ];
+/// let max_integral_pos = [1.0, 1.0, 1.0];
+/// let max_integral_att = [1.0, 1.0, 1.0];
+/// let mass = 1.0;
+/// let gravity = 9.81;
+/// let pid_controller = PIDController::new(kpid_pos, kpid_att, max_integral_pos, max_integral_att, mass, gravity);
+/// ```
+pub struct PIDController {
+ /// PID gain for position control including proportional, derivative, and integral gains
+ pub kpid_pos: [Vector3<f32>; 3],
+ /// PID gain for attitude control including proportional, derivative, and integral gains
+ pub kpid_att: [Vector3<f32>; 3],
+ /// Accumulated integral error for position
+ pub integral_pos_error: Vector3<f32>,
+ /// Accumulated integral error for attitude
+ pub integral_att_error: Vector3<f32>,
+ /// Maximum allowed integral error for position
+ pub max_integral_pos: Vector3<f32>,
+ /// Maximum allowed integral error for attitude
+ pub max_integral_att: Vector3<f32>,
+ /// Mass of the quadrotor
+ pub mass: f32,
+ /// Gravity constant
+ pub gravity: f32,
+}
+/// Implementation of PIDController
+impl PIDController {
+ /// Creates a new PIDController with default gains
+ /// gains are in the order of proportional, derivative, and integral
+ /// # Arguments
+ /// * `_kpid_pos` - PID gains for position control
+ /// * `_kpid_att` - PID gains for attitude control
+ /// * `_max_integral_pos` - Maximum allowed integral error for position
+ /// * `_max_integral_att` - Maximum allowed integral error for attitude
+ /// # Returns
+ /// * A new PIDController instance
+ /// # Example
+ /// ```
+ /// use nalgebra::Vector3;
+ /// use peng_quad::PIDController;
+ /// let kpid_pos = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
+ /// let kpid_att = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
+ /// let max_integral_pos = [1.0, 1.0, 1.0];
+ /// let max_integral_att = [1.0, 1.0, 1.0];
+ /// let mass = 1.0;
+ /// let gravity = 9.81;
+ /// let pid = PIDController::new(kpid_pos, kpid_att, max_integral_pos, max_integral_att, mass, gravity);
+ /// ```
+ pub fn new(
+ _kpid_pos: [[f32; 3]; 3],
+ _kpid_att: [[f32; 3]; 3],
+ _max_integral_pos: [f32; 3],
+ _max_integral_att: [f32; 3],
+ _mass: f32,
+ _gravity: f32,
+ ) -> 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),
+ mass: _mass,
+ gravity: _gravity,
+ }
+ }
+ /// 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 attitude control torques
+ /// # Example
+ /// ```
+ /// use nalgebra::{UnitQuaternion, Vector3};
+ /// use peng_quad::PIDController;
+ ///
+ /// let kpid_pos = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
+ /// let kpid_att = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
+ /// let max_integral_pos = [1.0, 1.0, 1.0];
+ /// let max_integral_att = [1.0, 1.0, 1.0];
+ /// let mass = 1.0;
+ /// let gravity = 9.81;
+ /// let mut pid = PIDController::new(kpid_pos, kpid_att, max_integral_pos, max_integral_att, mass, gravity);
+ /// let desired_orientation = UnitQuaternion::identity();
+ /// let current_orientation = UnitQuaternion::identity();
+ /// let current_angular_velocity = Vector3::zeros();
+ /// let dt = 0.01;
+ /// let control_torques = pid.compute_attitude_control(&desired_orientation, ¤t_orientation, ¤t_angular_velocity, dt);
+ /// ```
+ pub fn compute_attitude_control(
+ &mut self,
+ desired_orientation: &UnitQuaternion<f32>,
+ current_orientation: &UnitQuaternion<f32>,
+ current_angular_velocity: &Vector3<f32>,
+ dt: f32,
+ ) -> Vector3<f32> {
+ 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
+ /// # Example
+ /// ```
+ /// use nalgebra::{UnitQuaternion, Vector3};
+ /// use peng_quad::PIDController;
+ ///
+ /// let kpid_pos = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
+ /// let kpid_att = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
+ /// let max_integral_pos = [1.0, 1.0, 1.0];
+ /// let max_integral_att = [1.0, 1.0, 1.0];
+ /// let mass = 1.0;
+ /// let gravity = 9.81;
+ /// let mut pid = PIDController::new(kpid_pos, kpid_att, max_integral_pos, max_integral_att, mass, gravity);
+ /// let desired_position = Vector3::new(0.0, 0.0, 1.0);
+ /// let desired_velocity = Vector3::zeros();
+ /// let desired_yaw = 0.0;
+ /// let current_position = Vector3::zeros();
+ /// let current_velocity = Vector3::zeros();
+ /// let dt = 0.01;
+ /// let (thrust, desired_orientation) = pid.compute_position_control(&desired_position, &desired_velocity, desired_yaw, ¤t_position, ¤t_velocity, dt);
+ /// ```
+ pub fn compute_position_control(
+ &mut self,
+ desired_position: &Vector3<f32>,
+ desired_velocity: &Vector3<f32>,
+ desired_yaw: f32,
+ current_position: &Vector3<f32>,
+ current_velocity: &Vector3<f32>,
+ dt: f32,
+ ) -> (f32, UnitQuaternion<f32>) {
+ 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, self.gravity);
+ let total_acceleration = acceleration + gravity_compensation;
+ let thrust = self.mass * total_acceleration.norm();
+ let desired_orientation = if total_acceleration.norm() > 1e-6 {
+ let z_body = total_acceleration.normalize();
+ let 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
+/// # Example
+/// ```
+/// use peng_quad::PlannerType;
+/// use peng_quad::HoverPlanner;
+/// let hover_planner : PlannerType = PlannerType::Hover(HoverPlanner{
+/// target_position: nalgebra::Vector3::new(0.0, 0.0, 1.0),
+/// target_yaw: 0.0,
+/// });
+/// ```
+pub enum PlannerType {
+ /// Hover planner
+ Hover(HoverPlanner),
+ /// Minimum jerk line planner
+ MinimumJerkLine(MinimumJerkLinePlanner),
+ /// Minimum jerk circle planner
+ Lissajous(LissajousPlanner),
+ /// Minimum jerk circle planner
+ Circle(CirclePlanner),
+ /// Minimum jerk landing planner
+ Landing(LandingPlanner),
+ /// Obstacle avoidance planner
+ ObstacleAvoidance(ObstacleAvoidancePlanner),
+ /// Minimum snap waypoint planner
+ MinimumSnapWaypoint(MinimumSnapWaypointPlanner),
+}
+/// Implementation of the planner type
+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
+ /// # Example
+ /// ```
+ /// use nalgebra::Vector3;
+ /// use peng_quad::PlannerType;
+ /// use peng_quad::HoverPlanner;
+ /// let hover_planner = HoverPlanner {
+ /// target_position: Vector3::new(0.0, 0.0, 1.0),
+ /// target_yaw: 0.0
+ /// };
+ /// let hover_planner_type = PlannerType::Hover(hover_planner);
+ /// let (desired_position, desired_velocity, desired_yaw) = hover_planner_type.plan(Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 0.0);
+ /// ```
+ pub fn plan(
+ &self,
+ current_position: Vector3<f32>,
+ current_velocity: Vector3<f32>,
+ time: f32,
+ ) -> (Vector3<f32>, Vector3<f32>, 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
+ /// # Example
+ /// ```
+ /// use nalgebra::Vector3;
+ /// use peng_quad::PlannerType;
+ /// use peng_quad::HoverPlanner;
+ /// use peng_quad::Planner;
+ /// let hover_planner = HoverPlanner{
+ /// target_position: Vector3::new(0.0, 0.0, 1.0),
+ /// target_yaw: 0.0,
+ /// };
+ /// let is_finished = hover_planner.is_finished(Vector3::new(0.0, 0.0, 0.0), 0.0);
+ /// ```
+ pub fn is_finished(
+ &self,
+ current_position: Vector3<f32>,
+ time: f32,
+ ) -> Result<bool, SimulationError> {
+ match self {
+ PlannerType::Hover(p) => p.is_finished(current_position, time),
+ PlannerType::MinimumJerkLine(p) => p.is_finished(current_position, time),
+ PlannerType::Lissajous(p) => p.is_finished(current_position, time),
+ PlannerType::Circle(p) => p.is_finished(current_position, time),
+ PlannerType::Landing(p) => p.is_finished(current_position, time),
+ PlannerType::ObstacleAvoidance(p) => p.is_finished(current_position, time),
+ PlannerType::MinimumSnapWaypoint(p) => p.is_finished(current_position, time),
+ }
+ }
+}
+/// Trait defining the interface for trajectory planners
+/// # Example
+/// ```
+/// use nalgebra::Vector3;
+/// use peng_quad::{Planner, SimulationError};
+/// struct TestPlanner;
+/// impl Planner for TestPlanner {
+/// fn plan(
+/// &self,
+/// current_position: Vector3<f32>,
+/// current_velocity: Vector3<f32>,
+/// time: f32,
+/// ) -> (Vector3<f32>, Vector3<f32>, f32) {
+/// (Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 0.0)
+/// }
+/// fn is_finished(
+/// &self,
+/// current_position: Vector3<f32>,
+/// time: f32,
+/// ) -> Result<bool, SimulationError> {
+/// Ok(true)
+/// }
+/// }
+/// ```
+pub 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
+ /// # Example
+ /// ```
+ /// use nalgebra::Vector3;
+ /// use peng_quad::{Planner, SimulationError};
+ /// struct TestPlanner;
+ /// impl Planner for TestPlanner {
+ /// fn plan(
+ /// &self,
+ /// current_position: Vector3<f32>,
+ /// current_velocity: Vector3<f32>,
+ /// time: f32,
+ /// ) -> (Vector3<f32>, Vector3<f32>, f32) {
+ /// (Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 0.0)
+ /// }
+ /// fn is_finished(
+ /// &self,
+ /// current_position: Vector3<f32>,
+ /// time: f32,
+ /// ) -> Result<bool, SimulationError> {
+ /// Ok(true)
+ /// }
+ /// }
+ /// ```
+ fn plan(
+ &self,
+ current_position: Vector3<f32>,
+ current_velocity: Vector3<f32>,
+ time: f32,
+ ) -> (Vector3<f32>, Vector3<f32>, 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
+ /// # Example
+ /// ```
+ /// use nalgebra::Vector3;
+ /// use peng_quad::{Planner, SimulationError};
+ /// struct TestPlanner;
+ /// impl Planner for TestPlanner {
+ /// fn plan(
+ /// &self,
+ /// current_position: Vector3<f32>,
+ /// current_velocity: Vector3<f32>,
+ /// time: f32,
+ /// ) -> (Vector3<f32>, Vector3<f32>, f32) {
+ /// (Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 0.0)
+ /// }
+ /// fn is_finished(
+ /// &self,
+ /// current_position: Vector3<f32>,
+ /// time: f32,
+ /// ) -> Result<bool, SimulationError> {
+ /// Ok(true)
+ /// }
+ /// }
+ /// ```
+ fn is_finished(
+ &self,
+ current_position: Vector3<f32>,
+ time: f32,
+ ) -> Result<bool, SimulationError>;
+}
+/// Planner for hovering at a fixed position
+/// # Example
+/// ```
+/// use nalgebra::Vector3;
+/// use peng_quad::HoverPlanner;
+/// let hover_planner = HoverPlanner {
+/// target_position: Vector3::new(0.0, 0.0, 0.0),
+/// target_yaw: 0.0,
+/// };
+/// ```
+pub struct HoverPlanner {
+ /// Target position for hovering
+ pub target_position: Vector3<f32>,
+ /// Target yaw angle for hovering
+ pub target_yaw: f32,
+}
+/// Implementation of the `Planner` trait for the `HoverPlanner`
+impl Planner for HoverPlanner {
+ fn plan(
+ &self,
+ _current_position: Vector3<f32>,
+ _current_velocity: Vector3<f32>,
+ _time: f32,
+ ) -> (Vector3<f32>, Vector3<f32>, f32) {
+ (self.target_position, Vector3::zeros(), self.target_yaw)
+ }
+
+ fn is_finished(
+ &self,
+ _current_position: Vector3<f32>,
+ _time: f32,
+ ) -> Result<bool, SimulationError> {
+ Ok(false) // Hover planner never "finished"
+ }
+}
+/// Planner for minimum jerk trajectories along a straight line
+/// # Example
+/// ```
+/// use nalgebra::Vector3;
+/// use peng_quad::MinimumJerkLinePlanner;
+/// let minimum_jerk_line_planner = MinimumJerkLinePlanner {
+/// start_position: Vector3::new(0.0, 0.0, 0.0),
+/// end_position: Vector3::new(1.0, 1.0, 1.0),
+/// start_yaw: 0.0,
+/// end_yaw: 0.0,
+/// start_time: 0.0,
+/// duration: 1.0,
+/// };
+/// ```
+pub struct MinimumJerkLinePlanner {
+ /// Starting position of the trajectory
+ pub start_position: Vector3<f32>,
+ /// Ending position of the trajectory
+ pub end_position: Vector3<f32>,
+ /// Starting yaw angle
+ pub start_yaw: f32,
+ /// Ending yaw angle
+ pub end_yaw: f32,
+ /// Start time of the trajectory
+ pub start_time: f32,
+ /// Duration of the trajectory
+ pub duration: f32,
+}
+/// Implementation of the planner trait for minimum jerk line planner
+impl Planner for MinimumJerkLinePlanner {
+ fn plan(
+ &self,
+ _current_position: Vector3<f32>,
+ _current_velocity: Vector3<f32>,
+ time: f32,
+ ) -> (Vector3<f32>, Vector3<f32>, f32) {
+ let t = ((time - self.start_time) / self.duration).clamp(0.0, 1.0);
+ let s = 10.0 * t.powi(3) - 15.0 * t.powi(4) + 6.0 * t.powi(5);
+ let 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<f32>,
+ _time: f32,
+ ) -> Result<bool, SimulationError> {
+ Ok((_current_position - self.end_position).norm() < 0.01
+ && _time >= self.start_time + self.duration)
+ }
+}
+/// Planner for Lissajous curve trajectories
+/// # Example
+/// ```
+/// use nalgebra::Vector3;
+/// use peng_quad::LissajousPlanner;
+/// let lissajous_planner = LissajousPlanner {
+/// start_position: Vector3::new(0.0, 0.0, 0.0),
+/// center: Vector3::new(1.0, 1.0, 1.0),
+/// amplitude: Vector3::new(1.0, 1.0, 1.0),
+/// frequency: Vector3::new(1.0, 1.0, 1.0),
+/// phase: Vector3::new(0.0, 0.0, 0.0),
+/// start_time: 0.0,
+/// duration: 1.0,
+/// start_yaw: 0.0,
+/// end_yaw: 0.0,
+/// ramp_time: 0.1,
+/// };
+/// ```
+pub struct LissajousPlanner {
+ /// Starting position of the trajectory
+ pub start_position: Vector3<f32>,
+ /// Center of the Lissajous curve
+ pub center: Vector3<f32>,
+ /// Amplitude of the Lissajous curve
+ pub amplitude: Vector3<f32>,
+ /// Frequency of the Lissajous curve
+ pub frequency: Vector3<f32>,
+ /// Phase of the Lissajous curve
+ pub phase: Vector3<f32>,
+ /// Start time of the trajectory
+ pub start_time: f32,
+ /// Duration of the trajectory
+ pub duration: f32,
+ /// Starting yaw angle
+ pub start_yaw: f32,
+ /// Ending yaw angle
+ pub end_yaw: f32,
+ /// Ramp-up time for smooth transitions
+ pub ramp_time: f32,
+}
+/// Implementation of the planner trait for Lissajous curve trajectories
+impl Planner for LissajousPlanner {
+ fn plan(
+ &self,
+ _current_position: Vector3<f32>,
+ _current_velocity: Vector3<f32>,
+ time: f32,
+ ) -> (Vector3<f32>, Vector3<f32>, 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<f32>,
+ time: f32,
+ ) -> Result<bool, SimulationError> {
+ Ok(time >= self.start_time + self.duration)
+ }
+}
+/// Planner for circular trajectories
+/// # Example
+/// ```
+/// use nalgebra::Vector3;
+/// use peng_quad::CirclePlanner;
+/// let circle_planner = CirclePlanner {
+/// center: Vector3::new(1.0, 1.0, 1.0),
+/// radius: 1.0,
+/// angular_velocity: 1.0,
+/// start_position: Vector3::new(0.0, 0.0, 0.0),
+/// start_time: 0.0,
+/// duration: 1.0,
+/// start_yaw: 0.0,
+/// end_yaw: 0.0,
+/// ramp_time: 0.1,
+/// };
+/// ```
+pub struct CirclePlanner {
+ /// Center of the circular trajectory
+ pub center: Vector3<f32>,
+ /// Radius of the circular trajectory
+ pub radius: f32,
+ /// Angular velocity of the circular motion
+ pub angular_velocity: f32,
+ /// Starting position of the trajectory
+ pub start_position: Vector3<f32>,
+ /// Start time of the trajectory
+ pub start_time: f32,
+ /// Duration of the trajectory
+ pub duration: f32,
+ /// Starting yaw angle
+ pub start_yaw: f32,
+ /// Ending yaw angle
+ pub end_yaw: f32,
+ /// Ramp-up time for smooth transitions
+ pub ramp_time: f32,
+}
+/// Implementation of the Planner trait for CirclePlanner
+impl Planner for CirclePlanner {
+ fn plan(
+ &self,
+ _current_position: Vector3<f32>,
+ _current_velocity: Vector3<f32>,
+ time: f32,
+ ) -> (Vector3<f32>, Vector3<f32>, 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<f32>,
+ time: f32,
+ ) -> Result<bool, SimulationError> {
+ Ok(time >= self.start_time + self.duration)
+ }
+}
+/// Planner for landing maneuvers
+/// # Example
+/// ```
+/// use nalgebra::Vector3;
+/// use peng_quad::LandingPlanner;
+/// let landing_planner = LandingPlanner {
+/// start_position: Vector3::new(0.0, 0.0, 1.0),
+/// start_time: 0.0,
+/// duration: 1.0,
+/// start_yaw: 0.0,
+/// };
+/// ```
+pub struct LandingPlanner {
+ /// Starting position of the landing maneuver
+ pub start_position: Vector3<f32>,
+ /// Start time of the landing maneuver
+ pub start_time: f32,
+ /// Duration of the landing maneuver
+ pub duration: f32,
+ /// Starting yaw angle
+ pub start_yaw: f32,
+}
+/// Implementation of the Planner trait for LandingPlanner
+impl Planner for LandingPlanner {
+ fn plan(
+ &self,
+ _current_position: Vector3<f32>,
+ _current_velocity: Vector3<f32>,
+ time: f32,
+ ) -> (Vector3<f32>, Vector3<f32>, 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<f32>,
+ time: f32,
+ ) -> Result<bool, SimulationError> {
+ Ok(current_position.z < 0.05 || time >= self.start_time + self.duration)
+ }
+}
+/// Manages different trajectory planners and switches between them
+/// # Example
+/// ```
+/// use nalgebra::Vector3;
+/// use peng_quad::PlannerManager;
+/// let initial_position = Vector3::new(0.0, 0.0, 1.0);
+/// let initial_yaw = 0.0;
+/// let planner_manager = PlannerManager::new(initial_position, initial_yaw);
+/// ```
+pub struct PlannerManager {
+ /// The current planner
+ pub current_planner: PlannerType,
+}
+/// Implementation of the PlannerManager
+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
+ /// # Example
+ /// ```
+ /// use nalgebra::Vector3;
+ /// use peng_quad::PlannerManager;
+ /// let initial_position = Vector3::new(0.0, 0.0, 1.0);
+ /// let initial_yaw = 0.0;
+ /// let planner_manager = PlannerManager::new(initial_position, initial_yaw);
+ /// ```
+ pub fn new(initial_position: Vector3<f32>, 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
+ /// # Example
+ /// ```
+ /// use nalgebra::Vector3;
+ /// use peng_quad::{PlannerManager, CirclePlanner, PlannerType};
+ /// let initial_position = Vector3::new(0.0, 0.0, 1.0);
+ /// let initial_yaw = 0.0;
+ /// let mut planner_manager = PlannerManager::new(initial_position, initial_yaw);
+ /// let new_planner = CirclePlanner {
+ /// center: Vector3::new(0.0, 0.0, 1.0),
+ /// radius: 1.0,
+ /// angular_velocity: 1.0,
+ /// start_yaw: 0.0,
+ /// end_yaw: 0.0,
+ /// start_time: 0.0,
+ /// duration: 10.0,
+ /// ramp_time: 1.0,
+ /// start_position: Vector3::new(0.0, 0.0, 1.0),
+ /// };
+ /// planner_manager.set_planner(PlannerType::Circle(new_planner));
+ /// ```
+ 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
+ /// # Errors
+ /// * Returns a SimulationError if the current planner is not finished
+ /// # Example
+ /// ```
+ /// use nalgebra::{Vector3, UnitQuaternion};
+ /// use peng_quad::{PlannerManager, SimulationError};
+ /// let initial_position = Vector3::new(0.0, 0.0, 1.0);
+ /// let initial_yaw = 0.0;
+ /// let mut planner_manager = PlannerManager::new(initial_position, initial_yaw);
+ /// let current_position = Vector3::new(0.0, 0.0, 1.0);
+ /// let current_orientation = UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0);
+ /// let current_velocity = Vector3::new(0.0, 0.0, 0.0);
+ /// let obstacles = vec![];
+ /// let time = 0.0;
+ /// let result = planner_manager.update(current_position, current_orientation, current_velocity, time, &obstacles);
+ /// match result {
+ /// Ok((target_position, target_velocity, target_yaw)) => {
+ /// println!("Target Position: {:?}", target_position);
+ /// println!("Target Velocity: {:?}", target_velocity);
+ /// println!("Target Yaw: {:?}", target_yaw);
+ /// }
+ /// Err(SimulationError) => {
+ /// log::error!("Error: Planner is not finished");
+ /// }
+ /// }
+ /// ```
+ pub fn update(
+ &mut self,
+ current_position: Vector3<f32>,
+ current_orientation: UnitQuaternion<f32>,
+ current_velocity: Vector3<f32>,
+ time: f32,
+ obstacles: &[Obstacle],
+ ) -> Result<(Vector3<f32>, Vector3<f32>, f32), SimulationError> {
+ if self.current_planner.is_finished(current_position, time)? {
+ log::info!("Time: {:.2} s,\tSwitch Hover", 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.to_owned();
+ }
+ 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
+/// # Example
+/// ```
+/// use nalgebra::Vector3;
+/// use peng_quad::{ObstacleAvoidancePlanner, Obstacle};
+/// let planner = ObstacleAvoidancePlanner {
+/// target_position: Vector3::new(0.0, 0.0, 1.0),
+/// start_time: 0.0,
+/// duration: 10.0,
+/// start_yaw: 0.0,
+/// end_yaw: 0.0,
+/// obstacles: vec![Obstacle {
+/// position: Vector3::new(1.0, 0.0, 1.0),
+/// velocity: Vector3::new(0.0, 0.0, 0.0),
+/// radius: 0.5,
+/// }],
+/// k_att: 1.0,
+/// k_rep: 1.0,
+/// k_vortex: 1.0,
+/// d0: 1.0,
+/// d_target: 1.0,
+/// max_speed: 1.0,
+/// };
+/// ```
+pub struct ObstacleAvoidancePlanner {
+ /// Target position of the planner
+ pub target_position: Vector3<f32>,
+ /// Start time of the planner
+ pub start_time: f32,
+ /// Duration of the planner
+ pub duration: f32,
+ /// Starting yaw angle
+ pub start_yaw: f32,
+ /// Ending yaw angle
+ pub end_yaw: f32,
+ /// List of obstacles
+ pub obstacles: Vec<Obstacle>,
+ /// Attractive force gain
+ pub k_att: f32,
+ /// Repulsive force gain
+ pub k_rep: f32,
+ /// Vortex force gain
+ pub k_vortex: f32,
+ /// Influence distance of obstacles
+ pub d0: f32,
+ /// Influence distance of target
+ pub d_target: f32,
+ /// Maximum speed of the quadrotor
+ pub max_speed: f32,
+}
+/// Implementation of the Planner trait for ObstacleAvoidancePlanner
+impl Planner for ObstacleAvoidancePlanner {
+ fn plan(
+ &self,
+ current_position: Vector3<f32>,
+ current_velocity: Vector3<f32>,
+ time: f32,
+ ) -> (Vector3<f32>, Vector3<f32>, f32) {
+ let t = ((time - self.start_time) / self.duration).clamp(0.0, 1.0);
+ let distance_to_target = (self.target_position - current_position).norm();
+ let f_att = self.k_att
+ * self.smooth_attractive_force(distance_to_target)
+ * (self.target_position - current_position).normalize();
+ // Repulsive force from obstacles
+ let mut f_rep = Vector3::zeros();
+ let mut f_vortex = 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();
+ f_vortex +=
+ self.k_vortex * current_velocity.cross(&diff).normalize() / distance.powi(2);
+ }
+ }
+ let f_total = f_att + f_rep + f_vortex;
+ let desired_velocity = f_total.normalize() * self.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<f32>,
+ time: f32,
+ ) -> Result<bool, SimulationError> {
+ Ok((current_position - self.target_position).norm() < 0.1
+ && time >= self.start_time + self.duration)
+ }
+}
+/// Implementation of the ObstacleAvoidancePlanner
+impl ObstacleAvoidancePlanner {
+ /// A smooth attractive force function that transitions from linear to exponential decay
+ /// When the distance to the target is less than the target distance, the force is linear
+ /// When the distance is greater, the force decays exponentially
+ /// # Arguments
+ /// * `distance` - The distance to the target
+ /// # Returns
+ /// * The attractive force
+ /// # Example
+ /// ```
+ /// use peng_quad::ObstacleAvoidancePlanner;
+ /// let planner = ObstacleAvoidancePlanner {
+ /// target_position: nalgebra::Vector3::new(0.0, 0.0, 1.0),
+ /// start_time: 0.0,
+ /// duration: 10.0,
+ /// start_yaw: 0.0,
+ /// end_yaw: 0.0,
+ /// obstacles: vec![],
+ /// k_att: 1.0,
+ /// k_rep: 1.0,
+ /// k_vortex: 1.0,
+ /// d0: 1.0,
+ /// d_target: 1.0,
+ /// max_speed: 1.0,
+ /// };
+ /// let distance = 1.0;
+ /// let force = planner.smooth_attractive_force(distance);
+ /// ```
+ #[inline]
+ pub fn smooth_attractive_force(&self, distance: f32) -> f32 {
+ if distance <= self.d_target {
+ distance
+ } else {
+ self.d_target + (distance - self.d_target).tanh()
+ }
+ }
+}
+/// Waypoint planner that generates a minimum snap trajectory between waypoints
+/// # Example
+/// ```
+/// use peng_quad::MinimumSnapWaypointPlanner;
+/// use nalgebra::Vector3;
+/// let planner = MinimumSnapWaypointPlanner::new(
+/// vec![Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 0.0, 0.0)],
+/// vec![0.0, 0.0],
+/// vec![1.0],
+/// 0.0,
+/// );
+/// ```
+pub struct MinimumSnapWaypointPlanner {
+ /// List of waypoints
+ pub waypoints: Vec<Vector3<f32>>,
+ /// List of yaw angles
+ pub yaws: Vec<f32>,
+ /// List of segment times to reach each waypoint
+ pub times: Vec<f32>,
+ /// Coefficients for the x, y, and z components of the trajectory
+ pub coefficients: Vec<Vec<Vector3<f32>>>,
+ /// Coefficients for the yaw component of the trajectory
+ pub yaw_coefficients: Vec<Vec<f32>>,
+ /// Start time of the trajectory
+ pub start_time: f32,
+}
+/// Implementation of the MinimumSnapWaypointPlanner
+impl MinimumSnapWaypointPlanner {
+ /// Generate a new minimum snap waypoint planner
+ /// # Arguments
+ /// * `waypoints` - List of waypoints
+ /// * `yaws` - List of yaw angles
+ /// * `segment_times` - List of segment times to reach each waypoint
+ /// * `start_time` - Start time of the trajectory
+ /// # Returns
+ /// * A new minimum snap waypoint planner
+ /// # Errors
+ /// * Returns an error if the number of waypoints, yaws, and segment times do not match
+ /// # Example
+ /// ```
+ /// use peng_quad::MinimumSnapWaypointPlanner;
+ /// use nalgebra::Vector3;
+ /// let waypoints = vec![Vector3::zeros(), Vector3::new(1.0, 0.0, 0.0)];
+ /// let yaws = vec![0.0, 0.0];
+ /// let segment_times = vec![1.0];
+ /// let start_time = 0.0;
+ /// let planner = MinimumSnapWaypointPlanner::new(waypoints, yaws, segment_times, start_time);
+ /// ```
+ pub fn new(
+ waypoints: Vec<Vector3<f32>>,
+ yaws: Vec<f32>,
+ segment_times: Vec<f32>,
+ start_time: f32,
+ ) -> Result<Self, SimulationError> {
+ 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
+ /// # Errors
+ /// * Returns an error if the nalgebra solver fails to solve the linear system
+ /// # Example
+ /// ```
+ /// use peng_quad::MinimumSnapWaypointPlanner;
+ /// use nalgebra::Vector3;
+ /// let waypoints = vec![Vector3::zeros(), Vector3::new(1.0, 0.0, 0.0)];
+ /// let yaws = vec![0.0, 0.0];
+ /// let segment_times = vec![1.0];
+ /// let start_time = 0.0;
+ /// let mut planner = MinimumSnapWaypointPlanner::new(waypoints, yaws, segment_times, start_time).unwrap();
+ /// planner.compute_minimum_snap_trajectories();
+ /// ```
+ pub 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::<f32, 8, 8>::zeros();
+ let mut b = SMatrix::<f32, 8, 3>::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(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
+ /// # Errors
+ /// * Returns an error if nalgebra fails to solve for the coefficients
+ /// # Example
+ /// ```
+ /// use peng_quad::MinimumSnapWaypointPlanner;
+ /// use nalgebra::Vector3;
+ /// let waypoints = vec![Vector3::zeros(), Vector3::new(1.0, 0.0, 0.0)];
+ /// let yaws = vec![0.0, 0.0];
+ /// let segment_times = vec![1.0];
+ /// let start_time = 0.0;
+ /// let mut planner = MinimumSnapWaypointPlanner::new(waypoints, yaws, segment_times, start_time).unwrap();
+ /// planner.compute_minimum_snap_trajectories();
+ /// planner.compute_minimum_acceleration_yaw_trajectories();
+ /// ```
+ pub 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, start_yaw, end_yaw) = (self.times[i], self.yaws[i], self.yaws[i + 1]);
+ let mut a = SMatrix::<f32, 4, 4>::zeros();
+ let mut b = SMatrix::<f32, 4, 1>::zeros();
+ (a[(0, 0)], a[(1, 1)]) = (1.0, 1.0);
+ (b[0], b[2]) = (start_yaw, end_yaw);
+ 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);
+ }
+ }
+ 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)
+ /// # Example
+ /// ```
+ /// use nalgebra::Vector3;
+ /// use peng_quad::MinimumSnapWaypointPlanner;
+ /// let waypoints = vec![Vector3::zeros(), Vector3::new(1.0, 0.0, 0.0)];
+ /// let yaws = vec![0.0, 0.0];
+ /// let segment_times = vec![1.0];
+ /// let start_time = 0.0;
+ /// let mut planner = MinimumSnapWaypointPlanner::new(waypoints, yaws, segment_times, start_time).unwrap();
+ /// planner.compute_minimum_snap_trajectories();
+ /// planner.compute_minimum_acceleration_yaw_trajectories();
+ /// let (position, velocity, yaw, yaw_rate) = planner.evaluate_polynomial(0.5, &planner.coefficients[0], &planner.yaw_coefficients[0]);
+ /// ```
+ pub fn evaluate_polynomial(
+ &self,
+ t: f32,
+ coeffs: &[Vector3<f32>],
+ yaw_coeffs: &[f32],
+ ) -> (Vector3<f32>, Vector3<f32>, 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)
+ }
+}
+/// Implement the `Planner` trait for `MinimumSnapWaypointPlanner`
+impl Planner for MinimumSnapWaypointPlanner {
+ fn plan(
+ &self,
+ _current_position: Vector3<f32>,
+ _current_velocity: Vector3<f32>,
+ time: f32,
+ ) -> (Vector3<f32>, Vector3<f32>, 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<f32>,
+ time: f32,
+ ) -> Result<bool, SimulationError> {
+ let last_waypoint = self.waypoints.last().ok_or(SimulationError::OtherError(
+ "No waypoints available".to_string(),
+ ))?;
+ Ok(time >= self.start_time + self.times.iter().sum::<f32>()
+ && (current_position - last_waypoint).norm() < 0.1)
+ }
+}
+/// Represents a step in the planner schedule.
+/// # Example
+/// ```
+/// use peng_quad::PlannerStepConfig;
+/// let step = PlannerStepConfig {
+/// step: 0,
+/// planner_type: "MinimumJerkLocalPlanner".to_string(),
+/// params: serde_yaml::Value::Null,
+/// };
+/// ```
+pub struct PlannerStepConfig {
+ /// The simulation step at which this planner should be activated (in ms unit).
+ pub step: usize,
+ /// The type of planner to use for this step.
+ pub planner_type: String,
+ /// Additional parameters for the planner, stored as a YAML value.
+ pub params: serde_yaml::Value,
+}
+/// Updates the planner based on the current simulation step and configuration
+/// # Arguments
+/// * `planner_manager` - The PlannerManager instance to update
+/// * `step` - The current simulation step in ms unit
+/// * `time` - The current simulation time
+/// * `simulation_frequency' - The simulation frequency in Hz
+/// * `quad` - The Quadrotor instance
+/// * `obstacles` - The current obstacles in the simulation
+/// * `planner_config` - The planner configuration
+/// # Errors
+/// * If the planner could not be created
+/// # Example
+/// ```
+/// use peng_quad::{PlannerManager, Quadrotor, Obstacle, PlannerStepConfig, update_planner};
+/// use nalgebra::Vector3;
+/// let simulation_frequency = 1000;
+/// let initial_position = Vector3::new(0.0, 0.0, 0.0);
+/// let initial_yaw = 0.0;
+/// let mut planner_manager = PlannerManager::new(initial_position, initial_yaw);
+/// let step = 0;
+/// let time = 0.0;
+/// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+/// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+/// let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+/// let obstacles = vec![Obstacle::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 1.0)];
+/// let planner_config = vec![PlannerStepConfig {
+/// step: 0,
+/// planner_type: "MinimumJerkLine".to_string(),
+/// params:
+/// serde_yaml::from_str(r#"
+/// end_position: [0.0, 0.0, 1.0]
+/// end_yaw: 0.0
+/// duration: 2.0
+/// "#).unwrap(),
+/// }];
+/// update_planner(&mut planner_manager, step, time, simulation_frequency, &quadrotor, &obstacles, &planner_config).unwrap();
+/// ```
+pub fn update_planner(
+ planner_manager: &mut PlannerManager,
+ step: usize,
+ time: f32,
+ simulation_frequency: usize,
+ quad: &Quadrotor,
+ obstacles: &[Obstacle],
+ planner_config: &[PlannerStepConfig],
+) -> Result<(), SimulationError> {
+ if let Some(planner_step) = planner_config
+ .iter()
+ .find(|s| s.step * simulation_frequency == step * 1000)
+ {
+ log::info!("Time: {:.2} s,\tSwitch {}", time, planner_step.planner_type);
+ planner_manager.set_planner(create_planner(planner_step, quad, time, obstacles)?);
+ }
+ Ok(())
+}
+/// Creates a planner based on the configuration
+/// # Arguments
+/// * `step` - The configuration for the planner step in ms unit
+/// * `quad` - The Quadrotor instance
+/// * `time` - The current simulation time
+/// * `obstacles` - The current obstacles in the simulation
+/// # Returns
+/// * `PlannerType` - The created planner
+/// # Errors
+/// * If the planner type is not recognized
+/// # Example
+/// ```
+/// use peng_quad::{PlannerType, Quadrotor, Obstacle, PlannerStepConfig, create_planner};
+/// use nalgebra::Vector3;
+/// let step = PlannerStepConfig {
+/// step: 0,
+/// planner_type: "MinimumJerkLine".to_string(),
+/// params:
+/// serde_yaml::from_str(r#"
+/// end_position: [0.0, 0.0, 1.0]
+/// end_yaw: 0.0
+/// duration: 2.0
+/// "#).unwrap(),
+/// };
+/// let time = 0.0;
+/// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+/// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+/// let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+/// let obstacles = vec![Obstacle::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 1.0)];
+/// let planner = create_planner(&step, &quadrotor, time, &obstacles).unwrap();
+/// match planner {
+/// PlannerType::MinimumJerkLine(_) => log::info!("Created MinimumJerkLine planner"),
+/// _ => log::info!("Created another planner"),
+/// }
+/// ```
+pub fn create_planner(
+ step: &PlannerStepConfig,
+ quad: &Quadrotor,
+ time: f32,
+ obstacles: &[Obstacle],
+) -> Result<PlannerType, SimulationError> {
+ let params = &step.params;
+ match step.planner_type.as_str() {
+ "MinimumJerkLine" => Ok(PlannerType::MinimumJerkLine(MinimumJerkLinePlanner {
+ start_position: quad.position,
+ end_position: parse_vector3(params, "end_position")?,
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: parse_f32(params, "end_yaw")?,
+ start_time: time,
+ duration: parse_f32(params, "duration")?,
+ })),
+ "Lissajous" => Ok(PlannerType::Lissajous(LissajousPlanner {
+ start_position: quad.position,
+ center: parse_vector3(params, "center")?,
+ amplitude: parse_vector3(params, "amplitude")?,
+ frequency: parse_vector3(params, "frequency")?,
+ phase: parse_vector3(params, "phase")?,
+ start_time: time,
+ duration: parse_f32(params, "duration")?,
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: parse_f32(params, "end_yaw")?,
+ ramp_time: parse_f32(params, "ramp_time")?,
+ })),
+ "Circle" => Ok(PlannerType::Circle(CirclePlanner {
+ center: parse_vector3(params, "center")?,
+ radius: parse_f32(params, "radius")?,
+ angular_velocity: parse_f32(params, "angular_velocity")?,
+ start_position: quad.position,
+ start_time: time,
+ duration: parse_f32(params, "duration")?,
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: quad.orientation.euler_angles().2,
+ ramp_time: parse_f32(params, "ramp_time")?,
+ })),
+ "ObstacleAvoidance" => Ok(PlannerType::ObstacleAvoidance(ObstacleAvoidancePlanner {
+ target_position: parse_vector3(params, "target_position")?,
+ start_time: time,
+ duration: parse_f32(params, "duration")?,
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: parse_f32(params, "end_yaw")?,
+ obstacles: obstacles.to_owned(),
+ k_att: parse_f32(params, "k_att")?,
+ k_rep: parse_f32(params, "k_rep")?,
+ k_vortex: parse_f32(params, "k_vortex")?,
+ d0: parse_f32(params, "d0")?,
+ d_target: parse_f32(params, "d_target")?,
+ max_speed: parse_f32(params, "max_speed")?,
+ })),
+ "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[0].as_f64()? as f32,
+ coords[1].as_f64()? as f32,
+ coords[2].as_f64()? as f32,
+ ))
+ })
+ .ok_or(SimulationError::OtherError("Invalid waypoint".to_string()))
+ })
+ .collect::<Result<Vec<Vector3<f32>>, SimulationError>>()?,
+ );
+ let mut yaws = vec![quad.orientation.euler_angles().2];
+ yaws.extend(
+ params["yaws"]
+ .as_sequence()
+ .ok_or(SimulationError::OtherError("Invalid yaws".to_string()))?
+ .iter()
+ .map(|y| {
+ y.as_f64()
+ .map(|v| v as f32)
+ .ok_or(SimulationError::OtherError("Invalid yaw".to_string()))
+ })
+ .collect::<Result<Vec<f32>, SimulationError>>()?,
+ );
+ 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::<Result<Vec<f32>, SimulationError>>()?;
+ MinimumSnapWaypointPlanner::new(waypoints, yaws, segment_times, time)
+ .map(PlannerType::MinimumSnapWaypoint)
+ }
+ "Landing" => Ok(PlannerType::Landing(LandingPlanner {
+ start_position: quad.position,
+ start_time: time,
+ duration: parse_f32(params, "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
+/// # Arguments
+/// * `value` - YAML value
+/// * `key` - key to parse
+/// # Returns
+/// * `Vector3<f32>` - parsed vector
+/// # Errors
+/// * `SimulationError` - if the value is not a valid vector
+/// # Example
+/// ```
+/// use nalgebra::Vector3;
+/// use peng_quad::{parse_vector3, SimulationError};
+/// let value = serde_yaml::from_str("test: [1.0, 2.0, 3.0]").unwrap();
+/// assert_eq!(parse_vector3(&value, "test").unwrap(), Vector3::new(1.0, 2.0, 3.0));
+/// ```
+pub fn parse_vector3(
+ value: &serde_yaml::Value,
+ key: &str,
+) -> Result<Vector3<f32>, 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
+/// # Arguments
+/// * `value` - YAML value
+/// * `key` - key to parse
+/// # Returns
+/// * `f32` - parsed value
+/// # Errors
+/// * `SimulationError` - if the value is not a valid f32
+/// # Example
+/// ```
+/// use peng_quad::{parse_f32, SimulationError};
+/// let value = serde_yaml::from_str("key: 1.0").unwrap();
+/// let result = parse_f32(&value, "key").unwrap();
+/// assert_eq!(result, 1.0);
+/// ```
+pub fn parse_f32(value: &serde_yaml::Value, key: &str) -> Result<f32, SimulationError> {
+ value[key]
+ .as_f64()
+ .map(|v| v as f32)
+ .ok_or_else(|| SimulationError::OtherError(format!("Invalid {}", key)))
+}
+/// Represents an obstacle in the simulation
+/// # Example
+/// ```
+/// use peng_quad::Obstacle;
+/// use nalgebra::Vector3;
+/// let obstacle = Obstacle::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 1.0);
+/// ```
+#[derive(Clone)]
+pub struct Obstacle {
+ /// The position of the obstacle
+ pub position: Vector3<f32>,
+ /// The velocity of the obstacle
+ pub velocity: Vector3<f32>,
+ /// The radius of the obstacle
+ pub radius: f32,
+}
+/// Implementation of the Obstacle
+impl Obstacle {
+ /// Creates a new obstacle with the given position, velocity, and radius
+ /// # Arguments
+ /// * `position` - The position of the obstacle
+ /// * `velocity` - The velocity of the obstacle
+ /// * `radius` - The radius of the obstacle
+ /// # Returns
+ /// * The new obstacle instance
+ /// # Example
+ /// ```
+ /// use peng_quad::Obstacle;
+ /// use nalgebra::Vector3;
+ /// let obstacle = Obstacle::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 1.0);
+ /// ```
+ pub fn new(position: Vector3<f32>, velocity: Vector3<f32>, radius: f32) -> Self {
+ Self {
+ position,
+ velocity,
+ radius,
+ }
+ }
+}
+/// Represents a maze in the simulation
+/// # Example
+/// ```
+/// use peng_quad::{Maze, Obstacle};
+/// use nalgebra::Vector3;
+/// let maze = Maze {
+/// lower_bounds: [0.0, 0.0, 0.0],
+/// upper_bounds: [1.0, 1.0, 1.0],
+/// obstacles: vec![Obstacle::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 1.0)],
+/// obstacles_velocity_bounds: [0.0, 0.0, 0.0],
+/// obstacles_radius_bounds: [0.0, 0.0],
+/// };
+/// ```
+pub struct Maze {
+ /// The lower bounds of the maze in the x, y, and z directions
+ pub lower_bounds: [f32; 3],
+ /// The upper bounds of the maze in the x, y, and z directions
+ pub upper_bounds: [f32; 3],
+ /// The obstacles in the maze
+ pub obstacles: Vec<Obstacle>,
+ /// The bounds of the obstacles' velocity
+ pub obstacles_velocity_bounds: [f32; 3],
+ /// The bounds of the obstacles' radius
+ pub obstacles_radius_bounds: [f32; 2],
+}
+/// Implementation of the maze
+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
+ /// # Returns
+ /// * The new maze instance
+ /// # Example
+ /// ```
+ /// use peng_quad::Maze;
+ /// let maze = Maze::new([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 5, [0.1, 0.1, 0.1], [0.1, 0.5]);
+ /// ```
+ pub fn new(
+ lower_bounds: [f32; 3],
+ upper_bounds: [f32; 3],
+ num_obstacles: usize,
+ obstacles_velocity_bounds: [f32; 3],
+ obstacles_radius_bounds: [f32; 2],
+ ) -> Self {
+ let mut maze = Maze {
+ lower_bounds,
+ upper_bounds,
+ obstacles: Vec::new(),
+ obstacles_velocity_bounds,
+ obstacles_radius_bounds,
+ };
+ maze.generate_obstacles(num_obstacles);
+ maze
+ }
+ /// Generates the obstacles in the maze
+ /// # Arguments
+ /// * `num_obstacles` - The number of obstacles to generate
+ /// # Example
+ /// ```
+ /// use peng_quad::Maze;
+ /// let mut maze = Maze::new([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 5, [0.1, 0.1, 0.1], [0.1, 0.5]);
+ /// maze.generate_obstacles(5);
+ /// ```
+ pub fn generate_obstacles(&mut self, num_obstacles: usize) {
+ let mut rng = ChaCha8Rng::from_entropy();
+ self.obstacles = (0..num_obstacles)
+ .map(|_| {
+ let position = Vector3::new(
+ rand::Rng::gen_range(&mut rng, self.lower_bounds[0]..self.upper_bounds[0]),
+ rand::Rng::gen_range(&mut rng, self.lower_bounds[1]..self.upper_bounds[1]),
+ rand::Rng::gen_range(&mut rng, self.lower_bounds[2]..self.upper_bounds[2]),
+ );
+ let v_bounds = self.obstacles_velocity_bounds;
+ let r_bounds = self.obstacles_radius_bounds;
+ let velocity = Vector3::new(
+ rand::Rng::gen_range(&mut rng, -v_bounds[0]..v_bounds[0]),
+ rand::Rng::gen_range(&mut rng, -v_bounds[1]..v_bounds[1]),
+ rand::Rng::gen_range(&mut rng, -v_bounds[2]..v_bounds[2]),
+ );
+ let radius = rand::Rng::gen_range(&mut rng, r_bounds[0]..r_bounds[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
+ /// # Example
+ /// ```
+ /// use peng_quad::Maze;
+ /// let mut maze = Maze::new([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 5, [0.1, 0.1, 0.1], [0.1, 0.5]);
+ /// maze.update_obstacles(0.1);
+ /// ```
+ 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
+/// # Example
+/// ```
+/// use peng_quad::Camera;
+/// let camera = Camera::new((800, 600), 60.0, 0.1, 100.0);
+/// ```
+pub struct Camera {
+ /// The resolution of the camera
+ pub resolution: (usize, usize),
+ /// The field of view of the camera
+ pub fov: f32,
+ /// The near clipping plane of the camera
+ pub near: f32,
+ /// The far clipping plane of the camera
+ pub far: f32,
+ /// The aspect ratio of the camera
+ pub aspect_ratio: f32,
+ /// The ray directions of each pixel in the camera
+ pub ray_directions: Vec<Vector3<f32>>,
+}
+/// Implementation of the camera
+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
+ /// # Returns
+ /// * The new camera instance
+ /// # Example
+ /// ```
+ /// use peng_quad::Camera;
+ /// let camera = Camera::new((800, 600), 60.0, 0.1, 100.0);
+ /// ```
+ 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
+ /// # Errors
+ /// * If the depth buffer is not large enough to store the depth values
+ /// # Example
+ /// ```
+ /// use peng_quad::{Camera, Maze};
+ /// use nalgebra::{Vector3, UnitQuaternion};
+ /// let camera = Camera::new((800, 600), 60.0, 0.1, 100.0);
+ /// let quad_position = Vector3::new(0.0, 0.0, 0.0);
+ /// let quad_orientation = UnitQuaternion::identity();
+ /// let mut maze = Maze::new([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 5, [0.1, 0.1, 0.1], [0.1, 0.5]);
+ /// let mut depth_buffer = vec![0.0; 800 * 600];
+ /// let use_multithreading = true;
+ /// camera.render_depth(&quad_position, &quad_orientation, &maze, &mut depth_buffer, use_multithreading);
+ /// ```
+ pub fn render_depth(
+ &self,
+ quad_position: &Vector3<f32>,
+ quad_orientation: &UnitQuaternion<f32>,
+ maze: &Maze,
+ depth_buffer: &mut Vec<f32>,
+ use_multi_threading: bool,
+ ) -> Result<(), SimulationError> {
+ let (width, height) = self.resolution;
+ let total_pixels = width * height;
+ 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();
+ if use_multi_threading {
+ depth_buffer.reserve((total_pixels - depth_buffer.capacity()).max(0));
+ depth_buffer
+ .par_iter_mut()
+ .enumerate()
+ .try_for_each(|(i, depth)| {
+ let direction = rotation_camera_to_world * self.ray_directions[i];
+ *depth =
+ self.ray_cast(quad_position, &rotation_world_to_camera, &direction, maze)?;
+ Ok::<(), SimulationError>(())
+ })?;
+ } else {
+ depth_buffer.clear();
+ depth_buffer.reserve((total_pixels - depth_buffer.capacity()).max(0));
+ for i in 0..total_pixels {
+ depth_buffer.push(self.ray_cast(
+ quad_position,
+ &rotation_world_to_camera,
+ &(rotation_camera_to_world * self.ray_directions[i]),
+ maze,
+ )?);
+ }
+ }
+ Ok(())
+ }
+ /// 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
+ /// # Errors
+ /// * If the ray does not hit any obstacles
+ /// # Example
+ /// ```
+ /// use peng_quad::{Camera, Maze};
+ /// use nalgebra::{Vector3, Matrix3};
+ /// let camera = Camera::new((800, 600), 60.0, 0.1, 100.0);
+ /// let origin = Vector3::new(0.0, 0.0, 0.0);
+ /// let rotation_world_to_camera = Matrix3::identity();
+ /// let direction = Vector3::new(1.0, 0.0, 0.0);
+ /// let mut maze = Maze::new([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 5, [0.1, 0.1, 0.1], [0.1, 0.5]);
+ /// let distance = camera.ray_cast(&origin, &rotation_world_to_camera, &direction, &maze);
+ /// ```
+ pub fn ray_cast(
+ &self,
+ origin: &Vector3<f32>,
+ rotation_world_to_camera: &Matrix3<f32>,
+ direction: &Vector3<f32>,
+ maze: &Maze,
+ ) -> Result<f32, SimulationError> {
+ 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(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();
+ let t = -b - fast_sqrt(discriminant);
+ 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(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
+/// # Errors
+/// * If the data cannot be logged to the recording stream
+/// # Example
+/// ```no_run
+/// use peng_quad::{Quadrotor, log_data};
+/// use nalgebra::Vector3;
+/// let rec = rerun::RecordingStreamBuilder::new("peng").connect().unwrap();
+/// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+/// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+/// let quad = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
+/// let desired_position = Vector3::new(0.0, 0.0, 0.0);
+/// let desired_velocity = Vector3::new(0.0, 0.0, 0.0);
+/// let measured_accel = Vector3::new(0.0, 0.0, 0.0);
+/// let measured_gyro = Vector3::new(0.0, 0.0, 0.0);
+/// log_data(&rec, &quad, &desired_position, &desired_velocity, &measured_accel, &measured_gyro).unwrap();
+/// ```
+pub fn log_data(
+ rec: &rerun::RecordingStream,
+ quad: &Quadrotor,
+ desired_position: &Vector3<f32>,
+ desired_velocity: &Vector3<f32>,
+ measured_accel: &Vector3<f32>,
+ measured_gyro: &Vector3<f32>,
+) -> 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<f32> = 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
+/// # Errors
+/// * If the data cannot be logged to the recording stream
+/// # Example
+/// ```no_run
+/// use peng_quad::{Maze, log_maze_tube};
+/// use rerun::RecordingStreamBuilder;
+/// let rec = rerun::RecordingStreamBuilder::new("log.rerun").connect().unwrap();
+/// let mut maze = Maze::new([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 5, [0.1, 0.1, 0.1], [0.1, 0.5]);
+/// log_maze_tube(&rec, &maze).unwrap();
+/// ```
+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[0] + upper_bounds[0]) / 2.0,
+ (lower_bounds[1] + upper_bounds[1]) / 2.0,
+ (lower_bounds[2] + upper_bounds[2]) / 2.0,
+ );
+ let half_sizes = rerun::external::glam::Vec3::new(
+ (upper_bounds[0] - lower_bounds[0]) / 2.0,
+ (upper_bounds[1] - lower_bounds[1]) / 2.0,
+ (upper_bounds[2] - lower_bounds[2]) / 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
+/// # Errors
+/// * If the data cannot be logged to the recording stream
+/// # Example
+/// ```no_run
+/// use peng_quad::{Maze, log_maze_obstacles};
+/// let rec = rerun::RecordingStreamBuilder::new("log.rerun").connect().unwrap();
+/// let mut maze = Maze::new([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 5, [0.1, 0.1, 0.1], [0.1, 0.5]);
+/// log_maze_obstacles(&rec, &maze).unwrap();
+/// ```
+pub fn log_maze_obstacles(
+ rec: &rerun::RecordingStream,
+ maze: &Maze,
+) -> Result<(), SimulationError> {
+ let (positions, radii): (Vec<(f32, f32, f32)>, Vec<f32>) = maze
+ .obstacles
+ .iter()
+ .map(|obstacle| {
+ let pos = obstacle.position;
+ ((pos.x, pos.y, pos.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
+/// # Example
+/// ```
+/// use peng_quad::Trajectory;
+/// let initial_point = nalgebra::Vector3::new(0.0, 0.0, 0.0);
+/// let mut trajectory = Trajectory::new(initial_point);
+/// ```
+pub struct Trajectory {
+ /// A vector of 3D points
+ pub points: Vec<Vector3<f32>>,
+ /// The last point that was logged
+ pub last_logged_point: Vector3<f32>,
+ /// The minimum distance between points to log
+ pub min_distance_threadhold: f32,
+}
+/// Implement the Trajectory struct
+impl Trajectory {
+ /// Create a new Trajectory instance
+ /// # Arguments
+ /// * `initial_point` - The initial point to add to the trajectory
+ /// # Returns
+ /// * A new Trajectory instance
+ /// # Example
+ /// ```
+ /// use peng_quad::Trajectory;
+ /// let initial_point = nalgebra::Vector3::new(0.0, 0.0, 0.0);
+ /// let mut trajectory = Trajectory::new(initial_point);
+ /// ```
+ pub fn new(initial_point: Vector3<f32>) -> 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
+ /// # Example
+ /// ```
+ /// use peng_quad::Trajectory;
+ /// let mut trajectory = Trajectory::new(nalgebra::Vector3::new(0.0, 0.0, 0.0));
+ /// let point = nalgebra::Vector3::new(1.0, 0.0, 0.0);
+ /// assert_eq!(trajectory.add_point(point), true);
+ /// assert_eq!(trajectory.add_point(point), false);
+ /// ```
+ pub fn add_point(&mut self, point: Vector3<f32>) -> 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
+/// # Errors
+/// * If the data cannot be logged to the recording stream
+/// # Example
+/// ```no_run
+/// use peng_quad::{Trajectory, log_trajectory};
+/// use nalgebra::Vector3;
+/// let rec = rerun::RecordingStreamBuilder::new("log.rerun").connect().unwrap();
+/// let mut trajectory = Trajectory::new(nalgebra::Vector3::new(0.0, 0.0, 0.0));
+/// trajectory.add_point(nalgebra::Vector3::new(1.0, 0.0, 0.0));
+/// log_trajectory(&rec, &trajectory).unwrap();
+/// ```
+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::<Vec<(f32, f32, f32)>>();
+ 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
+/// # Errors
+/// * If the data cannot be logged to the recording stream
+/// # Example
+/// ```no_run
+/// use peng_quad::log_mesh;
+/// let rec = rerun::RecordingStreamBuilder::new("log.rerun").connect().unwrap();
+/// log_mesh(&rec, 10, 0.1).unwrap();
+/// ```
+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<rerun::external::glam::Vec3> = (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<Vec<rerun::external::glam::Vec3>> = (0..grid_size)
+ .map(|i| points[i * grid_size..(i + 1) * grid_size].to_vec())
+ .collect();
+ let vertical_lines: Vec<Vec<rerun::external::glam::Vec3>> = (0..grid_size)
+ .map(|j| (0..grid_size).map(|i| points[i * grid_size + j]).collect())
+ .collect();
+ let line_strips: Vec<Vec<rerun::external::glam::Vec3>> =
+ 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
+/// # Errors
+/// * If the data cannot be logged to the recording stream
+/// # Example
+/// ```no_run
+/// use peng_quad::log_depth_image;
+/// let rec = rerun::RecordingStreamBuilder::new("log.rerun").connect().unwrap();
+/// let depth_image = vec![0.0; 640 * 480];
+/// log_depth_image(&rec, &depth_image, 640, 480, 0.0, 1.0).unwrap();
+/// ```
+pub fn log_depth_image(
+ rec: &rerun::RecordingStream,
+ depth_image: &[f32],
+ width: usize,
+ height: usize,
+ min_depth: f32,
+ max_depth: f32,
+) -> Result<(), SimulationError> {
+ let mut image = rerun::external::ndarray::Array::zeros((height, width, 3));
+ let depth_range = max_depth - min_depth;
+ image
+ .axis_iter_mut(rerun::external::ndarray::Axis(0))
+ .enumerate()
+ .for_each(|(y, mut row)| {
+ for (x, mut pixel) in row
+ .axis_iter_mut(rerun::external::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(())
+}
+/// creates pinhole camera
+/// # Arguments
+/// * `rec` - The rerun::RecordingStream instance
+/// * `width` - The width component of the camera resolution
+/// * `height` - The height component of the camera resolution
+/// * `fov` - The fov of the camera
+/// * `cam_position` - The position vector of the camera (aligns with the quad)
+/// * `cam_orientation` - The orientation quaternion of quad
+/// * `cam_transform` - The transform matrix between quad and camera alignment
+/// * `depth_image` - The depth image data
+/// # Errors
+/// * If the data cannot be logged to the recording stream
+/// # Example
+/// ```no_run
+/// use peng_quad::pinhole_depth;
+/// use nalgebra::{Vector3, UnitQuaternion};
+/// let rec = rerun::RecordingStreamBuilder::new("log.rerun").connect().unwrap();
+/// let depth_image = vec![ 0.0f32 ; 640 * 480];
+/// let cam_position = Vector3::new(0.0,0.0,0.0);
+/// let cam_orientation = UnitQuaternion::identity();
+/// let cam_transform = [0.0, 0.0, 1.0, -1.0, 0.0, 0.0, 0.0, -1.0, 0.0];
+/// pinhole_depth(&rec, 128usize, 96usize, 90.0, cam_position, cam_orientation, cam_transform, &depth_image).unwrap();
+
+pub fn pinhole_depth(
+ rec: &rerun::RecordingStream,
+ width: usize,
+ height: usize,
+ fov: f32,
+ cam_position: Vector3<f32>,
+ cam_orientation: UnitQuaternion<f32>,
+ cam_transform: [f32; 9],
+ depth_image: &[f32],
+) -> Result<(), SimulationError> {
+ let fov_x = (width as f32 / height as f32 * (fov / 2.0).tan()).atan() * 2.0;
+ let horizontal_focal_length = (width as f32 / 2.0) / ((fov_x/ 2.0).tan());
+ let vertical_focal_length = (height as f32 / 2.0) / ((fov / 2.0).tan());
+ let pinhole_camera = rerun::Pinhole::from_focal_length_and_resolution(
+ (horizontal_focal_length, vertical_focal_length),
+ (width as f32, height as f32),
+ )
+ .with_camera_xyz(rerun::components::ViewCoordinates::RDF)
+ .with_resolution((width as f32, height as f32))
+ .with_principal_point((width as f32 / 2.0, height as f32 / 2.0));
+ let rotated_camera_orientation = UnitQuaternion::from_rotation_matrix(
+ &(cam_orientation.to_rotation_matrix()
+ * Rotation3::from_matrix_unchecked(Matrix3::from_row_slice(&cam_transform))),
+ );
+ let cam_transform = rerun::Transform3D::from_translation_rotation(
+ rerun::Vec3D::new(cam_position.x, cam_position.y, cam_position.z),
+ rerun::Quaternion::from_xyzw([
+ rotated_camera_orientation.i,
+ rotated_camera_orientation.j,
+ rotated_camera_orientation.k,
+ rotated_camera_orientation.w,
+ ]),
+ );
+ rec.log("world/quad/cam", &cam_transform)?;
+ rec.log("world/quad/cam", &pinhole_camera)?;
+ let depth_image_rerun =
+ rerun::external::ndarray::Array::from_shape_vec((height, width), depth_image.to_vec())
+ .unwrap();
+ rec.log(
+ "world/quad/cam/rerun_depth",
+ &rerun::DepthImage::try_from(depth_image_rerun)
+ .unwrap()
+ .with_meter(1.0),
+ )?;
+
+ 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]
+/// # Example
+/// ```
+/// use peng_quad::color_map_fn;
+/// let color = color_map_fn(128.0);
+/// ```
+#[inline]
+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)
+}
+
+/// Fast square root function
+/// # Arguments
+/// * `x` - The input value
+/// # Returns
+/// * The square root of the input value
+#[inline(always)]
+fn fast_sqrt(x: f32) -> f32 {
+ let i = x.to_bits();
+ let i = 0x1fbd1df5 + (i >> 1);
+ f32::from_bits(i)
+}
+fn:) to \
+ restrict the search to a given item kind.","Accepted kinds are: fn, mod, struct, \
+ enum, trait, type, macro, \
+ and const.","Search functions by type signature (e.g., vec -> usize or \
+ -> vec or String, enum:Cow -> bool)","You can look for items with an exact name by putting double quotes around \
+ your request: \"string\"","Look for functions that accept or return \
+ slices and \
+ arrays by writing \
+ square brackets (e.g., -> [u8] or [] -> Option)","Look for items inside another one by searching for a path: vec::Vec",].map(x=>""+x+"
").join("");const div_infos=document.createElement("div");addClass(div_infos,"infos");div_infos.innerHTML="${value.replaceAll(" ", " ")}`}else{error[index]=value}});output+=`