Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Test angle car models' lateral limits #1231

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Test angle car models' lateral limits #1231

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
f56bee8
test more models!
sshane Sep 10, 2024
42801df
Impreza 2020: reach max in ~0.8s instead of ~0.6
sshane Sep 10, 2024
56cecf4
Merge remote-tracking branch 'upstream/master' into test-angle-limits
sshane Sep 10, 2024
6b07614
stash so far
sshane Sep 11, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions opendbc/car/nissan/values.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@ class CarControllerParams:
ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., 3.5, 0.4])
LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower
STEER_THRESHOLD = 1.0
STEER_STEP = 1 # 100 Hz

def __init__(self, CP):
pass
Expand Down
136 changes: 114 additions & 22 deletions opendbc/car/tests/test_lateral_limits.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,14 +1,17 @@
#!/usr/bin/env python3
from collections import defaultdict
import importlib
import math
from parameterized import parameterized_class
import pytest
import sys

from opendbc.car import DT_CTRL
from opendbc.car.common.numpy_fast import interp
from opendbc.car.car_helpers import interfaces
from opendbc.car.fingerprints import all_known_cars
from opendbc.car.interfaces import get_torque_params
from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel # TODO: no VM in opendbc

CAR_MODELS = all_known_cars()

Expand All @@ -29,42 +32,130 @@ class TestLateralLimits:
@classmethod
def setup_class(cls):
CarInterface, _, _, _ = interfaces[cls.car_model]
CP = CarInterface.get_non_essential_params(cls.car_model)
cls.CP = CarInterface.get_non_essential_params(cls.car_model)
cls.VM = VehicleModel(cls.CP)

if CP.dashcamOnly:
if cls.CP.dashcamOnly:
pytest.skip("Platform is behind dashcamOnly")

# TODO: test all platforms
if CP.steerControlType != 'torque':
pytest.skip()
# # TODO: test all platforms
# if CP.steerControlType != 'torque':
# pytest.skip()

if CP.notCar:
if cls.CP.notCar:
pytest.skip()

CarControllerParams = importlib.import_module(f'opendbc.car.{CP.carName}.values').CarControllerParams
cls.control_params = CarControllerParams(CP)
CarControllerParams = importlib.import_module(f'opendbc.car.{cls.CP.carName}.values').CarControllerParams
cls.control_params = CarControllerParams(cls.CP)
cls.torque_params = get_torque_params()[cls.car_model]

@staticmethod
def calculate_0_5s_jerk(control_params, torque_params):
def calculate_0_5s_jerk(CP, VM, control_params, torque_params):
steer_step = control_params.STEER_STEP
max_lat_accel = torque_params['MAX_LAT_ACCEL_MEASURED']

# Steer up/down delta per 10ms frame, in percentage of max torque
steer_up_per_frame = control_params.STEER_DELTA_UP / control_params.STEER_MAX / steer_step
steer_down_per_frame = control_params.STEER_DELTA_DOWN / control_params.STEER_MAX / steer_step

