diff --git a/robotd/src/main.rs b/robotd/src/main.rs index 1fcfb242..6adc0c45 100644 --- a/robotd/src/main.rs +++ b/robotd/src/main.rs @@ -1281,6 +1281,19 @@ impl Bringup { } } +/// One low-pass step toward `target`. +/// +/// A non-finite target is dropped, not folded in: `ema += α·(inf − ema)` is `inf` on this +/// tick and on every tick after, because nothing finite can climb back out of it. The wire +/// can produce one — JSON parses `1e400` as infinity — and the safety layer below refuses +/// non-finite joint targets rather than clamping them, so a single bad `robot.move` would +/// otherwise freeze the robot on its hold pose until reboot. +fn slew(ema: &mut f64, target: f64, alpha: f64) { + if target.is_finite() { + *ema += alpha * (target - *ema); + } +} + async fn adopt_startup_pose( safety: &mut Safety, state: &RobotState, @@ -2375,14 +2388,14 @@ async fn control_loop( twist_ema = [0.0; 3]; } for (ema, target) in twist_ema.iter_mut().zip(twist_target) { - *ema += cmd_alpha * (target - *ema); + slew(ema, target, cmd_alpha); } for (ema, target) in head_ema.iter_mut().zip(gated.head) { - *ema += head_alpha * (target - *ema); + slew(ema, target, head_alpha); } if snapshot.pose.active { for (ema, target) in body_ema.iter_mut().zip(snapshot.pose.body) { - *ema += cmd_alpha * (target - *ema); + slew(ema, target, cmd_alpha); } } else { body_ema = [0.0; 3]; @@ -7943,4 +7956,20 @@ mod tests { assert!(Bringup::Limp.homing_target(since).is_none()); assert!(Bringup::Ready.homing_target(since).is_none()); } + + /// `1e400` on the wire parses as infinity. Folded into the EMA it is permanent — nothing + /// finite climbs back out — and with the safety layer refusing non-finite targets, one bad + /// `robot.move` would freeze the robot on its hold pose until reboot. Dropped instead. + #[test] + fn a_non_finite_command_does_not_poison_the_filter() { + let mut ema = 0.5; + slew(&mut ema, f64::INFINITY, 0.3); + slew(&mut ema, f64::NEG_INFINITY, 0.3); + slew(&mut ema, f64::NAN, 0.3); + assert_eq!(ema, 0.5, "non-finite targets are dropped, not folded in"); + + // And the filter still works afterwards: the next real command slews as always. + slew(&mut ema, 1.0, 0.3); + assert!((ema - 0.65).abs() < 1e-12, "{}", ema); + } }