feat: 初始化

crates/mecanum/Cargo.toml

@@ -0,0 +1,16 @@
+[package]
+name = "mecanum"
+version.workspace = true
+edition.workspace = true
+rust-version.workspace = true
+license.workspace = true
+description = "Mecanum wheel chassis forward/inverse kinematics"
+
+[lints]
+workspace = true
+
+[dependencies]
+
+[dev-dependencies]
+proptest = { workspace = true }
+approx = { workspace = true }

crates/mecanum/proptest-regressions/lib.txt

@@ -0,0 +1,7 @@
+# Seeds for failure cases proptest has generated in the past. It is
+# automatically read and these particular cases re-run before any
+# novel cases are generated.
+#
+# It is recommended to check this file in to source control so that
+# everyone who runs the test benefits from these saved cases.
+cc 698ba63bf948a3702145ece17ed0f014133de0d0b3797fd8bdda514c5d2480a7 # shrinks to vx = 0.0, vy = 0.0, omega = -2.3901656

crates/mecanum/src/lib.rs

@@ -0,0 +1,284 @@
+//! 麦克纳姆轮底盘正 / 逆运动学。
+//!
+//! # 约定
+//!
+//! - 机器人坐标系（ISO）：x 前、y 左、ω 绕 z 逆时针为正；
+//! - 轮序固定为 `[前左, 前右, 后左, 后右]`（常量 [`FL`] / [`FR`] / [`RL`] /
+//!   [`RR`]）；
+//! - 辊子 45°、俯视呈 X 型布置（RoboMaster 底盘标准布局）；
+//! - 角速度单位 rad/s，线速度 m/s，换算见 [`wheel_rpm_from_angular`]。
+//!
+//! 四轮线速度（`a = half_length + half_width`）：
+//!
+//! ```text
+//! v_FL = vx - vy - a·ω
+//! v_FR = vx + vy + a·ω
+//! v_RL = vx + vy - a·ω
+//! v_RR = vx - vy + a·ω
+//! ```
+//!
+//! 正解为逆解的左伪逆（JᵀJ 对角，可解析求平均）。
+
+use std::f32::consts::TAU;
+
+/// 前左轮下标。
+pub const FL: usize = 0;
+/// 前右轮下标。
+pub const FR: usize = 1;
+/// 后左轮下标。
+pub const RL: usize = 2;
+/// 后右轮下标。
+pub const RR: usize = 3;
+
+/// 底盘平面速度：x 前进（m/s）、y 左移（m/s）、ω 逆时针（rad/s）。
+#[derive(Debug, Clone, Copy, PartialEq, Default)]
+pub struct Twist {
+    /// 前进速度（m/s，前正）。
+    pub vx: f32,
+    /// 横移速度（m/s，左正）。
+    pub vy: f32,
+    /// 角速度（rad/s，逆时针正）。
+    pub omega: f32,
+}
+
+impl Twist {
+    /// 全零速度。
+    pub const ZERO: Self = Self { vx: 0.0, vy: 0.0, omega: 0.0 };
+
+    /// 幅值限幅：任一分量超界时按比例收缩整体，保持运动方向不变。
+    ///
+    /// `max_forward` / `max_strafe` 单位 m/s，`max_spin` 单位 rad/s。
+    #[must_use]
+    pub fn clamped(self, max_forward: f32, max_strafe: f32, max_spin: f32) -> Self {
+        let scale = [
+            (self.vx.abs() / max_forward).max(f32::EPSILON),
+            (self.vy.abs() / max_strafe).max(f32::EPSILON),
+            (self.omega.abs() / max_spin).max(f32::EPSILON),
+        ]
+        .into_iter()
+        .fold(1.0f32, f32::max);
+        Self { vx: self.vx / scale, vy: self.vy / scale, omega: self.omega / scale }
+    }
+}
+
+/// 麦轮底盘几何参数。
+#[derive(Debug, Clone, Copy, PartialEq)]
+pub struct Mecanum {
+    /// 前后轮轴线到底盘中心的纵向半距 lx（米，正）。
+    pub half_length: f32,
+    /// 左右轮到底盘中心的横向半距 ly（米，正）。
+    pub half_width: f32,
+    /// 车轮半径（米，正）。
+    pub wheel_radius: f32,
+}
+
+impl Mecanum {
+    /// 按几何参数构造。
+    ///
+    /// # Panics
+    ///
+    /// debug 构建下参数非正时 panic（配置层应先行校验）。
+    #[must_use]
+    pub fn new(half_length: f32, half_width: f32, wheel_radius: f32) -> Self {
+        debug_assert!(
+            half_length > 0.0 && half_width > 0.0 && wheel_radius > 0.0,
+            "mecanum geometry must be positive"
+        );
+        Self { half_length, half_width, wheel_radius }
+    }
+
+    /// 逆解：底盘速度 → 四轮角速度（rad/s，顺序 FL/FR/RL/RR）。
+    #[must_use]
+    pub fn wheel_speeds(&self, twist: &Twist) -> [f32; 4] {
+        let a = self.half_length + self.half_width;
+        [
+            twist.vx - twist.vy - a * twist.omega,
+            twist.vx + twist.vy + a * twist.omega,
+            twist.vx + twist.vy - a * twist.omega,
+            twist.vx - twist.vy + a * twist.omega,
+        ]
+        .map(|v| v / self.wheel_radius)
+    }
+
+    /// 正解：四轮角速度（rad/s）→ 底盘速度（最小二乘意义）。
+    #[must_use]
+    pub fn twist(&self, wheels: &[f32; 4]) -> Twist {
+        let w = wheels.map(|x| x * self.wheel_radius);
+        let a = self.half_length + self.half_width;
+        Twist {
+            vx: (w[FL] + w[FR] + w[RL] + w[RR]) / 4.0,
+            vy: (-w[FL] + w[FR] + w[RL] - w[RR]) / 4.0,
+            omega: (-w[FL] + w[FR] - w[RL] + w[RR]) / (4.0 * a),
+        }
+    }
+
+    /// 逆解并直接输出四轮转速（rpm）。
+    #[must_use]
+    pub fn wheel_rpms(&self, twist: &Twist) -> [f32; 4] {
+        self.wheel_speeds(twist).map(wheel_rpm_from_angular)
+    }
+
+    /// 由四轮转速（rpm）正解底盘速度。
+    #[must_use]
+    pub fn twist_from_wheel_rpms(&self, wheel_rpms: &[f32; 4]) -> Twist {
+        self.twist(&wheel_rpms.map(wheel_angular_from_rpm))
+    }
+}
+
+/// 轮角速度（rad/s）→ 转速（rpm）。
+#[must_use]
+pub const fn wheel_rpm_from_angular(rad_per_s: f32) -> f32 {
+    rad_per_s * 60.0 / TAU
+}
+
+/// 轮转速（rpm）→ 角速度（rad/s）。
+#[must_use]
+pub const fn wheel_angular_from_rpm(rpm: f32) -> f32 {
+    rpm * TAU / 60.0
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use approx::assert_relative_eq;
+
+    /// 0.2m 半轴距 / 0.25m 半轮距 / 76.2mm 轮半径的典型 RM 底盘。
+    fn chassis() -> Mecanum {
+        Mecanum::new(0.2, 0.25, 0.0762)
+    }
+
+    fn assert_twist_eq(a: &Twist, b: &Twist) {
+        assert_relative_eq!(a.vx, b.vx, epsilon = 1e-5);
+        assert_relative_eq!(a.vy, b.vy, epsilon = 1e-5);
+        assert_relative_eq!(a.omega, b.omega, epsilon = 1e-5);
+    }
+
+    #[test]
+    fn pure_forward_drives_all_wheels_equally() {
+        let m = chassis();
+        let w = m.wheel_speeds(&Twist { vx: 1.0, ..Twist::ZERO });
+        for x in w {
+            assert_relative_eq!(x, 1.0 / 0.0762, epsilon = 1e-5);
+        }
+    }
+
+    #[test]
+    fn pure_strafe_left_opposes_diagonal_wheels() {
+        let m = chassis();
+        let w = m.wheel_speeds(&Twist { vy: 2.0, ..Twist::ZERO });
+        let v = 2.0 / 0.0762;
+        assert_relative_eq!(w[FL], -v, epsilon = 1e-5);
+        assert_relative_eq!(w[FR], v, epsilon = 1e-5);
+        assert_relative_eq!(w[RL], v, epsilon = 1e-5);
+        assert_relative_eq!(w[RR], -v, epsilon = 1e-5);
+    }
+
+    #[test]
+    fn pure_rotation_splits_left_right() {
+        let m = chassis();
+        let w = m.wheel_speeds(&Twist { omega: 1.0, ..Twist::ZERO });
+        let v = (0.2 + 0.25) / 0.0762;
+        // 逆时针：左侧后退、右侧前进。
+        assert_relative_eq!(w[FL], -v, epsilon = 1e-5);
+        assert_relative_eq!(w[FR], v, epsilon = 1e-5);
+        assert_relative_eq!(w[RL], -v, epsilon = 1e-5);
+        assert_relative_eq!(w[RR], v, epsilon = 1e-5);
+    }
+
+    #[test]
+    fn diagonal_motion_idles_two_wheels() {
+        // 前左 45° 对角线：vx = vy ⇒ FL、RR 停转。
+        let m = chassis();
+        let w = m.wheel_speeds(&Twist { vx: 1.0, vy: 1.0, omega: 0.0 });
+        assert_relative_eq!(w[FL], 0.0, epsilon = 1e-6);
+        assert_relative_eq!(w[RR], 0.0, epsilon = 1e-6);
+        assert_relative_eq!(w[FR], 2.0 / 0.0762, epsilon = 1e-5);
+        assert_relative_eq!(w[RL], 2.0 / 0.0762, epsilon = 1e-5);
+    }
+
+    #[test]
+    fn zero_twist_zero_wheels() {
+        let wheels = chassis().wheel_speeds(&Twist::ZERO);
+        assert!(wheels.iter().all(|w| f64::from(*w).abs() < 1e-12), "{wheels:?}");
+    }
+
+    #[test]
+    fn inverse_then_forward_recovers_twist() {
+        let m = chassis();
+        let t = Twist { vx: 1.5, vy: -0.8, omega: 2.2 };
+        assert_twist_eq(&m.twist(&m.wheel_speeds(&t)), &t);
+    }
+
+    #[test]
+    fn rpm_helpers_roundtrip() {
+        assert_relative_eq!(
+            wheel_rpm_from_angular(wheel_angular_from_rpm(482.0)),
+            482.0,
+            epsilon = 1e-4
+        );
+        assert_relative_eq!(wheel_angular_from_rpm(60.0 / TAU), 1.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn rpm_path_matches_angular_path() {
+        let m = chassis();
+        let t = Twist { vx: 0.3, vy: 1.1, omega: -0.7 };
+        assert_twist_eq(&m.twist_from_wheel_rpms(&m.wheel_rpms(&t)), &t);
+    }
+
+    #[test]
+    fn clamp_preserves_direction_and_limits() {
+        let t = Twist { vx: 4.0, vy: 1.0, omega: 0.5 }.clamped(2.0, 2.0, 3.0);
+        assert_relative_eq!(t.vx, 2.0, epsilon = 1e-6);
+        assert_relative_eq!(t.vy, 0.5, epsilon = 1e-6);
+        assert_relative_eq!(t.omega, 0.25, epsilon = 1e-6);
+        // 限内不缩放。
+        let keep = Twist { vx: 1.0, vy: -1.0, omega: 2.0 }.clamped(2.0, 2.0, 3.0);
+        assert_eq!(keep, Twist { vx: 1.0, vy: -1.0, omega: 2.0 });
+    }
+}
+
+#[cfg(test)]
+mod proptests {
+    use super::*;
+    use approx::assert_relative_eq;
+    use proptest::prelude::*;
+
+    proptest! {
+        /// 正逆解互逆：forward(inverse(t)) == t（对任意合理几何与速度）。
+        #[test]
+        fn kinematics_roundtrip(
+            lx in 0.05f32..0.5,
+            ly in 0.05f32..0.5,
+            r in 0.03f32..0.2,
+            vx in -3.0f32..3.0,
+            vy in -3.0f32..3.0,
+            omega in -6.0f32..6.0,
+        ) {
+            let m = Mecanum::new(lx, ly, r);
+            let t = Twist { vx, vy, omega };
+            let back = m.twist(&m.wheel_speeds(&t));
+            assert_relative_eq!(back.vx, t.vx, epsilon = 1e-4);
+            assert_relative_eq!(back.vy, t.vy, epsilon = 1e-4);
+            assert_relative_eq!(back.omega, t.omega, epsilon = 1e-4);
+        }
+
+        /// 左右镜像对称：vy、ω 取反（镜像底盘）等价于 FL↔FR、RL↔RR 对调。
+        #[test]
+        fn strafe_symmetry(
+            vx in -2.0f32..2.0,
+            vy in -2.0f32..2.0,
+            omega in -4.0f32..4.0,
+        ) {
+            let m = Mecanum::new(0.2, 0.25, 0.0762);
+            let t = Twist { vx, vy, omega };
+            let mirrored = Twist { vx, vy: -vy, omega: -omega };
+            let w = m.wheel_speeds(&t);
+            let w2 = m.wheel_speeds(&mirrored);
+            assert_relative_eq!(w[FL], w2[FR], epsilon = 1e-4);
+            assert_relative_eq!(w[FR], w2[FL], epsilon = 1e-4);
+            assert_relative_eq!(w[RL], w2[RR], epsilon = 1e-4);
+            assert_relative_eq!(w[RR], w2[RL], epsilon = 1e-4);
+        }
+    }
+}