From 38441c9778c56fe29b988a1d0c38c2de970ad3c3 Mon Sep 17 00:00:00 2001 From: eunwoo song Date: Sun, 6 Sep 2026 19:00:28 +0900 Subject: [PATCH] Scale multirotor rank validation --- CHANGELOG.md | 4 ++ .../multicalc/src/plant/multirotor_mixing.rs | 13 +++-- .../tests/suite/plant/multirotor_mixing.rs | 48 +++++++++++++++++++ 3 files changed, 61 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6aa4004d..a7600ecb 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -119,6 +119,10 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Fixed +- **Scale-aware multirotor rank validation.** The allocation round-trip tolerance now follows the + scalar epsilon and matrix condition scale, so large valid layouts receive the same answer at + `f32` and `f64` while rank-deficient layouts remain rejected. + - **Large-circle occupancy rims.** `occupy_circle` now derives its sample count from a maximum arc length relative to grid resolution, replacing its fixed minimum angular step so large rims remain closed to ray casts. Non-finite radii now leave the map unchanged. (#296) diff --git a/crates/multicalc/src/plant/multirotor_mixing.rs b/crates/multicalc/src/plant/multirotor_mixing.rs index 8b71e993..0dbe87db 100644 --- a/crates/multicalc/src/plant/multirotor_mixing.rs +++ b/crates/multicalc/src/plant/multirotor_mixing.rs @@ -125,6 +125,8 @@ impl MultirotorMixer { /// [`PlantError::Linalg`] if the layout cannot be inverted, or /// [`PlantError::RotorLayoutNotIndependent`] if the rotors cannot produce every wanted push /// and turn — which is what happens with fewer than four rotors, or with rotors all in a line. + /// The independence check accounts for the scalar precision and the allocation's numerical + /// scale, so equivalent layouts receive the same answer at `f32` and `f64`. pub fn new( positions: [Vector3D; ROTOR_COUNT], spins: [RotorSpin; ROTOR_COUNT], @@ -161,11 +163,14 @@ impl MultirotorMixer { let distribution = allocation.pseudo_inverse()?; // Going out to the rotors and back has to land where it started; when it does not, some - // wanted push or turn is one the rotors simply cannot produce. The bar is loose enough - // that single precision passes and tight enough that a missing direction, which is off by - // about one, always fails. + // wanted push or turn is one the rotors simply cannot produce. Matrix multiplication error + // grows with the scalar epsilon, the inner dimension, and the norms of both factors. Cap + // the resulting forward-error bound below the order-one error of a missing direction. let round_trip = allocation * distribution; - let bar = T::from_f64(1e-4); + let operation_count = T::from_usize(ROTOR_COUNT.max(4)); + let condition_scale = allocation.frobenius_norm() * distribution.frobenius_norm(); + let bar = + (T::EPSILON * operation_count * condition_scale.max(T::ONE)).min(T::from_f64(0.25)); for row in 0..4 { for col in 0..4 { let wanted = if row == col { T::ONE } else { T::ZERO }; diff --git a/crates/multicalc/tests/suite/plant/multirotor_mixing.rs b/crates/multicalc/tests/suite/plant/multirotor_mixing.rs index df50cb98..96eabc4c 100644 --- a/crates/multicalc/tests/suite/plant/multirotor_mixing.rs +++ b/crates/multicalc/tests/suite/plant/multirotor_mixing.rs @@ -252,6 +252,54 @@ fn single_precision_round_trips_too() { assert!(produced.torque().norm() < 1e-4); } +fn assert_scaled_layout_is_accepted() { + let mixer = MultirotorMixer::<4, T>::quadrotor_x( + T::from_f64(15_000.0), + T::from_f64(1_600.0), + T::from_f64(MINIMUM_THRUST), + T::from_f64(MAXIMUM_THRUST), + ); + assert!(mixer.is_ok()); +} + +#[test] +fn scaled_layout_is_accepted_at_both_precisions() { + assert_scaled_layout_is_accepted::(); + assert_scaled_layout_is_accepted::(); +} + +fn assert_rank_deficient_layout_is_refused() { + let positions = [ + Vector::new([T::from_f64(0.1), T::ZERO, T::ZERO]), + Vector::new([T::from_f64(0.2), T::ZERO, T::ZERO]), + Vector::new([T::from_f64(-0.1), T::ZERO, T::ZERO]), + Vector::new([T::from_f64(-0.2), T::ZERO, T::ZERO]), + ]; + let spins = [ + RotorSpin::Clockwise, + RotorSpin::CounterClockwise, + RotorSpin::Clockwise, + RotorSpin::CounterClockwise, + ]; + + assert_eq!( + MultirotorMixer::<4, T>::new( + positions, + spins, + T::from_f64(TORQUE_PER_THRUST), + T::from_f64(MINIMUM_THRUST), + T::from_f64(MAXIMUM_THRUST), + ), + Err(PlantError::RotorLayoutNotIndependent) + ); +} + +#[test] +fn rank_deficient_layout_is_refused_at_both_precisions() { + assert_rank_deficient_layout_is_refused::(); + assert_rank_deficient_layout_is_refused::(); +} + #[test] fn a_mixer_drives_a_body() { let mass = 0.8;