xil: the dyno's robot driver takes a transport delay, an actuator function and an accel mode

CommonLib/xil/driver.py

@@ -15,9 +15,12 @@
 a coupling.
 """
 
+from collections import deque
+
 
 class RobotDriver(object):
-    """PI on speed error, producing one pedal split into throttle and brake.
+    """PI on speed error (``mode='speed'``) or on acceleration error
+    (``mode='accel'``), producing one pedal split into throttle and brake.
 
     One axis, not two: positive is throttle, negative is brake, and they are
     never both non-zero. That is what a driver does and what a VehicleControl
@@ -27,7 +30,13 @@ class RobotDriver(object):
     def __init__(self, kp=0.45, ki=0.25, max_throttle=1.0, max_brake=1.0,
                  standstill_ref_mps=0.05, standstill_brake=0.3,
                  max_accel_mps2=None, max_decel_mps2=None,
-                 stop_speed_mps=0.0, stop_accel_mps2=0.0):
+                 stop_speed_mps=0.0, stop_accel_mps2=0.0, delay_s=0.0,
+                 actuator=None, mode='speed', kp_accel=0.05, ki_accel=0.5,
+                 accel_filter_s=0.1):
+        if mode not in ('speed', 'accel'):
+            raise ValueError("mode must be 'speed' or 'accel', got %r" % (mode,))
+        #: 'speed' chases v_ref; 'accel' chases a_ref and ignores v_ref.
+        self.mode = mode
         self.kp = kp
         self.ki = ki
         self.max_throttle = max_throttle
@@ -58,8 +67,24 @@ def __init__(self, kp=0.45, ki=0.25, max_throttle=1.0, max_brake=1.0,
         #: less than stop_accel_mps2 (openpilot: 0.3 and 0.1). 0.0 is off.
         self.stop_speed_mps = stop_speed_mps
         self.stop_accel_mps2 = stop_accel_mps2
+        #: Transport delay between the robot's command and the pedal, in s; 0.0 is
+        #: off. Held per step, so it resolves to whole steps of the caller's dt.
+        self.delay_s = delay_s
+        #: Your pedal dynamics, after the delay: actuator(throttle, brake, dt)
+        #: -> (throttle, brake). Its reset(), if it has one, runs with ours.
+        self.actuator = actuator
+        #: Accel mode: PI gains on a_ref - a_meas, no feedforward.
+        self.kp_accel = kp_accel
+        self.ki_accel = ki_accel
+        #: Time constant of the first-order filter on dv/dt that gives a_meas.
+        self.accel_filter_s = accel_filter_s
+        self.a_meas = 0.0
+        self._v_prev = None
         self.integral = 0.0
         self.pedal = 0.0
+        self._clock = 0.0
+        self._sent = deque()
+        self._applied = (0.0, 0.0)
 
     #: How far the ramp may sit from the measured speed. Anti-windup for the
     #: setpoint itself: without it, a ramp advancing past a vehicle that cannot
@@ -70,21 +95,52 @@ def reset(self):
         self.integral = 0.0
         self.pedal = 0.0
         self._ramp = None
+        self._clock = 0.0
+        self._sent.clear()
+        self._applied = (0.0, 0.0)
+        self.a_meas = 0.0
+        self._v_prev = None
+        if hasattr(self.actuator, 'reset'):
+            self.actuator.reset()
 
     def step(self, v_ref, v_measured, dt, a_ref=None):
-        """Returns (throttle, brake), each in [0, 1], never both positive.
+        """Returns (throttle, brake), each in [0, 1], never both positive,
+        unless an ``actuator`` says otherwise.
 
-        ``a_ref`` is the acceleration command, read only by the hold below.
+        ``a_ref`` is the acceleration command: the reference in accel mode,
+        otherwise read only by the hold. With ``delay_s`` set, this is the command of ``delay_s`` ago, and
+        (0, 0) until there is one; ``pedal`` stays the command just made.
         """
         if dt <= 0.0:
             raise ValueError('dt must be positive')
+        command = self._command(v_ref, v_measured, dt, a_ref)
+        if self.delay_s > 0.0:
+            self._sent.append((self._clock, command))
+            due = self._clock - self.delay_s + 1e-9
+            self._clock += dt
+            while self._sent and self._sent[0][0] <= due:
+                self._applied = self._sent.popleft()[1]
+            command = self._applied
+        if self.actuator is not None:
+            command = self.actuator(command[0], command[1], dt)
+        return command
+
+    def _command(self, v_ref, v_measured, dt, a_ref):
+        if self.mode == 'accel' and a_ref is None:
+            raise ValueError("mode 'accel' needs a_ref")
+        if self._v_prev is not None:
+            self.a_meas += ((v_measured - self._v_prev) / dt - self.a_meas)                 * dt / (self.accel_filter_s + dt)
+        self._v_prev = v_measured
 
         if a_ref is not None and v_measured < self.stop_speed_mps \
                 and a_ref < self.stop_accel_mps2:
             self.integral = 0.0
             self.pedal = -self.standstill_brake
             return 0.0, min(self.max_brake, self.standstill_brake)
 
+        if self.mode == 'accel':
+            return self._accel(a_ref, v_measured, dt)
+
         # THE ENVELOPE. A RAMPED setpoint, advanced from its own last value --
         # not the incoming reference clamped to the measured speed.
         #
@@ -119,11 +175,23 @@ def step(self, v_ref, v_measured, dt, a_ref=None):
             self.pedal = -self.standstill_brake
             return 0.0, min(self.max_brake, self.standstill_brake)
 
