From f5a5b9e141066866b1d3f01a80642c019fb399aa Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 26 Jun 2026 16:32:15 -0500 Subject: [PATCH] lat control refactor --- selfdrive/controls/lib/latcontrol_torque.py | 2036 +--------------- .../controls/lib/latcontrol_vehicle_tunes.py | 2042 +++++++++++++++++ 2 files changed, 2044 insertions(+), 2034 deletions(-) create mode 100644 selfdrive/controls/lib/latcontrol_vehicle_tunes.py diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 80a89a54bc..b755b09c0d 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -3,17 +3,14 @@ import numpy as np from collections import deque from cereal import log -from opendbc.car.gm.values import CAR as GM_CAR from opendbc.car.honda.values import CAR as HONDA_CAR, HondaFlags -from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR -from opendbc.car.toyota.values import CAR as TOYOTA_CAR from opendbc.car.lateral import get_friction -from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY, CV +from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.pid import PIDController from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED from openpilot.selfdrive.controls.lib.latcontrol import LatControl -from openpilot.starpilot.common.testing_grounds import testing_ground +from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import * # noqa: F403 # At higher speeds (25+mph) we can assume: # Lateral acceleration achieved by a specific car correlates to @@ -47,2035 +44,6 @@ DEADZONE_BOOST_LAT_ACCEL = 0.15 UNWIND_D_DES_THRESHOLD = -1.0 UNWIND_LAT_ACCEL_NEAR_ZERO = 0.3 MIN_LATERAL_CONTROL_SPEED = 0.3 -CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30 -CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT = 1.20 -CIVIC_BOSCH_MODIFIED_A_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.00 -CIVIC_BOSCH_MODIFIED_B_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.75 -CIVIC_BOSCH_MODIFIED_B_TRANSITION_SPEED = 12.0 -CIVIC_BOSCH_MODIFIED_B_PHASE_SCALE = 0.08 -CIVIC_BOSCH_MODIFIED_B_FF_ONSET = 0.18 -CIVIC_BOSCH_MODIFIED_B_FF_ONSET_WIDTH = 0.07 -CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF = 1.35 -CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF_WIDTH = 0.38 -CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_LEFT = 0.14 -CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_RIGHT = 0.24 -CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_LEFT = 0.04 -CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_RIGHT = 0.00 -CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_LEFT = 0.40 -CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_RIGHT = 0.60 -CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT = 0.02 -CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 -CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT = 0.26 -CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT = 0.40 -CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT = -0.10 -CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT = 0.02 -CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT = -0.05 -CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT = 0.02 -CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT = 0.24 -CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT = 0.06 -CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = -0.02 -CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.01 -CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.22 -CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.05 -CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_MAX = 0.12 -CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT = 0.24 -CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT_WIDTH = 0.05 -CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED = 18.0 -CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED_WIDTH = 2.5 -CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.50 -CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.82 -CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.00 -CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT = 0.00 -CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 4.40 -CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 6.20 -CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00 -CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 -CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 2.80 -CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 4.20 - -BOLT_2022_2023_CARS = ( - GM_CAR.CHEVROLET_BOLT_ACC_2022_2023, - GM_CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL, - GM_CAR.CHEVROLET_BOLT_CC_2022_2023, -) -BOLT_2018_2021_CARS = ( - GM_CAR.CHEVROLET_BOLT_CC_2018_2021, -) -BOLT_2017_CARS = ( - GM_CAR.CHEVROLET_BOLT_CC_2017, -) -BOLT_CARS = BOLT_2022_2023_CARS + BOLT_2018_2021_CARS + BOLT_2017_CARS -VOLT_STANDARD_CARS = ( - GM_CAR.CHEVROLET_VOLT, - GM_CAR.CHEVROLET_VOLT_ASCM, - GM_CAR.CHEVROLET_VOLT_CAMERA, - GM_CAR.CHEVROLET_VOLT_CC, -) -SILVERADO_CARS = ( - GM_CAR.CHEVROLET_SILVERADO, - GM_CAR.CHEVROLET_SILVERADO_CC, -) -GENESIS_G90_CARS = ( - HYUNDAI_CAR.GENESIS_G90, -) -PALISADE_CARS = ( - HYUNDAI_CAR.HYUNDAI_PALISADE, - HYUNDAI_CAR.HYUNDAI_PALISADE_2023, -) -IONIQ_5_CARS = ( - HYUNDAI_CAR.HYUNDAI_IONIQ_5, -) -IONIQ_EV_OLD_CARS = ( - HYUNDAI_CAR.HYUNDAI_IONIQ_EV_LTD, - HYUNDAI_CAR.HYUNDAI_IONIQ_EV_2020, -) -IONIQ_6_CARS = ( - HYUNDAI_CAR.HYUNDAI_IONIQ_6, -) -SONATA_HYBRID_CARS = ( - HYUNDAI_CAR.HYUNDAI_SONATA_HYBRID, -) -SONATA_CARS = ( - HYUNDAI_CAR.HYUNDAI_SONATA, -) -ELANTRA_NON_SCC_CARS = ( - HYUNDAI_CAR.HYUNDAI_ELANTRA_2022_NON_SCC, - HYUNDAI_CAR.HYUNDAI_ELANTRA_HEV_2022_NON_SCC, -) -KIA_EV6_CARS = ( - HYUNDAI_CAR.KIA_EV6, -) -KIA_XCEED_CARS = ( - HYUNDAI_CAR.KIA_XCEED_PHEV, -) -KIA_NIRO_PHEV_2022_CARS = ( - HYUNDAI_CAR.KIA_NIRO_PHEV_2022, -) -KIA_FORTE_CARS = ( - HYUNDAI_CAR.KIA_FORTE, - HYUNDAI_CAR.KIA_FORTE_2019_NON_SCC, - HYUNDAI_CAR.KIA_FORTE_2021_NON_SCC, -) -PRIUS_CARS = ( - TOYOTA_CAR.TOYOTA_PRIUS, -) - -BOLT_2017_LATERAL_TESTING_GROUND_ID = testing_ground.id_3 -BOLT_2017_STEER_RATIO_TEST_SCALE = 1.045 -BOLT_2017_STEER_RATIO_ONSET_SPEED = 20.0 * CV.MPH_TO_MS -BOLT_2017_STEER_RATIO_ONSET_WIDTH = 4.0 * CV.MPH_TO_MS -BOLT_2017_CENTER_TAPER_LAT = 0.10 -BOLT_2017_CENTER_TAPER_WIDTH = 0.03 -BOLT_2017_CENTER_TAPER_GAIN = 0.055 -BOLT_2017_TORQUE_SCALE_BP = [0.0, 0.2, 0.5, 1.0, 1.5, 2.5] -BOLT_2017_TORQUE_SCALE_LEFT = [1.0, 1.0, 1.065, 1.060, 1.055, 1.045] -BOLT_2017_TORQUE_SCALE_RIGHT = [1.0, 1.0, 1.035, 1.020, 0.995, 0.985] -BOLT_2017_TRANSITION_SPEED = 10.0 -BOLT_2017_PHASE_SCALE = 0.12 -BOLT_2017_TURN_IN_BOOST_LEFT = 0.28 -BOLT_2017_TURN_IN_BOOST_RIGHT = 0.18 -BOLT_2017_UNWIND_TAPER_LEFT = 0.08 -BOLT_2017_UNWIND_TAPER_RIGHT = 0.28 - -BOLT_2018_2021_LATERAL_TESTING_GROUND_ID = testing_ground.id_4 -BOLT_2018_2021_STEER_RATIO_TEST_SCALE = 1.01 -BOLT_2018_2021_TORQUE_GAIN_LEFT = 0.090 -BOLT_2018_2021_TORQUE_GAIN_RIGHT = 0.050 -BOLT_2018_2021_TORQUE_ONSET = 0.18 -BOLT_2018_2021_TORQUE_ONSET_WIDTH = 0.08 -BOLT_2018_2021_TORQUE_CUTOFF = 1.05 -BOLT_2018_2021_TORQUE_CUTOFF_WIDTH = 0.24 -BOLT_2018_2021_JERK_TAPER_CUTOFF = 0.42 -BOLT_2018_2021_CENTER_TAPER_LAT = 0.12 -BOLT_2018_2021_CENTER_TAPER_WIDTH = 0.04 -BOLT_2018_2021_CENTER_TAPER_GAIN = 0.35 -BOLT_2018_2021_TRANSITION_SPEED = 8.5 -BOLT_2018_2021_PHASE_SCALE = 0.10 -BOLT_2018_2021_TURN_IN_BOOST_LEFT = 0.22 -BOLT_2018_2021_TURN_IN_BOOST_RIGHT = 0.12 -BOLT_2018_2021_UNWIND_TAPER_GAIN_LEFT = 0.80 -BOLT_2018_2021_UNWIND_TAPER_GAIN_RIGHT = 1.04 -BOLT_2018_2021_FRICTION_MULT = 1.01 -BOLT_2018_2021_FRICTION_LAT_RISE = 0.24 -BOLT_2018_2021_FRICTION_JERK_RISE = 0.28 -BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.16 -BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.16 -BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_LEFT = 0.15 -BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.25 -BOLT_2018_2021_TURN_IN_FRICTION_BOOST_LEFT = 0.08 -BOLT_2018_2021_TURN_IN_FRICTION_BOOST_RIGHT = 0.08 -BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_LEFT = 0.17 -BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_RIGHT = 0.27 - -BOLT_2022_2023_LATERAL_TESTING_GROUND_ID = testing_ground.id_5 -BOLT_2022_2023_FF_GAIN_LEFT = 0.11 -BOLT_2022_2023_FF_GAIN_RIGHT = 0.06 -BOLT_2022_2023_FF_ONSET = 0.12 -BOLT_2022_2023_FF_ONSET_WIDTH = 0.07 -BOLT_2022_2023_FF_CUTOFF = 1.35 -BOLT_2022_2023_FF_CUTOFF_WIDTH = 0.28 -BOLT_2022_2023_TRANSITION_SPEED = 9.0 -BOLT_2022_2023_PHASE_SCALE = 0.12 -BOLT_2022_2023_TURN_IN_BOOST_LEFT = 0.18 -BOLT_2022_2023_TURN_IN_BOOST_RIGHT = 0.13 -BOLT_2022_2023_UNWIND_TAPER_LEFT = 0.38 -BOLT_2022_2023_UNWIND_TAPER_RIGHT = 0.40 -BOLT_2022_2023_FRICTION_MULT = 1.09 -BOLT_2022_2023_FRICTION_LAT_RISE = 0.22 -BOLT_2022_2023_FRICTION_JERK_RISE = 0.26 -BOLT_2022_2023_CENTER_TAPER_MAX = 0.11 -BOLT_2022_2023_CENTER_TAPER_LAT = 0.18 -BOLT_2022_2023_CENTER_TAPER_LAT_WIDTH = 0.03 -BOLT_2022_2023_CENTER_TAPER_SPEED = 25.0 -BOLT_2022_2023_CENTER_TAPER_SPEED_WIDTH = 2.5 -BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.16 -BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.12 -BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT = 0.26 -BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.28 -BOLT_2022_2023_TURN_IN_FRICTION_BOOST_LEFT = 0.10 -BOLT_2022_2023_TURN_IN_FRICTION_BOOST_RIGHT = 0.07 -BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_LEFT = 0.27 -BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_RIGHT = 0.28 - -VOLT_STANDARD_LATERAL_TESTING_GROUND_ID = testing_ground.id_3 -VOLT_STANDARD_FF_GAIN_LEFT = 0.13 -VOLT_STANDARD_FF_GAIN_RIGHT = 0.06 -VOLT_STANDARD_FF_ONSET = 0.10 -VOLT_STANDARD_FF_ONSET_WIDTH = 0.05 -VOLT_STANDARD_FF_CUTOFF = 1.38 -VOLT_STANDARD_FF_CUTOFF_WIDTH = 0.28 -VOLT_STANDARD_TRANSITION_SPEED = 10.0 -VOLT_STANDARD_PHASE_SCALE = 0.10 -VOLT_STANDARD_TURN_IN_BOOST_LEFT = 0.20 -VOLT_STANDARD_TURN_IN_BOOST_RIGHT = 0.20 -VOLT_STANDARD_UNWIND_TAPER_LEFT = 0.20 -VOLT_STANDARD_UNWIND_TAPER_RIGHT = 0.20 -VOLT_STANDARD_FRICTION_MULT = 1.00 -VOLT_STANDARD_FRICTION_LAT_RISE = 0.20 -VOLT_STANDARD_FRICTION_JERK_RISE = 0.20 -VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.05 -VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.05 -VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_LEFT = 0.05 -VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.05 -VOLT_STANDARD_TURN_IN_FRICTION_BOOST_LEFT = 0.00 -VOLT_STANDARD_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 -VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_LEFT = 0.05 -VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_RIGHT = 0.05 -VOLT_STANDARD_CENTER_TAPER_MAX = 0.12 -VOLT_STANDARD_CENTER_TAPER_LAT = 0.10 -VOLT_STANDARD_CENTER_TAPER_LAT_WIDTH = 0.018 -VOLT_STANDARD_CENTER_TAPER_SPEED = 20.0 -VOLT_STANDARD_CENTER_TAPER_SPEED_WIDTH = 2.5 - -SILVERADO_CENTER_TAPER_MAX = 0.22 -SILVERADO_CENTER_TAPER_LAT = 0.18 -SILVERADO_CENTER_TAPER_LAT_WIDTH = 0.05 -SILVERADO_CENTER_TAPER_SPEED = 12.0 -SILVERADO_CENTER_TAPER_SPEED_WIDTH = 2.5 - -SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT = 1.05 -SONATA_HYBRID_FF_REDUCTION_LEFT = 0.09 -SONATA_HYBRID_FF_REDUCTION_RIGHT = 0.22 -SONATA_HYBRID_FF_ONSET = 0.18 -SONATA_HYBRID_FF_ONSET_WIDTH = 0.08 -SONATA_HYBRID_FF_CUTOFF = 1.35 -SONATA_HYBRID_FF_CUTOFF_WIDTH = 0.40 -SONATA_HYBRID_TRANSITION_SPEED = 8.0 -SONATA_HYBRID_PHASE_SCALE = 0.12 -SONATA_HYBRID_TURN_IN_BOOST_LEFT = 0.12 -SONATA_HYBRID_TURN_IN_BOOST_RIGHT = 0.02 -SONATA_HYBRID_UNWIND_TAPER_LEFT = 0.18 -SONATA_HYBRID_UNWIND_TAPER_RIGHT = 0.10 -SONATA_HYBRID_CENTER_TAPER_MAX = 0.07 -SONATA_HYBRID_CENTER_TAPER_LAT = 0.16 -SONATA_HYBRID_CENTER_TAPER_LAT_WIDTH = 0.025 -SONATA_HYBRID_CENTER_TAPER_SPEED = 22.0 -SONATA_HYBRID_CENTER_TAPER_SPEED_WIDTH = 2.5 -SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_MAX = 0.14 -SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT = 0.10 -SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02 -SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 7.5 -SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.0 - -SONATA_FF_REDUCTION_LEFT = 0.04 -SONATA_FF_REDUCTION_RIGHT = 0.26 -SONATA_FF_ONSET = 0.18 -SONATA_FF_ONSET_WIDTH = 0.08 -SONATA_FF_CUTOFF = 1.40 -SONATA_FF_CUTOFF_WIDTH = 0.42 -SONATA_TRANSITION_SPEED = 8.5 -SONATA_PHASE_SCALE = 0.12 -SONATA_TURN_IN_BOOST_LEFT = 0.18 -SONATA_TURN_IN_BOOST_RIGHT = 0.00 -SONATA_UNWIND_TAPER_LEFT = 0.28 -SONATA_UNWIND_TAPER_RIGHT = 0.00 -SONATA_CENTER_TAPER_MAX = 0.04 -SONATA_CENTER_TAPER_LAT = 0.15 -SONATA_CENTER_TAPER_LAT_WIDTH = 0.025 -SONATA_CENTER_TAPER_SPEED = 22.0 -SONATA_CENTER_TAPER_SPEED_WIDTH = 2.5 -SONATA_LOW_SPEED_CENTER_TAPER_MAX = 0.08 -SONATA_LOW_SPEED_CENTER_TAPER_LAT = 0.10 -SONATA_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02 -SONATA_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 7.0 -SONATA_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.0 - -ELANTRA_NON_SCC_FF_ADJUST_LEFT = 0.02 -ELANTRA_NON_SCC_FF_ADJUST_RIGHT = -0.02 -ELANTRA_NON_SCC_FF_ONSET = 0.14 -ELANTRA_NON_SCC_FF_ONSET_WIDTH = 0.06 -ELANTRA_NON_SCC_FF_CUTOFF = 1.10 -ELANTRA_NON_SCC_FF_CUTOFF_WIDTH = 0.34 -ELANTRA_NON_SCC_TRANSITION_SPEED = 8.0 -ELANTRA_NON_SCC_PHASE_SCALE = 0.10 -ELANTRA_NON_SCC_TURN_IN_BOOST_LEFT = 0.10 -ELANTRA_NON_SCC_TURN_IN_BOOST_RIGHT = 0.12 -ELANTRA_NON_SCC_UNWIND_TAPER_LEFT = 0.22 -ELANTRA_NON_SCC_UNWIND_TAPER_RIGHT = 0.12 - -KIA_XCEED_FF_REDUCTION_LEFT = 0.04 -KIA_XCEED_FF_REDUCTION_RIGHT = 0.10 -KIA_XCEED_FF_ONSET = 0.16 -KIA_XCEED_FF_ONSET_WIDTH = 0.06 -KIA_XCEED_FF_CUTOFF = 1.30 -KIA_XCEED_FF_CUTOFF_WIDTH = 0.36 -KIA_XCEED_TRANSITION_SPEED = 11.5 -KIA_XCEED_PHASE_SCALE = 0.10 -KIA_XCEED_TURN_IN_BOOST_LEFT = 0.26 -KIA_XCEED_TURN_IN_BOOST_RIGHT = 0.06 -KIA_XCEED_UNWIND_TAPER_LEFT = 0.20 -KIA_XCEED_UNWIND_TAPER_RIGHT = 0.14 -KIA_XCEED_CENTER_TAPER_MAX = 0.06 -KIA_XCEED_CENTER_TAPER_LAT = 0.12 -KIA_XCEED_CENTER_TAPER_LAT_WIDTH = 0.03 -KIA_XCEED_CENTER_TAPER_SPEED = 17.5 -KIA_XCEED_CENTER_TAPER_SPEED_WIDTH = 2.5 - -KIA_NIRO_PHEV_2022_CENTER_TAPER_MAX = 0.06 -KIA_NIRO_PHEV_2022_CENTER_TAPER_LAT = 0.12 -KIA_NIRO_PHEV_2022_CENTER_TAPER_LAT_WIDTH = 0.03 -KIA_NIRO_PHEV_2022_CENTER_TAPER_SPEED = 23.0 -KIA_NIRO_PHEV_2022_CENTER_TAPER_SPEED_WIDTH = 2.5 -KIA_NIRO_PHEV_2022_FRICTION_CENTER_LAT = 0.12 -KIA_NIRO_PHEV_2022_FRICTION_CENTER_LAT_WIDTH = 0.03 -KIA_NIRO_PHEV_2022_FRICTION_SPEED = 23.0 -KIA_NIRO_PHEV_2022_FRICTION_SPEED_WIDTH = 2.5 -KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK = 0.22 -KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK_WIDTH = 0.06 -KIA_NIRO_PHEV_2022_FRICTION_THRESHOLD_GAIN = 0.12 - -KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT = 1.05 -KIA_FORTE_FF_REDUCTION_LEFT = 0.05 -KIA_FORTE_FF_REDUCTION_RIGHT = 0.10 -KIA_FORTE_FF_ONSET = 0.16 -KIA_FORTE_FF_ONSET_WIDTH = 0.06 -KIA_FORTE_FF_CUTOFF = 1.20 -KIA_FORTE_FF_CUTOFF_WIDTH = 0.36 -KIA_FORTE_TRANSITION_SPEED = 9.0 -KIA_FORTE_PHASE_SCALE = 0.10 -KIA_FORTE_TURN_IN_BOOST_LEFT = 0.10 -KIA_FORTE_TURN_IN_BOOST_RIGHT = 0.05 -KIA_FORTE_UNWIND_TAPER_LEFT = 0.18 -KIA_FORTE_UNWIND_TAPER_RIGHT = 0.02 -KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.10 -KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.14 -KIA_FORTE_CRAWL_TURN_IN_FF_SPEED = 4.5 -KIA_FORTE_CRAWL_TURN_IN_FF_SPEED_WIDTH = 0.8 -KIA_FORTE_CRAWL_TURN_IN_FF_LAT = 0.10 -KIA_FORTE_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.05 -KIA_FORTE_CENTER_TAPER_MAX = 0.12 -KIA_FORTE_CENTER_TAPER_LAT = 0.18 -KIA_FORTE_CENTER_TAPER_LAT_WIDTH = 0.04 -KIA_FORTE_CENTER_TAPER_SPEED = 22.5 -KIA_FORTE_CENTER_TAPER_SPEED_WIDTH = 3.0 -KIA_FORTE_FRICTION_CENTER_LAT = 0.18 -KIA_FORTE_FRICTION_CENTER_LAT_WIDTH = 0.04 -KIA_FORTE_FRICTION_SPEED = 24.0 -KIA_FORTE_FRICTION_SPEED_WIDTH = 3.0 -KIA_FORTE_FRICTION_CALM_JERK = 0.24 -KIA_FORTE_FRICTION_CALM_JERK_WIDTH = 0.08 -KIA_FORTE_FRICTION_THRESHOLD_GAIN = 0.10 - -PALISADE_BASE_LAT_ACCEL_FACTOR_MULT = 0.98 -PALISADE_FF_GAIN_LEFT = 0.14 -PALISADE_FF_GAIN_RIGHT = 0.12 -PALISADE_FF_ONSET = 0.08 -PALISADE_FF_ONSET_WIDTH = 0.04 -PALISADE_FF_CUTOFF = 1.25 -PALISADE_FF_CUTOFF_WIDTH = 0.36 -PALISADE_TRANSITION_SPEED = 9.0 -PALISADE_PHASE_SCALE = 0.11 -PALISADE_TURN_IN_BOOST_LEFT = 0.34 -PALISADE_TURN_IN_BOOST_RIGHT = 0.24 -PALISADE_UNWIND_TAPER_LEFT = 0.18 -PALISADE_UNWIND_TAPER_RIGHT = 0.30 -PALISADE_FRICTION_MULT = 1.02 -PALISADE_FRICTION_LAT_RISE = 0.20 -PALISADE_FRICTION_JERK_RISE = 0.24 -PALISADE_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.18 -PALISADE_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.14 -PALISADE_UNWIND_THRESHOLD_INCREASE_LEFT = 0.14 -PALISADE_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.22 -PALISADE_TURN_IN_FRICTION_BOOST_LEFT = 0.08 -PALISADE_TURN_IN_FRICTION_BOOST_RIGHT = 0.06 -PALISADE_UNWIND_FRICTION_REDUCTION_LEFT = 0.12 -PALISADE_UNWIND_FRICTION_REDUCTION_RIGHT = 0.20 - -GENESIS_G90_LATERAL_TESTING_GROUND_ID = testing_ground.id_4 -GENESIS_G90_FF_GAIN_LEFT = 0.32 -GENESIS_G90_FF_GAIN_RIGHT = 0.16 -GENESIS_G90_FF_ONSET = 0.10 -GENESIS_G90_FF_ONSET_WIDTH = 0.05 -GENESIS_G90_FF_CUTOFF = 2.35 -GENESIS_G90_FF_CUTOFF_WIDTH = 0.48 -GENESIS_G90_TRANSITION_SPEED = 10.0 -GENESIS_G90_PHASE_SCALE = 0.12 -GENESIS_G90_TURN_IN_BOOST_LEFT = 0.66 -GENESIS_G90_TURN_IN_BOOST_RIGHT = 0.50 -GENESIS_G90_UNWIND_TAPER_LEFT = 0.10 -GENESIS_G90_UNWIND_TAPER_RIGHT = 0.55 -GENESIS_G90_FRICTION_MULT = 1.02 -GENESIS_G90_FRICTION_LAT_RISE = 0.22 -GENESIS_G90_FRICTION_JERK_RISE = 0.24 -GENESIS_G90_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.28 -GENESIS_G90_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.22 -GENESIS_G90_UNWIND_THRESHOLD_INCREASE_LEFT = 0.06 -GENESIS_G90_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.32 -GENESIS_G90_TURN_IN_FRICTION_BOOST_LEFT = 0.16 -GENESIS_G90_TURN_IN_FRICTION_BOOST_RIGHT = 0.14 -GENESIS_G90_UNWIND_FRICTION_REDUCTION_LEFT = 0.04 -GENESIS_G90_UNWIND_FRICTION_REDUCTION_RIGHT = 0.30 - -IONIQ_5_BASE_LAT_ACCEL_FACTOR_MULT = 1.22 -IONIQ_5_FF_ONSET = 0.10 -IONIQ_5_FF_ONSET_WIDTH = 0.05 -IONIQ_5_FF_CUTOFF = 1.20 -IONIQ_5_FF_CUTOFF_WIDTH = 0.30 -IONIQ_5_TRANSITION_SPEED = 12.5 -IONIQ_5_PHASE_SCALE = 0.10 -IONIQ_5_FF_REDUCTION_LEFT = 0.15 -IONIQ_5_FF_REDUCTION_RIGHT = 0.25 -IONIQ_5_TURN_IN_BOOST_LEFT = 0.10 -IONIQ_5_TURN_IN_BOOST_RIGHT = 0.04 -IONIQ_5_UNWIND_TAPER_LEFT = 0.92 -IONIQ_5_UNWIND_TAPER_RIGHT = 1.04 -IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.05 -IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.03 -IONIQ_5_UNWIND_THRESHOLD_INCREASE_LEFT = 0.46 -IONIQ_5_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.50 -IONIQ_5_TURN_IN_FRICTION_BOOST_LEFT = 0.02 -IONIQ_5_TURN_IN_FRICTION_BOOST_RIGHT = 0.01 -IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT = 0.42 -IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT = 0.44 -IONIQ_5_CENTER_TAPER_MAX = 0.18 -IONIQ_5_CENTER_TAPER_LAT = 0.16 -IONIQ_5_CENTER_TAPER_LAT_WIDTH = 0.04 -IONIQ_5_CENTER_TAPER_SPEED = 15.0 -IONIQ_5_CENTER_TAPER_SPEED_WIDTH = 2.2 - -IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT = 1.16 -IONIQ_EV_OLD_FF_REDUCTION_LEFT = 0.16 -IONIQ_EV_OLD_FF_REDUCTION_RIGHT = 0.30 -IONIQ_EV_OLD_FF_ONSET = 0.14 -IONIQ_EV_OLD_FF_ONSET_WIDTH = 0.05 -IONIQ_EV_OLD_FF_CUTOFF = 1.10 -IONIQ_EV_OLD_FF_CUTOFF_WIDTH = 0.30 -IONIQ_EV_OLD_TRANSITION_SPEED = 10.0 -IONIQ_EV_OLD_PHASE_SCALE = 0.10 -IONIQ_EV_OLD_TURN_IN_BOOST_LEFT = 0.01 -IONIQ_EV_OLD_TURN_IN_BOOST_RIGHT = 0.00 -IONIQ_EV_OLD_UNWIND_TAPER_LEFT = 0.26 -IONIQ_EV_OLD_UNWIND_TAPER_RIGHT = 0.06 -IONIQ_EV_OLD_CENTER_TAPER_MAX = 0.14 -IONIQ_EV_OLD_CENTER_TAPER_LAT = 0.12 -IONIQ_EV_OLD_CENTER_TAPER_LAT_WIDTH = 0.03 -IONIQ_EV_OLD_CENTER_TAPER_SPEED = 22.0 -IONIQ_EV_OLD_CENTER_TAPER_SPEED_WIDTH = 2.2 - -IONIQ_6_FF_GAIN_LEFT = 0.045 -IONIQ_6_FF_GAIN_RIGHT = 0.015 -IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT = 1.22 -IONIQ_6_BASE_FRICTION_THRESHOLD = 0.36 -IONIQ_6_FF_ONSET = 0.10 -IONIQ_6_FF_ONSET_WIDTH = 0.04 -IONIQ_6_FF_CUTOFF = 0.48 -IONIQ_6_FF_CUTOFF_WIDTH = 0.12 -IONIQ_6_TRANSITION_SPEED = 10.0 -IONIQ_6_PHASE_SCALE = 0.10 -IONIQ_6_TURN_IN_BOOST_LEFT = 1.64 -IONIQ_6_TURN_IN_BOOST_RIGHT = 2.10 -IONIQ_6_UNWIND_TAPER_LEFT = 3.18 -IONIQ_6_UNWIND_TAPER_RIGHT = 8.20 -IONIQ_6_FRICTION_MULT = 0.928 -IONIQ_6_FRICTION_LAT_RISE = 0.20 -IONIQ_6_FRICTION_JERK_RISE = 0.24 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.78 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 1.42 -IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 3.90 -IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 10.20 -IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.44 -IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.94 -IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 3.55 -IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 9.10 -IONIQ_6_CENTER_TAPER_MAX = 0.082 -IONIQ_6_CENTER_TAPER_LAT = 0.24 -IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.025 -IONIQ_6_CENTER_TAPER_SPEED = 18.0 -IONIQ_6_CENTER_TAPER_SPEED_WIDTH = 2.5 -IONIQ_6_HIGHWAY_CENTER_TAPER_MAX = 0.046 -IONIQ_6_HIGHWAY_CENTER_TAPER_LAT = 0.10 -IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH = 0.035 -IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED = 24.5 -IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED_WIDTH = 1.8 -IONIQ_6_HIGHWAY_OUTPUT_TAPER_MAX = 0.10 -IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT = 0.14 -IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH = 0.04 -IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED = 23.5 -IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH = 2.0 -IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_MAX = 0.18 -IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT = 1.05 -IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT_WIDTH = 0.22 -IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK = 0.24 -IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK_WIDTH = 0.14 -IONIQ_6_LOW_MID_CENTER_TAPER_MAX = 0.088 -IONIQ_6_LOW_MID_CENTER_TAPER_LAT = 0.28 -IONIQ_6_LOW_MID_CENTER_TAPER_LAT_WIDTH = 0.06 -IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MIN = 8.5 -IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MAX = 16.5 -IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH = 1.5 -IONIQ_6_DIRECTIONAL_TAPER_LAT_START = 0.19 -IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90 -IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.06 -IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.11 -IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.45 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 2.15 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 4.25 -IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT = 0.48 -IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT = 0.52 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.16 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.04 -IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 0.60 -IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.14 -IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.98 -IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 11.2 -IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 1.5 -IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT = 0.10 -IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH = 0.06 -IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.18 -IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.24 -IONIQ_6_CRAWL_TURN_IN_FF_SPEED = 5.3 -IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH = 1.0 -IONIQ_6_CRAWL_TURN_IN_FF_LAT = 0.06 -IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.035 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE = 0.46 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED = 3.25 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH = 0.45 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR = 1.9 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH = 1.20 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE = 5.5 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH = 2.4 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_START = 0.66 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_WIDTH = 0.12 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_FLOOR = 0.26 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_BP = [0.0, 0.35, 0.65, 1.0] -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_V = [1.0, 1.0, 0.88, 0.08] -IONIQ_6_LOW_SPEED_UNWIND_ASSIST_MAX_TORQUE = 0.30 -IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED = 3.35 -IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED_WIDTH = 0.50 -IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR = 1.6 -IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR_WIDTH = 0.95 -IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE = 10.5 -IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE_WIDTH = 4.0 -IONIQ_6_LOW_SPEED_UNWIND_ASSIST_BLEND = 0.52 -IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST = 0.10 -IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED = 18.0 -IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH = 2.5 -IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_START = 0.06 -IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END = 0.22 -IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH = 0.035 -IONIQ_6_LOW_SPEED_PID_RESET_SPEED = 0.1 * CV.MPH_TO_MS -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.90 -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.18 -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.06 -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.17 -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.78 -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.10 -IONIQ_6_OUTPUT_TAPER_SPEED = 8.5 -IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5 -IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.90 -IONIQ_6_OUTPUT_DIRECTIONAL_TAPER_BLEND = 0.97 - -KIA_EV6_LATERAL_TESTING_GROUND_ID = testing_ground.id_6 -KIA_EV6_LATERAL_TESTING_GROUND_VARIANT = "C" -KIA_EV6_FF_GAIN_LEFT = 0.12 -KIA_EV6_FF_GAIN_RIGHT = 0.17 -KIA_EV6_FF_ONSET = 0.08 -KIA_EV6_FF_ONSET_WIDTH = 0.04 -KIA_EV6_FF_CUTOFF = 1.90 -KIA_EV6_FF_CUTOFF_WIDTH = 0.40 -KIA_EV6_TRANSITION_SPEED = 14.5 -KIA_EV6_PHASE_SCALE = 0.09 -KIA_EV6_TURN_IN_BOOST_LEFT = 0.48 -KIA_EV6_TURN_IN_BOOST_RIGHT = 0.60 -KIA_EV6_UNWIND_TAPER_LEFT = 0.56 -KIA_EV6_UNWIND_TAPER_RIGHT = 0.54 -KIA_EV6_FRICTION_MULT = 1.01 -KIA_EV6_FRICTION_LAT_RISE = 0.18 -KIA_EV6_FRICTION_JERK_RISE = 0.22 -KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.24 -KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.40 -KIA_EV6_UNWIND_THRESHOLD_INCREASE_LEFT = 0.28 -KIA_EV6_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.24 -KIA_EV6_TURN_IN_FRICTION_BOOST_LEFT = 0.12 -KIA_EV6_TURN_IN_FRICTION_BOOST_RIGHT = 0.24 -KIA_EV6_UNWIND_FRICTION_REDUCTION_LEFT = 0.28 -KIA_EV6_UNWIND_FRICTION_REDUCTION_RIGHT = 0.22 -KIA_EV6_CENTER_TAPER_MAX = 0.12 -KIA_EV6_CENTER_TAPER_LAT = 0.16 -KIA_EV6_CENTER_TAPER_LAT_WIDTH = 0.04 -KIA_EV6_CENTER_TAPER_SPEED = 17.0 -KIA_EV6_CENTER_TAPER_SPEED_WIDTH = 2.8 -KIA_EV6_LOW_SPEED_CENTER_TAPER_MAX = 0.24 -KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT = 0.08 -KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02 -KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 8.5 -KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.4 - -VOLT_PLEXY_LATERAL_TESTING_GROUND_ID = testing_ground.id_7 -VOLT_PLEXY_FF_EXTRA_MULT_LEFT = 0.97 -VOLT_PLEXY_FF_EXTRA_MULT_RIGHT = 1.16 -VOLT_PLEXY_FRICTION_SCALE_MULT_LEFT = 1.00 -VOLT_PLEXY_FRICTION_SCALE_MULT_RIGHT = 1.05 -VOLT_PLEXY_FRICTION_THRESHOLD_MULT_LEFT = 1.00 -VOLT_PLEXY_FRICTION_THRESHOLD_MULT_RIGHT = 0.96 -VOLT_PLEXY_CENTER_TAPER_REDUCTION_MULT = 0.98 -PRIUS_TRANSITION_SPEED = 11.0 -PRIUS_PHASE_SCALE = 0.09 -PRIUS_FF_GAIN_LEFT = 0.08 -PRIUS_FF_GAIN_RIGHT = 0.09 -PRIUS_FF_ONSET = 0.15 -PRIUS_FF_ONSET_WIDTH = 0.08 -PRIUS_FF_CUTOFF = 1.45 -PRIUS_FF_CUTOFF_WIDTH = 0.36 -PRIUS_FRICTION_LAT_RISE = 0.18 -PRIUS_FRICTION_JERK_RISE = 0.22 -PRIUS_TURN_IN_BOOST_LEFT = 0.38 -PRIUS_TURN_IN_BOOST_RIGHT = 0.34 -PRIUS_UNWIND_TAPER_LEFT = 0.28 -PRIUS_UNWIND_TAPER_RIGHT = 0.42 -PRIUS_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.22 -PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.22 -PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT = 0.28 -PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.38 -PRIUS_TURN_IN_FRICTION_BOOST_LEFT = 0.10 -PRIUS_TURN_IN_FRICTION_BOOST_RIGHT = 0.10 -PRIUS_UNWIND_FRICTION_REDUCTION_LEFT = 0.16 -PRIUS_UNWIND_FRICTION_REDUCTION_RIGHT = 0.22 -PRIUS_CENTER_TAPER_MAX = 0.12 -PRIUS_CENTER_TAPER_LAT = 0.14 -PRIUS_CENTER_TAPER_LAT_WIDTH = 0.035 -PRIUS_CENTER_TAPER_SPEED = 20.0 -PRIUS_CENTER_TAPER_SPEED_WIDTH = 2.5 - -TRAILER_LOAD_FULL_ASSIST_KG = 15000.0 * CV.LB_TO_KG -TRAILER_LATERAL_MIN_SPEED = 15.0 * CV.MPH_TO_MS -TRAILER_LATERAL_FULL_SPEED = 35.0 * CV.MPH_TO_MS -TRAILER_LATERAL_LAT_RISE = 0.30 -TRAILER_LATERAL_FF_GAIN = 0.05 -TRAILER_LATERAL_FRICTION_GAIN = 0.03 - - -def _sigmoid(x: float) -> float: - if x >= 0.0: - z = math.exp(-x) - return 1.0 / (1.0 + z) - - z = math.exp(x) - return z / (1.0 + z) - - -def get_friction_threshold(v_ego: float) -> float: - # Keep the speed-scaled friction threshold behavior. - return float(np.interp(v_ego, [1 * CV.MPH_TO_MS, 20 * CV.MPH_TO_MS, 75 * CV.MPH_TO_MS], [0.16, 0.19, 0.27])) - - -def get_trailer_lateral_assist_factor(trailer_load_kg: float, v_ego: float, desired_lateral_accel: float) -> float: - load_factor = np.clip(trailer_load_kg / TRAILER_LOAD_FULL_ASSIST_KG, 0.0, 1.0) - speed_factor = np.interp(v_ego, [TRAILER_LATERAL_MIN_SPEED, TRAILER_LATERAL_FULL_SPEED], [0.0, 1.0]) - lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / TRAILER_LATERAL_LAT_RISE) - return float(load_factor * speed_factor * lat_factor) - - -def get_trailer_lateral_ff_scale(trailer_load_kg: float, v_ego: float, desired_lateral_accel: float) -> float: - return 1.0 + TRAILER_LATERAL_FF_GAIN * get_trailer_lateral_assist_factor(trailer_load_kg, v_ego, desired_lateral_accel) - - -def get_trailer_lateral_friction_scale(trailer_load_kg: float, v_ego: float, desired_lateral_accel: float) -> float: - return 1.0 + TRAILER_LATERAL_FRICTION_GAIN * get_trailer_lateral_assist_factor(trailer_load_kg, v_ego, desired_lateral_accel) - - -def _prius_sigmoid(x: float) -> float: - return _sigmoid(x) - - -def _prius_low_speed_factor(v_ego: float) -> float: - return 1.0 / (1.0 + (max(v_ego, 0.0) / PRIUS_TRANSITION_SPEED) ** 2) - - -def _prius_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - return math.tanh((desired_lateral_accel * desired_lateral_jerk) / PRIUS_PHASE_SCALE) - - -def _prius_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def _prius_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / PRIUS_FRICTION_LAT_RISE) - jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / PRIUS_FRICTION_JERK_RISE) - return _prius_low_speed_factor(v_ego) * lat_factor * jerk_factor - - -def get_prius_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - gain = _prius_side_value(desired_lateral_accel, PRIUS_FF_GAIN_LEFT, PRIUS_FF_GAIN_RIGHT) - abs_lateral_accel = abs(desired_lateral_accel) - onset = _prius_sigmoid((abs_lateral_accel - PRIUS_FF_ONSET) / PRIUS_FF_ONSET_WIDTH) - cutoff = _prius_sigmoid((PRIUS_FF_CUTOFF - abs_lateral_accel) / PRIUS_FF_CUTOFF_WIDTH) - extra_scale = gain * onset * cutoff - phase = _prius_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - low_speed_factor = _prius_low_speed_factor(v_ego) - turn_in_boost = 1.0 + (_prius_side_value(desired_lateral_accel, PRIUS_TURN_IN_BOOST_LEFT, PRIUS_TURN_IN_BOOST_RIGHT) * - turn_in_weight * (0.35 + 0.65 * low_speed_factor)) - unwind_taper = 1.0 - (_prius_side_value(desired_lateral_accel, PRIUS_UNWIND_TAPER_LEFT, PRIUS_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.35 + 0.65 * low_speed_factor)) - return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) - - -def get_prius_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: - base_threshold = get_friction_threshold(v_ego) - transition_envelope = _prius_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _prius_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - threshold_scale = 1.0 - (_prius_side_value(desired_lateral_accel, PRIUS_TURN_IN_THRESHOLD_REDUCTION_LEFT, PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * - transition_envelope * turn_in_weight) - threshold_scale += (_prius_side_value(desired_lateral_accel, PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT, PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT) * - transition_envelope * unwind_weight) - return base_threshold * min(max(threshold_scale, 0.86), 1.16) - - -def get_prius_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - transition_envelope = _prius_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _prius_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - friction_scale = 1.0 - friction_scale += (_prius_side_value(desired_lateral_accel, PRIUS_TURN_IN_FRICTION_BOOST_LEFT, PRIUS_TURN_IN_FRICTION_BOOST_RIGHT) * - transition_envelope * turn_in_weight) - friction_scale -= (_prius_side_value(desired_lateral_accel, PRIUS_UNWIND_FRICTION_REDUCTION_LEFT, PRIUS_UNWIND_FRICTION_REDUCTION_RIGHT) * - transition_envelope * unwind_weight) - return min(max(friction_scale, 0.90), 1.14) - - -def get_prius_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: - speed_weight = _prius_sigmoid((v_ego - PRIUS_CENTER_TAPER_SPEED) / PRIUS_CENTER_TAPER_SPEED_WIDTH) - center_weight = _prius_sigmoid((PRIUS_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / PRIUS_CENTER_TAPER_LAT_WIDTH) - reduction = PRIUS_CENTER_TAPER_MAX * speed_weight * center_weight - return 1.0 - reduction - - -def civic_bosch_modified_lateral_testing_ground_active() -> bool: - return testing_ground.use("8", "B") - - -def civic_bosch_modified_a_lateral_testing_ground_active() -> bool: - return testing_ground.use("8", "A") - - -def get_civic_bosch_modified_a_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: - speed_weight = _sigmoid((v_ego - CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED) / - CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED_WIDTH) - center_weight = _sigmoid((CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / - CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT_WIDTH) - reduction = CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_MAX * speed_weight * center_weight - return 1.0 - reduction - - -def _civic_bosch_modified_b_low_speed_factor(v_ego: float) -> float: - return 1.0 / (1.0 + (max(v_ego, 0.0) / CIVIC_BOSCH_MODIFIED_B_TRANSITION_SPEED) ** 2) - - -def _civic_bosch_modified_b_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - return math.tanh((desired_lateral_accel * desired_lateral_jerk) / CIVIC_BOSCH_MODIFIED_B_PHASE_SCALE) - - -def _civic_bosch_modified_b_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def get_civic_bosch_modified_b_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - abs_lateral_accel = abs(desired_lateral_accel) - onset = _sigmoid((abs_lateral_accel - CIVIC_BOSCH_MODIFIED_B_FF_ONSET) / CIVIC_BOSCH_MODIFIED_B_FF_ONSET_WIDTH) - cutoff = _sigmoid((CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF - abs_lateral_accel) / CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF_WIDTH) - base_reduction = _civic_bosch_modified_b_side_value(desired_lateral_accel, - CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_LEFT, - CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_RIGHT) * onset * cutoff - - phase = _civic_bosch_modified_b_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - low_speed_factor = _civic_bosch_modified_b_low_speed_factor(v_ego) - a_variant_active = civic_bosch_modified_a_lateral_testing_ground_active() - variant_active = civic_bosch_modified_lateral_testing_ground_active() - if a_variant_active: - base_reduction -= (_civic_bosch_modified_b_side_value(desired_lateral_accel, - CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT, - CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT) * onset * cutoff) - if variant_active: - base_reduction += (_civic_bosch_modified_b_side_value(desired_lateral_accel, - CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT, - CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT) * onset * cutoff) - - turn_in_boost = 1.0 + (_civic_bosch_modified_b_side_value(desired_lateral_accel, - CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_LEFT, - CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_RIGHT) * - turn_in_weight * (0.40 + 0.60 * low_speed_factor)) - if a_variant_active: - turn_in_boost *= 1.0 + (_civic_bosch_modified_b_side_value(desired_lateral_accel, - CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT, - CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT) * - turn_in_weight * (0.40 + 0.60 * low_speed_factor)) - if variant_active: - turn_in_boost *= 1.0 + (_civic_bosch_modified_b_side_value(desired_lateral_accel, - CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT, - CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT) * - turn_in_weight * (0.40 + 0.60 * low_speed_factor)) - unwind_taper = 1.0 - (_civic_bosch_modified_b_side_value(desired_lateral_accel, - CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_LEFT, - CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.35 + 0.65 * low_speed_factor)) - if a_variant_active: - unwind_taper *= 1.0 - (_civic_bosch_modified_b_side_value(desired_lateral_accel, - CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT, - CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.35 + 0.65 * low_speed_factor)) - if variant_active: - unwind_taper *= 1.0 - (_civic_bosch_modified_b_side_value(desired_lateral_accel, - CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT, - CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.35 + 0.65 * low_speed_factor)) - return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0) - - -def get_civic_bosch_modified_b_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - if desired_lateral_accel == 0.0 or desired_lateral_jerk == 0.0: - return 1.0 - - abs_lateral_accel = abs(desired_lateral_accel) - onset = _sigmoid((abs_lateral_accel - CIVIC_BOSCH_MODIFIED_B_FF_ONSET) / CIVIC_BOSCH_MODIFIED_B_FF_ONSET_WIDTH) - cutoff = _sigmoid((CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF - abs_lateral_accel) / CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF_WIDTH) - envelope = onset * cutoff * _civic_bosch_modified_b_low_speed_factor(v_ego) - phase = _civic_bosch_modified_b_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - a_variant_active = civic_bosch_modified_a_lateral_testing_ground_active() - variant_active = civic_bosch_modified_lateral_testing_ground_active() - - friction_scale = 1.0 - friction_scale += (_civic_bosch_modified_b_side_value(desired_lateral_accel, - CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT, - CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT) * - envelope * turn_in_weight) - if a_variant_active: - friction_scale += (_civic_bosch_modified_b_side_value(desired_lateral_accel, - CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT, - CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT) * - envelope * turn_in_weight) - if variant_active: - friction_scale += (_civic_bosch_modified_b_side_value(desired_lateral_accel, - CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT, - CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT) * - envelope * turn_in_weight) - friction_scale -= (_civic_bosch_modified_b_side_value(desired_lateral_accel, - CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT, - CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT) * - envelope * unwind_weight) - if a_variant_active: - friction_scale -= (_civic_bosch_modified_b_side_value(desired_lateral_accel, - CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT, - CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT) * - envelope * unwind_weight) - if variant_active: - friction_scale -= (_civic_bosch_modified_b_side_value(desired_lateral_accel, - CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT, - CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT) * - envelope * unwind_weight) - return min(max(friction_scale, 0.82), 1.06) - - -def bolt_2017_lateral_testing_ground_active() -> bool: - return testing_ground.use(BOLT_2017_LATERAL_TESTING_GROUND_ID) - - -def _bolt_2017_sigmoid(x: float) -> float: - return _sigmoid(x) - - -def _bolt_2017_high_speed_factor(v_ego: float) -> float: - return _bolt_2017_sigmoid((max(v_ego, 0.0) - BOLT_2017_STEER_RATIO_ONSET_SPEED) / BOLT_2017_STEER_RATIO_ONSET_WIDTH) - - -def get_bolt_2017_steer_ratio_scale(v_ego: float) -> float: - return 1.0 + ((BOLT_2017_STEER_RATIO_TEST_SCALE - 1.0) * _bolt_2017_high_speed_factor(v_ego)) - - -def get_bolt_2017_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: - center_window = _bolt_2017_sigmoid((BOLT_2017_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / BOLT_2017_CENTER_TAPER_WIDTH) - return 1.0 - (BOLT_2017_CENTER_TAPER_GAIN * _bolt_2017_high_speed_factor(v_ego) * center_window) - - -def _bolt_2017_low_speed_factor(v_ego: float) -> float: - return 1.0 / (1.0 + (max(v_ego, 0.0) / BOLT_2017_TRANSITION_SPEED) ** 2) - - -def _bolt_2017_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - return math.tanh((desired_lateral_accel * desired_lateral_jerk) / BOLT_2017_PHASE_SCALE) - - -def _bolt_2017_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def get_bolt_2017_base_torque_scale(desired_lateral_accel: float) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - scale_values = BOLT_2017_TORQUE_SCALE_LEFT if desired_lateral_accel > 0.0 else BOLT_2017_TORQUE_SCALE_RIGHT - return float(np.interp(abs(desired_lateral_accel), BOLT_2017_TORQUE_SCALE_BP, scale_values)) - - -def get_bolt_2017_torque_scale(desired_lateral_accel: float, desired_lateral_jerk: float = 0.0, v_ego: float = 30.0) -> float: - base_scale = get_bolt_2017_base_torque_scale(desired_lateral_accel) - scale = base_scale - if base_scale > 1.0 and desired_lateral_jerk != 0.0: - low_speed_factor = _bolt_2017_low_speed_factor(v_ego) - phase = _bolt_2017_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - turn_in_boost = 1.0 + (_bolt_2017_side_value(desired_lateral_accel, BOLT_2017_TURN_IN_BOOST_LEFT, BOLT_2017_TURN_IN_BOOST_RIGHT) * - turn_in_weight * (0.35 + 0.65 * low_speed_factor)) - unwind_taper = 1.0 - (_bolt_2017_side_value(desired_lateral_accel, BOLT_2017_UNWIND_TAPER_LEFT, BOLT_2017_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.45 + 0.55 * low_speed_factor)) - scale = 1.0 + ((base_scale - 1.0) * turn_in_boost * max(unwind_taper, 0.0)) - - return scale * get_bolt_2017_center_taper_scale(desired_lateral_accel, v_ego) - - -def bolt_2018_2021_lateral_testing_ground_active() -> bool: - return testing_ground.use(BOLT_2018_2021_LATERAL_TESTING_GROUND_ID) - - -def _bolt_2018_2021_sigmoid(x: float) -> float: - return _sigmoid(x) - - -def _bolt_2018_2021_low_speed_factor(v_ego: float) -> float: - return 1.0 / (1.0 + (max(v_ego, 0.0) / BOLT_2018_2021_TRANSITION_SPEED) ** 2) - - -def _bolt_2018_2021_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - return math.tanh((desired_lateral_accel * desired_lateral_jerk) / BOLT_2018_2021_PHASE_SCALE) - - -def _bolt_2018_2021_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def _bolt_2018_2021_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / BOLT_2018_2021_FRICTION_LAT_RISE) - jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / BOLT_2018_2021_FRICTION_JERK_RISE) - return _bolt_2018_2021_low_speed_factor(v_ego) * lat_factor * jerk_factor - - -def get_bolt_2018_2021_torque_scale(desired_lateral_accel: float) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - gain = BOLT_2018_2021_TORQUE_GAIN_LEFT if desired_lateral_accel > 0.0 else BOLT_2018_2021_TORQUE_GAIN_RIGHT - abs_lateral_accel = abs(desired_lateral_accel) - onset = _bolt_2018_2021_sigmoid((abs_lateral_accel - BOLT_2018_2021_TORQUE_ONSET) / BOLT_2018_2021_TORQUE_ONSET_WIDTH) - cutoff = _bolt_2018_2021_sigmoid((BOLT_2018_2021_TORQUE_CUTOFF - abs_lateral_accel) / BOLT_2018_2021_TORQUE_CUTOFF_WIDTH) - return 1.0 + gain * onset * cutoff - - -def get_bolt_2018_2021_dynamic_torque_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - base_scale = get_bolt_2018_2021_torque_scale(desired_lateral_accel) - extra_scale = max(base_scale - 1.0, 0.0) - abs_lateral_accel = abs(desired_lateral_accel) - low_speed_factor = _bolt_2018_2021_low_speed_factor(v_ego) - high_speed_factor = 1.0 - low_speed_factor - center_window = _bolt_2018_2021_sigmoid((BOLT_2018_2021_CENTER_TAPER_LAT - abs_lateral_accel) / BOLT_2018_2021_CENTER_TAPER_WIDTH) - center_taper = 1.0 - (BOLT_2018_2021_CENTER_TAPER_GAIN * high_speed_factor * center_window) - phase = _bolt_2018_2021_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - jerk_taper = 1.0 / (1.0 + (abs(desired_lateral_jerk) / BOLT_2018_2021_JERK_TAPER_CUTOFF) ** 2) - turn_in_boost = 1.0 + (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_TURN_IN_BOOST_LEFT, BOLT_2018_2021_TURN_IN_BOOST_RIGHT) * - turn_in_weight * low_speed_factor) - unwind_weight = max(-phase, 0.0) - unwind_taper = 1.0 - (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_UNWIND_TAPER_GAIN_LEFT, BOLT_2018_2021_UNWIND_TAPER_GAIN_RIGHT) * - unwind_weight * (0.55 + 0.45 * low_speed_factor)) - return 1.0 + (extra_scale * center_taper * jerk_taper * turn_in_boost * max(unwind_taper, 0.0)) - - -def get_bolt_2018_2021_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: - base_threshold = get_friction_threshold(v_ego) - transition_envelope = _bolt_2018_2021_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _bolt_2018_2021_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - threshold_scale = 1.0 - (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_LEFT, BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * - transition_envelope * turn_in_weight) - threshold_scale += (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_LEFT, BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_RIGHT) * - transition_envelope * unwind_weight) - return base_threshold * min(max(threshold_scale, 0.82), 1.12) - - -def get_bolt_2018_2021_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - transition_envelope = _bolt_2018_2021_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _bolt_2018_2021_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - friction_scale = BOLT_2018_2021_FRICTION_MULT - friction_scale += (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_TURN_IN_FRICTION_BOOST_LEFT, BOLT_2018_2021_TURN_IN_FRICTION_BOOST_RIGHT) * - transition_envelope * turn_in_weight) - friction_scale -= (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_LEFT, BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_RIGHT) * - transition_envelope * unwind_weight) - return min(max(friction_scale, 0.88), 1.10) - - -def bolt_2022_2023_lateral_testing_ground_active() -> bool: - return testing_ground.use(BOLT_2022_2023_LATERAL_TESTING_GROUND_ID) - - -def _bolt_2022_2023_sigmoid(x: float) -> float: - return _sigmoid(x) - - -def _bolt_2022_2023_low_speed_factor(v_ego: float) -> float: - return 1.0 / (1.0 + (max(v_ego, 0.0) / BOLT_2022_2023_TRANSITION_SPEED) ** 2) - - -def _bolt_2022_2023_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - return math.tanh((desired_lateral_accel * desired_lateral_jerk) / BOLT_2022_2023_PHASE_SCALE) - - -def _bolt_2022_2023_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def _bolt_2022_2023_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / BOLT_2022_2023_FRICTION_LAT_RISE) - jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / BOLT_2022_2023_FRICTION_JERK_RISE) - return _bolt_2022_2023_low_speed_factor(v_ego) * lat_factor * jerk_factor - - -def get_bolt_2022_2023_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - gain = _bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_FF_GAIN_LEFT, BOLT_2022_2023_FF_GAIN_RIGHT) - abs_lateral_accel = abs(desired_lateral_accel) - onset = _bolt_2022_2023_sigmoid((abs_lateral_accel - BOLT_2022_2023_FF_ONSET) / BOLT_2022_2023_FF_ONSET_WIDTH) - cutoff = _bolt_2022_2023_sigmoid((BOLT_2022_2023_FF_CUTOFF - abs_lateral_accel) / BOLT_2022_2023_FF_CUTOFF_WIDTH) - extra_scale = gain * onset * cutoff - speed_weight = _bolt_2022_2023_sigmoid((v_ego - BOLT_2022_2023_CENTER_TAPER_SPEED) / BOLT_2022_2023_CENTER_TAPER_SPEED_WIDTH) - center_weight = _bolt_2022_2023_sigmoid((BOLT_2022_2023_CENTER_TAPER_LAT - abs_lateral_accel) / BOLT_2022_2023_CENTER_TAPER_LAT_WIDTH) - center_taper = 1.0 - (BOLT_2022_2023_CENTER_TAPER_MAX * speed_weight * center_weight) - low_speed_factor = _bolt_2022_2023_low_speed_factor(v_ego) - transition_envelope = _bolt_2022_2023_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - turn_in_boost = 1.0 + (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_BOOST_LEFT, BOLT_2022_2023_TURN_IN_BOOST_RIGHT) * - turn_in_weight * low_speed_factor) - unwind_envelope = (0.25 + 0.75 * low_speed_factor) * (1.0 + 0.45 * transition_envelope) - unwind_taper = 1.0 - (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_TAPER_LEFT, BOLT_2022_2023_UNWIND_TAPER_RIGHT) * - unwind_weight * unwind_envelope) - return 1.0 + (extra_scale * center_taper * turn_in_boost * max(unwind_taper, 0.0)) - - -def get_bolt_2022_2023_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: - base_threshold = get_friction_threshold(v_ego) - transition_envelope = _bolt_2022_2023_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - threshold_scale = 1.0 - (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_LEFT, BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * - transition_envelope * turn_in_weight) - threshold_scale += (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT, BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_RIGHT) * - transition_envelope * unwind_weight) - return base_threshold * min(max(threshold_scale, 0.84), 1.14) - - -def get_bolt_2022_2023_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - transition_envelope = _bolt_2022_2023_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - friction_scale = BOLT_2022_2023_FRICTION_MULT - friction_scale += (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_FRICTION_BOOST_LEFT, BOLT_2022_2023_TURN_IN_FRICTION_BOOST_RIGHT) * - transition_envelope * turn_in_weight) - friction_scale -= (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_LEFT, BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_RIGHT) * - transition_envelope * unwind_weight) - return min(max(friction_scale, 0.92), 1.22) - - -def volt_standard_lateral_testing_ground_active() -> bool: - return testing_ground.use(VOLT_STANDARD_LATERAL_TESTING_GROUND_ID) - - -def _volt_standard_sigmoid(x: float) -> float: - return _sigmoid(x) - - -def _volt_standard_low_speed_factor(v_ego: float) -> float: - return 1.0 / (1.0 + (max(v_ego, 0.0) / VOLT_STANDARD_TRANSITION_SPEED) ** 2) - - -def _volt_standard_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - return math.tanh((desired_lateral_accel * desired_lateral_jerk) / VOLT_STANDARD_PHASE_SCALE) - - -def _volt_standard_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def _volt_standard_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / VOLT_STANDARD_FRICTION_LAT_RISE) - jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / VOLT_STANDARD_FRICTION_JERK_RISE) - return _volt_standard_low_speed_factor(v_ego) * lat_factor * jerk_factor - - -def get_volt_standard_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - gain = _volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_FF_GAIN_LEFT, VOLT_STANDARD_FF_GAIN_RIGHT) - abs_lateral_accel = abs(desired_lateral_accel) - onset = _volt_standard_sigmoid((abs_lateral_accel - VOLT_STANDARD_FF_ONSET) / VOLT_STANDARD_FF_ONSET_WIDTH) - cutoff = _volt_standard_sigmoid((VOLT_STANDARD_FF_CUTOFF - abs_lateral_accel) / VOLT_STANDARD_FF_CUTOFF_WIDTH) - extra_scale = gain * onset * cutoff - phase = _volt_standard_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - low_speed_factor = _volt_standard_low_speed_factor(v_ego) - turn_in_boost = 1.0 + (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_TURN_IN_BOOST_LEFT, VOLT_STANDARD_TURN_IN_BOOST_RIGHT) * - turn_in_weight * low_speed_factor) - unwind_taper = 1.0 - (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_UNWIND_TAPER_LEFT, VOLT_STANDARD_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.30 + 0.70 * low_speed_factor)) - return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) - - -def get_volt_standard_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: - base_threshold = get_friction_threshold(v_ego) - transition_envelope = _volt_standard_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _volt_standard_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - threshold_scale = 1.0 - (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_LEFT, VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * - transition_envelope * turn_in_weight) - threshold_scale += (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_LEFT, VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_RIGHT) * - transition_envelope * unwind_weight) - return base_threshold * min(max(threshold_scale, 0.84), 1.12) - - -def get_volt_standard_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - transition_envelope = _volt_standard_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _volt_standard_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - friction_scale = VOLT_STANDARD_FRICTION_MULT - friction_scale += (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_TURN_IN_FRICTION_BOOST_LEFT, VOLT_STANDARD_TURN_IN_FRICTION_BOOST_RIGHT) * - transition_envelope * turn_in_weight) - friction_scale -= (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_LEFT, VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_RIGHT) * - transition_envelope * unwind_weight) - return min(max(friction_scale, 0.90), 1.14) - - -def get_volt_standard_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: - speed_weight = _volt_standard_sigmoid((v_ego - VOLT_STANDARD_CENTER_TAPER_SPEED) / VOLT_STANDARD_CENTER_TAPER_SPEED_WIDTH) - center_weight = _volt_standard_sigmoid((VOLT_STANDARD_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / VOLT_STANDARD_CENTER_TAPER_LAT_WIDTH) - reduction = VOLT_STANDARD_CENTER_TAPER_MAX * speed_weight * center_weight - return 1.0 - reduction - - -def get_silverado_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: - speed_weight = _sigmoid((v_ego - SILVERADO_CENTER_TAPER_SPEED) / SILVERADO_CENTER_TAPER_SPEED_WIDTH) - center_weight = _sigmoid((SILVERADO_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / SILVERADO_CENTER_TAPER_LAT_WIDTH) - reduction = SILVERADO_CENTER_TAPER_MAX * speed_weight * center_weight - return 1.0 - reduction - - -def _sonata_hybrid_sigmoid(x: float) -> float: - return _sigmoid(x) - - -def _sonata_hybrid_low_speed_factor(v_ego: float) -> float: - return 1.0 / (1.0 + (max(v_ego, 0.0) / SONATA_HYBRID_TRANSITION_SPEED) ** 2) - - -def _sonata_hybrid_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - return math.tanh((desired_lateral_accel * desired_lateral_jerk) / SONATA_HYBRID_PHASE_SCALE) - - -def _sonata_hybrid_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def get_sonata_hybrid_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - abs_lateral_accel = abs(desired_lateral_accel) - onset = _sonata_hybrid_sigmoid((abs_lateral_accel - SONATA_HYBRID_FF_ONSET) / SONATA_HYBRID_FF_ONSET_WIDTH) - cutoff = _sonata_hybrid_sigmoid((SONATA_HYBRID_FF_CUTOFF - abs_lateral_accel) / SONATA_HYBRID_FF_CUTOFF_WIDTH) - base_reduction = _sonata_hybrid_side_value(desired_lateral_accel, - SONATA_HYBRID_FF_REDUCTION_LEFT, - SONATA_HYBRID_FF_REDUCTION_RIGHT) * onset * cutoff - phase = _sonata_hybrid_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - low_speed_factor = _sonata_hybrid_low_speed_factor(v_ego) - turn_in_boost = 1.0 + (_sonata_hybrid_side_value(desired_lateral_accel, - SONATA_HYBRID_TURN_IN_BOOST_LEFT, - SONATA_HYBRID_TURN_IN_BOOST_RIGHT) * - turn_in_weight * low_speed_factor) - unwind_taper = 1.0 - (_sonata_hybrid_side_value(desired_lateral_accel, - SONATA_HYBRID_UNWIND_TAPER_LEFT, - SONATA_HYBRID_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.35 + 0.65 * low_speed_factor)) - return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0) - - -def get_sonata_hybrid_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: - speed_weight = _sonata_hybrid_sigmoid((v_ego - SONATA_HYBRID_CENTER_TAPER_SPEED) / SONATA_HYBRID_CENTER_TAPER_SPEED_WIDTH) - center_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / SONATA_HYBRID_CENTER_TAPER_LAT_WIDTH) - reduction = SONATA_HYBRID_CENTER_TAPER_MAX * speed_weight * center_weight - low_speed_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) / - SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH) - low_speed_center_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / - SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT_WIDTH) - reduction += SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_MAX * low_speed_weight * low_speed_center_weight - return 1.0 - reduction - - -def _sonata_sigmoid(x: float) -> float: - return _sigmoid(x) - - -def _sonata_low_speed_factor(v_ego: float) -> float: - return 1.0 / (1.0 + (max(v_ego, 0.0) / SONATA_TRANSITION_SPEED) ** 2) - - -def _sonata_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - return math.tanh((desired_lateral_accel * desired_lateral_jerk) / SONATA_PHASE_SCALE) - - -def _sonata_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def get_sonata_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - abs_lateral_accel = abs(desired_lateral_accel) - onset = _sonata_sigmoid((abs_lateral_accel - SONATA_FF_ONSET) / SONATA_FF_ONSET_WIDTH) - cutoff = _sonata_sigmoid((SONATA_FF_CUTOFF - abs_lateral_accel) / SONATA_FF_CUTOFF_WIDTH) - base_reduction = _sonata_side_value(desired_lateral_accel, SONATA_FF_REDUCTION_LEFT, SONATA_FF_REDUCTION_RIGHT) * onset * cutoff - phase = _sonata_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - low_speed_factor = _sonata_low_speed_factor(v_ego) - turn_in_boost = 1.0 + (_sonata_side_value(desired_lateral_accel, SONATA_TURN_IN_BOOST_LEFT, SONATA_TURN_IN_BOOST_RIGHT) * - turn_in_weight * low_speed_factor) - unwind_taper = 1.0 - (_sonata_side_value(desired_lateral_accel, SONATA_UNWIND_TAPER_LEFT, SONATA_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.35 + 0.65 * low_speed_factor)) - return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0) - - -def get_sonata_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: - speed_weight = _sonata_sigmoid((v_ego - SONATA_CENTER_TAPER_SPEED) / SONATA_CENTER_TAPER_SPEED_WIDTH) - center_weight = _sonata_sigmoid((SONATA_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / SONATA_CENTER_TAPER_LAT_WIDTH) - reduction = SONATA_CENTER_TAPER_MAX * speed_weight * center_weight - low_speed_weight = _sonata_sigmoid((SONATA_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) / - SONATA_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH) - low_speed_center_weight = _sonata_sigmoid((SONATA_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / - SONATA_LOW_SPEED_CENTER_TAPER_LAT_WIDTH) - reduction += SONATA_LOW_SPEED_CENTER_TAPER_MAX * low_speed_weight * low_speed_center_weight - return 1.0 - reduction - - -def _elantra_non_scc_sigmoid(x: float) -> float: - return _sigmoid(x) - - -def _elantra_non_scc_low_speed_factor(v_ego: float) -> float: - return 1.0 / (1.0 + (max(v_ego, 0.0) / ELANTRA_NON_SCC_TRANSITION_SPEED) ** 2) - - -def _elantra_non_scc_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - return math.tanh((desired_lateral_accel * desired_lateral_jerk) / ELANTRA_NON_SCC_PHASE_SCALE) - - -def _elantra_non_scc_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def get_elantra_non_scc_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - abs_lateral_accel = abs(desired_lateral_accel) - onset = _elantra_non_scc_sigmoid((abs_lateral_accel - ELANTRA_NON_SCC_FF_ONSET) / ELANTRA_NON_SCC_FF_ONSET_WIDTH) - cutoff = _elantra_non_scc_sigmoid((ELANTRA_NON_SCC_FF_CUTOFF - abs_lateral_accel) / ELANTRA_NON_SCC_FF_CUTOFF_WIDTH) - low_speed_factor = _elantra_non_scc_low_speed_factor(v_ego) - envelope = onset * cutoff * low_speed_factor - base_scale = 1.0 - (_elantra_non_scc_side_value(desired_lateral_accel, - ELANTRA_NON_SCC_FF_ADJUST_LEFT, - ELANTRA_NON_SCC_FF_ADJUST_RIGHT) * envelope) - phase = _elantra_non_scc_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - turn_in_boost = 1.0 + (_elantra_non_scc_side_value(desired_lateral_accel, - ELANTRA_NON_SCC_TURN_IN_BOOST_LEFT, - ELANTRA_NON_SCC_TURN_IN_BOOST_RIGHT) * - turn_in_weight * (0.35 + 0.65 * low_speed_factor)) - unwind_taper = 1.0 - (_elantra_non_scc_side_value(desired_lateral_accel, - ELANTRA_NON_SCC_UNWIND_TAPER_LEFT, - ELANTRA_NON_SCC_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.35 + 0.65 * low_speed_factor)) - return base_scale * turn_in_boost * max(unwind_taper, 0.0) - - -def _kia_xceed_sigmoid(x: float) -> float: - return _sigmoid(x) - - -def _kia_xceed_low_speed_factor(v_ego: float) -> float: - return 1.0 / (1.0 + (max(v_ego, 0.0) / KIA_XCEED_TRANSITION_SPEED) ** 2) - - -def _kia_xceed_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - return math.tanh((desired_lateral_accel * desired_lateral_jerk) / KIA_XCEED_PHASE_SCALE) - - -def _kia_xceed_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def get_kia_xceed_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - abs_lateral_accel = abs(desired_lateral_accel) - onset = _kia_xceed_sigmoid((abs_lateral_accel - KIA_XCEED_FF_ONSET) / KIA_XCEED_FF_ONSET_WIDTH) - cutoff = _kia_xceed_sigmoid((KIA_XCEED_FF_CUTOFF - abs_lateral_accel) / KIA_XCEED_FF_CUTOFF_WIDTH) - base_reduction = _kia_xceed_side_value(desired_lateral_accel, - KIA_XCEED_FF_REDUCTION_LEFT, - KIA_XCEED_FF_REDUCTION_RIGHT) * onset * cutoff - phase = _kia_xceed_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - low_speed_factor = _kia_xceed_low_speed_factor(v_ego) - turn_in_boost = 1.0 + (_kia_xceed_side_value(desired_lateral_accel, - KIA_XCEED_TURN_IN_BOOST_LEFT, - KIA_XCEED_TURN_IN_BOOST_RIGHT) * - turn_in_weight * low_speed_factor) - unwind_taper = 1.0 - (_kia_xceed_side_value(desired_lateral_accel, - KIA_XCEED_UNWIND_TAPER_LEFT, - KIA_XCEED_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.35 + 0.65 * low_speed_factor)) - return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0) - - -def get_kia_xceed_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: - speed_weight = _kia_xceed_sigmoid((v_ego - KIA_XCEED_CENTER_TAPER_SPEED) / KIA_XCEED_CENTER_TAPER_SPEED_WIDTH) - center_weight = _kia_xceed_sigmoid((KIA_XCEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_XCEED_CENTER_TAPER_LAT_WIDTH) - reduction = KIA_XCEED_CENTER_TAPER_MAX * speed_weight * center_weight - return 1.0 - reduction - - -def get_kia_niro_phev_2022_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: - speed_weight = _sigmoid((v_ego - KIA_NIRO_PHEV_2022_CENTER_TAPER_SPEED) / KIA_NIRO_PHEV_2022_CENTER_TAPER_SPEED_WIDTH) - center_weight = _sigmoid((KIA_NIRO_PHEV_2022_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_NIRO_PHEV_2022_CENTER_TAPER_LAT_WIDTH) - reduction = KIA_NIRO_PHEV_2022_CENTER_TAPER_MAX * speed_weight * center_weight - return 1.0 - reduction - - -def get_kia_niro_phev_2022_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: - base_threshold = get_friction_threshold(v_ego) - speed_weight = _sigmoid((v_ego - KIA_NIRO_PHEV_2022_FRICTION_SPEED) / KIA_NIRO_PHEV_2022_FRICTION_SPEED_WIDTH) - center_weight = _sigmoid((KIA_NIRO_PHEV_2022_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) / KIA_NIRO_PHEV_2022_FRICTION_CENTER_LAT_WIDTH) - calm_jerk_weight = _sigmoid((KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK - abs(desired_lateral_jerk)) / KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK_WIDTH) - threshold_scale = 1.0 + (KIA_NIRO_PHEV_2022_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight * calm_jerk_weight) - return base_threshold * min(max(threshold_scale, 1.0), 1.18) - - -def _kia_forte_sigmoid(x: float) -> float: - return _sigmoid(x) - - -def _kia_forte_low_speed_factor(v_ego: float) -> float: - return 1.0 / (1.0 + (max(v_ego, 0.0) / KIA_FORTE_TRANSITION_SPEED) ** 2) - - -def _kia_forte_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - return math.tanh((desired_lateral_accel * desired_lateral_jerk) / KIA_FORTE_PHASE_SCALE) - - -def _kia_forte_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def get_kia_forte_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - abs_lateral_accel = abs(desired_lateral_accel) - onset = _kia_forte_sigmoid((abs_lateral_accel - KIA_FORTE_FF_ONSET) / KIA_FORTE_FF_ONSET_WIDTH) - cutoff = _kia_forte_sigmoid((KIA_FORTE_FF_CUTOFF - abs_lateral_accel) / KIA_FORTE_FF_CUTOFF_WIDTH) - base_reduction = _kia_forte_side_value(desired_lateral_accel, KIA_FORTE_FF_REDUCTION_LEFT, KIA_FORTE_FF_REDUCTION_RIGHT) * onset * cutoff - phase = _kia_forte_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - low_speed_factor = _kia_forte_low_speed_factor(v_ego) - turn_in_boost = 1.0 + (_kia_forte_side_value(desired_lateral_accel, KIA_FORTE_TURN_IN_BOOST_LEFT, KIA_FORTE_TURN_IN_BOOST_RIGHT) * - turn_in_weight * (0.35 + 0.65 * low_speed_factor)) - unwind_taper = 1.0 - (_kia_forte_side_value(desired_lateral_accel, KIA_FORTE_UNWIND_TAPER_LEFT, KIA_FORTE_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.35 + 0.65 * low_speed_factor)) - crawl_turn_in_scale = 0.0 - if desired_lateral_accel * desired_lateral_jerk > 0.0: - crawl_speed_weight = _kia_forte_sigmoid((KIA_FORTE_CRAWL_TURN_IN_FF_SPEED - max(v_ego, 0.0)) / - KIA_FORTE_CRAWL_TURN_IN_FF_SPEED_WIDTH) - crawl_lat_weight = _kia_forte_sigmoid((abs_lateral_accel - KIA_FORTE_CRAWL_TURN_IN_FF_LAT) / - KIA_FORTE_CRAWL_TURN_IN_FF_LAT_WIDTH) - crawl_turn_in_scale = _kia_forte_side_value(desired_lateral_accel, KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_LEFT, - KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_RIGHT) * crawl_speed_weight * crawl_lat_weight - return ((1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)) + crawl_turn_in_scale - - -def get_kia_forte_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: - speed_weight = _kia_forte_sigmoid((v_ego - KIA_FORTE_CENTER_TAPER_SPEED) / KIA_FORTE_CENTER_TAPER_SPEED_WIDTH) - center_weight = _kia_forte_sigmoid((KIA_FORTE_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_FORTE_CENTER_TAPER_LAT_WIDTH) - reduction = KIA_FORTE_CENTER_TAPER_MAX * speed_weight * center_weight - return 1.0 - reduction - - -def get_kia_forte_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: - base_threshold = get_friction_threshold(v_ego) - speed_weight = _kia_forte_sigmoid((v_ego - KIA_FORTE_FRICTION_SPEED) / KIA_FORTE_FRICTION_SPEED_WIDTH) - center_weight = _kia_forte_sigmoid((KIA_FORTE_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) / KIA_FORTE_FRICTION_CENTER_LAT_WIDTH) - calm_jerk_weight = _kia_forte_sigmoid((KIA_FORTE_FRICTION_CALM_JERK - abs(desired_lateral_jerk)) / KIA_FORTE_FRICTION_CALM_JERK_WIDTH) - threshold_scale = 1.0 + (KIA_FORTE_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight * calm_jerk_weight) - return base_threshold * min(max(threshold_scale, 1.0), 1.18) - - -def _palisade_sigmoid(x: float) -> float: - return _sigmoid(x) - - -def _palisade_low_speed_factor(v_ego: float) -> float: - return 1.0 / (1.0 + (max(v_ego, 0.0) / PALISADE_TRANSITION_SPEED) ** 2) - - -def _palisade_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - return math.tanh((desired_lateral_accel * desired_lateral_jerk) / PALISADE_PHASE_SCALE) - - -def _palisade_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def _palisade_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / PALISADE_FRICTION_LAT_RISE) - jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / PALISADE_FRICTION_JERK_RISE) - return _palisade_low_speed_factor(v_ego) * lat_factor * jerk_factor - - -def get_palisade_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - gain = _palisade_side_value(desired_lateral_accel, PALISADE_FF_GAIN_LEFT, PALISADE_FF_GAIN_RIGHT) - abs_lateral_accel = abs(desired_lateral_accel) - onset = _palisade_sigmoid((abs_lateral_accel - PALISADE_FF_ONSET) / PALISADE_FF_ONSET_WIDTH) - cutoff = _palisade_sigmoid((PALISADE_FF_CUTOFF - abs_lateral_accel) / PALISADE_FF_CUTOFF_WIDTH) - extra_scale = gain * onset * cutoff - phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - low_speed_factor = _palisade_low_speed_factor(v_ego) - turn_in_boost = 1.0 + (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_BOOST_LEFT, PALISADE_TURN_IN_BOOST_RIGHT) * - turn_in_weight * (0.35 + 0.65 * low_speed_factor)) - unwind_taper = 1.0 - (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_TAPER_LEFT, PALISADE_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.35 + 0.65 * low_speed_factor)) - return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) - - -def get_palisade_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: - base_threshold = get_friction_threshold(v_ego) - transition_envelope = _palisade_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - threshold_scale = 1.0 - (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_THRESHOLD_REDUCTION_LEFT, PALISADE_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * - transition_envelope * turn_in_weight) - threshold_scale += (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_THRESHOLD_INCREASE_LEFT, PALISADE_UNWIND_THRESHOLD_INCREASE_RIGHT) * - transition_envelope * unwind_weight) - return base_threshold * min(max(threshold_scale, 0.84), 1.14) - - -def get_palisade_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - transition_envelope = _palisade_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - friction_scale = PALISADE_FRICTION_MULT - friction_scale += (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_FRICTION_BOOST_LEFT, PALISADE_TURN_IN_FRICTION_BOOST_RIGHT) * - transition_envelope * turn_in_weight) - friction_scale -= (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_FRICTION_REDUCTION_LEFT, PALISADE_UNWIND_FRICTION_REDUCTION_RIGHT) * - transition_envelope * unwind_weight) - return min(max(friction_scale, 0.92), 1.12) - - -def genesis_g90_lateral_testing_ground_active() -> bool: - return testing_ground.use(GENESIS_G90_LATERAL_TESTING_GROUND_ID) - - -def _genesis_g90_sigmoid(x: float) -> float: - return _sigmoid(x) - - -def _genesis_g90_low_speed_factor(v_ego: float) -> float: - return 1.0 / (1.0 + (max(v_ego, 0.0) / GENESIS_G90_TRANSITION_SPEED) ** 2) - - -def _genesis_g90_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - return math.tanh((desired_lateral_accel * desired_lateral_jerk) / GENESIS_G90_PHASE_SCALE) - - -def _genesis_g90_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def _genesis_g90_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / GENESIS_G90_FRICTION_LAT_RISE) - jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / GENESIS_G90_FRICTION_JERK_RISE) - return _genesis_g90_low_speed_factor(v_ego) * lat_factor * jerk_factor - - -def get_genesis_g90_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - gain = _genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_FF_GAIN_LEFT, GENESIS_G90_FF_GAIN_RIGHT) - abs_lateral_accel = abs(desired_lateral_accel) - onset = _genesis_g90_sigmoid((abs_lateral_accel - GENESIS_G90_FF_ONSET) / GENESIS_G90_FF_ONSET_WIDTH) - cutoff = _genesis_g90_sigmoid((GENESIS_G90_FF_CUTOFF - abs_lateral_accel) / GENESIS_G90_FF_CUTOFF_WIDTH) - extra_scale = gain * onset * cutoff - phase = _genesis_g90_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - low_speed_factor = _genesis_g90_low_speed_factor(v_ego) - turn_in_boost = 1.0 + (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_TURN_IN_BOOST_LEFT, GENESIS_G90_TURN_IN_BOOST_RIGHT) * - turn_in_weight * (0.35 + 0.65 * low_speed_factor)) - unwind_taper = 1.0 - (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_UNWIND_TAPER_LEFT, GENESIS_G90_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.30 + 0.70 * low_speed_factor)) - return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) - - -def get_genesis_g90_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: - base_threshold = get_friction_threshold(v_ego) - transition_envelope = _genesis_g90_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _genesis_g90_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - threshold_scale = 1.0 - (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_TURN_IN_THRESHOLD_REDUCTION_LEFT, GENESIS_G90_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * - transition_envelope * turn_in_weight) - threshold_scale += (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_UNWIND_THRESHOLD_INCREASE_LEFT, GENESIS_G90_UNWIND_THRESHOLD_INCREASE_RIGHT) * - transition_envelope * unwind_weight) - return base_threshold * min(max(threshold_scale, 0.86), 1.12) - - -def get_genesis_g90_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - transition_envelope = _genesis_g90_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _genesis_g90_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - friction_scale = GENESIS_G90_FRICTION_MULT - friction_scale += (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_TURN_IN_FRICTION_BOOST_LEFT, GENESIS_G90_TURN_IN_FRICTION_BOOST_RIGHT) * - transition_envelope * turn_in_weight) - friction_scale -= (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_UNWIND_FRICTION_REDUCTION_LEFT, GENESIS_G90_UNWIND_FRICTION_REDUCTION_RIGHT) * - transition_envelope * unwind_weight) - return min(max(friction_scale, 0.92), 1.12) - - -def _ioniq_5_sigmoid(x: float) -> float: - return _sigmoid(x) - - -def _ioniq_5_low_speed_factor(v_ego: float) -> float: - return 1.0 / (1.0 + (max(v_ego, 0.0) / IONIQ_5_TRANSITION_SPEED) ** 2) - - -def _ioniq_5_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - return math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_5_PHASE_SCALE) - - -def _ioniq_5_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def _ioniq_5_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - abs_lateral_accel = abs(desired_lateral_accel) - onset = _ioniq_5_sigmoid((abs_lateral_accel - IONIQ_5_FF_ONSET) / IONIQ_5_FF_ONSET_WIDTH) - cutoff = _ioniq_5_sigmoid((IONIQ_5_FF_CUTOFF - abs_lateral_accel) / IONIQ_5_FF_CUTOFF_WIDTH) - return onset * cutoff * _ioniq_5_low_speed_factor(v_ego) - - -def get_ioniq_5_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - envelope = _ioniq_5_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _ioniq_5_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - low_speed_factor = _ioniq_5_low_speed_factor(v_ego) - - base_reduction = _ioniq_5_side_value(desired_lateral_accel, IONIQ_5_FF_REDUCTION_LEFT, IONIQ_5_FF_REDUCTION_RIGHT) * envelope - turn_in_boost = 1.0 + (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_TURN_IN_BOOST_LEFT, IONIQ_5_TURN_IN_BOOST_RIGHT) * - turn_in_weight * (0.35 + 0.65 * low_speed_factor)) - unwind_taper = 1.0 - (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_UNWIND_TAPER_LEFT, IONIQ_5_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.35 + 0.65 * low_speed_factor)) - return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0) - - -def get_ioniq_5_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: - base_threshold = get_friction_threshold(v_ego) - envelope = _ioniq_5_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _ioniq_5_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - - threshold_scale = 1.0 - (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_LEFT, IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * - envelope * turn_in_weight) - threshold_scale += (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_UNWIND_THRESHOLD_INCREASE_LEFT, IONIQ_5_UNWIND_THRESHOLD_INCREASE_RIGHT) * - envelope * unwind_weight) - return base_threshold * min(max(threshold_scale, 0.86), 1.18) - - -def get_ioniq_5_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - if desired_lateral_accel == 0.0 or desired_lateral_jerk == 0.0: - return 1.0 - - envelope = _ioniq_5_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _ioniq_5_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - - friction_scale = 1.0 - friction_scale += (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_TURN_IN_FRICTION_BOOST_LEFT, IONIQ_5_TURN_IN_FRICTION_BOOST_RIGHT) * - envelope * turn_in_weight) - friction_scale -= (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT, IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT) * - envelope * unwind_weight) - return min(max(friction_scale, 0.86), 1.04) - - -def get_ioniq_5_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: - speed_weight = _ioniq_5_sigmoid((v_ego - IONIQ_5_CENTER_TAPER_SPEED) / IONIQ_5_CENTER_TAPER_SPEED_WIDTH) - center_weight = _ioniq_5_sigmoid((IONIQ_5_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_5_CENTER_TAPER_LAT_WIDTH) - reduction = IONIQ_5_CENTER_TAPER_MAX * speed_weight * center_weight - return 1.0 - reduction - - -def _ioniq_ev_old_sigmoid(x: float) -> float: - return _sigmoid(x) - - -def _ioniq_ev_old_low_speed_factor(v_ego: float) -> float: - return 1.0 / (1.0 + (max(v_ego, 0.0) / IONIQ_EV_OLD_TRANSITION_SPEED) ** 2) - - -def _ioniq_ev_old_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - return math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_EV_OLD_PHASE_SCALE) - - -def _ioniq_ev_old_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def get_ioniq_ev_old_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - abs_lateral_accel = abs(desired_lateral_accel) - onset = _ioniq_ev_old_sigmoid((abs_lateral_accel - IONIQ_EV_OLD_FF_ONSET) / IONIQ_EV_OLD_FF_ONSET_WIDTH) - cutoff = _ioniq_ev_old_sigmoid((IONIQ_EV_OLD_FF_CUTOFF - abs_lateral_accel) / IONIQ_EV_OLD_FF_CUTOFF_WIDTH) - base_reduction = _ioniq_ev_old_side_value(desired_lateral_accel, IONIQ_EV_OLD_FF_REDUCTION_LEFT, IONIQ_EV_OLD_FF_REDUCTION_RIGHT) * onset * cutoff - phase = _ioniq_ev_old_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - low_speed_factor = _ioniq_ev_old_low_speed_factor(v_ego) - turn_in_boost = 1.0 + (_ioniq_ev_old_side_value(desired_lateral_accel, IONIQ_EV_OLD_TURN_IN_BOOST_LEFT, IONIQ_EV_OLD_TURN_IN_BOOST_RIGHT) * - turn_in_weight * (0.35 + 0.65 * low_speed_factor)) - unwind_taper = 1.0 - (_ioniq_ev_old_side_value(desired_lateral_accel, IONIQ_EV_OLD_UNWIND_TAPER_LEFT, IONIQ_EV_OLD_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.35 + 0.65 * low_speed_factor)) - return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0) - - -def get_ioniq_ev_old_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: - speed_weight = _ioniq_ev_old_sigmoid((v_ego - IONIQ_EV_OLD_CENTER_TAPER_SPEED) / IONIQ_EV_OLD_CENTER_TAPER_SPEED_WIDTH) - center_weight = _ioniq_ev_old_sigmoid((IONIQ_EV_OLD_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_EV_OLD_CENTER_TAPER_LAT_WIDTH) - reduction = IONIQ_EV_OLD_CENTER_TAPER_MAX * speed_weight * center_weight - return 1.0 - reduction - - -def _ioniq_6_sigmoid(x: float) -> float: - return _sigmoid(x) - - -def _ioniq_6_low_speed_factor(v_ego: float) -> float: - return 1.0 / (1.0 + (max(v_ego, 0.0) / IONIQ_6_TRANSITION_SPEED) ** 2) - - -def _ioniq_6_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - return math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_6_PHASE_SCALE) - - -def _ioniq_6_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def _ioniq_6_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / IONIQ_6_FRICTION_LAT_RISE) - jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / IONIQ_6_FRICTION_JERK_RISE) - return _ioniq_6_low_speed_factor(v_ego) * lat_factor * jerk_factor - - -def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - gain = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_FF_GAIN_LEFT, IONIQ_6_FF_GAIN_RIGHT) - abs_lateral_accel = abs(desired_lateral_accel) - onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_FF_ONSET) / IONIQ_6_FF_ONSET_WIDTH) - cutoff = _ioniq_6_sigmoid((IONIQ_6_FF_CUTOFF - abs_lateral_accel) / IONIQ_6_FF_CUTOFF_WIDTH) - extra_scale = gain * onset * cutoff - phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - low_speed_factor = _ioniq_6_low_speed_factor(v_ego) - turn_in_boost = 1.0 + (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_TURN_IN_BOOST_LEFT, IONIQ_6_TURN_IN_BOOST_RIGHT) * - turn_in_weight * low_speed_factor) - unwind_taper = 1.0 - (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_UNWIND_TAPER_LEFT, IONIQ_6_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.30 + 0.70 * low_speed_factor)) - crawl_turn_in_scale = 0.0 - if desired_lateral_accel * desired_lateral_jerk > 0.0: - crawl_speed_weight = _ioniq_6_sigmoid((IONIQ_6_CRAWL_TURN_IN_FF_SPEED - max(v_ego, 0.0)) / - IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH) - crawl_lat_weight = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_CRAWL_TURN_IN_FF_LAT) / - IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH) - crawl_turn_in_scale = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT, - IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT) * crawl_speed_weight * crawl_lat_weight - high_speed_right_turn_in_scale = 0.0 - if desired_lateral_accel < 0.0 and desired_lateral_accel * desired_lateral_jerk > 0.0: - high_speed_weight = _ioniq_6_sigmoid((max(v_ego, 0.0) - IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED) / - IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH) - high_speed_lat_onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_START) / - IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH) - high_speed_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END - abs_lateral_accel) / - IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH) - high_speed_right_turn_in_scale = IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST * high_speed_weight * high_speed_lat_onset * high_speed_lat_cutoff - return (1.0 + crawl_turn_in_scale + high_speed_right_turn_in_scale + - (extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego) - - -def get_ioniq_6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: - base_threshold = max(get_friction_threshold(v_ego), IONIQ_6_BASE_FRICTION_THRESHOLD) - transition_envelope = _ioniq_6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - threshold_scale = 1.0 - (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT, IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * - transition_envelope * turn_in_weight) - threshold_scale += (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT, IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT) * - transition_envelope * unwind_weight) - return base_threshold * min(max(threshold_scale, 0.82), 1.18) - - -def get_ioniq_6_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - transition_envelope = _ioniq_6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - friction_scale = IONIQ_6_FRICTION_MULT - friction_scale += (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT, IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT) * - transition_envelope * turn_in_weight) - friction_scale -= (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT, IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT) * - transition_envelope * unwind_weight) - return min(max(friction_scale, 0.82), 1.08) - - -def get_ioniq_6_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: - speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_CENTER_TAPER_SPEED) / IONIQ_6_CENTER_TAPER_SPEED_WIDTH) - center_weight = _ioniq_6_sigmoid((IONIQ_6_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_6_CENTER_TAPER_LAT_WIDTH) - high_speed_reduction = IONIQ_6_CENTER_TAPER_MAX * speed_weight * center_weight - - highway_speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED) / IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED_WIDTH) - highway_center_weight = _ioniq_6_sigmoid((IONIQ_6_HIGHWAY_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / - IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH) - highway_center_reduction = IONIQ_6_HIGHWAY_CENTER_TAPER_MAX * highway_speed_weight * highway_center_weight - - low_mid_onset = _ioniq_6_sigmoid((v_ego - IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MIN) / IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH) - low_mid_cutoff = _ioniq_6_sigmoid((IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MAX - v_ego) / IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH) - low_mid_speed_weight = low_mid_onset * low_mid_cutoff - low_mid_center_weight = _ioniq_6_sigmoid((IONIQ_6_LOW_MID_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / - IONIQ_6_LOW_MID_CENTER_TAPER_LAT_WIDTH) - low_mid_reduction = IONIQ_6_LOW_MID_CENTER_TAPER_MAX * low_mid_speed_weight * low_mid_center_weight - - return 1.0 - min(high_speed_reduction + highway_center_reduction + low_mid_reduction, 0.12) - - -def get_ioniq_6_directional_taper_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float | None = None) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - abs_lateral_accel = abs(desired_lateral_accel) - onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_DIRECTIONAL_TAPER_LAT_START) / IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH) - cutoff = _ioniq_6_sigmoid((IONIQ_6_DIRECTIONAL_TAPER_LAT_END - abs_lateral_accel) / IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH) - band_weight = onset * cutoff - heavy_band_weight = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START) / IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH) - phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk) - unwind_weight = max(-phase, 0.0) * _ioniq_6_sigmoid((abs(desired_lateral_jerk) - IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET) / - IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH) - low_speed_relief_weight = 0.0 - if v_ego is not None: - low_speed_weight = _ioniq_6_sigmoid((IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED - max(v_ego, 0.0)) / - IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH) - tight_turn_weight = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT) / - IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH) - low_speed_relief_weight = IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF * low_speed_weight * tight_turn_weight * (1.0 - unwind_weight) - base_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT, IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT) - unwind_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT) - heavy_base_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT) - heavy_unwind_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT) - base_reduction *= 1.0 - low_speed_relief_weight - heavy_base_reduction *= 1.0 - low_speed_relief_weight - reduction = band_weight * (base_reduction + unwind_reduction * unwind_weight) - reduction += heavy_band_weight * (heavy_base_reduction + heavy_unwind_reduction * unwind_weight) - floor = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT, IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT) - floor -= _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT) * unwind_weight - return max(1.0 - reduction, floor) - - -def get_ioniq_6_output_taper_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_OUTPUT_TAPER_SPEED) / IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH) - center_taper = get_ioniq_6_center_taper_scale(desired_lateral_accel, v_ego) - directional_taper = get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego) - center_scale = 1.0 - ((1.0 - center_taper) * IONIQ_6_OUTPUT_CENTER_TAPER_BLEND * speed_weight) - directional_scale = 1.0 - ((1.0 - directional_taper) * IONIQ_6_OUTPUT_DIRECTIONAL_TAPER_BLEND * speed_weight) - return center_scale * directional_scale - - -def get_ioniq_6_highway_output_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: - speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED) / IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH) - center_weight = _ioniq_6_sigmoid((IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / - IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH) - reduction = IONIQ_6_HIGHWAY_OUTPUT_TAPER_MAX * speed_weight * center_weight - return 1.0 - reduction - - -def get_ioniq_6_highway_transition_output_taper_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED) / IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH) - center_weight = _ioniq_6_sigmoid((IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / - IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT_WIDTH) - jerk_weight = _ioniq_6_sigmoid((abs(desired_lateral_jerk) - IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK) / - IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK_WIDTH) - reduction = IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_MAX * speed_weight * center_weight * jerk_weight - return 1.0 - reduction - - -def get_ioniq_6_low_speed_angle_assist_torque(desired_angle_deg: float, actual_angle_deg: float, - current_output_torque: float, v_ego: float) -> float: - angle_error = desired_angle_deg - actual_angle_deg - if desired_angle_deg * angle_error > 0.0: - speed_weight = _ioniq_6_sigmoid((IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED - max(v_ego, 0.0)) / - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH) - error_weight = _ioniq_6_sigmoid((abs(angle_error) - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR) / - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH) - desired_angle_weight = _ioniq_6_sigmoid((abs(desired_angle_deg) - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE) / - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH) - tracking_ratio = abs(actual_angle_deg) / max(abs(desired_angle_deg), 1e-3) - tracking_taper = _ioniq_6_sigmoid((tracking_ratio - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_START) / - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_WIDTH) - tracking_scale = max(1.0 - tracking_taper, IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_FLOOR) - assist_torque = math.copysign(IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE * speed_weight * error_weight * desired_angle_weight * tracking_scale, -angle_error) - if abs(assist_torque) < 1e-4: - return current_output_torque - - if current_output_torque * assist_torque >= 0.0: - add_scale = float(np.interp(abs(current_output_torque), - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_BP, - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_V)) - return float(np.clip(current_output_torque + (assist_torque * add_scale), -1.0, 1.0)) - - return float(np.clip(current_output_torque + assist_torque, -1.0, 1.0)) - - speed_weight = _ioniq_6_sigmoid((IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED - max(v_ego, 0.0)) / - IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED_WIDTH) - error_weight = _ioniq_6_sigmoid((abs(angle_error) - IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR) / - IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR_WIDTH) - actual_angle_weight = _ioniq_6_sigmoid((abs(actual_angle_deg) - IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE) / - IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE_WIDTH) - assist_torque = math.copysign(IONIQ_6_LOW_SPEED_UNWIND_ASSIST_MAX_TORQUE * speed_weight * error_weight * actual_angle_weight, -angle_error) - if abs(assist_torque) < 1e-4: - return current_output_torque - - if current_output_torque * assist_torque >= 0.0: - assist_torque *= IONIQ_6_LOW_SPEED_UNWIND_ASSIST_BLEND - - return float(np.clip(current_output_torque + assist_torque, -1.0, 1.0)) - - -def kia_ev6_lateral_testing_ground_active() -> bool: - return testing_ground.use(KIA_EV6_LATERAL_TESTING_GROUND_ID, KIA_EV6_LATERAL_TESTING_GROUND_VARIANT) - - -def _kia_ev6_sigmoid(x: float) -> float: - return _sigmoid(x) - - -def _kia_ev6_low_speed_factor(v_ego: float) -> float: - return 1.0 / (1.0 + (max(v_ego, 0.0) / KIA_EV6_TRANSITION_SPEED) ** 2) - - -def _kia_ev6_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - return math.tanh((desired_lateral_accel * desired_lateral_jerk) / KIA_EV6_PHASE_SCALE) - - -def _kia_ev6_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def _kia_ev6_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / KIA_EV6_FRICTION_LAT_RISE) - jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / KIA_EV6_FRICTION_JERK_RISE) - return _kia_ev6_low_speed_factor(v_ego) * lat_factor * jerk_factor - - -def get_kia_ev6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - if desired_lateral_accel == 0.0: - return 1.0 - - gain = _kia_ev6_side_value(desired_lateral_accel, KIA_EV6_FF_GAIN_LEFT, KIA_EV6_FF_GAIN_RIGHT) - abs_lateral_accel = abs(desired_lateral_accel) - onset = _kia_ev6_sigmoid((abs_lateral_accel - KIA_EV6_FF_ONSET) / KIA_EV6_FF_ONSET_WIDTH) - cutoff = _kia_ev6_sigmoid((KIA_EV6_FF_CUTOFF - abs_lateral_accel) / KIA_EV6_FF_CUTOFF_WIDTH) - extra_scale = gain * onset * cutoff - phase = _kia_ev6_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - low_speed_factor = _kia_ev6_low_speed_factor(v_ego) - turn_in_boost = 1.0 + (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_TURN_IN_BOOST_LEFT, KIA_EV6_TURN_IN_BOOST_RIGHT) * - turn_in_weight * (0.35 + 0.65 * low_speed_factor)) - unwind_taper = 1.0 - (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_UNWIND_TAPER_LEFT, KIA_EV6_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.35 + 0.65 * low_speed_factor)) - return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) - - -def get_kia_ev6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: - base_threshold = get_friction_threshold(v_ego) - transition_envelope = _kia_ev6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _kia_ev6_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - threshold_scale = 1.0 - (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_LEFT, KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * - transition_envelope * turn_in_weight) - threshold_scale += (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_UNWIND_THRESHOLD_INCREASE_LEFT, KIA_EV6_UNWIND_THRESHOLD_INCREASE_RIGHT) * - transition_envelope * unwind_weight) - return base_threshold * min(max(threshold_scale, 0.82), 1.16) - - -def get_kia_ev6_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - transition_envelope = _kia_ev6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) - phase = _kia_ev6_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - friction_scale = KIA_EV6_FRICTION_MULT - friction_scale += (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_TURN_IN_FRICTION_BOOST_LEFT, KIA_EV6_TURN_IN_FRICTION_BOOST_RIGHT) * - transition_envelope * turn_in_weight) - friction_scale -= (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_UNWIND_FRICTION_REDUCTION_LEFT, KIA_EV6_UNWIND_FRICTION_REDUCTION_RIGHT) * - transition_envelope * unwind_weight) - return min(max(friction_scale, 0.90), 1.10) - - -def get_kia_ev6_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: - speed_weight = _kia_ev6_sigmoid((v_ego - KIA_EV6_CENTER_TAPER_SPEED) / KIA_EV6_CENTER_TAPER_SPEED_WIDTH) - center_weight = _kia_ev6_sigmoid((KIA_EV6_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_EV6_CENTER_TAPER_LAT_WIDTH) - reduction = KIA_EV6_CENTER_TAPER_MAX * speed_weight * center_weight - return 1.0 - reduction - - -def get_kia_ev6_low_speed_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: - speed_weight = _kia_ev6_sigmoid((KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) / KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH) - center_weight = _kia_ev6_sigmoid((KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT_WIDTH) - reduction = KIA_EV6_LOW_SPEED_CENTER_TAPER_MAX * speed_weight * center_weight - return 1.0 - reduction - - -def volt_plexy_lateral_testing_ground_active() -> bool: - return testing_ground.use(VOLT_PLEXY_LATERAL_TESTING_GROUND_ID) - - -def _volt_plexy_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: - return left_value if desired_lateral_accel >= 0.0 else right_value - - -def get_volt_plexy_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - standard_scale = get_volt_standard_ff_scale(desired_lateral_accel, desired_lateral_jerk, v_ego) - extra_scale = standard_scale - 1.0 - extra_mult = _volt_plexy_side_value(desired_lateral_accel, VOLT_PLEXY_FF_EXTRA_MULT_LEFT, VOLT_PLEXY_FF_EXTRA_MULT_RIGHT) - return 1.0 + (extra_scale * extra_mult) - - -def get_volt_plexy_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: - standard_threshold = get_volt_standard_friction_threshold(v_ego, desired_lateral_accel, desired_lateral_jerk) - threshold_mult = _volt_plexy_side_value(desired_lateral_accel, VOLT_PLEXY_FRICTION_THRESHOLD_MULT_LEFT, VOLT_PLEXY_FRICTION_THRESHOLD_MULT_RIGHT) - return standard_threshold * threshold_mult - - -def get_volt_plexy_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: - standard_scale = get_volt_standard_friction_scale(v_ego, desired_lateral_accel, desired_lateral_jerk) - friction_extra = standard_scale - 1.0 - friction_mult = _volt_plexy_side_value(desired_lateral_accel, VOLT_PLEXY_FRICTION_SCALE_MULT_LEFT, VOLT_PLEXY_FRICTION_SCALE_MULT_RIGHT) - return 1.0 + (friction_extra * friction_mult) - - -def get_volt_plexy_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: - standard_scale = get_volt_standard_center_taper_scale(desired_lateral_accel, v_ego) - standard_reduction = 1.0 - standard_scale - return 1.0 - (standard_reduction * VOLT_PLEXY_CENTER_TAPER_REDUCTION_MULT) - class LatControlTorque(LatControl): def __init__(self, CP, CI, dt): diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py new file mode 100644 index 0000000000..c5e6303e12 --- /dev/null +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -0,0 +1,2042 @@ +import math +import numpy as np + +from opendbc.car.gm.values import CAR as GM_CAR +from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR +from opendbc.car.toyota.values import CAR as TOYOTA_CAR +from openpilot.common.constants import CV +from openpilot.starpilot.common.testing_grounds import testing_ground + +CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30 +CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT = 1.20 +CIVIC_BOSCH_MODIFIED_A_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.00 +CIVIC_BOSCH_MODIFIED_B_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.75 +CIVIC_BOSCH_MODIFIED_B_TRANSITION_SPEED = 12.0 +CIVIC_BOSCH_MODIFIED_B_PHASE_SCALE = 0.08 +CIVIC_BOSCH_MODIFIED_B_FF_ONSET = 0.18 +CIVIC_BOSCH_MODIFIED_B_FF_ONSET_WIDTH = 0.07 +CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF = 1.35 +CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF_WIDTH = 0.38 +CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_LEFT = 0.14 +CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_RIGHT = 0.24 +CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_LEFT = 0.04 +CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_RIGHT = 0.00 +CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_LEFT = 0.40 +CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_RIGHT = 0.60 +CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT = 0.02 +CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 +CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT = 0.26 +CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT = 0.40 +CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT = -0.10 +CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT = 0.02 +CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT = -0.05 +CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT = 0.02 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT = 0.24 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT = 0.06 +CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = -0.02 +CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.01 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.22 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.05 +CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_MAX = 0.12 +CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT = 0.24 +CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT_WIDTH = 0.05 +CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED = 18.0 +CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED_WIDTH = 2.5 +CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.50 +CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.82 +CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.00 +CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT = 0.00 +CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 4.40 +CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 6.20 +CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00 +CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 +CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 2.80 +CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 4.20 + +BOLT_2022_2023_CARS = ( + GM_CAR.CHEVROLET_BOLT_ACC_2022_2023, + GM_CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL, + GM_CAR.CHEVROLET_BOLT_CC_2022_2023, +) +BOLT_2018_2021_CARS = ( + GM_CAR.CHEVROLET_BOLT_CC_2018_2021, +) +BOLT_2017_CARS = ( + GM_CAR.CHEVROLET_BOLT_CC_2017, +) +BOLT_CARS = BOLT_2022_2023_CARS + BOLT_2018_2021_CARS + BOLT_2017_CARS +VOLT_STANDARD_CARS = ( + GM_CAR.CHEVROLET_VOLT, + GM_CAR.CHEVROLET_VOLT_ASCM, + GM_CAR.CHEVROLET_VOLT_CAMERA, + GM_CAR.CHEVROLET_VOLT_CC, +) +SILVERADO_CARS = ( + GM_CAR.CHEVROLET_SILVERADO, + GM_CAR.CHEVROLET_SILVERADO_CC, +) +GENESIS_G90_CARS = ( + HYUNDAI_CAR.GENESIS_G90, +) +PALISADE_CARS = ( + HYUNDAI_CAR.HYUNDAI_PALISADE, + HYUNDAI_CAR.HYUNDAI_PALISADE_2023, +) +IONIQ_5_CARS = ( + HYUNDAI_CAR.HYUNDAI_IONIQ_5, +) +IONIQ_EV_OLD_CARS = ( + HYUNDAI_CAR.HYUNDAI_IONIQ_EV_LTD, + HYUNDAI_CAR.HYUNDAI_IONIQ_EV_2020, +) +IONIQ_6_CARS = ( + HYUNDAI_CAR.HYUNDAI_IONIQ_6, +) +SONATA_HYBRID_CARS = ( + HYUNDAI_CAR.HYUNDAI_SONATA_HYBRID, +) +SONATA_CARS = ( + HYUNDAI_CAR.HYUNDAI_SONATA, +) +ELANTRA_NON_SCC_CARS = ( + HYUNDAI_CAR.HYUNDAI_ELANTRA_2022_NON_SCC, + HYUNDAI_CAR.HYUNDAI_ELANTRA_HEV_2022_NON_SCC, +) +KIA_EV6_CARS = ( + HYUNDAI_CAR.KIA_EV6, +) +KIA_XCEED_CARS = ( + HYUNDAI_CAR.KIA_XCEED_PHEV, +) +KIA_NIRO_PHEV_2022_CARS = ( + HYUNDAI_CAR.KIA_NIRO_PHEV_2022, +) +KIA_FORTE_CARS = ( + HYUNDAI_CAR.KIA_FORTE, + HYUNDAI_CAR.KIA_FORTE_2019_NON_SCC, + HYUNDAI_CAR.KIA_FORTE_2021_NON_SCC, +) +PRIUS_CARS = ( + TOYOTA_CAR.TOYOTA_PRIUS, +) + +BOLT_2017_LATERAL_TESTING_GROUND_ID = testing_ground.id_3 +BOLT_2017_STEER_RATIO_TEST_SCALE = 1.045 +BOLT_2017_STEER_RATIO_ONSET_SPEED = 20.0 * CV.MPH_TO_MS +BOLT_2017_STEER_RATIO_ONSET_WIDTH = 4.0 * CV.MPH_TO_MS +BOLT_2017_CENTER_TAPER_LAT = 0.10 +BOLT_2017_CENTER_TAPER_WIDTH = 0.03 +BOLT_2017_CENTER_TAPER_GAIN = 0.055 +BOLT_2017_TORQUE_SCALE_BP = [0.0, 0.2, 0.5, 1.0, 1.5, 2.5] +BOLT_2017_TORQUE_SCALE_LEFT = [1.0, 1.0, 1.065, 1.060, 1.055, 1.045] +BOLT_2017_TORQUE_SCALE_RIGHT = [1.0, 1.0, 1.035, 1.020, 0.995, 0.985] +BOLT_2017_TRANSITION_SPEED = 10.0 +BOLT_2017_PHASE_SCALE = 0.12 +BOLT_2017_TURN_IN_BOOST_LEFT = 0.28 +BOLT_2017_TURN_IN_BOOST_RIGHT = 0.18 +BOLT_2017_UNWIND_TAPER_LEFT = 0.08 +BOLT_2017_UNWIND_TAPER_RIGHT = 0.28 + +BOLT_2018_2021_LATERAL_TESTING_GROUND_ID = testing_ground.id_4 +BOLT_2018_2021_STEER_RATIO_TEST_SCALE = 1.01 +BOLT_2018_2021_TORQUE_GAIN_LEFT = 0.090 +BOLT_2018_2021_TORQUE_GAIN_RIGHT = 0.050 +BOLT_2018_2021_TORQUE_ONSET = 0.18 +BOLT_2018_2021_TORQUE_ONSET_WIDTH = 0.08 +BOLT_2018_2021_TORQUE_CUTOFF = 1.05 +BOLT_2018_2021_TORQUE_CUTOFF_WIDTH = 0.24 +BOLT_2018_2021_JERK_TAPER_CUTOFF = 0.42 +BOLT_2018_2021_CENTER_TAPER_LAT = 0.12 +BOLT_2018_2021_CENTER_TAPER_WIDTH = 0.04 +BOLT_2018_2021_CENTER_TAPER_GAIN = 0.35 +BOLT_2018_2021_TRANSITION_SPEED = 8.5 +BOLT_2018_2021_PHASE_SCALE = 0.10 +BOLT_2018_2021_TURN_IN_BOOST_LEFT = 0.22 +BOLT_2018_2021_TURN_IN_BOOST_RIGHT = 0.12 +BOLT_2018_2021_UNWIND_TAPER_GAIN_LEFT = 0.80 +BOLT_2018_2021_UNWIND_TAPER_GAIN_RIGHT = 1.04 +BOLT_2018_2021_FRICTION_MULT = 1.01 +BOLT_2018_2021_FRICTION_LAT_RISE = 0.24 +BOLT_2018_2021_FRICTION_JERK_RISE = 0.28 +BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.16 +BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.16 +BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_LEFT = 0.15 +BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.25 +BOLT_2018_2021_TURN_IN_FRICTION_BOOST_LEFT = 0.08 +BOLT_2018_2021_TURN_IN_FRICTION_BOOST_RIGHT = 0.08 +BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_LEFT = 0.17 +BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_RIGHT = 0.27 + +BOLT_2022_2023_LATERAL_TESTING_GROUND_ID = testing_ground.id_5 +BOLT_2022_2023_FF_GAIN_LEFT = 0.11 +BOLT_2022_2023_FF_GAIN_RIGHT = 0.06 +BOLT_2022_2023_FF_ONSET = 0.12 +BOLT_2022_2023_FF_ONSET_WIDTH = 0.07 +BOLT_2022_2023_FF_CUTOFF = 1.35 +BOLT_2022_2023_FF_CUTOFF_WIDTH = 0.28 +BOLT_2022_2023_TRANSITION_SPEED = 9.0 +BOLT_2022_2023_PHASE_SCALE = 0.12 +BOLT_2022_2023_TURN_IN_BOOST_LEFT = 0.18 +BOLT_2022_2023_TURN_IN_BOOST_RIGHT = 0.13 +BOLT_2022_2023_UNWIND_TAPER_LEFT = 0.38 +BOLT_2022_2023_UNWIND_TAPER_RIGHT = 0.40 +BOLT_2022_2023_FRICTION_MULT = 1.09 +BOLT_2022_2023_FRICTION_LAT_RISE = 0.22 +BOLT_2022_2023_FRICTION_JERK_RISE = 0.26 +BOLT_2022_2023_CENTER_TAPER_MAX = 0.11 +BOLT_2022_2023_CENTER_TAPER_LAT = 0.18 +BOLT_2022_2023_CENTER_TAPER_LAT_WIDTH = 0.03 +BOLT_2022_2023_CENTER_TAPER_SPEED = 25.0 +BOLT_2022_2023_CENTER_TAPER_SPEED_WIDTH = 2.5 +BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.16 +BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.12 +BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT = 0.26 +BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.28 +BOLT_2022_2023_TURN_IN_FRICTION_BOOST_LEFT = 0.10 +BOLT_2022_2023_TURN_IN_FRICTION_BOOST_RIGHT = 0.07 +BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_LEFT = 0.27 +BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_RIGHT = 0.28 + +VOLT_STANDARD_LATERAL_TESTING_GROUND_ID = testing_ground.id_3 +VOLT_STANDARD_FF_GAIN_LEFT = 0.13 +VOLT_STANDARD_FF_GAIN_RIGHT = 0.06 +VOLT_STANDARD_FF_ONSET = 0.10 +VOLT_STANDARD_FF_ONSET_WIDTH = 0.05 +VOLT_STANDARD_FF_CUTOFF = 1.38 +VOLT_STANDARD_FF_CUTOFF_WIDTH = 0.28 +VOLT_STANDARD_TRANSITION_SPEED = 10.0 +VOLT_STANDARD_PHASE_SCALE = 0.10 +VOLT_STANDARD_TURN_IN_BOOST_LEFT = 0.20 +VOLT_STANDARD_TURN_IN_BOOST_RIGHT = 0.20 +VOLT_STANDARD_UNWIND_TAPER_LEFT = 0.20 +VOLT_STANDARD_UNWIND_TAPER_RIGHT = 0.20 +VOLT_STANDARD_FRICTION_MULT = 1.00 +VOLT_STANDARD_FRICTION_LAT_RISE = 0.20 +VOLT_STANDARD_FRICTION_JERK_RISE = 0.20 +VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.05 +VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.05 +VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_LEFT = 0.05 +VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.05 +VOLT_STANDARD_TURN_IN_FRICTION_BOOST_LEFT = 0.00 +VOLT_STANDARD_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 +VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_LEFT = 0.05 +VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_RIGHT = 0.05 +VOLT_STANDARD_CENTER_TAPER_MAX = 0.12 +VOLT_STANDARD_CENTER_TAPER_LAT = 0.10 +VOLT_STANDARD_CENTER_TAPER_LAT_WIDTH = 0.018 +VOLT_STANDARD_CENTER_TAPER_SPEED = 20.0 +VOLT_STANDARD_CENTER_TAPER_SPEED_WIDTH = 2.5 + +SILVERADO_CENTER_TAPER_MAX = 0.22 +SILVERADO_CENTER_TAPER_LAT = 0.18 +SILVERADO_CENTER_TAPER_LAT_WIDTH = 0.05 +SILVERADO_CENTER_TAPER_SPEED = 12.0 +SILVERADO_CENTER_TAPER_SPEED_WIDTH = 2.5 + +SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT = 1.05 +SONATA_HYBRID_FF_REDUCTION_LEFT = 0.09 +SONATA_HYBRID_FF_REDUCTION_RIGHT = 0.22 +SONATA_HYBRID_FF_ONSET = 0.18 +SONATA_HYBRID_FF_ONSET_WIDTH = 0.08 +SONATA_HYBRID_FF_CUTOFF = 1.35 +SONATA_HYBRID_FF_CUTOFF_WIDTH = 0.40 +SONATA_HYBRID_TRANSITION_SPEED = 8.0 +SONATA_HYBRID_PHASE_SCALE = 0.12 +SONATA_HYBRID_TURN_IN_BOOST_LEFT = 0.12 +SONATA_HYBRID_TURN_IN_BOOST_RIGHT = 0.02 +SONATA_HYBRID_UNWIND_TAPER_LEFT = 0.18 +SONATA_HYBRID_UNWIND_TAPER_RIGHT = 0.10 +SONATA_HYBRID_CENTER_TAPER_MAX = 0.07 +SONATA_HYBRID_CENTER_TAPER_LAT = 0.16 +SONATA_HYBRID_CENTER_TAPER_LAT_WIDTH = 0.025 +SONATA_HYBRID_CENTER_TAPER_SPEED = 22.0 +SONATA_HYBRID_CENTER_TAPER_SPEED_WIDTH = 2.5 +SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_MAX = 0.14 +SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT = 0.10 +SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02 +SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 7.5 +SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.0 + +SONATA_FF_REDUCTION_LEFT = 0.04 +SONATA_FF_REDUCTION_RIGHT = 0.26 +SONATA_FF_ONSET = 0.18 +SONATA_FF_ONSET_WIDTH = 0.08 +SONATA_FF_CUTOFF = 1.40 +SONATA_FF_CUTOFF_WIDTH = 0.42 +SONATA_TRANSITION_SPEED = 8.5 +SONATA_PHASE_SCALE = 0.12 +SONATA_TURN_IN_BOOST_LEFT = 0.18 +SONATA_TURN_IN_BOOST_RIGHT = 0.00 +SONATA_UNWIND_TAPER_LEFT = 0.28 +SONATA_UNWIND_TAPER_RIGHT = 0.00 +SONATA_CENTER_TAPER_MAX = 0.04 +SONATA_CENTER_TAPER_LAT = 0.15 +SONATA_CENTER_TAPER_LAT_WIDTH = 0.025 +SONATA_CENTER_TAPER_SPEED = 22.0 +SONATA_CENTER_TAPER_SPEED_WIDTH = 2.5 +SONATA_LOW_SPEED_CENTER_TAPER_MAX = 0.08 +SONATA_LOW_SPEED_CENTER_TAPER_LAT = 0.10 +SONATA_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02 +SONATA_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 7.0 +SONATA_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.0 + +ELANTRA_NON_SCC_FF_ADJUST_LEFT = 0.02 +ELANTRA_NON_SCC_FF_ADJUST_RIGHT = -0.02 +ELANTRA_NON_SCC_FF_ONSET = 0.14 +ELANTRA_NON_SCC_FF_ONSET_WIDTH = 0.06 +ELANTRA_NON_SCC_FF_CUTOFF = 1.10 +ELANTRA_NON_SCC_FF_CUTOFF_WIDTH = 0.34 +ELANTRA_NON_SCC_TRANSITION_SPEED = 8.0 +ELANTRA_NON_SCC_PHASE_SCALE = 0.10 +ELANTRA_NON_SCC_TURN_IN_BOOST_LEFT = 0.10 +ELANTRA_NON_SCC_TURN_IN_BOOST_RIGHT = 0.12 +ELANTRA_NON_SCC_UNWIND_TAPER_LEFT = 0.22 +ELANTRA_NON_SCC_UNWIND_TAPER_RIGHT = 0.12 + +KIA_XCEED_FF_REDUCTION_LEFT = 0.04 +KIA_XCEED_FF_REDUCTION_RIGHT = 0.10 +KIA_XCEED_FF_ONSET = 0.16 +KIA_XCEED_FF_ONSET_WIDTH = 0.06 +KIA_XCEED_FF_CUTOFF = 1.30 +KIA_XCEED_FF_CUTOFF_WIDTH = 0.36 +KIA_XCEED_TRANSITION_SPEED = 11.5 +KIA_XCEED_PHASE_SCALE = 0.10 +KIA_XCEED_TURN_IN_BOOST_LEFT = 0.26 +KIA_XCEED_TURN_IN_BOOST_RIGHT = 0.06 +KIA_XCEED_UNWIND_TAPER_LEFT = 0.20 +KIA_XCEED_UNWIND_TAPER_RIGHT = 0.14 +KIA_XCEED_CENTER_TAPER_MAX = 0.06 +KIA_XCEED_CENTER_TAPER_LAT = 0.12 +KIA_XCEED_CENTER_TAPER_LAT_WIDTH = 0.03 +KIA_XCEED_CENTER_TAPER_SPEED = 17.5 +KIA_XCEED_CENTER_TAPER_SPEED_WIDTH = 2.5 + +KIA_NIRO_PHEV_2022_CENTER_TAPER_MAX = 0.06 +KIA_NIRO_PHEV_2022_CENTER_TAPER_LAT = 0.12 +KIA_NIRO_PHEV_2022_CENTER_TAPER_LAT_WIDTH = 0.03 +KIA_NIRO_PHEV_2022_CENTER_TAPER_SPEED = 23.0 +KIA_NIRO_PHEV_2022_CENTER_TAPER_SPEED_WIDTH = 2.5 +KIA_NIRO_PHEV_2022_FRICTION_CENTER_LAT = 0.12 +KIA_NIRO_PHEV_2022_FRICTION_CENTER_LAT_WIDTH = 0.03 +KIA_NIRO_PHEV_2022_FRICTION_SPEED = 23.0 +KIA_NIRO_PHEV_2022_FRICTION_SPEED_WIDTH = 2.5 +KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK = 0.22 +KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK_WIDTH = 0.06 +KIA_NIRO_PHEV_2022_FRICTION_THRESHOLD_GAIN = 0.12 + +KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT = 1.05 +KIA_FORTE_FF_REDUCTION_LEFT = 0.05 +KIA_FORTE_FF_REDUCTION_RIGHT = 0.10 +KIA_FORTE_FF_ONSET = 0.16 +KIA_FORTE_FF_ONSET_WIDTH = 0.06 +KIA_FORTE_FF_CUTOFF = 1.20 +KIA_FORTE_FF_CUTOFF_WIDTH = 0.36 +KIA_FORTE_TRANSITION_SPEED = 9.0 +KIA_FORTE_PHASE_SCALE = 0.10 +KIA_FORTE_TURN_IN_BOOST_LEFT = 0.10 +KIA_FORTE_TURN_IN_BOOST_RIGHT = 0.05 +KIA_FORTE_UNWIND_TAPER_LEFT = 0.18 +KIA_FORTE_UNWIND_TAPER_RIGHT = 0.02 +KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.10 +KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.14 +KIA_FORTE_CRAWL_TURN_IN_FF_SPEED = 4.5 +KIA_FORTE_CRAWL_TURN_IN_FF_SPEED_WIDTH = 0.8 +KIA_FORTE_CRAWL_TURN_IN_FF_LAT = 0.10 +KIA_FORTE_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.05 +KIA_FORTE_CENTER_TAPER_MAX = 0.12 +KIA_FORTE_CENTER_TAPER_LAT = 0.18 +KIA_FORTE_CENTER_TAPER_LAT_WIDTH = 0.04 +KIA_FORTE_CENTER_TAPER_SPEED = 22.5 +KIA_FORTE_CENTER_TAPER_SPEED_WIDTH = 3.0 +KIA_FORTE_FRICTION_CENTER_LAT = 0.18 +KIA_FORTE_FRICTION_CENTER_LAT_WIDTH = 0.04 +KIA_FORTE_FRICTION_SPEED = 24.0 +KIA_FORTE_FRICTION_SPEED_WIDTH = 3.0 +KIA_FORTE_FRICTION_CALM_JERK = 0.24 +KIA_FORTE_FRICTION_CALM_JERK_WIDTH = 0.08 +KIA_FORTE_FRICTION_THRESHOLD_GAIN = 0.10 + +PALISADE_BASE_LAT_ACCEL_FACTOR_MULT = 0.98 +PALISADE_FF_GAIN_LEFT = 0.14 +PALISADE_FF_GAIN_RIGHT = 0.12 +PALISADE_FF_ONSET = 0.08 +PALISADE_FF_ONSET_WIDTH = 0.04 +PALISADE_FF_CUTOFF = 1.25 +PALISADE_FF_CUTOFF_WIDTH = 0.36 +PALISADE_TRANSITION_SPEED = 9.0 +PALISADE_PHASE_SCALE = 0.11 +PALISADE_TURN_IN_BOOST_LEFT = 0.34 +PALISADE_TURN_IN_BOOST_RIGHT = 0.24 +PALISADE_UNWIND_TAPER_LEFT = 0.18 +PALISADE_UNWIND_TAPER_RIGHT = 0.30 +PALISADE_FRICTION_MULT = 1.02 +PALISADE_FRICTION_LAT_RISE = 0.20 +PALISADE_FRICTION_JERK_RISE = 0.24 +PALISADE_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.18 +PALISADE_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.14 +PALISADE_UNWIND_THRESHOLD_INCREASE_LEFT = 0.14 +PALISADE_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.22 +PALISADE_TURN_IN_FRICTION_BOOST_LEFT = 0.08 +PALISADE_TURN_IN_FRICTION_BOOST_RIGHT = 0.06 +PALISADE_UNWIND_FRICTION_REDUCTION_LEFT = 0.12 +PALISADE_UNWIND_FRICTION_REDUCTION_RIGHT = 0.20 + +GENESIS_G90_LATERAL_TESTING_GROUND_ID = testing_ground.id_4 +GENESIS_G90_FF_GAIN_LEFT = 0.32 +GENESIS_G90_FF_GAIN_RIGHT = 0.16 +GENESIS_G90_FF_ONSET = 0.10 +GENESIS_G90_FF_ONSET_WIDTH = 0.05 +GENESIS_G90_FF_CUTOFF = 2.35 +GENESIS_G90_FF_CUTOFF_WIDTH = 0.48 +GENESIS_G90_TRANSITION_SPEED = 10.0 +GENESIS_G90_PHASE_SCALE = 0.12 +GENESIS_G90_TURN_IN_BOOST_LEFT = 0.66 +GENESIS_G90_TURN_IN_BOOST_RIGHT = 0.50 +GENESIS_G90_UNWIND_TAPER_LEFT = 0.10 +GENESIS_G90_UNWIND_TAPER_RIGHT = 0.55 +GENESIS_G90_FRICTION_MULT = 1.02 +GENESIS_G90_FRICTION_LAT_RISE = 0.22 +GENESIS_G90_FRICTION_JERK_RISE = 0.24 +GENESIS_G90_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.28 +GENESIS_G90_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.22 +GENESIS_G90_UNWIND_THRESHOLD_INCREASE_LEFT = 0.06 +GENESIS_G90_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.32 +GENESIS_G90_TURN_IN_FRICTION_BOOST_LEFT = 0.16 +GENESIS_G90_TURN_IN_FRICTION_BOOST_RIGHT = 0.14 +GENESIS_G90_UNWIND_FRICTION_REDUCTION_LEFT = 0.04 +GENESIS_G90_UNWIND_FRICTION_REDUCTION_RIGHT = 0.30 + +IONIQ_5_BASE_LAT_ACCEL_FACTOR_MULT = 1.22 +IONIQ_5_FF_ONSET = 0.10 +IONIQ_5_FF_ONSET_WIDTH = 0.05 +IONIQ_5_FF_CUTOFF = 1.20 +IONIQ_5_FF_CUTOFF_WIDTH = 0.30 +IONIQ_5_TRANSITION_SPEED = 12.5 +IONIQ_5_PHASE_SCALE = 0.10 +IONIQ_5_FF_REDUCTION_LEFT = 0.15 +IONIQ_5_FF_REDUCTION_RIGHT = 0.25 +IONIQ_5_TURN_IN_BOOST_LEFT = 0.10 +IONIQ_5_TURN_IN_BOOST_RIGHT = 0.04 +IONIQ_5_UNWIND_TAPER_LEFT = 0.92 +IONIQ_5_UNWIND_TAPER_RIGHT = 1.04 +IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.05 +IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.03 +IONIQ_5_UNWIND_THRESHOLD_INCREASE_LEFT = 0.46 +IONIQ_5_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.50 +IONIQ_5_TURN_IN_FRICTION_BOOST_LEFT = 0.02 +IONIQ_5_TURN_IN_FRICTION_BOOST_RIGHT = 0.01 +IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT = 0.42 +IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT = 0.44 +IONIQ_5_CENTER_TAPER_MAX = 0.18 +IONIQ_5_CENTER_TAPER_LAT = 0.16 +IONIQ_5_CENTER_TAPER_LAT_WIDTH = 0.04 +IONIQ_5_CENTER_TAPER_SPEED = 15.0 +IONIQ_5_CENTER_TAPER_SPEED_WIDTH = 2.2 + +IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT = 1.16 +IONIQ_EV_OLD_FF_REDUCTION_LEFT = 0.16 +IONIQ_EV_OLD_FF_REDUCTION_RIGHT = 0.30 +IONIQ_EV_OLD_FF_ONSET = 0.14 +IONIQ_EV_OLD_FF_ONSET_WIDTH = 0.05 +IONIQ_EV_OLD_FF_CUTOFF = 1.10 +IONIQ_EV_OLD_FF_CUTOFF_WIDTH = 0.30 +IONIQ_EV_OLD_TRANSITION_SPEED = 10.0 +IONIQ_EV_OLD_PHASE_SCALE = 0.10 +IONIQ_EV_OLD_TURN_IN_BOOST_LEFT = 0.01 +IONIQ_EV_OLD_TURN_IN_BOOST_RIGHT = 0.00 +IONIQ_EV_OLD_UNWIND_TAPER_LEFT = 0.26 +IONIQ_EV_OLD_UNWIND_TAPER_RIGHT = 0.06 +IONIQ_EV_OLD_CENTER_TAPER_MAX = 0.14 +IONIQ_EV_OLD_CENTER_TAPER_LAT = 0.12 +IONIQ_EV_OLD_CENTER_TAPER_LAT_WIDTH = 0.03 +IONIQ_EV_OLD_CENTER_TAPER_SPEED = 22.0 +IONIQ_EV_OLD_CENTER_TAPER_SPEED_WIDTH = 2.2 + +IONIQ_6_FF_GAIN_LEFT = 0.045 +IONIQ_6_FF_GAIN_RIGHT = 0.015 +IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT = 1.22 +IONIQ_6_BASE_FRICTION_THRESHOLD = 0.36 +IONIQ_6_FF_ONSET = 0.10 +IONIQ_6_FF_ONSET_WIDTH = 0.04 +IONIQ_6_FF_CUTOFF = 0.48 +IONIQ_6_FF_CUTOFF_WIDTH = 0.12 +IONIQ_6_TRANSITION_SPEED = 10.0 +IONIQ_6_PHASE_SCALE = 0.10 +IONIQ_6_TURN_IN_BOOST_LEFT = 1.64 +IONIQ_6_TURN_IN_BOOST_RIGHT = 2.10 +IONIQ_6_UNWIND_TAPER_LEFT = 3.18 +IONIQ_6_UNWIND_TAPER_RIGHT = 8.20 +IONIQ_6_FRICTION_MULT = 0.928 +IONIQ_6_FRICTION_LAT_RISE = 0.20 +IONIQ_6_FRICTION_JERK_RISE = 0.24 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.78 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 1.42 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 3.90 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 10.20 +IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.44 +IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.94 +IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 3.55 +IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 9.10 +IONIQ_6_CENTER_TAPER_MAX = 0.082 +IONIQ_6_CENTER_TAPER_LAT = 0.24 +IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.025 +IONIQ_6_CENTER_TAPER_SPEED = 18.0 +IONIQ_6_CENTER_TAPER_SPEED_WIDTH = 2.5 +IONIQ_6_HIGHWAY_CENTER_TAPER_MAX = 0.046 +IONIQ_6_HIGHWAY_CENTER_TAPER_LAT = 0.10 +IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH = 0.035 +IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED = 24.5 +IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED_WIDTH = 1.8 +IONIQ_6_HIGHWAY_OUTPUT_TAPER_MAX = 0.10 +IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT = 0.14 +IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH = 0.04 +IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED = 23.5 +IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH = 2.0 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_MAX = 0.18 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT = 1.05 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT_WIDTH = 0.22 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK = 0.24 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK_WIDTH = 0.14 +IONIQ_6_LOW_MID_CENTER_TAPER_MAX = 0.088 +IONIQ_6_LOW_MID_CENTER_TAPER_LAT = 0.28 +IONIQ_6_LOW_MID_CENTER_TAPER_LAT_WIDTH = 0.06 +IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MIN = 8.5 +IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MAX = 16.5 +IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH = 1.5 +IONIQ_6_DIRECTIONAL_TAPER_LAT_START = 0.19 +IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90 +IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.06 +IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.11 +IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.45 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 2.15 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 4.25 +IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT = 0.48 +IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT = 0.52 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.16 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.04 +IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 0.60 +IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.14 +IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.98 +IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 11.2 +IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 1.5 +IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT = 0.10 +IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH = 0.06 +IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.18 +IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.24 +IONIQ_6_CRAWL_TURN_IN_FF_SPEED = 5.3 +IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH = 1.0 +IONIQ_6_CRAWL_TURN_IN_FF_LAT = 0.06 +IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.035 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE = 0.46 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED = 3.25 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH = 0.45 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR = 1.9 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH = 1.20 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE = 5.5 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH = 2.4 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_START = 0.66 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_WIDTH = 0.12 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_FLOOR = 0.26 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_BP = [0.0, 0.35, 0.65, 1.0] +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_V = [1.0, 1.0, 0.88, 0.08] +IONIQ_6_LOW_SPEED_UNWIND_ASSIST_MAX_TORQUE = 0.30 +IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED = 3.35 +IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED_WIDTH = 0.50 +IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR = 1.6 +IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR_WIDTH = 0.95 +IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE = 10.5 +IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE_WIDTH = 4.0 +IONIQ_6_LOW_SPEED_UNWIND_ASSIST_BLEND = 0.52 +IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST = 0.10 +IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED = 18.0 +IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH = 2.5 +IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_START = 0.06 +IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END = 0.22 +IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH = 0.035 +IONIQ_6_LOW_SPEED_PID_RESET_SPEED = 0.1 * CV.MPH_TO_MS +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.90 +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.18 +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.06 +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.17 +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.78 +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.10 +IONIQ_6_OUTPUT_TAPER_SPEED = 8.5 +IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5 +IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.90 +IONIQ_6_OUTPUT_DIRECTIONAL_TAPER_BLEND = 0.97 + +KIA_EV6_LATERAL_TESTING_GROUND_ID = testing_ground.id_6 +KIA_EV6_LATERAL_TESTING_GROUND_VARIANT = "C" +KIA_EV6_FF_GAIN_LEFT = 0.12 +KIA_EV6_FF_GAIN_RIGHT = 0.17 +KIA_EV6_FF_ONSET = 0.08 +KIA_EV6_FF_ONSET_WIDTH = 0.04 +KIA_EV6_FF_CUTOFF = 1.90 +KIA_EV6_FF_CUTOFF_WIDTH = 0.40 +KIA_EV6_TRANSITION_SPEED = 14.5 +KIA_EV6_PHASE_SCALE = 0.09 +KIA_EV6_TURN_IN_BOOST_LEFT = 0.48 +KIA_EV6_TURN_IN_BOOST_RIGHT = 0.60 +KIA_EV6_UNWIND_TAPER_LEFT = 0.56 +KIA_EV6_UNWIND_TAPER_RIGHT = 0.54 +KIA_EV6_FRICTION_MULT = 1.01 +KIA_EV6_FRICTION_LAT_RISE = 0.18 +KIA_EV6_FRICTION_JERK_RISE = 0.22 +KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.24 +KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.40 +KIA_EV6_UNWIND_THRESHOLD_INCREASE_LEFT = 0.28 +KIA_EV6_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.24 +KIA_EV6_TURN_IN_FRICTION_BOOST_LEFT = 0.12 +KIA_EV6_TURN_IN_FRICTION_BOOST_RIGHT = 0.24 +KIA_EV6_UNWIND_FRICTION_REDUCTION_LEFT = 0.28 +KIA_EV6_UNWIND_FRICTION_REDUCTION_RIGHT = 0.22 +KIA_EV6_CENTER_TAPER_MAX = 0.12 +KIA_EV6_CENTER_TAPER_LAT = 0.16 +KIA_EV6_CENTER_TAPER_LAT_WIDTH = 0.04 +KIA_EV6_CENTER_TAPER_SPEED = 17.0 +KIA_EV6_CENTER_TAPER_SPEED_WIDTH = 2.8 +KIA_EV6_LOW_SPEED_CENTER_TAPER_MAX = 0.24 +KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT = 0.08 +KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02 +KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 8.5 +KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.4 + +VOLT_PLEXY_LATERAL_TESTING_GROUND_ID = testing_ground.id_7 +VOLT_PLEXY_FF_EXTRA_MULT_LEFT = 0.97 +VOLT_PLEXY_FF_EXTRA_MULT_RIGHT = 1.16 +VOLT_PLEXY_FRICTION_SCALE_MULT_LEFT = 1.00 +VOLT_PLEXY_FRICTION_SCALE_MULT_RIGHT = 1.05 +VOLT_PLEXY_FRICTION_THRESHOLD_MULT_LEFT = 1.00 +VOLT_PLEXY_FRICTION_THRESHOLD_MULT_RIGHT = 0.96 +VOLT_PLEXY_CENTER_TAPER_REDUCTION_MULT = 0.98 +PRIUS_TRANSITION_SPEED = 11.0 +PRIUS_PHASE_SCALE = 0.09 +PRIUS_FF_GAIN_LEFT = 0.08 +PRIUS_FF_GAIN_RIGHT = 0.09 +PRIUS_FF_ONSET = 0.15 +PRIUS_FF_ONSET_WIDTH = 0.08 +PRIUS_FF_CUTOFF = 1.45 +PRIUS_FF_CUTOFF_WIDTH = 0.36 +PRIUS_FRICTION_LAT_RISE = 0.18 +PRIUS_FRICTION_JERK_RISE = 0.22 +PRIUS_TURN_IN_BOOST_LEFT = 0.38 +PRIUS_TURN_IN_BOOST_RIGHT = 0.34 +PRIUS_UNWIND_TAPER_LEFT = 0.28 +PRIUS_UNWIND_TAPER_RIGHT = 0.42 +PRIUS_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.22 +PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.22 +PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT = 0.28 +PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.38 +PRIUS_TURN_IN_FRICTION_BOOST_LEFT = 0.10 +PRIUS_TURN_IN_FRICTION_BOOST_RIGHT = 0.10 +PRIUS_UNWIND_FRICTION_REDUCTION_LEFT = 0.16 +PRIUS_UNWIND_FRICTION_REDUCTION_RIGHT = 0.22 +PRIUS_CENTER_TAPER_MAX = 0.12 +PRIUS_CENTER_TAPER_LAT = 0.14 +PRIUS_CENTER_TAPER_LAT_WIDTH = 0.035 +PRIUS_CENTER_TAPER_SPEED = 20.0 +PRIUS_CENTER_TAPER_SPEED_WIDTH = 2.5 + +TRAILER_LOAD_FULL_ASSIST_KG = 15000.0 * CV.LB_TO_KG +TRAILER_LATERAL_MIN_SPEED = 15.0 * CV.MPH_TO_MS +TRAILER_LATERAL_FULL_SPEED = 35.0 * CV.MPH_TO_MS +TRAILER_LATERAL_LAT_RISE = 0.30 +TRAILER_LATERAL_FF_GAIN = 0.05 +TRAILER_LATERAL_FRICTION_GAIN = 0.03 + + +def _sigmoid(x: float) -> float: + if x >= 0.0: + z = math.exp(-x) + return 1.0 / (1.0 + z) + + z = math.exp(x) + return z / (1.0 + z) + + +def get_friction_threshold(v_ego: float) -> float: + # Keep the speed-scaled friction threshold behavior. + return float(np.interp(v_ego, [1 * CV.MPH_TO_MS, 20 * CV.MPH_TO_MS, 75 * CV.MPH_TO_MS], [0.16, 0.19, 0.27])) + + +def get_trailer_lateral_assist_factor(trailer_load_kg: float, v_ego: float, desired_lateral_accel: float) -> float: + load_factor = np.clip(trailer_load_kg / TRAILER_LOAD_FULL_ASSIST_KG, 0.0, 1.0) + speed_factor = np.interp(v_ego, [TRAILER_LATERAL_MIN_SPEED, TRAILER_LATERAL_FULL_SPEED], [0.0, 1.0]) + lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / TRAILER_LATERAL_LAT_RISE) + return float(load_factor * speed_factor * lat_factor) + + +def get_trailer_lateral_ff_scale(trailer_load_kg: float, v_ego: float, desired_lateral_accel: float) -> float: + return 1.0 + TRAILER_LATERAL_FF_GAIN * get_trailer_lateral_assist_factor(trailer_load_kg, v_ego, desired_lateral_accel) + + +def get_trailer_lateral_friction_scale(trailer_load_kg: float, v_ego: float, desired_lateral_accel: float) -> float: + return 1.0 + TRAILER_LATERAL_FRICTION_GAIN * get_trailer_lateral_assist_factor(trailer_load_kg, v_ego, desired_lateral_accel) + + +def _prius_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _prius_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / PRIUS_TRANSITION_SPEED) ** 2) + + +def _prius_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / PRIUS_PHASE_SCALE) + + +def _prius_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def _prius_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / PRIUS_FRICTION_LAT_RISE) + jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / PRIUS_FRICTION_JERK_RISE) + return _prius_low_speed_factor(v_ego) * lat_factor * jerk_factor + + +def get_prius_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + gain = _prius_side_value(desired_lateral_accel, PRIUS_FF_GAIN_LEFT, PRIUS_FF_GAIN_RIGHT) + abs_lateral_accel = abs(desired_lateral_accel) + onset = _prius_sigmoid((abs_lateral_accel - PRIUS_FF_ONSET) / PRIUS_FF_ONSET_WIDTH) + cutoff = _prius_sigmoid((PRIUS_FF_CUTOFF - abs_lateral_accel) / PRIUS_FF_CUTOFF_WIDTH) + extra_scale = gain * onset * cutoff + phase = _prius_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _prius_low_speed_factor(v_ego) + turn_in_boost = 1.0 + (_prius_side_value(desired_lateral_accel, PRIUS_TURN_IN_BOOST_LEFT, PRIUS_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.35 + 0.65 * low_speed_factor)) + unwind_taper = 1.0 - (_prius_side_value(desired_lateral_accel, PRIUS_UNWIND_TAPER_LEFT, PRIUS_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) + + +def get_prius_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + base_threshold = get_friction_threshold(v_ego) + transition_envelope = _prius_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _prius_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + threshold_scale = 1.0 - (_prius_side_value(desired_lateral_accel, PRIUS_TURN_IN_THRESHOLD_REDUCTION_LEFT, PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + transition_envelope * turn_in_weight) + threshold_scale += (_prius_side_value(desired_lateral_accel, PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT, PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT) * + transition_envelope * unwind_weight) + return base_threshold * min(max(threshold_scale, 0.86), 1.16) + + +def get_prius_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + transition_envelope = _prius_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _prius_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + friction_scale = 1.0 + friction_scale += (_prius_side_value(desired_lateral_accel, PRIUS_TURN_IN_FRICTION_BOOST_LEFT, PRIUS_TURN_IN_FRICTION_BOOST_RIGHT) * + transition_envelope * turn_in_weight) + friction_scale -= (_prius_side_value(desired_lateral_accel, PRIUS_UNWIND_FRICTION_REDUCTION_LEFT, PRIUS_UNWIND_FRICTION_REDUCTION_RIGHT) * + transition_envelope * unwind_weight) + return min(max(friction_scale, 0.90), 1.14) + + +def get_prius_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _prius_sigmoid((v_ego - PRIUS_CENTER_TAPER_SPEED) / PRIUS_CENTER_TAPER_SPEED_WIDTH) + center_weight = _prius_sigmoid((PRIUS_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / PRIUS_CENTER_TAPER_LAT_WIDTH) + reduction = PRIUS_CENTER_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + +def civic_bosch_modified_lateral_testing_ground_active() -> bool: + return testing_ground.use("8", "B") + + +def civic_bosch_modified_a_lateral_testing_ground_active() -> bool: + return testing_ground.use("8", "A") + + +def get_civic_bosch_modified_a_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _sigmoid((v_ego - CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED) / + CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED_WIDTH) + center_weight = _sigmoid((CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / + CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT_WIDTH) + reduction = CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + +def _civic_bosch_modified_b_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / CIVIC_BOSCH_MODIFIED_B_TRANSITION_SPEED) ** 2) + + +def _civic_bosch_modified_b_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / CIVIC_BOSCH_MODIFIED_B_PHASE_SCALE) + + +def _civic_bosch_modified_b_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def get_civic_bosch_modified_b_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + abs_lateral_accel = abs(desired_lateral_accel) + onset = _sigmoid((abs_lateral_accel - CIVIC_BOSCH_MODIFIED_B_FF_ONSET) / CIVIC_BOSCH_MODIFIED_B_FF_ONSET_WIDTH) + cutoff = _sigmoid((CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF - abs_lateral_accel) / CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF_WIDTH) + base_reduction = _civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_LEFT, + CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_RIGHT) * onset * cutoff + + phase = _civic_bosch_modified_b_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _civic_bosch_modified_b_low_speed_factor(v_ego) + a_variant_active = civic_bosch_modified_a_lateral_testing_ground_active() + variant_active = civic_bosch_modified_lateral_testing_ground_active() + if a_variant_active: + base_reduction -= (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT, + CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT) * onset * cutoff) + if variant_active: + base_reduction += (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT, + CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT) * onset * cutoff) + + turn_in_boost = 1.0 + (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_LEFT, + CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.40 + 0.60 * low_speed_factor)) + if a_variant_active: + turn_in_boost *= 1.0 + (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT, + CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.40 + 0.60 * low_speed_factor)) + if variant_active: + turn_in_boost *= 1.0 + (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT, + CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.40 + 0.60 * low_speed_factor)) + unwind_taper = 1.0 - (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_LEFT, + CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + if a_variant_active: + unwind_taper *= 1.0 - (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT, + CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + if variant_active: + unwind_taper *= 1.0 - (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT, + CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0) + + +def get_civic_bosch_modified_b_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + if desired_lateral_accel == 0.0 or desired_lateral_jerk == 0.0: + return 1.0 + + abs_lateral_accel = abs(desired_lateral_accel) + onset = _sigmoid((abs_lateral_accel - CIVIC_BOSCH_MODIFIED_B_FF_ONSET) / CIVIC_BOSCH_MODIFIED_B_FF_ONSET_WIDTH) + cutoff = _sigmoid((CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF - abs_lateral_accel) / CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF_WIDTH) + envelope = onset * cutoff * _civic_bosch_modified_b_low_speed_factor(v_ego) + phase = _civic_bosch_modified_b_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + a_variant_active = civic_bosch_modified_a_lateral_testing_ground_active() + variant_active = civic_bosch_modified_lateral_testing_ground_active() + + friction_scale = 1.0 + friction_scale += (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT, + CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT) * + envelope * turn_in_weight) + if a_variant_active: + friction_scale += (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT, + CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT) * + envelope * turn_in_weight) + if variant_active: + friction_scale += (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT, + CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT) * + envelope * turn_in_weight) + friction_scale -= (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT, + CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT) * + envelope * unwind_weight) + if a_variant_active: + friction_scale -= (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT, + CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT) * + envelope * unwind_weight) + if variant_active: + friction_scale -= (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT, + CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT) * + envelope * unwind_weight) + return min(max(friction_scale, 0.82), 1.06) + + +def bolt_2017_lateral_testing_ground_active() -> bool: + return testing_ground.use(BOLT_2017_LATERAL_TESTING_GROUND_ID) + + +def _bolt_2017_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _bolt_2017_high_speed_factor(v_ego: float) -> float: + return _bolt_2017_sigmoid((max(v_ego, 0.0) - BOLT_2017_STEER_RATIO_ONSET_SPEED) / BOLT_2017_STEER_RATIO_ONSET_WIDTH) + + +def get_bolt_2017_steer_ratio_scale(v_ego: float) -> float: + return 1.0 + ((BOLT_2017_STEER_RATIO_TEST_SCALE - 1.0) * _bolt_2017_high_speed_factor(v_ego)) + + +def get_bolt_2017_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + center_window = _bolt_2017_sigmoid((BOLT_2017_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / BOLT_2017_CENTER_TAPER_WIDTH) + return 1.0 - (BOLT_2017_CENTER_TAPER_GAIN * _bolt_2017_high_speed_factor(v_ego) * center_window) + + +def _bolt_2017_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / BOLT_2017_TRANSITION_SPEED) ** 2) + + +def _bolt_2017_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / BOLT_2017_PHASE_SCALE) + + +def _bolt_2017_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def get_bolt_2017_base_torque_scale(desired_lateral_accel: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + scale_values = BOLT_2017_TORQUE_SCALE_LEFT if desired_lateral_accel > 0.0 else BOLT_2017_TORQUE_SCALE_RIGHT + return float(np.interp(abs(desired_lateral_accel), BOLT_2017_TORQUE_SCALE_BP, scale_values)) + + +def get_bolt_2017_torque_scale(desired_lateral_accel: float, desired_lateral_jerk: float = 0.0, v_ego: float = 30.0) -> float: + base_scale = get_bolt_2017_base_torque_scale(desired_lateral_accel) + scale = base_scale + if base_scale > 1.0 and desired_lateral_jerk != 0.0: + low_speed_factor = _bolt_2017_low_speed_factor(v_ego) + phase = _bolt_2017_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + turn_in_boost = 1.0 + (_bolt_2017_side_value(desired_lateral_accel, BOLT_2017_TURN_IN_BOOST_LEFT, BOLT_2017_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.35 + 0.65 * low_speed_factor)) + unwind_taper = 1.0 - (_bolt_2017_side_value(desired_lateral_accel, BOLT_2017_UNWIND_TAPER_LEFT, BOLT_2017_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.45 + 0.55 * low_speed_factor)) + scale = 1.0 + ((base_scale - 1.0) * turn_in_boost * max(unwind_taper, 0.0)) + + return scale * get_bolt_2017_center_taper_scale(desired_lateral_accel, v_ego) + + +def bolt_2018_2021_lateral_testing_ground_active() -> bool: + return testing_ground.use(BOLT_2018_2021_LATERAL_TESTING_GROUND_ID) + + +def _bolt_2018_2021_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _bolt_2018_2021_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / BOLT_2018_2021_TRANSITION_SPEED) ** 2) + + +def _bolt_2018_2021_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / BOLT_2018_2021_PHASE_SCALE) + + +def _bolt_2018_2021_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def _bolt_2018_2021_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / BOLT_2018_2021_FRICTION_LAT_RISE) + jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / BOLT_2018_2021_FRICTION_JERK_RISE) + return _bolt_2018_2021_low_speed_factor(v_ego) * lat_factor * jerk_factor + + +def get_bolt_2018_2021_torque_scale(desired_lateral_accel: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + gain = BOLT_2018_2021_TORQUE_GAIN_LEFT if desired_lateral_accel > 0.0 else BOLT_2018_2021_TORQUE_GAIN_RIGHT + abs_lateral_accel = abs(desired_lateral_accel) + onset = _bolt_2018_2021_sigmoid((abs_lateral_accel - BOLT_2018_2021_TORQUE_ONSET) / BOLT_2018_2021_TORQUE_ONSET_WIDTH) + cutoff = _bolt_2018_2021_sigmoid((BOLT_2018_2021_TORQUE_CUTOFF - abs_lateral_accel) / BOLT_2018_2021_TORQUE_CUTOFF_WIDTH) + return 1.0 + gain * onset * cutoff + + +def get_bolt_2018_2021_dynamic_torque_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + base_scale = get_bolt_2018_2021_torque_scale(desired_lateral_accel) + extra_scale = max(base_scale - 1.0, 0.0) + abs_lateral_accel = abs(desired_lateral_accel) + low_speed_factor = _bolt_2018_2021_low_speed_factor(v_ego) + high_speed_factor = 1.0 - low_speed_factor + center_window = _bolt_2018_2021_sigmoid((BOLT_2018_2021_CENTER_TAPER_LAT - abs_lateral_accel) / BOLT_2018_2021_CENTER_TAPER_WIDTH) + center_taper = 1.0 - (BOLT_2018_2021_CENTER_TAPER_GAIN * high_speed_factor * center_window) + phase = _bolt_2018_2021_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + jerk_taper = 1.0 / (1.0 + (abs(desired_lateral_jerk) / BOLT_2018_2021_JERK_TAPER_CUTOFF) ** 2) + turn_in_boost = 1.0 + (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_TURN_IN_BOOST_LEFT, BOLT_2018_2021_TURN_IN_BOOST_RIGHT) * + turn_in_weight * low_speed_factor) + unwind_weight = max(-phase, 0.0) + unwind_taper = 1.0 - (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_UNWIND_TAPER_GAIN_LEFT, BOLT_2018_2021_UNWIND_TAPER_GAIN_RIGHT) * + unwind_weight * (0.55 + 0.45 * low_speed_factor)) + return 1.0 + (extra_scale * center_taper * jerk_taper * turn_in_boost * max(unwind_taper, 0.0)) + + +def get_bolt_2018_2021_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + base_threshold = get_friction_threshold(v_ego) + transition_envelope = _bolt_2018_2021_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _bolt_2018_2021_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + threshold_scale = 1.0 - (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_LEFT, BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + transition_envelope * turn_in_weight) + threshold_scale += (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_LEFT, BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_RIGHT) * + transition_envelope * unwind_weight) + return base_threshold * min(max(threshold_scale, 0.82), 1.12) + + +def get_bolt_2018_2021_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + transition_envelope = _bolt_2018_2021_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _bolt_2018_2021_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + friction_scale = BOLT_2018_2021_FRICTION_MULT + friction_scale += (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_TURN_IN_FRICTION_BOOST_LEFT, BOLT_2018_2021_TURN_IN_FRICTION_BOOST_RIGHT) * + transition_envelope * turn_in_weight) + friction_scale -= (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_LEFT, BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_RIGHT) * + transition_envelope * unwind_weight) + return min(max(friction_scale, 0.88), 1.10) + + +def bolt_2022_2023_lateral_testing_ground_active() -> bool: + return testing_ground.use(BOLT_2022_2023_LATERAL_TESTING_GROUND_ID) + + +def _bolt_2022_2023_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _bolt_2022_2023_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / BOLT_2022_2023_TRANSITION_SPEED) ** 2) + + +def _bolt_2022_2023_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / BOLT_2022_2023_PHASE_SCALE) + + +def _bolt_2022_2023_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def _bolt_2022_2023_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / BOLT_2022_2023_FRICTION_LAT_RISE) + jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / BOLT_2022_2023_FRICTION_JERK_RISE) + return _bolt_2022_2023_low_speed_factor(v_ego) * lat_factor * jerk_factor + + +def get_bolt_2022_2023_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + gain = _bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_FF_GAIN_LEFT, BOLT_2022_2023_FF_GAIN_RIGHT) + abs_lateral_accel = abs(desired_lateral_accel) + onset = _bolt_2022_2023_sigmoid((abs_lateral_accel - BOLT_2022_2023_FF_ONSET) / BOLT_2022_2023_FF_ONSET_WIDTH) + cutoff = _bolt_2022_2023_sigmoid((BOLT_2022_2023_FF_CUTOFF - abs_lateral_accel) / BOLT_2022_2023_FF_CUTOFF_WIDTH) + extra_scale = gain * onset * cutoff + speed_weight = _bolt_2022_2023_sigmoid((v_ego - BOLT_2022_2023_CENTER_TAPER_SPEED) / BOLT_2022_2023_CENTER_TAPER_SPEED_WIDTH) + center_weight = _bolt_2022_2023_sigmoid((BOLT_2022_2023_CENTER_TAPER_LAT - abs_lateral_accel) / BOLT_2022_2023_CENTER_TAPER_LAT_WIDTH) + center_taper = 1.0 - (BOLT_2022_2023_CENTER_TAPER_MAX * speed_weight * center_weight) + low_speed_factor = _bolt_2022_2023_low_speed_factor(v_ego) + transition_envelope = _bolt_2022_2023_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + turn_in_boost = 1.0 + (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_BOOST_LEFT, BOLT_2022_2023_TURN_IN_BOOST_RIGHT) * + turn_in_weight * low_speed_factor) + unwind_envelope = (0.25 + 0.75 * low_speed_factor) * (1.0 + 0.45 * transition_envelope) + unwind_taper = 1.0 - (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_TAPER_LEFT, BOLT_2022_2023_UNWIND_TAPER_RIGHT) * + unwind_weight * unwind_envelope) + return 1.0 + (extra_scale * center_taper * turn_in_boost * max(unwind_taper, 0.0)) + + +def get_bolt_2022_2023_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + base_threshold = get_friction_threshold(v_ego) + transition_envelope = _bolt_2022_2023_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + threshold_scale = 1.0 - (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_LEFT, BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + transition_envelope * turn_in_weight) + threshold_scale += (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT, BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_RIGHT) * + transition_envelope * unwind_weight) + return base_threshold * min(max(threshold_scale, 0.84), 1.14) + + +def get_bolt_2022_2023_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + transition_envelope = _bolt_2022_2023_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + friction_scale = BOLT_2022_2023_FRICTION_MULT + friction_scale += (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_FRICTION_BOOST_LEFT, BOLT_2022_2023_TURN_IN_FRICTION_BOOST_RIGHT) * + transition_envelope * turn_in_weight) + friction_scale -= (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_LEFT, BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_RIGHT) * + transition_envelope * unwind_weight) + return min(max(friction_scale, 0.92), 1.22) + + +def volt_standard_lateral_testing_ground_active() -> bool: + return testing_ground.use(VOLT_STANDARD_LATERAL_TESTING_GROUND_ID) + + +def _volt_standard_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _volt_standard_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / VOLT_STANDARD_TRANSITION_SPEED) ** 2) + + +def _volt_standard_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / VOLT_STANDARD_PHASE_SCALE) + + +def _volt_standard_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def _volt_standard_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / VOLT_STANDARD_FRICTION_LAT_RISE) + jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / VOLT_STANDARD_FRICTION_JERK_RISE) + return _volt_standard_low_speed_factor(v_ego) * lat_factor * jerk_factor + + +def get_volt_standard_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + gain = _volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_FF_GAIN_LEFT, VOLT_STANDARD_FF_GAIN_RIGHT) + abs_lateral_accel = abs(desired_lateral_accel) + onset = _volt_standard_sigmoid((abs_lateral_accel - VOLT_STANDARD_FF_ONSET) / VOLT_STANDARD_FF_ONSET_WIDTH) + cutoff = _volt_standard_sigmoid((VOLT_STANDARD_FF_CUTOFF - abs_lateral_accel) / VOLT_STANDARD_FF_CUTOFF_WIDTH) + extra_scale = gain * onset * cutoff + phase = _volt_standard_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _volt_standard_low_speed_factor(v_ego) + turn_in_boost = 1.0 + (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_TURN_IN_BOOST_LEFT, VOLT_STANDARD_TURN_IN_BOOST_RIGHT) * + turn_in_weight * low_speed_factor) + unwind_taper = 1.0 - (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_UNWIND_TAPER_LEFT, VOLT_STANDARD_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.30 + 0.70 * low_speed_factor)) + return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) + + +def get_volt_standard_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + base_threshold = get_friction_threshold(v_ego) + transition_envelope = _volt_standard_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _volt_standard_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + threshold_scale = 1.0 - (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_LEFT, VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + transition_envelope * turn_in_weight) + threshold_scale += (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_LEFT, VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_RIGHT) * + transition_envelope * unwind_weight) + return base_threshold * min(max(threshold_scale, 0.84), 1.12) + + +def get_volt_standard_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + transition_envelope = _volt_standard_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _volt_standard_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + friction_scale = VOLT_STANDARD_FRICTION_MULT + friction_scale += (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_TURN_IN_FRICTION_BOOST_LEFT, VOLT_STANDARD_TURN_IN_FRICTION_BOOST_RIGHT) * + transition_envelope * turn_in_weight) + friction_scale -= (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_LEFT, VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_RIGHT) * + transition_envelope * unwind_weight) + return min(max(friction_scale, 0.90), 1.14) + + +def get_volt_standard_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _volt_standard_sigmoid((v_ego - VOLT_STANDARD_CENTER_TAPER_SPEED) / VOLT_STANDARD_CENTER_TAPER_SPEED_WIDTH) + center_weight = _volt_standard_sigmoid((VOLT_STANDARD_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / VOLT_STANDARD_CENTER_TAPER_LAT_WIDTH) + reduction = VOLT_STANDARD_CENTER_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + +def get_silverado_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _sigmoid((v_ego - SILVERADO_CENTER_TAPER_SPEED) / SILVERADO_CENTER_TAPER_SPEED_WIDTH) + center_weight = _sigmoid((SILVERADO_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / SILVERADO_CENTER_TAPER_LAT_WIDTH) + reduction = SILVERADO_CENTER_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + +def _sonata_hybrid_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _sonata_hybrid_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / SONATA_HYBRID_TRANSITION_SPEED) ** 2) + + +def _sonata_hybrid_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / SONATA_HYBRID_PHASE_SCALE) + + +def _sonata_hybrid_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def get_sonata_hybrid_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + abs_lateral_accel = abs(desired_lateral_accel) + onset = _sonata_hybrid_sigmoid((abs_lateral_accel - SONATA_HYBRID_FF_ONSET) / SONATA_HYBRID_FF_ONSET_WIDTH) + cutoff = _sonata_hybrid_sigmoid((SONATA_HYBRID_FF_CUTOFF - abs_lateral_accel) / SONATA_HYBRID_FF_CUTOFF_WIDTH) + base_reduction = _sonata_hybrid_side_value(desired_lateral_accel, + SONATA_HYBRID_FF_REDUCTION_LEFT, + SONATA_HYBRID_FF_REDUCTION_RIGHT) * onset * cutoff + phase = _sonata_hybrid_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _sonata_hybrid_low_speed_factor(v_ego) + turn_in_boost = 1.0 + (_sonata_hybrid_side_value(desired_lateral_accel, + SONATA_HYBRID_TURN_IN_BOOST_LEFT, + SONATA_HYBRID_TURN_IN_BOOST_RIGHT) * + turn_in_weight * low_speed_factor) + unwind_taper = 1.0 - (_sonata_hybrid_side_value(desired_lateral_accel, + SONATA_HYBRID_UNWIND_TAPER_LEFT, + SONATA_HYBRID_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0) + + +def get_sonata_hybrid_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _sonata_hybrid_sigmoid((v_ego - SONATA_HYBRID_CENTER_TAPER_SPEED) / SONATA_HYBRID_CENTER_TAPER_SPEED_WIDTH) + center_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / SONATA_HYBRID_CENTER_TAPER_LAT_WIDTH) + reduction = SONATA_HYBRID_CENTER_TAPER_MAX * speed_weight * center_weight + low_speed_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) / + SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH) + low_speed_center_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / + SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT_WIDTH) + reduction += SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_MAX * low_speed_weight * low_speed_center_weight + return 1.0 - reduction + + +def _sonata_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _sonata_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / SONATA_TRANSITION_SPEED) ** 2) + + +def _sonata_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / SONATA_PHASE_SCALE) + + +def _sonata_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def get_sonata_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + abs_lateral_accel = abs(desired_lateral_accel) + onset = _sonata_sigmoid((abs_lateral_accel - SONATA_FF_ONSET) / SONATA_FF_ONSET_WIDTH) + cutoff = _sonata_sigmoid((SONATA_FF_CUTOFF - abs_lateral_accel) / SONATA_FF_CUTOFF_WIDTH) + base_reduction = _sonata_side_value(desired_lateral_accel, SONATA_FF_REDUCTION_LEFT, SONATA_FF_REDUCTION_RIGHT) * onset * cutoff + phase = _sonata_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _sonata_low_speed_factor(v_ego) + turn_in_boost = 1.0 + (_sonata_side_value(desired_lateral_accel, SONATA_TURN_IN_BOOST_LEFT, SONATA_TURN_IN_BOOST_RIGHT) * + turn_in_weight * low_speed_factor) + unwind_taper = 1.0 - (_sonata_side_value(desired_lateral_accel, SONATA_UNWIND_TAPER_LEFT, SONATA_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0) + + +def get_sonata_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _sonata_sigmoid((v_ego - SONATA_CENTER_TAPER_SPEED) / SONATA_CENTER_TAPER_SPEED_WIDTH) + center_weight = _sonata_sigmoid((SONATA_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / SONATA_CENTER_TAPER_LAT_WIDTH) + reduction = SONATA_CENTER_TAPER_MAX * speed_weight * center_weight + low_speed_weight = _sonata_sigmoid((SONATA_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) / + SONATA_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH) + low_speed_center_weight = _sonata_sigmoid((SONATA_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / + SONATA_LOW_SPEED_CENTER_TAPER_LAT_WIDTH) + reduction += SONATA_LOW_SPEED_CENTER_TAPER_MAX * low_speed_weight * low_speed_center_weight + return 1.0 - reduction + + +def _elantra_non_scc_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _elantra_non_scc_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / ELANTRA_NON_SCC_TRANSITION_SPEED) ** 2) + + +def _elantra_non_scc_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / ELANTRA_NON_SCC_PHASE_SCALE) + + +def _elantra_non_scc_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def get_elantra_non_scc_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + abs_lateral_accel = abs(desired_lateral_accel) + onset = _elantra_non_scc_sigmoid((abs_lateral_accel - ELANTRA_NON_SCC_FF_ONSET) / ELANTRA_NON_SCC_FF_ONSET_WIDTH) + cutoff = _elantra_non_scc_sigmoid((ELANTRA_NON_SCC_FF_CUTOFF - abs_lateral_accel) / ELANTRA_NON_SCC_FF_CUTOFF_WIDTH) + low_speed_factor = _elantra_non_scc_low_speed_factor(v_ego) + envelope = onset * cutoff * low_speed_factor + base_scale = 1.0 - (_elantra_non_scc_side_value(desired_lateral_accel, + ELANTRA_NON_SCC_FF_ADJUST_LEFT, + ELANTRA_NON_SCC_FF_ADJUST_RIGHT) * envelope) + phase = _elantra_non_scc_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + turn_in_boost = 1.0 + (_elantra_non_scc_side_value(desired_lateral_accel, + ELANTRA_NON_SCC_TURN_IN_BOOST_LEFT, + ELANTRA_NON_SCC_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.35 + 0.65 * low_speed_factor)) + unwind_taper = 1.0 - (_elantra_non_scc_side_value(desired_lateral_accel, + ELANTRA_NON_SCC_UNWIND_TAPER_LEFT, + ELANTRA_NON_SCC_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + return base_scale * turn_in_boost * max(unwind_taper, 0.0) + + +def _kia_xceed_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _kia_xceed_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / KIA_XCEED_TRANSITION_SPEED) ** 2) + + +def _kia_xceed_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / KIA_XCEED_PHASE_SCALE) + + +def _kia_xceed_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def get_kia_xceed_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + abs_lateral_accel = abs(desired_lateral_accel) + onset = _kia_xceed_sigmoid((abs_lateral_accel - KIA_XCEED_FF_ONSET) / KIA_XCEED_FF_ONSET_WIDTH) + cutoff = _kia_xceed_sigmoid((KIA_XCEED_FF_CUTOFF - abs_lateral_accel) / KIA_XCEED_FF_CUTOFF_WIDTH) + base_reduction = _kia_xceed_side_value(desired_lateral_accel, + KIA_XCEED_FF_REDUCTION_LEFT, + KIA_XCEED_FF_REDUCTION_RIGHT) * onset * cutoff + phase = _kia_xceed_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _kia_xceed_low_speed_factor(v_ego) + turn_in_boost = 1.0 + (_kia_xceed_side_value(desired_lateral_accel, + KIA_XCEED_TURN_IN_BOOST_LEFT, + KIA_XCEED_TURN_IN_BOOST_RIGHT) * + turn_in_weight * low_speed_factor) + unwind_taper = 1.0 - (_kia_xceed_side_value(desired_lateral_accel, + KIA_XCEED_UNWIND_TAPER_LEFT, + KIA_XCEED_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0) + + +def get_kia_xceed_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _kia_xceed_sigmoid((v_ego - KIA_XCEED_CENTER_TAPER_SPEED) / KIA_XCEED_CENTER_TAPER_SPEED_WIDTH) + center_weight = _kia_xceed_sigmoid((KIA_XCEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_XCEED_CENTER_TAPER_LAT_WIDTH) + reduction = KIA_XCEED_CENTER_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + +def get_kia_niro_phev_2022_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _sigmoid((v_ego - KIA_NIRO_PHEV_2022_CENTER_TAPER_SPEED) / KIA_NIRO_PHEV_2022_CENTER_TAPER_SPEED_WIDTH) + center_weight = _sigmoid((KIA_NIRO_PHEV_2022_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_NIRO_PHEV_2022_CENTER_TAPER_LAT_WIDTH) + reduction = KIA_NIRO_PHEV_2022_CENTER_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + +def get_kia_niro_phev_2022_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + base_threshold = get_friction_threshold(v_ego) + speed_weight = _sigmoid((v_ego - KIA_NIRO_PHEV_2022_FRICTION_SPEED) / KIA_NIRO_PHEV_2022_FRICTION_SPEED_WIDTH) + center_weight = _sigmoid((KIA_NIRO_PHEV_2022_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) / KIA_NIRO_PHEV_2022_FRICTION_CENTER_LAT_WIDTH) + calm_jerk_weight = _sigmoid((KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK - abs(desired_lateral_jerk)) / KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK_WIDTH) + threshold_scale = 1.0 + (KIA_NIRO_PHEV_2022_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight * calm_jerk_weight) + return base_threshold * min(max(threshold_scale, 1.0), 1.18) + + +def _kia_forte_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _kia_forte_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / KIA_FORTE_TRANSITION_SPEED) ** 2) + + +def _kia_forte_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / KIA_FORTE_PHASE_SCALE) + + +def _kia_forte_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def get_kia_forte_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + abs_lateral_accel = abs(desired_lateral_accel) + onset = _kia_forte_sigmoid((abs_lateral_accel - KIA_FORTE_FF_ONSET) / KIA_FORTE_FF_ONSET_WIDTH) + cutoff = _kia_forte_sigmoid((KIA_FORTE_FF_CUTOFF - abs_lateral_accel) / KIA_FORTE_FF_CUTOFF_WIDTH) + base_reduction = _kia_forte_side_value(desired_lateral_accel, KIA_FORTE_FF_REDUCTION_LEFT, KIA_FORTE_FF_REDUCTION_RIGHT) * onset * cutoff + phase = _kia_forte_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _kia_forte_low_speed_factor(v_ego) + turn_in_boost = 1.0 + (_kia_forte_side_value(desired_lateral_accel, KIA_FORTE_TURN_IN_BOOST_LEFT, KIA_FORTE_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.35 + 0.65 * low_speed_factor)) + unwind_taper = 1.0 - (_kia_forte_side_value(desired_lateral_accel, KIA_FORTE_UNWIND_TAPER_LEFT, KIA_FORTE_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + crawl_turn_in_scale = 0.0 + if desired_lateral_accel * desired_lateral_jerk > 0.0: + crawl_speed_weight = _kia_forte_sigmoid((KIA_FORTE_CRAWL_TURN_IN_FF_SPEED - max(v_ego, 0.0)) / + KIA_FORTE_CRAWL_TURN_IN_FF_SPEED_WIDTH) + crawl_lat_weight = _kia_forte_sigmoid((abs_lateral_accel - KIA_FORTE_CRAWL_TURN_IN_FF_LAT) / + KIA_FORTE_CRAWL_TURN_IN_FF_LAT_WIDTH) + crawl_turn_in_scale = _kia_forte_side_value(desired_lateral_accel, KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_LEFT, + KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_RIGHT) * crawl_speed_weight * crawl_lat_weight + return ((1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)) + crawl_turn_in_scale + + +def get_kia_forte_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _kia_forte_sigmoid((v_ego - KIA_FORTE_CENTER_TAPER_SPEED) / KIA_FORTE_CENTER_TAPER_SPEED_WIDTH) + center_weight = _kia_forte_sigmoid((KIA_FORTE_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_FORTE_CENTER_TAPER_LAT_WIDTH) + reduction = KIA_FORTE_CENTER_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + +def get_kia_forte_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + base_threshold = get_friction_threshold(v_ego) + speed_weight = _kia_forte_sigmoid((v_ego - KIA_FORTE_FRICTION_SPEED) / KIA_FORTE_FRICTION_SPEED_WIDTH) + center_weight = _kia_forte_sigmoid((KIA_FORTE_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) / KIA_FORTE_FRICTION_CENTER_LAT_WIDTH) + calm_jerk_weight = _kia_forte_sigmoid((KIA_FORTE_FRICTION_CALM_JERK - abs(desired_lateral_jerk)) / KIA_FORTE_FRICTION_CALM_JERK_WIDTH) + threshold_scale = 1.0 + (KIA_FORTE_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight * calm_jerk_weight) + return base_threshold * min(max(threshold_scale, 1.0), 1.18) + + +def _palisade_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _palisade_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / PALISADE_TRANSITION_SPEED) ** 2) + + +def _palisade_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / PALISADE_PHASE_SCALE) + + +def _palisade_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def _palisade_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / PALISADE_FRICTION_LAT_RISE) + jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / PALISADE_FRICTION_JERK_RISE) + return _palisade_low_speed_factor(v_ego) * lat_factor * jerk_factor + + +def get_palisade_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + gain = _palisade_side_value(desired_lateral_accel, PALISADE_FF_GAIN_LEFT, PALISADE_FF_GAIN_RIGHT) + abs_lateral_accel = abs(desired_lateral_accel) + onset = _palisade_sigmoid((abs_lateral_accel - PALISADE_FF_ONSET) / PALISADE_FF_ONSET_WIDTH) + cutoff = _palisade_sigmoid((PALISADE_FF_CUTOFF - abs_lateral_accel) / PALISADE_FF_CUTOFF_WIDTH) + extra_scale = gain * onset * cutoff + phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _palisade_low_speed_factor(v_ego) + turn_in_boost = 1.0 + (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_BOOST_LEFT, PALISADE_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.35 + 0.65 * low_speed_factor)) + unwind_taper = 1.0 - (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_TAPER_LEFT, PALISADE_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) + + +def get_palisade_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + base_threshold = get_friction_threshold(v_ego) + transition_envelope = _palisade_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + threshold_scale = 1.0 - (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_THRESHOLD_REDUCTION_LEFT, PALISADE_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + transition_envelope * turn_in_weight) + threshold_scale += (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_THRESHOLD_INCREASE_LEFT, PALISADE_UNWIND_THRESHOLD_INCREASE_RIGHT) * + transition_envelope * unwind_weight) + return base_threshold * min(max(threshold_scale, 0.84), 1.14) + + +def get_palisade_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + transition_envelope = _palisade_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + friction_scale = PALISADE_FRICTION_MULT + friction_scale += (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_FRICTION_BOOST_LEFT, PALISADE_TURN_IN_FRICTION_BOOST_RIGHT) * + transition_envelope * turn_in_weight) + friction_scale -= (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_FRICTION_REDUCTION_LEFT, PALISADE_UNWIND_FRICTION_REDUCTION_RIGHT) * + transition_envelope * unwind_weight) + return min(max(friction_scale, 0.92), 1.12) + + +def genesis_g90_lateral_testing_ground_active() -> bool: + return testing_ground.use(GENESIS_G90_LATERAL_TESTING_GROUND_ID) + + +def _genesis_g90_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _genesis_g90_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / GENESIS_G90_TRANSITION_SPEED) ** 2) + + +def _genesis_g90_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / GENESIS_G90_PHASE_SCALE) + + +def _genesis_g90_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def _genesis_g90_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / GENESIS_G90_FRICTION_LAT_RISE) + jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / GENESIS_G90_FRICTION_JERK_RISE) + return _genesis_g90_low_speed_factor(v_ego) * lat_factor * jerk_factor + + +def get_genesis_g90_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + gain = _genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_FF_GAIN_LEFT, GENESIS_G90_FF_GAIN_RIGHT) + abs_lateral_accel = abs(desired_lateral_accel) + onset = _genesis_g90_sigmoid((abs_lateral_accel - GENESIS_G90_FF_ONSET) / GENESIS_G90_FF_ONSET_WIDTH) + cutoff = _genesis_g90_sigmoid((GENESIS_G90_FF_CUTOFF - abs_lateral_accel) / GENESIS_G90_FF_CUTOFF_WIDTH) + extra_scale = gain * onset * cutoff + phase = _genesis_g90_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _genesis_g90_low_speed_factor(v_ego) + turn_in_boost = 1.0 + (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_TURN_IN_BOOST_LEFT, GENESIS_G90_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.35 + 0.65 * low_speed_factor)) + unwind_taper = 1.0 - (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_UNWIND_TAPER_LEFT, GENESIS_G90_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.30 + 0.70 * low_speed_factor)) + return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) + + +def get_genesis_g90_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + base_threshold = get_friction_threshold(v_ego) + transition_envelope = _genesis_g90_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _genesis_g90_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + threshold_scale = 1.0 - (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_TURN_IN_THRESHOLD_REDUCTION_LEFT, GENESIS_G90_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + transition_envelope * turn_in_weight) + threshold_scale += (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_UNWIND_THRESHOLD_INCREASE_LEFT, GENESIS_G90_UNWIND_THRESHOLD_INCREASE_RIGHT) * + transition_envelope * unwind_weight) + return base_threshold * min(max(threshold_scale, 0.86), 1.12) + + +def get_genesis_g90_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + transition_envelope = _genesis_g90_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _genesis_g90_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + friction_scale = GENESIS_G90_FRICTION_MULT + friction_scale += (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_TURN_IN_FRICTION_BOOST_LEFT, GENESIS_G90_TURN_IN_FRICTION_BOOST_RIGHT) * + transition_envelope * turn_in_weight) + friction_scale -= (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_UNWIND_FRICTION_REDUCTION_LEFT, GENESIS_G90_UNWIND_FRICTION_REDUCTION_RIGHT) * + transition_envelope * unwind_weight) + return min(max(friction_scale, 0.92), 1.12) + + +def _ioniq_5_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _ioniq_5_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / IONIQ_5_TRANSITION_SPEED) ** 2) + + +def _ioniq_5_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_5_PHASE_SCALE) + + +def _ioniq_5_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def _ioniq_5_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + abs_lateral_accel = abs(desired_lateral_accel) + onset = _ioniq_5_sigmoid((abs_lateral_accel - IONIQ_5_FF_ONSET) / IONIQ_5_FF_ONSET_WIDTH) + cutoff = _ioniq_5_sigmoid((IONIQ_5_FF_CUTOFF - abs_lateral_accel) / IONIQ_5_FF_CUTOFF_WIDTH) + return onset * cutoff * _ioniq_5_low_speed_factor(v_ego) + + +def get_ioniq_5_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + envelope = _ioniq_5_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _ioniq_5_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _ioniq_5_low_speed_factor(v_ego) + + base_reduction = _ioniq_5_side_value(desired_lateral_accel, IONIQ_5_FF_REDUCTION_LEFT, IONIQ_5_FF_REDUCTION_RIGHT) * envelope + turn_in_boost = 1.0 + (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_TURN_IN_BOOST_LEFT, IONIQ_5_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.35 + 0.65 * low_speed_factor)) + unwind_taper = 1.0 - (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_UNWIND_TAPER_LEFT, IONIQ_5_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0) + + +def get_ioniq_5_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + base_threshold = get_friction_threshold(v_ego) + envelope = _ioniq_5_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _ioniq_5_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + + threshold_scale = 1.0 - (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_LEFT, IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + envelope * turn_in_weight) + threshold_scale += (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_UNWIND_THRESHOLD_INCREASE_LEFT, IONIQ_5_UNWIND_THRESHOLD_INCREASE_RIGHT) * + envelope * unwind_weight) + return base_threshold * min(max(threshold_scale, 0.86), 1.18) + + +def get_ioniq_5_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + if desired_lateral_accel == 0.0 or desired_lateral_jerk == 0.0: + return 1.0 + + envelope = _ioniq_5_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _ioniq_5_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + + friction_scale = 1.0 + friction_scale += (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_TURN_IN_FRICTION_BOOST_LEFT, IONIQ_5_TURN_IN_FRICTION_BOOST_RIGHT) * + envelope * turn_in_weight) + friction_scale -= (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT, IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT) * + envelope * unwind_weight) + return min(max(friction_scale, 0.86), 1.04) + + +def get_ioniq_5_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _ioniq_5_sigmoid((v_ego - IONIQ_5_CENTER_TAPER_SPEED) / IONIQ_5_CENTER_TAPER_SPEED_WIDTH) + center_weight = _ioniq_5_sigmoid((IONIQ_5_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_5_CENTER_TAPER_LAT_WIDTH) + reduction = IONIQ_5_CENTER_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + +def _ioniq_ev_old_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _ioniq_ev_old_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / IONIQ_EV_OLD_TRANSITION_SPEED) ** 2) + + +def _ioniq_ev_old_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_EV_OLD_PHASE_SCALE) + + +def _ioniq_ev_old_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def get_ioniq_ev_old_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + abs_lateral_accel = abs(desired_lateral_accel) + onset = _ioniq_ev_old_sigmoid((abs_lateral_accel - IONIQ_EV_OLD_FF_ONSET) / IONIQ_EV_OLD_FF_ONSET_WIDTH) + cutoff = _ioniq_ev_old_sigmoid((IONIQ_EV_OLD_FF_CUTOFF - abs_lateral_accel) / IONIQ_EV_OLD_FF_CUTOFF_WIDTH) + base_reduction = _ioniq_ev_old_side_value(desired_lateral_accel, IONIQ_EV_OLD_FF_REDUCTION_LEFT, IONIQ_EV_OLD_FF_REDUCTION_RIGHT) * onset * cutoff + phase = _ioniq_ev_old_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _ioniq_ev_old_low_speed_factor(v_ego) + turn_in_boost = 1.0 + (_ioniq_ev_old_side_value(desired_lateral_accel, IONIQ_EV_OLD_TURN_IN_BOOST_LEFT, IONIQ_EV_OLD_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.35 + 0.65 * low_speed_factor)) + unwind_taper = 1.0 - (_ioniq_ev_old_side_value(desired_lateral_accel, IONIQ_EV_OLD_UNWIND_TAPER_LEFT, IONIQ_EV_OLD_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0) + + +def get_ioniq_ev_old_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _ioniq_ev_old_sigmoid((v_ego - IONIQ_EV_OLD_CENTER_TAPER_SPEED) / IONIQ_EV_OLD_CENTER_TAPER_SPEED_WIDTH) + center_weight = _ioniq_ev_old_sigmoid((IONIQ_EV_OLD_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_EV_OLD_CENTER_TAPER_LAT_WIDTH) + reduction = IONIQ_EV_OLD_CENTER_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + +def _ioniq_6_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _ioniq_6_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / IONIQ_6_TRANSITION_SPEED) ** 2) + + +def _ioniq_6_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_6_PHASE_SCALE) + + +def _ioniq_6_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def _ioniq_6_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / IONIQ_6_FRICTION_LAT_RISE) + jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / IONIQ_6_FRICTION_JERK_RISE) + return _ioniq_6_low_speed_factor(v_ego) * lat_factor * jerk_factor + + +def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + gain = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_FF_GAIN_LEFT, IONIQ_6_FF_GAIN_RIGHT) + abs_lateral_accel = abs(desired_lateral_accel) + onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_FF_ONSET) / IONIQ_6_FF_ONSET_WIDTH) + cutoff = _ioniq_6_sigmoid((IONIQ_6_FF_CUTOFF - abs_lateral_accel) / IONIQ_6_FF_CUTOFF_WIDTH) + extra_scale = gain * onset * cutoff + phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _ioniq_6_low_speed_factor(v_ego) + turn_in_boost = 1.0 + (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_TURN_IN_BOOST_LEFT, IONIQ_6_TURN_IN_BOOST_RIGHT) * + turn_in_weight * low_speed_factor) + unwind_taper = 1.0 - (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_UNWIND_TAPER_LEFT, IONIQ_6_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.30 + 0.70 * low_speed_factor)) + crawl_turn_in_scale = 0.0 + if desired_lateral_accel * desired_lateral_jerk > 0.0: + crawl_speed_weight = _ioniq_6_sigmoid((IONIQ_6_CRAWL_TURN_IN_FF_SPEED - max(v_ego, 0.0)) / + IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH) + crawl_lat_weight = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_CRAWL_TURN_IN_FF_LAT) / + IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH) + crawl_turn_in_scale = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT, + IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT) * crawl_speed_weight * crawl_lat_weight + high_speed_right_turn_in_scale = 0.0 + if desired_lateral_accel < 0.0 and desired_lateral_accel * desired_lateral_jerk > 0.0: + high_speed_weight = _ioniq_6_sigmoid((max(v_ego, 0.0) - IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED) / + IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH) + high_speed_lat_onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_START) / + IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH) + high_speed_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END - abs_lateral_accel) / + IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH) + high_speed_right_turn_in_scale = IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST * high_speed_weight * high_speed_lat_onset * high_speed_lat_cutoff + return (1.0 + crawl_turn_in_scale + high_speed_right_turn_in_scale + + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego) + + +def get_ioniq_6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + base_threshold = max(get_friction_threshold(v_ego), IONIQ_6_BASE_FRICTION_THRESHOLD) + transition_envelope = _ioniq_6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + threshold_scale = 1.0 - (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT, IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + transition_envelope * turn_in_weight) + threshold_scale += (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT, IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT) * + transition_envelope * unwind_weight) + return base_threshold * min(max(threshold_scale, 0.82), 1.18) + + +def get_ioniq_6_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + transition_envelope = _ioniq_6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + friction_scale = IONIQ_6_FRICTION_MULT + friction_scale += (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT, IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT) * + transition_envelope * turn_in_weight) + friction_scale -= (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT, IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT) * + transition_envelope * unwind_weight) + return min(max(friction_scale, 0.82), 1.08) + + +def get_ioniq_6_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_CENTER_TAPER_SPEED) / IONIQ_6_CENTER_TAPER_SPEED_WIDTH) + center_weight = _ioniq_6_sigmoid((IONIQ_6_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_6_CENTER_TAPER_LAT_WIDTH) + high_speed_reduction = IONIQ_6_CENTER_TAPER_MAX * speed_weight * center_weight + + highway_speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED) / IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED_WIDTH) + highway_center_weight = _ioniq_6_sigmoid((IONIQ_6_HIGHWAY_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / + IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH) + highway_center_reduction = IONIQ_6_HIGHWAY_CENTER_TAPER_MAX * highway_speed_weight * highway_center_weight + + low_mid_onset = _ioniq_6_sigmoid((v_ego - IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MIN) / IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH) + low_mid_cutoff = _ioniq_6_sigmoid((IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MAX - v_ego) / IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH) + low_mid_speed_weight = low_mid_onset * low_mid_cutoff + low_mid_center_weight = _ioniq_6_sigmoid((IONIQ_6_LOW_MID_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / + IONIQ_6_LOW_MID_CENTER_TAPER_LAT_WIDTH) + low_mid_reduction = IONIQ_6_LOW_MID_CENTER_TAPER_MAX * low_mid_speed_weight * low_mid_center_weight + + return 1.0 - min(high_speed_reduction + highway_center_reduction + low_mid_reduction, 0.12) + + +def get_ioniq_6_directional_taper_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float | None = None) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + abs_lateral_accel = abs(desired_lateral_accel) + onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_DIRECTIONAL_TAPER_LAT_START) / IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH) + cutoff = _ioniq_6_sigmoid((IONIQ_6_DIRECTIONAL_TAPER_LAT_END - abs_lateral_accel) / IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH) + band_weight = onset * cutoff + heavy_band_weight = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START) / IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH) + phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk) + unwind_weight = max(-phase, 0.0) * _ioniq_6_sigmoid((abs(desired_lateral_jerk) - IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET) / + IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH) + low_speed_relief_weight = 0.0 + if v_ego is not None: + low_speed_weight = _ioniq_6_sigmoid((IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED - max(v_ego, 0.0)) / + IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH) + tight_turn_weight = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT) / + IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH) + low_speed_relief_weight = IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF * low_speed_weight * tight_turn_weight * (1.0 - unwind_weight) + base_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT, IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT) + unwind_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT) + heavy_base_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT) + heavy_unwind_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT) + base_reduction *= 1.0 - low_speed_relief_weight + heavy_base_reduction *= 1.0 - low_speed_relief_weight + reduction = band_weight * (base_reduction + unwind_reduction * unwind_weight) + reduction += heavy_band_weight * (heavy_base_reduction + heavy_unwind_reduction * unwind_weight) + floor = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT, IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT) + floor -= _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT) * unwind_weight + return max(1.0 - reduction, floor) + + +def get_ioniq_6_output_taper_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_OUTPUT_TAPER_SPEED) / IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH) + center_taper = get_ioniq_6_center_taper_scale(desired_lateral_accel, v_ego) + directional_taper = get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego) + center_scale = 1.0 - ((1.0 - center_taper) * IONIQ_6_OUTPUT_CENTER_TAPER_BLEND * speed_weight) + directional_scale = 1.0 - ((1.0 - directional_taper) * IONIQ_6_OUTPUT_DIRECTIONAL_TAPER_BLEND * speed_weight) + return center_scale * directional_scale + + +def get_ioniq_6_highway_output_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED) / IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH) + center_weight = _ioniq_6_sigmoid((IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / + IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH) + reduction = IONIQ_6_HIGHWAY_OUTPUT_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + +def get_ioniq_6_highway_transition_output_taper_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED) / IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH) + center_weight = _ioniq_6_sigmoid((IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / + IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT_WIDTH) + jerk_weight = _ioniq_6_sigmoid((abs(desired_lateral_jerk) - IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK) / + IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK_WIDTH) + reduction = IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_MAX * speed_weight * center_weight * jerk_weight + return 1.0 - reduction + + +def get_ioniq_6_low_speed_angle_assist_torque(desired_angle_deg: float, actual_angle_deg: float, + current_output_torque: float, v_ego: float) -> float: + angle_error = desired_angle_deg - actual_angle_deg + if desired_angle_deg * angle_error > 0.0: + speed_weight = _ioniq_6_sigmoid((IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED - max(v_ego, 0.0)) / + IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH) + error_weight = _ioniq_6_sigmoid((abs(angle_error) - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR) / + IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH) + desired_angle_weight = _ioniq_6_sigmoid((abs(desired_angle_deg) - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE) / + IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH) + tracking_ratio = abs(actual_angle_deg) / max(abs(desired_angle_deg), 1e-3) + tracking_taper = _ioniq_6_sigmoid((tracking_ratio - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_START) / + IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_WIDTH) + tracking_scale = max(1.0 - tracking_taper, IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_FLOOR) + assist_torque = math.copysign(IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE * speed_weight * error_weight * desired_angle_weight * tracking_scale, -angle_error) + if abs(assist_torque) < 1e-4: + return current_output_torque + + if current_output_torque * assist_torque >= 0.0: + add_scale = float(np.interp(abs(current_output_torque), + IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_BP, + IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_V)) + return float(np.clip(current_output_torque + (assist_torque * add_scale), -1.0, 1.0)) + + return float(np.clip(current_output_torque + assist_torque, -1.0, 1.0)) + + speed_weight = _ioniq_6_sigmoid((IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED - max(v_ego, 0.0)) / + IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED_WIDTH) + error_weight = _ioniq_6_sigmoid((abs(angle_error) - IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR) / + IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR_WIDTH) + actual_angle_weight = _ioniq_6_sigmoid((abs(actual_angle_deg) - IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE) / + IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE_WIDTH) + assist_torque = math.copysign(IONIQ_6_LOW_SPEED_UNWIND_ASSIST_MAX_TORQUE * speed_weight * error_weight * actual_angle_weight, -angle_error) + if abs(assist_torque) < 1e-4: + return current_output_torque + + if current_output_torque * assist_torque >= 0.0: + assist_torque *= IONIQ_6_LOW_SPEED_UNWIND_ASSIST_BLEND + + return float(np.clip(current_output_torque + assist_torque, -1.0, 1.0)) + + +def kia_ev6_lateral_testing_ground_active() -> bool: + return testing_ground.use(KIA_EV6_LATERAL_TESTING_GROUND_ID, KIA_EV6_LATERAL_TESTING_GROUND_VARIANT) + + +def _kia_ev6_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _kia_ev6_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / KIA_EV6_TRANSITION_SPEED) ** 2) + + +def _kia_ev6_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / KIA_EV6_PHASE_SCALE) + + +def _kia_ev6_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def _kia_ev6_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / KIA_EV6_FRICTION_LAT_RISE) + jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / KIA_EV6_FRICTION_JERK_RISE) + return _kia_ev6_low_speed_factor(v_ego) * lat_factor * jerk_factor + + +def get_kia_ev6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + gain = _kia_ev6_side_value(desired_lateral_accel, KIA_EV6_FF_GAIN_LEFT, KIA_EV6_FF_GAIN_RIGHT) + abs_lateral_accel = abs(desired_lateral_accel) + onset = _kia_ev6_sigmoid((abs_lateral_accel - KIA_EV6_FF_ONSET) / KIA_EV6_FF_ONSET_WIDTH) + cutoff = _kia_ev6_sigmoid((KIA_EV6_FF_CUTOFF - abs_lateral_accel) / KIA_EV6_FF_CUTOFF_WIDTH) + extra_scale = gain * onset * cutoff + phase = _kia_ev6_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _kia_ev6_low_speed_factor(v_ego) + turn_in_boost = 1.0 + (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_TURN_IN_BOOST_LEFT, KIA_EV6_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.35 + 0.65 * low_speed_factor)) + unwind_taper = 1.0 - (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_UNWIND_TAPER_LEFT, KIA_EV6_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) + + +def get_kia_ev6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + base_threshold = get_friction_threshold(v_ego) + transition_envelope = _kia_ev6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _kia_ev6_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + threshold_scale = 1.0 - (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_LEFT, KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + transition_envelope * turn_in_weight) + threshold_scale += (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_UNWIND_THRESHOLD_INCREASE_LEFT, KIA_EV6_UNWIND_THRESHOLD_INCREASE_RIGHT) * + transition_envelope * unwind_weight) + return base_threshold * min(max(threshold_scale, 0.82), 1.16) + + +def get_kia_ev6_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + transition_envelope = _kia_ev6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _kia_ev6_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + friction_scale = KIA_EV6_FRICTION_MULT + friction_scale += (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_TURN_IN_FRICTION_BOOST_LEFT, KIA_EV6_TURN_IN_FRICTION_BOOST_RIGHT) * + transition_envelope * turn_in_weight) + friction_scale -= (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_UNWIND_FRICTION_REDUCTION_LEFT, KIA_EV6_UNWIND_FRICTION_REDUCTION_RIGHT) * + transition_envelope * unwind_weight) + return min(max(friction_scale, 0.90), 1.10) + + +def get_kia_ev6_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _kia_ev6_sigmoid((v_ego - KIA_EV6_CENTER_TAPER_SPEED) / KIA_EV6_CENTER_TAPER_SPEED_WIDTH) + center_weight = _kia_ev6_sigmoid((KIA_EV6_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_EV6_CENTER_TAPER_LAT_WIDTH) + reduction = KIA_EV6_CENTER_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + +def get_kia_ev6_low_speed_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _kia_ev6_sigmoid((KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) / KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH) + center_weight = _kia_ev6_sigmoid((KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT_WIDTH) + reduction = KIA_EV6_LOW_SPEED_CENTER_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + +def volt_plexy_lateral_testing_ground_active() -> bool: + return testing_ground.use(VOLT_PLEXY_LATERAL_TESTING_GROUND_ID) + + +def _volt_plexy_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def get_volt_plexy_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + standard_scale = get_volt_standard_ff_scale(desired_lateral_accel, desired_lateral_jerk, v_ego) + extra_scale = standard_scale - 1.0 + extra_mult = _volt_plexy_side_value(desired_lateral_accel, VOLT_PLEXY_FF_EXTRA_MULT_LEFT, VOLT_PLEXY_FF_EXTRA_MULT_RIGHT) + return 1.0 + (extra_scale * extra_mult) + + +def get_volt_plexy_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + standard_threshold = get_volt_standard_friction_threshold(v_ego, desired_lateral_accel, desired_lateral_jerk) + threshold_mult = _volt_plexy_side_value(desired_lateral_accel, VOLT_PLEXY_FRICTION_THRESHOLD_MULT_LEFT, VOLT_PLEXY_FRICTION_THRESHOLD_MULT_RIGHT) + return standard_threshold * threshold_mult + + +def get_volt_plexy_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + standard_scale = get_volt_standard_friction_scale(v_ego, desired_lateral_accel, desired_lateral_jerk) + friction_extra = standard_scale - 1.0 + friction_mult = _volt_plexy_side_value(desired_lateral_accel, VOLT_PLEXY_FRICTION_SCALE_MULT_LEFT, VOLT_PLEXY_FRICTION_SCALE_MULT_RIGHT) + return 1.0 + (friction_extra * friction_mult) + + +def get_volt_plexy_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + standard_scale = get_volt_standard_center_taper_scale(desired_lateral_accel, v_ego) + standard_reduction = 1.0 - standard_scale + return 1.0 - (standard_reduction * VOLT_PLEXY_CENTER_TAPER_REDUCTION_MULT) + + +__all__ = [name for name in globals() if not name.startswith("_") and name not in { + "math", "np", "GM_CAR", "HYUNDAI_CAR", "TOYOTA_CAR", "CV", "testing_ground", +}]