Programming for Robotics 1, Chapter 3: Challenge 3 solution (a second range sensor, and the highest safe approach speed)

(The task says "minimum safe approach speed". What is worked out is the HIGHEST speed at which the robot can still stop in time: that is the safe limit. Below it, everything is safe.)

The rule is: drive, and set the motors to zero as soon as a sensor reads under its threshold. How far does the robot go after the wall crosses the threshold?
    - The reading is delayed by the sensor's latency (0.30 s for A).
    - A new measurement is made only every period (0.10 s for A). In the worst case the wall crosses the threshold just after a measurement, so the next one, a whole period later, is the one that finds it.
    - Then the command is given. The motors do not stop at once: the lag carries the robot a further  v tau  (the area under the decaying speed curve, as in section 5), with tau = 0.08 s.
    So the robot goes  v (latency + period + tau)  past the point where the wall was at the threshold. The noise also matters: a reading can be up to three sigma low or high, so the threshold seen by the robot is only certain to within 3 sigma. The sensor must still be 0.02 m from the wall at the end (the body front is 0.09 - 0.07 = 0.02 m ahead of the sensor):
        threshold - 0.02 - 3 sigma - v (latency + period + tau) >= 0,      so      v_max = (threshold - 0.02 - 3 sigma) / (latency + period + tau).
    (a) A alone:   v_max = (0.20 - 0.02 - 0.015) / (0.30 + 0.10 + 0.08) = 0.165 / 0.48 = 0.344 m/s, which is a duty of 0.344 / 0.40 = 0.86.
    (c) B alone:   v_max = (0.12 - 0.02 - 0.006) / (0 + 0.02 + 0.08) = 0.094 / 0.10 = 0.94 m/s, a duty of 2.35. The robot cannot go faster than 0.40 m/s (duty 1.0), so B makes every speed the robot has safe.
    When two sensors both stop the robot, the safe speed is that of whichever stops it EARLIER in distance terms, here B, so the pair is as safe as B.

The simulation sweeps the duty from 0.50 to 1.00 in steps of 0.01. A duty is called safe when none of ten runs bumps, the ten differing only in the start position by 0, 3, 6 ... 27 mm, which moves where the passes fall relative to the wall and so exercises different phases of the 0.10 s period. The "limit" is the highest duty before the first duty that bumps.

CODE:
arena = World(3.0, 2.0)
A_THRESH, A_LATENCY, A_PERIOD, A_SIGMA = 0.20, 0.30, 0.10, 0.005       # sensor A: long range, slow and late
B_THRESH, B_PERIOD, B_SIGMA = 0.12, 0.02, 0.002                        # sensor B: short range, every pass, no delay
TAU, DT, SENSOR_TO_FRONT = 0.08, 0.02, 0.09 - 0.07                     # motor lag; pass length; body front is 2 cm ahead of the sensor


def safe_speed(thresh, latency, period, sigma):
    """Worst case: the wall crosses the threshold just after a measurement. Stop distance = v (latency + period + tau)."""
    return (thresh - SENSOR_TO_FRONT - 3 * sigma) / (latency + period + TAU)


def approach(duty, offset_m, use_b):
    """Drive at a duty towards the wall; stop when A reads under its threshold (or B under its own, if used)."""
    sim = Simulator(arena, pose=(0.3 + offset_m, 1.0, 0.0), latency_s=A_LATENCY, period_s=A_PERIOD, sigma_m=A_SIGMA)
    if use_b:
        sim.add_range_sensor("near", max_range_m=0.35, sigma_m=B_SIGMA, period_s=B_PERIOD)
    sim.set_duty(duty, duty)
    for _ in range(int(14.0 / DT)):
        sim.advance(DT)
        a = sim.range_m()
        b = sim.range_of("near") if use_b else None
        if (a is not None and a < A_THRESH) or (b is not None and b < B_THRESH):
            sim.set_duty(0.0, 0.0)
    return sim.bumps > 0


def safe_at(duty, use_b):
    """Safe means: no bump for any of ten start offsets (they vary where the passes fall relative to the wall)."""
    return not any(approach(duty, k * 0.003, use_b) for k in range(10))


vmax_a = safe_speed(A_THRESH, A_LATENCY, A_PERIOD, A_SIGMA)
vmax_b = safe_speed(B_THRESH, 0.0, B_PERIOD, B_SIGMA)
print("formula: A alone v_max = %.3f m/s (duty %.2f);  B alone v_max = %.3f m/s (duty %.2f, above the robot's 0.40 m/s limit)"
      % (vmax_a, vmax_a / 0.40, vmax_b, vmax_b / 0.40))
duties = [d / 100 for d in range(50, 101)]
flags = [safe_at(d, False) for d in duties]
first_bad = flags.index(False) if False in flags else len(duties)
limit_a = duties[first_bad - 1]
print("simulated, A only:     the highest duty below the first bump is %.2f (%.3f m/s)" % (limit_a, 0.40 * limit_a))
print("check the formula:     duty %.2f (the formula's speed) bumps: %s;  duty 1.00 bumps: %s"
      % (vmax_a / 0.40, not safe_at(vmax_a / 0.40, False), not safe_at(1.0, False)))
print("with B added:          duty 1.00 bumps: %s" % (not safe_at(1.0, True)))
END CODE
OUTPUT:
formula: A alone v_max = 0.344 m/s (duty 0.86);  B alone v_max = 0.940 m/s (duty 2.35, above the robot's 0.40 m/s limit)
simulated, A only:     the highest duty below the first bump is 0.92 (0.368 m/s)
check the formula:     duty 0.86 (the formula's speed) bumps: False;  duty 1.00 bumps: True
with B added:          duty 1.00 bumps: False
END OUTPUT

Reading the output: the formula says A alone is safe up to 0.344 m/s (duty 0.86). The simulation first bumps just after duty 0.92 (0.368 m/s): the formula is safe (the robot does not bump at the formula's speed) and about 7 % conservative, which is the right way round for a safety formula: it assumes the worst phase and a three-sigma noise error together, and ten offsets seldom hit both. At duty 1.00, A alone bumps. With B added, duty 1.00 does not bump.

Check on method: (1) A sweep finds where it FAILED, not where it is safe: a safe duty in ten runs may fail on the eleventh (a different phase or noise), so the formula, not the sweep, is the guarantee and the sweep is its test. (2) The latency counts at full value even though the robot is not told anything: the delay is in the chain, not in the code. (3) Adding B changed the answer by changing the delay term, not the speed: a short-range sensor with no delay is worth more than a long-range sensor with a late reading. (4) The result depends on tau, the motor lag: heavier robots with a larger tau have a lower safe speed, so tau should be measured (section 13).