From 5938c3704d62f508a8ee00c6534478f8ccd5af26 Mon Sep 17 00:00:00 2001 From: yunlishao-vibe <267182001+yunlishao-vibe@users.noreply.github.com> Date: Tue, 29 Sep 2026 18:50:02 -0400 Subject: [PATCH 1/3] xil: the robot driver takes a transport delay, delay_s, on its pedal --- CommonLib/xil/driver.py | 25 ++++++++++++++++++++++++- tests/Python/unit/test_xil_dyno.py | 24 ++++++++++++++++++++++++ 2 files changed, 48 insertions(+), 1 deletion(-) diff --git a/CommonLib/xil/driver.py b/CommonLib/xil/driver.py index 7a4df63d..8530cfe7 100644 --- a/CommonLib/xil/driver.py +++ b/CommonLib/xil/driver.py @@ -15,6 +15,8 @@ a coupling. """ +from collections import deque + class RobotDriver(object): """PI on speed error, producing one pedal split into throttle and brake. @@ -27,7 +29,7 @@ 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): self.kp = kp self.ki = ki self.max_throttle = max_throttle @@ -58,8 +60,14 @@ 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 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,15 +78,30 @@ 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) def step(self, v_ref, v_measured, dt, a_ref=None): """Returns (throttle, brake), each in [0, 1], never both positive. ``a_ref`` is the acceleration command, read only by the hold below. + 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: + return command + 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] + return self._applied + def _command(self, v_ref, v_measured, dt, a_ref): if a_ref is not None and v_measured < self.stop_speed_mps \ and a_ref < self.stop_accel_mps2: self.integral = 0.0 diff --git a/tests/Python/unit/test_xil_dyno.py b/tests/Python/unit/test_xil_dyno.py index 6bdd568c..61d28303 100644 --- a/tests/Python/unit/test_xil_dyno.py +++ b/tests/Python/unit/test_xil_dyno.py @@ -294,6 +294,30 @@ 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_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_the_bench_can_fail_to_reach_the_reference(): """The gap between asked and achieved is the point of having a plant.""" sim = DynoSim() From 90f0ce877313cb490054b45faf46b99a2654e09c Mon Sep 17 00:00:00 2001 From: yunlishao-vibe <267182001+yunlishao-vibe@users.noreply.github.com> Date: Tue, 29 Sep 2026 18:56:02 -0400 Subject: [PATCH 2/3] xil: the robot driver takes an actuator function, the pedal's own dynamics after the delay --- CommonLib/xil/driver.py | 29 +++++++++++++++++++---------- tests/Python/unit/test_xil_dyno.py | 30 ++++++++++++++++++++++++++++++ 2 files changed, 49 insertions(+), 10 deletions(-) diff --git a/CommonLib/xil/driver.py b/CommonLib/xil/driver.py index 8530cfe7..1b8ebc5c 100644 --- a/CommonLib/xil/driver.py +++ b/CommonLib/xil/driver.py @@ -29,7 +29,8 @@ 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, delay_s=0.0): + stop_speed_mps=0.0, stop_accel_mps2=0.0, delay_s=0.0, + actuator=None): self.kp = kp self.ki = ki self.max_throttle = max_throttle @@ -63,6 +64,9 @@ def __init__(self, kp=0.45, ki=0.25, max_throttle=1.0, max_brake=1.0, #: 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 self.integral = 0.0 self.pedal = 0.0 self._clock = 0.0 @@ -81,9 +85,12 @@ def reset(self): self._clock = 0.0 self._sent.clear() self._applied = (0.0, 0.0) + 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. With ``delay_s`` set, this is the command of ``delay_s`` ago, and @@ -92,14 +99,16 @@ def step(self, v_ref, v_measured, dt, a_ref=None): 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: - return command - 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] - return self._applied + 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 a_ref is not None and v_measured < self.stop_speed_mps \ diff --git a/tests/Python/unit/test_xil_dyno.py b/tests/Python/unit/test_xil_dyno.py index 61d28303..75db0ff9 100644 --- a/tests/Python/unit/test_xil_dyno.py +++ b/tests/Python/unit/test_xil_dyno.py @@ -311,6 +311,36 @@ def test_reset_empties_the_delay_line(): 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)): From b790d571a45e7cb2aef1bdc2f1b2bc6d8a099ee4 Mon Sep 17 00:00:00 2001 From: yunlishao-vibe <267182001+yunlishao-vibe@users.noreply.github.com> Date: Wed, 30 Sep 2026 10:20:22 -0400 Subject: [PATCH 3/3] xil: the robot driver has an accel mode, PI on acceleration error, no feedforward --- CommonLib/xil/driver.py | 52 +++++++++++++++++++++++++----- tests/Python/unit/test_xil_dyno.py | 38 ++++++++++++++++++++++ 2 files changed, 82 insertions(+), 8 deletions(-) diff --git a/CommonLib/xil/driver.py b/CommonLib/xil/driver.py index 1b8ebc5c..ae2cc656 100644 --- a/CommonLib/xil/driver.py +++ b/CommonLib/xil/driver.py @@ -19,7 +19,8 @@ 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 @@ -30,7 +31,12 @@ 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, delay_s=0.0, - actuator=None): + 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 @@ -67,6 +73,13 @@ def __init__(self, kp=0.45, ki=0.25, max_throttle=1.0, max_brake=1.0, #: 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 @@ -85,6 +98,8 @@ def reset(self): 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() @@ -92,8 +107,8 @@ def step(self, v_ref, v_measured, dt, a_ref=None): """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. - With ``delay_s`` set, this is the command of ``delay_s`` ago, and + ``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: @@ -111,12 +126,21 @@ def step(self, v_ref, v_measured, dt, a_ref=None): 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. # @@ -151,11 +175,23 @@ def _command(self, v_ref, v_measured, dt, a_ref): 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) diff --git a/tests/Python/unit/test_xil_dyno.py b/tests/Python/unit/test_xil_dyno.py index 75db0ff9..0b924f00 100644 --- a/tests/Python/unit/test_xil_dyno.py +++ b/tests/Python/unit/test_xil_dyno.py @@ -348,6 +348,44 @@ def test_a_delayed_driver_still_settles_on_the_reference(): 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()