1use crate::racing_mppi_3d::{RacingGateLap3D, RacingGatePlane3D};
27use rand::{rngs::StdRng, SeedableRng};
28use rand_distr::{Distribution, Normal};
29use rust_robotics_core::{RoboticsError, RoboticsResult};
30
31#[derive(Debug, Clone, Copy, PartialEq)]
34pub struct QuadrotorState {
35 pub position: [f64; 3],
36 pub velocity: [f64; 3],
37 pub attitude: [f64; 4],
38}
39
40impl QuadrotorState {
41 pub fn at(x: f64, y: f64, z: f64) -> Self {
44 Self {
45 position: [x, y, z],
46 velocity: [0.0, 0.0, 0.0],
47 attitude: [1.0, 0.0, 0.0, 0.0],
48 }
49 }
50
51 pub fn speed(self) -> f64 {
52 norm(self.velocity)
53 }
54
55 pub fn thrust_axis(self) -> [f64; 3] {
57 rotate(self.attitude, [0.0, 0.0, 1.0])
58 }
59
60 pub fn tilt_angle(self) -> f64 {
62 self.thrust_axis()[2].clamp(-1.0, 1.0).acos()
63 }
64}
65
66#[derive(Debug, Clone, Copy, PartialEq)]
68pub struct QuadrotorControl {
69 pub thrust: f64,
71 pub body_rates: [f64; 3],
73}
74
75impl QuadrotorControl {
76 pub fn new(thrust: f64, body_rates: [f64; 3]) -> Self {
77 Self { thrust, body_rates }
78 }
79
80 pub fn hover(gravity: f64) -> Self {
82 Self {
83 thrust: gravity,
84 body_rates: [0.0, 0.0, 0.0],
85 }
86 }
87
88 pub fn body_rate_magnitude(self) -> f64 {
90 norm(self.body_rates)
91 }
92}
93
94#[derive(Debug, Clone, Copy, PartialEq)]
101pub struct QuadrotorParams {
102 pub gravity: f64,
104 pub drag: f64,
106 pub min_thrust: f64,
108 pub max_thrust: f64,
110 pub max_body_rate: f64,
112 pub max_speed: f64,
114}
115
116impl Default for QuadrotorParams {
117 fn default() -> Self {
118 Self {
119 gravity: 9.81,
120 drag: 0.3,
121 min_thrust: 0.0,
122 max_thrust: 24.0,
123 max_body_rate: 6.0,
124 max_speed: 7.0,
125 }
126 }
127}
128
129impl QuadrotorParams {
130 pub fn new(
131 gravity: f64,
132 drag: f64,
133 min_thrust: f64,
134 max_thrust: f64,
135 max_body_rate: f64,
136 max_speed: f64,
137 ) -> RoboticsResult<Self> {
138 if !gravity.is_finite() || gravity < 0.0 {
139 return Err(RoboticsError::InvalidParameter(
140 "quadrotor gravity must be finite and non-negative".to_string(),
141 ));
142 }
143 if !drag.is_finite() || drag < 0.0 {
144 return Err(RoboticsError::InvalidParameter(
145 "quadrotor drag must be finite and non-negative".to_string(),
146 ));
147 }
148 if !min_thrust.is_finite() || min_thrust < 0.0 {
149 return Err(RoboticsError::InvalidParameter(
150 "quadrotor min_thrust must be finite and non-negative".to_string(),
151 ));
152 }
153 if !max_thrust.is_finite() || max_thrust <= min_thrust {
154 return Err(RoboticsError::InvalidParameter(
155 "quadrotor max_thrust must be finite and exceed min_thrust".to_string(),
156 ));
157 }
158 if !max_body_rate.is_finite() || max_body_rate <= 0.0 {
159 return Err(RoboticsError::InvalidParameter(
160 "quadrotor max_body_rate must be finite and positive".to_string(),
161 ));
162 }
163 if !max_speed.is_finite() || max_speed <= 0.0 {
164 return Err(RoboticsError::InvalidParameter(
165 "quadrotor max_speed must be finite and positive".to_string(),
166 ));
167 }
168 Ok(Self {
169 gravity,
170 drag,
171 min_thrust,
172 max_thrust,
173 max_body_rate,
174 max_speed,
175 })
176 }
177
178 pub fn saturate(self, control: QuadrotorControl) -> QuadrotorControl {
180 let thrust = control.thrust.clamp(self.min_thrust, self.max_thrust);
181 let mut rates = control.body_rates;
182 let magnitude = norm(rates);
183 if magnitude > self.max_body_rate && magnitude > 0.0 {
184 let scale = self.max_body_rate / magnitude;
185 rates = [rates[0] * scale, rates[1] * scale, rates[2] * scale];
186 }
187 QuadrotorControl::new(thrust, rates)
188 }
189
190 pub fn step(self, state: QuadrotorState, control: QuadrotorControl, dt: f64) -> QuadrotorState {
192 let control = self.saturate(control);
193
194 let attitude = normalize_quat(integrate_attitude(state.attitude, control.body_rates, dt));
196
197 let thrust_axis = rotate(attitude, [0.0, 0.0, 1.0]);
199 let accel = [
200 control.thrust * thrust_axis[0] - self.drag * state.velocity[0],
201 control.thrust * thrust_axis[1] - self.drag * state.velocity[1],
202 control.thrust * thrust_axis[2] - self.gravity - self.drag * state.velocity[2],
203 ];
204 let mut velocity = [
205 state.velocity[0] + accel[0] * dt,
206 state.velocity[1] + accel[1] * dt,
207 state.velocity[2] + accel[2] * dt,
208 ];
209 let speed = norm(velocity);
210 if speed > self.max_speed && speed > 0.0 {
211 let scale = self.max_speed / speed;
212 velocity = [
213 velocity[0] * scale,
214 velocity[1] * scale,
215 velocity[2] * scale,
216 ];
217 }
218 let position = [
219 state.position[0] + velocity[0] * dt,
220 state.position[1] + velocity[1] * dt,
221 state.position[2] + velocity[2] * dt,
222 ];
223
224 QuadrotorState {
225 position,
226 velocity,
227 attitude,
228 }
229 }
230}
231
232#[derive(Debug, Clone, PartialEq)]
234pub struct QuadrotorLapReport {
235 pub steps: usize,
237 pub gates_passed: usize,
239 pub laps_completed: usize,
241 pub lap_fraction: f64,
243 pub mean_speed: f64,
245 pub max_speed: f64,
247 pub path_length: f64,
249 pub gate_distance: f64,
251 pub min_aperture_margin: f64,
254 pub mean_tilt: f64,
256 pub max_tilt: f64,
258 pub mean_body_rate: f64,
260 pub first_lap_time: Option<f64>,
262 pub path: Vec<[f64; 3]>,
264}
265
266#[derive(Debug, Clone, PartialEq)]
268pub struct QuadrotorMppiConfig {
269 pub horizon: usize,
270 pub samples: usize,
271 pub dt: f64,
272 pub thrust_sigma: f64,
274 pub rate_sigma: f64,
276 pub velocity_weight: f64,
278 pub rate_weight: f64,
280 pub level_weight: f64,
282 pub lambda: f64,
283 pub seed: u64,
284}
285
286impl Default for QuadrotorMppiConfig {
287 fn default() -> Self {
288 Self {
289 horizon: 28,
290 samples: 900,
291 dt: 0.05,
292 thrust_sigma: 5.0,
293 rate_sigma: 5.0,
294 velocity_weight: 0.01,
295 rate_weight: 0.01,
296 level_weight: 0.6,
297 lambda: 1.0,
298 seed: 7,
299 }
300 }
301}
302
303fn validate_config(config: &QuadrotorMppiConfig) -> RoboticsResult<()> {
304 if config.horizon == 0 {
305 return Err(RoboticsError::InvalidParameter(
306 "quadrotor MPPI horizon must be positive".to_string(),
307 ));
308 }
309 if config.samples == 0 {
310 return Err(RoboticsError::InvalidParameter(
311 "quadrotor MPPI samples must be positive".to_string(),
312 ));
313 }
314 if !config.dt.is_finite() || config.dt <= 0.0 {
315 return Err(RoboticsError::InvalidParameter(
316 "quadrotor MPPI dt must be finite and positive".to_string(),
317 ));
318 }
319 if !config.thrust_sigma.is_finite() || config.thrust_sigma <= 0.0 {
320 return Err(RoboticsError::InvalidParameter(
321 "quadrotor MPPI thrust_sigma must be finite and positive".to_string(),
322 ));
323 }
324 if !config.rate_sigma.is_finite() || config.rate_sigma <= 0.0 {
325 return Err(RoboticsError::InvalidParameter(
326 "quadrotor MPPI rate_sigma must be finite and positive".to_string(),
327 ));
328 }
329 if !config.lambda.is_finite() || config.lambda <= 0.0 {
330 return Err(RoboticsError::InvalidParameter(
331 "quadrotor MPPI lambda must be finite and positive".to_string(),
332 ));
333 }
334 if config.velocity_weight < 0.0 || config.rate_weight < 0.0 || config.level_weight < 0.0 {
335 return Err(RoboticsError::InvalidParameter(
336 "quadrotor MPPI weights must be non-negative".to_string(),
337 ));
338 }
339 Ok(())
340}
341
342#[derive(Debug, Clone, PartialEq)]
344pub struct QuadrotorMppiPlan {
345 pub control: QuadrotorControl,
346 pub best_cost: f64,
348 pub normalized_effective_sample_size: f64,
350}
351
352#[derive(Debug, Clone)]
354pub struct QuadrotorMppiController {
355 config: QuadrotorMppiConfig,
356 params: QuadrotorParams,
357 nominal: Vec<QuadrotorControl>,
358 rng: StdRng,
359}
360
361impl QuadrotorMppiController {
362 pub fn new(config: QuadrotorMppiConfig, params: QuadrotorParams) -> RoboticsResult<Self> {
363 validate_config(&config)?;
364 let nominal = vec![QuadrotorControl::hover(params.gravity); config.horizon];
365 let rng = StdRng::seed_from_u64(config.seed);
366 Ok(Self {
367 config,
368 params,
369 nominal,
370 rng,
371 })
372 }
373
374 pub fn config(&self) -> &QuadrotorMppiConfig {
375 &self.config
376 }
377
378 pub fn predicted_positions(&self, start: QuadrotorState) -> Vec<[f64; 3]> {
384 self.rollout(start, &self.nominal).0
385 }
386
387 fn rollout(
388 &self,
389 start: QuadrotorState,
390 controls: &[QuadrotorControl],
391 ) -> (Vec<[f64; 3]>, f64) {
392 let mut state = start;
393 let mut positions = Vec::with_capacity(controls.len() + 1);
394 positions.push(state.position);
395 let mut regularizer = 0.0;
396 for &control in controls {
397 state = self.params.step(state, control, self.config.dt);
398 let speed = state.speed();
399 let rate = control.body_rate_magnitude();
400 let tilt_cost = 1.0 - state.thrust_axis()[2];
402 regularizer += self.config.velocity_weight * speed * speed
403 + self.config.rate_weight * rate * rate
404 + self.config.level_weight * tilt_cost;
405 positions.push(state.position);
406 }
407 (positions, regularizer)
408 }
409
410 pub fn plan(
412 &mut self,
413 start: QuadrotorState,
414 lap: &RacingGateLap3D,
415 passed: usize,
416 ) -> RoboticsResult<QuadrotorMppiPlan> {
417 let thrust_noise = Normal::new(0.0, self.config.thrust_sigma).map_err(|_| {
418 RoboticsError::InvalidParameter("invalid quadrotor thrust distribution".to_string())
419 })?;
420 let rate_noise = Normal::new(0.0, self.config.rate_sigma).map_err(|_| {
421 RoboticsError::InvalidParameter("invalid quadrotor rate distribution".to_string())
422 })?;
423
424 let mut costs = Vec::with_capacity(self.config.samples);
425 let mut sequences = Vec::with_capacity(self.config.samples);
426 let mut best_cost = f64::INFINITY;
427
428 for _ in 0..self.config.samples {
429 let mut controls = Vec::with_capacity(self.config.horizon);
430 for &base in &self.nominal {
431 let noisy = QuadrotorControl::new(
432 base.thrust + thrust_noise.sample(&mut self.rng),
433 [
434 base.body_rates[0] + rate_noise.sample(&mut self.rng),
435 base.body_rates[1] + rate_noise.sample(&mut self.rng),
436 base.body_rates[2] + rate_noise.sample(&mut self.rng),
437 ],
438 );
439 controls.push(self.params.saturate(noisy));
440 }
441 let (positions, regularizer) = self.rollout(start, &controls);
442 let (gate_cost, _) = lap.score_positions(&positions, passed);
443 let cost = gate_cost + regularizer;
444 best_cost = best_cost.min(cost);
445 costs.push(cost);
446 sequences.push(controls);
447 }
448
449 let min_cost = costs.iter().copied().fold(f64::INFINITY, f64::min);
450 let mut weights = Vec::with_capacity(costs.len());
451 let mut weight_sum = 0.0;
452 for &cost in &costs {
453 let weight = (-(cost - min_cost) / self.config.lambda).exp();
454 weight_sum += weight;
455 weights.push(weight);
456 }
457
458 if weight_sum <= 0.0 || !weight_sum.is_finite() {
459 let best_index = costs
460 .iter()
461 .enumerate()
462 .min_by(|a, b| a.1.partial_cmp(b.1).unwrap_or(std::cmp::Ordering::Equal))
463 .map(|(index, _)| index)
464 .unwrap_or(0);
465 let control = sequences[best_index][0];
466 return Ok(QuadrotorMppiPlan {
467 control,
468 best_cost,
469 normalized_effective_sample_size: 0.0,
470 });
471 }
472
473 let mut updated = vec![QuadrotorControl::new(0.0, [0.0, 0.0, 0.0]); self.config.horizon];
474 let mut sum_sq = 0.0;
475 for (weight, controls) in weights.iter().zip(&sequences) {
476 let normalized = weight / weight_sum;
477 sum_sq += normalized * normalized;
478 for (acc, control) in updated.iter_mut().zip(controls) {
479 acc.thrust += normalized * control.thrust;
480 acc.body_rates[0] += normalized * control.body_rates[0];
481 acc.body_rates[1] += normalized * control.body_rates[1];
482 acc.body_rates[2] += normalized * control.body_rates[2];
483 }
484 }
485 for control in &mut updated {
486 *control = self.params.saturate(*control);
487 }
488
489 let first_control = updated[0];
490 self.nominal.clear();
492 self.nominal.extend_from_slice(&updated[1..]);
493 self.nominal.push(*updated.last().unwrap());
494
495 let normalized_effective_sample_size = if sum_sq > 0.0 {
496 1.0 / (sum_sq * self.config.samples as f64)
497 } else {
498 0.0
499 };
500
501 Ok(QuadrotorMppiPlan {
502 control: first_control,
503 best_cost,
504 normalized_effective_sample_size,
505 })
506 }
507}
508
509pub fn simulate_quadrotor_race(
516 config: QuadrotorMppiConfig,
517 params: QuadrotorParams,
518 lap: &RacingGateLap3D,
519 start: QuadrotorState,
520 max_steps: usize,
521 target_laps: usize,
522) -> RoboticsResult<QuadrotorLapReport> {
523 let dt = config.dt;
524 let gate_count = lap.gate_count();
525 let mut controller = QuadrotorMppiController::new(config, params)?;
526
527 let mut state = start;
528 let mut passed = 0usize;
529 let mut path = vec![state.position];
530 let mut sum_speed = 0.0;
531 let mut max_speed = 0.0_f64;
532 let mut sum_tilt = 0.0;
533 let mut max_tilt = 0.0_f64;
534 let mut sum_body_rate = 0.0;
535 let mut path_length = 0.0;
536 let mut min_aperture_margin = f64::INFINITY;
537 let mut first_lap_time = None;
538 let mut executed_steps = 0usize;
539
540 for step in 0..max_steps {
541 let plan = controller.plan(state, lap, passed)?;
542 let next = params.step(state, plan.control, dt);
543 let from = state.position;
544 let to = next.position;
545
546 loop {
547 let gate = lap.gate_for_pass_count(passed);
548 if !gate.crosses(from, to, lap.crossing_tolerance) {
549 break;
550 }
551 let margin = aperture_crossing_margin(gate, from, to);
552 min_aperture_margin = min_aperture_margin.min(margin);
553 passed += 1;
554 if passed == gate_count && first_lap_time.is_none() {
555 first_lap_time = Some((step + 1) as f64 * dt);
556 }
557 if !lap.closed && passed >= gate_count {
558 break;
559 }
560 }
561
562 path_length += distance(from, to);
563 let speed = next.speed();
564 sum_speed += speed;
565 max_speed = max_speed.max(speed);
566 let tilt = next.tilt_angle();
567 sum_tilt += tilt;
568 max_tilt = max_tilt.max(tilt);
569 sum_body_rate += plan.control.body_rate_magnitude();
570 state = next;
571 path.push(to);
572 executed_steps += 1;
573
574 if target_laps > 0 && passed >= target_laps * gate_count {
575 break;
576 }
577 if !lap.closed && passed >= gate_count {
578 break;
579 }
580 }
581
582 let laps_completed = passed / gate_count;
583 let lap_fraction = (passed % gate_count) as f64 / gate_count as f64;
584 let active_gate = lap.gate_for_pass_count(passed);
585 let gate_distance = distance(active_gate.center(), state.position);
586 let denom = executed_steps.max(1) as f64;
587
588 Ok(QuadrotorLapReport {
589 steps: executed_steps,
590 gates_passed: passed,
591 laps_completed,
592 lap_fraction,
593 mean_speed: sum_speed / denom,
594 max_speed,
595 path_length,
596 gate_distance,
597 min_aperture_margin,
598 mean_tilt: sum_tilt / denom,
599 max_tilt,
600 mean_body_rate: sum_body_rate / denom,
601 first_lap_time,
602 path,
603 })
604}
605
606fn aperture_crossing_margin(gate: &RacingGatePlane3D, from: [f64; 3], to: [f64; 3]) -> f64 {
607 let from_signed = gate.signed_distance(from);
608 let to_signed = gate.signed_distance(to);
609 let denom = to_signed - from_signed;
610 let t = if denom.abs() > 1e-9 {
611 (-from_signed / denom).clamp(0.0, 1.0)
612 } else {
613 0.0
614 };
615 let crossing = [
616 from[0] + t * (to[0] - from[0]),
617 from[1] + t * (to[1] - from[1]),
618 from[2] + t * (to[2] - from[2]),
619 ];
620 gate.aperture_margin(crossing)
621}
622
623fn rotate(q: [f64; 4], v: [f64; 3]) -> [f64; 3] {
627 let [w, x, y, z] = q;
628 let qv = [x, y, z];
630 let t = scale(cross(qv, v), 2.0);
631 let cross_t = cross(qv, t);
632 [
633 v[0] + w * t[0] + cross_t[0],
634 v[1] + w * t[1] + cross_t[1],
635 v[2] + w * t[2] + cross_t[2],
636 ]
637}
638
639fn integrate_attitude(q: [f64; 4], rates: [f64; 3], dt: f64) -> [f64; 4] {
641 let [qw, qx, qy, qz] = q;
642 let [wx, wy, wz] = rates;
643 let dw = -(qx * wx + qy * wy + qz * wz);
645 let dx = qw * wx + qy * wz - qz * wy;
646 let dy = qw * wy + qz * wx - qx * wz;
647 let dz = qw * wz + qx * wy - qy * wx;
648 let half_dt = 0.5 * dt;
649 [
650 qw + half_dt * dw,
651 qx + half_dt * dx,
652 qy + half_dt * dy,
653 qz + half_dt * dz,
654 ]
655}
656
657fn normalize_quat(q: [f64; 4]) -> [f64; 4] {
658 let n = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]).sqrt();
659 if !n.is_finite() || n <= 0.0 {
660 return [1.0, 0.0, 0.0, 0.0];
661 }
662 [q[0] / n, q[1] / n, q[2] / n, q[3] / n]
663}
664
665fn cross(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
666 [
667 a[1] * b[2] - a[2] * b[1],
668 a[2] * b[0] - a[0] * b[2],
669 a[0] * b[1] - a[1] * b[0],
670 ]
671}
672
673fn scale(a: [f64; 3], s: f64) -> [f64; 3] {
674 [a[0] * s, a[1] * s, a[2] * s]
675}
676
677fn norm(a: [f64; 3]) -> f64 {
678 (a[0] * a[0] + a[1] * a[1] + a[2] * a[2]).sqrt()
679}
680
681fn distance(a: [f64; 3], b: [f64; 3]) -> f64 {
682 let d = [a[0] - b[0], a[1] - b[1], a[2] - b[2]];
683 norm(d)
684}
685
686#[cfg(test)]
687mod tests {
688 use super::*;
689
690 fn gate(center: [f64; 3], normal: [f64; 3]) -> RacingGatePlane3D {
691 RacingGatePlane3D::new(center, normal, [0.0, 0.0, 1.0], 0.8, 0.8).unwrap()
692 }
693
694 fn straight_lap() -> RacingGateLap3D {
695 let gates = vec![
696 gate([1.5, 0.0, 0.0], [1.0, 0.0, 0.0]),
697 gate([3.0, 0.0, 0.0], [1.0, 0.0, 0.0]),
698 gate([4.5, 0.0, 0.0], [1.0, 0.0, 0.0]),
699 ];
700 RacingGateLap3D::open(gates).unwrap()
701 }
702
703 #[test]
704 fn preview_does_not_consume_rng_or_change_the_next_plan() {
705 let params = QuadrotorParams::default();
706 let mut controller = QuadrotorMppiController::new(
707 QuadrotorMppiConfig {
708 samples: 64,
709 horizon: 12,
710 ..Default::default()
711 },
712 params,
713 )
714 .unwrap();
715 let state = QuadrotorState::at(0.0, 0.0, 0.0);
716 let plan = controller.plan(state, &straight_lap(), 0).unwrap();
717 let state = params.step(state, plan.control, controller.config().dt);
718 let mut unobserved = controller.clone();
719 let preview = controller.predicted_positions(state);
720 assert_eq!(preview[0], state.position);
721 assert_eq!(preview.len(), controller.config().horizon + 1);
722 assert!(preview.iter().flatten().all(|v| v.is_finite()));
723 assert_eq!(preview, controller.predicted_positions(state));
724 assert_eq!(
725 controller.plan(state, &straight_lap(), 0).unwrap(),
726 unobserved.plan(state, &straight_lap(), 0).unwrap()
727 );
728 }
729
730 #[test]
731 fn hover_holds_position() {
732 let params = QuadrotorParams::default();
733 let mut state = QuadrotorState::at(0.0, 0.0, 1.0);
734 for _ in 0..20 {
735 state = params.step(state, QuadrotorControl::hover(params.gravity), 0.05);
736 }
737 assert!(state.speed() < 1e-6);
739 assert!((state.position[2] - 1.0).abs() < 1e-6);
740 assert!(state.tilt_angle() < 1e-9);
741 }
742
743 #[test]
744 fn body_rate_tilts_then_thrust_moves_horizontally() {
745 let params = QuadrotorParams::default();
746 let mut state = QuadrotorState::at(0.0, 0.0, 0.0);
747 for _ in 0..6 {
749 state = params.step(
750 state,
751 QuadrotorControl::new(params.gravity, [0.0, 1.0, 0.0]),
752 0.05,
753 );
754 }
755 assert!(state.tilt_angle() > 0.1, "tilt {}", state.tilt_angle());
756 for _ in 0..6 {
758 state = params.step(
759 state,
760 QuadrotorControl::new(params.gravity + 4.0, [0.0, 0.0, 0.0]),
761 0.05,
762 );
763 }
764 assert!(state.velocity[0].abs() > 0.1, "vx {}", state.velocity[0]);
765 }
766
767 #[test]
768 fn attitude_stays_normalized() {
769 let params = QuadrotorParams::default();
770 let mut state = QuadrotorState::at(0.0, 0.0, 0.0);
771 for _ in 0..40 {
772 state = params.step(state, QuadrotorControl::new(11.0, [2.0, -1.5, 0.8]), 0.05);
773 }
774 let q = state.attitude;
775 let n = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]).sqrt();
776 assert!((n - 1.0).abs() < 1e-9, "quat norm {n}");
777 }
778
779 #[test]
780 fn saturation_clamps_thrust_and_rates() {
781 let params = QuadrotorParams::default();
782 let saturated = params.saturate(QuadrotorControl::new(1000.0, [100.0, 0.0, 0.0]));
783 assert!((saturated.thrust - params.max_thrust).abs() < 1e-9);
784 assert!((saturated.body_rate_magnitude() - params.max_body_rate).abs() < 1e-9);
785 }
786
787 #[test]
788 fn controller_makes_gate_progress() {
789 let lap = straight_lap();
790 let report = simulate_quadrotor_race(
791 QuadrotorMppiConfig::default(),
792 QuadrotorParams::default(),
793 &lap,
794 QuadrotorState::at(0.0, 0.0, 0.0),
795 120,
796 1,
797 )
798 .unwrap();
799 assert!(
800 report.gates_passed >= 1,
801 "expected at least one gate, got {}",
802 report.gates_passed
803 );
804 assert!(report.max_tilt > 0.05, "max tilt {}", report.max_tilt);
806 assert!(report.path_length > 0.0);
807 }
808
809 #[test]
810 fn simulation_is_deterministic() {
811 let lap = straight_lap();
812 let run = || {
813 simulate_quadrotor_race(
814 QuadrotorMppiConfig::default(),
815 QuadrotorParams::default(),
816 &lap,
817 QuadrotorState::at(0.0, 0.0, 0.0),
818 80,
819 1,
820 )
821 .unwrap()
822 };
823 let first = run();
824 let second = run();
825 assert_eq!(first.path, second.path);
826 assert_eq!(first.gates_passed, second.gates_passed);
827 }
828
829 #[test]
830 fn invalid_config_is_rejected() {
831 let config = QuadrotorMppiConfig {
832 horizon: 0,
833 ..QuadrotorMppiConfig::default()
834 };
835 assert!(QuadrotorMppiController::new(config, QuadrotorParams::default()).is_err());
836 }
837
838 #[test]
839 fn invalid_params_are_rejected() {
840 assert!(QuadrotorParams::new(9.81, 0.3, 5.0, 4.0, 6.0, 7.0).is_err());
841 }
842}