List of all items
Structs
- CirclePlanner
- HoverPlanner
- Imu
- LissajousPlanner
- MinimumJerkLinePlanner
- PIDController
- PlannerManager
- Quadrotor
pub(crate) enum PlannerType {
+ Hover(HoverPlanner),
+ MinimumJerkLine(MinimumJerkLinePlanner),
+ Lissajous(LissajousPlanner),
+ Circle(CirclePlanner),
+}Enum representing different types of trajectory planners
+Hover at current position
+Minimum jerk trajectory along a straight line
+Lissajous curve trajectory
+Circular trajectory
+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 update_planner(
+ planner_manager: &mut PlannerManager,
+ step: usize,
+ time: f32,
+ quad: &Quadrotor,
+)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 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 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) 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).\nChecks if the current trajectory is finished\nChecks if the current trajectory is finished\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\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\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\nStart time of the trajectory\nStart time of the trajectory\nStart time of the trajectory\nStarting yaw angle\nStarting yaw angle\nStarting yaw angle\nTarget position for hovering\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 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 +
//! # 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.01),
+ ki_att: Vector3::new(0.00, 0.00, 0.01),
+ 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),
+}
+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),
+ }
+ }
+ /// 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),
+ }
+ }
+}
+
+/// 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
+ }
+}
+/// 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,
+ ) -> (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,
+ });
+ }
+ self.current_planner
+ .plan(current_position, current_velocity, time)
+ }
+}
+/// 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) {
+ 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.0),
+ start_yaw: quad.orientation.euler_angles().2,
+ end_yaw: 0.0,
+ start_time: time,
+ duration: 3.0,
+ })),
+ _ => {}
+ }
+}
+/// 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)]),
+ )
+ .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,
+ ]),
+ ),
+ )
+ .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();
+ }
+}
+/// 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 mut i = 0;
+ loop {
+ let time = quad.time_step * i as f32;
+ rec.set_time_seconds("timestamp", time);
+ update_planner(&mut planner_manager, i, time, &quad);
+ let (desired_position, desired_velocity, desired_yaw) =
+ planner_manager.update(quad.position, quad.orientation, quad.velocity, time);
+ 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);
+ log_data(
+ &rec,
+ &quad,
+ &desired_position,
+ &_measured_accel,
+ &_measured_gyro,
+ );
+ i += 1;
+ if i >= 7000 {
+ 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+=`