mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-11-21 03:33:47 +08:00
Compare commits
28 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| be43e286e4 | |||
| adc286412a | |||
| e254fcb2d1 | |||
| 7462078529 | |||
| d3b041495b | |||
| 9041ce516d | |||
| 98fb0138f6 | |||
| 5ebc89b3e9 | |||
| da6f2ead1f | |||
| c36b2a87f2 | |||
| feaf52c1f2 | |||
| 9cd1e24e13 | |||
| 22d4bf0b66 | |||
| dd8f558f90 | |||
| d9b22cd0ca | |||
| ae1dad23ee | |||
| 54a49cfb7c | |||
| 42bce6ea07 | |||
| 3251fe3d0f | |||
| 39a62b6e64 | |||
| d3ba463e14 | |||
| c60051860b | |||
| 051599a996 | |||
| 0b5620ef67 | |||
| 7bbb2472ce | |||
| 5cc028dbb4 | |||
| 94058fe486 | |||
| 5ae8e98f9b |
@@ -121,7 +121,12 @@ jobs:
|
||||
grep -qs "^version https://git-lfs" "$f" || continue
|
||||
OID=$(awk -F'sha256:' '/^oid/{print $2}' "$f"); SIZE=$(awk '/^size/{print $2}' "$f")
|
||||
HREF=$(curl -fsSL --retry 3 -X POST -H "Accept: application/vnd.git-lfs+json" -H "Content-Type: application/vnd.git-lfs+json" "$LFS_URL/objects/batch" \
|
||||
-d "{\"operation\":\"download\",\"transfer\":[\"basic\"],\"objects\":[{\"oid\":\"$OID\",\"size\":$SIZE}]}" | jq -r '.objects[0].actions.download.href')
|
||||
-d "{\"operation\":\"download\",\"transfer\":[\"basic\"],\"objects\":[{\"oid\":\"$OID\",\"size\":$SIZE}]}" | jq -r '.objects[0].actions.download.href // empty')
|
||||
if [ -z "$HREF" ]; then
|
||||
HF_PATH=$(curl -fsSL --retry 3 "https://huggingface.co/api/models/commaai/openpilot_driving_models/tree/main?recursive=true" | jq -r --arg oid "$OID" 'map(select(.lfs.oid == $oid))[0].path // empty')
|
||||
[ -n "$HF_PATH" ] || { echo "::error::$f ($OID) is in neither openpilot's LFS nor commaai/openpilot_driving_models"; exit 1; }
|
||||
HREF="https://huggingface.co/commaai/openpilot_driving_models/resolve/main/$HF_PATH"
|
||||
fi
|
||||
aria2c -x 16 -s 16 -k 16M --max-tries=5 --retry-wait=5 --file-allocation=none --summary-interval=30 --allow-overwrite=true \
|
||||
-d "$(dirname "$f")" -o "$(basename "$f")" "$HREF"
|
||||
echo "$OID $f" | sha256sum -c -
|
||||
@@ -129,8 +134,9 @@ jobs:
|
||||
}
|
||||
UPSTREAM_PKL=selfdrive/modeld/models/big_driving_tinygrad.pkl
|
||||
if [ "${{ inputs.target_hardware }}" != "chestnut" ]; then
|
||||
git lfs pull -X "**/selfdrive/modeld/models/big_*.onnx,**/selfdrive/modeld/models/dmonitoring_*.onnx,**/selfdrive/modeld/models/big_*.pkl,**/selfdrive/modeld/models/dmonitoring_*.pkl"
|
||||
git lfs pull -X "**/selfdrive/modeld/models/big_*.onnx,**/selfdrive/modeld/models/dmonitoring_*.onnx,**/selfdrive/modeld/models/big_*.pkl,**/selfdrive/modeld/models/dmonitoring_*.pkl" || true
|
||||
rm -f selfdrive/modeld/models/big_*.onnx selfdrive/modeld/models/dmonitoring_*.onnx selfdrive/modeld/models/big_*.pkl selfdrive/modeld/models/dmonitoring_*.pkl
|
||||
fetch_lfs selfdrive/modeld/models/*.onnx
|
||||
elif compgen -G "selfdrive/modeld/models/big_*.onnx" > /dev/null; then
|
||||
fetch_lfs selfdrive/modeld/models/big_*.onnx selfdrive/modeld/models/big_*.pkl
|
||||
find selfdrive/modeld/models -type f \( -name "*.onnx" -o -name "*.pkl" \) ! -name "big_*.onnx" ! -name "big_*.pkl" -delete
|
||||
|
||||
@@ -28,6 +28,8 @@ class DesireHelper:
|
||||
self.alc = AutoLaneChangeController(self)
|
||||
self.lane_turn_controller = LaneTurnController(self)
|
||||
self.lane_turn_direction = TurnDirection.none
|
||||
self.turn_frames = 0
|
||||
self.turn_repulse = 98
|
||||
|
||||
@staticmethod
|
||||
def get_lane_change_direction(CS):
|
||||
@@ -95,8 +97,10 @@ class DesireHelper:
|
||||
self.prev_one_blinker = one_blinker and lateral_active
|
||||
|
||||
if self.lane_turn_direction != TurnDirection.none:
|
||||
self.desire = TURN_DESIRES[self.lane_turn_direction]
|
||||
self.turn_frames += 1
|
||||
self.desire = TURN_DESIRES[self.lane_turn_direction] if self.turn_frames % self.turn_repulse else log.Desire.none
|
||||
else:
|
||||
self.turn_frames = 0
|
||||
self.desire = log.Desire.none
|
||||
if self.lane_change_state == LaneChangeState.laneChangeStarting:
|
||||
if self.lane_change_direction == LaneChangeDirection.left:
|
||||
|
||||
@@ -3,7 +3,7 @@ from collections.abc import Callable
|
||||
import base64
|
||||
import ctypes
|
||||
import gc
|
||||
from functools import cached_property
|
||||
from functools import cached_property, partial
|
||||
import os
|
||||
os.environ['GMMU'] = '0' # for chestnut fast loading, noop for qcom
|
||||
os.environ.setdefault('AM_POWER_LIMIT', '100')
|
||||
@@ -43,6 +43,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
|
||||
from openpilot.selfdrive.modeld.helpers import MODELS_DIR, chestnut_present, chestnut_compiled, modeld_pkl_path, load_oob, wait_for_chestnut
|
||||
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
||||
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
|
||||
from openpilot.sunnypilot.modeld_v2.turn_planner import TurnPlanner
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
|
||||
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
@@ -54,7 +55,8 @@ BIG_MODEL_TIMEOUT = 60
|
||||
|
||||
|
||||
def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
|
||||
lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action:
|
||||
lat_action_t: float, long_action_t: float, v_ego: float,
|
||||
turn_shaper: Callable[[float], float] | None = None) -> log.ModelDataV2.Action:
|
||||
if 'action' not in model_output:
|
||||
plan = model_output['plan'][0]
|
||||
desired_accel = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0],
|
||||
@@ -69,6 +71,8 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
|
||||
else:
|
||||
desired_accel = model_output['action'][0,1]
|
||||
desired_curvature = model_output['action'][0,0] / (max(1.0, v_ego))**2
|
||||
if turn_shaper is not None:
|
||||
desired_curvature = turn_shaper(desired_curvature)
|
||||
stop = should_stop(v_ego, desired_accel)
|
||||
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
|
||||
if v_ego > MIN_LAT_CONTROL_SPEED:
|
||||
@@ -340,6 +344,7 @@ def main(demo=False):
|
||||
|
||||
DH = DesireHelper()
|
||||
RELC = RoadEdgeLaneChangeController()
|
||||
turn_planner = TurnPlanner(CP.wheelbase)
|
||||
|
||||
while True:
|
||||
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
|
||||
@@ -448,7 +453,9 @@ def main(demo=False):
|
||||
|
||||
if not sm['carControl'].latActive:
|
||||
prev_action.desiredCurvature = sm['controlsState'].curvature
|
||||
action = get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
|
||||
action = get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego,
|
||||
partial(turn_planner.update, model_output, v_ego, sm['controlsState'].curvature,
|
||||
sm['carControl'].latActive, is_rhd))
|
||||
prev_action = action
|
||||
fill_model_msg(modelv2_send, model_output, action,
|
||||
publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
|
||||
|
||||
@@ -1,3 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:65a08adc31d5c456219687d99b7bf5e44d61dae2d49ea67850e76105c7248cce
|
||||
size 60918562
|
||||
oid sha256:416c99bbd5563b01518be6de91b856986f66ffa03451ea9c451736dcc99896fd
|
||||
size 83976077
|
||||
|
||||
@@ -8,10 +8,10 @@ import numpy as np
|
||||
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
|
||||
|
||||
LANE_WIDTH_DEFAULT = 3.6 # m
|
||||
LANE_WIDTH_CLIP = (2.8, 4.5) # m
|
||||
LANE_WIDTH_CLIP = (2.4, 4.5) # m
|
||||
LOOKAHEAD_TIME = 1.5 # s
|
||||
MIN_LOOKAHEAD = 10.0 # m
|
||||
MIN_SPEED = 5.0 # m/s
|
||||
@@ -19,6 +19,11 @@ LINE_STD = (0.2, 0.6)
|
||||
POINT_STD = 0.1
|
||||
YIELD = (0.2, 0.45)
|
||||
X_IDXS = np.array(ModelConstants.X_IDXS)
|
||||
JUMP = 0.5
|
||||
JUMP_TAU = 0.5
|
||||
JUMP_ACCEPT = 3.0
|
||||
WIDEN = 0.4
|
||||
YAW_RATE = Plan.ORIENTATION_RATE.start + 2 # matched .991 correlation to actual car
|
||||
|
||||
|
||||
class Blendful:
|
||||
@@ -36,7 +41,8 @@ class Blendful:
|
||||
self.l_prob = 0.0
|
||||
self.r_prob = 0.0
|
||||
self.model_curvature = 0.0
|
||||
self.smooth = 0.0 # smooths the correction target below .53 lat accel
|
||||
self.ref = None
|
||||
self.jumped = np.zeros(2)
|
||||
self.curvature = 0.0
|
||||
|
||||
def update(self, model_output: dict[str, np.ndarray], v_ego: float, active: bool, model_curvature: float, enabled: bool = True) -> float:
|
||||
@@ -52,8 +58,8 @@ class Blendful:
|
||||
self.usual_offset.initialized = False
|
||||
self.lane_width.initialized = False
|
||||
self.going_around = False
|
||||
self.ref = None
|
||||
target = 0.0 if fading else self._target(model_output, v_ego, model_curvature)
|
||||
target = self.smooth = self.smooth + (target - self.smooth) * DT_MDL / (DT_MDL + np.interp(abs(model_curvature) * v_ego ** 2, [0.18, 0.53], [0.5, 0.0]))
|
||||
build = 0.5 / max(v_ego, 1.0) ** 2 * DT_MDL # jerk over 1 second
|
||||
release = (0.5 if fading else 2.0) / max(v_ego, 1.0) ** 2 * DT_MDL # lets go fast, but past zero it builds like any other pull
|
||||
k = self.curvature
|
||||
@@ -63,6 +69,10 @@ class Blendful:
|
||||
def _target(self, model_output: dict[str, np.ndarray], v_ego: float, model_curvature: float) -> float:
|
||||
lookahead = max(v_ego * LOOKAHEAD_TIME, MIN_LOOKAHEAD)
|
||||
lines = model_output['lane_lines'][0, :, :, 0]
|
||||
plan = model_output['plan'][0]
|
||||
self._jumps(lines[1:3], v_ego, plan[0, YAW_RATE] if plan.shape[-1] > YAW_RATE else model_curvature * v_ego,
|
||||
np.interp(abs(model_curvature) * v_ego ** 2, [0.18, 0.53], [JUMP_TAU, 0.0]), lookahead)
|
||||
held = bool(np.any((self.jumped > 0) & (self.jumped < JUMP_ACCEPT)))
|
||||
stds = model_output['lane_lines_stds'][0, :, :, 0]
|
||||
|
||||
lone_weight = np.interp(abs(model_curvature) * v_ego ** 2, [0.18, 0.53], [0.5, 1.0]) # 0.5 on straights, 1.0 in curves
|
||||
@@ -83,6 +93,8 @@ class Blendful:
|
||||
lane_trust = min(l_prob, r_prob)
|
||||
else:
|
||||
lane_trust = l_prob + r_prob - l_prob * r_prob
|
||||
if held or (l_prob * r_prob > 0.5 and np.interp(lookahead, X_IDXS, lines[2] - lines[1]) > self.lane_width.x + WIDEN):
|
||||
lane_trust = 0.0
|
||||
if lone_weight > 0.5 and lane_trust < self.line_trust:
|
||||
lane_trust = self.line_trust + (lane_trust - self.line_trust) * DT_MDL / (lone_weight - 0.5 + DT_MDL)
|
||||
self.line_trust = lane_trust
|
||||
@@ -96,7 +108,7 @@ class Blendful:
|
||||
win = (X_IDXS >= 0.5 * lookahead) & (X_IDXS <= 1.5 * lookahead)
|
||||
x = X_IDXS[win]
|
||||
half_width = self.lane_width.x / 2
|
||||
if l_prob * r_prob > 0.5:
|
||||
if l_prob * r_prob > 0.5 and not held:
|
||||
width = np.interp(lookahead, X_IDXS, lines[2]) - np.interp(lookahead, X_IDXS, lines[1])
|
||||
self.lane_width.update(float(np.clip(width, *LANE_WIDTH_CLIP)))
|
||||
half_width = np.clip(lines[2, win] - lines[1, win], *LANE_WIDTH_CLIP) / 2
|
||||
@@ -143,3 +155,14 @@ class Blendful:
|
||||
self.trust = lane_trust if lane_trust < self.trust else self.trust + (lane_trust - self.trust) * DT_MDL / (1.5 + DT_MDL)
|
||||
self.dk = dk
|
||||
return float(self.trust * dk)
|
||||
|
||||
def _jumps(self, raw: np.ndarray, v_ego: float, yaw_rate: float, tau: float, lookahead: float) -> None:
|
||||
if self.ref is None:
|
||||
self.ref, self.jumped = raw.copy(), np.zeros(2)
|
||||
return
|
||||
ref = np.array([np.interp(X_IDXS + v_ego * DT_MDL, X_IDXS, line) for line in self.ref]) - yaw_rate * DT_MDL * X_IDXS
|
||||
off = np.abs([np.interp(lookahead, X_IDXS, d) for d in raw - ref]) > JUMP
|
||||
self.jumped = np.where(off & ~off[::-1], self.jumped + DT_MDL, 0.0)
|
||||
self.ref = ref + (raw - ref) * DT_MDL / (DT_MDL + tau)
|
||||
accept = (self.jumped >= JUMP_ACCEPT) | off.all()
|
||||
self.ref[accept] = raw[accept]
|
||||
|
||||
@@ -17,16 +17,28 @@ def main():
|
||||
parser.add_argument("--frame", required=True, help="width,height,stride,y_height,uv_height,buffer_size")
|
||||
parser.add_argument("--warp-to", required=True, help="model input WxH")
|
||||
parser.add_argument("--output", required=True)
|
||||
parser.add_argument("--light", action="store_true")
|
||||
parser.add_argument("--split-frames", action="store_true", help="one input per camera (frame, big_frame), bound to the camera buffers by pointer")
|
||||
args = parser.parse_args()
|
||||
|
||||
from examples.openpilot.compile_warp import make_frame_prepare, parse_frame, parse_size
|
||||
from tinygrad import Device, Tensor, TinyJit
|
||||
frame = parse_frame(args.frame)
|
||||
warp = make_frame_prepare(frame, *parse_size(args.warp_to))
|
||||
run = TinyJit(lambda input_frame, M_inv: Tensor.stack(*(warp(input_frame[i], M_inv[i]) for i in range(2))), prune=True)
|
||||
n = 1 if args.light else 2
|
||||
cams = list(range(n))
|
||||
rng = np.random.default_rng(42)
|
||||
inputs = {"input_frame": Tensor(rng.integers(0, 256, (2, frame.size), dtype=np.uint8), device=Device.DEFAULT).realize(),
|
||||
"M_inv": Tensor((rng.standard_normal((2, 3, 3)) * 8).astype(np.float32), device=Device.DEFAULT).realize()}
|
||||
frames = rng.integers(0, 256, (2, frame.size), dtype=np.uint8)
|
||||
m_inv = Tensor((rng.standard_normal((n, 3, 3)) * 8).astype(np.float32), device=Device.DEFAULT).realize()
|
||||
if args.split_frames:
|
||||
names = [("frame", "big_frame")[c] for c in cams]
|
||||
run = TinyJit(lambda M_inv, **f: Tensor.stack(*(warp(f[nm], M_inv[i]) for i, nm in enumerate(names))), prune=True)
|
||||
inputs = {nm: Tensor(frames[c], device=Device.DEFAULT).realize() for nm, c in zip(names, cams, strict=True)} | {"M_inv": m_inv}
|
||||
specs = {nm: ((frame.size,), "|u1", Device.DEFAULT) for nm in names} | {"M_inv": ((n, 3, 3), "<f4", Device.DEFAULT)} # noqa: C420
|
||||
else:
|
||||
run = TinyJit(lambda input_frame, M_inv: Tensor.stack(*(warp(input_frame[c], M_inv[i]) for i, c in enumerate(cams))), prune=True)
|
||||
inputs = {"input_frame": Tensor(frames, device=Device.DEFAULT).realize(), "M_inv": m_inv}
|
||||
specs = {"input_frame": ((2, frame.size), "|u1", Device.DEFAULT), "M_inv": ((n, 3, 3), "<f4", Device.DEFAULT)}
|
||||
with record_kernel_linear() as recorded:
|
||||
run(**inputs)
|
||||
run(**inputs)
|
||||
@@ -34,7 +46,6 @@ def main():
|
||||
assert len(recorded) == 1, f"expected one captured kernel list, got {len(recorded)}"
|
||||
part = serialize_jit(run, recorded[0], check)
|
||||
PlainJit(*part)
|
||||
specs = {"input_frame": ((2, frame.size), "|u1", Device.DEFAULT), "M_inv": ((2, 3, 3), "<f4", Device.DEFAULT)}
|
||||
write_bundle(args.output, {"run": part}, {"input_specs": specs, "metadata": {}})
|
||||
print(f"Saved warp to {args.output} ({len(part[0]['calls'])} kernels)")
|
||||
|
||||
|
||||
@@ -19,6 +19,13 @@ def pack_layout(shapes: dict[str, tuple], frame_size: int) -> tuple[dict[str, in
|
||||
offsets[name], offset = offset, offset + -(-math.prod(shape) * 4 // 128) * 128
|
||||
return offsets | {"frames": offset}, offset + 2 * frame_size
|
||||
|
||||
EXTRA_WARPS = {"light_img": "light_tfm"}
|
||||
SPLIT_FRAMES = ("big_frame", "frame")
|
||||
|
||||
def host_shapes(input_shapes: dict[str, tuple], states) -> dict[str, tuple]:
|
||||
shapes = {name: shape for name, shape in input_shapes.items() if name not in states and name != "new_img" and name not in EXTRA_WARPS}
|
||||
return shapes | {tfm: (1, 3, 3) for img, tfm in EXTRA_WARPS.items() if img in input_shapes}
|
||||
|
||||
|
||||
class _Blobs: # the model's and the warp's blobs as one address space, for a part whose ranges span both files
|
||||
def __init__(self, *blobs):
|
||||
@@ -30,40 +37,59 @@ class _Blobs: # the model's and the warp's blobs as one address space, for a pa
|
||||
|
||||
|
||||
def fused_with_warp(cam_w: int, cam_h: int, chestnut: bool): # load_model's extend: a native model runs upload, warp and model as one submit
|
||||
return partial(frame_part, f"{BASEDIR}/openpilot/sunnypilot/modeld_v2/models/{'big_' if chestnut else ''}driving_warp_{cam_w}x{cam_h}_tinygrad.pkl")
|
||||
models, big = f"{BASEDIR}/openpilot/sunnypilot/modeld_v2/models", 'big_' if chestnut else ''
|
||||
return partial(frame_part, f"{models}/{big}driving_warp_{cam_w}x{cam_h}_tinygrad.pkl",
|
||||
light_warp_path=f"{models}/{big}driving_lightwarp_{cam_w}x{cam_h}_tinygrad.pkl")
|
||||
|
||||
|
||||
def frame_part(warp_path: str, parts: dict, extras: dict) -> dict:
|
||||
def frame_part(warp_path: str, parts: dict, extras: dict, light_warp_path: str | None = None) -> dict:
|
||||
if "input_specs" not in extras: # a legacy model warps in its own parts
|
||||
return {}
|
||||
(model, model_blob), (warp, warp_blob) = parts["run"], read_parts(warp_path)[0]["run"]
|
||||
states = sorted(name for name in extras["input_specs"] if f"next_{name}" in extras["metadata"]["output_shapes"])
|
||||
host = {name: shape for name, (shape, _, _) in extras["input_specs"].items() if name not in states and name != "new_img"}
|
||||
frame_size = next(s["numel"] for s in warp["inputs"] if s["name"] == "input_frame") // 2
|
||||
offsets, packed = pack_layout(host, frame_size)
|
||||
shift, upload, u8 = len(model_blob), len(model["buffers"]) + len(warp["buffers"]), repr(_dtype("dtypes.uint8"))
|
||||
host = host_shapes({name: shape for name, (shape, _, _) in extras["input_specs"].items()}, states)
|
||||
paths = {"light_img": light_warp_path}
|
||||
extra = [(img, tfm, *read_parts(paths[img])[0]["run"]) for img, tfm in EXTRA_WARPS.items() if img in extras["input_specs"]]
|
||||
split = any(s["name"] in SPLIT_FRAMES for s in warp["inputs"])
|
||||
assert all(any(s["name"] in SPLIT_FRAMES for s in part["inputs"]) == split for _, _, part, _ in extra), "camera and extra warps must share a frame layout"
|
||||
offsets, packed = pack_layout(host, 0 if split else next(s["numel"] for s in warp["inputs"] if s["name"] == "input_frame") // 2)
|
||||
shift = len(model_blob)
|
||||
buf_base, blob_base = [len(model["buffers"]) + len(warp["buffers"])], [len(model_blob) + len(warp_blob)]
|
||||
for _, _, part, blob in extra:
|
||||
buf_base.append(buf_base[-1] + len(part["buffers"])); blob_base.append(blob_base[-1] + len(blob))
|
||||
upload, u8 = buf_base[-1], repr(_dtype("dtypes.uint8"))
|
||||
extra_at = {img: (part, buf_base[i], blob_base[i], tfm) for i, (img, tfm, part, _) in enumerate(extra)}
|
||||
|
||||
def view(name: str, nbytes: int, dtype: str) -> dict:
|
||||
return {"buffer": upload, "offset": offsets[name], "nbytes": nbytes, "dtype": dtype}
|
||||
outputs = sorted(s["slot"] for s in model["inputs"] if s["name"] is None)
|
||||
names = sorted(["host", *states])
|
||||
frame_specs = {s["name"]: s for _, _, part, _ in [(None, None, warp, None), *extra] for s in part["inputs"] if s["name"] in SPLIT_FRAMES}
|
||||
names = sorted(["host", *states, *frame_specs])
|
||||
slots = {s["slot"]: names.index(s["name"]) for s in model["inputs"] if s["name"] in states} | \
|
||||
{slot: len(names) + i for i, slot in enumerate(outputs)}
|
||||
returned = warp["returns"]["tensors"][0]
|
||||
def warped(part: dict, base: int) -> dict:
|
||||
returned = part["returns"]["tensors"][0]
|
||||
return {k: v for k, v in returned.items() if k != "shape"} | {"buffer": returned["buffer"] + base}
|
||||
model_inputs = {s["slot"]: {"input": slots[s["slot"]]} if s["slot"] in slots else
|
||||
{k: v for k, v in returned.items() if k != "shape"} | {"buffer": returned["buffer"] + len(model["buffers"])} if s["name"] == "new_img" else
|
||||
warped(warp, len(model["buffers"])) if s["name"] == "new_img" else
|
||||
warped(extra_at[s["name"]][0], extra_at[s["name"]][1]) if s["name"] in extra_at else
|
||||
view(s["name"], s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"]) for s in model["inputs"]}
|
||||
warp_inputs = {s["slot"]: view("frames" if s["name"] == "input_frame" else "tfm", s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"])
|
||||
for s in warp["inputs"]}
|
||||
def warp_inputs(part: dict, tfm: str) -> dict:
|
||||
return {s["slot"]: {"input": names.index(s["name"])} if s["name"] in frame_specs else
|
||||
view("frames" if s["name"] == "input_frame" else tfm, s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"]) for s in part["inputs"]}
|
||||
|
||||
def remap(calls: list, inputs: dict, buffers: int, lib: int) -> list:
|
||||
return [call | ({"lib": [call["lib"][0] + lib, call["lib"][1] + lib]} if "lib" in call else {}) |
|
||||
{"args": [dict(inputs[a["input"]]) if "input" in a else a | {"buffer": a["buffer"] + buffers} for a in call["args"]]} for call in calls]
|
||||
header = {"input_names": names, "returns": {"kind": "NoneType", "tensors": []}, "check": None,
|
||||
"inputs": [{"slot": names.index("host"), "name": "host", "dtype": u8, "numel": packed, "device": "NPY"}] +
|
||||
[s | {"slot": slots[s["slot"]]} for s in model["inputs"] if s["slot"] in slots],
|
||||
"buffers": model["buffers"] + [b | ({"init": [b["init"][0] + shift, b["init"][1] + shift]} if "init" in b else {}) for b in warp["buffers"]] +
|
||||
[s | {"slot": slots[s["slot"]]} for s in model["inputs"] if s["slot"] in slots] +
|
||||
[s | {"slot": names.index(name)} for name, s in frame_specs.items()],
|
||||
"buffers": model["buffers"] + [b | ({"init": [b["init"][0] + d, b["init"][1] + d]} if "init" in b else {})
|
||||
for part, d in ((warp, shift), *((p, bb) for p, _, bb, _ in extra_at.values())) for b in part["buffers"]] +
|
||||
[{"nbytes": packed, "device": model["inputs"][0]["device"], "dtype": u8}],
|
||||
"calls": [{"kind": "copy", "args": [{"buffer": upload, "offset": 0, "nbytes": packed, "dtype": u8}, {"input": names.index("host")}]}] +
|
||||
remap(warp["calls"], warp_inputs, len(model["buffers"]), shift) + remap(model["calls"], model_inputs, 0, 0)}
|
||||
return {"run": (header, _Blobs(model_blob, warp_blob))} # replaces the model's run: one set of buffers, no separate warp
|
||||
remap(warp["calls"], warp_inputs(warp, "tfm"), len(model["buffers"]), shift) +
|
||||
[c for p, b0, bb, tfm in extra_at.values() for c in remap(p["calls"], warp_inputs(p, tfm), b0, bb)] +
|
||||
remap(model["calls"], model_inputs, 0, 0)}
|
||||
return {"run": (header, _Blobs(model_blob, warp_blob, *(blob for *_, blob in extra)))}
|
||||
|
||||
@@ -270,15 +270,32 @@ def _program_uop(call: dict, lib: bytes):
|
||||
UOp(Ops.SOURCE, arg=""), UOp(Ops.BINARY, arg=lib)), arg=info)
|
||||
|
||||
|
||||
def _input_tensors(kwargs: dict) -> list: # in _prepare_jit_inputs' order
|
||||
from tinygrad import Tensor
|
||||
tensors = [t for _, t in sorted(kwargs.items()) if t.__class__ is Tensor]
|
||||
for value in kwargs.values():
|
||||
tensors += [t for t in (value.values() if isinstance(value, dict) else value if isinstance(value, (tuple, list)) else ())
|
||||
if t.__class__ is Tensor and not any(t is y for y in tensors)]
|
||||
return tensors
|
||||
|
||||
|
||||
class _Jit:
|
||||
def __init__(self, captured, input_names: list, buffers: list):
|
||||
self.captured, self.input_names, self.buffers = captured, input_names, buffers
|
||||
|
||||
def __call__(self, *args, **kwargs):
|
||||
import tinygrad.engine.jit as jit
|
||||
from tinygrad.uop.ops import Ops
|
||||
# same names, no variables: the binding is the tensors' own uops, without _prepare_jit_inputs' graph rewrites
|
||||
if not args and self.__dict__.get("_names") == sorted(kwargs):
|
||||
tensors = _input_tensors(kwargs)
|
||||
if all(t.uop.is_realized for t in tensors):
|
||||
return self.captured([t.uop.src[0] if t.uop.op is Ops.UNSHARD else t.uop for t in tensors], {})
|
||||
input_uops, var_vals, names, _ = jit._prepare_jit_inputs(args, kwargs)
|
||||
if names != self.input_names:
|
||||
raise ValueError(f"expected inputs {self.input_names}, got {names}")
|
||||
if not args and not var_vals:
|
||||
self._names = sorted(kwargs)
|
||||
return self.captured(input_uops, var_vals)
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,22 @@
|
||||
"""
|
||||
Copyright (c) 2021-, 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
|
||||
|
||||
from openpilot.common.transformations.camera import get_view_frame_from_calib_frame, view_frame_from_device_frame
|
||||
from openpilot.common.transformations.orientation import rot_from_euler
|
||||
|
||||
LIGHT_INPUT_SIZE = (512, 256)
|
||||
light_intrinsics = np.array([
|
||||
[1000.0, 0.0, 256.0],
|
||||
[0.0, 1000.0, 200.0],
|
||||
[0.0, 0.0, 1.0]])
|
||||
calib_from_light = np.linalg.inv((light_intrinsics @ get_view_frame_from_calib_frame(0, 0, 0, 0))[:, :3])
|
||||
|
||||
|
||||
def get_light_warp_matrix(device_from_calib_euler: np.ndarray, intrinsics: np.ndarray) -> np.ndarray:
|
||||
camera_from_calib = intrinsics @ view_frame_from_device_frame @ rot_from_euler(device_from_calib_euler)
|
||||
return (camera_from_calib @ calib_from_light).astype(np.float32)
|
||||
@@ -11,7 +11,7 @@ import pickle
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.sunnypilot.modeld_v2.frame_fusion import pack_layout
|
||||
from openpilot.sunnypilot.modeld_v2.frame_fusion import host_shapes, pack_layout, SPLIT_FRAMES
|
||||
from openpilot.sunnypilot.modeld_v2.helpers import load_model
|
||||
from openpilot.sunnypilot.modeld_v2.compile_modeld import (POLICY_INPUTS, derive_frame_skip,
|
||||
make_split_input_queues, make_supercombo_input_queues)
|
||||
@@ -169,16 +169,18 @@ class NativeTinygradAdapter(BaseModelAdapter):
|
||||
self.warp_frame_size = self.frame_copy_size
|
||||
self._init_common()
|
||||
self.run_model = self.jits['run']
|
||||
self.split = [name for name in SPLIT_FRAMES if name in self.run_model.input_names]
|
||||
self.frame_inputs: dict[str, Tensor] = {}
|
||||
|
||||
self.input_queues = {name: Tensor(np.zeros(shape, dtype=dtype), device=self.model_device).realize()
|
||||
for name, (shape, dtype) in self.input_shapes.items() if name in self.state_pairs}
|
||||
shapes = {name: shape for name, (shape, _) in self.input_shapes.items() if name not in self.state_pairs and name != 'new_img'}
|
||||
offsets, packed = pack_layout(shapes, self.warp_frame_size)
|
||||
shapes = host_shapes({name: shape for name, (shape, _) in self.input_shapes.items()}, self.state_pairs)
|
||||
offsets, packed = pack_layout(shapes, 0 if self.split else self.warp_frame_size)
|
||||
self.packed_input = np.zeros(packed, dtype=np.uint8)
|
||||
self.host = Tensor(self.packed_input, device='NPY')
|
||||
self.numpy_inputs = {name: np.ndarray(shape, dtype=np.float32, buffer=self.packed_input, offset=offsets[name])
|
||||
for name, shape in ({'tfm': (2, 3, 3)} | shapes).items()}
|
||||
self.frames = self.packed_input[offsets['frames']:].reshape(2, self.warp_frame_size)
|
||||
self.frames = self.packed_input[offsets['frames']:].reshape(2, -1)
|
||||
|
||||
self.outputs = {name: Tensor(np.zeros(shape, dtype=dtype), device=device).realize() for name, (shape, dtype, device) in self.jits['output_specs'].items()}
|
||||
for name, next_name in self.state_pairs.items():
|
||||
@@ -187,17 +189,29 @@ class NativeTinygradAdapter(BaseModelAdapter):
|
||||
|
||||
def copy_frames(self, bufs):
|
||||
for i, key in enumerate(self._vision_input_names):
|
||||
if key in bufs:
|
||||
if key not in bufs:
|
||||
continue
|
||||
if not self.split:
|
||||
data = bufs[key].data if hasattr(bufs[key], 'data') else bufs[key]
|
||||
np.copyto(self.frames[i], np.frombuffer(data, dtype=np.uint8, count=self.warp_frame_size))
|
||||
elif (name := ('frame', 'big_frame')[i]) in self.split:
|
||||
if isinstance(bufs[key], np.ndarray): # cached memory: the GPU would read stale lines in place
|
||||
self.frame_inputs[name] = Tensor(np.ascontiguousarray(bufs[key][:self.warp_frame_size]), device=self.model_device).realize()
|
||||
continue
|
||||
ptr = np.frombuffer(bufs[key].data, dtype=np.uint8).ctypes.data # visionipc buffers are coherent
|
||||
if (key, ptr) not in self._blob_cache:
|
||||
self._blob_cache[(key, ptr)] = Tensor.from_blob(ptr, (self.warp_frame_size,), dtype='uint8', device=self.model_device)
|
||||
self.frame_inputs[name] = self._blob_cache[(key, ptr)]
|
||||
|
||||
def reset_warmup_buffers(self) -> None:
|
||||
self.packed_input[:] = 0
|
||||
self.frame_inputs.clear()
|
||||
self._blob_cache.clear()
|
||||
for key in self.state_pairs:
|
||||
self.input_queues[key].assign(0).realize()
|
||||
|
||||
def run(self):
|
||||
self.run_model(host=self.host, **self.input_queues, output_buffers=self.outputs)
|
||||
self.run_model(host=self.host, **self.input_queues, **self.frame_inputs, output_buffers=self.outputs)
|
||||
return self.outputs['outputs']
|
||||
|
||||
|
||||
|
||||
@@ -11,6 +11,7 @@ from functools import partial
|
||||
import gc
|
||||
import os
|
||||
os.environ['GMMU'] = '0'
|
||||
os.environ['QCOM_PRIORITY'] = '1'
|
||||
from tinygrad.device import Device
|
||||
import numpy as np
|
||||
import threading
|
||||
@@ -41,9 +42,11 @@ from openpilot.selfdrive.modeld.modeld import ChestnutGpuState
|
||||
from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser
|
||||
from openpilot.sunnypilot.modeld_v2.blendful import Blendful
|
||||
from openpilot.sunnypilot.modeld_v2.turn_planner import TurnPlanner
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
|
||||
from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants
|
||||
from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
|
||||
from openpilot.sunnypilot.modeld_v2.light_frame import get_light_warp_matrix
|
||||
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
||||
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
|
||||
from openpilot.sunnypilot.modeld_v2.helpers import load_model
|
||||
@@ -171,7 +174,7 @@ class ModelState(ModelStateBase):
|
||||
np.where(inputs[desire_key] - self.prev_desire > .99, inputs[desire_key], 0)
|
||||
self.prev_desire[:] = inputs[desire_key]
|
||||
|
||||
for key in ('traffic_convention', 'lateral_control_params', 'action_t'):
|
||||
for key in ('traffic_convention', 'lateral_control_params', 'action_t', 'speed_limit'):
|
||||
if key in self.numpy_inputs and key in inputs:
|
||||
self.numpy_inputs[key][:] = inputs[key]
|
||||
|
||||
@@ -179,6 +182,8 @@ class ModelState(ModelStateBase):
|
||||
for i, key in enumerate(self._vision_input_names):
|
||||
if key in transforms:
|
||||
self.numpy_inputs['tfm'][i] = transforms[key].reshape(3, 3)
|
||||
if 'light_tfm' in self.numpy_inputs and 'light' in transforms:
|
||||
self.numpy_inputs['light_tfm'][0] = transforms['light'].reshape(3, 3)
|
||||
else:
|
||||
self.numpy_inputs['tfm'][:, :] = transforms[self._road_key].reshape(3, 3)
|
||||
self.numpy_inputs['big_tfm'][:, :] = transforms[self._wide_key].reshape(3, 3)
|
||||
@@ -231,7 +236,8 @@ class ModelState(ModelStateBase):
|
||||
|
||||
def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
|
||||
lat_action_t: float, long_action_t: float, v_ego: float,
|
||||
blendful: Callable[[float], float] | None = None) -> log.ModelDataV2.Action:
|
||||
blendful: Callable[[float], float] | None = None,
|
||||
turn_shaper: Callable[[float], float] | None = None) -> log.ModelDataV2.Action:
|
||||
if 'action' not in model_output:
|
||||
plan = model_output['plan'][0]
|
||||
desired_accel = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS,
|
||||
@@ -245,6 +251,8 @@ class ModelState(ModelStateBase):
|
||||
desired_curvature = model_output['action'][0, 0] / (max(2.5, v_ego))**2
|
||||
if blendful is not None:
|
||||
desired_curvature += blendful(desired_curvature)
|
||||
if turn_shaper is not None:
|
||||
desired_curvature = turn_shaper(desired_curvature)
|
||||
|
||||
stop = v_ego < 0.3 and desired_accel < 0.1
|
||||
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
|
||||
@@ -338,7 +346,7 @@ def main(demo=False):
|
||||
pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutGpuState"] if CHESTNUT else [])
|
||||
pm = PubMaster(pub_socks)
|
||||
sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState",
|
||||
"carControl", "controlsState", "lateralDelay"])
|
||||
"carControl", "controlsState", "lateralDelay", "longitudinalPlanSP"])
|
||||
|
||||
publish_state = PublishState()
|
||||
chestnut_state = ChestnutGpuState(pm, model.chestnut) if CHESTNUT else None
|
||||
@@ -351,6 +359,7 @@ def main(demo=False):
|
||||
|
||||
model_transform_main = np.zeros((3, 3), dtype=np.float32)
|
||||
model_transform_extra = np.zeros((3, 3), dtype=np.float32)
|
||||
model_transform_light = np.zeros((3, 3), dtype=np.float32)
|
||||
live_calib_seen = False
|
||||
buf_main, buf_extra = None, None
|
||||
meta_main = FrameMeta()
|
||||
@@ -369,10 +378,14 @@ def main(demo=False):
|
||||
prev_action = log.ModelDataV2.Action()
|
||||
|
||||
DH = DesireHelper()
|
||||
desire_frames = [q.shape[0] for m in (model, small_model) if m is not None for name, q in m.adapter.input_queues.items() if 'desire' in name]
|
||||
if desire_frames:
|
||||
DH.turn_repulse = min(desire_frames) - 2
|
||||
meta_constants = load_meta_constants()
|
||||
RELC = RoadEdgeLaneChangeController()
|
||||
blendful = Blendful()
|
||||
blendful_enabled = False
|
||||
turn_planner = TurnPlanner(CP.wheelbase)
|
||||
|
||||
while True:
|
||||
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
|
||||
@@ -425,6 +438,7 @@ def main(demo=False):
|
||||
model_transform_main = get_warp_matrix(device_from_calib_euler, main_intrinsics, False).astype(np.float32)
|
||||
model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.wide_road.intrinsics, True).astype(np.float32)
|
||||
model_transform_main, model_transform_extra = camera_offset_helper.update(model_transform_main, model_transform_extra, sm, main_wide_camera)
|
||||
model_transform_light = get_light_warp_matrix(device_from_calib_euler, main_intrinsics)
|
||||
live_calib_seen = True
|
||||
|
||||
traffic_convention = np.zeros(2)
|
||||
@@ -446,6 +460,7 @@ def main(demo=False):
|
||||
|
||||
bufs = {name: buf_extra if 'big' in name else buf_main for name in model.vision_input_names}
|
||||
transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.vision_input_names}
|
||||
transforms['light'] = model_transform_light
|
||||
|
||||
frame_delay = DT_MDL # compensate for time passed since the frame was captured: current_time - timestamp_eof is 50ms on average
|
||||
action_delay = DT_MDL / 2 # middle of the interval between model output (current state) and next frame (expected state)
|
||||
@@ -463,6 +478,10 @@ def main(demo=False):
|
||||
if 'action_t' in model.numpy_inputs:
|
||||
inputs['action_t'] = np.array([lat_action_t, long_action_t], dtype=np.float32)
|
||||
|
||||
if 'speed_limit' in model.numpy_inputs:
|
||||
resolver = sm['longitudinalPlanSP'].speedLimit.resolver
|
||||
inputs['speed_limit'] = np.array([resolver.speedLimitFinal if resolver.speedLimitValid else 0.0], dtype=np.float32)
|
||||
|
||||
mt1 = time.perf_counter()
|
||||
try:
|
||||
send_chestnut = (chestnut_state is not None and
|
||||
@@ -492,7 +511,9 @@ def main(demo=False):
|
||||
prev_action.desiredCurvature = sm['controlsState'].curvature
|
||||
blendful_active = desire == log.Desire.none and DH.lane_change_state == log.LaneChangeState.off
|
||||
action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego,
|
||||
partial(blendful.update, model_output, v_ego, blendful_active, enabled=blendful_enabled))
|
||||
partial(blendful.update, model_output, v_ego, blendful_active, enabled=blendful_enabled),
|
||||
partial(turn_planner.update, model_output, v_ego, sm['controlsState'].curvature,
|
||||
sm['carControl'].latActive, is_rhd))
|
||||
prev_action = action
|
||||
fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
|
||||
publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
|
||||
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:1baac6f02902df5689e9e275633c122a9955b7c57de2681e0137e1724c1f32dd
|
||||
size 3463278
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:0478837491c584b2212e31a91c494a908b8fb048205700f05a02a13ee7fc9b90
|
||||
size 7690094
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:580fadf61ded34b4b19f0ba1f5e4b61c0ea489ca34a0952ccf2878593e3f68c4
|
||||
size 1851239
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:44700f3533abb3e65232c8aca46b48384a59218545ed6bb505c5d4762204359b
|
||||
size 3964263
|
||||
@@ -1,3 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:4b22fd6bcb3a3e8a642e6866449dd9c30c12e694ff0741cad9e34e4b00d8c5fa
|
||||
size 3698799
|
||||
oid sha256:1f5dd8855de07d0e8b9ed836934e1dcbe77aa27c3b5c77eee1f21888f974ba8b
|
||||
size 3699072
|
||||
|
||||
@@ -1,3 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:9a17fd6acece4fab23784ef0f3f8bba28a049e9599e5e9e5ee7b36e4b1d24a6f
|
||||
size 7924847
|
||||
oid sha256:226d40112a292077b7eca26b97273c56ab3a84feee8ddf9b6d02d46c2a7b8403
|
||||
size 7924864
|
||||
|
||||
@@ -144,6 +144,21 @@ class TestPlainArtifact(OpenpilotTestCase):
|
||||
for name in ("outputs", "next_state_feat_q"):
|
||||
assert np.array_equal(inputs["output_buffers"][name].numpy(), expected["output_buffers"][name].numpy()), name
|
||||
|
||||
split_path = os.path.join(self.tmp.name, "warp_split_tinygrad.pkl")
|
||||
with mock.patch.object(sys, "argv", ["compile_warp", "--frame", FRAME, "--warp-to", f"{MODEL_W}x{MODEL_H}", "--output", split_path, "--split-frames"]):
|
||||
compile_warp.main()
|
||||
split = load_model(path, cache=False, extend=functools.partial(frame_part, split_path))
|
||||
assert split["run"].input_names == ["big_frame", "frame", "host", "state_feat_q"]
|
||||
offsets, packed = pack_layout(HOST, 0)
|
||||
host = np.zeros(packed, np.uint8)
|
||||
for name, shape in ({"tfm": (2, 3, 3)} | HOST).items():
|
||||
np.ndarray(shape, np.float32, host, offsets[name])[...] = tfm.numpy() if name == "tfm" else inputs[name].numpy()
|
||||
out = {"outputs": Tensor.zeros(OUT).contiguous().realize(), "next_state_feat_q": Tensor.zeros(STATE).contiguous().realize()}
|
||||
split["run"](host=Tensor(host, device="NPY"), frame=Tensor(frames.numpy()[0]).realize(), big_frame=Tensor(frames.numpy()[1]).realize(),
|
||||
state_feat_q=expected["state_feat_q"], output_buffers=out)
|
||||
for name in ("outputs", "next_state_feat_q"):
|
||||
assert np.array_equal(out[name].numpy(), expected["output_buffers"][name].numpy()), name
|
||||
|
||||
def test_tinygrad_api_the_loader_reads(self):
|
||||
import tinygrad.engine.jit as jit
|
||||
from tinygrad.device import Buffer, TinyELF
|
||||
|
||||
@@ -0,0 +1,210 @@
|
||||
"""
|
||||
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 math
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
|
||||
from openpilot.sunnypilot.modeld_v2.turn_planner import TurnPlanner, compute_plan_peak_curv
|
||||
|
||||
|
||||
def make_plan(curv: float, speed: float = 4.0) -> dict[str, np.ndarray]:
|
||||
"""Build a synthetic model_output dict with a constant-curvature arc plan."""
|
||||
t = np.array(ModelConstants.T_IDXS, dtype=np.float32)
|
||||
s = speed * t
|
||||
plan = np.zeros((1, len(t), ModelConstants.PLAN_WIDTH), dtype=np.float32)
|
||||
if abs(curv) < 1e-6:
|
||||
plan[0, :, Plan.POSITION][:, 0] = s
|
||||
plan[0, :, Plan.POSITION][:, 1] = 0.0
|
||||
plan[0, :, Plan.T_FROM_CURRENT_EULER][:, 2] = 0.0
|
||||
else:
|
||||
r = 1.0 / curv
|
||||
theta = s * curv
|
||||
plan[0, :, Plan.POSITION][:, 0] = r * np.sin(theta)
|
||||
plan[0, :, Plan.POSITION][:, 1] = r * (1.0 - np.cos(theta))
|
||||
plan[0, :, Plan.T_FROM_CURRENT_EULER][:, 2] = theta
|
||||
return {'plan': plan}
|
||||
|
||||
|
||||
class TestTurnPlanner(OpenpilotTestCase):
|
||||
def test_plan_peak_curv_extraction(self):
|
||||
assert abs(compute_plan_peak_curv(make_plan(0.0, 5.0))) < 1e-5
|
||||
pk = compute_plan_peak_curv(make_plan(0.12, 4.0))
|
||||
assert abs(pk - 0.12) < 0.01
|
||||
pk_l = compute_plan_peak_curv(make_plan(-0.15, 4.0))
|
||||
assert abs(pk_l + 0.15) < 0.01
|
||||
assert compute_plan_peak_curv({}) == 0.0
|
||||
|
||||
def test_untouched_at_highway_speed(self):
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
for i in range(60):
|
||||
k = 0.01 * math.sin(i * 0.1)
|
||||
out = tp.update(make_plan(k, 25.0), v_ego=25.0, actual_curv=k, active=True, is_rhd=False, model_curv=k)
|
||||
assert out == k
|
||||
|
||||
def test_untouched_on_gentle_low_speed_curve(self):
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
for i in range(80):
|
||||
k = 0.025 * min(1.0, i / 20.0)
|
||||
out = tp.update(make_plan(k, 6.0), v_ego=6.0, actual_curv=k, active=True, is_rhd=False, model_curv=k)
|
||||
assert out == k
|
||||
|
||||
def test_sign_preservation_and_magnitude_bound_under_jitter(self):
|
||||
"""Output must never oppose model_curv or exceed APEX_PATH_BOOST_CAP * |model_curv|."""
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
# Reproduces a5 seg31 Turn 1: brief negative jitter then sharp positive right turn
|
||||
ks = [-0.015] * 15 + [0.016, 0.05, 0.11, 0.14, 0.17, 0.18, 0.14, 0.08, 0.02, -0.01]
|
||||
plan_right = make_plan(0.15, 3.0)
|
||||
for k in ks:
|
||||
out = tp.update(plan_right, v_ego=3.0, actual_curv=k * 0.7, active=True, is_rhd=False, model_curv=k)
|
||||
assert abs(out) <= abs(k) * 1.35 + 1e-9
|
||||
if k > 0:
|
||||
assert out >= 0.0
|
||||
elif k < 0:
|
||||
assert out <= 0.0
|
||||
|
||||
def test_zero_lag_on_turn_exit_unwinding(self):
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
plan_tight = make_plan(0.14, 4.0)
|
||||
# Ramp up entry and hold through apex so buffer is at peak curvature
|
||||
for i in range(25):
|
||||
k = 0.14 * (i / 25.0)
|
||||
tp.update(plan_tight, v_ego=4.0, actual_curv=k * 0.8, active=True, is_rhd=False, model_curv=k)
|
||||
for _ in range(15):
|
||||
tp.update(plan_tight, v_ego=4.0, actual_curv=0.14, active=True, is_rhd=False, model_curv=0.14)
|
||||
# Now unwind monotonically: output must equal model_curv with zero lag
|
||||
for i in range(1, 20):
|
||||
k = max(0.0, 0.14 * (1.0 - i / 15.0))
|
||||
out = tp.update(make_plan(k, 4.0), v_ego=4.0, actual_curv=k + 0.01, active=True, is_rhd=False, model_curv=k)
|
||||
assert abs(out - k) < 1e-3
|
||||
|
||||
def test_standstill_launch_and_mid_turn_pause(self):
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
# Stopped straight at stop bar, then launches into a sharp right turn
|
||||
for _ in range(10):
|
||||
tp.update(make_plan(0.12, 0.0), v_ego=0.0, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.002)
|
||||
# First few steps of launch should hold back early curvature ramp
|
||||
outs = []
|
||||
for i in range(1, 20):
|
||||
k = 0.006 * i
|
||||
outs.append(tp.update(make_plan(0.12, 2.5), v_ego=2.5, actual_curv=0.004 * i, active=True, is_rhd=False, model_curv=k))
|
||||
assert outs[10] < 0.006 * 11 - 0.01
|
||||
|
||||
# Pause mid-turn (actual_curv = 0.05) should preserve turn_psi and not dip below 0.05 on resume
|
||||
tp.turn_psi = math.radians(30.0)
|
||||
tp.update(make_plan(0.08, 0.0), v_ego=0.0, actual_curv=0.05, active=True, is_rhd=False, model_curv=0.05)
|
||||
assert tp.turn_psi >= math.radians(29.0)
|
||||
resumed = tp.update(make_plan(0.08, 2.0), v_ego=2.0, actual_curv=0.05, active=True, is_rhd=False, model_curv=0.06)
|
||||
assert resumed >= 0.05 - 1e-6
|
||||
|
||||
def test_wheelbase_and_rhd_scaling(self):
|
||||
tp_short = TurnPlanner(wheelbase=2.4)
|
||||
tp_long = TurnPlanner(wheelbase=3.4)
|
||||
plan_r = make_plan(0.14, 4.2)
|
||||
for _ in range(25):
|
||||
tp_short.update(plan_r, 4.2, 0.0, True, False, 0.02)
|
||||
tp_long.update(plan_r, 4.2, 0.0, True, False, 0.02)
|
||||
assert tp_long.shift_dist > tp_short.shift_dist * 1.3
|
||||
|
||||
# RHD gives larger shift on left turns (k < 0) than right turns (k > 0)
|
||||
tp_rhd_l = TurnPlanner(wheelbase=2.89)
|
||||
tp_rhd_r = TurnPlanner(wheelbase=2.89)
|
||||
for _ in range(25):
|
||||
tp_rhd_l.update(make_plan(-0.14, 3.5), 3.5, 0.0, True, True, -0.02)
|
||||
tp_rhd_r.update(make_plan(0.14, 3.5), 3.5, 0.0, True, True, 0.02)
|
||||
assert tp_rhd_l.shift_dist > tp_rhd_r.shift_dist * 1.2
|
||||
|
||||
def test_closed_loop_90deg_turn_clears_curb_without_oncoming_overshoot(self):
|
||||
"""Closed-loop Tesla Model Y 90-deg right turn around R=2.5 m curb."""
|
||||
L, W_half, x_cam, lane_w, curb_r = 2.89, 0.96, 1.50, 3.60, 2.5
|
||||
y_curb = lane_w / 2.0
|
||||
x_dest = curb_r + lane_w / 2.0
|
||||
x_yellow = x_dest + lane_w / 2.0
|
||||
|
||||
pts = [(x, 0.0) for x in np.linspace(-20, x_dest - 2.0, 200)]
|
||||
pts += [(x_dest - 2.0 + 2.0 * math.cos(th), 2.0 + 2.0 * math.sin(th)) for th in np.linspace(-math.pi / 2, 0, 50)]
|
||||
pts += [(x_dest, y) for y in np.linspace(2.0, 25.0, 200)]
|
||||
ref = np.array(pts)
|
||||
|
||||
def run(use_planner: bool) -> tuple[float, float]:
|
||||
X, Y, psi, k_act = -15.0, 0.0, 0.0, 0.0
|
||||
v = 3.5
|
||||
tp = TurnPlanner(wheelbase=L, dt=DT_MDL) if use_planner else None
|
||||
min_clr, max_ovr = 99.0, -99.0
|
||||
for _ in range(400):
|
||||
xc, yc = X + x_cam * math.cos(psi), Y + x_cam * math.sin(psi)
|
||||
i_near = int(np.argmin(np.hypot(ref[:, 0] - xc, ref[:, 1] - yc)))
|
||||
i_tgt = i_near
|
||||
while i_tgt < len(ref) - 1 and np.hypot(ref[i_tgt, 0] - xc, ref[i_tgt, 1] - yc) < 6.5:
|
||||
i_tgt += 1
|
||||
dx, dy = ref[i_tgt, 0] - xc, ref[i_tgt, 1] - yc
|
||||
lx = dx * math.cos(psi) + dy * math.sin(psi)
|
||||
ly = -dx * math.sin(psi) + dy * math.cos(psi)
|
||||
k_mod = float(np.clip(2.0 * ly / max(lx**2 + ly**2, 1.0), -0.19, 0.19))
|
||||
plan = make_plan(0.14 if (max(0.0, x_dest - xc) < 15.0 and psi < math.radians(75)) else 0.0, v)
|
||||
|
||||
k_cmd = tp.update(plan, v, k_act, True, False, k_mod) if tp is not None else k_mod
|
||||
k_act += (float(np.clip(k_cmd, -0.19, 0.19)) - k_act) * (DT_MDL / (0.22 + DT_MDL))
|
||||
|
||||
ds = v * DT_MDL
|
||||
psi += k_act * ds
|
||||
X += ds * math.cos(psi)
|
||||
Y += ds * math.sin(psi)
|
||||
|
||||
x_ir, y_ir = X - W_half * math.sin(psi), Y + W_half * math.cos(psi)
|
||||
x_of, y_of = X + L * math.cos(psi) + W_half * math.sin(psi), Y + L * math.sin(psi) - W_half * math.cos(psi)
|
||||
if x_ir <= 0.0:
|
||||
clr = y_curb - y_ir
|
||||
elif y_ir >= y_curb + curb_r:
|
||||
clr = x_ir - curb_r
|
||||
else:
|
||||
clr = math.hypot(x_ir, y_ir - (y_curb + curb_r)) - curb_r
|
||||
min_clr = min(min_clr, clr)
|
||||
if y_of > 2.0:
|
||||
max_ovr = max(max_ovr, x_of - x_yellow)
|
||||
if Y > 15.0:
|
||||
break
|
||||
return min_clr, max_ovr
|
||||
|
||||
base_clr, _ = run(False)
|
||||
plan_clr, plan_ovr = run(True)
|
||||
assert base_clr < -0.5 # baseline curbs the rear wheel badly
|
||||
assert plan_clr > 0.20 # TurnPlanner clears the curb by > 20 cm
|
||||
assert plan_ovr < 0.0 # stays inside the oncoming yellow line
|
||||
|
||||
def test_rolling_speed_taper(self):
|
||||
"""Shift is strong at low speed, light at rolling speed, 0 at 19+ mph, and time-capped at crawl."""
|
||||
tp_crawl = TurnPlanner(wheelbase=2.89)
|
||||
tp_low = TurnPlanner(wheelbase=2.89)
|
||||
tp_roll = TurnPlanner(wheelbase=2.89)
|
||||
tp_fast = TurnPlanner(wheelbase=2.89)
|
||||
for _ in range(30):
|
||||
tp_crawl.update(make_plan(0.14, 1.8), v_ego=1.8, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02)
|
||||
tp_low.update(make_plan(0.14, 3.5), v_ego=3.5, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02)
|
||||
tp_roll.update(make_plan(0.14, 6.0), v_ego=6.0, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02)
|
||||
tp_fast.update(make_plan(0.14, 9.0), v_ego=9.0, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02)
|
||||
assert tp_crawl.shift_dist <= 1.8 * 0.48 + 0.05 # time-capped so crawl starts don't stall
|
||||
assert tp_low.shift_dist > 1.3
|
||||
assert 0.3 < tp_roll.shift_dist < 0.7
|
||||
assert tp_fast.shift_dist == 0.0
|
||||
|
||||
def test_apex_1s_path_boost_and_exit_unwind(self):
|
||||
"""Once past entry, blends with 1s path to boost under-steering apex and unwind hanging exit."""
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
# Drive past entry (psi > 30 deg)
|
||||
for _ in range(35):
|
||||
tp.update(make_plan(0.14, 4.0), v_ego=4.0, actual_curv=0.12, active=True, is_rhd=False, model_curv=0.10)
|
||||
# Apex (psi ~ 48 deg): action under-steers at 0.10 while 1s path says 0.14 -> boosted cleanly above 0.10
|
||||
apex_out = tp.update(make_plan(0.14, 4.0), v_ego=4.0, actual_curv=0.12, active=True, is_rhd=False, model_curv=0.10)
|
||||
assert apex_out > 0.115
|
||||
# Advance past 60 deg into late-turn exit
|
||||
for _ in range(20):
|
||||
tp.update(make_plan(0.14, 4.0), v_ego=4.0, actual_curv=0.12, active=True, is_rhd=False, model_curv=0.14)
|
||||
# Exit hang: action still stuck at 0.20 while 1s path already unwound to 0.10 -> pulls down cleanly
|
||||
exit_out = tp.update(make_plan(0.10, 4.0), v_ego=4.0, actual_curv=0.15, active=True, is_rhd=False, model_curv=0.20)
|
||||
assert exit_out < 0.15
|
||||
@@ -0,0 +1,214 @@
|
||||
"""
|
||||
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 math
|
||||
from collections import deque
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
|
||||
|
||||
# Two-stage rear-axle protected turn planner:
|
||||
# Stage 1 (Entry): wheelbase-scaled spatial delay on curvature buildup so the
|
||||
# inside rear tire clears the corner curb instead of off-tracking over it.
|
||||
# Capped in time (MAX_SHIFT_TIME) so crawl-speed starts never feel stalled.
|
||||
# Stage 2 (Apex & Exit): once past initial corner entry, smoothly blends
|
||||
# action curvature with the model's own 1-second path curvature so tight
|
||||
# apexes don't under-steer/go wide and exits unwind cleanly.
|
||||
SHIFT_WHEELBASE_RATIO = 0.58 # fraction of wheelbase to delay turn-in (~1.68 m for 2.89 m WB)
|
||||
WHEELBASE_CLIP = (2.0, 4.0) # m
|
||||
OPPOSITE_SIDE_WEIGHT = 0.70 # lighter shift for non-curb-side turns
|
||||
MAX_SHIFT_TIME = 0.48 # s, caps entry delay at crawl speeds (2-5 mph)
|
||||
|
||||
TURN_SPEED_TAPER = (7.5, 10.5) # m/s (~17-23.5 mph): overall active window
|
||||
SHIFT_SPEED_BP = (3.5, 6.0, 8.5) # m/s (~7.8, 13.4, 19.0 mph)
|
||||
SHIFT_SPEED_V = (1.0, 0.35, 0.0) # full from stop, light on rolling turns, 0 above 19 mph
|
||||
CURV_GATE = (0.04, 0.09) # 1/m (R=25 m -> R=11 m): ignores gentle curves
|
||||
|
||||
ENTRY_PHASE_RELEASE_DEG = (8.0, 22.0) # deg: release entry shift crisply once rotating
|
||||
APEX_BLEND_PHASE_DEG = (10.0, 25.0) # deg: ramp in 1s-path blend through apex & exit
|
||||
APEX_PATH_BLEND_MAX = 0.65 # max weight on 1s path curvature during apex/exit
|
||||
APEX_PATH_BOOST_CAP = 1.35 # max ratio vs |model_curv| for safety
|
||||
PATH_AVG_HORIZON_S = 1.0 # s, horizon for average path curvature
|
||||
|
||||
SHIFT_BUILD_TAU = 0.15 # s
|
||||
SHIFT_RELEASE_TAU = 0.07 # s
|
||||
APEX_BLEND_TAU = 0.12 # s
|
||||
BUFFER_MAX_DIST = 4.5 # m
|
||||
MIN_MOVE_SPEED = 0.3 # m/s
|
||||
MID_TURN_CURV_THRESH = 0.02 # 1/m
|
||||
PLAN_S_RANGE = (1.0, 18.0) # m
|
||||
PLAN_MIN_SEG_DS = 0.8 # m
|
||||
|
||||
T_IDXS = np.array(ModelConstants.T_IDXS, dtype=np.float32)
|
||||
|
||||
|
||||
def compute_plan_metrics(model_output: dict[str, np.ndarray]) -> tuple[float, float]:
|
||||
"""Return (peak_upcoming_curv, avg_1s_path_curv) from model_output['plan']."""
|
||||
plan_arr = model_output.get('plan')
|
||||
if plan_arr is None or plan_arr.ndim < 3 or plan_arr.shape[1] < 3:
|
||||
return 0.0, 0.0
|
||||
|
||||
plan = plan_arr[0]
|
||||
pos = plan[:, Plan.POSITION]
|
||||
yaws = plan[:, Plan.T_FROM_CURRENT_EULER][:, 2]
|
||||
ds = np.hypot(np.diff(pos[:, 0]), np.diff(pos[:, 1]))
|
||||
s = np.concatenate(([0.0], np.cumsum(ds)))
|
||||
|
||||
peak_k = 0.0
|
||||
n = len(yaws)
|
||||
for i in range(n - 2):
|
||||
if s[i] < PLAN_S_RANGE[0]:
|
||||
continue
|
||||
if s[i] > PLAN_S_RANGE[1]:
|
||||
break
|
||||
seg_ds = s[i + 2] - s[i]
|
||||
if seg_ds < PLAN_MIN_SEG_DS:
|
||||
continue
|
||||
k = float((yaws[i + 2] - yaws[i]) / seg_ds)
|
||||
if abs(k) > abs(peak_k):
|
||||
peak_k = k
|
||||
peak_k = float(np.clip(peak_k, -0.25, 0.25))
|
||||
|
||||
if len(yaws) == len(T_IDXS):
|
||||
s_1s = float(np.interp(PATH_AVG_HORIZON_S, T_IDXS, s))
|
||||
yaw_1s = float(np.interp(PATH_AVG_HORIZON_S, T_IDXS, yaws))
|
||||
path_k_1s = float(np.clip(yaw_1s / s_1s, -0.25, 0.25)) if s_1s >= PLAN_MIN_SEG_DS else 0.0
|
||||
else:
|
||||
path_k_1s = 0.0
|
||||
|
||||
return peak_k, path_k_1s
|
||||
|
||||
|
||||
def compute_plan_peak_curv(model_output: dict[str, np.ndarray]) -> float:
|
||||
return compute_plan_metrics(model_output)[0]
|
||||
|
||||
|
||||
class TurnPlanner:
|
||||
"""
|
||||
Two-stage rear-axle protected turn planner for tight low-speed turns.
|
||||
|
||||
Stage 1 (Entry): delays curvature buildup by a wheelbase-proportional distance
|
||||
(time-capped at MAX_SHIFT_TIME) so the rear axle clears the inside curb.
|
||||
Stage 2 (Apex & Exit): blends action curvature with the model's 1-second path
|
||||
curvature once past entry, preventing wide apexes and exit hang-ups.
|
||||
"""
|
||||
|
||||
def __init__(self, wheelbase: float = 2.7, dt: float = DT_MDL):
|
||||
self.wheelbase = float(np.clip(wheelbase, *WHEELBASE_CLIP))
|
||||
self.dt = dt
|
||||
self.s = 0.0
|
||||
self.buf_s: deque[float] = deque()
|
||||
self.buf_k: deque[float] = deque()
|
||||
self.turn_sign = 0.0
|
||||
self.turn_psi = 0.0
|
||||
self.shift_dist = 0.0
|
||||
self.apex_blend = 0.0
|
||||
self.plan_peak_curv = 0.0
|
||||
self.path_curv_1s = 0.0
|
||||
|
||||
def reset(self) -> None:
|
||||
self.s = 0.0
|
||||
self.buf_s.clear()
|
||||
self.buf_k.clear()
|
||||
self.turn_sign = 0.0
|
||||
self.turn_psi = 0.0
|
||||
self.shift_dist = 0.0
|
||||
self.apex_blend = 0.0
|
||||
self.plan_peak_curv = 0.0
|
||||
self.path_curv_1s = 0.0
|
||||
|
||||
def update(self, model_output: dict[str, np.ndarray], v_ego: float, actual_curv: float,
|
||||
active: bool, is_rhd: bool, model_curv: float, enabled: bool = True) -> float:
|
||||
if not enabled or not active:
|
||||
self.reset()
|
||||
return model_curv
|
||||
|
||||
if v_ego < MIN_MOVE_SPEED:
|
||||
# Stopped straight (e.g., at a stop bar): reset turn phase so launch gets full protection.
|
||||
# Stopped mid-turn (e.g., yielding to a pedestrian): preserve turn_psi so resume doesn't re-delay.
|
||||
if abs(actual_curv) < MID_TURN_CURV_THRESH:
|
||||
self.turn_psi = 0.0
|
||||
self.turn_sign = 0.0
|
||||
self.shift_dist = 0.0
|
||||
self.apex_blend = 0.0
|
||||
self.buf_s.clear()
|
||||
self.buf_k.clear()
|
||||
self.buf_s.append(self.s)
|
||||
self.buf_k.append(float(actual_curv))
|
||||
return model_curv
|
||||
|
||||
ds = v_ego * self.dt
|
||||
self.s += ds
|
||||
self.buf_s.append(self.s)
|
||||
self.buf_k.append(float(model_curv))
|
||||
while len(self.buf_s) > 2 and (self.s - self.buf_s[0]) > BUFFER_MAX_DIST:
|
||||
self.buf_s.popleft()
|
||||
self.buf_k.popleft()
|
||||
|
||||
turn_speed_w = float(np.interp(v_ego, TURN_SPEED_TAPER, [1.0, 0.0]))
|
||||
if turn_speed_w <= 0.0 and self.shift_dist < 1e-3 and self.apex_blend < 1e-3:
|
||||
self.turn_psi = 0.0
|
||||
self.turn_sign = 0.0
|
||||
self.plan_peak_curv = 0.0
|
||||
self.path_curv_1s = 0.0
|
||||
return model_curv
|
||||
|
||||
plan_peak, path_k_1s = compute_plan_metrics(model_output)
|
||||
self.plan_peak_curv = plan_peak
|
||||
self.path_curv_1s = path_k_1s
|
||||
|
||||
tightness = max(abs(model_curv), abs(plan_peak))
|
||||
curv_w = float(np.interp(tightness, CURV_GATE, [0.0, 1.0]))
|
||||
|
||||
ref_k = model_curv if abs(model_curv) > 0.015 else plan_peak
|
||||
if abs(ref_k) > 0.035 and turn_speed_w > 0.05:
|
||||
new_sign = 1.0 if ref_k > 0.0 else -1.0
|
||||
if new_sign != self.turn_sign:
|
||||
self.turn_sign = new_sign
|
||||
self.turn_psi = 0.0
|
||||
self.turn_psi = max(0.0, self.turn_psi + actual_curv * self.turn_sign * ds)
|
||||
else:
|
||||
self.turn_psi = max(0.0, self.turn_psi - 0.5 * self.dt)
|
||||
if self.turn_psi < 0.02:
|
||||
self.turn_sign = 0.0
|
||||
|
||||
psi_deg = math.degrees(self.turn_psi)
|
||||
|
||||
# Stage 1: Entry rear-axle spatial shift (time-capped so crawl-speed starts stay responsive)
|
||||
shift_speed_w = float(np.interp(v_ego, SHIFT_SPEED_BP, SHIFT_SPEED_V))
|
||||
entry_phase_w = float(np.interp(psi_deg, ENTRY_PHASE_RELEASE_DEG, [1.0, 0.0]))
|
||||
is_curb_side = (ref_k < 0.0) if is_rhd else (ref_k > 0.0)
|
||||
side_w = 1.0 if is_curb_side else OPPOSITE_SIDE_WEIGHT
|
||||
|
||||
raw_shift = SHIFT_WHEELBASE_RATIO * self.wheelbase * shift_speed_w * curv_w * entry_phase_w * side_w
|
||||
target_shift = min(raw_shift, v_ego * MAX_SHIFT_TIME)
|
||||
tau = SHIFT_RELEASE_TAU if target_shift < self.shift_dist else SHIFT_BUILD_TAU
|
||||
self.shift_dist += (target_shift - self.shift_dist) * (self.dt / (tau + self.dt))
|
||||
|
||||
out_k = model_curv
|
||||
if self.shift_dist >= 0.05 and len(self.buf_s) >= 2:
|
||||
k_delayed = float(np.interp(self.s - self.shift_dist, self.buf_s, self.buf_k))
|
||||
# Same sign as model_curv (or 0), never exceeds |model_curv|, zero lag on unwind.
|
||||
if model_curv >= 0.0:
|
||||
out_k = float(np.clip(k_delayed, 0.0, model_curv))
|
||||
else:
|
||||
out_k = float(np.clip(k_delayed, model_curv, 0.0))
|
||||
|
||||
# Stage 2: Apex & Exit 1s-path blend (prevents wide apexes and exit hang-ups)
|
||||
apex_phase_w = float(np.interp(psi_deg, APEX_BLEND_PHASE_DEG, [0.0, 1.0]))
|
||||
target_blend = APEX_PATH_BLEND_MAX * turn_speed_w * curv_w * apex_phase_w
|
||||
self.apex_blend += (target_blend - self.apex_blend) * (self.dt / (APEX_BLEND_TAU + self.dt))
|
||||
|
||||
if self.apex_blend > 0.01 and out_k * path_k_1s > 0.0:
|
||||
# Through the apex, boost when 1s path asks for tighter curvature than action (prevents wide turns).
|
||||
# Only pull curvature down toward 1s path late in the turn (psi > 60 deg) to prevent exit hang-ups.
|
||||
if abs(path_k_1s) > abs(out_k) or psi_deg > 60.0:
|
||||
max_boosted = abs(model_curv) * APEX_PATH_BOOST_CAP
|
||||
target_k = float(np.clip(path_k_1s, -max_boosted, max_boosted))
|
||||
out_k = (1.0 - self.apex_blend) * out_k + self.apex_blend * target_k
|
||||
|
||||
return out_k
|
||||
+1
-1
Submodule tinygrad_repo updated: 30ddfc2eac...301a0d1ded
Reference in New Issue
Block a user