Programming for Robotics 1, Chapter 5: Challenge 3 solution (the watchdog timeout against the stopping distance)

Idea: after the controller goes quiet, the robot keeps driving for the timeout (the watchdog has not noticed yet) and then rolls for another tau (the motor's lag, 0.08 s in the simulator) while the wheels slow down, so the distance is at most v x timeout + v x tau. For a budget of 0.05 m at 0.30 m/s that gives timeout <= (0.05 - 0.30 x 0.08) / 0.30 = 0.0867 s, which is 4.3 passes of 20 ms. That is the upper limit. The lower limit comes from the other side: the timeout must be longer than the longest gap between two commands that a healthy program can have, or the watchdog stops a robot that is fine. The code measures both: the distance rolled after silence for several timeouts, and whether a single pass that is 70 ms late trips a watchdog while the controller is still talking.

CODE:
V, TAU, BUDGET, DT = 0.30, 0.08, 0.05, 0.02              # speed, the motor's time constant, the stopping budget (m), the loop period (s)
geom = Geometry.from_config(Config)
print("(a) prediction: distance <= V x timeout + V x TAU <= %.2f m  ->  timeout <= %.4f s = %.1f loop passes of %.0f ms"
      % (BUDGET, (BUDGET - V * TAU) / V, (BUDGET - V * TAU) / V / DT, DT * 1000))


def silent_run(timeout, hiccup_s=0.0):
    """Drive at V; the controller goes quiet at t = 1.0 s. Returns (the distance rolled after it went quiet, whether the base was held before that
    (a false trip), the time the base first held). hiccup_s: one pass at t = 0.5 s is late by that much (a pause in the program, say)."""
    sim = Simulator(World(10.0, 4.0), seed=1, pose=(1.0, 2.0, 0.0))
    base = DriveBase(sim, geom)
    CommandWatchdog(base, timeout)
    loop = FixedRateLoop(DT, sim.now_s, sim.advance)
    seen = {"quiet_x": None, "false_trip": False, "held_at": None, "hiccuped": False}

    def step(dt_s):
        t = sim.now_s()
        if t < 1.0:
            base.set_velocity(V, 0.0)
        elif seen["quiet_x"] is None:
            seen["quiet_x"] = sim.pose()[0] - 1.0
        late = 0.0
        if hiccup_s and t >= 0.5 and not seen["hiccuped"]:
            seen["hiccuped"] = True
            late = hiccup_s
            sim.advance(late)                            # the world moves on while the program is not looking
        base.update(dt_s + late)
        if base.hold and seen["held_at"] is None:
            seen["held_at"] = sim.now_s()
            if t < 1.0:
                seen["false_trip"] = True
    loop.run(step, 200)
    return sim.pose()[0] - 1.0 - seen["quiet_x"], seen["false_trip"], seen["held_at"]


print("(b) measured: the controller goes silent at t = 1.0 s while driving at %.2f m/s" % V)
print("  timeout   bound V x timeout + V x TAU   rolled after silence   zeroed at   within %.0f cm?" % (BUDGET * 100))
for timeout in (0.25, 0.10, 0.08, 0.06, 0.04):
    rolled, _, held_at = silent_run(timeout)
    bound = V * timeout + V * TAU
    print("   %.2f s        %.3f m                     %.3f m              %.2f s      %s" % (timeout, bound, rolled, held_at, "yes" if rolled <= BUDGET else "no"))
print("(c) the other side: one late pass (70 ms) while the controller is still talking, at t = 0.5 s; does the watchdog trip falsely?")
for timeout in (0.10, 0.08, 0.06, 0.04):
    _, false_trip, _ = silent_run(timeout, hiccup_s=0.07)
    print("  timeout %.2f s: %s" % (timeout, "FALSE TRIP (the base was held although the controller was fine)" if false_trip else "no false trip"))
END CODE
OUTPUT:
(a) prediction: distance <= V x timeout + V x TAU <= 0.05 m  ->  timeout <= 0.0867 s = 4.3 loop passes of 20 ms
(b) measured: the controller goes silent at t = 1.0 s while driving at 0.30 m/s
  timeout   bound V x timeout + V x TAU   rolled after silence   zeroed at   within 5 cm?
   0.25 s        0.099 m                     0.090 m              1.22 s      no
   0.10 s        0.054 m                     0.042 m              1.06 s      yes
   0.08 s        0.048 m                     0.036 m              1.04 s      yes
   0.06 s        0.042 m                     0.030 m              1.02 s      yes
   0.04 s        0.036 m                     0.024 m              1.00 s      yes
(c) the other side: one late pass (70 ms) while the controller is still talking, at t = 0.5 s; does the watchdog trip falsely?
  timeout 0.10 s: no false trip
  timeout 0.08 s: FALSE TRIP (the base was held although the controller was fine)
  timeout 0.06 s: FALSE TRIP (the base was held although the controller was fine)
  timeout 0.04 s: FALSE TRIP (the base was held although the controller was fine)
END OUTPUT

Reading the output. (a) The bound gives 0.0867 s. (b) The measured distances follow the bound from below: 0.090 m for 0.25 s (over the budget), 0.042 m for 0.10 s, 0.036 m for 0.08 s, 0.030 m for 0.06 s, 0.024 m for 0.04 s; the bound counts the whole of tau, and the measured distance is a little less (the wheels are already slowing while the duty falls), so the bound is safe. Judged by the measurement alone, 0.10 s would pass (0.042 m); judged by the bound, which says 0.054 m for 0.10 s, it misses the budget, and the bound is what to design with, because the measurement is of one robot on one day. The timeouts the bound accepts are 0.08 s and less. (c) With one pass 70 ms late (a gap of 90 ms between commands) the timeouts 0.08, 0.06 and 0.04 s trip while the controller is fine, and 0.10 s does not. So with a budget of 5 cm and a program that can pause for 70 ms, there is no timeout that is both safe by the bound and free of false stops: either shorten the pause (find what causes it, with LoopStats of Programming for Robotics 1 section 2), or lower the speed (at 0.2 m/s the same budget allows (0.05 - 0.016) / 0.2 = 0.17 s), or accept a larger budget. The numbers decide the trade, which is the point.

Check on method: the ways to get this wrong are (1) choosing the timeout from the distance alone and never testing the false stops, which gives a robot that stops every time the program is slow; (2) forgetting the term v x tau, which is a large part of the distance for short timeouts (0.024 m of the 0.054 m here); (3) measuring the distance from the moment the watchdog fires instead of from the moment the controller went quiet: the timeout is part of the distance.