1use std::hint::black_box;
2use std::time::Instant;
3
4use nalgebra::{DMatrix, Vector3};
5
6use crate::drone_3d_trajectory::{
7 compute_control, generate_trajectory_segment_full, generate_waypoint_trajectory_with_durations,
8 sample_trajectory_segments, simulate_desired_states, step_dynamics, ControlOutput,
9 ControllerGains, DesiredState, QuadrotorParams, QuadrotorState, SimulationConfig,
10 SimulationRecord, TrajectorySegment,
11};
12use crate::minimum_snap_trajectory::{
13 generate_minimum_snap_segment_full, generate_waypoint_minimum_snap_trajectory_with_durations,
14 sample_minimum_snap_trajectory, MinimumSnapBoundary, MinimumSnapSegment,
15};
16
17#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
18pub enum DroneTrajectoryVariant {
19 QuinticUniform,
20 QuinticDistanceScaled,
21 MinimumSnapDistanceScaled,
22}
23
24impl DroneTrajectoryVariant {
25 pub fn id(self) -> &'static str {
26 match self {
27 Self::QuinticUniform => "quintic-uniform",
28 Self::QuinticDistanceScaled => "quintic-distance-scaled",
29 Self::MinimumSnapDistanceScaled => "minimum-snap-distance-scaled",
30 }
31 }
32
33 pub fn design_style(self) -> &'static str {
34 match self {
35 Self::QuinticUniform => "fixed-duration-quintic",
36 Self::QuinticDistanceScaled => "distance-scaled-quintic",
37 Self::MinimumSnapDistanceScaled => "distance-scaled-minimum-snap",
38 }
39 }
40}
41
42#[derive(Debug, Clone)]
43pub struct DroneTrajectoryCase {
44 pub family_name: &'static str,
45 pub waypoints: Vec<Vector3<f64>>,
46 pub baseline_segment_duration: f64,
47 pub cruise_speed: f64,
48 pub min_segment_duration: f64,
49 pub dt: f64,
50 pub boundary_mode: DroneBoundaryMode,
51 pub tangent_scale: f64,
52 pub acceleration_scale: f64,
53 pub jerk_scale: f64,
54}
55
56#[derive(Debug, Clone, Copy, PartialEq, Eq)]
57pub enum DroneBoundaryMode {
58 StopGo,
59 PassThrough,
60 PassThroughAccel,
61 PassThroughAccelJerk,
62 CoupledContinuity,
63}
64
65#[derive(Debug, Clone)]
66pub struct DroneTrajectoryMetrics {
67 pub family_name: &'static str,
68 pub variant: DroneTrajectoryVariant,
69 pub generation_us: f64,
70 pub total_duration_s: f64,
71 pub tracking_rmse_m: f64,
72 pub max_position_error_m: f64,
73 pub jerk_rms: f64,
74 pub snap_rms: f64,
75 pub max_speed_mps: f64,
76 pub max_acceleration_mps2: f64,
77}
78
79#[derive(Debug, Clone)]
80pub struct DroneTrajectorySummary {
81 pub variant: DroneTrajectoryVariant,
82 pub mean_generation_us: f64,
83 pub mean_total_duration_s: f64,
84 pub mean_tracking_rmse_m: f64,
85 pub mean_max_position_error_m: f64,
86 pub mean_jerk_rms: f64,
87 pub mean_snap_rms: f64,
88 pub mean_max_speed_mps: f64,
89 pub mean_max_acceleration_mps2: f64,
90}
91
92#[derive(Debug, Clone, Copy, PartialEq)]
94pub struct LateralGainSet {
95 pub lateral_position_gain: f64,
96 pub lateral_velocity_gain: f64,
97 pub max_lateral_correction: f64,
98 pub max_attitude_command: f64,
99 pub rate_damping: Vector3<f64>,
100}
101
102impl Default for LateralGainSet {
103 fn default() -> Self {
104 Self {
105 lateral_position_gain: 0.15,
106 lateral_velocity_gain: 0.45,
107 max_lateral_correction: 0.75,
108 max_attitude_command: 0.30,
109 rate_damping: Vector3::new(6.0, 6.0, 4.0),
110 }
111 }
112}
113
114#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
115pub enum DroneControllerVariant {
116 BaselinePd,
117 AttitudeRateDampedPd,
118 AttitudeRateDampedLateralPd,
119}
120
121impl DroneControllerVariant {
122 pub fn id(self) -> &'static str {
123 match self {
124 Self::BaselinePd => "baseline-pd",
125 Self::AttitudeRateDampedPd => "attitude-rate-damped-pd",
126 Self::AttitudeRateDampedLateralPd => "attitude-rate-damped-lateral-pd",
127 }
128 }
129
130 pub fn design_style(self) -> &'static str {
131 match self {
132 Self::BaselinePd => "z-pd-plus-attitude-feedforward",
133 Self::AttitudeRateDampedPd => "baseline-pd-plus-attitude-rate-damping",
134 Self::AttitudeRateDampedLateralPd => {
135 "baseline-pd-plus-bounded-lateral-feedback-and-rate-damping"
136 }
137 }
138 }
139}
140
141#[derive(Debug, Clone)]
142pub struct DroneControllerMetrics {
143 pub family_name: &'static str,
144 pub trajectory_variant: DroneTrajectoryVariant,
145 pub controller_variant: DroneControllerVariant,
146 pub generation_us: f64,
147 pub total_duration_s: f64,
148 pub tracking_rmse_m: f64,
149 pub max_position_error_m: f64,
150 pub mean_thrust_n: f64,
151 pub max_thrust_n: f64,
152 pub mean_torque_norm_nm: f64,
153}
154
155#[derive(Debug, Clone)]
156pub struct DroneControllerSummary {
157 pub controller_variant: DroneControllerVariant,
158 pub mean_generation_us: f64,
159 pub mean_total_duration_s: f64,
160 pub mean_tracking_rmse_m: f64,
161 pub mean_max_position_error_m: f64,
162 pub mean_thrust_n: f64,
163 pub mean_max_thrust_n: f64,
164 pub mean_torque_norm_nm: f64,
165}
166
167#[derive(Debug, Clone)]
168struct BuiltTrajectory {
169 desired_states: Vec<DesiredState>,
170 jerk_norms: Vec<f64>,
171 snap_norms: Vec<f64>,
172 total_duration_s: f64,
173}
174
175pub fn default_drone_trajectory_cases() -> Vec<DroneTrajectoryCase> {
176 vec![
177 DroneTrajectoryCase {
178 family_name: "box-climb",
179 waypoints: vec![
180 Vector3::new(-4.0, -4.0, 1.5),
181 Vector3::new(4.0, -4.0, 2.5),
182 Vector3::new(4.0, 4.0, 3.5),
183 Vector3::new(-4.0, 4.0, 2.0),
184 ],
185 baseline_segment_duration: 4.0,
186 cruise_speed: 2.2,
187 min_segment_duration: 2.5,
188 dt: 0.05,
189 boundary_mode: DroneBoundaryMode::StopGo,
190 tangent_scale: 0.0,
191 acceleration_scale: 0.0,
192 jerk_scale: 0.0,
193 },
194 DroneTrajectoryCase {
195 family_name: "diamond-ascent",
196 waypoints: vec![
197 Vector3::new(0.0, -5.5, 1.0),
198 Vector3::new(5.0, 0.0, 2.8),
199 Vector3::new(0.0, 5.5, 4.2),
200 Vector3::new(-5.0, 0.0, 2.0),
201 ],
202 baseline_segment_duration: 3.6,
203 cruise_speed: 2.4,
204 min_segment_duration: 2.2,
205 dt: 0.05,
206 boundary_mode: DroneBoundaryMode::StopGo,
207 tangent_scale: 0.0,
208 acceleration_scale: 0.0,
209 jerk_scale: 0.0,
210 },
211 DroneTrajectoryCase {
212 family_name: "stair-zigzag",
213 waypoints: vec![
214 Vector3::new(-6.0, -2.0, 1.0),
215 Vector3::new(-2.0, 2.0, 2.0),
216 Vector3::new(2.0, -2.0, 3.0),
217 Vector3::new(6.0, 2.0, 4.0),
218 ],
219 baseline_segment_duration: 3.8,
220 cruise_speed: 2.5,
221 min_segment_duration: 2.4,
222 dt: 0.05,
223 boundary_mode: DroneBoundaryMode::StopGo,
224 tangent_scale: 0.0,
225 acceleration_scale: 0.0,
226 jerk_scale: 0.0,
227 },
228 ]
229}
230
231pub fn pass_through_drone_trajectory_cases() -> Vec<DroneTrajectoryCase> {
232 vec![
233 DroneTrajectoryCase {
234 family_name: "oval-cruise",
235 waypoints: vec![
236 Vector3::new(-6.0, 0.0, 1.5),
237 Vector3::new(-2.0, 5.0, 2.2),
238 Vector3::new(4.0, 5.5, 3.0),
239 Vector3::new(7.0, 0.5, 3.4),
240 Vector3::new(3.0, -5.0, 2.7),
241 Vector3::new(-3.0, -5.5, 1.8),
242 ],
243 baseline_segment_duration: 2.9,
244 cruise_speed: 3.0,
245 min_segment_duration: 1.8,
246 dt: 0.05,
247 boundary_mode: DroneBoundaryMode::PassThrough,
248 tangent_scale: 0.85,
249 acceleration_scale: 0.0,
250 jerk_scale: 0.0,
251 },
252 DroneTrajectoryCase {
253 family_name: "figure-eight-climb",
254 waypoints: vec![
255 Vector3::new(-5.5, 0.0, 1.0),
256 Vector3::new(-1.5, 4.5, 2.0),
257 Vector3::new(2.0, 0.0, 3.0),
258 Vector3::new(5.5, -4.5, 4.0),
259 Vector3::new(1.5, 0.0, 3.0),
260 Vector3::new(-2.0, 4.5, 2.0),
261 ],
262 baseline_segment_duration: 2.7,
263 cruise_speed: 3.1,
264 min_segment_duration: 1.7,
265 dt: 0.05,
266 boundary_mode: DroneBoundaryMode::PassThrough,
267 tangent_scale: 0.9,
268 acceleration_scale: 0.0,
269 jerk_scale: 0.0,
270 },
271 DroneTrajectoryCase {
272 family_name: "banked-diamond",
273 waypoints: vec![
274 Vector3::new(0.0, -6.0, 1.2),
275 Vector3::new(6.0, 0.0, 2.4),
276 Vector3::new(0.0, 6.0, 3.6),
277 Vector3::new(-6.0, 0.0, 2.4),
278 ],
279 baseline_segment_duration: 3.2,
280 cruise_speed: 3.0,
281 min_segment_duration: 2.0,
282 dt: 0.05,
283 boundary_mode: DroneBoundaryMode::PassThrough,
284 tangent_scale: 0.82,
285 acceleration_scale: 0.0,
286 jerk_scale: 0.0,
287 },
288 ]
289}
290
291pub fn pass_through_accel_drone_trajectory_cases() -> Vec<DroneTrajectoryCase> {
292 vec![
293 DroneTrajectoryCase {
294 family_name: "oval-cruise-accel",
295 waypoints: vec![
296 Vector3::new(-6.0, 0.0, 1.5),
297 Vector3::new(-2.0, 5.0, 2.2),
298 Vector3::new(4.0, 5.5, 3.0),
299 Vector3::new(7.0, 0.5, 3.4),
300 Vector3::new(3.0, -5.0, 2.7),
301 Vector3::new(-3.0, -5.5, 1.8),
302 ],
303 baseline_segment_duration: 2.9,
304 cruise_speed: 3.0,
305 min_segment_duration: 1.8,
306 dt: 0.05,
307 boundary_mode: DroneBoundaryMode::PassThroughAccel,
308 tangent_scale: 0.85,
309 acceleration_scale: 0.65,
310 jerk_scale: 0.0,
311 },
312 DroneTrajectoryCase {
313 family_name: "figure-eight-climb-accel",
314 waypoints: vec![
315 Vector3::new(-5.5, 0.0, 1.0),
316 Vector3::new(-1.5, 4.5, 2.0),
317 Vector3::new(2.0, 0.0, 3.0),
318 Vector3::new(5.5, -4.5, 4.0),
319 Vector3::new(1.5, 0.0, 3.0),
320 Vector3::new(-2.0, 4.5, 2.0),
321 ],
322 baseline_segment_duration: 2.7,
323 cruise_speed: 3.1,
324 min_segment_duration: 1.7,
325 dt: 0.05,
326 boundary_mode: DroneBoundaryMode::PassThroughAccel,
327 tangent_scale: 0.9,
328 acceleration_scale: 0.70,
329 jerk_scale: 0.0,
330 },
331 DroneTrajectoryCase {
332 family_name: "banked-diamond-accel",
333 waypoints: vec![
334 Vector3::new(0.0, -6.0, 1.2),
335 Vector3::new(6.0, 0.0, 2.4),
336 Vector3::new(0.0, 6.0, 3.6),
337 Vector3::new(-6.0, 0.0, 2.4),
338 ],
339 baseline_segment_duration: 3.2,
340 cruise_speed: 3.0,
341 min_segment_duration: 2.0,
342 dt: 0.05,
343 boundary_mode: DroneBoundaryMode::PassThroughAccel,
344 tangent_scale: 0.82,
345 acceleration_scale: 0.60,
346 jerk_scale: 0.0,
347 },
348 ]
349}
350
351pub fn pass_through_accel_jerk_drone_trajectory_cases() -> Vec<DroneTrajectoryCase> {
352 vec![
353 DroneTrajectoryCase {
354 family_name: "oval-cruise-accel-jerk",
355 waypoints: vec![
356 Vector3::new(-6.0, 0.0, 1.5),
357 Vector3::new(-2.0, 5.0, 2.2),
358 Vector3::new(4.0, 5.5, 3.0),
359 Vector3::new(7.0, 0.5, 3.4),
360 Vector3::new(3.0, -5.0, 2.7),
361 Vector3::new(-3.0, -5.5, 1.8),
362 ],
363 baseline_segment_duration: 2.9,
364 cruise_speed: 3.0,
365 min_segment_duration: 1.8,
366 dt: 0.05,
367 boundary_mode: DroneBoundaryMode::PassThroughAccelJerk,
368 tangent_scale: 0.85,
369 acceleration_scale: 0.65,
370 jerk_scale: 0.45,
371 },
372 DroneTrajectoryCase {
373 family_name: "figure-eight-climb-accel-jerk",
374 waypoints: vec![
375 Vector3::new(-5.5, 0.0, 1.0),
376 Vector3::new(-1.5, 4.5, 2.0),
377 Vector3::new(2.0, 0.0, 3.0),
378 Vector3::new(5.5, -4.5, 4.0),
379 Vector3::new(1.5, 0.0, 3.0),
380 Vector3::new(-2.0, 4.5, 2.0),
381 ],
382 baseline_segment_duration: 2.7,
383 cruise_speed: 3.1,
384 min_segment_duration: 1.7,
385 dt: 0.05,
386 boundary_mode: DroneBoundaryMode::PassThroughAccelJerk,
387 tangent_scale: 0.9,
388 acceleration_scale: 0.70,
389 jerk_scale: 0.50,
390 },
391 DroneTrajectoryCase {
392 family_name: "banked-diamond-accel-jerk",
393 waypoints: vec![
394 Vector3::new(0.0, -6.0, 1.2),
395 Vector3::new(6.0, 0.0, 2.4),
396 Vector3::new(0.0, 6.0, 3.6),
397 Vector3::new(-6.0, 0.0, 2.4),
398 ],
399 baseline_segment_duration: 3.2,
400 cruise_speed: 3.0,
401 min_segment_duration: 2.0,
402 dt: 0.05,
403 boundary_mode: DroneBoundaryMode::PassThroughAccelJerk,
404 tangent_scale: 0.82,
405 acceleration_scale: 0.60,
406 jerk_scale: 0.40,
407 },
408 ]
409}
410
411pub fn coupled_continuity_drone_trajectory_cases() -> Vec<DroneTrajectoryCase> {
412 vec![
413 DroneTrajectoryCase {
414 family_name: "oval-cruise-coupled",
415 waypoints: vec![
416 Vector3::new(-6.0, 0.0, 1.5),
417 Vector3::new(-2.0, 5.0, 2.2),
418 Vector3::new(4.0, 5.5, 3.0),
419 Vector3::new(7.0, 0.5, 3.4),
420 Vector3::new(3.0, -5.0, 2.7),
421 Vector3::new(-3.0, -5.5, 1.8),
422 ],
423 baseline_segment_duration: 2.9,
424 cruise_speed: 3.0,
425 min_segment_duration: 1.8,
426 dt: 0.05,
427 boundary_mode: DroneBoundaryMode::CoupledContinuity,
428 tangent_scale: 1.0,
429 acceleration_scale: 1.0,
430 jerk_scale: 1.0,
431 },
432 DroneTrajectoryCase {
433 family_name: "figure-eight-climb-coupled",
434 waypoints: vec![
435 Vector3::new(-5.5, 0.0, 1.0),
436 Vector3::new(-1.5, 4.5, 2.0),
437 Vector3::new(2.0, 0.0, 3.0),
438 Vector3::new(5.5, -4.5, 4.0),
439 Vector3::new(1.5, 0.0, 3.0),
440 Vector3::new(-2.0, 4.5, 2.0),
441 ],
442 baseline_segment_duration: 2.7,
443 cruise_speed: 3.1,
444 min_segment_duration: 1.7,
445 dt: 0.05,
446 boundary_mode: DroneBoundaryMode::CoupledContinuity,
447 tangent_scale: 1.0,
448 acceleration_scale: 1.0,
449 jerk_scale: 1.0,
450 },
451 DroneTrajectoryCase {
452 family_name: "banked-diamond-coupled",
453 waypoints: vec![
454 Vector3::new(0.0, -6.0, 1.2),
455 Vector3::new(6.0, 0.0, 2.4),
456 Vector3::new(0.0, 6.0, 3.6),
457 Vector3::new(-6.0, 0.0, 2.4),
458 ],
459 baseline_segment_duration: 3.2,
460 cruise_speed: 3.0,
461 min_segment_duration: 2.0,
462 dt: 0.05,
463 boundary_mode: DroneBoundaryMode::CoupledContinuity,
464 tangent_scale: 1.0,
465 acceleration_scale: 1.0,
466 jerk_scale: 1.0,
467 },
468 DroneTrajectoryCase {
469 family_name: "spiral-climb-coupled",
470 waypoints: vec![
471 Vector3::new(4.0, 0.0, 1.0),
472 Vector3::new(0.0, 4.0, 2.0),
473 Vector3::new(-4.0, 0.0, 3.0),
474 Vector3::new(0.0, -4.0, 4.0),
475 Vector3::new(4.0, 0.0, 5.0),
476 Vector3::new(0.0, 4.0, 6.0),
477 ],
478 baseline_segment_duration: 2.5,
479 cruise_speed: 3.2,
480 min_segment_duration: 1.6,
481 dt: 0.05,
482 boundary_mode: DroneBoundaryMode::CoupledContinuity,
483 tangent_scale: 1.0,
484 acceleration_scale: 1.0,
485 jerk_scale: 1.0,
486 },
487 DroneTrajectoryCase {
488 family_name: "reverse-s-coupled",
489 waypoints: vec![
490 Vector3::new(-6.0, -3.0, 1.5),
491 Vector3::new(-2.0, 3.0, 2.5),
492 Vector3::new(2.0, -3.0, 3.5),
493 Vector3::new(6.0, 3.0, 2.5),
494 ],
495 baseline_segment_duration: 3.0,
496 cruise_speed: 2.8,
497 min_segment_duration: 1.9,
498 dt: 0.05,
499 boundary_mode: DroneBoundaryMode::CoupledContinuity,
500 tangent_scale: 1.0,
501 acceleration_scale: 1.0,
502 jerk_scale: 1.0,
503 },
504 ]
505}
506
507pub fn default_drone_trajectory_variants() -> [DroneTrajectoryVariant; 3] {
508 [
509 DroneTrajectoryVariant::QuinticUniform,
510 DroneTrajectoryVariant::QuinticDistanceScaled,
511 DroneTrajectoryVariant::MinimumSnapDistanceScaled,
512 ]
513}
514
515pub fn default_drone_controller_variants() -> [DroneControllerVariant; 3] {
516 [
517 DroneControllerVariant::BaselinePd,
518 DroneControllerVariant::AttitudeRateDampedPd,
519 DroneControllerVariant::AttitudeRateDampedLateralPd,
520 ]
521}
522
523pub fn evaluate_drone_trajectory_variants(
524 cases: &[DroneTrajectoryCase],
525 variants: &[DroneTrajectoryVariant],
526) -> (Vec<DroneTrajectoryMetrics>, Vec<DroneTrajectorySummary>) {
527 let metrics: Vec<_> = cases
528 .iter()
529 .flat_map(|case| {
530 variants
531 .iter()
532 .copied()
533 .map(move |variant| evaluate_case_variant(case, variant))
534 })
535 .collect();
536
537 let summaries = variants
538 .iter()
539 .copied()
540 .map(|variant| summarize_variant(&metrics, variant))
541 .collect();
542
543 (metrics, summaries)
544}
545
546pub fn evaluate_drone_controller_variants(
547 cases: &[DroneTrajectoryCase],
548 trajectory_variants: &[DroneTrajectoryVariant],
549 controller_variants: &[DroneControllerVariant],
550) -> (Vec<DroneControllerMetrics>, Vec<DroneControllerSummary>) {
551 let metrics: Vec<_> = cases
552 .iter()
553 .flat_map(|case| {
554 trajectory_variants
555 .iter()
556 .copied()
557 .flat_map(move |trajectory_variant| {
558 controller_variants
559 .iter()
560 .copied()
561 .map(move |controller_variant| {
562 evaluate_controller_case_variant(
563 case,
564 trajectory_variant,
565 controller_variant,
566 )
567 })
568 })
569 })
570 .collect();
571
572 let summaries = controller_variants
573 .iter()
574 .copied()
575 .map(|controller_variant| summarize_controller_variant(&metrics, controller_variant))
576 .collect();
577
578 (metrics, summaries)
579}
580
581fn evaluate_case_variant(
582 case: &DroneTrajectoryCase,
583 variant: DroneTrajectoryVariant,
584) -> DroneTrajectoryMetrics {
585 let built = build_variant_trajectory(case, variant);
586 let simulation_config = SimulationConfig {
587 dt: case.dt,
588 segment_duration: case.baseline_segment_duration,
589 n_loops: 1,
590 ..Default::default()
591 };
592 let record =
593 simulate_desired_states(case.waypoints[0], &built.desired_states, &simulation_config);
594 let actual_positions = &record.positions[1..];
595
596 let mut squared_error_sum = 0.0;
597 let mut max_position_error_m = 0.0_f64;
598 for (actual, desired) in actual_positions.iter().zip(&built.desired_states) {
599 let error = (actual - desired.position).norm();
600 squared_error_sum += error * error;
601 max_position_error_m = max_position_error_m.max(error);
602 }
603
604 let tracking_rmse_m = (squared_error_sum / built.desired_states.len() as f64).sqrt();
605 let jerk_rms = rms(&built.jerk_norms);
606 let snap_rms = rms(&built.snap_norms);
607 let max_speed_mps = built
608 .desired_states
609 .iter()
610 .map(|state| state.velocity.norm())
611 .fold(0.0, f64::max);
612 let max_acceleration_mps2 = built
613 .desired_states
614 .iter()
615 .map(|state| state.acceleration.norm())
616 .fold(0.0, f64::max);
617
618 DroneTrajectoryMetrics {
619 family_name: case.family_name,
620 variant,
621 generation_us: benchmark_generation(case, variant, 128),
622 total_duration_s: built.total_duration_s,
623 tracking_rmse_m,
624 max_position_error_m,
625 jerk_rms,
626 snap_rms,
627 max_speed_mps,
628 max_acceleration_mps2,
629 }
630}
631
632fn summarize_variant(
633 metrics: &[DroneTrajectoryMetrics],
634 variant: DroneTrajectoryVariant,
635) -> DroneTrajectorySummary {
636 let rows: Vec<_> = metrics
637 .iter()
638 .filter(|row| row.variant == variant)
639 .collect();
640 let denom = rows.len() as f64;
641
642 DroneTrajectorySummary {
643 variant,
644 mean_generation_us: rows.iter().map(|row| row.generation_us).sum::<f64>() / denom,
645 mean_total_duration_s: rows.iter().map(|row| row.total_duration_s).sum::<f64>() / denom,
646 mean_tracking_rmse_m: rows.iter().map(|row| row.tracking_rmse_m).sum::<f64>() / denom,
647 mean_max_position_error_m: rows.iter().map(|row| row.max_position_error_m).sum::<f64>()
648 / denom,
649 mean_jerk_rms: rows.iter().map(|row| row.jerk_rms).sum::<f64>() / denom,
650 mean_snap_rms: rows.iter().map(|row| row.snap_rms).sum::<f64>() / denom,
651 mean_max_speed_mps: rows.iter().map(|row| row.max_speed_mps).sum::<f64>() / denom,
652 mean_max_acceleration_mps2: rows
653 .iter()
654 .map(|row| row.max_acceleration_mps2)
655 .sum::<f64>()
656 / denom,
657 }
658}
659
660fn evaluate_controller_case_variant(
661 case: &DroneTrajectoryCase,
662 trajectory_variant: DroneTrajectoryVariant,
663 controller_variant: DroneControllerVariant,
664) -> DroneControllerMetrics {
665 let built = build_variant_trajectory(case, trajectory_variant);
666 let simulation_config = SimulationConfig {
667 dt: case.dt,
668 segment_duration: case.baseline_segment_duration,
669 n_loops: 1,
670 ..Default::default()
671 };
672 let record = simulate_desired_states_with_controller_variant(
673 case.waypoints[0],
674 &built.desired_states,
675 &simulation_config,
676 controller_variant,
677 );
678 let actual_positions = &record.positions[1..];
679
680 let mut squared_error_sum = 0.0;
681 let mut max_position_error_m = 0.0_f64;
682 for (actual, desired) in actual_positions.iter().zip(&built.desired_states) {
683 let error = (actual - desired.position).norm();
684 squared_error_sum += error * error;
685 max_position_error_m = max_position_error_m.max(error);
686 }
687
688 let tracking_rmse_m = (squared_error_sum / built.desired_states.len() as f64).sqrt();
689 let mean_thrust_n = record
690 .controls
691 .iter()
692 .map(|control| control.thrust)
693 .sum::<f64>()
694 / record.controls.len() as f64;
695 let max_thrust_n = record
696 .controls
697 .iter()
698 .map(|control| control.thrust)
699 .fold(f64::NEG_INFINITY, f64::max);
700 let mean_torque_norm_nm = record
701 .controls
702 .iter()
703 .map(|control| {
704 Vector3::new(
705 control.roll_torque,
706 control.pitch_torque,
707 control.yaw_torque,
708 )
709 .norm()
710 })
711 .sum::<f64>()
712 / record.controls.len() as f64;
713
714 DroneControllerMetrics {
715 family_name: case.family_name,
716 trajectory_variant,
717 controller_variant,
718 generation_us: benchmark_generation(case, trajectory_variant, 128),
719 total_duration_s: built.total_duration_s,
720 tracking_rmse_m,
721 max_position_error_m,
722 mean_thrust_n,
723 max_thrust_n,
724 mean_torque_norm_nm,
725 }
726}
727
728fn summarize_controller_variant(
729 metrics: &[DroneControllerMetrics],
730 controller_variant: DroneControllerVariant,
731) -> DroneControllerSummary {
732 let rows: Vec<_> = metrics
733 .iter()
734 .filter(|row| row.controller_variant == controller_variant)
735 .collect();
736 let denom = rows.len() as f64;
737
738 DroneControllerSummary {
739 controller_variant,
740 mean_generation_us: rows.iter().map(|row| row.generation_us).sum::<f64>() / denom,
741 mean_total_duration_s: rows.iter().map(|row| row.total_duration_s).sum::<f64>() / denom,
742 mean_tracking_rmse_m: rows.iter().map(|row| row.tracking_rmse_m).sum::<f64>() / denom,
743 mean_max_position_error_m: rows.iter().map(|row| row.max_position_error_m).sum::<f64>()
744 / denom,
745 mean_thrust_n: rows.iter().map(|row| row.mean_thrust_n).sum::<f64>() / denom,
746 mean_max_thrust_n: rows.iter().map(|row| row.max_thrust_n).sum::<f64>() / denom,
747 mean_torque_norm_nm: rows.iter().map(|row| row.mean_torque_norm_nm).sum::<f64>() / denom,
748 }
749}
750
751fn benchmark_generation(
752 case: &DroneTrajectoryCase,
753 variant: DroneTrajectoryVariant,
754 iterations: usize,
755) -> f64 {
756 let start = Instant::now();
757 for _ in 0..iterations {
758 black_box(build_variant_trajectory(case, variant));
759 }
760 start.elapsed().as_secs_f64() * 1_000_000.0 / iterations as f64
761}
762
763fn simulate_desired_states_with_controller_variant(
764 start_position: Vector3<f64>,
765 desired_states: &[DesiredState],
766 config: &SimulationConfig,
767 controller_variant: DroneControllerVariant,
768) -> SimulationRecord {
769 let mut state = QuadrotorState::new(start_position);
770 let mut record = SimulationRecord {
771 positions: vec![state.position],
772 orientations: vec![state.orientation],
773 controls: Vec::with_capacity(desired_states.len()),
774 };
775
776 for desired in desired_states {
777 let control = compute_controller_variant(
778 &state,
779 desired,
780 &config.params,
781 &config.gains,
782 controller_variant,
783 );
784 step_dynamics(&mut state, &control, &config.params, config.dt);
785 record.positions.push(state.position);
786 record.orientations.push(state.orientation);
787 record.controls.push(control);
788 }
789
790 record
791}
792
793fn compute_controller_variant(
794 state: &QuadrotorState,
795 desired: &DesiredState,
796 params: &QuadrotorParams,
797 gains: &ControllerGains,
798 controller_variant: DroneControllerVariant,
799) -> ControlOutput {
800 match controller_variant {
801 DroneControllerVariant::BaselinePd => compute_control(state, desired, params, gains),
802 DroneControllerVariant::AttitudeRateDampedPd => {
803 compute_attitude_rate_damped_control(state, desired, params, gains)
804 }
805 DroneControllerVariant::AttitudeRateDampedLateralPd => {
806 compute_attitude_rate_damped_lateral_control(state, desired, params, gains)
807 }
808 }
809}
810
811fn compute_attitude_rate_damped_control(
812 state: &QuadrotorState,
813 desired: &DesiredState,
814 params: &QuadrotorParams,
815 gains: &ControllerGains,
816) -> ControlOutput {
817 let mut control = compute_control(state, desired, params, gains);
818 let rate_damping = Vector3::new(6.5, 6.5, 4.0);
819 control.roll_torque -= rate_damping.x * state.angular_velocity.x;
820 control.pitch_torque -= rate_damping.y * state.angular_velocity.y;
821 control.yaw_torque -= rate_damping.z * state.angular_velocity.z;
822 control
823}
824
825fn compute_attitude_rate_damped_lateral_control(
826 state: &QuadrotorState,
827 desired: &DesiredState,
828 params: &QuadrotorParams,
829 gains: &ControllerGains,
830) -> ControlOutput {
831 compute_attitude_rate_damped_lateral_control_with_gains(
832 state,
833 desired,
834 params,
835 gains,
836 &LateralGainSet::default(),
837 )
838}
839
840pub fn compute_attitude_rate_damped_lateral_control_with_gains(
841 state: &QuadrotorState,
842 desired: &DesiredState,
843 params: &QuadrotorParams,
844 gains: &ControllerGains,
845 lateral: &LateralGainSet,
846) -> ControlOutput {
847 let g = params.gravity;
848 let m = params.mass;
849 let des_yaw = desired.yaw;
850 let (sy, cy) = des_yaw.sin_cos();
851
852 let position_error = desired.position - state.position;
853 let velocity_error = desired.velocity - state.velocity;
854 let lateral_feedback = Vector3::new(
855 (lateral.lateral_position_gain * position_error.x
856 + lateral.lateral_velocity_gain * velocity_error.x)
857 .clamp(
858 -lateral.max_lateral_correction,
859 lateral.max_lateral_correction,
860 ),
861 (lateral.lateral_position_gain * position_error.y
862 + lateral.lateral_velocity_gain * velocity_error.y)
863 .clamp(
864 -lateral.max_lateral_correction,
865 lateral.max_lateral_correction,
866 ),
867 0.0,
868 );
869 let lateral_acceleration = desired.acceleration + lateral_feedback;
870
871 let thrust = m
872 * (g + desired.acceleration.z
873 + gains.kp_pos.z * position_error.z
874 + gains.kd_pos.z * velocity_error.z);
875 let des_roll = ((lateral_acceleration.x * sy - lateral_acceleration.y * cy) / g)
876 .clamp(-lateral.max_attitude_command, lateral.max_attitude_command);
877 let des_pitch = ((lateral_acceleration.x * cy - lateral_acceleration.y * sy) / g)
878 .clamp(-lateral.max_attitude_command, lateral.max_attitude_command);
879
880 ControlOutput {
881 thrust,
882 roll_torque: gains.kp_att.x * (des_roll - state.orientation.x)
883 - lateral.rate_damping.x * state.angular_velocity.x,
884 pitch_torque: gains.kp_att.y * (des_pitch - state.orientation.y)
885 - lateral.rate_damping.y * state.angular_velocity.y,
886 yaw_torque: gains.kp_att.z * (des_yaw - state.orientation.z)
887 - lateral.rate_damping.z * state.angular_velocity.z,
888 }
889}
890
891fn build_variant_trajectory(
892 case: &DroneTrajectoryCase,
893 variant: DroneTrajectoryVariant,
894) -> BuiltTrajectory {
895 let segment_durations = match variant {
896 DroneTrajectoryVariant::QuinticUniform => {
897 vec![case.baseline_segment_duration; case.waypoints.len()]
898 }
899 DroneTrajectoryVariant::QuinticDistanceScaled
900 | DroneTrajectoryVariant::MinimumSnapDistanceScaled => distance_scaled_durations(case),
901 };
902
903 match variant {
904 DroneTrajectoryVariant::QuinticUniform | DroneTrajectoryVariant::QuinticDistanceScaled => {
905 build_quintic_variant(case, &segment_durations)
906 }
907 DroneTrajectoryVariant::MinimumSnapDistanceScaled => {
908 build_minimum_snap_variant(case, &segment_durations)
909 }
910 }
911}
912
913fn build_quintic_variant(case: &DroneTrajectoryCase, segment_durations: &[f64]) -> BuiltTrajectory {
914 match case.boundary_mode {
915 DroneBoundaryMode::StopGo => {
916 let segments =
917 generate_waypoint_trajectory_with_durations(&case.waypoints, segment_durations);
918 build_quintic_bundle(&segments, segment_durations, case.dt)
919 }
920 DroneBoundaryMode::PassThrough
921 | DroneBoundaryMode::PassThroughAccel
922 | DroneBoundaryMode::PassThroughAccelJerk
923 | DroneBoundaryMode::CoupledContinuity => {
924 let boundary_profile = derive_boundary_profile(case, segment_durations);
925 let segments =
926 build_quintic_pass_through_segments(case, segment_durations, &boundary_profile);
927 build_quintic_bundle(&segments, segment_durations, case.dt)
928 }
929 }
930}
931
932fn build_minimum_snap_variant(
933 case: &DroneTrajectoryCase,
934 segment_durations: &[f64],
935) -> BuiltTrajectory {
936 match case.boundary_mode {
937 DroneBoundaryMode::StopGo => {
938 let segments = generate_waypoint_minimum_snap_trajectory_with_durations(
939 &case.waypoints,
940 segment_durations,
941 );
942 build_minimum_snap_bundle(&segments, case.dt)
943 }
944 DroneBoundaryMode::PassThrough
945 | DroneBoundaryMode::PassThroughAccel
946 | DroneBoundaryMode::PassThroughAccelJerk
947 | DroneBoundaryMode::CoupledContinuity => {
948 let boundary_profile = derive_boundary_profile(case, segment_durations);
949 let segments = build_minimum_snap_pass_through_segments(
950 case,
951 segment_durations,
952 &boundary_profile,
953 );
954 build_minimum_snap_bundle(&segments, case.dt)
955 }
956 }
957}
958
959#[derive(Debug, Clone)]
960struct WaypointBoundaryProfile {
961 velocities: Vec<Vector3<f64>>,
962 accelerations: Vec<Vector3<f64>>,
963 jerks: Vec<Vector3<f64>>,
964}
965
966fn derive_boundary_profile(
967 case: &DroneTrajectoryCase,
968 segment_durations: &[f64],
969) -> WaypointBoundaryProfile {
970 match case.boundary_mode {
971 DroneBoundaryMode::PassThrough
972 | DroneBoundaryMode::PassThroughAccel
973 | DroneBoundaryMode::PassThroughAccelJerk => {
974 derive_local_pass_through_profile(case, segment_durations)
975 }
976 DroneBoundaryMode::CoupledContinuity => {
977 derive_coupled_continuity_profile(case, segment_durations)
978 }
979 DroneBoundaryMode::StopGo => WaypointBoundaryProfile {
980 velocities: vec![Vector3::zeros(); case.waypoints.len()],
981 accelerations: vec![Vector3::zeros(); case.waypoints.len()],
982 jerks: vec![Vector3::zeros(); case.waypoints.len()],
983 },
984 }
985}
986
987fn derive_local_pass_through_profile(
988 case: &DroneTrajectoryCase,
989 segment_durations: &[f64],
990) -> WaypointBoundaryProfile {
991 let n = case.waypoints.len();
992 let mut velocities = vec![Vector3::zeros(); n];
993
994 for index in 0..n {
995 let prev = (index + n - 1) % n;
996 let next = (index + 1) % n;
997 let incoming = (case.waypoints[index] - case.waypoints[prev]) / segment_durations[prev];
998 let outgoing = (case.waypoints[next] - case.waypoints[index]) / segment_durations[index];
999 velocities[index] = 0.5 * case.tangent_scale * (incoming + outgoing);
1000 }
1001
1002 let mut accelerations = vec![Vector3::zeros(); n];
1003 if matches!(
1004 case.boundary_mode,
1005 DroneBoundaryMode::PassThroughAccel | DroneBoundaryMode::PassThroughAccelJerk
1006 ) {
1007 for index in 0..n {
1008 let prev = (index + n - 1) % n;
1009 let avg_dt = 0.5 * (segment_durations[prev] + segment_durations[index]);
1010 accelerations[index] =
1011 case.acceleration_scale * (velocities[index] - velocities[prev]) / avg_dt;
1012 }
1013 }
1014
1015 let mut jerks = vec![Vector3::zeros(); n];
1016 if matches!(case.boundary_mode, DroneBoundaryMode::PassThroughAccelJerk) {
1017 for index in 0..n {
1018 let prev = (index + n - 1) % n;
1019 let avg_dt = 0.5 * (segment_durations[prev] + segment_durations[index]);
1020 jerks[index] = case.jerk_scale * (accelerations[index] - accelerations[prev]) / avg_dt;
1021 }
1022 }
1023
1024 WaypointBoundaryProfile {
1025 velocities,
1026 accelerations,
1027 jerks,
1028 }
1029}
1030
1031fn derive_coupled_continuity_profile(
1032 case: &DroneTrajectoryCase,
1033 segment_durations: &[f64],
1034) -> WaypointBoundaryProfile {
1035 let mut velocities = solve_coupled_waypoint_velocities(&case.waypoints, segment_durations);
1036 for velocity in &mut velocities {
1037 *velocity *= case.tangent_scale;
1038 }
1039
1040 let mut accelerations =
1041 derive_coupled_waypoint_accelerations(&case.waypoints, segment_durations, &velocities);
1042 for acceleration in &mut accelerations {
1043 *acceleration *= case.acceleration_scale;
1044 }
1045
1046 let mut jerks = derive_coupled_waypoint_jerks(&case.waypoints, segment_durations, &velocities);
1047 for jerk in &mut jerks {
1048 *jerk *= case.jerk_scale;
1049 }
1050
1051 WaypointBoundaryProfile {
1052 velocities,
1053 accelerations,
1054 jerks,
1055 }
1056}
1057
1058fn solve_coupled_waypoint_velocities(
1059 waypoints: &[Vector3<f64>],
1060 segment_durations: &[f64],
1061) -> Vec<Vector3<f64>> {
1062 let n = waypoints.len();
1063 let mut system = DMatrix::zeros(n, n);
1064 let mut rhs = DMatrix::zeros(n, 3);
1065
1066 for index in 0..n {
1067 let prev = (index + n - 1) % n;
1068 let next = (index + 1) % n;
1069 let h_prev = segment_durations[prev];
1070 let h_next = segment_durations[index];
1071 let delta_prev = waypoints[index] - waypoints[prev];
1072 let delta_next = waypoints[next] - waypoints[index];
1073
1074 system[(index, prev)] = 2.0 / h_prev;
1075 system[(index, index)] = 4.0 / h_prev + 4.0 / h_next;
1076 system[(index, next)] = 2.0 / h_next;
1077
1078 let boundary_rhs =
1079 6.0 * delta_prev / (h_prev * h_prev) + 6.0 * delta_next / (h_next * h_next);
1080 rhs[(index, 0)] = boundary_rhs.x;
1081 rhs[(index, 1)] = boundary_rhs.y;
1082 rhs[(index, 2)] = boundary_rhs.z;
1083 }
1084
1085 let solution = system
1086 .lu()
1087 .solve(&rhs)
1088 .expect("Coupled continuity system should be invertible");
1089
1090 (0..n)
1091 .map(|index| {
1092 Vector3::new(
1093 solution[(index, 0)],
1094 solution[(index, 1)],
1095 solution[(index, 2)],
1096 )
1097 })
1098 .collect()
1099}
1100
1101fn derive_coupled_waypoint_accelerations(
1102 waypoints: &[Vector3<f64>],
1103 segment_durations: &[f64],
1104 velocities: &[Vector3<f64>],
1105) -> Vec<Vector3<f64>> {
1106 let n = waypoints.len();
1107 (0..n)
1108 .map(|index| {
1109 let prev = (index + n - 1) % n;
1110 let next = (index + 1) % n;
1111 let prev_delta = waypoints[index] - waypoints[prev];
1112 let next_delta = waypoints[next] - waypoints[index];
1113 let prev_end = cubic_segment_end_acceleration(
1114 prev_delta,
1115 segment_durations[prev],
1116 velocities[prev],
1117 velocities[index],
1118 );
1119 let next_start = cubic_segment_start_acceleration(
1120 next_delta,
1121 segment_durations[index],
1122 velocities[index],
1123 velocities[next],
1124 );
1125 0.5 * (prev_end + next_start)
1126 })
1127 .collect()
1128}
1129
1130fn derive_coupled_waypoint_jerks(
1131 waypoints: &[Vector3<f64>],
1132 segment_durations: &[f64],
1133 velocities: &[Vector3<f64>],
1134) -> Vec<Vector3<f64>> {
1135 let n = waypoints.len();
1136 (0..n)
1137 .map(|index| {
1138 let prev = (index + n - 1) % n;
1139 let next = (index + 1) % n;
1140 let prev_jerk = cubic_segment_jerk(
1141 waypoints[index] - waypoints[prev],
1142 segment_durations[prev],
1143 velocities[prev],
1144 velocities[index],
1145 );
1146 let next_jerk = cubic_segment_jerk(
1147 waypoints[next] - waypoints[index],
1148 segment_durations[index],
1149 velocities[index],
1150 velocities[next],
1151 );
1152 0.5 * (prev_jerk + next_jerk)
1153 })
1154 .collect()
1155}
1156
1157fn cubic_segment_start_acceleration(
1158 delta: Vector3<f64>,
1159 duration: f64,
1160 start_velocity: Vector3<f64>,
1161 end_velocity: Vector3<f64>,
1162) -> Vector3<f64> {
1163 6.0 * delta / (duration * duration) - (4.0 * start_velocity + 2.0 * end_velocity) / duration
1164}
1165
1166fn cubic_segment_end_acceleration(
1167 delta: Vector3<f64>,
1168 duration: f64,
1169 start_velocity: Vector3<f64>,
1170 end_velocity: Vector3<f64>,
1171) -> Vector3<f64> {
1172 -6.0 * delta / (duration * duration) + (2.0 * start_velocity + 4.0 * end_velocity) / duration
1173}
1174
1175fn cubic_segment_jerk(
1176 delta: Vector3<f64>,
1177 duration: f64,
1178 start_velocity: Vector3<f64>,
1179 end_velocity: Vector3<f64>,
1180) -> Vector3<f64> {
1181 -12.0 * delta / duration.powi(3) + 6.0 * (start_velocity + end_velocity) / duration.powi(2)
1182}
1183
1184fn build_quintic_pass_through_segments(
1185 case: &DroneTrajectoryCase,
1186 segment_durations: &[f64],
1187 profile: &WaypointBoundaryProfile,
1188) -> Vec<TrajectorySegment> {
1189 let n = case.waypoints.len();
1190 (0..n)
1191 .map(|index| {
1192 let next = (index + 1) % n;
1193 generate_trajectory_segment_full(
1194 &case.waypoints[index],
1195 &case.waypoints[next],
1196 &profile.velocities[index],
1197 &profile.velocities[next],
1198 &profile.accelerations[index],
1199 &profile.accelerations[next],
1200 segment_durations[index],
1201 )
1202 })
1203 .collect()
1204}
1205
1206fn build_minimum_snap_pass_through_segments(
1207 case: &DroneTrajectoryCase,
1208 segment_durations: &[f64],
1209 profile: &WaypointBoundaryProfile,
1210) -> Vec<MinimumSnapSegment> {
1211 let n = case.waypoints.len();
1212 (0..n)
1213 .map(|index| {
1214 let next = (index + 1) % n;
1215 let start = MinimumSnapBoundary {
1216 position: case.waypoints[index],
1217 velocity: profile.velocities[index],
1218 acceleration: profile.accelerations[index],
1219 jerk: profile.jerks[index],
1220 };
1221 let end = MinimumSnapBoundary {
1222 position: case.waypoints[next],
1223 velocity: profile.velocities[next],
1224 acceleration: profile.accelerations[next],
1225 jerk: profile.jerks[next],
1226 };
1227 generate_minimum_snap_segment_full(&start, &end, segment_durations[index])
1228 })
1229 .collect()
1230}
1231
1232fn build_quintic_bundle(
1233 segments: &[TrajectorySegment],
1234 segment_durations: &[f64],
1235 dt: f64,
1236) -> BuiltTrajectory {
1237 let desired_states = sample_trajectory_segments(segments, segment_durations, dt);
1238 let mut jerk_norms = Vec::new();
1239 let mut snap_norms = Vec::new();
1240
1241 for (segment, duration) in segments.iter().zip(segment_durations.iter().copied()) {
1242 let mut t = 0.0;
1243 while t <= duration {
1244 jerk_norms.push(segment.jerk(t).norm());
1245 snap_norms.push(segment.snap(t).norm());
1246 t += dt;
1247 }
1248 }
1249
1250 BuiltTrajectory {
1251 desired_states,
1252 jerk_norms,
1253 snap_norms,
1254 total_duration_s: segment_durations.iter().sum(),
1255 }
1256}
1257
1258fn build_minimum_snap_bundle(segments: &[MinimumSnapSegment], dt: f64) -> BuiltTrajectory {
1259 let desired_states = sample_minimum_snap_trajectory(segments, dt);
1260 let mut jerk_norms = Vec::new();
1261 let mut snap_norms = Vec::new();
1262
1263 for segment in segments {
1264 let mut t = 0.0;
1265 while t <= segment.duration {
1266 jerk_norms.push(segment.jerk(t).norm());
1267 snap_norms.push(segment.snap(t).norm());
1268 t += dt;
1269 }
1270 }
1271
1272 BuiltTrajectory {
1273 desired_states,
1274 jerk_norms,
1275 snap_norms,
1276 total_duration_s: segments.iter().map(|segment| segment.duration).sum(),
1277 }
1278}
1279
1280fn distance_scaled_durations(case: &DroneTrajectoryCase) -> Vec<f64> {
1281 let mut raw: Vec<f64> = (0..case.waypoints.len())
1282 .map(|index| {
1283 let next = (index + 1) % case.waypoints.len();
1284 let distance = (case.waypoints[next] - case.waypoints[index]).norm();
1285 (distance / case.cruise_speed).max(case.min_segment_duration)
1286 })
1287 .collect();
1288
1289 let target_total = case.baseline_segment_duration * case.waypoints.len() as f64;
1290 let raw_total: f64 = raw.iter().sum();
1291 let scale = target_total / raw_total;
1292 for duration in &mut raw {
1293 *duration *= scale;
1294 }
1295 raw
1296}
1297
1298fn rms(values: &[f64]) -> f64 {
1299 let sum = values.iter().map(|value| value * value).sum::<f64>();
1300 (sum / values.len() as f64).sqrt()
1301}
1302
1303#[cfg(test)]
1304mod tests {
1305 use super::*;
1306
1307 #[test]
1308 fn coupled_continuity_profile_preserves_c2_waypoint_acceleration() {
1309 let case = coupled_continuity_drone_trajectory_cases()
1310 .into_iter()
1311 .next()
1312 .expect("coupled continuity cases should exist");
1313 let segment_durations = distance_scaled_durations(&case);
1314 let profile = derive_coupled_continuity_profile(&case, &segment_durations);
1315 let n = case.waypoints.len();
1316
1317 for index in 0..n {
1318 let prev = (index + n - 1) % n;
1319 let next = (index + 1) % n;
1320 let prev_end = cubic_segment_end_acceleration(
1321 case.waypoints[index] - case.waypoints[prev],
1322 segment_durations[prev],
1323 profile.velocities[prev],
1324 profile.velocities[index],
1325 );
1326 let next_start = cubic_segment_start_acceleration(
1327 case.waypoints[next] - case.waypoints[index],
1328 segment_durations[index],
1329 profile.velocities[index],
1330 profile.velocities[next],
1331 );
1332
1333 assert!(
1334 (prev_end - next_start).norm() < 1e-8,
1335 "waypoint {index} lost coupled acceleration continuity: prev_end={prev_end:?}, next_start={next_start:?}"
1336 );
1337 assert!(
1338 (profile.accelerations[index] - prev_end).norm() < 1e-8,
1339 "stored acceleration should match the coupled continuity solution at waypoint {index}"
1340 );
1341 }
1342 }
1343}
1344
1345#[derive(Debug, Clone)]
1347pub struct GainSweepMetrics {
1348 pub family_name: &'static str,
1349 pub trajectory_variant: DroneTrajectoryVariant,
1350 pub gain_set: LateralGainSet,
1351 pub gain_label: String,
1352 pub tracking_rmse_m: f64,
1353 pub max_position_error_m: f64,
1354 pub mean_thrust_n: f64,
1355 pub max_thrust_n: f64,
1356 pub mean_torque_norm_nm: f64,
1357}
1358
1359pub fn evaluate_lateral_gain_case(
1361 case: &DroneTrajectoryCase,
1362 trajectory_variant: DroneTrajectoryVariant,
1363 lateral: &LateralGainSet,
1364 gain_label: &str,
1365) -> GainSweepMetrics {
1366 let built = build_variant_trajectory(case, trajectory_variant);
1367 let simulation_config = SimulationConfig {
1368 dt: case.dt,
1369 segment_duration: case.baseline_segment_duration,
1370 n_loops: 1,
1371 ..Default::default()
1372 };
1373
1374 let mut state = QuadrotorState::new(case.waypoints[0]);
1375 let mut controls = Vec::with_capacity(built.desired_states.len());
1376 let mut squared_error_sum = 0.0;
1377 let mut max_position_error_m = 0.0_f64;
1378
1379 for desired in &built.desired_states {
1380 let control = compute_attitude_rate_damped_lateral_control_with_gains(
1381 &state,
1382 desired,
1383 &simulation_config.params,
1384 &simulation_config.gains,
1385 lateral,
1386 );
1387 step_dynamics(
1388 &mut state,
1389 &control,
1390 &simulation_config.params,
1391 simulation_config.dt,
1392 );
1393 let error = (state.position - desired.position).norm();
1394 squared_error_sum += error * error;
1395 max_position_error_m = max_position_error_m.max(error);
1396 controls.push(control);
1397 }
1398
1399 let tracking_rmse_m = (squared_error_sum / built.desired_states.len() as f64).sqrt();
1400 let mean_thrust_n = controls.iter().map(|c| c.thrust).sum::<f64>() / controls.len() as f64;
1401 let max_thrust_n = controls
1402 .iter()
1403 .map(|c| c.thrust)
1404 .fold(f64::NEG_INFINITY, f64::max);
1405 let mean_torque_norm_nm = controls
1406 .iter()
1407 .map(|c| Vector3::new(c.roll_torque, c.pitch_torque, c.yaw_torque).norm())
1408 .sum::<f64>()
1409 / controls.len() as f64;
1410
1411 GainSweepMetrics {
1412 family_name: case.family_name,
1413 trajectory_variant,
1414 gain_set: *lateral,
1415 gain_label: gain_label.to_string(),
1416 tracking_rmse_m,
1417 max_position_error_m,
1418 mean_thrust_n,
1419 max_thrust_n,
1420 mean_torque_norm_nm,
1421 }
1422}