pamoja_kit/safety.rs
1//! Keeping a moving robot safe: stop on command, stop on silence, and never lurch.
2//!
3//! A robot that drives itself is dangerous when something goes wrong, so safety here is a real
4//! feature rather than an afterthought. Three pieces compose into a [`SafetyGate`] that every
5//! motion command passes through: an [`EStop`] that latches the machine stopped until a person
6//! clears it, a [`Watchdog`] that stops the machine if the commands stop arriving (a crashed
7//! controller or a dropped link), and [`Limits`] that bound how fast and how abruptly the robot
8//! may move. The gate fails safe: when stopped it commands zero, and it eases back up through the
9//! limits rather than jumping.
10
11use crate::motion::{cap, clamp, finite_or_zero, magnitude, Twist};
12use crate::Ramp;
13use libm::sqrtf;
14
15/// An emergency stop that latches: once engaged it holds until explicitly reset.
16///
17/// Unlike a transient condition, an e-stop must stay tripped after the event that caused it, so a
18/// person decides when it is safe to move again. While engaged, [`gate`](EStop::gate) forces any
19/// command to a full stop.
20///
21/// # Examples
22///
23/// ```
24/// use pamoja_kit::{EStop, Twist};
25///
26/// let mut estop = EStop::new();
27/// let cmd = Twist::planar(1.0, 0.0);
28/// assert_eq!(estop.gate(cmd), cmd); // clear: passes through
29///
30/// estop.engage();
31/// assert_eq!(estop.gate(cmd), Twist::zero()); // latched: stopped
32/// estop.reset();
33/// assert_eq!(estop.gate(cmd), cmd); // cleared by a person
34/// ```
35#[derive(Clone, Copy, Debug, Default)]
36pub struct EStop {
37 engaged: bool,
38}
39
40impl EStop {
41 /// Creates a cleared e-stop.
42 ///
43 /// # Returns
44 ///
45 /// An e-stop that is not engaged.
46 pub fn new() -> Self {
47 Self { engaged: false }
48 }
49
50 /// Engages the stop; it latches until [`reset`](EStop::reset).
51 pub fn engage(&mut self) {
52 self.engaged = true;
53 }
54
55 /// Clears the stop, allowing motion again.
56 pub fn reset(&mut self) {
57 self.engaged = false;
58 }
59
60 /// Returns whether the stop is currently engaged.
61 ///
62 /// # Returns
63 ///
64 /// `true` while latched.
65 pub fn is_engaged(&self) -> bool {
66 self.engaged
67 }
68
69 /// Returns the command to apply: the input when clear, a full stop when engaged.
70 ///
71 /// # Arguments
72 ///
73 /// * `desired` - the command that would be applied if clear.
74 ///
75 /// # Returns
76 ///
77 /// `desired` when clear, or [`Twist::zero`] when engaged.
78 pub fn gate(&self, desired: Twist) -> Twist {
79 if self.engaged {
80 Twist::zero()
81 } else {
82 desired
83 }
84 }
85}
86
87/// A deadman timer: it expires unless fed often enough, catching a stalled controller or link.
88///
89/// Autonomy assumes a stream of fresh commands. If that stream stops, because the controller hung
90/// or the radio dropped, the last command must not run forever. A watchdog counts the time since it
91/// was last fed and reports expiry once that exceeds its timeout; the caller feeds it each time a
92/// fresh command arrives. It accumulates a supplied `dt` rather than reading a clock, so it works
93/// the same on a microcontroller with no wall time.
94///
95/// # Examples
96///
97/// ```
98/// use pamoja_kit::Watchdog;
99///
100/// let mut dog = Watchdog::new(0.5); // expire after 0.5 s of silence
101/// dog.feed();
102/// assert!(!dog.update(0.3)); // 0.3 s since feeding: still alive
103/// assert!(dog.update(0.3)); // 0.6 s total: expired
104/// dog.feed(); // a fresh command revives it
105/// assert!(!dog.is_expired());
106/// ```
107#[derive(Clone, Copy, Debug)]
108pub struct Watchdog {
109 timeout: f32,
110 elapsed: f32,
111}
112
113impl Watchdog {
114 /// Creates a watchdog that expires after `timeout` without being fed.
115 ///
116 /// # Arguments
117 ///
118 /// * `timeout` - the allowed silence before expiry; its magnitude is used, and one that is
119 /// not a number is taken as zero, so the watchdog expires rather than never.
120 ///
121 /// # Returns
122 ///
123 /// A freshly fed watchdog.
124 pub fn new(timeout: f32) -> Self {
125 Self {
126 timeout: cap(timeout),
127 elapsed: 0.0,
128 }
129 }
130
131 /// Feeds the watchdog, resetting the silence timer.
132 pub fn feed(&mut self) {
133 self.elapsed = 0.0;
134 }
135
136 /// Advances the timer by `dt` and returns whether it has expired.
137 ///
138 /// # Arguments
139 ///
140 /// * `dt` - the time since the previous update; its magnitude is used. One that is not a
141 /// finite number means the time is unknown, and the watchdog expires until it is fed,
142 /// since silence of unknown length is not safe to assume short.
143 ///
144 /// # Returns
145 ///
146 /// `true` if the watchdog is now expired.
147 pub fn update(&mut self, dt: f32) -> bool {
148 if dt.is_finite() {
149 self.elapsed += magnitude(dt);
150 } else {
151 self.elapsed = f32::INFINITY;
152 }
153 self.is_expired()
154 }
155
156 /// Returns whether the watchdog is currently expired.
157 ///
158 /// # Returns
159 ///
160 /// `true` if the time since feeding exceeds the timeout.
161 pub fn is_expired(&self) -> bool {
162 self.elapsed > self.timeout
163 }
164}
165
166/// Bounds a robot's speed and acceleration so commands stay within what the machine can do safely.
167///
168/// A raw command can be too fast or too sudden: a full-speed setpoint snaps the wheels, a hard
169/// reverse strips traction. [`Limits`] caps the planar speed and yaw rate, then eases each toward
170/// the capped target at a bounded acceleration, so motion is smooth and within envelope. The
171/// easing reuses [`Ramp`] as its slew-rate limiter, with the step set to `accel * dt` each update.
172///
173/// # Examples
174///
175/// ```
176/// use pamoja_kit::{Limits, Twist};
177///
178/// // Up to 1 m/s and 2 rad/s, easing on at 0.5 m/s^2 and 4 rad/s^2.
179/// let mut limits = Limits::new(1.0, 2.0, 0.5, 4.0);
180///
181/// // Asking for full speed from rest: the first 0.1 s step is acceleration-limited.
182/// let cmd = limits.apply(Twist::planar(1.0, 0.0), 0.1);
183/// assert!((cmd.vx - 0.05).abs() < 1e-6); // 0.5 m/s^2 * 0.1 s
184/// ```
185#[derive(Clone, Copy, Debug)]
186pub struct Limits {
187 max_linear: f32,
188 max_angular: f32,
189 max_linear_accel: f32,
190 max_angular_accel: f32,
191 vx: Ramp,
192 vy: Ramp,
193 omega: Ramp,
194}
195
196impl Limits {
197 /// Creates limits from the speed and acceleration ceilings.
198 ///
199 /// # Arguments
200 ///
201 /// * `max_linear` - the largest planar speed; its magnitude is used.
202 /// * `max_angular` - the largest yaw rate; its magnitude is used.
203 /// * `max_linear_accel` - the largest change in linear speed per second; its magnitude is used.
204 /// * `max_angular_accel` - the largest change in yaw rate per second; its magnitude is used.
205 ///
206 /// A ceiling that is not a number is taken as zero, so a mistake holds the robot still.
207 ///
208 /// # Returns
209 ///
210 /// Limits starting from rest.
211 pub fn new(
212 max_linear: f32,
213 max_angular: f32,
214 max_linear_accel: f32,
215 max_angular_accel: f32,
216 ) -> Self {
217 Self {
218 max_linear: cap(max_linear),
219 max_angular: cap(max_angular),
220 max_linear_accel: cap(max_linear_accel),
221 max_angular_accel: cap(max_angular_accel),
222 vx: Ramp::new(0.0, 0.0),
223 vy: Ramp::new(0.0, 0.0),
224 omega: Ramp::new(0.0, 0.0),
225 }
226 }
227
228 /// Resets the remembered motion to rest, so the next command eases up from zero.
229 pub fn reset(&mut self) {
230 self.vx.set(0.0);
231 self.vy.set(0.0);
232 self.omega.set(0.0);
233 }
234
235 /// Bounds a desired command in speed and acceleration and returns the safe command.
236 ///
237 /// # Arguments
238 ///
239 /// * `desired` - the requested body motion. A component that is not a finite number is
240 /// taken as zero, so a planner that fails eases the robot to a stop.
241 /// * `dt` - the time since the previous call, setting the acceleration step. One that is
242 /// not a finite number allows no change this call.
243 ///
244 /// # Returns
245 ///
246 /// The command after capping speed and easing toward it within the acceleration limit.
247 pub fn apply(&mut self, desired: Twist, dt: f32) -> Twist {
248 let desired = Twist::new(
249 finite_or_zero(desired.vx),
250 finite_or_zero(desired.vy),
251 finite_or_zero(desired.omega),
252 );
253 let dt = if dt.is_finite() { magnitude(dt) } else { 0.0 };
254 let bounded = self.clamp_speed(desired);
255 let linear_step = self.max_linear_accel * dt;
256 let angular_step = self.max_angular_accel * dt;
257 Twist::new(
258 self.vx.update_capped(bounded.vx, linear_step),
259 self.vy.update_capped(bounded.vy, linear_step),
260 self.omega.update_capped(bounded.omega, angular_step),
261 )
262 }
263
264 // Caps the planar speed (scaling vx and vy together so direction is kept) and the yaw rate.
265 fn clamp_speed(&self, twist: Twist) -> Twist {
266 let speed = sqrtf(twist.vx * twist.vx + twist.vy * twist.vy);
267 let (vx, vy) = if speed > self.max_linear {
268 let scale = self.max_linear / speed;
269 (twist.vx * scale, twist.vy * scale)
270 } else {
271 (twist.vx, twist.vy)
272 };
273 Twist::new(
274 vx,
275 vy,
276 clamp(twist.omega, -self.max_angular, self.max_angular),
277 )
278 }
279}
280
281/// The single gate every motion command passes through, composing e-stop, watchdog, and limits.
282///
283/// This is the one call a control loop makes to drive safely: feed it the desired motion and the
284/// time step, and it returns what is actually safe to command. It stops hard (commands zero and
285/// forgets its motion history, so resuming eases from rest) whenever the e-stop is engaged or the
286/// watchdog has expired; otherwise it returns the desired motion bounded by the [`Limits`]. Call
287/// [`feed`](SafetyGate::feed) whenever a fresh command arrives to keep the watchdog satisfied.
288///
289/// # Examples
290///
291/// ```
292/// use pamoja_kit::{Limits, SafetyGate, Twist};
293///
294/// let limits = Limits::new(1.0, 2.0, 0.5, 4.0);
295/// let mut gate = SafetyGate::new(limits, 0.2); // stop if unfed for 0.2 s
296///
297/// gate.feed();
298/// let cmd = gate.command(Twist::planar(1.0, 0.0), 0.1);
299/// assert!((cmd.vx - 0.05).abs() < 1e-6); // eased on, acceleration-limited
300///
301/// gate.engage_estop();
302/// assert_eq!(gate.command(Twist::planar(1.0, 0.0), 0.1), Twist::zero());
303/// ```
304#[derive(Clone, Copy, Debug)]
305pub struct SafetyGate {
306 estop: EStop,
307 watchdog: Watchdog,
308 limits: Limits,
309}
310
311impl SafetyGate {
312 /// Creates a gate from motion limits and a watchdog timeout.
313 ///
314 /// # Arguments
315 ///
316 /// * `limits` - the speed and acceleration bounds for normal motion.
317 /// * `watchdog_timeout` - the allowed silence before the gate stops the robot.
318 ///
319 /// # Returns
320 ///
321 /// The gate, cleared and freshly fed.
322 pub fn new(limits: Limits, watchdog_timeout: f32) -> Self {
323 Self {
324 estop: EStop::new(),
325 watchdog: Watchdog::new(watchdog_timeout),
326 limits,
327 }
328 }
329
330 /// Feeds the watchdog; call this whenever a fresh command arrives.
331 pub fn feed(&mut self) {
332 self.watchdog.feed();
333 }
334
335 /// Engages the latching emergency stop.
336 pub fn engage_estop(&mut self) {
337 self.estop.engage();
338 }
339
340 /// Clears the emergency stop.
341 pub fn reset_estop(&mut self) {
342 self.estop.reset();
343 }
344
345 /// Returns whether the gate is currently forcing a stop.
346 ///
347 /// # Returns
348 ///
349 /// `true` if the e-stop is engaged or the watchdog has expired.
350 pub fn is_stopped(&self) -> bool {
351 self.estop.is_engaged() || self.watchdog.is_expired()
352 }
353
354 /// Returns the safe command for a desired motion over a time step.
355 ///
356 /// # Arguments
357 ///
358 /// * `desired` - the requested body motion.
359 /// * `dt` - the time since the previous call.
360 ///
361 /// # Returns
362 ///
363 /// [`Twist::zero`] when stopped, otherwise the desired motion bounded by the limits.
364 pub fn command(&mut self, desired: Twist, dt: f32) -> Twist {
365 self.watchdog.update(dt);
366 if self.is_stopped() {
367 self.limits.reset();
368 return Twist::zero();
369 }
370 self.limits.apply(desired, dt)
371 }
372}
373
374#[cfg(test)]
375mod tests {
376 use super::*;
377
378 #[test]
379 fn estop_latches_until_reset() {
380 let mut estop = EStop::new();
381 let cmd = Twist::planar(1.0, 0.5);
382 assert_eq!(estop.gate(cmd), cmd);
383 estop.engage();
384 assert!(estop.is_engaged());
385 assert_eq!(estop.gate(cmd), Twist::zero());
386 estop.reset();
387 assert_eq!(estop.gate(cmd), cmd);
388 }
389
390 #[test]
391 fn watchdog_expires_on_silence_and_revives_on_feeding() {
392 let mut dog = Watchdog::new(0.5);
393 dog.feed();
394 assert!(!dog.update(0.3));
395 assert!(dog.update(0.3)); // 0.6 > 0.5
396 dog.feed();
397 assert!(!dog.is_expired());
398 }
399
400 #[test]
401 fn limits_cap_planar_speed_by_scaling() {
402 let mut limits = Limits::new(1.0, 10.0, 100.0, 100.0); // accel high so slew is not the cap
403 // A 3-4-5 triangle at speed 5 scales down to length 1, keeping direction.
404 let cmd = limits.apply(Twist::new(3.0, 4.0, 0.0), 1.0);
405 assert!((cmd.vx - 0.6).abs() < 1e-5);
406 assert!((cmd.vy - 0.8).abs() < 1e-5);
407 }
408
409 #[test]
410 fn limits_ease_in_at_the_acceleration_bound() {
411 let mut limits = Limits::new(1.0, 2.0, 0.5, 4.0);
412 assert!((limits.apply(Twist::planar(1.0, 0.0), 0.1).vx - 0.05).abs() < 1e-6);
413 assert!((limits.apply(Twist::planar(1.0, 0.0), 0.1).vx - 0.10).abs() < 1e-6);
414 }
415
416 #[test]
417 fn gate_stops_on_estop_and_on_watchdog_expiry() {
418 let mut gate = SafetyGate::new(Limits::new(1.0, 2.0, 100.0, 100.0), 0.2);
419
420 gate.feed();
421 assert!(gate.command(Twist::planar(1.0, 0.0), 0.1).vx > 0.0);
422
423 // Stop feeding: after enough time the watchdog trips and the gate zeroes the command.
424 assert_eq!(gate.command(Twist::planar(1.0, 0.0), 0.5), Twist::zero());
425 assert!(gate.is_stopped());
426
427 // Feeding revives it.
428 gate.feed();
429 assert!(gate.command(Twist::planar(1.0, 0.0), 0.1).vx > 0.0);
430
431 // The e-stop overrides even a fed watchdog.
432 gate.feed();
433 gate.engage_estop();
434 assert_eq!(gate.command(Twist::planar(1.0, 0.0), 0.1), Twist::zero());
435 }
436
437 #[test]
438 fn a_time_step_that_is_not_a_number_stops_the_gate_until_fed() {
439 let mut gate = SafetyGate::new(Limits::new(1.0, 2.0, 100.0, 100.0), 0.2);
440 gate.feed();
441 assert!(gate.command(Twist::planar(1.0, 0.0), 0.1).vx > 0.0);
442 assert_eq!(
443 gate.command(Twist::planar(1.0, 0.0), f32::NAN),
444 Twist::zero()
445 );
446 assert_eq!(gate.command(Twist::planar(1.0, 0.0), 0.01), Twist::zero());
447 gate.feed();
448 assert!(gate.command(Twist::planar(1.0, 0.0), 0.1).vx > 0.0);
449 }
450
451 #[test]
452 fn a_watchdog_timeout_that_is_not_a_number_expires() {
453 let mut dog = Watchdog::new(f32::NAN);
454 assert!(dog.update(0.001));
455 }
456
457 #[test]
458 fn a_command_that_is_not_a_number_eases_to_a_stop() {
459 let mut limits = Limits::new(1.0, 2.0, 0.5, 4.0);
460 limits.apply(Twist::planar(1.0, 0.0), 1.0);
461 let easing = limits.apply(Twist::new(f32::NAN, 0.0, f32::INFINITY), 1.0);
462 assert_eq!(easing, Twist::new(0.0, 0.0, 0.0));
463 assert!(limits.apply(Twist::planar(1.0, 0.0), f32::NAN).vx == 0.0);
464 }
465
466 #[test]
467 fn a_ceiling_that_is_not_a_number_holds_the_robot_still() {
468 let mut limits = Limits::new(f32::NAN, f32::NAN, 100.0, 100.0);
469 assert_eq!(limits.apply(Twist::planar(1.0, 1.0), 1.0), Twist::zero());
470 }
471}