-        err = v_ref - v_measured
-        raw = self.kp * err + self.ki * self.integral
+        return self._pi(v_ref - v_measured, self.kp, self.ki, dt)
+
+    def _accel(self, a_ref, v_measured, dt):
+        if self.max_accel_mps2 is not None:
+            a_ref = min(a_ref, self.max_accel_mps2)
+        if self.max_decel_mps2 is not None:
+            a_ref = max(a_ref, -self.max_decel_mps2)
+        if a_ref <= 0.0 and v_measured <= self.standstill_ref_mps:
+            self.integral = 0.0
+            self.pedal = -self.standstill_brake
+            return 0.0, min(self.max_brake, self.standstill_brake)
+        return self._pi(a_ref - self.a_meas, self.kp_accel, self.ki_accel, dt)
+
+    def _pi(self, err, kp, ki, dt):
+        raw = kp * err + ki * self.integral
         if -self.max_brake < raw < self.max_throttle:
             self.integral += err * dt           # anti-windup: freeze at the rail
         self.pedal = max(-self.max_brake,
-                         min(self.max_throttle,
-                             self.kp * err + self.ki * self.integral))
+                         min(self.max_throttle, kp * err + ki * self.integral))
         return (self.pedal, 0.0) if self.pedal >= 0.0 else (0.0, -self.pedal)

tests/Python/unit/test_xil_dyno.py

@@ -294,6 +294,98 @@ def test_separate_throttle_and_brake_ceilings():
     assert thr == pytest.approx(0.25)
 
 
+def test_a_delay_applies_the_command_of_that_long_ago():
+    plain, late = RobotDriver(), RobotDriver(delay_s=0.1)
+    sent = [plain.step(20.0, 0.0, 0.05) for _ in range(6)]
+    got = [late.step(20.0, 0.0, 0.05) for _ in range(6)]
+    assert got[:2] == [(0.0, 0.0)] * 2
+    assert got[2:] == sent[:4]
+    assert late.pedal == plain.pedal
+
+
+def test_reset_empties_the_delay_line():
+    driver = RobotDriver(delay_s=0.1)
+    for _ in range(5):
+        driver.step(20.0, 0.0, 0.05)
+    driver.reset()
+    assert driver.step(20.0, 0.0, 0.05) == (0.0, 0.0)
+
+
+def test_the_actuator_gets_the_delayed_command_and_has_the_last_word():
+    seen = []
+
+    def half(thr, brk, dt):
+        seen.append((thr, brk, dt))
+        return 0.5 * thr, brk
+
+    plain = RobotDriver()
+    late = RobotDriver(delay_s=0.05, actuator=half)
+    sent = [plain.step(20.0, 0.0, 0.05) for _ in range(3)]
+    got = [late.step(20.0, 0.0, 0.05) for _ in range(3)]
+    assert seen == [(0.0, 0.0, 0.05)] + [c + (0.05,) for c in sent[:2]]
+    assert got == [(0.0, 0.0)] + [(0.5 * t, b) for t, b in sent[:2]]
+
+
+def test_reset_resets_the_actuator_if_it_can():
+    class Lag(object):
+        resets = 0
+
+        def __call__(self, thr, brk, dt):
+            return thr, brk
+
+        def reset(self):
+            self.resets += 1
+
+    lag = Lag()
+    DynoSim(driver=RobotDriver(actuator=lag)).reset()
+    assert lag.resets == 1
+
+
+def test_a_delayed_driver_still_settles_on_the_reference():
+    sim = DynoSim(driver=RobotDriver(delay_s=0.2))
+    for _ in range(int(60.0 / 0.05)):
+        state = sim.step(15.0, 0.05)
+    assert state.speed == pytest.approx(15.0, abs=0.05)
+
+
+def test_an_unknown_mode_is_rejected():
+    with pytest.raises(ValueError):
+        RobotDriver(mode='torque')
+
+
+def test_accel_mode_needs_an_acceleration_command():
+    with pytest.raises(ValueError):
+        RobotDriver(mode='accel').step(5.0, 0.0, 0.05)
+
+
+def test_accel_mode_tracks_a_constant_acceleration():
+    sim = DynoSim(driver=RobotDriver(mode='accel'))
+    speeds = [sim.step(0.0, 0.05, a_ref=1.0).speed for _ in range(160)]
+    assert speeds[-1] == pytest.approx(8.0, abs=0.3)
+    assert (speeds[-1] - speeds[-81]) / 4.0 == pytest.approx(1.0, abs=0.02)
+
+
+def test_accel_mode_ignores_the_speed_reference():
+    a, b = RobotDriver(mode='accel'), RobotDriver(mode='accel')
+    for k in range(20):
+        v = 0.1 * k
+        assert a.step(0.0, v, 0.05, a_ref=0.5) == b.step(30.0, v, 0.05, a_ref=0.5)
+
+
+def test_accel_mode_caps_the_command_at_the_envelope():
+    sim = DynoSim(driver=RobotDriver(mode='accel', max_accel_mps2=1.8))
+    speeds = [sim.step(0.0, 0.05, a_ref=5.0).speed for _ in range(160)]
+    assert (speeds[-1] - speeds[-81]) / 4.0 == pytest.approx(1.8, abs=0.05)
+
+
+def test_accel_mode_holds_the_brake_at_rest_when_not_asked_to_move():
+    driver = RobotDriver(mode='accel')
+    for _ in range(20):
+        thr, brk = driver.step(0.0, 0.0, 0.05, a_ref=0.0)
+    assert thr == 0.0 and brk == pytest.approx(0.3)
+    assert driver.integral == 0.0
+
+
 def test_the_bench_can_fail_to_reach_the_reference():
     """The gap between asked and achieved is the point of having a plant."""
     sim = DynoSim()