List of all items
Structs
- CirclePlanner
- HoverPlanner
- Imu
- LandingPlanner
- LissajousPlanner
- MinimumJerkLinePlanner
- Obstacle
- ObstacleAvoidancePlanner
- PIDController
- PlannerManager
- Quadrotor
- Trajectory
pub(crate) enum PlannerType {
+ Hover(HoverPlanner),
+ MinimumJerkLine(MinimumJerkLinePlanner),
+ Lissajous(LissajousPlanner),
+ Circle(CirclePlanner),
+ Landing(LandingPlanner),
+ ObstacleAvoidance(ObstacleAvoidancePlanner),
+}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
+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>,
+ 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_obstacles(rec: &RecordingStream, obstacles: &[Obstacle])log obstacle data to the rerun recording stream
+rec - The rerun::RecordingStream instanceobstacles - A slice of obstaclespub(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_obstacles(
+ obstacles: &mut Vec<Obstacle>,
+ bounds: &Vector3<f32>,
+ dt: f32,
+)Updates the positions of the obstacles +Bounces them off the boundaries if they collide +Keeps them within the bounds of the simulation
+obstacles - A mutable reference to the vector of obstaclesbounds - The bounds of the simulation spacedt - The time steppub(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 visualizationTo use this crate in your project, add the following to your Cargo.toml:
[dependencies]
+quadrotor_sim = "0.1.0"
+Then, you can use the crate in your Rust code:
+ +use quadrotor_sim::{Quadrotor, PIDController, PlannerManager};
+
+fn main() {
+ let mut quad = Quadrotor::new();
+ let controller = PIDController::new();
+ let planner = PlannerManager::new(quad.position, 0.0);
+
+ // Simulation loop
+ // ...
+}pub(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
+Creates a new IMU with default parameters
+let imu = Imu::new();
+assert_eq!(imu.accel_noise_std, 0.02);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 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 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 +This planner 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
+Creates a new PIDController with default gains
+let controller = PIDController::new();
+assert_eq!(controller.kp_pos, Vector3::new(7.1, 7.1, 11.9));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
+let quad = Quadrotor::new();
+assert_eq!(quad.mass, 1.3);Updates the quadrotor’s dynamics with control inputs
+control_thrust - The total thrust force applied to the quadrotorcontrol_torque - The 3D torque vector applied to the quadrotorlet mut quad = Quadrotor::new();
+let thrust = 10.0;
+let torque = Vector3::new(0.1, 0.1, 0.1);
+quad.update_dynamics_with_controls(thrust, &torque);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 Trajectory {
+ pub(crate) points: Vec<Vector3<f32>>,
+}points: Vec<Vector3<f32>>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) 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).\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 mesh data to the rerun recording stream\nlog obstacle 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\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\nSimulates IMU readings with added bias and noise\nSimulates IMU readings with added noise\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 positions of the obstacles Bounces them off …\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..1201c37b
--- /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 +
//! # 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
+//!
+//! ## Usage
+//!
+//! To use this crate in your project, add the following to your `Cargo.toml`:
+//!
+//! ```toml
+//! [dependencies]
+//! quadrotor_sim = "0.1.0"
+//! ```
+//!
+//! Then, you can use the crate in your Rust code:
+//!
+//! ```rust
+//! use quadrotor_sim::{Quadrotor, PIDController, PlannerManager};
+//!
+//! fn main() {
+//! let mut quad = Quadrotor::new();
+//! let controller = PIDController::new();
+//! let planner = PlannerManager::new(quad.position, 0.0);
+//!
+//! // Simulation loop
+//! // ...
+//! }
+//! ```
+//!
+use nalgebra::{Matrix3, Rotation3, 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
+ ///
+ /// # Examples
+ ///
+ /// ```
+ /// let quad = Quadrotor::new();
+ /// assert_eq!(quad.mass, 1.3);
+ /// ```
+ pub fn new() -> 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: 0.01,
+ // thrust_coefficient: 0.0,
+ drag_coefficient: 0.000,
+ inertia_matrix,
+ inertia_matrix_inv: inertia_matrix.try_inverse().unwrap(),
+ }
+ }
+ #[allow(dead_code)]
+ pub fn update_dynamics(&mut self) {
+ // Update position and velocity based on current state and simple physics
+ let acceleration = Vector3::new(0.0, 0.0, -self.gravity);
+ self.velocity += acceleration * self.time_step;
+ self.position += self.velocity * self.time_step;
+ self.orientation *=
+ UnitQuaternion::from_scaled_axis(self.angular_velocity * self.time_step);
+ }
+
+ /// 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
+ ///
+ /// # Examples
+ ///
+ /// ```
+ /// let mut quad = Quadrotor::new();
+ /// let thrust = 10.0;
+ /// let torque = Vector3::new(0.1, 0.1, 0.1);
+ /// quad.update_dynamics_with_controls(thrust, &torque);
+ /// ```
+ 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
+ ///
+ /// # Examples
+ ///
+ /// ```
+ /// let imu = Imu::new();
+ /// assert_eq!(imu.accel_noise_std, 0.02);
+ 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
+ ///
+ /// # Examples
+ ///
+ /// ```
+ /// let controller = PIDController::new();
+ /// assert_eq!(controller.kp_pos, Vector3::new(7.1, 7.1, 11.9));
+ /// ```
+ 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),
+}
+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),
+ }
+ }
+ /// 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),
+ }
+ }
+}
+
+/// 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
+/// This planner 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
+ }
+}
+
+/// 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>,
+) {
+ 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 => 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,
+ }
+ }
+}
+/// Generates a random set of obstacles
+///
+/// # Arguments
+///
+/// * `num_obstacles` - The number of obstacles to generate
+/// * `bounds` - The bounds of the simulation space
+///
+/// # Returns
+///
+/// A vector of randomly generated obstacles
+fn generate_random_obstacles(num_obstacles: usize, bounds: Vector3<f32>) -> Vec<Obstacle> {
+ let mut rng = rand::thread_rng();
+ let mut obstacles = Vec::new();
+ for _ in 0..num_obstacles {
+ let position = Vector3::new(
+ rand::Rng::gen_range(&mut rng, -bounds.x..bounds.x),
+ rand::Rng::gen_range(&mut rng, -bounds.y..bounds.y),
+ rand::Rng::gen_range(&mut rng, 0.0..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);
+ obstacles.push(Obstacle::new(position, velocity, radius));
+ }
+ obstacles
+}
+
+/// Updates the positions of the obstacles
+/// Bounces them off the boundaries if they collide
+/// Keeps them within the bounds of the simulation
+///
+/// # Arguments
+///
+/// * `obstacles` - A mutable reference to the vector of obstacles
+/// * `bounds` - The bounds of the simulation space
+/// * `dt` - The time step
+fn update_obstacles(obstacles: &mut Vec<Obstacle>, bounds: &Vector3<f32>, dt: f32) {
+ for obstacle in obstacles.iter_mut() {
+ // Update position
+ obstacle.position += obstacle.velocity * dt;
+ // Bounce off boundaries
+ for i in 0..3 {
+ if obstacle.position[i] - obstacle.radius < 0.0
+ || obstacle.position[i] + obstacle.radius > bounds[i]
+ {
+ obstacle.velocity[i] = -obstacle.velocity[i];
+ }
+ }
+ // Ensure obstacles stay within bounds
+ obstacle.position = obstacle
+ .position
+ .zip_map(bounds, |p, b| p.clamp(obstacle.radius, b - obstacle.radius));
+ }
+}
+
+/// 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>,
+ measured_accel: &Vector3<f32>,
+ measured_gyro: &Vector3<f32>,
+) {
+ rec.log(
+ "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(
+ "quadrotor",
+ &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),
+ ] {
+ rec.log(name, &rerun::Scalar::new(value as f64)).unwrap();
+ }
+}
+
+/// A struct to hold trajectory data
+/// # Fields
+/// * `points` - A vector of 3D points
+struct Trajectory {
+ points: Vec<Vector3<f32>>,
+}
+
+/// 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("quadrotor_path", &rerun::LineStrips3D::new([path]))
+ .unwrap();
+}
+
+/// log obstacle data to the rerun recording stream
+/// # Arguments
+/// * `rec` - The rerun::RecordingStream instance
+/// * `obstacles` - A slice of obstacles
+fn log_obstacles(rec: &rerun::RecordingStream, obstacles: &[Obstacle]) {
+ for (i, obstacle) in obstacles.iter().enumerate() {
+ // generate name of obstacle i
+ let name = format!("obstacles/obstacle_{}", i);
+ rec.log(
+ name,
+ &rerun::Points3D::new([(
+ obstacle.position.x,
+ obstacle.position.y,
+ obstacle.position.z,
+ )])
+ .with_radii([obstacle.radius]),
+ )
+ .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(
+ "mesh",
+ &rerun::LineStrips3D::new(line_strips)
+ .with_colors([rerun::Color::from_rgb(255, 255, 255)])
+ .with_radii([0.02]),
+ )
+ .unwrap();
+}
+/// Main function to run the quadrotor simulation
+fn main() {
+ let mut quad = Quadrotor::new();
+ let mut controller = PIDController::new();
+ let mut imu = Imu::new();
+ let rec = rerun::RecordingStreamBuilder::new("quadrotor_simulation")
+ .connect()
+ .unwrap();
+
+ let mut planner_manager = PlannerManager::new(Vector3::zeros(), 0.0);
+ let bounds = Vector3::new(2.0, 2.0, 2.0);
+ let mut obstacles = generate_random_obstacles(10, bounds);
+ let mut trajectory = Trajectory {
+ points: vec![Vector3::new(0.0, 0.0, 0.0)],
+ };
+ rec.set_time_seconds("timestamp", 0);
+ log_mesh(&rec, 5, 0.5);
+ let mut i = 0;
+ loop {
+ let time = quad.time_step * i as f32;
+ rec.set_time_seconds("timestamp", time);
+ update_obstacles(&mut obstacles, &bounds, quad.time_step);
+ update_planner(&mut planner_manager, i, time, &quad, &obstacles);
+ let (desired_position, desired_velocity, desired_yaw) = planner_manager.update(
+ quad.position,
+ quad.orientation,
+ quad.velocity,
+ time,
+ &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,
+ );
+ quad.update_dynamics_with_controls(thrust, &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 % 5 == 0 {
+ trajectory.points.push(quad.position);
+ if i >= 7000 && i < 9000 {
+ log_obstacles(&rec, &obstacles);
+ }
+ log_trajectory(&rec, &trajectory);
+ log_data(
+ &rec,
+ &quad,
+ &desired_position,
+ &_measured_accel,
+ &_measured_gyro,
+ );
+ }
+ i += 1;
+ if i >= 9000 {
+ 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+=`