From a53f8c3e04c3d6086d22ad9f9978f2c3b4f28fb6 Mon Sep 17 00:00:00 2001
From: hadelan <eriksonblarafghfjhghk429@gmail.com>
Date: Wed, 2 Sep 2026 14:42:36 +0800
Subject: [PATCH] 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.
---
 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 0ae1c738..562f5124 100644
--- a/robotd/src/main.rs
+++ b/robotd/src/main.rs
@@ -1376,6 +1376,30 @@ fn slew(ema: &mut f64, target: f64, alpha: f64) {
     }
 }
 
+/// 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 }),
+    }
+}
+
 async fn adopt_startup_pose<T: RobotIo>(
     safety: &mut Safety<T>,
     state: &RobotState,
@@ -2203,7 +2227,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(),
@@ -2219,13 +2245,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"
+                );
             }
         }
 
@@ -8138,4 +8165,49 @@ mod tests {
         slew(&mut ema, 1.0, 0.3);
         assert!((ema - 0.65).abs() < 1e-12, "{}", ema);
     }
+
+    /// 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 })
+        );
+    }
 }