Skip to main content

rust_robotics_control/
racing_mppi_quadrotor.rs

1//! Reference-free racing MPPI with a full quadrotor attitude model.
2//!
3//! This deepens [`crate::racing_mppi_3d`] from a point-mass drone to a quadrotor
4//! whose orientation matters. The control input is the standard low-level agile-
5//! racing abstraction used by differential-flatness controllers: a mass-
6//! normalized collective thrust along the body z-axis plus three body rates.
7//!
8//! - [`QuadrotorState`] carries position, velocity, and a unit-quaternion
9//!   attitude.
10//! - [`QuadrotorParams`] integrates the rigid-body translational dynamics
11//!   (`a = thrust * body_z - gravity - drag * v`) together with quaternion
12//!   attitude kinematics driven by the commanded body rates.
13//! - [`QuadrotorMppiController`] samples thrust/body-rate perturbations around a
14//!   hover nominal, rolls them through the attitude dynamics, and scores the
15//!   resulting positions with the reference-free gate-progress objective from
16//!   [`crate::racing_mppi_3d`]. Because horizontal motion can only come from
17//!   tilting the thrust vector, the position objective drives the *attitude*:
18//!   the drone learns to pitch and roll toward the next gate.
19//! - [`simulate_quadrotor_race`] flies a deterministic, seeded controller around
20//!   a lap and reports [`QuadrotorLapReport`], which adds attitude metrics (tilt
21//!   angle and body-rate effort) to the lap-progress metrics.
22//!
23//! The gate geometry ([`RacingGatePlane3D`], [`RacingGateLap3D`]) is reused
24//! directly from [`crate::racing_mppi_3d`].
25
26use crate::racing_mppi_3d::{RacingGateLap3D, RacingGatePlane3D};
27use rand::{rngs::StdRng, SeedableRng};
28use rand_distr::{Distribution, Normal};
29use rust_robotics_core::{RoboticsError, RoboticsResult};
30
31/// State of the quadrotor: position \[m\], velocity \[m/s\], and a unit
32/// quaternion attitude `[w, x, y, z]` (body-to-world rotation).
33#[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    /// Level hover state at the given position (identity attitude, zero
42    /// velocity).
43    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    /// The body z-axis (thrust direction) expressed in the world frame.
56    pub fn thrust_axis(self) -> [f64; 3] {
57        rotate(self.attitude, [0.0, 0.0, 1.0])
58    }
59
60    /// Tilt angle \[rad\] between the thrust axis and world up, in `[0, pi]`.
61    pub fn tilt_angle(self) -> f64 {
62        self.thrust_axis()[2].clamp(-1.0, 1.0).acos()
63    }
64}
65
66/// Commanded collective thrust and body rates for the quadrotor.
67#[derive(Debug, Clone, Copy, PartialEq)]
68pub struct QuadrotorControl {
69    /// Mass-normalized collective thrust \[m/s^2\] along the body z-axis.
70    pub thrust: f64,
71    /// Body angular rates `[wx, wy, wz]` \[rad/s\].
72    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    /// Hover command: thrust balancing gravity, zero body rates.
81    pub fn hover(gravity: f64) -> Self {
82        Self {
83            thrust: gravity,
84            body_rates: [0.0, 0.0, 0.0],
85        }
86    }
87
88    /// Body-rate magnitude \[rad/s\].
89    pub fn body_rate_magnitude(self) -> f64 {
90        norm(self.body_rates)
91    }
92}
93
94/// Quadrotor rigid-body parameters and actuation limits.
95///
96/// Translational dynamics are semi-implicit: the collective thrust along the
97/// current body z-axis, minus gravity and linear drag, updates the velocity,
98/// the speed is capped, and the position integrates the new velocity. Attitude
99/// is integrated from the commanded body rates and renormalized each step.
100#[derive(Debug, Clone, Copy, PartialEq)]
101pub struct QuadrotorParams {
102    /// Downward gravitational acceleration \[m/s^2\].
103    pub gravity: f64,
104    /// Linear aerodynamic drag coefficient \[1/s\].
105    pub drag: f64,
106    /// Minimum mass-normalized collective thrust \[m/s^2\].
107    pub min_thrust: f64,
108    /// Maximum mass-normalized collective thrust \[m/s^2\].
109    pub max_thrust: f64,
110    /// Maximum body-rate magnitude \[rad/s\].
111    pub max_body_rate: f64,
112    /// Maximum speed \[m/s\] the body can reach.
113    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    /// Clamp a command to the thrust and body-rate limits.
179    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    /// Advance the quadrotor one step under the commanded thrust and body rates.
191    pub fn step(self, state: QuadrotorState, control: QuadrotorControl, dt: f64) -> QuadrotorState {
192        let control = self.saturate(control);
193
194        // Attitude kinematics: q_dot = 0.5 * q (x) [0, wx, wy, wz].
195        let attitude = normalize_quat(integrate_attitude(state.attitude, control.body_rates, dt));
196
197        // Translational dynamics in the world frame.
198        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/// Lap-progress and attitude metrics from a quadrotor racing rollout.
233#[derive(Debug, Clone, PartialEq)]
234pub struct QuadrotorLapReport {
235    /// Control steps executed.
236    pub steps: usize,
237    /// Total gate passes across the whole rollout (counts repeated laps).
238    pub gates_passed: usize,
239    /// Whole laps completed.
240    pub laps_completed: usize,
241    /// Fraction of the current (partial) lap completed, in \[0, 1).
242    pub lap_fraction: f64,
243    /// Mean speed \[m/s\] over the rollout.
244    pub mean_speed: f64,
245    /// Peak speed \[m/s\] over the rollout.
246    pub max_speed: f64,
247    /// Executed path length \[m\].
248    pub path_length: f64,
249    /// Distance \[m\] to the active gate at the end of the rollout.
250    pub gate_distance: f64,
251    /// Smallest normalized aperture margin at any gate crossing (1 = center,
252    /// 0 = edge); `INFINITY` if no gate was crossed.
253    pub min_aperture_margin: f64,
254    /// Mean tilt angle \[rad\] of the thrust axis from world up.
255    pub mean_tilt: f64,
256    /// Peak tilt angle \[rad\] of the thrust axis from world up.
257    pub max_tilt: f64,
258    /// Mean commanded body-rate magnitude \[rad/s\].
259    pub mean_body_rate: f64,
260    /// Time \[s\] of the first completed lap, or `None`.
261    pub first_lap_time: Option<f64>,
262    /// Executed positions, including the start.
263    pub path: Vec<[f64; 3]>,
264}
265
266/// Configuration for the deterministic quadrotor racing MPPI controller.
267#[derive(Debug, Clone, PartialEq)]
268pub struct QuadrotorMppiConfig {
269    pub horizon: usize,
270    pub samples: usize,
271    pub dt: f64,
272    /// Std-dev of thrust perturbations \[m/s^2\].
273    pub thrust_sigma: f64,
274    /// Std-dev of body-rate perturbations \[rad/s\].
275    pub rate_sigma: f64,
276    /// Weight on the squared speed regularizer.
277    pub velocity_weight: f64,
278    /// Weight on the squared body-rate regularizer.
279    pub rate_weight: f64,
280    /// Weight penalizing tilt (keeps the drone from flipping under noise).
281    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/// Result of one `plan` call.
343#[derive(Debug, Clone, PartialEq)]
344pub struct QuadrotorMppiPlan {
345    pub control: QuadrotorControl,
346    /// Lowest rollout cost among the samples (diagnostic).
347    pub best_cost: f64,
348    /// Normalized effective sample size in \[0, 1\] (diagnostic).
349    pub normalized_effective_sample_size: f64,
350}
351
352/// Deterministic, seeded MPPI controller for quadrotor gate racing.
353#[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    /// Roll out the current warm-start sequence from `start`.
379    ///
380    /// After [`Self::plan`], the sequence has shifted past the returned command.
381    /// Pass the state *after applying that command* to visualize the remaining
382    /// controller horizon. This does not sample, advance the RNG, or replan.
383    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            // 1 - cos(tilt) is 0 when level and grows as the drone tips over.
401            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    /// Plan the next control given the current state, lap, and pass count.
411    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        // Warm-start: shift the nominal forward, repeat the last command.
491        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
509/// Drive the quadrotor racing MPPI controller around a lap and report
510/// lap-progress and attitude metrics.
511///
512/// The rollout runs for at most `max_steps` control steps and stops early once
513/// `target_laps` whole laps have been completed. Everything is deterministic for
514/// a fixed `config.seed`.
515pub 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
623// --- Quaternion and vector helpers (kept private to avoid a math dependency).
624
625/// Rotate a vector by a unit quaternion `[w, x, y, z]` (body to world).
626fn rotate(q: [f64; 4], v: [f64; 3]) -> [f64; 3] {
627    let [w, x, y, z] = q;
628    // v + 2 * qv x (qv x v + w v), with qv = (x, y, z).
629    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
639/// Integrate attitude one step from body rates: q + 0.5 * (q (x) [0, w]) * dt.
640fn 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    // Quaternion product q (x) [0, wx, wy, wz].
644    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        // Thrust balances gravity, so it stays put.
738        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        // Pitch about the body y-axis to tilt the thrust vector toward +x.
748        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        // Now hold attitude and apply extra thrust; horizontal velocity grows.
757        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        // Flying through gates requires tilting the thrust vector.
805        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}