robotd, kinematics, robotd-params: four hardening fixes for the drive loop and startup

kinematics/src/hand.rs

@@ -143,7 +143,9 @@ impl Tracker {
             }
         }
 
-        if in_band.len() < self.config.min_zones {
+        // The band must hold at least one zone whatever `min_zones` says: at 0 the length
+        // check passes on an empty band, and the percentile index below would panic on it.
+        if in_band.len() < self.config.min_zones.max(1) {
             // Hold the last hand briefly. This is the whole anti-chop mechanism: the note
             // rides over a dropout, and stops when one lasts.
             return match self.last {
@@ -318,6 +320,21 @@ mod tests {
         assert!(tracker.track(&distance, &status, Instant::now()).is_some());
     }
 
+    /// `min_zones = 0` is a legal config, and an empty band then passes the length check —
+    /// straight into a percentile index that has nothing to index. No zones is no hand,
+    /// whatever the floor is.
+    #[test]
+    fn an_empty_band_is_not_a_hand_even_with_min_zones_zero() {
+        let mut tracker = Tracker::new(Config {
+            min_zones: 0,
+            ..Config::default()
+        });
+        assert_eq!(tracker.track(&[], &[], Instant::now()), None);
+        // But a single usable zone still is one: the floor is on emptiness, not on count.
+        let (distance, status) = frame(0.25, 5, 1);
+        assert!(tracker.track(&distance, &status, Instant::now()).is_some());
+    }
+
     /// A negative distance under a believed status is a failed convergence, and a frame
     /// shorter than the grid must not panic or read past its end — the wire carries vectors,
     /// and a peer from another release can send fewer.

robotd-params/src/lib.rs

@@ -650,7 +650,9 @@ pub struct PolicyParams {
     pub mode: Mode,
     /// Policy paths. Absent means the mode's default inside the release directory, so a
     /// normal update ships them; point one elsewhere to try a build without cutting a
-    /// release. The literal `"none"` disables a slot outright — the prototype's convention.
+    /// release. The literal `"none"` disables an optional slot outright — the prototype's
+    /// convention. Not this one: `walk` is the network the controller loads, and refusing
+    /// the config beats panicking in `resolved()`.
     pub walk: Option<PathBuf>,
     /// Standing policy. Without one the walking policy runs at every velocity.
     pub stand: Option<PathBuf>,
@@ -1703,6 +1705,11 @@ pub enum ParamsError {
         min: u32,
         max: u32,
     },
+    #[error(
+        "{path}: policy.walk is the one slot \"none\" cannot disable — \
+         the controller has nothing to load without it"
+    )]
+    NoneWalk { path: String },
 }
 
 /// The band `media.bitrate` is accepted in, bits per second.
@@ -1791,6 +1798,15 @@ impl Params {
                 max: BITRATE_MAX,
             });
         }
+        // `walk` is the one slot `resolved()` cannot leave empty — every mode has a default
+        // for it, and the controller has nothing to load without it — so the "none" sentinel
+        // that legitimately disables the optional slots would panic there. Refuse it here,
+        // where a bad value is still a config error with the path attached.
+        if self.policy.walk.as_deref().is_some_and(is_none_sentinel) {
+            return Err(ParamsError::NoneWalk {
+                path: path.display().to_string(),
+            });
+        }
         Ok(())
     }
 
@@ -2903,6 +2919,22 @@ mod tests {
         );
     }
 
+    /// `walk` is the exception to `"none"`: it is the one slot `resolved()` cannot leave
+    /// empty, so the sentinel would panic there. Refused at load instead, with the path in
+    /// the error — and case-insensitively, the same as the optional slots spell it.
+    #[test]
+    fn a_none_walk_is_refused_at_load() {
+        let dir = tempfile::tempdir().unwrap();
+        for literal in ["none", "None"] {
+            let path = write(dir.path(), &format!("[policy]\nwalk = \"{literal}\"\n"));
+            let err = Params::load(&path, true).unwrap_err();
+            assert!(matches!(err, ParamsError::NoneWalk { .. }), "{err}");
+        }
+        // And the optional slots still take it.
+        let path = write(dir.path(), "[policy]\nstand = \"none\"\n");
+        assert!(Params::load(&path, true).is_ok());
+    }
+
     /// A typo in a key is named and ignored, and the setting it was aimed at keeps its default.
     ///
     /// It used to be fatal, on the argument that silently ignoring `min_acheived_hz` leaves the

robotd/src/main.rs

@@ -1250,6 +1250,43 @@ impl Bringup {
     }
 }
 
