diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py index e2c5bf7a6d..215b4314f8 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py @@ -125,10 +125,10 @@ class ModelsLayoutMici(NavScroller): "while the model still sets the cruising speed. Turns off the accel boost. " + "Needs openpilot longitudinal. Not available with the Default model.")) - self.path_steering_toggle = BigParamControl(tr("turn correction (alpha)"), "TurnInHold", - description=tr("In slow turns, corrects the model's steering toward the line FSD and you take, with a correction learned for " + - "that model from your drives. It can only move steering within the range the model's own plan allows. A model " + - "without a learned correction is only kept from turning tighter than its planned line at your speed. " + + self.path_steering_toggle = BigParamControl(tr("FSD-timed turns (alpha)"), "TurnInHold", + description=tr("In turns the model's plan shows (under about 20 mph), steers when and how hard FSD does: the model says where " + + "the corner is and how far it turns, FSD's own turns set when to start and the peak for your speed, a little " + + "under FSD for now. Outside those turns, a slow turn is kept from going tighter than the model's planned line. " + "Not available with the Default model.")) self.map_turn_toggle = BigParamControl(tr("signal navigation (experimental)"), "MapboxNextTurnEnabled", diff --git a/openpilot/sunnypilot/modeld_v2/fsd_turn_profile.py b/openpilot/sunnypilot/modeld_v2/fsd_turn_profile.py new file mode 100644 index 0000000000..173df45610 --- /dev/null +++ b/openpilot/sunnypilot/modeld_v2/fsd_turn_profile.py @@ -0,0 +1,119 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import numpy as np + +# FSD-timed turns: the model's plan says where the corner is and how much it turns; FSD's own turns say when to start +# turning and how hard. From qlogs of routes 85-b1 (curvature from the wheel angle), FSD's 21 turns of 60-120 deg: +# peak curvature = 0.190 - 0.018 v (held out, within 0.010 1/m; the driver's own turns sit at 1.05x of it) +# and its curvature over time around the half-heading point follows TEMPLATE (median of 23 FSD turns >= 45 deg, +# normalized to its peak): rising from ~3 s before, peaking just after, gone ~3 s after. +# openpilot's turns (mostly Light Speed 2) turned in 1.7 m earlier than that and peaked 0.93x. +# A turn's profile here is the rule's peak at the car's speed, the template stretched in time so the turn adds up to +# the heading the plan (plus what is already turned) says it needs, placed at the plan's half-heading point. +TAU = np.arange(-5.0, 5.01, 0.1) # s from the half-heading point +TEMPLATE = np.array([ + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.002, 0.006, 0.008, 0.008, 0.008, 0.008, 0.014, 0.027, 0.043, + 0.046, 0.048, 0.051, 0.056, 0.072, 0.089, 0.109, 0.133, 0.172, 0.212, 0.261, 0.309, 0.359, 0.414, 0.454, 0.488, 0.523, + 0.575, 0.644, 0.683, 0.733, 0.784, 0.824, 0.871, 0.889, 0.915, 0.928, 0.940, 0.954, 0.956, 0.966, 0.972, 0.979, 0.985, + 0.986, 0.989, 0.978, 0.973, 0.956, 0.929, 0.894, 0.857, 0.814, 0.779, 0.738, 0.696, 0.649, 0.607, 0.581, 0.552, 0.509, + 0.465, 0.445, 0.397, 0.345, 0.315, 0.288, 0.260, 0.230, 0.198, 0.167, 0.139, 0.115, 0.099, 0.085, 0.072, 0.062, 0.058, + 0.054, 0.045, 0.038, 0.029, 0.022, 0.021, 0.017, 0.013, 0.010, 0.009, 0.009, 0.008, 0.000, 0.000, 0.000, 0.000]) +TEMPLATE /= TEMPLATE.max() +TEMPLATE_WIDTH = float(np.trapezoid(TEMPLATE, TAU)) # s: the integral of curvature/peak over time +RULE_INTERCEPT, RULE_SLOPE = 0.190, -0.018 # 1/m, 1/m per m/s +RULE_SPEEDS = (3.0, 8.0) # m/s: the FSD turns it was fitted on; below, its crawl value, above, its lateral accel +# First road test: a quarter of FSD's turns replayed 20%+ over FSD's own peak (corners the speed rule can't see), and +# 10-20% too much already felt like far too much on the road, so the rule is taken 8% low: an under-turn can be helped. +PEAK_SCALE = 0.92 +MIN_TURN = np.radians(45.) # heading change that makes it a turn (as the labels) +MIN_TURN_NO_CUE = np.radians(60.) +MAX_SPEED = 9.0 # m/s +CONFIRM_FRAMES = 10 # the plan must show the turn for 0.5 s before it is taken over +BLEND_IN_S, BLEND_OUT_S = 0.5, 1.0 +END_LEVEL = 0.05 # past the half-heading point, hand back once the profile is this low +PHANTOM = np.radians(15.) # before the half-heading point, a turn the plan no longer shows is dropped +TIMING_SPEED = 3.0 # m/s: below it the time to the half-heading point is counted at this speed, so the wheel winds in + # while pulling away from a stop the way FSD does, instead of waiting until the car is moving +MODE_PROFILE, MODE_MAX = 'profile', 'max' # the profile alone, or whichever of action and profile turns more + + +def rule_peak(v_ego: float) -> float: + v = float(np.clip(v_ego, *RULE_SPEEDS)) + peak = RULE_INTERCEPT + RULE_SLOPE * v + if v_ego > RULE_SPEEDS[1]: # past the fitted speeds, hold its lateral acceleration rather than its curvature + peak *= (RULE_SPEEDS[1] / v_ego) ** 2 + return PEAK_SCALE * peak + + +def _width(v_ego: float, total: float) -> float: + return float(np.clip(total / (max(v_ego, 1.0) * rule_peak(v_ego)), 1.5, 8.0)) # s, so the profile adds up to the turn + + +def profile(tau: float, v_ego: float, total: float) -> float: + """Curvature toward the turn tau s from its half-heading point, for a turn of `total` rad at v_ego.""" + return rule_peak(v_ego) * float(np.interp(tau * TEMPLATE_WIDTH / _width(v_ego, total), TAU, TEMPLATE, left=0., right=0.)) + + +class FsdTurnProfile: + def __init__(self, mode: str = MODE_PROFILE): + self.mode = mode + self.reset() + + def reset(self): + self.active, self.direction, self.confirm = False, 0., 0 + self.done = self.total = self.since_mid = 0. + self.past_mid = False + self.v_mid = 0. + self.weight = 0. + self.last = 0. + + def update(self, plan_x: np.ndarray, plan_y: np.ndarray, plan_yaw: np.ndarray, yaw_rate: float, v_ego: float, + dt: float, action: float, turn_sign: float = 0., lead: float = 0.55) -> float: + """The steering command: the action, or a turn's FSD-timed profile while one is being taken (blended at the + ends). Curvature, yaw and yaw rate share the plan's sign convention; turn_sign is the held turn cue (0 = none). + lead is lat_action_t: the template is the curvature the car had, so the command looks it up that much ahead.""" + remaining = float(plan_yaw[-1]) # heading the plan still turns over its horizon + if not self.active: + cued = turn_sign != 0. and np.sign(turn_sign) == np.sign(remaining) + wanted = abs(remaining) >= (MIN_TURN if cued else MIN_TURN_NO_CUE) and v_ego < MAX_SPEED + self.confirm = self.confirm + 1 if wanted and (self.confirm == 0 or np.sign(remaining) == self.direction) else 0 + self.direction = float(np.sign(remaining)) if wanted else 0. + if self.confirm >= CONFIRM_FRAMES: + self.active, self.done, self.total, self.past_mid, self.since_mid = True, 0., abs(remaining), False, 0. + self.weight = max(self.weight - dt / BLEND_OUT_S, 0.) + command = self.weight * self.last + (1. - self.weight) * action + return float(command) + + self.done += self.direction * yaw_rate * dt + estimate = max(self.done + self.direction * remaining, self.done) + if self.past_mid: # past the half-heading point the plan's remaining heading starts to include the road beyond the + estimate = min(estimate, self.total) # corner, so the turn's size can only be revised down + self.total += 0.2 * (estimate - self.total) + half = self.total / 2. + if not self.past_mid and self.done >= half: + self.past_mid, self.v_mid = True, v_ego + if self.past_mid: + # the template's time is distance from the half-heading point over the speed there; cars speed up out of a turn + self.since_mid += max(v_ego, 0.) * dt + tau = self.since_mid / max(self.v_mid, TIMING_SPEED) + else: + arc = np.concatenate([[0.], np.cumsum(np.hypot(np.diff(plan_x), np.diff(plan_y)))]) + heading = self.done + self.direction * np.asarray(plan_yaw) + reach = np.flatnonzero(heading >= half) + to_mid = float(np.interp(half, heading[:reach[0] + 1], arc[:reach[0] + 1])) if len(reach) and reach[0] > 0 else ( + 0. if len(reach) else float(arc[-1])) + tau = -to_mid / max(v_ego, TIMING_SPEED) + level = profile(tau + lead, self.v_mid if self.past_mid else v_ego, self.total) + self.last = self.direction * level + if self.mode == MODE_MAX: # the action keeps its own timing where it leads; the profile lifts it to FSD's turn + self.last = self.direction * max(level, self.direction * action) + self.weight = min(self.weight + dt / BLEND_IN_S, 1.) + ended = self.past_mid and level < END_LEVEL * rule_peak(self.v_mid) + phantom = not self.past_mid and self.direction * remaining < PHANTOM and self.done < PHANTOM + if ended or phantom or v_ego > MAX_SPEED + 2.: + self.active, self.confirm, self.direction = False, 0, 0. + return float(self.weight * self.last + (1. - self.weight) * action) diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index c1d2d2376b..0ad9337f09 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/openpilot/sunnypilot/modeld_v2/modeld.py @@ -53,7 +53,7 @@ from openpilot.sunnypilot.modeld_v2.steering_smoothing import lat_smooth_seconds from openpilot.sunnypilot.modeld_v2.turn_desire_refresh import TurnDesireRefresh, desire_input from openpilot.sunnypilot.modeld_v2.turn_plan_floor import apply_turn_plan_floor from openpilot.sunnypilot.modeld_v2 import path_steering -from openpilot.sunnypilot.modeld_v2.turn_correction import apply_turn_correction, correction_for +from openpilot.sunnypilot.modeld_v2.fsd_turn_profile import FsdTurnProfile from openpilot.sunnypilot.modeld_v2.plan_long import PlanLong from openpilot.sunnypilot.modeld_v2.navigation_input import (NavigationInputBuffer, NavigationStatePublisher, SignalRouteGuard, destination_route_context, navigation_capability) @@ -107,7 +107,7 @@ class ModelState(ModelStateBase): model_bundle = get_active_bundle(chestnut=chestnut) self.generation = model_bundle.generation if model_bundle is not None else None self.model_name = model_bundle.displayName if model_bundle is not None else '' - self.turn_correction = correction_for(model_bundle.ref) if model_bundle is not None else None + self.fsd_turn = FsdTurnProfile() overrides = {override.key: override.value for override in model_bundle.overrides} if model_bundle else {} self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0")) @@ -270,25 +270,25 @@ class ModelState(ModelStateBase): desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2 self.action_curvature, self.turn_plan_curvature = desired_curvature, 0. plan = model_output['plan'][0] - if path_steering_enabled and self.turn_correction is not None: - # this model has a learned turn correction toward the FSD/driver line; it replaces the floor and the path cap + if turn_sign: + # the action head under-commits in slow turns while the model's own plan has the turn; use the plan as a floor self.turn_plan_curvature = get_curvature_from_plan(plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan[:, Plan.ORIENTATION_RATE][:, 2], self.constants.T_IDXS, v_ego, lat_action_t) - path = path_steering.path_curvature(plan[:, Plan.POSITION][:, 0], plan[:, Plan.POSITION][:, 1], - plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan[0, Plan.ORIENTATION_RATE][2], - v_ego, a_ego, lat_action_t) - desired_curvature = apply_turn_correction(self.turn_correction, desired_curvature, self.turn_plan_curvature, path, v_ego) - else: - if turn_sign: - # the action head under-commits in slow turns while the model's own plan has the turn; use the plan as a floor - self.turn_plan_curvature = get_curvature_from_plan(plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan[:, Plan.ORIENTATION_RATE][:, 2], - self.constants.T_IDXS, v_ego, lat_action_t) - desired_curvature = apply_turn_plan_floor(desired_curvature, self.turn_plan_curvature, turn_sign) - if path_steering_enabled and path_steering.path_weight(v_ego) > 0.: + desired_curvature = apply_turn_plan_floor(desired_curvature, self.turn_plan_curvature, turn_sign) + if path_steering_enabled: + if path_steering.path_weight(v_ego) > 0.: path = path_steering.path_curvature(plan[:, Plan.POSITION][:, 0], plan[:, Plan.POSITION][:, 1], plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan[0, Plan.ORIENTATION_RATE][2], v_ego, a_ego, lat_action_t) desired_curvature = path_steering.apply_path_steering(desired_curvature, path, v_ego) + # in a turn the plan shows, steer FSD's turn profile: the plan says where the corner is and how much it turns, + # FSD's turns say when to start and how hard (fsd_turn_profile.py); outside one this returns the command above + yaw_rate = float(model_output['pose'][0, 5]) if 'pose' in model_output else 0. + desired_curvature = self.fsd_turn.update(plan[:, Plan.POSITION][:, 0], plan[:, Plan.POSITION][:, 1], + plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], yaw_rate, v_ego, DT_MDL, + desired_curvature, turn_sign, lead=lat_action_t) + else: + self.fsd_turn.reset() if blendful is not None: desired_curvature += blendful(desired_curvature) diff --git a/openpilot/sunnypilot/modeld_v2/tests/test_fsd_turn_profile.py b/openpilot/sunnypilot/modeld_v2/tests/test_fsd_turn_profile.py new file mode 100644 index 0000000000..38fc6241e0 --- /dev/null +++ b/openpilot/sunnypilot/modeld_v2/tests/test_fsd_turn_profile.py @@ -0,0 +1,88 @@ +import numpy as np +import pytest + +from openpilot.sunnypilot.modeld_v2 import fsd_turn_profile as ftp + +T = np.linspace(0., 10., 33) ** 2 / 10. # like ModelConstants.T_IDXS +DT = 0.05 +RIGHT_90 = float(np.radians(90.)) + + +def corner_plan(distance_to_corner, turn, v): + """A plan driving straight distance_to_corner metres, then turning `turn` rad over an arc of radius 8 m.""" + s = v * T + radius = 8. + arc = abs(turn) * radius + yaw = np.sign(turn) * np.clip((s - distance_to_corner) / radius, 0., abs(turn)) + x = np.concatenate([[0.], np.cumsum(np.cos(yaw[1:]) * np.diff(s))]) + y = np.concatenate([[0.], np.cumsum(np.sin(yaw[1:]) * np.diff(s))]) + return x, y, yaw, arc + + +def drive(turn=RIGHT_90, v=5., start=40., cue=1., steps=400, action=None): + """Toy closed loop: the car takes each command 0.55 s later (the lead the controller looks ahead by), the plan shows + the corner where it still is, and the model's action steers what the turn still needs over the next 8 m.""" + controller = ftp.FsdTurnProfile() + heading, travelled = 0., 0. + commands, delay = [], round(0.55 / DT) + for _ in range(steps): + remaining = turn - heading + x, y, yaw, _ = corner_plan(max(start - travelled, 0.), remaining, v) + model = action if action is not None else (float(np.clip(remaining / 8., -0.2, 0.2)) if travelled > start - 5. else 0.) + applied = commands[-delay] if len(commands) >= delay else 0. + commands.append(controller.update(x, y, yaw, applied * v, v, DT, model, cue)) + heading += applied * v * DT + travelled += v * DT + return np.asarray(commands), heading, controller + + +def test_a_cued_right_turn_follows_the_speed_rule_and_completes(): + commands, heading, controller = drive() + assert np.max(commands) == pytest.approx(ftp.rule_peak(5.), rel=0.1) + assert np.degrees(heading) == pytest.approx(90., abs=8.) + assert not controller.active and abs(commands[-1]) < 0.01 + + +def test_left_turns_mirror_right_turns(): + right, _, _ = drive(turn=np.radians(90.), cue=1.) + left, _, _ = drive(turn=np.radians(-90.), cue=-1.) + assert np.allclose(left, -right, atol=1e-6) + + +def test_no_cue_needs_a_bigger_turn(): + commands, _, controller = drive(turn=np.radians(50.), cue=0., action=0.) + assert np.max(np.abs(commands)) == 0. + commands, _, _ = drive(turn=np.radians(90.), cue=0.) + assert np.max(commands) > 0.05 + + +def test_road_speed_is_left_to_the_model(): + commands, _, _ = drive(v=12., action=0.01) + assert np.allclose(commands, 0.01) + + +def test_a_turn_the_plan_stops_showing_is_dropped_back_to_the_action(): + controller = ftp.FsdTurnProfile() + x, y, yaw, _ = corner_plan(30., np.radians(90.), 5.) + for _ in range(20): + controller.update(x, y, yaw, 0., 5., DT, 0.02, 1.) + assert controller.active + straight = (5. * T, np.zeros_like(T), np.zeros_like(T)) + for _ in range(40): + command = controller.update(*straight, 0., 5., DT, 0.02, 1.) + assert not controller.active and command == pytest.approx(0.02, abs=1e-3) + + +def test_the_peak_rule_holds_lateral_accel_past_its_speeds(): + assert ftp.rule_peak(1.) == ftp.rule_peak(3.) + assert ftp.rule_peak(10.) * 10. ** 2 == pytest.approx(ftp.rule_peak(8.) * 8. ** 2) + assert ftp.rule_peak(3.) == pytest.approx(ftp.PEAK_SCALE * (0.190 - 0.018 * 3.)) + + +def test_pulling_away_from_a_stop_winds_in_before_the_car_is_moving(): + controller = ftp.FsdTurnProfile() + x, y, yaw, _ = corner_plan(4., np.radians(90.), 3.) + command = 0. + for _ in range(40): # stopped 4 m from the corner with the turn cued + command = controller.update(x, y, yaw, 0., 0.2, DT, 0., 1.) + assert command > 0.03 diff --git a/openpilot/sunnypilot/modeld_v2/tests/test_turn_correction.py b/openpilot/sunnypilot/modeld_v2/tests/test_turn_correction.py deleted file mode 100644 index d62af8dc40..0000000000 --- a/openpilot/sunnypilot/modeld_v2/tests/test_turn_correction.py +++ /dev/null @@ -1,35 +0,0 @@ -import pytest - -from openpilot.sunnypilot.modeld_v2 import turn_correction as tc - -C = tc.TurnCorrection('test', v_full=6., v_end=10., theta_path=0.1, theta_time=0.8) - - -def test_no_change_at_road_speed_and_full_below_v_full(): - assert tc.apply_turn_correction(C, 0.05, 0.10, 0.12, 12.) == 0.05 - full = tc.apply_turn_correction(C, 0.05, 0.10, 0.12, 3.) - assert full == pytest.approx(0.05 + 0.1 * 0.07 + 0.8 * 0.05) - half = tc.apply_turn_correction(C, 0.05, 0.10, 0.12, 8.) - assert half == pytest.approx(0.05 + 0.5 * (full - 0.05)) - - -def test_it_stays_within_the_models_own_range(): - # the plan's time axis says far more than both the action and the line at the car's speed: bounded at path + 10% - assert tc.apply_turn_correction(C, 0.10, 0.40, 0.14, 3.) == pytest.approx(0.14 + 0.1 * 0.04) - # a turn the action and the line both say is straight is not invented from the time axis - assert tc.apply_turn_correction(C, 0.0, 0.2, 0.0, 3.) == 0.0 - # nor reversed: action and line right, the time axis left - assert tc.apply_turn_correction(C, 0.1, -0.2, 0.1, 3.) == pytest.approx(0.1) - - -def test_it_can_lower_an_overcommitted_action_toward_the_line(): - lower = tc.TurnCorrection('test', 4.5, 7., theta_path=0.9, theta_time=0.) - assert tc.apply_turn_correction(lower, -0.21, -0.21, -0.14, 3.) == pytest.approx(-0.21 + 0.9 * 0.07) - - -def test_lookup_by_ref_and_local_prefix(monkeypatch): - monkeypatch.setattr(tc, 'CORRECTIONS', {'abc': C, 'local-ff_turns_600-': C}) - assert tc.correction_for('abc') is C - assert tc.correction_for('local-ff_turns_600-0123456789abcdef') is C - assert tc.correction_for('local-other-0123') is None - assert tc.correction_for('') is None diff --git a/openpilot/sunnypilot/modeld_v2/turn_correction.py b/openpilot/sunnypilot/modeld_v2/turn_correction.py deleted file mode 100644 index 86fcf6ba9c..0000000000 --- a/openpilot/sunnypilot/modeld_v2/turn_correction.py +++ /dev/null @@ -1,56 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -from dataclasses import dataclass - -import numpy as np - -# Learned turn correction: per model, a small correction on top of its action toward the line FSD and the driver -# took, fitted by openpilot.distill's supervised/fit_turn_correction.py on open-loop frames of driver/FSD turns and -# closed-loop frames of the user's engaged drives (where the driver or FSD was steering, or a turn openpilot steered -# with no push-back), scored leave-one-route-out. It replaces the 0.75 turn plan floor and the path cap for a model -# that has one. With -# time = the plan's curvature at action_t on its time axis (get_curvature_from_plan) -# path = the plan's heading at the arc length the car covers in action_t (path_steering.path_curvature) -# command = action + s(v) * (theta_path * (path - action) + theta_time * (time - action)), s = 1 below v_full and 0 -# from v_end, and the command stays within the range action..path spans (plus 10%), so it can only move the action -# toward the model's own line, never invent or reverse a turn. -MARGIN = 0.1 - - -@dataclass(frozen=True) -class TurnCorrection: - name: str - v_full: float - v_end: float - theta_path: float - theta_time: float - - -# bundle ref (or a local build's ref prefix ending in '-') -> correction -CORRECTIONS: dict[str, TurnCorrection] = { - # Nov 9 2026, closed-loop frames of routes 98, 9a, 9e, 9f, b0 (26 turns, 3640 frames). Held out by route, mean - # |command - driven| 1-3 m/s 0.039 -> 0.021 (0.75 floor 0.029), 3-4.5 m/s 0.028 -> 0.014, 4.5-6 m/s 0.017 -> 0.008, - # 6-9 m/s 0.006 -> 0.004; peak command / peak driven per turn 0.74 (floor 0.83) -> 1.02 [10-90%: 0.84-1.26]. - 'b92fc4828664f8767bce5e92f1630ed319cbe509': TurnCorrection('Light Speed 2', v_full=6.0, v_end=10.0, - theta_path=0.0554, theta_time=0.8708), -} - - -def correction_for(ref: str) -> TurnCorrection | None: - if ref in CORRECTIONS: - return CORRECTIONS[ref] - return next((c for key, c in CORRECTIONS.items() if key.endswith('-') and ref.startswith(key)), None) - - -def apply_turn_correction(correction: TurnCorrection, action: float, time: float, path: float, v_ego: float) -> float: - weight = float(np.clip((correction.v_end - v_ego) / (correction.v_end - correction.v_full), 0., 1.)) - if weight == 0.: - return action - raw = action + weight * (correction.theta_path * (path - action) + correction.theta_time * (time - action)) - lo, hi = min(action, path), max(action, path) - margin = MARGIN * (hi - lo) - return float(np.clip(raw, lo - margin, hi + margin))