From 704bda5c66d978d7b51fe833536623ea660fa5cf Mon Sep 17 00:00:00 2001 From: hadelan Date: Wed, 2 Sep 2026 14:42:36 +0800 Subject: [PATCH 1/4] robotd: never start the homing ramp for a mode switch while limp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit A mode switch queued with torque off jumped straight to Homing, skipping the set_torque the enable path owns. The ramp then "finished" over dead motors: the other mode's policies loaded, the state reported Ready and homed, and the robot lay on the floor. The same request on a tick with no position sample wedged mode_change forever — its only consumer is the ramp finishing — refusing every later switch as already in flight. Keep Limp (the enable path turns the motors on and ramps; the queued switch completes when that ramp does), and refuse the switch outright on a sample-less tick. Assisted-by: Kimi:kimi-code --- robotd/src/main.rs | 88 +++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 80 insertions(+), 8 deletions(-) diff --git a/robotd/src/main.rs b/robotd/src/main.rs index aa7c46bc..d8932390 100644 --- a/robotd/src/main.rs +++ b/robotd/src/main.rs @@ -992,6 +992,30 @@ 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 { + match bringup { + Bringup::Limp => Some(Bringup::Limp), + _ => positions.map(|from| Bringup::Homing { from, since: now }), + } +} + async fn adopt_startup_pose( safety: &mut Safety, state: &RobotState, @@ -1544,7 +1568,9 @@ async fn control_loop( ); } 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(), @@ -1560,13 +1586,14 @@ async fn control_loop( } 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" + ); } } @@ -4651,4 +4678,49 @@ 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 }) + ); + } } From b4f907e786e896e6c73a1e419f2f73da24e42cb0 Mon Sep 17 00:00:00 2001 From: hadelan Date: Wed, 2 Sep 2026 14:47:06 +0800 Subject: [PATCH 2/4] robotd: drop non-finite command targets instead of poisoning the command filters MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit JSON parses 1e400 as infinity, so a client can put one on the wire. Folded into the twist/head/body EMAs it is permanent — ema += α·(inf − ema) is inf on that tick and every tick after — and the safety layer refuses non-finite joint targets rather than clamping them, so a single bad robot.move froze the robot on its hold pose until reboot. Assisted-by: Kimi:kimi-code --- robotd/src/main.rs | 35 ++++++++++++++++++++++++++++++++--- 1 file changed, 32 insertions(+), 3 deletions(-) diff --git a/robotd/src/main.rs b/robotd/src/main.rs index d8932390..4aa6eae1 100644 --- a/robotd/src/main.rs +++ b/robotd/src/main.rs @@ -1016,6 +1016,19 @@ fn mode_switch_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, @@ -1778,14 +1791,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]; @@ -4723,4 +4736,20 @@ mod tests { 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); + } } From e65b2ccdc4b48795ef0ec28317dbaf92da7819e7 Mon Sep 17 00:00:00 2001 From: hadelan Date: Wed, 2 Sep 2026 14:48:18 +0800 Subject: [PATCH 3/4] kinematics: an empty band is no hand even when min_zones is 0 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit min_zones = 0 is a legal config value, and it makes the length check pass on an empty band — straight into the low-percentile index below, which panics on an empty Vec. Floor the check at one zone: no zones is no hand, whatever the configured floor is. Assisted-by: Kimi:kimi-code --- kinematics/src/hand.rs | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/kinematics/src/hand.rs b/kinematics/src/hand.rs index 8ca49019..1b08ec4c 100644 --- a/kinematics/src/hand.rs +++ b/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. From 7c59914abe0c98eaef55780aba1df2568c6f3560 Mon Sep 17 00:00:00 2001 From: hadelan Date: Wed, 2 Sep 2026 14:52:49 +0800 Subject: [PATCH 4/4] robotd-params: refuse policy.walk = "none" at load instead of panicking MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The "none" sentinel disables the optional policy slots, but 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. A config that said walk = "none" got past load and panicked in resolved() at startup. Validate refuses it now, as a config error with the path attached. Assisted-by: Kimi:kimi-code --- robotd-params/src/lib.rs | 34 +++++++++++++++++++++++++++++++++- 1 file changed, 33 insertions(+), 1 deletion(-) diff --git a/robotd-params/src/lib.rs b/robotd-params/src/lib.rs index 5aa8ba09..99f0b379 100644 --- a/robotd-params/src/lib.rs +++ b/robotd-params/src/lib.rs @@ -512,7 +512,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, /// Standing policy. Without one the walking policy runs at every velocity. pub stand: Option, @@ -901,6 +903,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. @@ -989,6 +996,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(()) } @@ -1373,6 +1389,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