+/// Where an accepted mode switch leaves the bring-up state, or `None` to refuse it this tick.
+///
+/// Two cases matter:
+///
+/// - **`Limp` stays `Limp`.** Torque is off there, so there is nothing to ramp from yet — and
+///   the ramp must not start: the enable path below turns the motors on and starts the homing
+///   ramp itself, and the queued switch completes when that ramp does. Jumping to `Homing`
+///   here would skip `set_torque`, the ramp would "finish" over dead motors, and the other
+///   mode's policies would load onto a robot lying on the floor while reporting `Ready`.
+/// - **No position sample refuses.** The ramp starts from the joints' actual positions, so a
+///   tick without a read cannot arm it. Queuing the switch anyway would leave it in
+///   `mode_change` forever — its only consumer is the ramp finishing — and every later switch
+///   would be refused as already in flight.
+fn mode_switch_bringup(
+    bringup: Bringup,
+    positions: Option<[f64; NUM_JOINTS]>,
+    now: Instant,
+) -> Option<Bringup> {
+    match bringup {
+        Bringup::Limp => Some(Bringup::Limp),
+        _ => positions.map(|from| Bringup::Homing { from, since: now }),
+    }
+}
+
+/// 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<T: RobotIo>(
     safety: &mut Safety<T>,
     state: &RobotState,
@@ -2043,7 +2080,9 @@ async fn control_loop<T: RobotIo>(
                 );
             } else if mode_change.is_some() {
                 tracing::warn!(mode = target.as_str(), "a mode switch is already in flight");
-            } else {
+            } else if let Some(next) =
+                mode_switch_bringup(bringup, sensors.as_ref().map(|s| s.positions), tick_start)
+            {
                 tracing::warn!(
                     from = policy_params.mode.as_str(),
                     to = target.as_str(),
@@ -2059,13 +2098,14 @@ async fn control_loop<T: RobotIo>(
                 }
                 mode_change = Some(target);
                 // Home the robot with the machinery `init` and a fall recovery already use: it
-                // ramps per tick, and `driving` is false until it reaches Ready.
-                if let Some(sensors) = sensors.as_ref() {
-                    bringup = Bringup::Homing {
-                        from: sensors.positions,
-                        since: tick_start,
-                    };
-                }
+                // ramps per tick, and `driving` is false until it reaches Ready. From `Limp`
+                // this is a no-op — the enable path below owns the torque and the ramp.
+                bringup = next;
+            } else {
+                tracing::warn!(
+                    mode = target.as_str(),
+                    "mode switch refused: no position sample this tick"
+                );
             }
         }
 
@@ -2336,14 +2376,14 @@ async fn control_loop<T: RobotIo>(
             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];
@@ -7730,4 +7770,65 @@ mod tests {
         assert!(Bringup::Limp.homing_target(since).is_none());
         assert!(Bringup::Ready.homing_target(since).is_none());
     }
+
+    /// A mode switch requested while `Limp` must stay `Limp`: the enable path owns
+    /// `set_torque`, and jumping straight to `Homing` would run the whole ramp over dead
+    /// motors — finishing "successfully", loading the other mode's policies, and reporting
+    /// `Ready` for a robot still lying on the floor.
+    #[test]
+    fn a_mode_switch_from_limp_waits_for_the_enable_path() {
+        let now = Instant::now();
+        assert_eq!(
+            mode_switch_bringup(Bringup::Limp, Some([0.0; NUM_JOINTS]), now),
+            Some(Bringup::Limp)
+        );
+    }
+
+    /// A tick without a position sample cannot arm the ramp. Queuing the switch anyway would
+    /// leave it in `mode_change` forever — its only consumer is the ramp finishing — and every
+    /// later switch would be refused as already in flight. Refusing beats wedging.
+    #[test]
+    fn a_mode_switch_without_a_position_sample_is_refused() {
+        let now = Instant::now();
+        assert_eq!(mode_switch_bringup(Bringup::Ready, None, now), None);
+        assert_eq!(
+            mode_switch_bringup(
+                Bringup::Homing {
+                    from: [0.0; NUM_JOINTS],
+                    since: now
+                },
+                None,
+                now
+            ),
+            None
+        );
+    }
+
+    /// The ordinary case: re-home from wherever the joints are, so the other mode's policies
+    /// load at a known pose with the robot standing still.
+    #[test]
+    fn a_mode_switch_restarts_the_homing_ramp() {
+        let now = Instant::now();
+        let from = [0.1; NUM_JOINTS];
+        assert_eq!(
+            mode_switch_bringup(Bringup::Ready, Some(from), now),
+            Some(Bringup::Homing { from, since: now })
+        );
+    }
+
+    /// `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);
+    }
 }