pamoja_kit/
motion.rs

1//! Shared planar motion types: a body twist and a world pose.
2
3use core::f32::consts::PI;
4
5/// A planar body velocity: the command a wheeled robot is driven with.
6///
7/// The frame follows the robotics convention (ROS REP-103): x points forward, y points to
8/// the robot's left, and a positive `omega` turns counter-clockwise (toward the left).
9/// Nonholonomic drives (differential, Ackermann, skid-steer) cannot move sideways and ignore
10/// `vy`; holonomic drives (mecanum, omni) use it.
11///
12/// # Examples
13///
14/// ```
15/// use pamoja_kit::Twist;
16///
17/// let forward = Twist::planar(1.0, 0.0); // 1 m/s ahead, no turn
18/// assert_eq!(forward.vy, 0.0);
19/// assert_eq!(Twist::zero(), Twist::new(0.0, 0.0, 0.0));
20/// ```
21#[derive(Clone, Copy, Debug, PartialEq)]
22pub struct Twist {
23    /// Forward speed along the x axis.
24    pub vx: f32,
25    /// Leftward speed along the y axis; zero for drives that cannot strafe.
26    pub vy: f32,
27    /// Yaw rate about the z axis, positive counter-clockwise.
28    pub omega: f32,
29}
30
31impl Twist {
32    /// Creates a twist from its three components.
33    ///
34    /// # Arguments
35    ///
36    /// * `vx` - forward speed.
37    /// * `vy` - leftward speed.
38    /// * `omega` - yaw rate, positive counter-clockwise.
39    ///
40    /// # Returns
41    ///
42    /// The twist.
43    pub fn new(vx: f32, vy: f32, omega: f32) -> Self {
44        Self { vx, vy, omega }
45    }
46
47    /// Creates a planar twist with no sideways motion (`vy = 0`).
48    ///
49    /// # Arguments
50    ///
51    /// * `vx` - forward speed.
52    /// * `omega` - yaw rate, positive counter-clockwise.
53    ///
54    /// # Returns
55    ///
56    /// The twist with `vy` zero.
57    pub fn planar(vx: f32, omega: f32) -> Self {
58        Self { vx, vy: 0.0, omega }
59    }
60
61    /// Returns the zero twist: stopped.
62    ///
63    /// # Returns
64    ///
65    /// A twist whose every component is zero.
66    pub fn zero() -> Self {
67        Self {
68            vx: 0.0,
69            vy: 0.0,
70            omega: 0.0,
71        }
72    }
73}
74
75/// A planar pose in the world frame: position and heading.
76///
77/// # Examples
78///
79/// ```
80/// use pamoja_kit::Pose;
81///
82/// let start = Pose::origin();
83/// assert_eq!((start.x, start.y, start.theta), (0.0, 0.0, 0.0));
84/// ```
85#[derive(Clone, Copy, Debug, PartialEq)]
86pub struct Pose {
87    /// Position along the world x axis, in metres.
88    pub x: f32,
89    /// Position along the world y axis, in metres.
90    pub y: f32,
91    /// Heading from the world x axis, in radians, in `(-pi, pi]`, positive counter-clockwise.
92    pub theta: f32,
93}
94
95impl Pose {
96    /// Creates a pose; the heading is wrapped into `(-pi, pi]`.
97    ///
98    /// # Arguments
99    ///
100    /// * `x` - position along the world x axis.
101    /// * `y` - position along the world y axis.
102    /// * `theta` - heading in radians, wrapped to `(-pi, pi]`.
103    ///
104    /// # Returns
105    ///
106    /// The pose.
107    pub fn new(x: f32, y: f32, theta: f32) -> Self {
108        Self {
109            x,
110            y,
111            theta: wrap_pi(theta),
112        }
113    }
114
115    /// Returns the origin pose: at `(0, 0)` facing along the x axis.
116    ///
117    /// # Returns
118    ///
119    /// The origin pose.
120    pub fn origin() -> Self {
121        Self {
122            x: 0.0,
123            y: 0.0,
124            theta: 0.0,
125        }
126    }
127}
128
129// Wraps an angle in radians into the half-open interval `(-pi, pi]`.
130pub(crate) fn wrap_pi(angle: f32) -> f32 {
131    let two_pi = 2.0 * PI;
132    let mut a = angle % two_pi;
133    if a > PI {
134        a -= two_pi;
135    } else if a <= -PI {
136        a += two_pi;
137    }
138    a
139}
140
141// `f32::abs` lives in `std`, so this `no_std` crate takes the magnitude by hand.
142pub(crate) fn magnitude(value: f32) -> f32 {
143    if value < 0.0 {
144        -value
145    } else {
146        value
147    }
148}
149
150// `f32::clamp` lives in `std`; clamp by hand. Callers pass `low <= high`.
151pub(crate) fn clamp(value: f32, low: f32, high: f32) -> f32 {
152    if value < low {
153        low
154    } else if value > high {
155        high
156    } else {
157        value
158    }
159}
160
161#[cfg(test)]
162mod tests {
163    use super::*;
164
165    #[test]
166    fn wrap_pi_folds_into_the_canonical_interval() {
167        // Interior values pass through; a value past 2pi folds to its coterminal angle.
168        assert!(wrap_pi(0.0).abs() < 1e-6);
169        assert!((wrap_pi(1.0) - 1.0).abs() < 1e-6);
170        assert!((wrap_pi(2.0 * PI + 0.3) - 0.3).abs() < 1e-5);
171        // Whatever the input, the result lands within (-pi, pi].
172        for k in -10..=10 {
173            let w = wrap_pi(k as f32 * 1.3);
174            assert!(w > -PI - 1e-4 && w <= PI + 1e-4);
175        }
176    }
177
178    #[test]
179    fn clamp_and_magnitude_behave() {
180        assert_eq!(clamp(5.0, 0.0, 1.0), 1.0);
181        assert_eq!(clamp(-5.0, 0.0, 1.0), 0.0);
182        assert_eq!(magnitude(-3.0), 3.0);
183    }
184}
pamoja_kit/motion.rs

pamoja_kit/
motion.rs