# Lateral acceleration reached in 0.5 seconds, clipping to max torque
accel_up_0_5_sec = min(steer_up_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * max_lat_accel
accel_down_0_5_sec = min(steer_down_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * max_lat_accel

# Convert to m/s^3
return accel_up_0_5_sec / JERK_MEAS_T, accel_down_0_5_sec / JERK_MEAS_T
if CP.steerControlType == 'torque':
# Steer up/down delta per 10ms frame, in percentage of max torque
steer_up_per_frame = control_params.STEER_DELTA_UP / control_params.STEER_MAX / steer_step
steer_down_per_frame = control_params.STEER_DELTA_DOWN / control_params.STEER_MAX / steer_step

# Lateral acceleration reached in 0.5 seconds, clipping to max torque
accel_up_0_5_sec = min(steer_up_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * max_lat_accel
accel_down_0_5_sec = min(steer_down_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * max_lat_accel

# Convert to m/s^3
return accel_up_0_5_sec / JERK_MEAS_T, accel_down_0_5_sec / JERK_MEAS_T
else:
# We need to test the entire speed range for angle cars as jerk is speed dependent
print(CP.carFingerprint, control_params.ANGLE_RATE_LIMIT_UP, steer_step)

up_jerks = []
down_jerks = []

speeds = [1e-3, 5., 15., 35.] # m/s
for speed in speeds:
up_rate = interp(speed, control_params.ANGLE_RATE_LIMIT_UP.speed_bp, control_params.ANGLE_RATE_LIMIT_UP.angle_v)
down_rate = interp(speed, control_params.ANGLE_RATE_LIMIT_DOWN.speed_bp, control_params.ANGLE_RATE_LIMIT_DOWN.angle_v)

# Ford is already curvature
if CP.carName != 'ford':
up_rate = VM.calc_curvature(math.radians(up_rate), speed, 0)
down_rate = VM.calc_curvature(math.radians(down_rate), speed, 0)
print(up_rate, down_rate)
continue

accel_up_per_frame = up_rate * speed ** 2 / steer_step
accel_down_per_frame = down_rate * speed ** 2 / steer_step

accel_up_0_5_sec = min(accel_up_per_frame * JERK_MEAS_T / DT_CTRL, max_lat_accel)
accel_down_0_5_sec = min(accel_down_per_frame * JERK_MEAS_T / DT_CTRL, max_lat_accel)

print('speed', speed)
print('accel_up_per_frame', accel_up_per_frame, 'accel_down_per_frame', accel_down_per_frame)
print('accel_up_0_5_sec', accel_up_0_5_sec, 'accel_down_0_5_sec', accel_down_0_5_sec)
up_jerks.append(accel_up_0_5_sec / JERK_MEAS_T)
down_jerks.append(accel_down_0_5_sec / JERK_MEAS_T)
# continue
#
# accel_up_0_5_sec = min(curvature_up_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * MAX_LAT_ACCEL
# accel_down_0_5_sec = min(curvature_down_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * MAX_LAT_ACCEL
# jerk_up, jerk_down = accel_up_0_5_sec / JERK_MEAS_T, accel_down_0_5_sec / JERK_MEAS_T
# print(up_rate, down_rate)
# print('jerk_up', jerk_up, 'jerk_down', jerk_down)
#
# # curvature_up = self.VM.calc_curvature(math.radians(1.6) * (100 / steer_step), speed, 0)
# # jerk_up = curvature * speed ** 2
# # print('speed', speed, 'curvature', curvature, 'jerk', jerk)
return max(up_jerks), max(down_jerks)

def test_jerk_limits(self):
up_jerk, down_jerk = self.calculate_0_5s_jerk(self.control_params, self.torque_params)
# print(self.CP.carFingerprint, self.CP.steerControlType)
# if self.CP.steerControlType == 'torque':
up_jerk, down_jerk = self.calculate_0_5s_jerk(self.CP, self.VM, self.control_params, self.torque_params)
assert up_jerk <= MAX_LAT_JERK_UP + MAX_LAT_JERK_UP_TOLERANCE
assert down_jerk <= MAX_LAT_JERK_DOWN
# else:
# steer_step = self.control_params.STEER_STEP
# max_lat_accel = self.torque_params['MAX_LAT_ACCEL_MEASURED']
#
#
# # We need to test the entire speed range for angle cars as jerk is speed dependent
# print(self.CP.carFingerprint, self.control_params.ANGLE_RATE_LIMIT_UP, self.control_params.STEER_STEP)
# speeds = [1e-3, 5., 15., 35.] # m/s
# for speed in speeds:
# up_rate = interp(speed, self.control_params.ANGLE_RATE_LIMIT_UP.speed_bp, self.control_params.ANGLE_RATE_LIMIT_UP.angle_v)
# down_rate = interp(speed, self.control_params.ANGLE_RATE_LIMIT_DOWN.speed_bp, self.control_params.ANGLE_RATE_LIMIT_DOWN.angle_v)
#
# # Ford is already curvature
# if self.CP.carName != 'ford':
# up_rate = self.VM.calc_curvature(math.radians(up_rate), speed, 0)
# down_rate = self.VM.calc_curvature(math.radians(down_rate), speed, 0)
#
# accel_up_per_frame = up_rate * speed ** 2 / self.control_params.STEER_STEP
# accel_down_per_frame = down_rate * speed ** 2 / self.control_params.STEER_STEP
#
# accel_up_0_5_sec = min(accel_up_per_frame * JERK_MEAS_T / DT_CTRL, max_lat_accel)
# accel_down_0_5_sec = min(accel_down_per_frame * JERK_MEAS_T / DT_CTRL, max_lat_accel)
#
# print('speed', speed)
# print('accel_up_per_frame', accel_up_per_frame, 'accel_down_per_frame', accel_down_per_frame)
# print('accel_up_0_5_sec', accel_up_0_5_sec, 'accel_down_0_5_sec', accel_down_0_5_sec)
# continue
#
# accel_up_0_5_sec = min(curvature_up_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * MAX_LAT_ACCEL
# accel_down_0_5_sec = min(curvature_down_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * MAX_LAT_ACCEL
# jerk_up, jerk_down = accel_up_0_5_sec / JERK_MEAS_T, accel_down_0_5_sec / JERK_MEAS_T
# print(up_rate, down_rate)
# print('jerk_up', jerk_up, 'jerk_down', jerk_down)
#
#
#
# # curvature_up = self.VM.calc_curvature(math.radians(1.6) * (100 / self.control_params.STEER_STEP), speed, 0)
# # jerk_up = curvature * speed ** 2
# # print('speed', speed, 'curvature', curvature, 'jerk', jerk)
#
# if self.CP.carFingerprint != 'TOYOTA_RAV4_TSS2_2023':
# return

def test_max_lateral_accel(self):
assert self.torque_params["MAX_LAT_ACCEL_MEASURED"] <= MAX_LAT_ACCEL
Expand All @@ -90,9 +181,10 @@ def class_setup(self, request):
yield
cls = request.cls
if hasattr(cls, "control_params"):
up_jerk, down_jerk = TestLateralLimits.calculate_0_5s_jerk(cls.control_params, cls.torque_params)
up_jerk, down_jerk = TestLateralLimits.calculate_0_5s_jerk(cls.CP, cls.VM, cls.control_params, cls.torque_params)
self.car_model_jerks[cls.car_model] = {"up_jerk": up_jerk, "down_jerk": down_jerk}


if __name__ == '__main__':
sys.exit(pytest.main([__file__, '-n0', '--no-summary'], plugins=[LatAccelReport()])) # noqa: TID251
sys.exit(pytest.main([__file__, '-n0', '--no-summary'],))
# plugins=[LatAccelReport()])) # noqa: TID251
Loading