rust_robotics_control/
racing_mppi_motor.rs

1//! Reference-free racing MPPI with a motor-level rotor-mixing quadrotor.
2//!
3//! This deepens [`crate::racing_mppi_quadrotor`] one more level: instead of
4//! commanding body rates directly, the control input is the four rotor thrusts.
5//! A rotor-mixing map turns them into a collective thrust and body torques, the
6//! body angular velocity becomes a *state* driven by those torques (with light
7//! aerodynamic rate damping), and every rotor saturates at a maximum thrust.
8//!
9//! That saturation is the point: a rotor cannot supply maximum collective lift
10//! and a large differential torque at the same time, so demanding an aggressive
11//! attitude change *steals thrust authority*. A thrust-limited drone therefore
12//! saturates its rotors more often and is genuinely less agile — a trade-off the
13//! body-rate model (which commands rates for free) cannot express.
14//!
15//! - [`MotorQuadState`] carries position, velocity, a unit-quaternion attitude,
16//!   and the body angular velocity.
17//! - [`MotorQuadParams`] integrates the rotor-mixing rotational dynamics
18//!   (control-affine, with the inertia folded into roll/pitch/yaw gains and a
19//!   rate-damping term) and the rigid-body translational dynamics.
20//! - [`MotorMppiController`] samples per-rotor thrust perturbations around a
21//!   hover nominal and scores the resulting positions with the reference-free
22//!   gate-progress objective from [`crate::racing_mppi_3d`].
23//! - [`simulate_motor_race`] reports [`MotorRacingLapReport`], which adds a rotor
24//!   saturation fraction to the lap-progress and attitude metrics.
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 motor-level quadrotor.
32#[derive(Debug, Clone, Copy, PartialEq)]
33pub struct MotorQuadState {
34    pub position: [f64; 3],
35    pub velocity: [f64; 3],
36    /// Unit quaternion `[w, x, y, z]` (body-to-world).
37    pub attitude: [f64; 4],
38    /// Body angular velocity `[wx, wy, wz]` \[rad/s\].
39    pub body_rates: [f64; 3],
40}
41
42impl MotorQuadState {
43    /// Level hover state at the given position.
44    pub fn at(x: f64, y: f64, z: f64) -> Self {
45        Self {
46            position: [x, y, z],
47            velocity: [0.0, 0.0, 0.0],
48            attitude: [1.0, 0.0, 0.0, 0.0],
49            body_rates: [0.0, 0.0, 0.0],
50        }
51    }
52
53    pub fn speed(self) -> f64 {
54        norm(self.velocity)
55    }
56
57    /// Body z-axis (thrust direction) in the world frame.
58    pub fn thrust_axis(self) -> [f64; 3] {
59        rotate(self.attitude, [0.0, 0.0, 1.0])
60    }
61
62    /// Tilt angle \[rad\] of the thrust axis from world up, in `[0, pi]`.
63    pub fn tilt_angle(self) -> f64 {
64        self.thrust_axis()[2].clamp(-1.0, 1.0).acos()
65    }
66}
67
68/// Four rotor thrust commands `[f0, f1, f2, f3]` in mass-normalized accel units.
69///
70/// Rotors are laid out in an X configuration: `f0` front-right, `f1`
71/// front-left, `f2` rear-left, `f3` rear-right (the exact labeling only matters
72/// through the fixed mixing signs below).
73#[derive(Debug, Clone, Copy, PartialEq)]
74pub struct MotorCommand {
75    pub rotors: [f64; 4],
76}
77
78impl MotorCommand {
79    pub fn new(rotors: [f64; 4]) -> Self {
80        Self { rotors }
81    }
82
83    /// Even hover command: every rotor carries a quarter of gravity.
84    pub fn hover(gravity: f64) -> Self {
85        Self {
86            rotors: [gravity / 4.0; 4],
87        }
88    }
89
90    /// Collective (mass-normalized) thrust \[m/s^2\].
91    pub fn collective(self) -> f64 {
92        self.rotors.iter().sum()
93    }
94}
95
96/// Motor-level quadrotor parameters and actuation limits.
97#[derive(Debug, Clone, Copy, PartialEq)]
98pub struct MotorQuadParams {
99    /// Downward gravitational acceleration \[m/s^2\].
100    pub gravity: f64,
101    /// Linear aerodynamic drag coefficient \[1/s\].
102    pub drag: f64,
103    /// Maximum thrust per rotor \[m/s^2\] (mass-normalized).
104    pub max_rotor_thrust: f64,
105    /// Roll/pitch angular-acceleration gain per unit rotor-thrust difference.
106    pub torque_gain: f64,
107    /// Yaw angular-acceleration gain per unit rotor-thrust difference.
108    pub yaw_gain: f64,
109    /// Aerodynamic body-rate damping \[1/s\].
110    pub rate_damping: f64,
111    /// Maximum speed \[m/s\].
112    pub max_speed: f64,
113}
114
115impl Default for MotorQuadParams {
116    fn default() -> Self {
117        Self {
118            gravity: 9.81,
119            drag: 0.3,
120            max_rotor_thrust: 6.0,
121            torque_gain: 9.0,
122            yaw_gain: 2.0,
123            rate_damping: 1.2,
124            max_speed: 7.0,
125        }
126    }
127}
128
129impl MotorQuadParams {
130    pub fn new(
131        gravity: f64,
132        drag: f64,
133        max_rotor_thrust: f64,
134        torque_gain: f64,
135        yaw_gain: f64,
136        rate_damping: f64,
137        max_speed: f64,
138    ) -> RoboticsResult<Self> {
139        let finite_nonneg = |v: f64| v.is_finite() && v >= 0.0;
140        if !finite_nonneg(gravity) || !finite_nonneg(drag) || !finite_nonneg(rate_damping) {
141            return Err(RoboticsError::InvalidParameter(
142                "motor quad gravity/drag/rate_damping must be finite and non-negative".to_string(),
143            ));
144        }
145        if !max_rotor_thrust.is_finite() || max_rotor_thrust <= 0.0 {
146            return Err(RoboticsError::InvalidParameter(
147                "motor quad max_rotor_thrust must be finite and positive".to_string(),
148            ));
149        }
150        if !torque_gain.is_finite()
151            || torque_gain <= 0.0
152            || !yaw_gain.is_finite()
153            || yaw_gain <= 0.0
154        {
155            return Err(RoboticsError::InvalidParameter(
156                "motor quad torque gains must be finite and positive".to_string(),
157            ));
158        }
159        if !max_speed.is_finite() || max_speed <= 0.0 {
160            return Err(RoboticsError::InvalidParameter(
161                "motor quad max_speed must be finite and positive".to_string(),
162            ));
163        }
164        // Gravity must be hoverable within the rotor limit.
165        if gravity > 4.0 * max_rotor_thrust {
166            return Err(RoboticsError::InvalidParameter(
167                "motor quad cannot hover: gravity exceeds 4 * max_rotor_thrust".to_string(),
168            ));
169        }
170        Ok(Self {
171            gravity,
172            drag,
173            max_rotor_thrust,
174            torque_gain,
175            yaw_gain,
176            rate_damping,
177            max_speed,
178        })
179    }
180
181    /// Clamp each rotor thrust to `[0, max_rotor_thrust]`.
182    pub fn saturate(self, command: MotorCommand) -> MotorCommand {
183        let mut rotors = command.rotors;
184        for r in &mut rotors {
185            *r = r.clamp(0.0, self.max_rotor_thrust);
186        }
187        MotorCommand::new(rotors)
188    }
189
190    /// Whether any rotor of the command is at (or above) its thrust limit.
191    pub fn is_saturated(self, command: MotorCommand) -> bool {
192        command
193            .rotors
194            .iter()
195            .any(|&r| r >= self.max_rotor_thrust - 1e-9 || r <= 1e-9)
196    }
197
198    /// Body torques (as angular accelerations) from the rotor mix.
199    fn angular_accel_from_rotors(self, rotors: [f64; 4]) -> [f64; 3] {
200        let [f0, f1, f2, f3] = rotors;
201        // X-mixer (inertia folded into the gains):
202        //   roll  : left rotors (f1,f2) vs right rotors (f0,f3)
203        //   pitch : front rotors (f0,f1) vs rear rotors (f2,f3)
204        //   yaw   : one diagonal (f0,f2) vs the other (f1,f3)
205        let roll = self.torque_gain * ((f1 + f2) - (f0 + f3));
206        let pitch = self.torque_gain * ((f0 + f1) - (f2 + f3));
207        let yaw = self.yaw_gain * ((f0 + f2) - (f1 + f3));
208        [roll, pitch, yaw]
209    }
210
211    /// Advance the quadrotor one step under the rotor command.
212    pub fn step(self, state: MotorQuadState, command: MotorCommand, dt: f64) -> MotorQuadState {
213        let command = self.saturate(command);
214
215        // Rotational dynamics: rate = rate + (torque - damping*rate) * dt.
216        let torque = self.angular_accel_from_rotors(command.rotors);
217        let mut body_rates = [0.0; 3];
218        for i in 0..3 {
219            let rate_dot = torque[i] - self.rate_damping * state.body_rates[i];
220            body_rates[i] = state.body_rates[i] + rate_dot * dt;
221        }
222
223        // Attitude kinematics from the (updated) body rates.
224        let attitude = normalize_quat(integrate_attitude(state.attitude, body_rates, dt));
225
226        // Translational dynamics.
227        let thrust = command.collective();
228        let thrust_axis = rotate(attitude, [0.0, 0.0, 1.0]);
229        let accel = [
230            thrust * thrust_axis[0] - self.drag * state.velocity[0],
231            thrust * thrust_axis[1] - self.drag * state.velocity[1],
232            thrust * thrust_axis[2] - self.gravity - self.drag * state.velocity[2],
233        ];
234        let mut velocity = [
235            state.velocity[0] + accel[0] * dt,
236            state.velocity[1] + accel[1] * dt,
237            state.velocity[2] + accel[2] * dt,
238        ];
239        let speed = norm(velocity);
240        if speed > self.max_speed && speed > 0.0 {
241            let scale = self.max_speed / speed;
242            velocity = [
243                velocity[0] * scale,
244                velocity[1] * scale,
245                velocity[2] * scale,
246            ];
247        }
248        let position = [
249            state.position[0] + velocity[0] * dt,
250            state.position[1] + velocity[1] * dt,
251            state.position[2] + velocity[2] * dt,
252        ];
253
254        MotorQuadState {
255            position,
256            velocity,
257            attitude,
258            body_rates,
259        }
260    }
261}
262
263/// Lap-progress, attitude, and motor-saturation metrics.
264#[derive(Debug, Clone, PartialEq)]
265pub struct MotorRacingLapReport {
266    pub steps: usize,
267    pub gates_passed: usize,
268    pub laps_completed: usize,
269    pub lap_fraction: f64,
270    pub mean_speed: f64,
271    pub max_speed: f64,
272    pub path_length: f64,
273    pub gate_distance: f64,
274    pub min_aperture_margin: f64,
275    pub mean_tilt: f64,
276    pub max_tilt: f64,
277    pub max_body_rate: f64,
278    /// Fraction of executed steps whose rotor command hit a thrust limit.
279    pub saturation_fraction: f64,
280    pub first_lap_time: Option<f64>,
281    pub path: Vec<[f64; 3]>,
282}
283
284/// Configuration for the motor-level racing MPPI controller.
285#[derive(Debug, Clone, PartialEq)]
286pub struct MotorMppiConfig {
287    pub horizon: usize,
288    pub samples: usize,
289    pub dt: f64,
290    /// Std-dev of per-rotor thrust perturbations \[m/s^2\].
291    pub rotor_sigma: f64,
292    pub velocity_weight: f64,
293    pub rotor_weight: f64,
294    pub level_weight: f64,
295    pub lambda: f64,
296    pub seed: u64,
297}
298
299impl Default for MotorMppiConfig {
300    fn default() -> Self {
301        Self {
302            horizon: 28,
303            samples: 800,
304            dt: 0.05,
305            rotor_sigma: 2.6,
306            velocity_weight: 0.01,
307            rotor_weight: 0.004,
308            level_weight: 0.6,
309            lambda: 1.0,
310            seed: 7,
311        }
312    }
313}
314
315fn validate_config(config: &MotorMppiConfig) -> RoboticsResult<()> {
316    if config.horizon == 0 || config.samples == 0 {
317        return Err(RoboticsError::InvalidParameter(
318            "motor MPPI horizon and samples must be positive".to_string(),
319        ));
320    }
321    if !config.dt.is_finite() || config.dt <= 0.0 {
322        return Err(RoboticsError::InvalidParameter(
323            "motor MPPI dt must be finite and positive".to_string(),
324        ));
325    }
326    if !config.rotor_sigma.is_finite() || config.rotor_sigma <= 0.0 {
327        return Err(RoboticsError::InvalidParameter(
328            "motor MPPI rotor_sigma must be finite and positive".to_string(),
329        ));
330    }
331    if !config.lambda.is_finite() || config.lambda <= 0.0 {
332        return Err(RoboticsError::InvalidParameter(
333            "motor MPPI lambda must be finite and positive".to_string(),
334        ));
335    }
336    if config.velocity_weight < 0.0 || config.rotor_weight < 0.0 || config.level_weight < 0.0 {
337        return Err(RoboticsError::InvalidParameter(
338            "motor MPPI weights must be non-negative".to_string(),
339        ));
340    }
341    Ok(())
342}
343
344/// Result of one `plan` call.
345#[derive(Debug, Clone, PartialEq)]
346pub struct MotorMppiPlan {
347    pub command: MotorCommand,
348    pub best_cost: f64,
349    pub normalized_effective_sample_size: f64,
350}
351
352/// Deterministic, seeded motor-level MPPI controller for gate racing.
353#[derive(Debug, Clone)]
354pub struct MotorMppiController {
355    config: MotorMppiConfig,
356    params: MotorQuadParams,
357    nominal: Vec<MotorCommand>,
358    rng: StdRng,
359}
360
361impl MotorMppiController {
362    pub fn new(config: MotorMppiConfig, params: MotorQuadParams) -> RoboticsResult<Self> {
363        validate_config(&config)?;
364        let nominal = vec![MotorCommand::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) -> &MotorMppiConfig {
375        &self.config
376    }
377
378    fn rollout(&self, start: MotorQuadState, controls: &[MotorCommand]) -> (Vec<[f64; 3]>, f64) {
379        let mut state = start;
380        let mut positions = Vec::with_capacity(controls.len() + 1);
381        positions.push(state.position);
382        let mut regularizer = 0.0;
383        for &command in controls {
384            state = self.params.step(state, command, self.config.dt);
385            let speed = state.speed();
386            let tilt_cost = 1.0 - state.thrust_axis()[2];
387            // Penalize rotor effort away from the even hover thrust.
388            let hover = self.params.gravity / 4.0;
389            let rotor_effort: f64 = command.rotors.iter().map(|&r| (r - hover).powi(2)).sum();
390            regularizer += self.config.velocity_weight * speed * speed
391                + self.config.rotor_weight * rotor_effort
392                + self.config.level_weight * tilt_cost;
393            positions.push(state.position);
394        }
395        (positions, regularizer)
396    }
397
398    /// Plan the next rotor command given state, lap, and pass count.
399    pub fn plan(
400        &mut self,
401        start: MotorQuadState,
402        lap: &RacingGateLap3D,
403        passed: usize,
404    ) -> RoboticsResult<MotorMppiPlan> {
405        let noise = Normal::new(0.0, self.config.rotor_sigma).map_err(|_| {
406            RoboticsError::InvalidParameter("invalid motor MPPI rotor distribution".to_string())
407        })?;
408
409        let mut costs = Vec::with_capacity(self.config.samples);
410        let mut sequences = Vec::with_capacity(self.config.samples);
411        let mut best_cost = f64::INFINITY;
412
413        for _ in 0..self.config.samples {
414            let mut controls = Vec::with_capacity(self.config.horizon);
415            for &base in &self.nominal {
416                let mut rotors = base.rotors;
417                for r in &mut rotors {
418                    *r += noise.sample(&mut self.rng);
419                }
420                controls.push(self.params.saturate(MotorCommand::new(rotors)));
421            }
422            let (positions, regularizer) = self.rollout(start, &controls);
423            let (gate_cost, _) = lap.score_positions(&positions, passed);
424            let cost = gate_cost + regularizer;
425            best_cost = best_cost.min(cost);
426            costs.push(cost);
427            sequences.push(controls);
428        }
429
430        let min_cost = costs.iter().copied().fold(f64::INFINITY, f64::min);
431        let mut weights = Vec::with_capacity(costs.len());
432        let mut weight_sum = 0.0;
433        for &cost in &costs {
434            let weight = (-(cost - min_cost) / self.config.lambda).exp();
435            weight_sum += weight;
436            weights.push(weight);
437        }
438
439        if weight_sum <= 0.0 || !weight_sum.is_finite() {
440            let best_index = costs
441                .iter()
442                .enumerate()
443                .min_by(|a, b| a.1.partial_cmp(b.1).unwrap_or(std::cmp::Ordering::Equal))
444                .map(|(index, _)| index)
445                .unwrap_or(0);
446            let command = sequences[best_index][0];
447            return Ok(MotorMppiPlan {
448                command,
449                best_cost,
450                normalized_effective_sample_size: 0.0,
451            });
452        }
453
454        let mut updated = vec![MotorCommand::new([0.0; 4]); self.config.horizon];
455        let mut sum_sq = 0.0;
456        for (weight, controls) in weights.iter().zip(&sequences) {
457            let normalized = weight / weight_sum;
458            sum_sq += normalized * normalized;
459            for (acc, command) in updated.iter_mut().zip(controls) {
460                for k in 0..4 {
461                    acc.rotors[k] += normalized * command.rotors[k];
462                }
463            }
464        }
465        for command in &mut updated {
466            *command = self.params.saturate(*command);
467        }
468
469        let first_command = updated[0];
470        self.nominal.clear();
471        self.nominal.extend_from_slice(&updated[1..]);
472        self.nominal.push(*updated.last().unwrap());
473
474        let normalized_effective_sample_size = if sum_sq > 0.0 {
475            1.0 / (sum_sq * self.config.samples as f64)
476        } else {
477            0.0
478        };
479
480        Ok(MotorMppiPlan {
481            command: first_command,
482            best_cost,
483            normalized_effective_sample_size,
484        })
485    }
486}
487
488/// Drive the motor-level racing MPPI controller around a lap and report metrics.
489pub fn simulate_motor_race(
490    config: MotorMppiConfig,
491    params: MotorQuadParams,
492    lap: &RacingGateLap3D,
493    start: MotorQuadState,
494    max_steps: usize,
495    target_laps: usize,
496) -> RoboticsResult<MotorRacingLapReport> {
497    let dt = config.dt;
498    let gate_count = lap.gate_count();
499    let mut controller = MotorMppiController::new(config, params)?;
500
501    let mut state = start;
502    let mut passed = 0usize;
503    let mut path = vec![state.position];
504    let mut sum_speed = 0.0;
505    let mut max_speed = 0.0_f64;
506    let mut sum_tilt = 0.0;
507    let mut max_tilt = 0.0_f64;
508    let mut max_body_rate = 0.0_f64;
509    let mut saturated_steps = 0usize;
510    let mut path_length = 0.0;
511    let mut min_aperture_margin = f64::INFINITY;
512    let mut first_lap_time = None;
513    let mut executed_steps = 0usize;
514
515    for step in 0..max_steps {
516        let plan = controller.plan(state, lap, passed)?;
517        let next = params.step(state, plan.command, dt);
518        let from = state.position;
519        let to = next.position;
520
521        loop {
522            let gate = lap.gate_for_pass_count(passed);
523            if !gate.crosses(from, to, lap.crossing_tolerance) {
524                break;
525            }
526            let margin = aperture_crossing_margin(gate, from, to);
527            min_aperture_margin = min_aperture_margin.min(margin);
528            passed += 1;
529            if passed == gate_count && first_lap_time.is_none() {
530                first_lap_time = Some((step + 1) as f64 * dt);
531            }
532            if !lap.closed && passed >= gate_count {
533                break;
534            }
535        }
536
537        path_length += distance(from, to);
538        let speed = next.speed();
539        sum_speed += speed;
540        max_speed = max_speed.max(speed);
541        let tilt = next.tilt_angle();
542        sum_tilt += tilt;
543        max_tilt = max_tilt.max(tilt);
544        max_body_rate = max_body_rate.max(norm(next.body_rates));
545        if params.is_saturated(plan.command) {
546            saturated_steps += 1;
547        }
548        state = next;
549        path.push(to);
550        executed_steps += 1;
551
552        if target_laps > 0 && passed >= target_laps * gate_count {
553            break;
554        }
555        if !lap.closed && passed >= gate_count {
556            break;
557        }
558    }
559
560    let laps_completed = passed / gate_count;
561    let lap_fraction = (passed % gate_count) as f64 / gate_count as f64;
562    let active_gate = lap.gate_for_pass_count(passed);
563    let gate_distance = distance(active_gate.center(), state.position);
564    let denom = executed_steps.max(1) as f64;
565
566    Ok(MotorRacingLapReport {
567        steps: executed_steps,
568        gates_passed: passed,
569        laps_completed,
570        lap_fraction,
571        mean_speed: sum_speed / denom,
572        max_speed,
573        path_length,
574        gate_distance,
575        min_aperture_margin,
576        mean_tilt: sum_tilt / denom,
577        max_tilt,
578        max_body_rate,
579        saturation_fraction: saturated_steps as f64 / denom,
580        first_lap_time,
581        path,
582    })
583}
584
585fn aperture_crossing_margin(gate: &RacingGatePlane3D, from: [f64; 3], to: [f64; 3]) -> f64 {
586    let from_signed = gate.signed_distance(from);
587    let to_signed = gate.signed_distance(to);
588    let denom = to_signed - from_signed;
589    let t = if denom.abs() > 1e-9 {
590        (-from_signed / denom).clamp(0.0, 1.0)
591    } else {
592        0.0
593    };
594    let crossing = [
595        from[0] + t * (to[0] - from[0]),
596        from[1] + t * (to[1] - from[1]),
597        from[2] + t * (to[2] - from[2]),
598    ];
599    gate.aperture_margin(crossing)
600}
601
602// --- Quaternion and vector helpers ---------------------------------------
603
604fn rotate(q: [f64; 4], v: [f64; 3]) -> [f64; 3] {
605    let [w, x, y, z] = q;
606    let qv = [x, y, z];
607    let t = scale(cross(qv, v), 2.0);
608    let cross_t = cross(qv, t);
609    [
610        v[0] + w * t[0] + cross_t[0],
611        v[1] + w * t[1] + cross_t[1],
612        v[2] + w * t[2] + cross_t[2],
613    ]
614}
615
616fn integrate_attitude(q: [f64; 4], rates: [f64; 3], dt: f64) -> [f64; 4] {
617    let [qw, qx, qy, qz] = q;
618    let [wx, wy, wz] = rates;
619    let dw = -(qx * wx + qy * wy + qz * wz);
620    let dx = qw * wx + qy * wz - qz * wy;
621    let dy = qw * wy + qz * wx - qx * wz;
622    let dz = qw * wz + qx * wy - qy * wx;
623    let half_dt = 0.5 * dt;
624    [
625        qw + half_dt * dw,
626        qx + half_dt * dx,
627        qy + half_dt * dy,
628        qz + half_dt * dz,
629    ]
630}
631
632fn normalize_quat(q: [f64; 4]) -> [f64; 4] {
633    let n = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]).sqrt();
634    if !n.is_finite() || n <= 0.0 {
635        return [1.0, 0.0, 0.0, 0.0];
636    }
637    [q[0] / n, q[1] / n, q[2] / n, q[3] / n]
638}
639
640fn cross(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
641    [
642        a[1] * b[2] - a[2] * b[1],
643        a[2] * b[0] - a[0] * b[2],
644        a[0] * b[1] - a[1] * b[0],
645    ]
646}
647
648fn scale(a: [f64; 3], s: f64) -> [f64; 3] {
649    [a[0] * s, a[1] * s, a[2] * s]
650}
651
652fn norm(a: [f64; 3]) -> f64 {
653    (a[0] * a[0] + a[1] * a[1] + a[2] * a[2]).sqrt()
654}
655
656fn distance(a: [f64; 3], b: [f64; 3]) -> f64 {
657    let d = [a[0] - b[0], a[1] - b[1], a[2] - b[2]];
658    norm(d)
659}
660
661#[cfg(test)]
662mod tests {
663    use super::*;
664
665    fn gate(center: [f64; 3], normal: [f64; 3]) -> RacingGatePlane3D {
666        RacingGatePlane3D::new(center, normal, [0.0, 0.0, 1.0], 0.9, 0.9).unwrap()
667    }
668
669    fn straight_lap() -> RacingGateLap3D {
670        let gates = vec![
671            gate([1.5, 0.0, 0.0], [1.0, 0.0, 0.0]),
672            gate([3.0, 0.0, 0.0], [1.0, 0.0, 0.0]),
673            gate([4.5, 0.0, 0.0], [1.0, 0.0, 0.0]),
674        ];
675        RacingGateLap3D::open(gates).unwrap()
676    }
677
678    #[test]
679    fn hover_holds_position() {
680        let params = MotorQuadParams::default();
681        let mut state = MotorQuadState::at(0.0, 0.0, 1.0);
682        for _ in 0..30 {
683            state = params.step(state, MotorCommand::hover(params.gravity), 0.05);
684        }
685        assert!(state.speed() < 1e-6, "speed {}", state.speed());
686        assert!((state.position[2] - 1.0).abs() < 1e-6);
687        assert!(state.tilt_angle() < 1e-9);
688    }
689
690    #[test]
691    fn differential_rotors_build_body_rate_and_tilt() {
692        let params = MotorQuadParams::default();
693        let mut state = MotorQuadState::at(0.0, 0.0, 0.0);
694        let hover = params.gravity / 4.0;
695        // Pitch command: front rotors high, rear rotors low.
696        let command = MotorCommand::new([hover + 1.5, hover + 1.5, hover - 1.5, hover - 1.5]);
697        for _ in 0..6 {
698            state = params.step(state, command, 0.05);
699        }
700        assert!(
701            norm(state.body_rates) > 0.1,
702            "body rate {:?}",
703            state.body_rates
704        );
705        assert!(state.tilt_angle() > 0.05, "tilt {}", state.tilt_angle());
706    }
707
708    #[test]
709    fn rate_damping_bounds_spin() {
710        let params = MotorQuadParams::default();
711        let mut state = MotorQuadState::at(0.0, 0.0, 0.0);
712        let hover = params.gravity / 4.0;
713        let command = MotorCommand::new([hover + 2.0, hover + 2.0, hover - 2.0, hover - 2.0]);
714        for _ in 0..200 {
715            state = params.step(state, command, 0.05);
716        }
717        // Damping caps the steady-state rate at torque / damping, not infinity.
718        assert!(norm(state.body_rates).is_finite());
719        assert!(norm(state.body_rates) < 200.0);
720    }
721
722    #[test]
723    fn saturation_is_detected_and_clamped() {
724        let params = MotorQuadParams::default();
725        let command = MotorCommand::new([100.0, 0.0, 1.0, 1.0]);
726        assert!(params.is_saturated(command));
727        let saturated = params.saturate(command);
728        assert!((saturated.rotors[0] - params.max_rotor_thrust).abs() < 1e-9);
729        assert!(saturated.rotors[1] >= 0.0);
730    }
731
732    #[test]
733    fn rejects_unhoverable_params() {
734        // gravity 9.81 needs 4 * max_rotor >= 9.81 -> max_rotor >= 2.45.
735        assert!(MotorQuadParams::new(9.81, 0.3, 2.0, 9.0, 2.0, 1.2, 7.0).is_err());
736    }
737
738    #[test]
739    fn controller_makes_gate_progress() {
740        let lap = straight_lap();
741        let report = simulate_motor_race(
742            MotorMppiConfig::default(),
743            MotorQuadParams::default(),
744            &lap,
745            MotorQuadState::at(0.0, 0.0, 0.0),
746            140,
747            1,
748        )
749        .unwrap();
750        assert!(
751            report.gates_passed >= 1,
752            "expected at least one gate, got {}",
753            report.gates_passed
754        );
755        assert!(report.max_tilt > 0.05, "max tilt {}", report.max_tilt);
756        assert!((0.0..=1.0).contains(&report.saturation_fraction));
757    }
758
759    #[test]
760    fn simulation_is_deterministic() {
761        let lap = straight_lap();
762        let run = || {
763            simulate_motor_race(
764                MotorMppiConfig::default(),
765                MotorQuadParams::default(),
766                &lap,
767                MotorQuadState::at(0.0, 0.0, 0.0),
768                100,
769                1,
770            )
771            .unwrap()
772        };
773        let first = run();
774        let second = run();
775        assert_eq!(first.path, second.path);
776        assert_eq!(first.gates_passed, second.gates_passed);
777    }
778
779    #[test]
780    fn invalid_config_is_rejected() {
781        let config = MotorMppiConfig {
782            samples: 0,
783            ..MotorMppiConfig::default()
784        };
785        assert!(MotorMppiController::new(config, MotorQuadParams::default()).is_err());
786    }
787}
rust_robotics_control/racing_mppi_motor.rs

rust_robotics_control/
racing_mppi_motor.rs
