pub enum PlannerType {
+ Hover(HoverPlanner),
+ MinimumJerkLine(MinimumJerkLinePlanner),
+ Lissajous(LissajousPlanner),
+ Circle(CirclePlanner),
+ Landing(LandingPlanner),
+ ObstacleAvoidance(ObstacleAvoidancePlanner),
+ MinimumSnapWaypoint(MinimumSnapWaypointPlanner),
+}Enum representing different types of trajectory planners
+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 enum SimulationError {
+ RerunError(RecordingStreamError),
+ RerunSpawnError(SpawnError),
+ NalgebraError(String),
+ NormalError(NormalError),
+ OtherError(String),
+}self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub fn log_data(
+ rec: &RecordingStream,
+ quad: &Quadrotor,
+ desired_position: &Vector3<f32>,
+ desired_velocity: &Vector3<f32>,
+ measured_accel: &Vector3<f32>,
+ measured_gyro: &Vector3<f32>,
+) -> Result<(), SimulationError>Logs simulation data to the rerun recording stream
+rec - The rerun::RecordingStream instancequad - The Quadrotor instancedesired_position - The desired position vectormeasured_accel - The measured acceleration vectormeasured_gyro - The measured angular velocity vectorpub fn log_depth_image(
+ rec: &RecordingStream,
+ depth_image: &Vec<f32>,
+ width: usize,
+ height: usize,
+ min_depth: f32,
+ max_depth: f32,
+) -> Result<(), SimulationError>log depth image data to the rerun recording stream
+rec - The rerun::RecordingStream instancedepth_image - The depth image datawidth - The width of the depth imageheight - The height of the depth imagemin_depth - The minimum depth valuemax_depth - The maximum depth valuepub fn log_maze_obstacles(
+ rec: &RecordingStream,
+ maze: &Maze,
+) -> Result<(), SimulationError>Log the maze obstacles to the rerun recording stream
+rec - The rerun::RecordingStream instancemaze - The maze instancepub fn log_maze_tube(
+ rec: &RecordingStream,
+ maze: &Maze,
+) -> Result<(), SimulationError>Log the maze tube to the rerun recording stream
+rec - The rerun::RecordingStream instancemaze - The maze instancepub fn log_mesh(
+ rec: &RecordingStream,
+ division: usize,
+ spacing: f32,
+) -> Result<(), SimulationError>log mesh data to the rerun recording stream
+rec - The rerun::RecordingStream instancedivision - The number of divisions in the meshspacing - The spacing between divisionspub fn log_trajectory(
+ rec: &RecordingStream,
+ trajectory: &Trajectory,
+) -> Result<(), SimulationError>log trajectory data to the rerun recording stream
+rec - The rerun::RecordingStream instancetrajectory - The Trajectory instancepub fn update_planner(
+ planner_manager: &mut PlannerManager,
+ step: usize,
+ time: f32,
+ quad: &Quadrotor,
+ obstacles: &Vec<Obstacle>,
+ planner_config: &Vec<PlannerStepConfig>,
+) -> Result<(), SimulationError>Updates the planner based on the current simulation step and configuration
+planner_manager - The PlannerManager instance to updatestep - The current simulation steptime - The current simulation timequad - The Quadrotor instanceobstacles - The current obstacles in the simulationplanner_config - The planner configurationThis 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 struct Camera {
+ pub resolution: (usize, usize),
+ pub fov: f32,
+ pub near: f32,
+ pub far: f32,
+ pub aspect_ratio: f32,
+ pub ray_directions: Vec<Vector3<f32>>,
+}Represents a camera in the simulation which is used to render the depth of the scene
+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§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 struct CirclePlanner { /* private fields */ }Planner for circular trajectories
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct HoverPlanner { /* private fields */ }Planner for hovering at a fixed position
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct Imu {
+ pub accel_bias: Vector3<f32>,
+ pub gyro_bias: Vector3<f32>,
+ pub accel_noise_std: f32,
+ pub gyro_noise_std: f32,
+ pub 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
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct LandingPlanner { /* private fields */ }Planner for landing maneuvers
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct LissajousPlanner { /* private fields */ }Planner for Lissajous curve trajectories
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct Maze {
+ pub lower_bounds: Vector3<f32>,
+ pub upper_bounds: Vector3<f32>,
+ pub 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 struct MinimumJerkLinePlanner { /* private fields */ }Planner for minimum jerk trajectories along a straight line
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct MinimumSnapWaypointPlanner { /* private fields */ }Waypoint planner that generates a minimum snap trajectory between waypoints
+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 trajectoryself into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct Obstacle {
+ pub position: Vector3<f32>,
+ pub velocity: Vector3<f32>,
+ pub radius: f32,
+}Represents an obstacle in the simulation
+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 struct ObstacleAvoidancePlanner { /* private fields */ }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
+self into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct PIDController { /* private fields */ }PID controller for quadrotor position and attitude control
+Creates a new PIDController with default gains +gains are in the order of proportional, derivative, and integral
+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 struct PlannerManager { /* private fields */ }Manages different trajectory planners and switches between them
+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 struct PlannerStepConfig {
+ pub step: usize,
+ pub planner_type: String,
+ pub params: Value,
+}step: usize§planner_type: String§params: Valueself into a Left variant of Either<Self, Self>
+if into_left is true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself into a Left variant of Either<Self, Self>
+if into_left(&self) returns true.
+Converts self into a Right variant of Either<Self, Self>
+otherwise. Read moreself from the equivalent element of its
+superset. Read moreself is actually part of its subset T (and can be converted to it).self.to_subset but without any property checks. Always succeeds.self to the equivalent element of its superset.Subscriber to this type, returning a
+[WithDispatch] wrapper. Read morepub struct Quadrotor {
+ pub position: Vector3<f32>,
+ pub velocity: Vector3<f32>,
+ pub orientation: UnitQuaternion<f32>,
+ pub angular_velocity: Vector3<f32>,
+ pub mass: f32,
+ pub gravity: f32,
+ pub time_step: f32,
+ pub drag_coefficient: f32,
+ pub inertia_matrix: Matrix3<f32>,
+ pub inertia_matrix_inv: Matrix3<f32>,
+}Represents a quadrotor with its physical properties and state
+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 struct Trajectory { /* private fields */ }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 logself 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 moreU::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nCalls U::from(self).\nChecks if the current trajectory is finished\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\nMass of the quadrotor in kg\nCreates a new Quadrotor with default parameters\nCreates a new IMU with default parameters\nCreates a new PIDController with default gains gains are …\nCreates a new PlannerManager with an initial hover planner\nCreates a new maze with the given bounds and number of …\nCreates a new camera with the given resolution, field of …\nCurrent orientation of the quadrotor\nPlans the trajectory based on the current planner type\nCurrent position of the quadrotor in 3D space\nCasts a ray from the camera origin in the given direction\nSimulates IMU readings with added bias and noise\nSimulates IMU readings\nRenders the depth of the scene from the perspective of the …\nSets a new planner\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 +
//! # Quadrotor Simulation
+//! This crate provides a comprehensive simulation environment for quadrotor drones.
+//! It includes models for quadrotor dynamics, IMU simulation, various trajectory planners,
+//! and a PID controller for position and attitude control.
+//! ## Features
+//! - Realistic quadrotor dynamics simulation
+//! - IMU sensor simulation with configurable noise parameters
+//! - Multiple trajectory planners including hover, minimum jerk, Lissajous curves, and circular paths
+//! - PID controller for position and attitude control
+//! - Integration with the `rerun` crate for visualization
+use nalgebra::{Matrix3, Rotation3, SMatrix, UnitQuaternion, Vector3};
+use rand_distr::{Distribution, Normal};
+use std::f32::consts::PI;
+#[derive(thiserror::Error, Debug)]
+pub enum SimulationError {
+ #[error("Rerun error: {0}")]
+ RerunError(#[from] rerun::RecordingStreamError),
+ #[error("Rerun spawn error: {0}")]
+ RerunSpawnError(#[from] rerun::SpawnError),
+ #[error("Nalgebra error: {0}")]
+ NalgebraError(String),
+ #[error("Normal error: {0}")]
+ NormalError(#[from] rand_distr::NormalError),
+ #[error("Other error: {0}")]
+ OtherError(String),
+}
+/// Represents a quadrotor with its physical properties and state
+pub struct Quadrotor {
+ /// Current position of the quadrotor in 3D space
+ pub position: Vector3<f32>,
+ /// Current velocity of the quadrotor
+ pub velocity: Vector3<f32>,
+ /// Current orientation of the quadrotor
+ pub orientation: UnitQuaternion<f32>,
+ /// Current angular velocity of the quadrotor
+ pub angular_velocity: Vector3<f32>,
+ /// Mass of the quadrotor in kg
+ pub mass: f32,
+ /// Gravitational acceleration in m/s^2
+ pub gravity: f32,
+ /// Simulation time step in seconds
+ pub time_step: f32,
+ /// Drag coefficient
+ pub drag_coefficient: f32,
+ /// Inertia matrix of the quadrotor
+ pub inertia_matrix: Matrix3<f32>,
+ /// Inverse of the inertia matrix
+ pub inertia_matrix_inv: Matrix3<f32>,
+}
+
+impl Quadrotor {
+ /// Creates a new Quadrotor with default parameters
+ /// * Arguments
+ /// * `time_step` - The simulation time step in seconds
+ pub fn new(
+ time_step: f32,
+ _mass: f32,
+ _gravity: f32,
+ _drag_coefficients: f32,
+ _inertia_matrix: [f32; 9],
+ ) -> Result<Self, SimulationError> {
+ let inertia_matrix = Matrix3::from_row_slice(&_inertia_matrix);
+ let inertia_matrix_inv =
+ inertia_matrix
+ .try_inverse()
+ .ok_or(SimulationError::NalgebraError(
+ "Failed to invert inertia matrix".to_string(),
+ ))?;
+ Ok(Self {
+ position: Vector3::zeros(),
+ velocity: Vector3::zeros(),
+ orientation: UnitQuaternion::identity(),
+ angular_velocity: Vector3::zeros(),
+ mass: 1.3,
+ gravity: 9.81,
+ time_step,
+ drag_coefficient: 0.000,
+ inertia_matrix,
+ inertia_matrix_inv,
+ })
+ }
+ /// Updates the quadrotor's dynamics with control inputs
+ /// # Arguments
+ /// * `control_thrust` - The total thrust force applied to the quadrotor
+ /// * `control_torque` - The 3D torque vector applied to the quadrotor
+ pub fn update_dynamics_with_controls(
+ &mut self,
+ control_thrust: f32,
+ control_torque: &Vector3<f32>,
+ ) {
+ let gravity_force = Vector3::new(0.0, 0.0, -self.mass * self.gravity);
+ let drag_force = -self.drag_coefficient * self.velocity.norm() * self.velocity;
+ let thrust_world = self.orientation * Vector3::new(0.0, 0.0, control_thrust);
+ let acceleration = (thrust_world + gravity_force + drag_force) / self.mass;
+ self.velocity += acceleration * self.time_step;
+ self.position += self.velocity * self.time_step;
+ let inertia_angular_velocity = self.inertia_matrix * self.angular_velocity;
+ let gyroscopic_torque = self.angular_velocity.cross(&inertia_angular_velocity);
+ let angular_acceleration = self.inertia_matrix_inv * (control_torque - gyroscopic_torque);
+ self.angular_velocity += angular_acceleration * self.time_step;
+ self.orientation *=
+ UnitQuaternion::from_scaled_axis(self.angular_velocity * self.time_step);
+ }
+ /// Simulates IMU readings
+ /// # Returns
+ /// A tuple containing the actual acceleration and angular velocity
+ pub fn read_imu(&self) -> Result<(Vector3<f32>, Vector3<f32>), SimulationError> {
+ let gravity_world = Vector3::new(0.0, 0.0, self.gravity);
+ let true_acceleration =
+ self.orientation.inverse() * (self.velocity / self.time_step - gravity_world);
+ Ok((true_acceleration, self.angular_velocity))
+ }
+}
+/// Represents an Inertial Measurement Unit (IMU) with bias and noise characteristics
+pub struct Imu {
+ /// Accelerometer bias
+ pub accel_bias: Vector3<f32>,
+ /// Gyroscope bias
+ pub gyro_bias: Vector3<f32>,
+ /// Standard deviation of accelerometer noise
+ pub accel_noise_std: f32,
+ /// Standard deviation of gyroscope noise
+ pub gyro_noise_std: f32,
+ /// Bias instability coefficient
+ pub bias_instability: f32,
+}
+
+impl Imu {
+ /// Creates a new IMU with default parameters
+ pub fn new(_accel_noise_std: f32, _gyro_noise_std: f32, _bias_instability: f32) -> Self {
+ Self {
+ accel_bias: Vector3::zeros(),
+ gyro_bias: Vector3::zeros(),
+ accel_noise_std: _accel_noise_std,
+ gyro_noise_std: _gyro_noise_std,
+ bias_instability: _bias_instability,
+ }
+ }
+ /// Updates the IMU biases over time
+ /// # Arguments
+ /// * `dt` - Time step for the update
+ pub fn update(&mut self, dt: f32) -> Result<(), SimulationError> {
+ let bias_drift = Normal::new(0.0, self.bias_instability * dt.sqrt())?;
+ let drift_vector =
+ || Vector3::from_iterator((0..3).map(|_| bias_drift.sample(&mut rand::thread_rng())));
+ self.accel_bias += drift_vector();
+ self.gyro_bias += drift_vector();
+ Ok(())
+ }
+ /// Simulates IMU readings with added bias and noise
+ /// # Arguments
+ /// * `true_acceleration` - The true acceleration vector
+ /// * `true_angular_velocity` - The true angular velocity vector
+ /// # Returns
+ /// A tuple containing the measured acceleration and angular velocity
+ pub fn read(
+ &self,
+ true_acceleration: Vector3<f32>,
+ true_angular_velocity: Vector3<f32>,
+ ) -> Result<(Vector3<f32>, Vector3<f32>), SimulationError> {
+ let accel_noise = Normal::new(0.0, self.accel_noise_std)?;
+ let gyro_noise = Normal::new(0.0, self.gyro_noise_std)?;
+ let accel_noise_sample =
+ || Vector3::from_iterator((0..3).map(|_| accel_noise.sample(&mut rand::thread_rng())));
+ let gyro_noise_sample =
+ || Vector3::from_iterator((0..3).map(|_| gyro_noise.sample(&mut rand::thread_rng())));
+ let measured_acceleration = true_acceleration + self.accel_bias + accel_noise_sample();
+ let measured_ang_velocity = true_angular_velocity + self.gyro_bias + gyro_noise_sample();
+ Ok((measured_acceleration, measured_ang_velocity))
+ }
+}
+/// PID controller for quadrotor position and attitude control
+pub struct PIDController {
+ /// PID gain for position control including proportional, derivative, and integral gains
+ kpid_pos: [Vector3<f32>; 3],
+ /// PID gain for attitude control including proportional, derivative, and integral gains
+ kpid_att: [Vector3<f32>; 3],
+ /// 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
+ /// gains are in the order of proportional, derivative, and integral
+ pub fn new(
+ _kpid_pos: [[f32; 3]; 3],
+ _kpid_att: [[f32; 3]; 3],
+ _max_integral_pos: [f32; 3],
+ _max_integral_att: [f32; 3],
+ ) -> Self {
+ Self {
+ kpid_pos: _kpid_pos.map(Vector3::from),
+ kpid_att: _kpid_att.map(Vector3::from),
+ integral_pos_error: Vector3::zeros(),
+ integral_att_error: Vector3::zeros(),
+ max_integral_pos: Vector3::from(_max_integral_pos),
+ max_integral_att: Vector3::from(_max_integral_att),
+ }
+ }
+ /// Computes attitude control torques
+ /// # Arguments
+ /// * `desired_orientation` - The desired orientation quaternion
+ /// * `current_orientation` - The current orientation quaternion
+ /// * `current_angular_velocity` - The current angular velocity
+ /// * `dt` - Time step
+ /// # Returns
+ /// The computed control torque vector
+ pub fn compute_attitude_control(
+ &mut self,
+ desired_orientation: &UnitQuaternion<f32>,
+ current_orientation: &UnitQuaternion<f32>,
+ current_angular_velocity: &Vector3<f32>,
+ dt: f32,
+ ) -> Vector3<f32> {
+ let error_orientation = current_orientation.inverse() * desired_orientation;
+ let (roll_error, pitch_error, yaw_error) = error_orientation.euler_angles();
+ let error_angles = Vector3::new(roll_error, pitch_error, yaw_error);
+ self.integral_att_error += error_angles * dt;
+ self.integral_att_error = self
+ .integral_att_error
+ .zip_map(&self.max_integral_att, |int, max| int.clamp(-max, max));
+ let error_angular_velocity = -current_angular_velocity; // TODO: Add desired angular velocity
+ self.kpid_att[0].component_mul(&error_angles)
+ + self.kpid_att[1].component_mul(&error_angular_velocity)
+ + self.kpid_att[2].component_mul(&self.integral_att_error)
+ }
+ /// Computes position control thrust and desired orientation
+ /// # Arguments
+ /// * `desired_position` - The desired position
+ /// * `desired_velocity` - The desired velocity
+ /// * `desired_yaw` - The desired yaw angle
+ /// * `current_position` - The current position
+ /// * `current_velocity` - The current velocity
+ /// * `dt` - Time step
+ /// * `mass` - Mass of the quadrotor
+ /// * `gravity` - Gravitational acceleration
+ /// # Returns
+ /// A tuple containing the computed thrust and desired orientation quaternion
+ pub fn compute_position_control(
+ &mut self,
+ desired_position: &Vector3<f32>,
+ desired_velocity: &Vector3<f32>,
+ desired_yaw: f32,
+ current_position: &Vector3<f32>,
+ current_velocity: &Vector3<f32>,
+ dt: f32,
+ 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
+ .zip_map(&self.max_integral_pos, |int, max| int.clamp(-max, max));
+ let acceleration = self.kpid_pos[0].component_mul(&error_position)
+ + self.kpid_pos[1].component_mul(&error_velocity)
+ + self.kpid_pos[2].component_mul(&self.integral_pos_error);
+ let gravity_compensation = Vector3::new(0.0, 0.0, gravity);
+ let total_acceleration = acceleration + gravity_compensation;
+ let thrust = mass * total_acceleration.norm();
+ let desired_orientation = if total_acceleration.norm() > 1e-6 {
+ let z_body = total_acceleration.normalize();
+ let yaw_rotation = UnitQuaternion::from_euler_angles(0.0, 0.0, desired_yaw);
+ let x_body_horizontal = yaw_rotation * Vector3::new(1.0, 0.0, 0.0);
+ let y_body = z_body.cross(&x_body_horizontal).normalize();
+ let x_body = y_body.cross(&z_body);
+ UnitQuaternion::from_rotation_matrix(&Rotation3::from_matrix_unchecked(
+ Matrix3::from_columns(&[x_body, y_body, z_body]),
+ ))
+ } else {
+ UnitQuaternion::from_euler_angles(0.0, 0.0, desired_yaw)
+ };
+ (thrust, desired_orientation)
+ }
+}
+/// Enum representing different types of trajectory planners
+pub enum PlannerType {
+ Hover(HoverPlanner),
+ MinimumJerkLine(MinimumJerkLinePlanner),
+ Lissajous(LissajousPlanner),
+ Circle(CirclePlanner),
+ Landing(LandingPlanner),
+ ObstacleAvoidance(ObstacleAvoidancePlanner),
+ MinimumSnapWaypoint(MinimumSnapWaypointPlanner),
+}
+impl PlannerType {
+ /// Plans the trajectory based on the current planner type
+ /// # Arguments
+ /// * `current_position` - The current position of the quadrotor
+ /// * `current_velocity` - The current velocity of the quadrotor
+ /// * `time` - The current simulation time
+ /// # Returns
+ /// A tuple containing the desired position, velocity, and yaw angle
+ pub fn plan(
+ &self,
+ current_position: Vector3<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
+ pub fn is_finished(
+ &self,
+ current_position: Vector3<f32>,
+ time: f32,
+ ) -> Result<bool, SimulationError> {
+ match self {
+ PlannerType::Hover(p) => p.is_finished(current_position, time),
+ PlannerType::MinimumJerkLine(p) => p.is_finished(current_position, time),
+ PlannerType::Lissajous(p) => p.is_finished(current_position, time),
+ PlannerType::Circle(p) => p.is_finished(current_position, time),
+ PlannerType::Landing(p) => p.is_finished(current_position, time),
+ PlannerType::ObstacleAvoidance(p) => p.is_finished(current_position, time),
+ PlannerType::MinimumSnapWaypoint(p) => p.is_finished(current_position, time),
+ }
+ }
+}
+/// Trait defining the interface for trajectory planners
+trait Planner {
+ /// Plans the trajectory based on the current state and time
+ /// # Arguments
+ /// * `current_position` - The current position of the quadrotor
+ /// * `current_velocity` - The current velocity of the quadrotor
+ /// * `time` - The current simulation time
+ /// # Returns
+ fn plan(
+ &self,
+ current_position: Vector3<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,
+ ) -> Result<bool, SimulationError>;
+}
+/// Planner for hovering at a fixed position
+pub 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,
+ ) -> Result<bool, SimulationError> {
+ Ok(false) // Hover planner never "finished"
+ }
+}
+/// Planner for minimum jerk trajectories along a straight line
+pub 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).clamp(0.0, 1.0);
+ let s = 10.0 * t.powi(3) - 15.0 * t.powi(4) + 6.0 * t.powi(5);
+ let s_dot = (30.0 * t.powi(2) - 60.0 * t.powi(3) + 30.0 * t.powi(4)) / self.duration;
+ let position = self.start_position + (self.end_position - self.start_position) * s;
+ let velocity = (self.end_position - self.start_position) * s_dot;
+ let yaw = self.start_yaw + (self.end_yaw - self.start_yaw) * s;
+ (position, velocity, yaw)
+ }
+
+ fn is_finished(
+ &self,
+ _current_position: Vector3<f32>,
+ _time: f32,
+ ) -> Result<bool, SimulationError> {
+ Ok((_current_position - self.end_position).norm() < 0.01
+ && _time >= self.start_time + self.duration)
+ }
+}
+/// Planner for Lissajous curve trajectories
+pub 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).clamp(0.0, 1.0);
+ let smooth_start = if t < self.ramp_time / self.duration {
+ let t_ramp = t / (self.ramp_time / self.duration);
+ t_ramp * t_ramp * (3.0 - 2.0 * t_ramp)
+ } else {
+ 1.0
+ };
+ let velocity_ramp = if t < self.ramp_time / self.duration {
+ smooth_start
+ } else if t > 1.0 - self.ramp_time / self.duration {
+ let t_down = (1.0 - t) / (self.ramp_time / self.duration);
+ t_down * t_down * (3.0 - 2.0 * t_down)
+ } else {
+ 1.0
+ };
+ let ang_pos = self.frequency * t * 2.0 * PI + self.phase;
+ let lissajous = self.amplitude.component_mul(&ang_pos.map(f32::sin));
+ let position =
+ self.start_position + smooth_start * ((self.center + lissajous) - self.start_position);
+ let mut velocity = Vector3::new(
+ self.amplitude.x * self.frequency.x * 2.0 * PI * ang_pos.x.cos(),
+ self.amplitude.y * self.frequency.y * 2.0 * PI * ang_pos.y.cos(),
+ self.amplitude.z * self.frequency.z * 2.0 * PI * ang_pos.z.cos(),
+ ) * velocity_ramp
+ / self.duration;
+ if t < self.ramp_time / self.duration {
+ let transition_velocity = (self.center - self.start_position)
+ * (2.0 * t / self.ramp_time - 2.0 * t * t / (self.ramp_time * self.ramp_time))
+ / self.duration;
+ velocity += transition_velocity;
+ }
+ let yaw = self.start_yaw + (self.end_yaw - self.start_yaw) * t;
+ (position, velocity, yaw)
+ }
+
+ fn is_finished(
+ &self,
+ _current_position: Vector3<f32>,
+ time: f32,
+ ) -> Result<bool, SimulationError> {
+ Ok(time >= self.start_time + self.duration)
+ }
+}
+/// Planner for circular trajectories
+pub 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,
+ ) -> Result<bool, SimulationError> {
+ Ok(time >= self.start_time + self.duration)
+ }
+}
+
+/// Planner for landing maneuvers
+pub 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,
+ ) -> Result<bool, SimulationError> {
+ Ok(current_position.z < 0.05 || time >= self.start_time + self.duration)
+ }
+}
+/// Manages different trajectory planners and switches between them
+pub struct PlannerManager {
+ current_planner: PlannerType,
+}
+
+impl PlannerManager {
+ /// Creates a new PlannerManager with an initial hover planner
+ /// # Arguments
+ /// * `initial_position` - The initial position for hovering
+ /// * `initial_yaw` - The initial yaw angle for hovering
+ /// # Returns
+ /// A new PlannerManager instance
+ pub fn new(initial_position: Vector3<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
+ pub fn set_planner(&mut self, new_planner: PlannerType) {
+ self.current_planner = new_planner;
+ }
+ /// Updates the current planner and returns the desired position, velocity, and yaw
+ /// # Arguments
+ /// * `current_position` - The current position of the quadrotor
+ /// * `current_orientation` - The current orientation of the quadrotor
+ /// * `current_velocity` - The current velocity of the quadrotor
+ /// * `time` - The current simulation time
+ /// # Returns
+ /// A tuple containing the desired position, velocity, and yaw angle
+ pub fn update(
+ &mut self,
+ current_position: Vector3<f32>,
+ current_orientation: UnitQuaternion<f32>,
+ current_velocity: Vector3<f32>,
+ time: f32,
+ obstacles: &Vec<Obstacle>,
+ ) -> Result<(Vector3<f32>, Vector3<f32>, f32), SimulationError> {
+ if self.current_planner.is_finished(current_position, time)? {
+ log::info!("Time: {:.2} s,\tSwitch Hover", time);
+ self.current_planner = PlannerType::Hover(HoverPlanner {
+ target_position: current_position,
+ target_yaw: current_orientation.euler_angles().2,
+ });
+ }
+ // Update obstacles for ObstacleAvoidancePlanner if needed
+ if let PlannerType::ObstacleAvoidance(ref mut planner) = self.current_planner {
+ planner.obstacles = obstacles.clone();
+ }
+ Ok(self
+ .current_planner
+ .plan(current_position, current_velocity, time))
+ }
+}
+/// Obstacle avoidance planner that uses a potential field approach to avoid obstacles
+/// The planner calculates a repulsive force for each obstacle and an attractive force towards the goal
+/// The resulting force is then used to calculate the desired position and velocity
+pub struct ObstacleAvoidancePlanner {
+ /// Target position of the planner
+ target_position: Vector3<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,
+ /// Vortex force gain
+ k_vortex: f32,
+ /// Influence distance of obstacles
+ d0: f32,
+ /// Influence distance of target
+ d_target: f32,
+ /// Maximum speed of the quadrotor
+ max_speed: 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);
+ let distance_to_target = (self.target_position - current_position).norm();
+ let f_att = self.k_att
+ * self.smooth_attractive_force(distance_to_target)
+ * (self.target_position - current_position).normalize();
+ // Repulsive force from obstacles
+ let mut f_rep = Vector3::zeros();
+ let mut f_vortex = Vector3::zeros();
+ for obstacle in &self.obstacles {
+ let diff = current_position - obstacle.position;
+ let distance = diff.norm();
+ if distance < self.d0 {
+ f_rep += self.k_rep
+ * (1.0 / distance - 1.0 / self.d0)
+ * (1.0 / distance.powi(2))
+ * diff.normalize();
+ f_vortex +=
+ self.k_vortex * current_velocity.cross(&diff).normalize() / distance.powi(2);
+ }
+ }
+ let f_total = f_att + f_rep + f_vortex;
+ let desired_velocity = f_total.normalize() * self.max_speed.min(f_total.norm());
+ let desired_position = current_position + desired_velocity * self.duration * (1.0 - t);
+ let desired_yaw = self.start_yaw + (self.end_yaw - self.start_yaw) * t;
+ (desired_position, desired_velocity, desired_yaw)
+ }
+
+ fn is_finished(
+ &self,
+ current_position: Vector3<f32>,
+ time: f32,
+ ) -> Result<bool, SimulationError> {
+ Ok((current_position - self.target_position).norm() < 0.1
+ && time >= self.start_time + self.duration)
+ }
+}
+
+impl ObstacleAvoidancePlanner {
+ fn smooth_attractive_force(&self, distance: f32) -> f32 {
+ if distance <= self.d_target {
+ distance
+ } else {
+ self.d_target + (distance - self.d_target).tanh()
+ }
+ }
+}
+/// Waypoint planner that generates a minimum snap trajectory between waypoints
+/// # Arguments
+/// * `waypoints` - A list of waypoints to follow
+/// * `yaws` - A list of yaw angles at each waypoint
+/// * `segment_times` - A list of times to reach each waypoint
+/// * `start_time` - The start time of the trajectory
+pub struct MinimumSnapWaypointPlanner {
+ waypoints: Vec<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, SimulationError> {
+ if waypoints.len() < 2 {
+ return Err(SimulationError::OtherError(
+ "At least two waypoints are required".to_string(),
+ ));
+ }
+ if waypoints.len() != segment_times.len() + 1 || waypoints.len() != yaws.len() {
+ return Err(SimulationError::OtherError("Number of segment times must be one less than number of waypoints, and yaws must match waypoints".to_string()));
+ }
+ let mut planner = Self {
+ waypoints,
+ yaws,
+ times: segment_times,
+ coefficients: Vec::new(),
+ yaw_coefficients: Vec::new(),
+ start_time,
+ };
+ planner.compute_minimum_snap_trajectories()?;
+ planner.compute_minimum_acceleration_yaw_trajectories()?;
+ Ok(planner)
+ }
+ /// Compute the coefficients for the minimum snap trajectory, calculated for each segment between waypoints
+ fn compute_minimum_snap_trajectories(&mut self) -> Result<(), SimulationError> {
+ let n = self.waypoints.len() - 1;
+ for i in 0..n {
+ let duration = self.times[i];
+ let (start, end) = (self.waypoints[i], self.waypoints[i + 1]);
+ let mut a = SMatrix::<f32, 8, 8>::zeros();
+ let mut b = SMatrix::<f32, 8, 3>::zeros();
+ a.fixed_view_mut::<4, 4>(0, 0).fill_with_identity();
+ b.fixed_view_mut::<1, 3>(0, 0).copy_from(&start.transpose());
+ b.fixed_view_mut::<1, 3>(4, 0).copy_from(&end.transpose());
+ // End point constraints
+ for j in 0..8 {
+ a[(4, j)] = duration.powi(j as i32);
+ if j > 0 {
+ a[(5, j)] = j as f32 * duration.powi(j as i32 - 1);
+ }
+ if j > 1 {
+ a[(6, j)] = j as f32 * (j - 1) as f32 * duration.powi(j as i32 - 2);
+ }
+ if j > 2 {
+ a[(7, j)] =
+ j as f32 * (j - 1) as f32 * (j - 2) as f32 * duration.powi(j as i32 - 3);
+ }
+ }
+ let coeffs = a.lu().solve(&b).ok_or(SimulationError::NalgebraError(
+ "Failed to solve for coefficients in MinimumSnapWaypointPlanner".to_string(),
+ ))?;
+ self.coefficients.push(
+ (0..8)
+ .map(|j| Vector3::new(coeffs[(j, 0)], coeffs[(j, 1)], coeffs[(j, 2)]))
+ .collect(),
+ );
+ }
+ Ok(())
+ }
+ /// Compute the coefficients for yaw trajectories
+ /// The yaw trajectory is a cubic polynomial and interpolated between waypoints
+ fn compute_minimum_acceleration_yaw_trajectories(&mut self) -> Result<(), SimulationError> {
+ let n = self.yaws.len() - 1; // Number of segments
+ for i in 0..n {
+ let (duration, start_yaw, end_yaw) = (self.times[i], self.yaws[i], self.yaws[i + 1]);
+ let mut a = SMatrix::<f32, 4, 4>::zeros();
+ let mut b = SMatrix::<f32, 4, 1>::zeros();
+ (a[(0, 0)], a[(1, 1)]) = (1.0, 1.0);
+ (b[0], b[2]) = (start_yaw, end_yaw);
+ for j in 0..4 {
+ a[(2, j)] = duration.powi(j as i32);
+ if j > 0 {
+ a[(3, j)] = j as f32 * duration.powi(j as i32 - 1);
+ }
+ }
+ let yaw_coeffs = a.lu().solve(&b).ok_or(SimulationError::NalgebraError(
+ "Failed to solve for yaw coefficients in MinimumSnapWaypointPlanner".to_string(),
+ ))?;
+ self.yaw_coefficients.push(yaw_coeffs.as_slice().to_vec());
+ }
+ Ok(())
+ }
+ /// Evaluate the trajectory at a given time, returns the position, velocity, yaw, and yaw rate at the given time
+ /// # Arguments
+ /// * `t` - The time to evaluate the trajectory at
+ /// * `coeffs` - The coefficients for the position trajectory
+ /// * `yaw_coeffs` - The coefficients for the yaw trajectory
+ /// # Returns
+ /// * `position` - The position at the given time (meters)
+ /// * `velocity` - The velocity at the given time (meters / second)
+ /// * `yaw` - The yaw at the given time (radians)
+ /// * `yaw_rate` - The yaw rate at the given time (radians / second)
+ 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)
+ }
+}
+
+impl Planner for MinimumSnapWaypointPlanner {
+ fn plan(
+ &self,
+ _current_position: Vector3<f32>,
+ _current_velocity: Vector3<f32>,
+ time: f32,
+ ) -> (Vector3<f32>, Vector3<f32>, f32) {
+ let relative_time = time - self.start_time;
+ // Find the current segment
+ let mut segment_start_time = 0.0;
+ let mut current_segment = 0;
+ for (i, &segment_duration) in self.times.iter().enumerate() {
+ if relative_time < segment_start_time + segment_duration {
+ current_segment = i;
+ break;
+ }
+ segment_start_time += segment_duration;
+ }
+ // Evaluate the polynomial for the current segment
+ let segment_time = relative_time - segment_start_time;
+ let (position, velocity, yaw, _yaw_rate) = self.evaluate_polynomial(
+ segment_time,
+ &self.coefficients[current_segment],
+ &self.yaw_coefficients[current_segment],
+ );
+ (position, velocity, yaw)
+ }
+
+ fn is_finished(
+ &self,
+ current_position: Vector3<f32>,
+ time: f32,
+ ) -> Result<bool, SimulationError> {
+ let last_waypoint = self.waypoints.last().ok_or(SimulationError::OtherError(
+ "No waypoints available".to_string(),
+ ))?;
+ Ok(time >= self.start_time + self.times.iter().sum::<f32>()
+ && (current_position - last_waypoint).norm() < 0.1)
+ }
+}
+
+pub struct PlannerStepConfig {
+ pub step: usize,
+ pub planner_type: String,
+ pub params: serde_yaml::Value,
+}
+/// Updates the planner based on the current simulation step and configuration
+/// # Arguments
+/// * `planner_manager` - The PlannerManager instance to update
+/// * `step` - The current simulation step
+/// * `time` - The current simulation time
+/// * `quad` - The Quadrotor instance
+/// * `obstacles` - The current obstacles in the simulation
+/// * `planner_config` - The planner configuration
+pub fn update_planner(
+ planner_manager: &mut PlannerManager,
+ step: usize,
+ time: f32,
+ quad: &Quadrotor,
+ obstacles: &Vec<Obstacle>,
+ planner_config: &Vec<PlannerStepConfig>,
+) -> Result<(), SimulationError> {
+ if let Some(planner_step) = planner_config.iter().find(|s| s.step == step) {
+ log::info!("Time: {:.2} s,\tSwitch {}", time, planner_step.planner_type);
+ planner_manager.set_planner(create_planner(planner_step, quad, time, obstacles)?);
+ }
+ Ok(())
+}
+/// Creates a planner based on the configuration
+/// # Arguments
+/// * `step` - The configuration for the planner step
+/// * `quad` - The Quadrotor instance
+/// * `time` - The current simulation time
+/// * `obstacles` - The current obstacles in the simulation
+/// # Returns
+/// * `PlannerType` - The created planner
+fn create_planner(
+ step: &PlannerStepConfig,
+ quad: &Quadrotor,
+ time: f32,
+ obstacles: &Vec<Obstacle>,
+) -> Result<PlannerType, SimulationError> {
+ let params = &step.params;
+ match step.planner_type.as_str() {
+ "MinimumJerkLine" => Ok(PlannerType::MinimumJerkLine(MinimumJerkLinePlanner {
+ start_position: quad.position,
+ end_position: parse_vector3(params, "end_position")?,
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: parse_f32(params, "end_yaw")?,
+ start_time: time,
+ duration: parse_f32(params, "duration")?,
+ })),
+ "Lissajous" => Ok(PlannerType::Lissajous(LissajousPlanner {
+ start_position: quad.position,
+ center: parse_vector3(params, "center")?,
+ amplitude: parse_vector3(params, "amplitude")?,
+ frequency: parse_vector3(params, "frequency")?,
+ phase: parse_vector3(params, "phase")?,
+ start_time: time,
+ duration: parse_f32(params, "duration")?,
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: parse_f32(params, "end_yaw")?,
+ ramp_time: parse_f32(params, "ramp_time")?,
+ })),
+ "Circle" => Ok(PlannerType::Circle(CirclePlanner {
+ center: parse_vector3(params, "center")?,
+ radius: parse_f32(params, "radius")?,
+ angular_velocity: parse_f32(params, "angular_velocity")?,
+ start_position: quad.position,
+ start_time: time,
+ duration: parse_f32(params, "duration")?,
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: quad.orientation.euler_angles().2,
+ ramp_time: parse_f32(params, "ramp_time")?,
+ })),
+ "ObstacleAvoidance" => Ok(PlannerType::ObstacleAvoidance(ObstacleAvoidancePlanner {
+ target_position: parse_vector3(params, "target_position")?,
+ start_time: time,
+ duration: parse_f32(params, "duration")?,
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: parse_f32(params, "end_yaw")?,
+ obstacles: obstacles.clone(),
+ k_att: parse_f32(params, "k_att")?,
+ k_rep: parse_f32(params, "k_rep")?,
+ k_vortex: parse_f32(params, "k_vortex")?,
+ d0: parse_f32(params, "d0")?,
+ d_target: parse_f32(params, "d_target")?,
+ max_speed: parse_f32(params, "max_speed")?,
+ })),
+ "MinimumSnapWaypoint" => {
+ let mut waypoints = vec![quad.position];
+ waypoints.extend(
+ params["waypoints"]
+ .as_sequence()
+ .ok_or_else(|| SimulationError::OtherError("Invalid waypoints".to_string()))?
+ .iter()
+ .map(|w| {
+ w.as_sequence()
+ .and_then(|coords| {
+ Some(Vector3::new(
+ coords.get(0)?.as_f64()? as f32,
+ coords.get(1)?.as_f64()? as f32,
+ coords.get(2)?.as_f64()? as f32,
+ ))
+ })
+ .ok_or(SimulationError::OtherError("Invalid waypoint".to_string()))
+ })
+ .collect::<Result<Vec<Vector3<f32>>, SimulationError>>()?,
+ );
+ let mut yaws = vec![quad.orientation.euler_angles().2];
+ yaws.extend(
+ params["yaws"]
+ .as_sequence()
+ .ok_or(SimulationError::OtherError("Invalid yaws".to_string()))?
+ .iter()
+ .map(|y| {
+ y.as_f64()
+ .map(|v| v as f32)
+ .ok_or(SimulationError::OtherError("Invalid yaw".to_string()))
+ })
+ .collect::<Result<Vec<f32>, SimulationError>>()?,
+ );
+ let segment_times = params["segment_times"]
+ .as_sequence()
+ .ok_or_else(|| SimulationError::OtherError("Invalid segment_times".to_string()))?
+ .iter()
+ .map(|t| {
+ t.as_f64().map(|v| v as f32).ok_or_else(|| {
+ SimulationError::OtherError("Invalid segment time".to_string())
+ })
+ })
+ .collect::<Result<Vec<f32>, SimulationError>>()?;
+ MinimumSnapWaypointPlanner::new(waypoints, yaws, segment_times, time)
+ .map(PlannerType::MinimumSnapWaypoint)
+ }
+ "Landing" => Ok(PlannerType::Landing(LandingPlanner {
+ start_position: quad.position,
+ start_time: time,
+ duration: parse_f32(params, "duration")?,
+ start_yaw: quad.orientation.euler_angles().2,
+ })),
+ _ => Err(SimulationError::OtherError(format!(
+ "Unknown planner type: {}",
+ step.planner_type
+ ))),
+ }
+}
+// Helper function to parse Vector3 from YAML
+fn parse_vector3(value: &serde_yaml::Value, key: &str) -> Result<Vector3<f32>, SimulationError> {
+ value[key]
+ .as_sequence()
+ .and_then(|seq| {
+ if seq.len() == 3 {
+ Some(Vector3::new(
+ seq[0].as_f64()? as f32,
+ seq[1].as_f64()? as f32,
+ seq[2].as_f64()? as f32,
+ ))
+ } else {
+ None
+ }
+ })
+ .ok_or_else(|| SimulationError::OtherError(format!("Invalid {} vector", key)))
+}
+
+// Helper function to parse f32 from YAML
+fn parse_f32(value: &serde_yaml::Value, key: &str) -> Result<f32, SimulationError> {
+ value[key]
+ .as_f64()
+ .map(|v| v as f32)
+ .ok_or_else(|| SimulationError::OtherError(format!("Invalid {}", key)))
+}
+/// Represents an obstacle in the simulation
+/// # Fields
+/// * `position` - The position of the obstacle
+/// * `velocity` - The velocity of the obstacle
+/// * `radius` - The radius of the obstacle
+#[derive(Clone)]
+pub struct Obstacle {
+ pub position: Vector3<f32>,
+ pub velocity: Vector3<f32>,
+ pub 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
+pub struct Maze {
+ pub lower_bounds: Vector3<f32>,
+ pub upper_bounds: Vector3<f32>,
+ pub 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
+ pub 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
+ pub fn generate_obstacles(&mut self, num_obstacles: usize) {
+ let mut rng = rand::thread_rng();
+ self.obstacles = (0..num_obstacles)
+ .map(|_| {
+ let position = Vector3::new(
+ rand::Rng::gen_range(&mut rng, self.lower_bounds.x..self.upper_bounds.x),
+ rand::Rng::gen_range(&mut rng, self.lower_bounds.y..self.upper_bounds.y),
+ rand::Rng::gen_range(&mut rng, self.lower_bounds.z..self.upper_bounds.z),
+ );
+ let velocity = Vector3::new(
+ rand::Rng::gen_range(&mut rng, -0.2..0.2),
+ rand::Rng::gen_range(&mut rng, -0.2..0.2),
+ rand::Rng::gen_range(&mut rng, -0.1..0.1),
+ );
+ let radius = rand::Rng::gen_range(&mut rng, 0.05..0.1);
+ Obstacle::new(position, velocity, radius)
+ })
+ .collect();
+ }
+ /// Updates the obstacles in the maze, if an obstacle hits a boundary, it bounces off
+ /// # Arguments
+ /// * `dt` - The time step
+ pub fn update_obstacles(&mut self, dt: f32) {
+ self.obstacles.iter_mut().for_each(|obstacle| {
+ obstacle.position += obstacle.velocity * dt;
+ for i in 0..3 {
+ if obstacle.position[i] - obstacle.radius < self.lower_bounds[i]
+ || obstacle.position[i] + obstacle.radius > self.upper_bounds[i]
+ {
+ obstacle.velocity[i] *= -1.0;
+ }
+ }
+ });
+ }
+}
+/// Represents a camera in the simulation which is used to render the depth of the scene
+/// # Fields
+/// * `resolution` - The resolution of the camera
+/// * `fov` - The field of view of the camera
+/// * `near` - The near clipping plane of the camera
+/// * `far` - The far clipping plane of the camera
+/// * `tan_half_fov` - The tangent of half the field of view
+/// * `aspect_ratio` - The aspect ratio of the camera
+pub struct Camera {
+ pub resolution: (usize, usize),
+ pub fov: f32,
+ pub near: f32,
+ pub far: f32,
+ pub aspect_ratio: f32,
+ pub ray_directions: Vec<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
+ pub fn new(resolution: (usize, usize), fov: f32, near: f32, far: f32) -> Self {
+ let (width, height) = resolution;
+ let (aspect_ratio, tan_half_fov) = (width as f32 / height as f32, (fov / 2.0).tan());
+ let mut ray_directions = Vec::with_capacity(width * height);
+ for y in 0..height {
+ for x in 0..width {
+ let x_ndc = (2.0 * x as f32 / width as f32 - 1.0) * aspect_ratio * tan_half_fov;
+ let y_ndc = (1.0 - 2.0 * y as f32 / height as f32) * tan_half_fov;
+ ray_directions.push(Vector3::new(1.0, x_ndc, y_ndc).normalize());
+ }
+ }
+ Self {
+ resolution,
+ fov,
+ near,
+ far,
+ aspect_ratio,
+ ray_directions,
+ }
+ }
+ /// Renders the depth of the scene from the perspective of the quadrotor
+ /// # Arguments
+ /// * `quad_position` - The position of the quadrotor
+ /// * `quad_orientation` - The orientation of the quadrotor
+ /// * `maze` - The maze in the scene
+ /// * `depth_buffer` - The depth buffer to store the depth values
+ pub fn render_depth(
+ &self,
+ quad_position: &Vector3<f32>,
+ quad_orientation: &UnitQuaternion<f32>,
+ maze: &Maze,
+ depth_buffer: &mut Vec<f32>,
+ ) -> Result<(), SimulationError> {
+ let (width, height) = self.resolution;
+ let total_pixels = width * height;
+ depth_buffer.clear();
+ depth_buffer.reserve((total_pixels - depth_buffer.capacity()).max(0));
+ let rotation_camera_to_world = quad_orientation.to_rotation_matrix().matrix()
+ * Matrix3::new(1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0);
+ let rotation_world_to_camera = rotation_camera_to_world.transpose();
+ for i in 0..total_pixels {
+ depth_buffer.push(self.ray_cast(
+ quad_position,
+ &rotation_world_to_camera,
+ &(rotation_camera_to_world * self.ray_directions[i]),
+ maze,
+ )?);
+ }
+ Ok(())
+ }
+ /// Casts a ray from the camera origin in the given direction
+ /// # Arguments
+ /// * `origin` - The origin of the ray
+ /// * `rotation_world_to_camera` - The rotation matrix from world to camera coordinates
+ /// * `direction` - The direction of the ray
+ /// * `maze` - The maze in the scene
+ /// # Returns
+ /// The distance to the closest obstacle hit by the ray
+ pub fn ray_cast(
+ &self,
+ origin: &Vector3<f32>,
+ rotation_world_to_camera: &Matrix3<f32>,
+ direction: &Vector3<f32>,
+ maze: &Maze,
+ ) -> Result<f32, SimulationError> {
+ let mut closest_hit = self.far;
+ // Inline tube intersection
+ for axis in 0..3 {
+ if direction[axis].abs() > f32::EPSILON {
+ for &bound in &[maze.lower_bounds[axis], maze.upper_bounds[axis]] {
+ let t = (bound - origin[axis]) / direction[axis];
+ if t > self.near && t < closest_hit {
+ let intersection_point = origin + direction * t;
+ if (0..3).all(|i| {
+ i == axis
+ || (intersection_point[i] >= maze.lower_bounds[i]
+ && intersection_point[i] <= maze.upper_bounds[i])
+ }) {
+ closest_hit = t;
+ }
+ }
+ }
+ }
+ }
+ // Early exit if we've hit a wall closer than any possible obstacle
+ if closest_hit <= self.near {
+ return Ok(std::f32::INFINITY);
+ }
+ // Inline sphere intersection
+ for obstacle in &maze.obstacles {
+ let oc = origin - &obstacle.position;
+ let b = oc.dot(direction);
+ let c = oc.dot(&oc) - obstacle.radius * obstacle.radius;
+ let discriminant = b * b - c;
+ if discriminant >= 0.0 {
+ let t = -b - discriminant.sqrt();
+ if t > self.near && t < closest_hit {
+ closest_hit = t;
+ }
+ }
+ }
+ if closest_hit < self.far {
+ Ok((rotation_world_to_camera * direction * closest_hit).x)
+ } else {
+ Ok(std::f32::INFINITY)
+ }
+ }
+}
+/// Logs simulation data to the rerun recording stream
+/// # Arguments
+/// * `rec` - The rerun::RecordingStream instance
+/// * `quad` - The Quadrotor instance
+/// * `desired_position` - The desired position vector
+/// * `measured_accel` - The measured acceleration vector
+/// * `measured_gyro` - The measured angular velocity vector
+pub fn log_data(
+ rec: &rerun::RecordingStream,
+ quad: &Quadrotor,
+ desired_position: &Vector3<f32>,
+ desired_velocity: &Vector3<f32>,
+ measured_accel: &Vector3<f32>,
+ measured_gyro: &Vector3<f32>,
+) -> Result<(), SimulationError> {
+ rec.log(
+ "world/quad/desired_position",
+ &rerun::Points3D::new([(desired_position.x, desired_position.y, desired_position.z)])
+ .with_radii([0.1])
+ .with_colors([rerun::Color::from_rgb(255, 255, 255)]),
+ )?;
+ rec.log(
+ "world/quad/base_link",
+ &rerun::Transform3D::from_translation_rotation(
+ rerun::Vec3D::new(quad.position.x, quad.position.y, quad.position.z),
+ rerun::Quaternion::from_xyzw([
+ quad.orientation.i,
+ quad.orientation.j,
+ quad.orientation.k,
+ quad.orientation.w,
+ ]),
+ )
+ .with_axis_length(0.7),
+ )?;
+ let (quad_roll, quad_pitch, quad_yaw) = quad.orientation.euler_angles();
+ let quad_euler_angles: Vector3<f32> = Vector3::new(quad_roll, quad_pitch, quad_yaw);
+ for (pre, vec) in [
+ ("position", &quad.position),
+ ("velocity", &quad.velocity),
+ ("accel", measured_accel),
+ ("orientation", &quad_euler_angles),
+ ("gyro", measured_gyro),
+ ("desired_position", desired_position),
+ ("desired_velocity", desired_velocity),
+ ] {
+ for (i, a) in ["x", "y", "z"].iter().enumerate() {
+ rec.log(format!("{}/{}", pre, a), &rerun::Scalar::new(vec[i] as f64))?;
+ }
+ }
+ Ok(())
+}
+/// Log the maze tube to the rerun recording stream
+/// # Arguments
+/// * `rec` - The rerun::RecordingStream instance
+/// * `maze` - The maze instance
+pub fn log_maze_tube(rec: &rerun::RecordingStream, maze: &Maze) -> Result<(), SimulationError> {
+ let (lower_bounds, upper_bounds) = (maze.lower_bounds, maze.upper_bounds);
+ let center_position = rerun::external::glam::Vec3::new(
+ (lower_bounds.x + upper_bounds.x) / 2.0,
+ (lower_bounds.y + upper_bounds.y) / 2.0,
+ (lower_bounds.z + upper_bounds.z) / 2.0,
+ );
+ let half_sizes = rerun::external::glam::Vec3::new(
+ (upper_bounds.x - lower_bounds.x) / 2.0,
+ (upper_bounds.y - lower_bounds.y) / 2.0,
+ (upper_bounds.z - lower_bounds.z) / 2.0,
+ );
+ rec.log(
+ "world/maze/tube",
+ &rerun::Boxes3D::from_centers_and_half_sizes([center_position], [half_sizes])
+ .with_colors([rerun::Color::from_rgb(128, 128, 255)]),
+ )?;
+ Ok(())
+}
+/// Log the maze obstacles to the rerun recording stream
+/// # Arguments
+/// * `rec` - The rerun::RecordingStream instance
+/// * `maze` - The maze instance
+pub fn log_maze_obstacles(
+ rec: &rerun::RecordingStream,
+ maze: &Maze,
+) -> Result<(), SimulationError> {
+ let (positions, radii): (Vec<(f32, f32, f32)>, Vec<f32>) = maze
+ .obstacles
+ .iter()
+ .map(|obstacle| {
+ let pos = obstacle.position;
+ ((pos.x, pos.y, pos.z), obstacle.radius)
+ })
+ .unzip();
+ rec.log(
+ "world/maze/obstacles",
+ &rerun::Points3D::new(positions)
+ .with_radii(radii)
+ .with_colors([rerun::Color::from_rgb(255, 128, 128)]),
+ )?;
+ Ok(())
+}
+/// A struct to hold trajectory data
+/// # Fields
+/// * `points` - A vector of 3D points
+/// * `last_logged_point` - The last point that was logged
+/// * `min_distance_threadhold` - The minimum distance between points to log
+pub struct Trajectory {
+ points: Vec<Vector3<f32>>,
+ last_logged_point: Vector3<f32>,
+ min_distance_threadhold: f32,
+}
+
+impl Trajectory {
+ pub fn new(initial_point: Vector3<f32>) -> Self {
+ Self {
+ points: vec![initial_point],
+ last_logged_point: initial_point,
+ min_distance_threadhold: 0.05,
+ }
+ }
+ /// Add a point to the trajectory if it is further than the minimum distance threshold
+ /// # Arguments
+ /// * `point` - The point to add
+ /// # Returns
+ /// * `true` if the point was added, `false` otherwise
+ pub fn add_point(&mut self, point: Vector3<f32>) -> bool {
+ if (point - self.last_logged_point).norm() > self.min_distance_threadhold {
+ self.points.push(point);
+ self.last_logged_point = point;
+ true
+ } else {
+ false
+ }
+ }
+}
+/// log trajectory data to the rerun recording stream
+/// # Arguments
+/// * `rec` - The rerun::RecordingStream instance
+/// * `trajectory` - The Trajectory instance
+pub fn log_trajectory(
+ rec: &rerun::RecordingStream,
+ trajectory: &Trajectory,
+) -> Result<(), SimulationError> {
+ let path = trajectory
+ .points
+ .iter()
+ .map(|p| (p.x, p.y, p.z))
+ .collect::<Vec<(f32, f32, f32)>>();
+ rec.log(
+ "world/quad/path",
+ &rerun::LineStrips3D::new([path]).with_colors([rerun::Color::from_rgb(0, 255, 255)]),
+ )?;
+ Ok(())
+}
+/// log mesh data to the rerun recording stream
+/// # Arguments
+/// * `rec` - The rerun::RecordingStream instance
+/// * `division` - The number of divisions in the mesh
+/// * `spacing` - The spacing between divisions
+pub fn log_mesh(
+ rec: &rerun::RecordingStream,
+ division: usize,
+ spacing: f32,
+) -> Result<(), SimulationError> {
+ let grid_size: usize = division + 1;
+ let half_grid_size: f32 = (division as f32 * spacing) / 2.0;
+ let points: Vec<rerun::external::glam::Vec3> = (0..grid_size)
+ .flat_map(|i| {
+ (0..grid_size).map(move |j| {
+ rerun::external::glam::Vec3::new(
+ j as f32 * spacing - half_grid_size,
+ i as f32 * spacing - half_grid_size,
+ 0.0,
+ )
+ })
+ })
+ .collect();
+ let horizontal_lines: Vec<Vec<rerun::external::glam::Vec3>> = (0..grid_size)
+ .map(|i| points[i * grid_size..(i + 1) * grid_size].to_vec())
+ .collect();
+ let vertical_lines: Vec<Vec<rerun::external::glam::Vec3>> = (0..grid_size)
+ .map(|j| (0..grid_size).map(|i| points[i * grid_size + j]).collect())
+ .collect();
+ let line_strips: Vec<Vec<rerun::external::glam::Vec3>> =
+ horizontal_lines.into_iter().chain(vertical_lines).collect();
+ rec.log(
+ "world/mesh",
+ &rerun::LineStrips3D::new(line_strips)
+ .with_colors([rerun::Color::from_rgb(255, 255, 255)])
+ .with_radii([0.02]),
+ )?;
+ Ok(())
+}
+/// log depth image data to the rerun recording stream
+/// # Arguments
+/// * `rec` - The rerun::RecordingStream instance
+/// * `depth_image` - The depth image data
+/// * `width` - The width of the depth image
+/// * `height` - The height of the depth image
+/// * `min_depth` - The minimum depth value
+/// * `max_depth` - The maximum depth value
+pub fn log_depth_image(
+ rec: &rerun::RecordingStream,
+ depth_image: &Vec<f32>,
+ width: usize,
+ height: usize,
+ min_depth: f32,
+ max_depth: f32,
+) -> Result<(), SimulationError> {
+ let mut image = ndarray::Array::zeros((height, width, 3));
+ let depth_range = max_depth - min_depth;
+ image
+ .axis_iter_mut(ndarray::Axis(0))
+ .enumerate()
+ .for_each(|(y, mut row)| {
+ for (x, mut pixel) in row.axis_iter_mut(ndarray::Axis(0)).enumerate() {
+ let depth = depth_image[y * width + x];
+ let color = if depth.is_finite() {
+ let normalized_depth = ((depth - min_depth) / depth_range).clamp(0.0, 1.0);
+ color_map_fn(normalized_depth * 255.0)
+ } else {
+ (0, 0, 0)
+ };
+ (pixel[0], pixel[1], pixel[2]) = color;
+ }
+ });
+ let rerun_image = rerun::Image::from_color_model_and_tensor(rerun::ColorModel::RGB, image)
+ .map_err(|e| SimulationError::OtherError(format!("Failed to create rerun image: {}", e)))?;
+ rec.log("world/quad/cam/depth", &rerun_image)?;
+ Ok(())
+}
+/// turbo color map function
+/// # Arguments
+/// * `gray` - The gray value in the range [0, 255]
+/// # Returns
+/// * The RGB color value in the range [0, 255]
+#[inline]
+pub fn color_map_fn(gray: f32) -> (u8, u8, u8) {
+ let x = gray / 255.0;
+ let r = (34.61
+ + x * (1172.33 - x * (10793.56 - x * (33300.12 - x * (38394.49 - x * 14825.05)))))
+ .clamp(0.0, 255.0) as u8;
+ let g = (23.31 + x * (557.33 + x * (1225.33 - x * (3574.96 - x * (1073.77 + x * 707.56)))))
+ .clamp(0.0, 255.0) as u8;
+ let b = (27.2 + x * (3211.1 - x * (15327.97 - x * (27814.0 - x * (22569.18 - x * 6838.66)))))
+ .clamp(0.0, 255.0) as u8;
+ (r, g, b)
+}
+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+=`