pub(crate) enum PlannerType {
+ Hover(HoverPlanner),
+ MinimumJerkLine(MinimumJerkLinePlanner),
+ Lissajous(LissajousPlanner),
+ Circle(CirclePlanner),
+ Landing(LandingPlanner),
+ ObstacleAvoidance(ObstacleAvoidancePlanner),
+ MinimumSnapWaypoint(MinimumSnapWaypointPlanner),
+}Enum representing different types of trajectory planners
+Hover at current position
+Minimum jerk trajectory along a straight line
+Lissajous curve trajectory
+Circular trajectory
+Landing trajectory
+Obstacle Avoidance Planner based on Potential field
+Minimum snap waypoint Planner based on Polynomial Trajectory Generation
+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 timeA tuple containing the desired position, velocity, and yaw angle
+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(crate) fn log_data(
+ rec: &RecordingStream,
+ quad: &Quadrotor,
+ desired_position: &Vector3<f32>,
+ desired_velocity: &Vector3<f32>,
+ measured_accel: &Vector3<f32>,
+ measured_gyro: &Vector3<f32>,
+)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 vectorpub(crate) fn log_depth_image(
+ rec: &RecordingStream,
+ depth_image: &Vec<f32>,
+ width: usize,
+ height: usize,
+ min_depth: f32,
+ max_depth: f32,
+)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 valuepub(crate) fn log_maze_obstacles(rec: &RecordingStream, maze: &Maze)Log the maze obstacles to the rerun recording stream
+rec - The rerun::RecordingStream instancemaze - The maze instancepub(crate) fn log_maze_tube(rec: &RecordingStream, maze: &Maze)Log the maze tube to the rerun recording stream
+rec - The rerun::RecordingStream instancemaze - The maze instancepub(crate) fn log_trajectory(rec: &RecordingStream, trajectory: &Trajectory)log trajectory data to the rerun recording stream
+rec - The rerun::RecordingStream instancetrajectory - The Trajectory instancepub(crate) fn update_planner(
+ planner_manager: &mut PlannerManager,
+ step: usize,
+ time: f32,
+ quad: &Quadrotor,
+ obstacles: &Vec<Obstacle>,
+)Updates the planner based on the current simulation step
+planner_manager - The PlannerManager instance to updatestep - The current simulation steptime - The current simulation timequad - The Quadrotor instanceThis 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 visualizationpub(crate) struct Camera {
+ pub(crate) resolution: (usize, usize),
+ pub(crate) fov: f32,
+ pub(crate) near: f32,
+ pub(crate) far: f32,
+ pub(crate) tan_half_fov: f32,
+ pub(crate) aspect_ratio: f32,
+ pub(crate) ray_directions: Vec<Vector3<f32>>,
+}Represents a camera in the simulation which is used to render the depth of the scene
+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 cameratan_half_fov - The tangent of half the field of viewaspect_ratio - The aspect ratio of the cameraresolution: (usize, usize)§fov: f32§near: f32§far: f32§tan_half_fov: f32§aspect_ratio: f32§ray_directions: Vec<Vector3<f32>>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 cameraRenders 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 valuesCasts 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 sceneThe distance to the closest obstacle hit by the ray
+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(crate) struct CirclePlanner {
+ pub(crate) center: Vector3<f32>,
+ pub(crate) radius: f32,
+ pub(crate) angular_velocity: f32,
+ pub(crate) start_position: Vector3<f32>,
+ pub(crate) start_time: f32,
+ pub(crate) duration: f32,
+ pub(crate) start_yaw: f32,
+ pub(crate) end_yaw: f32,
+ pub(crate) ramp_time: f32,
+}Planner for circular trajectories
+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
+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(crate) struct HoverPlanner {
+ pub(crate) target_position: Vector3<f32>,
+ pub(crate) target_yaw: f32,
+}Planner for hovering at a fixed position
+target_position: Vector3<f32>Target position for hovering
+target_yaw: f32Target yaw angle for hovering
+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(crate) struct Imu {
+ pub(crate) accel_bias: Vector3<f32>,
+ pub(crate) gyro_bias: Vector3<f32>,
+ pub(crate) accel_noise_std: f32,
+ pub(crate) gyro_noise_std: f32,
+ pub(crate) bias_instability: f32,
+}Represents an Inertial Measurement Unit (IMU) with bias and noise characteristics
+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
+bias_instability: f32Bias instability coefficient
+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(crate) struct LandingPlanner {
+ pub(crate) start_position: Vector3<f32>,
+ pub(crate) start_time: f32,
+ pub(crate) duration: f32,
+ pub(crate) start_yaw: f32,
+}Planner for landing maneuvers
+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
+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(crate) struct LissajousPlanner {
+ pub(crate) start_position: Vector3<f32>,
+ pub(crate) center: Vector3<f32>,
+ pub(crate) amplitude: Vector3<f32>,
+ pub(crate) frequency: Vector3<f32>,
+ pub(crate) phase: Vector3<f32>,
+ pub(crate) start_time: f32,
+ pub(crate) duration: f32,
+ pub(crate) start_yaw: f32,
+ pub(crate) end_yaw: f32,
+ pub(crate) ramp_time: f32,
+}Planner for Lissajous curve trajectories
+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
+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(crate) struct Maze {
+ pub(crate) lower_bounds: Vector3<f32>,
+ pub(crate) upper_bounds: Vector3<f32>,
+ pub(crate) obstacles: Vec<Obstacle>,
+}Represents a maze in the simulation
+lower_bounds - The lower bounds of the mazeupper_bounds - The upper bounds of the mazeobstacles - The obstacles in the mazelower_bounds: Vector3<f32>§upper_bounds: Vector3<f32>§obstacles: Vec<Obstacle>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 mazeGenerates the obstacles in the maze
+num_obstacles - The number of obstacles to generateUpdates the obstacles in the maze, if an obstacle hits a boundary, it bounces off
+dt - The time stepself 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(crate) struct MinimumJerkLinePlanner {
+ pub(crate) start_position: Vector3<f32>,
+ pub(crate) end_position: Vector3<f32>,
+ pub(crate) start_yaw: f32,
+ pub(crate) end_yaw: f32,
+ pub(crate) start_time: f32,
+ pub(crate) duration: f32,
+}Planner for minimum jerk trajectories along a straight line
+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
+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(crate) struct MinimumSnapWaypointPlanner {
+ pub(crate) waypoints: Vec<Vector3<f32>>,
+ pub(crate) yaws: Vec<f32>,
+ pub(crate) times: Vec<f32>,
+ pub(crate) coefficients: Vec<Vec<Vector3<f32>>>,
+ pub(crate) yaw_coefficients: Vec<Vec<f32>>,
+ pub(crate) start_time: f32,
+}Waypoint planner that generates a minimum snap trajectory between waypoints +The planner calculates the coefficients for a minimum snap trajectory +and uses them to generate the desired position, velocity, and yaw +The trajectory is parameterized by time, and the planner interpolates between waypoints +using the calculated coefficients +The planner also supports specifying yaw angles at each waypoint
+waypoints - A list of waypoints to followyaws - A list of yaw angles at each waypointsegment_times - A list of times to reach each waypointstart_time - The start time of the trajectorywaypoints: Vec<Vector3<f32>>§yaws: Vec<f32>§times: Vec<f32>§coefficients: Vec<Vec<Vector3<f32>>>§yaw_coefficients: Vec<Vec<f32>>§start_time: f32Compute the coefficients for the minimum snap trajectory +The coefficients are calculated for each segment between waypoints +The trajectory is parameterized by time, and the planner interpolates between waypoints
+Compute the coefficients for yaw trajectories +The yaw trajectory is a cubic polynomial and interpolated between waypoints
+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)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(crate) struct Obstacle {
+ pub(crate) position: Vector3<f32>,
+ pub(crate) velocity: Vector3<f32>,
+ pub(crate) radius: f32,
+}Represents an obstacle in the simulation
+position - The position of the obstaclevelocity - The velocity of the obstacleradius - The radius of the obstacleposition: Vector3<f32>§velocity: Vector3<f32>§radius: f32self 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(crate) struct ObstacleAvoidancePlanner {
+ pub(crate) target_position: Vector3<f32>,
+ pub(crate) start_time: f32,
+ pub(crate) duration: f32,
+ pub(crate) start_yaw: f32,
+ pub(crate) end_yaw: f32,
+ pub(crate) obstacles: Vec<Obstacle>,
+ pub(crate) k_att: f32,
+ pub(crate) k_rep: f32,
+ pub(crate) d0: 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
+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
+d0: f32Influence distance of obstacles
+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(crate) struct PIDController {
+ pub(crate) kp_pos: Vector3<f32>,
+ pub(crate) kd_pos: Vector3<f32>,
+ pub(crate) kp_att: Vector3<f32>,
+ pub(crate) kd_att: Vector3<f32>,
+ pub(crate) ki_pos: Vector3<f32>,
+ pub(crate) ki_att: Vector3<f32>,
+ pub(crate) integral_pos_error: Vector3<f32>,
+ pub(crate) integral_att_error: Vector3<f32>,
+ pub(crate) max_integral_pos: Vector3<f32>,
+ pub(crate) max_integral_att: Vector3<f32>,
+}PID controller for quadrotor position and attitude control
+kp_pos: Vector3<f32>Proportional gain for position control
+kd_pos: Vector3<f32>Derivative gain for position control
+kp_att: Vector3<f32>Proportional gain for attitude control
+kd_att: Vector3<f32>Derivative gain for attitude control
+ki_pos: Vector3<f32>Integral gain for position control
+ki_att: Vector3<f32>Integral gain for attitude control
+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
+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 accelerationA tuple containing the computed thrust and desired orientation quaternion
+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(crate) struct PlannerManager {
+ pub(crate) current_planner: PlannerType,
+}Manages different trajectory planners and switches between them
+current_planner: PlannerTypeThe currently active 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 timeA tuple containing the desired position, velocity, and yaw angle
+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(crate) struct Quadrotor {
+ pub(crate) position: Vector3<f32>,
+ pub(crate) velocity: Vector3<f32>,
+ pub(crate) orientation: UnitQuaternion<f32>,
+ pub(crate) angular_velocity: Vector3<f32>,
+ pub(crate) mass: f32,
+ pub(crate) gravity: f32,
+ pub(crate) time_step: f32,
+ pub(crate) drag_coefficient: f32,
+ pub(crate) inertia_matrix: Matrix3<f32>,
+ pub(crate) inertia_matrix_inv: Matrix3<f32>,
+}Represents a quadrotor with its physical properties and state
+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
+Creates a new Quadrotor with default parameters
+time_step - The simulation time step in secondsUpdates 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 quadrotorself 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(crate) struct Trajectory {
+ pub(crate) points: Vec<Vector3<f32>>,
+ pub(crate) last_logged_point: Vector3<f32>,
+ pub(crate) min_distance_threadhold: f32,
+}A struct to hold trajectory data
+points - A vector of 3D pointslast_logged_point - The last point that was loggedmin_distance_threadhold - The minimum distance between points to logpoints: Vec<Vector3<f32>>§last_logged_point: Vector3<f32>§min_distance_threadhold: f32self 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(crate) 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) -> bool;
+}Trait defining the interface for trajectory planners
+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 timeA tuple containing the desired position, velocity, and yaw angle
+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\nDerivative gain for attitude control\nDerivative gain for position control\nIntegral gain for attitude control\nIntegral gain for position control\nProportional gain for attitude control\nProportional gain for position control\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\nMain function to run the quadrotor simulation\nMass of the quadrotor in kg\nMaximum allowed integral error for attitude\nMaximum allowed integral error for position\nCreates a new Quadrotor with default parameters\nCreates a new IMU with default parameters\nCreates a new PIDController with default gains\nCreates a new PlannerManager with an initial hover planner\nCreates a new maze with the given bounds and number of …\nCreates a new camera with the given resolution, field of …\nList of obstacles\nCurrent orientation of the quadrotor\nPhase of the Lissajous curve\nPlans the trajectory based on the current state and time\nPlans the trajectory based on the current planner type\nCurrent position of the quadrotor in 3D space\nRadius of the circular trajectory\nRamp-up time for smooth transitions\nRamp-up time for smooth transitions\nCasts a ray from the camera origin in the given direction\nSimulates IMU readings with added bias and noise\nSimulates IMU readings with added noise\nRenders the depth of the scene from the perspective of the …\nSets a new planner\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\nStarting yaw angle\nStarting yaw angle\nStarting yaw angle\nStarting yaw angle\nStarting yaw angle\nTarget position for hovering\nTarget position of the planner\nTarget yaw angle for hovering\nSimulation time step in seconds\nUpdates the IMU biases over time\nUpdates the current planner and returns the desired …\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\nCurrent velocity of the quadrotor")
\ No newline at end of file
diff --git a/settings.html b/settings.html
new file mode 100644
index 00000000..8b90870c
--- /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 +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 +
//! # Quadrotor Simulation
+//! This crate provides a comprehensive simulation environment for quadrotor drones.
+//! It includes models for quadrotor dynamics, IMU simulation, various trajectory planners,
+//! and a PID controller for position and attitude control.
+//! ## Features
+//! - Realistic quadrotor dynamics simulation
+//! - IMU sensor simulation with configurable noise parameters
+//! - Multiple trajectory planners including hover, minimum jerk, Lissajous curves, and circular paths
+//! - PID controller for position and attitude control
+//! - Integration with the `rerun` crate for visualization
+use nalgebra::{DVector, Matrix3, Rotation3, SMatrix, UnitQuaternion, Vector3};
+use rand_distr::{Distribution, Normal};
+/// Represents a quadrotor with its physical properties and state
+struct Quadrotor {
+ /// Current position of the quadrotor in 3D space
+ position: Vector3<f32>,
+ /// Current velocity of the quadrotor
+ velocity: Vector3<f32>,
+ /// Current orientation of the quadrotor
+ orientation: UnitQuaternion<f32>,
+ /// Current angular velocity of the quadrotor
+ angular_velocity: Vector3<f32>,
+ /// Mass of the quadrotor in kg
+ mass: f32,
+ /// Gravitational acceleration in m/s^2
+ gravity: f32,
+ /// Simulation time step in seconds
+ time_step: f32,
+ /// Drag coefficient
+ drag_coefficient: f32,
+ /// Inertia matrix of the quadrotor
+ inertia_matrix: Matrix3<f32>,
+ /// Inverse of the inertia matrix
+ inertia_matrix_inv: Matrix3<f32>,
+}
+
+impl Quadrotor {
+ /// Creates a new Quadrotor with default parameters
+ /// * Arguments
+ /// * `time_step` - The simulation time step in seconds
+ pub fn new(time_step: f32) -> Self {
+ let inertia_matrix = Matrix3::new(
+ 0.00304475, 0.0, 0.0, 0.0, 0.00454981, 0.0, 0.0, 0.0, 0.00281995,
+ );
+ Self {
+ position: Vector3::zeros(),
+ velocity: Vector3::zeros(),
+ orientation: UnitQuaternion::identity(),
+ angular_velocity: Vector3::zeros(),
+ mass: 1.3,
+ gravity: 9.81,
+ time_step,
+ // thrust_coefficient: 0.0,
+ drag_coefficient: 0.000,
+ inertia_matrix,
+ inertia_matrix_inv: inertia_matrix.try_inverse().unwrap(),
+ }
+ }
+ /// Updates the quadrotor's dynamics with control inputs
+ /// # Arguments
+ /// * `control_thrust` - The total thrust force applied to the quadrotor
+ /// * `control_torque` - The 3D torque vector applied to the quadrotor
+ pub fn update_dynamics_with_controls(
+ &mut self,
+ control_thrust: f32,
+ control_torque: &Vector3<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_body = Vector3::new(0.0, 0.0, control_thrust);
+ let thrust_world = self.orientation * thrust_body;
+ let total_force = thrust_world + gravity_force + drag_force;
+ let acceleration = total_force / self.mass;
+ self.velocity += acceleration * self.time_step;
+ self.position += self.velocity * self.time_step;
+ let inertia_angular_velocity = self.inertia_matrix * self.angular_velocity;
+ let gyroscopic_torque = self.angular_velocity.cross(&inertia_angular_velocity);
+ let angular_acceleration = self.inertia_matrix_inv * (control_torque - gyroscopic_torque);
+ self.angular_velocity += angular_acceleration * self.time_step;
+ self.orientation *=
+ UnitQuaternion::from_scaled_axis(self.angular_velocity * self.time_step);
+ }
+ /// Simulates IMU readings with added noise
+ /// # Returns
+ /// A tuple containing the measured acceleration and angular velocity
+ pub fn read_imu(&self) -> (Vector3<f32>, Vector3<f32>) {
+ let accel_noise = Normal::new(0.0, 0.02).unwrap();
+ let gyro_noise = Normal::new(0.0, 0.01).unwrap();
+ let mut rng = rand::thread_rng();
+ let gravity_world = Vector3::new(0.0, 0.0, self.gravity);
+ let specific_force =
+ self.orientation.inverse() * (self.velocity / self.time_step - gravity_world);
+ let measured_acceleration = specific_force
+ + Vector3::new(
+ accel_noise.sample(&mut rng),
+ accel_noise.sample(&mut rng),
+ accel_noise.sample(&mut rng),
+ );
+ let measured_angular_velocity = self.angular_velocity
+ + Vector3::new(
+ gyro_noise.sample(&mut rng),
+ gyro_noise.sample(&mut rng),
+ gyro_noise.sample(&mut rng),
+ );
+ (measured_acceleration, measured_angular_velocity)
+ }
+}
+/// Represents an Inertial Measurement Unit (IMU) with bias and noise characteristics
+struct Imu {
+ /// Accelerometer bias
+ accel_bias: Vector3<f32>,
+ /// Gyroscope bias
+ gyro_bias: Vector3<f32>,
+ /// Standard deviation of accelerometer noise
+ accel_noise_std: f32,
+ /// Standard deviation of gyroscope noise
+ gyro_noise_std: f32,
+ /// Bias instability coefficient
+ bias_instability: f32,
+}
+
+impl Imu {
+ /// Creates a new IMU with default parameters
+ pub fn new() -> Self {
+ Self {
+ accel_bias: Vector3::zeros(),
+ gyro_bias: Vector3::zeros(),
+ accel_noise_std: 0.02,
+ gyro_noise_std: 0.01,
+ bias_instability: 0.0001,
+ }
+ }
+ /// Updates the IMU biases over time
+ /// # Arguments
+ /// * `dt` - Time step for the update
+ pub fn update(&mut self, dt: f32) {
+ let bias_drift = Normal::new(0.0, self.bias_instability * dt.sqrt()).unwrap();
+ let drift_vector =
+ || Vector3::from_iterator((0..3).map(|_| bias_drift.sample(&mut rand::thread_rng())));
+ self.accel_bias += drift_vector();
+ self.gyro_bias += drift_vector();
+ }
+ /// Simulates IMU readings with added bias and noise
+ /// # Arguments
+ /// * `true_acceleration` - The true acceleration vector
+ /// * `true_angular_velocity` - The true angular velocity vector
+ /// # Returns
+ /// A tuple containing the measured acceleration and angular velocity
+ pub fn read(
+ &self,
+ true_acceleration: Vector3<f32>,
+ true_angular_velocity: Vector3<f32>,
+ ) -> (Vector3<f32>, Vector3<f32>) {
+ let mut rng = rand::thread_rng();
+ let accel_noise = Normal::new(0.0, self.accel_noise_std).unwrap();
+ let gyro_noise = Normal::new(0.0, self.gyro_noise_std).unwrap();
+ let measured_acceleration = true_acceleration
+ + self.accel_bias
+ + Vector3::new(
+ accel_noise.sample(&mut rng),
+ accel_noise.sample(&mut rng),
+ accel_noise.sample(&mut rng),
+ );
+ let measured_angular_velocity = true_angular_velocity
+ + self.gyro_bias
+ + Vector3::new(
+ gyro_noise.sample(&mut rng),
+ gyro_noise.sample(&mut rng),
+ gyro_noise.sample(&mut rng),
+ );
+ (measured_acceleration, measured_angular_velocity)
+ }
+}
+/// PID controller for quadrotor position and attitude control
+struct PIDController {
+ /// Proportional gain for position control
+ kp_pos: Vector3<f32>,
+ /// Derivative gain for position control
+ kd_pos: Vector3<f32>,
+ /// Proportional gain for attitude control
+ kp_att: Vector3<f32>,
+ /// Derivative gain for attitude control
+ kd_att: Vector3<f32>,
+ /// Integral gain for position control
+ ki_pos: Vector3<f32>,
+ /// Integral gain for attitude control
+ ki_att: Vector3<f32>,
+ /// Accumulated integral error for position
+ integral_pos_error: Vector3<f32>,
+ /// Accumulated integral error for attitude
+ integral_att_error: Vector3<f32>,
+ /// Maximum allowed integral error for position
+ max_integral_pos: Vector3<f32>,
+ /// Maximum allowed integral error for attitude
+ max_integral_att: Vector3<f32>,
+}
+
+impl PIDController {
+ /// Creates a new PIDController with default gains
+ fn new() -> Self {
+ Self {
+ kp_pos: Vector3::new(7.1, 7.1, 11.9),
+ kd_pos: Vector3::new(2.4, 2.4, 6.7),
+ kp_att: Vector3::new(1.5, 1.5, 1.0),
+ kd_att: Vector3::new(0.13, 0.13, 0.1),
+ ki_pos: Vector3::new(0.00, 0.00, 0.00),
+ ki_att: Vector3::new(0.00, 0.00, 0.00),
+ integral_pos_error: Vector3::zeros(),
+ integral_att_error: Vector3::zeros(),
+ max_integral_pos: Vector3::new(10.0, 10.0, 10.0),
+ max_integral_att: Vector3::new(1.0, 1.0, 1.0),
+ }
+ }
+ /// Computes attitude control torques
+ /// # Arguments
+ /// * `desired_orientation` - The desired orientation quaternion
+ /// * `current_orientation` - The current orientation quaternion
+ /// * `current_angular_velocity` - The current angular velocity
+ /// * `dt` - Time step
+ /// # Returns
+ /// The computed control torque vector
+ fn compute_attitude_control(
+ &mut self,
+ desired_orientation: &UnitQuaternion<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
+ .component_mul_assign(&self.max_integral_att.map(|x| x.signum()));
+ 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;
+ self.kp_att.component_mul(&error_angles)
+ + self.kd_att.component_mul(&error_angular_velocity)
+ + self.ki_att.component_mul(&self.integral_att_error)
+ }
+ /// Computes position control thrust and desired orientation
+ /// # Arguments
+ /// * `desired_position` - The desired position
+ /// * `desired_velocity` - The desired velocity
+ /// * `desired_yaw` - The desired yaw angle
+ /// * `current_position` - The current position
+ /// * `current_velocity` - The current velocity
+ /// * `dt` - Time step
+ /// * `mass` - Mass of the quadrotor
+ /// * `gravity` - Gravitational acceleration
+ /// # Returns
+ /// A tuple containing the computed thrust and desired orientation quaternion
+ fn compute_position_control(
+ &mut self,
+ desired_position: Vector3<f32>,
+ desired_velocity: Vector3<f32>,
+ desired_yaw: f32,
+ current_position: Vector3<f32>,
+ current_velocity: Vector3<f32>,
+ dt: f32,
+ mass: f32,
+ gravity: 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
+ .component_mul(&self.max_integral_pos.map(|x| x.signum()));
+ self.integral_pos_error = self
+ .integral_pos_error
+ .zip_map(&self.max_integral_pos, |int, max| int.clamp(-max, max));
+ let acceleration = self.kp_pos.component_mul(&error_position)
+ + self.kd_pos.component_mul(&error_velocity)
+ + self.ki_pos.component_mul(&self.integral_pos_error);
+ let gravity_compensation = Vector3::new(0.0, 0.0, gravity);
+ let total_acceleration = acceleration + gravity_compensation;
+ let thrust = mass * total_acceleration.norm();
+ let desired_orientation = if total_acceleration.norm() > 1e-6 {
+ let z_body = total_acceleration.normalize();
+ let yaw_rotation = UnitQuaternion::from_euler_angles(0.0, 0.0, desired_yaw);
+ let x_body_horizontal = yaw_rotation * Vector3::new(1.0, 0.0, 0.0);
+ let y_body = z_body.cross(&x_body_horizontal).normalize();
+ let x_body = y_body.cross(&z_body);
+ UnitQuaternion::from_rotation_matrix(&Rotation3::from_matrix_unchecked(
+ Matrix3::from_columns(&[x_body, y_body, z_body]),
+ ))
+ } else {
+ UnitQuaternion::from_euler_angles(0.0, 0.0, desired_yaw)
+ };
+ (thrust, desired_orientation)
+ }
+}
+/// Enum representing different types of trajectory planners
+enum PlannerType {
+ /// Hover at current position
+ Hover(HoverPlanner),
+ /// Minimum jerk trajectory along a straight line
+ MinimumJerkLine(MinimumJerkLinePlanner),
+ /// Lissajous curve trajectory
+ Lissajous(LissajousPlanner),
+ /// Circular trajectory
+ Circle(CirclePlanner),
+ /// Landing trajectory
+ Landing(LandingPlanner),
+ /// Obstacle Avoidance Planner based on Potential field
+ ObstacleAvoidance(ObstacleAvoidancePlanner),
+ /// Minimum snap waypoint Planner based on Polynomial Trajectory Generation
+ MinimumSnapWaypoint(MinimumSnapWaypointPlanner),
+}
+impl PlannerType {
+ /// Plans the trajectory based on the current planner type
+ /// # Arguments
+ /// * `current_position` - The current position of the quadrotor
+ /// * `current_velocity` - The current velocity of the quadrotor
+ /// * `time` - The current simulation time
+ /// # Returns
+ /// A tuple containing the desired position, velocity, and yaw angle
+ fn plan(
+ &self,
+ current_position: Vector3<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
+ fn is_finished(&self, current_position: Vector3<f32>, time: f32) -> bool {
+ 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
+trait Planner {
+ /// Plans the trajectory based on the current state and time
+ /// # Arguments
+ /// * `current_position` - The current position of the quadrotor
+ /// * `current_velocity` - The current velocity of the quadrotor
+ /// * `time` - The current simulation time
+ /// # Returns
+ /// A tuple containing the desired position, velocity, and yaw angle
+ fn plan(
+ &self,
+ current_position: Vector3<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
+ fn is_finished(&self, current_position: Vector3<f32>, time: f32) -> bool;
+}
+/// Planner for hovering at a fixed position
+struct HoverPlanner {
+ /// Target position for hovering
+ target_position: Vector3<f32>,
+ /// Target yaw angle for hovering
+ target_yaw: f32,
+}
+
+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) -> bool {
+ true // Hover planner is always "finished" as it's the default state
+ }
+}
+/// Planner for minimum jerk trajectories along a straight line
+struct MinimumJerkLinePlanner {
+ /// Starting position of the trajectory
+ start_position: Vector3<f32>,
+ /// Ending position of the trajectory
+ end_position: Vector3<f32>,
+ /// Starting yaw angle
+ start_yaw: f32,
+ /// Ending yaw angle
+ end_yaw: f32,
+ /// Start time of the trajectory
+ start_time: f32,
+ /// Duration of the trajectory
+ duration: f32,
+}
+
+impl Planner for MinimumJerkLinePlanner {
+ fn plan(
+ &self,
+ _current_position: Vector3<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 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) -> bool {
+ (_current_position - self.end_position).norm() < 0.01
+ && _time >= self.start_time + self.duration
+ }
+}
+/// Planner for Lissajous curve trajectories
+struct LissajousPlanner {
+ /// Starting position of the trajectory
+ start_position: Vector3<f32>,
+ /// Center of the Lissajous curve
+ center: Vector3<f32>,
+ /// Amplitude of the Lissajous curve
+ amplitude: Vector3<f32>,
+ /// Frequency of the Lissajous curve
+ frequency: Vector3<f32>,
+ /// Phase of the Lissajous curve
+ phase: Vector3<f32>,
+ /// Start time of the trajectory
+ start_time: f32,
+ /// Duration of the trajectory
+ duration: f32,
+ /// Starting yaw angle
+ start_yaw: f32,
+ /// Ending yaw angle
+ end_yaw: f32,
+ /// Ramp-up time for smooth transitions
+ ramp_time: f32,
+}
+
+impl Planner for LissajousPlanner {
+ fn plan(
+ &self,
+ _current_position: Vector3<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 lissajous = Vector3::new(
+ self.amplitude.x
+ * (self.frequency.x * t * 2.0 * std::f32::consts::PI + self.phase.x).sin(),
+ self.amplitude.y
+ * (self.frequency.y * t * 2.0 * std::f32::consts::PI + self.phase.y).sin(),
+ self.amplitude.z
+ * (self.frequency.z * t * 2.0 * std::f32::consts::PI + self.phase.z).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
+ * std::f32::consts::PI
+ * (self.frequency.x * t * 2.0 * std::f32::consts::PI + self.phase.x).cos(),
+ self.amplitude.y
+ * self.frequency.y
+ * 2.0
+ * std::f32::consts::PI
+ * (self.frequency.y * t * 2.0 * std::f32::consts::PI + self.phase.y).cos(),
+ self.amplitude.z
+ * self.frequency.z
+ * 2.0
+ * std::f32::consts::PI
+ * (self.frequency.z * t * 2.0 * std::f32::consts::PI + self.phase.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) -> bool {
+ time >= self.start_time + self.duration
+ }
+}
+/// Planner for circular trajectories
+struct CirclePlanner {
+ /// Center of the circular trajectory
+ center: Vector3<f32>,
+ /// Radius of the circular trajectory
+ radius: f32,
+ /// Angular velocity of the circular motion
+ angular_velocity: f32,
+ /// Starting position of the trajectory
+ start_position: Vector3<f32>,
+ /// Start time of the trajectory
+ start_time: f32,
+ /// Duration of the trajectory
+ duration: f32,
+ /// Starting yaw angle
+ start_yaw: f32,
+ /// Ending yaw angle
+ end_yaw: f32,
+ /// Ramp-up time for smooth transitions
+ ramp_time: f32,
+}
+
+impl Planner for CirclePlanner {
+ fn plan(
+ &self,
+ _current_position: Vector3<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) -> bool {
+ time >= self.start_time + self.duration
+ }
+}
+
+/// Planner for landing maneuvers
+struct LandingPlanner {
+ /// Starting position of the landing maneuver
+ start_position: Vector3<f32>,
+ /// Start time of the landing maneuver
+ start_time: f32,
+ /// Duration of the landing maneuver
+ duration: f32,
+ /// Starting yaw angle
+ start_yaw: f32,
+}
+
+impl Planner for LandingPlanner {
+ fn plan(
+ &self,
+ _current_position: Vector3<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) -> bool {
+ current_position.z < 0.05 || time >= self.start_time + self.duration
+ }
+}
+/// Manages different trajectory planners and switches between them
+struct PlannerManager {
+ /// The currently active planner
+ current_planner: PlannerType,
+}
+
+impl PlannerManager {
+ /// Creates a new PlannerManager with an initial hover planner
+ /// # Arguments
+ /// * `initial_position` - The initial position for hovering
+ /// * `initial_yaw` - The initial yaw angle for hovering
+ /// # Returns
+ /// A new PlannerManager instance
+ fn new(initial_position: Vector3<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
+ fn set_planner(&mut self, new_planner: PlannerType) {
+ self.current_planner = new_planner;
+ }
+ /// Updates the current planner and returns the desired position, velocity, and yaw
+ /// # Arguments
+ /// * `current_position` - The current position of the quadrotor
+ /// * `current_orientation` - The current orientation of the quadrotor
+ /// * `current_velocity` - The current velocity of the quadrotor
+ /// * `time` - The current simulation time
+ /// # Returns
+ /// A tuple containing the desired position, velocity, and yaw angle
+ fn update(
+ &mut self,
+ current_position: Vector3<f32>,
+ current_orientation: UnitQuaternion<f32>,
+ current_velocity: Vector3<f32>,
+ time: f32,
+ obstacles: &Vec<Obstacle>,
+ ) -> (Vector3<f32>, Vector3<f32>, f32) {
+ if self.current_planner.is_finished(current_position, time) {
+ self.current_planner = PlannerType::Hover(HoverPlanner {
+ target_position: current_position,
+ target_yaw: current_orientation.euler_angles().2,
+ });
+ }
+ // Update obstacles for ObstacleAvoidancePlanner if needed
+ if let PlannerType::ObstacleAvoidance(ref mut planner) = self.current_planner {
+ planner.obstacles = obstacles.clone();
+ }
+ self.current_planner
+ .plan(current_position, current_velocity, time)
+ }
+}
+/// Obstacle avoidance planner that uses a potential field approach to avoid obstacles
+/// The planner calculates a repulsive force for each obstacle and an attractive force towards the goal
+/// The resulting force is then used to calculate the desired position and velocity
+struct ObstacleAvoidancePlanner {
+ /// Target position of the planner
+ target_position: Vector3<f32>,
+ /// Start time of the planner
+ start_time: f32,
+ /// Duration of the planner
+ duration: f32,
+ /// Starting yaw angle
+ start_yaw: f32,
+ /// Ending yaw angle
+ end_yaw: f32,
+ /// List of obstacles
+ obstacles: Vec<Obstacle>,
+ /// Attractive force gain
+ k_att: f32,
+ /// Repulsive force gain
+ k_rep: f32,
+ /// Influence distance of obstacles
+ d0: f32,
+}
+
+impl Planner for ObstacleAvoidancePlanner {
+ fn plan(
+ &self,
+ current_position: Vector3<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);
+ // Attractive force towards the goal
+ let f_att = self.k_att * (self.target_position - current_position);
+ // Repulsive force from obstacles
+ let mut f_rep = Vector3::zeros();
+ for obstacle in &self.obstacles {
+ let diff = current_position - obstacle.position;
+ let distance = diff.norm();
+ if distance < self.d0 {
+ f_rep += self.k_rep
+ * (1.0 / distance - 1.0 / self.d0)
+ * (1.0 / distance.powi(2))
+ * diff.normalize();
+ }
+ }
+ // Total force
+ let f_total = f_att + f_rep;
+ // Desired velocity (capped at a maximum speed)
+ let max_speed: f32 = 1.0;
+ let desired_velocity = f_total.normalize() * max_speed.min(f_total.norm());
+ // Desired position (current position + desired velocity)
+ let desired_position = current_position + desired_velocity * self.duration * (1.0 - t);
+ // Interpolate yaw
+ 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) -> bool {
+ (current_position - self.target_position).norm() < 0.1
+ && time >= self.start_time + self.duration
+ }
+}
+
+/// Waypoint planner that generates a minimum snap trajectory between waypoints
+/// The planner calculates the coefficients for a minimum snap trajectory
+/// and uses them to generate the desired position, velocity, and yaw
+/// The trajectory is parameterized by time, and the planner interpolates between waypoints
+/// using the calculated coefficients
+/// The planner also supports specifying yaw angles at each waypoint
+/// # Arguments
+/// * `waypoints` - A list of waypoints to follow
+/// * `yaws` - A list of yaw angles at each waypoint
+/// * `segment_times` - A list of times to reach each waypoint
+/// * `start_time` - The start time of the trajectory
+struct MinimumSnapWaypointPlanner {
+ waypoints: Vec<Vector3<f32>>,
+ yaws: Vec<f32>,
+ times: Vec<f32>,
+ coefficients: Vec<Vec<Vector3<f32>>>,
+ yaw_coefficients: Vec<Vec<f32>>,
+ start_time: f32,
+}
+
+impl MinimumSnapWaypointPlanner {
+ fn new(
+ waypoints: Vec<Vector3<f32>>,
+ yaws: Vec<f32>,
+ segment_times: Vec<f32>,
+ start_time: f32,
+ ) -> Result<Self, &'static str> {
+ if waypoints.len() < 2 {
+ return Err("At least two waypoints are required");
+ }
+ if waypoints.len() != segment_times.len() + 1 || waypoints.len() != yaws.len() {
+ return Err("Number of segment times must be one less than number of waypoints, and yaws must match waypoints");
+ }
+ 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
+ /// The coefficients are calculated for each segment between waypoints
+ /// The trajectory is parameterized by time, and the planner interpolates between waypoints
+ fn compute_minimum_snap_trajectories(&mut self) {
+ let n = self.waypoints.len() - 1; // Number of segments
+
+ // Compute the coefficients for each segment
+ for i in 0..n {
+ let duration = self.times[i];
+ let start = self.waypoints[i];
+ let end = self.waypoints[i + 1];
+
+ //let mut a = DMatrix::zeros(8, 8);
+ let mut a = SMatrix::<f32, 8, 8>::zeros();
+ let mut b = DVector::zeros(8);
+
+ // Start point constraints
+ a[(0, 0)] = 1.0;
+ a[(1, 1)] = 1.0;
+ a[(2, 2)] = 2.0;
+ a[(3, 3)] = 6.0;
+ b[0] = start.x;
+
+ // 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);
+ }
+ }
+ b[4] = end.x;
+
+ let x_coeffs = a.lu().solve(&b).unwrap();
+
+ let mut b_y = b.clone();
+ b_y[0] = start.y;
+ b_y[4] = end.y;
+ let y_coeffs = a.lu().solve(&b_y).unwrap();
+
+ let mut b_z = b.clone();
+ b_z[0] = start.z;
+ b_z[4] = end.z;
+ let z_coeffs = a.lu().solve(&b_z).unwrap();
+
+ self.coefficients.push(vec![
+ Vector3::new(x_coeffs[0], y_coeffs[0], z_coeffs[0]),
+ Vector3::new(x_coeffs[1], y_coeffs[1], z_coeffs[1]),
+ Vector3::new(x_coeffs[2], y_coeffs[2], z_coeffs[2]),
+ Vector3::new(x_coeffs[3], y_coeffs[3], z_coeffs[3]),
+ Vector3::new(x_coeffs[4], y_coeffs[4], z_coeffs[4]),
+ Vector3::new(x_coeffs[5], y_coeffs[5], z_coeffs[5]),
+ Vector3::new(x_coeffs[6], y_coeffs[6], z_coeffs[6]),
+ Vector3::new(x_coeffs[7], y_coeffs[7], z_coeffs[7]),
+ ]);
+ }
+ }
+ /// Compute the coefficients for yaw trajectories
+ /// The yaw trajectory is a cubic polynomial and interpolated between waypoints
+ fn compute_minimum_acceleration_yaw_trajectories(&mut self) {
+ let n = self.yaws.len() - 1; // Number of segments
+
+ for i in 0..n {
+ let duration = self.times[i];
+ let start_yaw = self.yaws[i];
+ let end_yaw = self.yaws[i + 1];
+
+ let mut a = SMatrix::<f32, 4, 4>::zeros();
+ let mut b = DVector::zeros(4);
+
+ // Start point constraints
+ a[(0, 0)] = 1.0;
+ a[(1, 1)] = 1.0;
+ b[0] = start_yaw;
+
+ // End point constraints
+ for j in 0..4 {
+ a[(2, j)] = duration.powi(j as i32);
+ if j > 0 {
+ a[(3, j)] = j as f32 * duration.powi(j as i32 - 1);
+ }
+ }
+ b[2] = end_yaw;
+
+ let yaw_coeffs = a.lu().solve(&b).unwrap();
+ self.yaw_coefficients.push(yaw_coeffs.as_slice().to_vec());
+ }
+ }
+
+ /// Evaluate the trajectory at a given time, returns the position, velocity, yaw, and yaw rate at the given time
+ /// # Arguments
+ /// * `t` - The time to evaluate the trajectory at
+ /// * `coeffs` - The coefficients for the position trajectory
+ /// * `yaw_coeffs` - The coefficients for the yaw trajectory
+ /// # Returns
+ /// * `position` - The position at the given time (meters)
+ /// * `velocity` - The velocity at the given time (meters / second)
+ /// * `yaw` - The yaw at the given time (radians)
+ /// * `yaw_rate` - The yaw rate at the given time (radians / second)
+ fn evaluate_polynomial(
+ &self,
+ t: f32,
+ coeffs: &[Vector3<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)
+ }
+ 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) -> bool {
+ let total_duration: f32 = self.times.iter().sum();
+ time >= self.start_time + total_duration
+ && (current_position - *self.waypoints.last().unwrap()).norm() < 0.1
+ }
+}
+/// Updates the planner based on the current simulation step
+/// # Arguments
+/// * `planner_manager` - The PlannerManager instance to update
+/// * `step` - The current simulation step
+/// * `time` - The current simulation time
+/// * `quad` - The Quadrotor instance
+fn update_planner(
+ planner_manager: &mut PlannerManager,
+ step: usize,
+ time: f32,
+ quad: &Quadrotor,
+ obstacles: &Vec<Obstacle>,
+) {
+ let step = (step as f32 * 100.0 * quad.time_step) as i32;
+ match step {
+ 500 => planner_manager.set_planner(PlannerType::MinimumJerkLine(MinimumJerkLinePlanner {
+ start_position: quad.position,
+ end_position: Vector3::new(0.0, 0.0, 1.0),
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: 0.0,
+ start_time: time,
+ duration: 3.0,
+ })),
+ 1000 => planner_manager.set_planner(PlannerType::MinimumJerkLine(MinimumJerkLinePlanner {
+ start_position: quad.position,
+ end_position: Vector3::new(1.0, 0.0, 1.0),
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: 0.0,
+ start_time: time,
+ duration: 3.0,
+ })),
+ 1500 => planner_manager.set_planner(PlannerType::MinimumJerkLine(MinimumJerkLinePlanner {
+ start_position: quad.position,
+ end_position: Vector3::new(1.0, 1.0, 1.0),
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: std::f32::consts::PI / 2.0,
+ start_time: time,
+ duration: 3.0,
+ })),
+ 2000 => planner_manager.set_planner(PlannerType::MinimumJerkLine(MinimumJerkLinePlanner {
+ start_position: quad.position,
+ end_position: Vector3::new(0.0, 1.0, 1.0),
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: std::f32::consts::PI / 2.0,
+ start_time: time,
+ duration: 3.0,
+ })),
+ 2500 => planner_manager.set_planner(PlannerType::MinimumJerkLine(MinimumJerkLinePlanner {
+ start_position: quad.position,
+ end_position: Vector3::new(0.0, 0.0, 0.5),
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: 0.0,
+ start_time: time,
+ duration: 3.0,
+ })),
+ 3000 => planner_manager.set_planner(PlannerType::Lissajous(LissajousPlanner {
+ start_position: quad.position,
+ center: Vector3::new(0.5, 0.5, 1.0),
+ amplitude: Vector3::new(0.5, 0.5, 0.2),
+ frequency: Vector3::new(1.0, 2.0, 3.0),
+ phase: Vector3::new(0.0, std::f32::consts::PI / 2.0, 0.0),
+ start_time: time,
+ duration: 20.0,
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: quad.orientation.euler_angles().2 + 2.0 * std::f32::consts::PI,
+ ramp_time: 5.0,
+ })),
+ 5200 => planner_manager.set_planner(PlannerType::Circle(CirclePlanner {
+ center: Vector3::new(0.5, 0.5, 1.0),
+ radius: 0.5,
+ angular_velocity: 1.0,
+ start_position: quad.position,
+ start_time: time,
+ duration: 8.0,
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: quad.orientation.euler_angles().2,
+ ramp_time: 2.0,
+ })),
+ 6200 => planner_manager.set_planner(PlannerType::MinimumJerkLine(MinimumJerkLinePlanner {
+ start_position: quad.position,
+ end_position: Vector3::new(quad.position.x, quad.position.y, 0.5),
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: 0.0,
+ start_time: time,
+ duration: 5.0,
+ })),
+ 7000 => {
+ planner_manager.set_planner(PlannerType::ObstacleAvoidance(ObstacleAvoidancePlanner {
+ target_position: Vector3::new(1.5, 1.0, 1.0),
+ start_time: time,
+ duration: 15.0,
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: 0.0,
+ obstacles: obstacles.clone(),
+ k_att: 0.03,
+ k_rep: 0.02,
+ d0: 0.5,
+ }))
+ }
+ 8500 => {
+ let waypoints = vec![
+ quad.position,
+ Vector3::new(1.0, 1.0, 1.5),
+ Vector3::new(-1.0, 1.0, 1.75),
+ Vector3::new(0.0, -1.0, 1.0),
+ Vector3::new(0.0, 0.0, 0.5),
+ ];
+ let yaws = vec![
+ quad.orientation.euler_angles().2,
+ std::f32::consts::PI / 2.0,
+ std::f32::consts::PI,
+ -std::f32::consts::PI / 2.0,
+ 0.0,
+ ];
+ let segment_times = vec![5.0, 5.0, 5.0, 5.0];
+ match MinimumSnapWaypointPlanner::new(waypoints, yaws, segment_times, time) {
+ Ok(planner) => {
+ planner_manager.set_planner(PlannerType::MinimumSnapWaypoint(planner))
+ }
+ Err(e) => println!("Error creating MinimumSnapWaypointPlanner: {}", e),
+ }
+ }
+ 10500 => planner_manager.set_planner(PlannerType::Landing(LandingPlanner {
+ start_position: quad.position,
+ start_time: time,
+ duration: 5.0,
+ start_yaw: quad.orientation.euler_angles().2,
+ })),
+ _ => {}
+ }
+}
+/// Represents an obstacle in the simulation
+/// # Fields
+/// * `position` - The position of the obstacle
+/// * `velocity` - The velocity of the obstacle
+/// * `radius` - The radius of the obstacle
+#[derive(Clone)]
+struct Obstacle {
+ position: Vector3<f32>,
+ velocity: Vector3<f32>,
+ radius: f32,
+}
+
+impl Obstacle {
+ fn new(position: Vector3<f32>, velocity: Vector3<f32>, radius: f32) -> Self {
+ Self {
+ position,
+ velocity,
+ radius,
+ }
+ }
+}
+/// Represents a maze in the simulation
+/// # Fields
+/// * `lower_bounds` - The lower bounds of the maze
+/// * `upper_bounds` - The upper bounds of the maze
+/// * `obstacles` - The obstacles in the maze
+struct Maze {
+ lower_bounds: Vector3<f32>,
+ upper_bounds: Vector3<f32>,
+ obstacles: Vec<Obstacle>,
+}
+impl Maze {
+ /// Creates a new maze with the given bounds and number of obstacles
+ /// # Arguments
+ /// * `lower_bounds` - The lower bounds of the maze
+ /// * `upper_bounds` - The upper bounds of the maze
+ /// * `num_obstacles` - The number of obstacles in the maze
+ fn new(lower_bounds: Vector3<f32>, upper_bounds: Vector3<f32>, num_obstacles: usize) -> Self {
+ let mut maze = Maze {
+ lower_bounds,
+ upper_bounds,
+ obstacles: Vec::new(),
+ };
+ maze.generate_obstacles(num_obstacles);
+ maze
+ }
+ /// Generates the obstacles in the maze
+ /// # Arguments
+ /// * `num_obstacles` - The number of obstacles to generate
+ fn generate_obstacles(&mut self, num_obstacles: usize) {
+ let mut rng = rand::thread_rng();
+ self.obstacles = (0..num_obstacles)
+ .map(|_| {
+ let position = Vector3::new(
+ rand::Rng::gen_range(&mut rng, self.lower_bounds.x..self.upper_bounds.x),
+ rand::Rng::gen_range(&mut rng, self.lower_bounds.y..self.upper_bounds.y),
+ rand::Rng::gen_range(&mut rng, self.lower_bounds.z..self.upper_bounds.z),
+ );
+ let velocity = Vector3::new(
+ rand::Rng::gen_range(&mut rng, -0.2..0.2),
+ rand::Rng::gen_range(&mut rng, -0.2..0.2),
+ rand::Rng::gen_range(&mut rng, -0.1..0.1),
+ );
+ let radius = rand::Rng::gen_range(&mut rng, 0.05..0.1);
+ Obstacle::new(position, velocity, radius)
+ })
+ .collect();
+ }
+ /// Updates the obstacles in the maze, if an obstacle hits a boundary, it bounces off
+ /// # Arguments
+ /// * `dt` - The time step
+ fn update_obstacles(&mut self, dt: f32) {
+ self.obstacles.iter_mut().for_each(|obstacle| {
+ obstacle.position += obstacle.velocity * dt;
+ for i in 0..3 {
+ if obstacle.position[i] - obstacle.radius < self.lower_bounds[i]
+ || obstacle.position[i] + obstacle.radius > self.upper_bounds[i]
+ {
+ obstacle.velocity[i] *= -1.0;
+ }
+ }
+ });
+ }
+}
+/// Represents a camera in the simulation which is used to render the depth of the scene
+/// # Fields
+/// * `resolution` - The resolution of the camera
+/// * `fov` - The field of view of the camera
+/// * `near` - The near clipping plane of the camera
+/// * `far` - The far clipping plane of the camera
+/// * `tan_half_fov` - The tangent of half the field of view
+/// * `aspect_ratio` - The aspect ratio of the camera
+#[allow(dead_code)]
+struct Camera {
+ resolution: (usize, usize),
+ fov: f32,
+ near: f32,
+ far: f32,
+ tan_half_fov: f32,
+ aspect_ratio: f32,
+ ray_directions: Vec<Vector3<f32>>,
+}
+
+impl Camera {
+ /// Creates a new camera with the given resolution, field of view, near and far clipping planes
+ /// # Arguments
+ /// * `resolution` - The resolution of the camera
+ /// * `fov` - The field of view of the camera
+ /// * `near` - The near clipping plane of the camera
+ /// * `far` - The far clipping plane of the camera
+ fn new(resolution: (usize, usize), fov: f32, near: f32, far: f32) -> Self {
+ let (width, height) = resolution;
+ let aspect_ratio = width as f32 / height as f32;
+ let tan_half_fov = (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,
+ tan_half_fov,
+ aspect_ratio,
+ ray_directions,
+ }
+ }
+ /// Renders the depth of the scene from the perspective of the quadrotor
+ /// # Arguments
+ /// * `quad_position` - The position of the quadrotor
+ /// * `quad_orientation` - The orientation of the quadrotor
+ /// * `maze` - The maze in the scene
+ /// * `depth_buffer` - The depth buffer to store the depth values
+ fn render_depth(
+ &self,
+ quad_position: &Vector3<f32>,
+ quad_orientation: &UnitQuaternion<f32>,
+ maze: &Maze,
+ depth_buffer: &mut Vec<f32>,
+ ) {
+ let (width, height) = self.resolution;
+ let total_pixels = width * height;
+ depth_buffer.clear();
+ if depth_buffer.capacity() < total_pixels {
+ depth_buffer.reserve(total_pixels - depth_buffer.capacity());
+ }
+ let rotation_camera_to_world = quad_orientation.to_rotation_matrix().matrix()
+ * Matrix3::new(1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0);
+ let rotation_world_to_camera = rotation_camera_to_world.transpose();
+ depth_buffer.extend((0..total_pixels).map(|i| {
+ self.ray_cast(
+ quad_position,
+ &rotation_world_to_camera,
+ &(rotation_camera_to_world * self.ray_directions[i]),
+ maze,
+ )
+ .unwrap_or(std::f32::INFINITY)
+ }));
+ }
+ /// Casts a ray from the camera origin in the given direction
+ /// # Arguments
+ /// * `origin` - The origin of the ray
+ /// * `rotation_world_to_camera` - The rotation matrix from world to camera coordinates
+ /// * `direction` - The direction of the ray
+ /// * `maze` - The maze in the scene
+ /// # Returns
+ /// The distance to the closest obstacle hit by the ray
+ fn ray_cast(
+ &self,
+ origin: &Vector3<f32>,
+ rotation_world_to_camera: &Matrix3<f32>,
+ direction: &Vector3<f32>,
+ maze: &Maze,
+ ) -> Option<f32> {
+ 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 None;
+ }
+ // Inline sphere intersection
+ for obstacle in &maze.obstacles {
+ let oc = origin - &obstacle.position;
+ let b = oc.dot(direction);
+ let c = oc.dot(&oc) - obstacle.radius * obstacle.radius;
+ let discriminant = b * b - c;
+ if discriminant >= 0.0 {
+ let t = -b - discriminant.sqrt();
+ if t > self.near && t < closest_hit {
+ closest_hit = t;
+ }
+ }
+ }
+ if closest_hit < self.far {
+ let closest_pt = rotation_world_to_camera * direction * closest_hit;
+ Some(closest_pt.x)
+ } else {
+ None
+ }
+ }
+}
+/// Logs simulation data to the rerun recording stream
+/// # Arguments
+/// * `rec` - The rerun::RecordingStream instance
+/// * `quad` - The Quadrotor instance
+/// * `desired_position` - The desired position vector
+/// * `measured_accel` - The measured acceleration vector
+/// * `measured_gyro` - The measured angular velocity vector
+fn log_data(
+ rec: &rerun::RecordingStream,
+ quad: &Quadrotor,
+ desired_position: &Vector3<f32>,
+ desired_velocity: &Vector3<f32>,
+ measured_accel: &Vector3<f32>,
+ measured_gyro: &Vector3<f32>,
+) {
+ 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)]),
+ )
+ .unwrap();
+ 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),
+ )
+ .unwrap();
+ let (quad_roll, quad_pitch, quad_yaw) = quad.orientation.euler_angles();
+ for (name, value) in [
+ ("position/x", quad.position.x),
+ ("position/y", quad.position.y),
+ ("position/z", quad.position.z),
+ ("velocity/x", quad.velocity.x),
+ ("velocity/y", quad.velocity.y),
+ ("velocity/z", quad.velocity.z),
+ ("accel/x", measured_accel.x),
+ ("accel/y", measured_accel.y),
+ ("accel/z", measured_accel.z),
+ ("orientation/roll", quad_roll),
+ ("orientation/pitch", quad_pitch),
+ ("orientation/yaw", quad_yaw),
+ ("gyro/x", measured_gyro.x),
+ ("gyro/y", measured_gyro.y),
+ ("gyro/z", measured_gyro.z),
+ ("desired_position/x", desired_position.x),
+ ("desired_position/y", desired_position.y),
+ ("desired_position/z", desired_position.z),
+ ("desired_velocity/x", desired_velocity.x),
+ ("desired_velocity/y", desired_velocity.y),
+ ("desired_velocity/z", desired_velocity.z),
+ ] {
+ rec.log(name, &rerun::Scalar::new(value as f64)).unwrap();
+ }
+}
+/// Log the maze tube to the rerun recording stream
+/// # Arguments
+/// * `rec` - The rerun::RecordingStream instance
+/// * `maze` - The maze instance
+fn log_maze_tube(rec: &rerun::RecordingStream, maze: &Maze) {
+ let (lower_bounds, upper_bounds) = (maze.lower_bounds, maze.upper_bounds);
+ let center_position = rerun::external::glam::Vec3::new(
+ (lower_bounds.x + upper_bounds.x) / 2.0,
+ (lower_bounds.y + upper_bounds.y) / 2.0,
+ (lower_bounds.z + upper_bounds.z) / 2.0,
+ );
+ let half_sizes = rerun::external::glam::Vec3::new(
+ (upper_bounds.x - lower_bounds.x) / 2.0,
+ (upper_bounds.y - lower_bounds.y) / 2.0,
+ (upper_bounds.z - lower_bounds.z) / 2.0,
+ );
+ rec.log(
+ "world/maze/tube",
+ &rerun::Boxes3D::from_centers_and_half_sizes([center_position], [half_sizes])
+ .with_colors([rerun::Color::from_rgb(128, 128, 255)]),
+ )
+ .unwrap();
+}
+/// Log the maze obstacles to the rerun recording stream
+/// # Arguments
+/// * `rec` - The rerun::RecordingStream instance
+/// * `maze` - The maze instance
+fn log_maze_obstacles(rec: &rerun::RecordingStream, maze: &Maze) {
+ let (positions, radii): (Vec<_>, Vec<_>) = maze
+ .obstacles
+ .iter()
+ .map(|obstacle| {
+ (
+ (
+ obstacle.position.x,
+ obstacle.position.y,
+ obstacle.position.z,
+ ),
+ obstacle.radius,
+ )
+ })
+ .unzip();
+ rec.log(
+ "world/maze/obstacles",
+ &rerun::Points3D::new(positions)
+ .with_radii(radii)
+ .with_colors([rerun::Color::from_rgb(255, 128, 128)]),
+ )
+ .unwrap();
+}
+/// A struct to hold trajectory data
+/// # Fields
+/// * `points` - A vector of 3D points
+/// * `last_logged_point` - The last point that was logged
+/// * `min_distance_threadhold` - The minimum distance between points to log
+struct Trajectory {
+ points: Vec<Vector3<f32>>,
+ last_logged_point: Vector3<f32>,
+ min_distance_threadhold: f32,
+}
+
+impl Trajectory {
+ 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
+ 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
+fn log_trajectory(rec: &rerun::RecordingStream, trajectory: &Trajectory) {
+ let path = trajectory
+ .points
+ .iter()
+ .map(|p| (p.x, p.y, p.z))
+ .collect::<Vec<_>>();
+ rec.log(
+ "world/quad/path",
+ &rerun::LineStrips3D::new([path]).with_colors([rerun::Color::from_rgb(0, 255, 255)]),
+ )
+ .unwrap();
+}
+/// log mesh data to the rerun recording stream
+/// # Arguments
+/// * `rec` - The rerun::RecordingStream instance
+/// * `division` - The number of divisions in the mesh
+/// * `spacing` - The spacing between divisions
+fn log_mesh(rec: &rerun::RecordingStream, division: usize, spacing: f32) {
+ 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]),
+ )
+ .unwrap();
+}
+/// log depth image data to the rerun recording stream
+/// # Arguments
+/// * `rec` - The rerun::RecordingStream instance
+/// * `depth_image` - The depth image data
+/// * `width` - The width of the depth image
+/// * `height` - The height of the depth image
+/// * `min_depth` - The minimum depth value
+/// * `max_depth` - The maximum depth value
+fn log_depth_image(
+ rec: &rerun::RecordingStream,
+ depth_image: &Vec<f32>,
+ width: usize,
+ height: usize,
+ min_depth: f32,
+ max_depth: f32,
+) {
+ let mut image = ndarray::Array::zeros((height, width, 3));
+ let depth_range = max_depth - min_depth;
+ image
+ .axis_iter_mut(ndarray::Axis(0))
+ .enumerate()
+ .for_each(|(y, mut row)| {
+ for (x, mut pixel) in row.axis_iter_mut(ndarray::Axis(0)).enumerate() {
+ let depth = depth_image[y * width + x];
+ let color = if depth.is_finite() {
+ let normalized_depth = ((depth - min_depth) / depth_range).clamp(0.0, 1.0);
+ color_map_fn(normalized_depth * 255.0)
+ } else {
+ (0, 0, 0)
+ };
+ pixel[0] = color.0;
+ pixel[1] = color.1;
+ pixel[2] = color.2;
+ }
+ });
+ rec.log(
+ "world/quad/cam/depth",
+ &rerun::Image::from_color_model_and_tensor(rerun::ColorModel::RGB, image).unwrap(),
+ )
+ .unwrap();
+}
+/// turbo color map function
+/// # Arguments
+/// * `gray` - The gray value in the range [0, 255]
+/// # Returns
+/// * The RGB color value in the range [0, 255]
+fn color_map_fn(gray: f32) -> (u8, u8, u8) {
+ let x = gray / 255.0;
+ let r = (34.61
+ + x * (1172.33 - x * (10793.56 - x * (33300.12 - x * (38394.49 - x * 14825.05)))))
+ .clamp(0.0, 255.0) as u8;
+ let g = (23.31 + x * (557.33 + x * (1225.33 - x * (3574.96 - x * (1073.77 + x * 707.56)))))
+ .clamp(0.0, 255.0) as u8;
+ let b = (27.2 + x * (3211.1 - x * (15327.97 - x * (27814.0 - x * (22569.18 - x * 6838.66)))))
+ .clamp(0.0, 255.0) as u8;
+ (r, g, b)
+}
+
+/// Main function to run the quadrotor simulation
+fn main() {
+ let control_frequency = 200.0;
+ let simulation_frequency = 1000.0;
+ let log_frequency = 20.0;
+ let mut quad = Quadrotor::new(1.0 / simulation_frequency);
+ let mut controller = PIDController::new();
+ let mut imu = Imu::new();
+ let rec = rerun::RecordingStreamBuilder::new("Peng")
+ .connect()
+ .unwrap();
+ let upper_bounds = Vector3::new(3.0, 2.0, 2.0);
+ let lower_bounds = Vector3::new(-3.0, -2.0, 0.0);
+ let mut maze = Maze::new(lower_bounds, upper_bounds, 20);
+ let camera = Camera::new((128, 96), 90.0_f32.to_radians(), 0.1, 5.0);
+ let mut planner_manager = PlannerManager::new(Vector3::zeros(), 0.0);
+ let mut trajectory = Trajectory::new(Vector3::new(0.0, 0.0, 0.0));
+ let mut depth_buffer: Vec<f32> = vec![0.0; camera.resolution.0 * camera.resolution.1];
+ rec.set_time_seconds("timestamp", 0);
+ log_mesh(&rec, 7, 0.5);
+ log_maze_tube(&rec, &maze);
+ log_maze_obstacles(&rec, &maze);
+ let mut previous_thrust = 0.0;
+ let mut previous_torque = Vector3::zeros();
+ let mut i = 0;
+ loop {
+ let time = quad.time_step * i as f32;
+ rec.set_time_seconds("timestamp", time);
+ maze.update_obstacles(quad.time_step);
+ update_planner(&mut planner_manager, i, time, &quad, &maze.obstacles);
+ let (desired_position, desired_velocity, desired_yaw) = planner_manager.update(
+ quad.position,
+ quad.orientation,
+ quad.velocity,
+ time,
+ &maze.obstacles,
+ );
+ let (thrust, calculated_desired_orientation) = controller.compute_position_control(
+ desired_position,
+ desired_velocity,
+ desired_yaw,
+ quad.position,
+ quad.velocity,
+ quad.time_step,
+ quad.mass,
+ quad.gravity,
+ );
+ let torque = controller.compute_attitude_control(
+ &calculated_desired_orientation,
+ &quad.orientation,
+ &quad.angular_velocity,
+ quad.time_step,
+ );
+ if i % (simulation_frequency as usize / control_frequency as usize) == 0 {
+ quad.update_dynamics_with_controls(thrust, &torque);
+ previous_thrust = thrust;
+ previous_torque = torque;
+ } else {
+ quad.update_dynamics_with_controls(previous_thrust, &previous_torque);
+ }
+ imu.update(quad.time_step);
+ let (true_accel, true_gyro) = quad.read_imu();
+ let (_measured_accel, _measured_gyro) = imu.read(true_accel, true_gyro);
+ if i % (simulation_frequency as usize / log_frequency as usize) == 0 {
+ if trajectory.add_point(quad.position) {
+ log_trajectory(&rec, &trajectory);
+ }
+ log_data(
+ &rec,
+ &quad,
+ &desired_position,
+ &desired_velocity,
+ &_measured_accel,
+ &_measured_gyro,
+ );
+ camera.render_depth(&quad.position, &quad.orientation, &maze, &mut depth_buffer);
+ log_depth_image(
+ &rec,
+ &depth_buffer,
+ camera.resolution.0,
+ camera.resolution.1,
+ camera.near,
+ camera.far,
+ );
+ log_maze_obstacles(&rec, &maze);
+ }
+ i += 1;
+ if (i as f32 * 100.0 * quad.time_step) as i32 >= 11000 {
+ break;
+ }
+ }
+}
+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+=`